A Novel Real-Time Autonomous Localization Algorithm Based on Weighted Loosely Coupled Visual–Inertial Data of the Velocity Layer

Abstract

IMUs (inertial measurement units) and cameras are widely utilized and combined to autonomously measure the motion states of mobile robots. This paper presents a loosely coupled algorithm for autonomous localization, the ICEKF (IMU-aided camera extended Kalman filter), for the weighted data fusion of the IMU and visual measurement. The algorithm fuses motion information on the velocity layer, thereby mitigating the excessive accumulation of IMU errors caused by direct subtraction on the positional layer after quadratic integration. Furthermore, by incorporating a weighting mechanism, the algorithm allows for a flexible adjustment of the emphasis placed on IMU data versus visual information, which augments the robustness and adaptability of autonomous motion estimation for robots. The simulation and dataset experiments demonstrate that the ICEKF can provide reliable estimates for robot motion trajectories.

1. Introduction

1.1. Motivation

As the demand for autonomous mobility of robots continues to increase, localization technology continues to evolve. When robots perform motion tasks in environments such as extraterrestrial planets, rugged ground terrains, aerial spaces, or maritime environments [1,2,3], they face various challenges, including unstable satellite signals, impact and vibration upon sensors, limited visual features, and unpredictable light conditions. Furthermore, autonomous localization is a requisite ability for aerial [4], ground [5,6], or maritime [3,7] autonomous robots to successfully accomplish challenging tasks of local planning aiming for navigation to prevent collisions.

Integrated visual–inertial sensors do not require active signal emission or depend on external preset references, which benefits further data fusion as autonomous odometry. Two primary fusion strategies exist to facilitate fusion: loosely coupled methods and tightly coupled methods [8].

Loosely coupled methods are computationally efficient and maintain a higher update rate than tightly coupled methods do, thereby enabling low-cost and compact implementations. This approach additionally allows the simultaneous integration of localization information from multiple sources such as visual measurement, GNSS (global navigation satellite system), LIDAR (light detection and ranging), etc., [9,10]. Especially for application on the multiagent [11], loosely coupled methods are naturally capable of processing simultaneous multi-source information. Although the estimation accuracy is relatively low owing to the lack of an optimization process, which tight-coupled methods [12,13] usually perform, the integration of multiple measurement data into a loosely coupled framework is inevitable to address the challenges of environmental uncertainty during practical utilization. Therefore, a loosely coupled data fusion approach that is capable of adjusting the weights of the measurement results from individual sensors or agents is necessary to achieve a balanced output, considering how much the information source can be trusted.

1.2. Related Work

Loosely coupled visual–inertial odometry methods are usually constructed based on filter frameworks such as the EKF [14,15]. High-confidence visual keyframe measurements are then periodically utilized to correct the high-frequency integration results from IMUs. This basic structure effectively balances the updating rate and accuracy.

Kelly and Sukhatme [16] proposed a data fusion algorithm for the self-calibration of a monocular visual–inertial system with proven observability. With a similar filtering structure, Weiss and Siegwart [17] incorporated the world coordinate system drift in visual measurements, as well as the spatial relationships between the IMU and the monocular camera, into the EKF framework. This approach enables failure detection and scale-drift estimation by fusing of measurement data on the position and attitude layers.

Achtelik and Weiss [18] constructed a filter-based framework to recover the relative configuration of two drones performing IMUs and monocular visual measurements. To address error amplification during the error propagation process in EKFs due to possible inaccurate state modeling, BROSSARD et al. [19] proposed the visual–inertial invariant EKF algorithm, which is based on Lie algebra rules. Furthermore, researchers designed visual–inertial odometry that embraces the characteristics of novel filers such as the MEKF [20] and the equivariant filter [21,22] to broaden the application of filter-based techniques in motion estimation.

The aforementioned loosely coupled strategies have demonstrated remarkable performance when mainly focused on the fusion of positional layers. However, since the high-frequency update and the intrinsic drift of IMUs inevitably result in fast error accumulation, the effect of the final correction on the position and attitude layers will be suboptimal when the visual measurement update frequency drops or accuracy deteriorates. Furthermore, these methods are thus incapable of adjusting the emphasis on either source while encountering external visual interference.

1.3. Our Approach

This paper presents a loosely coupled algorithm for autonomous localization with a weighted data fusion on the linear velocity and angular velocity layers, namely the ICEKF. The weight of the IMU measurement can be adjusted according to whether the external conditions are favorable for visual measurement, thus increasing the overall accuracy of autonomous localization. The real-time performance is promising since the time complexity is $O(n)$. The local weak observability of the ICEKF is demonstrated.

The organization of this paper is as follows: the state vector of the ICEKF is established in the second section, the design of the propagation and update of the filter is discussed in the third section, the observability analysis is presented in the fourth section, and the fifth section carries out numerical studies of simulations and dataset experiments. Finally, a summary is provided.

2. Design of the State Vector

This section discusses the construction of the state vector and analyzes the weighted coupling process on the velocity layer. The forms of the ICEKF state vector are then deducted.

2.1. Definition of Variables in the ICEKF

The descriptions of the main coordinate frames and variables used in this study are listed in

Table 1. Coordinate frames and notations in the ICEKF.

Symbol	Description
w	fixed world coordinate frame
i	coordinate frame attached to the IMU
c	coordinate frame attached to the camera
ic	coordinate frame attached to the IMU-aided camera system
${\mathit{x}}_B^A$	$\mathit{x}$ represents a general viable vector, A is the coordinate frame attached to the vector, and B is the reference frame; for example, ${\mathit{p}}_w^c$ denotes the linear translation of the camera with the frame c, measured with respect to the world frame w
$\mathit{p}$	translation vector of rigid bodies along 3 axes, of which the quasi-quaternion description is $\overline{\mathit{p}}={[0,{\mathit{p}}^T]}^T$
$\overline{\mathit{q}}$	unit quaternion according to the Hamilton notation [16], written as $\overline{\mathit{q}}={[q_0,q_1,q_2,q_3]}^T={[q_0,{\mathit{q}}^T]}^T$
${\overline{\mathit{q}}}^{\ast }$	conjugate quaternion of $\overline{\mathit{q}}$$, \mathrm{and} {\overline{\mathit{q}}}^{\ast }\otimes \overline{\mathit{q}}=1$
$\mathit{R}$	rotation matrix converted from $\overline{\mathit{q}}$$, \mathrm{such} \mathrm{as} {\mathit{R}}_w^i=\mathit{R}\left({\overline{\mathit{q}}}_w^i\right)$
$⌊\mathit{x}\times ⌋$	skew-symmetric matrix of $\mathit{x}$$, \mathrm{and} ⌊\mathit{x}\times ⌋\mathit{y}=-\mathit{x}⌊\mathit{y}\times ⌋$ [23]
$\mathit{n}$	white Gaussian noise vector with zero mean and covariance ${\mathit{\sigma }}^2$
$\mathit{g}$	gravity vector in the world frame

. The relationships between the coordinate frames of the IMU-aided camera integration system are established, as shown in Figure 1, where ${\mathit{p}}_c^i$ and ${\overline{\mathit{q}}}_c^i$ represent the relative linear translation and rotation between the IMU and the camera, which are considered constants once calibrated. The IMU outputs linear acceleration ${\mathit{a}}_{mi}$ and angular velocity ${\mathit{\omega }}_{mi}$ in its rigid body coordinate frame. This paper considers the visual black-box measurement outputting rotation ${\overline{\mathit{q}}}_w^c$ and unscaled linear translation ${\mathit{p}}_w^c$ in the world frame. The fusion result is attached to the IMU coordinate frame, with the coupled linear translation and rotation denoted as ${\mathit{p}}_w^{ic}$ and ${\overline{\mathit{q}}}_w^{ic}$, respectively. The derivatives of the translation and rotation are ${\mathit{v}}_w^{ic}$ and ${\dot{\overline{\mathit{q}}}}_w^{ic}$. The units used in this paper are described in the SI, such as acceleration in m²/s and angular velocity in rad/s.

2.2. Construction of the State Vector

Given the necessity for information fusion on the velocity layer, while containing first-order derivatives and integrals, the state of the ICEKF is defined as a column vector consisting of 30 elements, as follows:

$$\mathit{X}={\left\{{{\mathit{p}}_w^{ic}}^T {{\mathit{v}}_w^c}^T {{\mathit{v}}_w^i}^T {{\overline{\mathit{q}}}_w^{ic}}^T {{\overline{\mathit{q}}}_w^i}^T {{\mathit{\omega }}_c^c}^T {{\mathit{\omega }}_i^i}^T{{\mathit{b}}_{ai}}^T{{\mathit{b}}_{\omega i}}^T\lambda \right\}}^T,$$

(1)

where ${\mathit{v}}_w^c$ is the linear velocity of the visual measurement derived from ${\mathit{p}}_w^c$; ${\mathit{\omega }}_c^c$ is the equivalent body angular velocity of the camera measured in the IMU frame; ${\overline{\mathit{q}}}_w^i$ and ${\mathit{\omega }}_i^i$ are the rotation velocity and body angular velocity, respectively; ${\mathit{b}}_{ai}$ and ${\mathit{b}}_{\omega i}$ are the biases of ${\mathit{a}}_{mi}$ and ${\mathit{\omega }}_{mi}$, respectively; $\lambda$ is the scale coefficient of the monocular visual translation. The remaining variables are described in the last section. Let ${\mathit{a}}_w^i$ be the linear acceleration of the IMU measured in the world frame, where ${\mathit{a}}_w^i={\mathit{R}}_w^i\left({\mathit{a}}_{mi}-{\mathit{b}}_{ai}-{\mathit{n}}_{{\mathit{a}}_{mi}}\right)$. Additionally, ${\mathit{\omega }}_i^i={\mathit{\omega }}_{mi}-{\mathit{b}}_{\omega i}-{\mathit{n}}_{{\mathit{\omega }}_{mi}}$.

The relationships among the variables in the ICEKF state are shown in Figure 2.

When the visual measurement is treated as a black box, ${\mathit{v}}_w^c={\dot{\mathit{p}}}_w^c$. To obtain ${\mathit{\omega }}_c^c$, ${\overline{\mathit{q}}}_w^c$ should be transformed to ${\overline{\mathit{q}}}_w^{c^{\prime }}$, which is the visual rotation described in the IMU frame. Then, ${\mathit{\omega }}_c^c$ can be obtained from the derivatives of the Euler angles recovered from ${\overline{\mathit{q}}}_w^{c^{\prime }}$. The coupling coefficients of the linear and angular velocities, ${\mu }_v,{\mu }_{\omega }\in [0,1]$, are independent of the state update. This design guarantees that the coupling weights are always adjustable, which increases the accuracy of motion estimation without impacting observability.

To focus on the filter construction in this paper, the following is assumed:

1. The drift of the visual world frame in the visual black-box measurement is negligible due to its slow variation.

2. The outcomes acquired through the first-order derivation of measurement results are modeled as random walk, including ${\mathit{\omega }}_c^c$ and ${\mathit{v}}_w^c$.

2.3. Coupling Process

The coupling process of the linear velocities of the IMU-camera system requires a weighted summation as follows:

$${\mathit{v}}_w^{ic}={\mu }_v{\mathit{v}}_w^i+\lambda (1-{\mu }_v){\mathit{R}}_c^i{\mathit{v}}_w^c,$$

(2)

The angular velocity, which covaries with the rotation of the coordinate frames, cannot be directly summed. According to Appendix A and the proposed lemmas in Appendix B, with ${\mu }_{\omega }\in [0,1]$, the following equations hold:

$$\frac{d}{dt}{\left({\overline{\mathit{q}}}_w^i\right)}^{{\mu }_{\omega }}=\frac{1}{2}{\left({\overline{\mathit{q}}}_w^i\right)}^{{\mu }_{\omega }}\otimes \left({\mu }_{\omega }{\overline{\mathit{\omega }}}_i^i\right),$$

(3)

$$\frac{d}{dt}{\left({\overline{\mathit{q}}}_w^{c^{\prime }}\right)}^{1-{\mu }_{\omega }}=\frac{1}{2}{\left({\overline{\mathit{q}}}_w^{c^{\prime }}\right)}^{1-{\mu }_{\omega }}\otimes \left(\left(1-{\mu }_{\omega }\right){\overline{\mathit{\omega }}}_c^c\right),$$

(4)

where ${\overline{\mathit{q}}}_w^{c^{\prime }}$ is the equivalent rotation of the visual measurement in the IMU frame.

After further deduction according to Appendix A, the derivative of ${\overline{\mathit{q}}}_w^{ic}$ is written as follows:

$$\begin{array}{l}{\dot{\overline{\mathit{q}}}}_w^{ic}=\frac{d}{dt}{\left({\overline{\mathit{q}}}_w^i\right)}^{{\mu }_{\omega }}\otimes {\left({\overline{\mathit{q}}}_w^{c^{\prime }}\right)}^{1-{\mu }_{\omega }}+{\left({\overline{\mathit{q}}}_w^i\right)}^{{\mu }_{\omega }}\otimes \frac{d}{dt}{\left({\overline{\mathit{q}}}_w^{c^{\prime }}\right)}^{1-{\mu }_{\omega }}\\ =\frac{{\mu }_{\mathit{\omega }}}{2}{\left({\overline{\mathit{q}}}_w^i\right)}^{{\mu }_{\omega }}\otimes {\overline{\mathit{\omega }}}_i^i\otimes {\left({\overline{\mathit{q}}}_w^{c^{\prime }}\right)}^{1-{\mu }_{\omega }}+\frac{1-{\mu }_{\mathit{\omega }}}{2}{\overline{\mathit{q}}}_w^{ic}\otimes {\overline{\mathit{\omega }}}_c^c\end{array},$$

(5)

where $\overline{\mathit{\omega }}=[0,\mathit{\omega }]$ is the supplementary vector of the body angular vector $\mathit{\omega }$.

Angular velocity fusion according to Equation (5) is performed while assuming that ${\overline{\mathit{q}}}_w^{ic}={\overline{\mathit{q}}}_w^{c^{\prime }}={\overline{\mathit{q}}}_w^i$, which are all attached to the IMU frame. Thus, potential errors can be caused by the assembly and measurement noise for a real IMU-camera integrated rig. However, after careful calibration and the filter process, the influence of the possible inconsistency is negligible.

2.4. Simplification of the State Vector

To simplify further discussion, the subscripts of the variables described in the world coordinate system are omitted; thus, the ICEKF state vector in Equation (1) is rewritten as follows:

$$\mathit{X}={\left\{{{\mathit{p}}_{ic}}^T {{\mathit{v}}_c}^T {{\mathit{v}}_i}^T {{\overline{\mathit{q}}}_{ic}}^T {{\overline{\mathit{q}}}_i}^T{{\mathit{\omega }}_c}^T{{\mathit{\omega }}_i}^T {{\mathit{b}}_a}^T{{\mathit{b}}_{\omega }}^T\lambda \right\}}^T,$$

(6)

The derivatives of the variables in the vector are as follows:

$$\begin{gathered} {\dot{\mathit{p}}}_{ic}={\mathit{v}}_{ic}={\mu }_{\mathit{v}}{\mathit{v}}_i+\lambda (1-{\mu }_{\mathit{v}}){\mathit{R}}_c^i{\mathit{v}}_c, {\dot{\mathit{v}}}_c={\mathit{n}}_{{\mathit{v}}_c}, {\dot{\mathit{v}}}_i={\mathit{R}}_i\left({\mathit{a}}_{mi}-{\mathit{b}}_{ai}-{\mathit{n}}_{{\mathit{a}}_{mi}}\right)-\mathit{g}, \\ {\dot{\overline{\mathit{q}}}}_{ic}=\frac{{\mu }_{\omega }}{2}{{\overline{\mathit{q}}}_i}^{{\mu }_{\omega }}\otimes {\overline{\mathit{\omega }}}_i\otimes {{\overline{\mathit{q}}}_c}^{1-{\mu }_{\omega }}+\frac{1-{\mu }_{\omega }}{2}{\overline{\mathit{q}}}_{ic}\otimes {\overline{\mathit{\omega }}}_c, {\dot{\overline{\mathit{q}}}}_i=\frac{1}{2}{\overline{\mathit{q}}}_i\otimes {\overline{\mathit{\omega }}}_i, {\dot{\mathit{\omega }}}_c={\mathit{n}}_{{\mathit{\omega }}_c}, {\dot{\mathit{\omega }}}_i=\frac{d}{dt}\left({\mathit{\omega }}_{mi}-{\mathit{b}}_{{\mathit{\omega }}_i}-{\mathit{n}}_{{\mathit{\omega }}_{mi}}\right), \\ {\dot{\mathit{b}}}_{ai}={\mathit{n}}_{{\mathit{b}}_{ai}}, {\dot{\mathit{b}}}_{\omega i}={\mathit{n}}_{{\mathit{b}}_{\mathit{\omega }i}},\dot{\lambda }=n_{\lambda }, \end{gathered}$$

(7)

where ${\mathit{v}}_c$, ${\mathit{\omega }}_c$, and $\lambda$ are modeled as random walk, and notably ${\overline{\mathit{q}}}_c={\overline{\mathit{q}}}_c^i\otimes {\overline{\mathit{q}}}_w^c={\overline{\mathit{q}}}_w^{c^{\prime }}$.

2.5. Error of the State Vector

Let $\widehat{\mathit{X}}$ denote the expectation of $\mathit{X}$, and let $\tilde{\mathit{X}}$ denote the error between the expectation, and the measurement, written as $\tilde{\mathit{X}}=\mathit{X}-\widehat{\mathit{X}}$. $\tilde{\mathit{X}}$ with 28 elements, is described as follows:

$$\tilde{\mathit{X}}={\left\{\Delta {{\mathit{p}}_{ic}}^T \Delta {{\mathit{v}}_c}^T \Delta {{\mathit{v}}_i}^T \delta {{\mathit{\theta }}_{ic}}^T \delta {{\mathit{\theta }}_i}^T \Delta {{\mathit{\omega }}_c}^T \Delta {{\mathit{\omega }}_i}^T \Delta {{\mathit{b}}_{ai}}^T\Delta {{\mathit{b}}_{\omega i}}^T \Delta \lambda \right\}}^T,$$

(8)

With the small-angle assumption, when the rotation angle corresponding to a unit quaternion $\overline{\mathit{q}}$ is very small, the error of $\overline{\mathit{q}}$ is written as $\delta \overline{\mathit{q}}={\left[q_0,\delta {\mathit{q}}^T\right]}^T\approx {\left[1,\frac{1}{2}\delta {\mathit{\theta }}^T\right]}^T$ [23]. Because the algorithm operates at a high update rate, the high-order terms that yield negligible computational results are disregarded, such as $\delta \mathit{q}\cdot \Delta \mathit{\omega }$, $\delta \mathit{q}\cdot \delta \mathit{q}$, and $\delta \mathit{q}\cdot \mathit{n}$. Then, the description of the derivative $\widetilde{\dot{\mathit{X}}}$ can be inspected.

According to Equation (2), the derivative of the coupled translation error is as follows:

$$\stackrel{.}{\Delta {\mathit{p}}_{ic}}={\mu }_v\Delta {\mathit{v}}_i+\lambda (1-{\mu }_v){\mathit{R}}_c^i\Delta {\mathit{v}}_c,$$

(9)

The expectation of the linear acceleration of the IMU is ${\widehat{\mathit{a}}}_i={\mathit{a}}_{mi}-{\widehat{\mathit{b}}}_{ai}$, and $\Delta {\mathit{b}}_{ai}={\mathit{b}}_{ai}-{\widehat{\mathit{b}}}_{ai}$. According to Appendix A, the rotation can be reformed as ${\mathit{R}}_i=\mathit{R}\left({\overline{\mathit{q}}}_i\right)\approx {\widehat{\mathit{R}}}_i\left({\mathit{I}}_3+⌊\delta {\mathit{\theta }}_i\times ⌋\right)$ under the small angle assumption. After neglecting the high-order terms, the error of the linear velocity of the IMU is written as follows:

$$\begin{array}{l}\stackrel{.}{\Delta {\mathit{v}}_i}={\mathit{a}}_i-{\widehat{\mathit{a}}}_i={\mathit{R}}_i({\mathit{a}}_{mi}-{\mathit{b}}_{ai}-{\mathit{n}}_{{\mathit{a}}_{mi}})-\mathit{g}-{\widehat{\mathit{R}}}_i({\mathit{a}}_{mi}-{\widehat{\mathit{b}}}_{ai})+\mathit{g}\\ \approx -{\widehat{\mathit{R}}}_i⌊{\widehat{\mathit{a}}}_i\times ⌋\delta {\mathit{\theta }}_i-{\widehat{\mathit{R}}}_i\Delta {\mathit{b}}_{ai}-{\mathit{n}}_{{\mathit{a}}_{mi}}\end{array},$$

(10)

For general quaternions and angular velocities, $\overline{\mathit{q}}=\widehat{\overline{\mathit{q}}}\otimes \delta \overline{\mathit{q}}$ and $\overline{\mathit{\omega }}=\widehat{\overline{\mathit{\omega }}}+\Delta \overline{\mathit{\omega }}$. By subjecting ${{\overline{\mathit{q}}}_{ic}}^{\ast }={{\left({\overline{\mathit{q}}}_c\right)}^{1-{\mu }_{\omega }}}^{\ast }\otimes {{\left({\overline{\mathit{q}}}_i\right)}^{{\mu }_{\omega }}}^{\ast }$ and ${\left({{\overline{\mathit{q}}}_i}^{{\mu }_{\mathit{\omega }}}\right)}^{\ast }\otimes {{\overline{\mathit{q}}}_i}^{{\mu }_{\omega }}=1$ to Equation (5), the error of the coupled rotation can be deducted as follows:

$$\begin{array}{l}\delta {\dot{\overline{\mathit{q}}}}_{ic}={{\widehat{\overline{\mathit{q}}}}_{ic}}^{\ast }\otimes \left(\frac{{\mu }_{\omega }}{2}{{\overline{\mathit{q}}}_i}^{{\mu }_{\mathit{\omega }}}\otimes {\overline{\mathit{\omega }}}_i\otimes {{\overline{\mathit{q}}}_c}^{1-{\mu }_{\mathit{\omega }}}+\frac{1-{\mu }_{\omega }}{2}{\overline{\mathit{q}}}_{ic}\otimes {\overline{\mathit{\omega }}}_c-\frac{{\mu }_{\omega }}{2}{{\widehat{\overline{\mathit{q}}}}_i}^{{\mu }_{\mathit{\omega }}}\otimes {\widehat{\overline{\mathit{\omega }}}}_i\otimes {{\widehat{\overline{\mathit{q}}}}_c}^{1-{\mu }_{\mathit{\omega }}}\otimes \delta {\overline{\mathit{q}}}_{ic}-\frac{1-{\mu }_{\omega }}{2}{\widehat{\overline{\mathit{q}}}}_{ic}\otimes {\widehat{\overline{\mathit{\omega }}}}_c\otimes \delta {\overline{\mathit{q}}}_{ic}\right)\\ =\left[\frac{{\mu }_{\omega }}{2}\delta {\overline{\mathit{q}}}_{ic}\otimes {{\overline{\mathit{q}}}_{ic}}^{\ast }\otimes {{\overline{\mathit{q}}}_i}^{{\mu }_{\mathit{\omega }}}\otimes \left({\widehat{\overline{\mathit{\omega }}}}_i+\Delta {\overline{\mathit{\omega }}}_i\right)\otimes {{\overline{\mathit{q}}}_c}^{1-{\mu }_{\mathit{\omega }}}-\frac{{\mu }_{\omega }}{2}{\left({{\widehat{\overline{\mathit{q}}}}_c}^{1-{\mu }_{\omega }}\right)}^{\ast }\otimes {\left({{\widehat{\overline{\mathit{q}}}}_i}^{{\mu }_{\omega }}\right)}^{\ast }\otimes {{\widehat{\overline{\mathit{q}}}}_i}^{{\mu }_{\mathit{\omega }}}\otimes {\widehat{\overline{\mathit{\omega }}}}_i\otimes {{\widehat{\overline{\mathit{q}}}}_c}^{1-{\mu }_{\mathit{\omega }}}\otimes \delta {\overline{\mathit{q}}}_{ic}\right]\\ +\left[\frac{1-{\mu }_{\omega }}{2}\delta {\overline{\mathit{q}}}_{ic}\otimes \left({\widehat{\overline{\mathit{\omega }}}}_c+\Delta {\overline{\mathit{\omega }}}_c\right)-\frac{1-{\mu }_{\omega }}{2}{\widehat{\overline{\mathit{\omega }}}}_c\otimes \delta {\overline{\mathit{q}}}_{ic}\right]\\ =\left[\begin{array}{l}\frac{{\mu }_{\mathit{\omega }}}{2}\delta {\overline{\mathit{q}}}_{ic}\otimes {\left({{\overline{\mathit{q}}}_c}^{1-{\mu }_{\mathit{\omega }}}\right)}^{\ast }\otimes {\widehat{\overline{\mathit{\omega }}}}_i\otimes {{\overline{\mathit{q}}}_c}^{1-{\mu }_{\omega }}\\ +\frac{{\mu }_{\mathit{\omega }}}{2}\delta {\overline{\mathit{q}}}_{ic}\otimes {\left({{\overline{\mathit{q}}}_c}^{1-{\mu }_{\mathit{\omega }}}\right)}^{\ast }\otimes \Delta {\overline{\mathit{\omega }}}_i\otimes {{\overline{\mathit{q}}}_c}^{1-{\mu }_{\omega }}\\ -\frac{{\mu }_{\mathit{\omega }}}{2}{\left({{\widehat{\overline{\mathit{q}}}}_c}^{1-{\mu }_{\mathit{\omega }}}\right)}^{\ast }\otimes {\widehat{\overline{\mathit{\omega }}}}_i\otimes {{\widehat{\overline{\mathit{q}}}}_c}^{1-{\mu }_{\omega }}\otimes \delta {\overline{\mathit{q}}}_{ic}\end{array}\right]+\left[\begin{array}{l}\frac{1-{\mu }_{\mathit{\omega }}}{2}\delta {\overline{\mathit{q}}}_{ic}\otimes {\widehat{\overline{\mathit{\omega }}}}_c\\ +\frac{1-{\mu }_{\mathit{\omega }}}{2}\delta {\overline{\mathit{q}}}_{ic}\otimes \Delta {\overline{\mathit{\omega }}}_c\\ -\frac{1-{\mu }_{\mathit{\omega }}}{2}{\widehat{\overline{\mathit{\omega }}}}_c\otimes \delta {\overline{\mathit{q}}}_{ic}\end{array}\right]\end{array},$$

(11)

According to Appendix A, the rotation of ${\mathit{p}}_{ic}$ with respect to ${{\overline{\mathit{q}}}_c}^{1-{\mu }_{\omega }}$ is written as follows:

$${\left({{\widehat{\overline{\mathit{q}}}}_c}^{1-{\mu }_{\mathit{\omega }}}\right)}^{\ast }\otimes {\overline{\mathit{p}}}_{ic}\otimes {{\widehat{\overline{\mathit{q}}}}_c}^{1-{\mu }_{\omega }}\stackrel{\wedge}{=}\widehat{\mathit{R}}{\left({{\widehat{\overline{\mathit{q}}}}_c}^{1-{\mu }_{\mathit{\omega }}}\right)}^T{\mathit{p}}_{ic}={{\widehat{\mathit{R}}}_{{\mu }_{\omega }c}}^T{\mathit{p}}_{ic},$$

(12)

where ${{\mathit{R}}_{{\mu }_{\omega }c}}^T$ is the rotation matrix converted from ${{\overline{\mathit{q}}}_c}^{1-{\mu }_{\omega }}$.

By subjecting Equation (12) to Equation (11) and disregarding the high-order terms, the following is obtained:

$$\begin{gathered} \stackrel{.}{\delta {\overline{\mathit{q}}}_{ic}}=\left\{\begin{array}{l}\frac{{\mu }_{\omega }}{2}\delta {\overline{\mathit{q}}}_{ic}\otimes \left[\begin{array}{l}0\\ {{\mathit{R}}_{{\mu }_{\omega }c}}^T{\widehat{\mathit{\omega }}}_i\end{array}\right]\\ -\frac{{\mu }_{\omega }}{2}\left[\begin{array}{l}0\\ {{\mathit{R}}_{{\mu }_{\omega }c}}^T{\widehat{\mathit{\omega }}}_i\end{array}\right]\otimes \delta {\overline{\mathit{q}}}_{ic}\\ +\frac{{\mu }_{\omega }}{2}\delta {\overline{\mathit{q}}}_{ic}\otimes \left[\begin{array}{l}0\\ {{\mathit{R}}_{{\mu }_{\omega }c}}^T\Delta {\mathit{\omega }}_i\end{array}\right]\end{array}\right\}+\left[\begin{array}{l}\frac{1-{\mu }_{\omega }}{2}\delta {\overline{\mathit{q}}}_{ic}\otimes {\widehat{\overline{\mathit{\omega }}}}_c\\ -\frac{1-{\mu }_{\omega }}{2}{\widehat{\overline{\mathit{\omega }}}}_c\otimes \delta {\overline{\mathit{q}}}_{ic}\\ +\frac{1-{\mu }_{\omega }}{2}\delta {\overline{\mathit{q}}}_{ic}\otimes \Delta {\overline{\mathit{\omega }}}_c\end{array}\right] \\ =\frac{{\mu }_{\omega }}{2}\left\{\begin{array}{l}\left[\begin{array}{cc}0& -{\left({{\mathit{R}}_{{\mu }_{\omega }c}}^T{\widehat{\mathit{\omega }}}_i\right)}^T\\ {{\mathit{R}}_c}^T{\widehat{\mathit{\omega }}}_i& -⌊\left({{\mathit{R}}_{{\mu }_{\omega }c}}^T{\widehat{\mathit{\omega }}}_i\right)\times ⌋\end{array}\right]\left[\begin{array}{l}{\mathit{q}}_0\\ \delta {\mathit{q}}_{ic}\end{array}\right]\\ -\left[\begin{array}{cc}0& -{\left({{\mathit{R}}_{{\mu }_{\omega }c}}^T{\widehat{\mathit{\omega }}}_i\right)}^T\\ {{\mathit{R}}_c}^T{\widehat{\mathit{\omega }}}_i& ⌊\left({{\mathit{R}}_{{\mu }_{\omega }c}}^T{\widehat{\mathit{\omega }}}_i\right)\times ⌋\end{array}\right]\left[\begin{array}{l}{\mathit{q}}_0\\ \delta {\mathit{q}}_{ic}\end{array}\right]\\ +\left[\begin{array}{cc}0& -{\left({{\mathit{R}}_{{\mu }_{\omega }c}}^T\Delta {\mathit{\omega }}_i\right)}^T\\ {{\mathit{R}}_c}^T\Delta {\mathit{\omega }}_i& -⌊\left({{\mathit{R}}_{{\mu }_{\omega }c}}^T\Delta {\mathit{\omega }}_i\right)\times ⌋\end{array}\right]\left[\begin{array}{l}{\mathit{q}}_0\\ \delta {\mathit{q}}_{ic}\end{array}\right]\end{array}\right\}+\frac{1-{\mu }_{\omega }}{2}\left[\begin{array}{l}\left[\begin{array}{cc}0& -{{\widehat{\mathit{\omega }}}_c}^T\\ {\widehat{\mathit{\omega }}}_c& -⌊{\widehat{\mathit{\omega }}}_c\times ⌋\end{array}\right]\left[\begin{array}{l}{\mathit{q}}_0\\ \delta {\mathit{q}}_{ic}\end{array}\right]\\ -\left[\begin{array}{cc}0& -{{\widehat{\mathit{\omega }}}_c}^T\\ {\widehat{\mathit{\omega }}}_c& ⌊{\widehat{\mathit{\omega }}}_c\times ⌋\end{array}\right]\left[\begin{array}{l}{\mathit{q}}_0\\ \delta {\mathit{q}}_{ic}\end{array}\right]\\ +\left[\begin{array}{cc}0& -{\left(\Delta {\mathit{\omega }}_c\right)}^T\\ \Delta {\mathit{\omega }}_c& -⌊\Delta {\widehat{\mathit{\omega }}}_c\times ⌋\end{array}\right]\left[\begin{array}{l}{\mathit{q}}_0\\ \delta {\mathit{q}}_{ic}\end{array}\right]\end{array}\right] \\ \approx -\frac{{\mu }_{\omega }}{2}\left\{\left[\begin{array}{l}0\\ 2⌊\left({{\mathit{R}}_{{\mu }_{\omega }c}}^T{\widehat{\mathit{\omega }}}_i\right)\times ⌋\delta {\mathit{q}}_{ic}\end{array}\right]-\left[\begin{array}{l}0\\ {\mathit{q}}_0{{\mathit{R}}_{{\mu }_{\omega }c}}^T\Delta {\mathit{\omega }}_i\end{array}\right]\right\}-\frac{1-{\mu }_{\omega }}{2}\left\{\left[\begin{array}{l}0\\ 2⌊{\widehat{\mathit{\omega }}}_c\times ⌋\delta {\mathit{q}}_{ic}\end{array}\right]-\left[\begin{array}{l}0\\ {\mathit{q}}_0\Delta {\mathit{\omega }}_c\end{array}\right]\right\}, \end{gathered}$$

(13)

Then, with the small angle assumption $\delta {\overline{\mathit{q}}}_{ic}={\left[q_0,\delta {{\mathit{q}}_{ic}}^T\right]}^T\approx {\left[1,\frac{1}{2}\delta {{\mathit{\theta }}_{ic}}^T\right]}^T$, the error of the coupled equivalent Euler angle is simplified as follows:

$$\stackrel{.}{\delta {\mathit{\theta }}_{ic}}=-{\mu }_{\omega }\left[⌊\left({{\mathit{R}}_{{\mu }_{\omega }c}}^T{\widehat{\mathit{\omega }}}_i\right)\times ⌋\delta {\mathit{\theta }}_{ic}-{{\mathit{R}}_{{\mu }_{\omega }c}}^T\Delta {\mathit{\omega }}_i\right]-\left(1-{\mu }_{\omega }\right)\left[⌊{\widehat{\mathit{\omega }}}_c\times ⌋\delta {\mathit{\theta }}_{ic}-\Delta {\mathit{\omega }}_c\right],$$

(14)

The expectation of the body angular velocity of the IMU is ${\widehat{\mathit{\omega }}}_i={\mathit{\omega }}_{mi}-{\widehat{\mathit{b}}}_{\omega i}$, and $\Delta {\mathit{b}}_{\omega i}={\mathit{b}}_{\omega i}-{\widehat{\mathit{b}}}_{\omega i}$. Resembling the process above, the error of the rotation of the IMU is written as follows:

$$\begin{array}{l}\stackrel{.}{\delta {\overline{\mathit{q}}}_i}={{\widehat{\overline{\mathit{q}}}}_i}^{\ast }\otimes (\stackrel{.}{{\overline{\mathit{q}}}_i}-\stackrel{.}{{\widehat{\overline{\mathit{q}}}}_i}\otimes \delta {\overline{\mathit{q}}}_i)=\frac{1}{2}\delta {\overline{\mathit{q}}}_i\otimes ({\widehat{\overline{\mathit{\omega }}}}_i+\Delta {\overline{\mathit{\omega }}}_i)-\frac{1}{2}{\widehat{\overline{\mathit{\omega }}}}_i\otimes \delta {\overline{\mathit{q}}}_i\\ =\frac{1}{2}\left[\begin{array}{cc}0& -{\widehat{\mathit{\omega }}}_i\\ {\widehat{\mathit{\omega }}}_i& -⌊{\widehat{\mathit{\omega }}}_i\times ⌋\end{array}\right]\left[\begin{array}{l}{q}_{i0}\\ \delta {\mathit{q}}_i\end{array}\right]-\frac{1}{2}\left[\begin{array}{cc}0& -{\widehat{\mathit{\omega }}}_i\\ {\widehat{\mathit{\omega }}}_i& ⌊{\widehat{\mathit{\omega }}}_i\times ⌋\end{array}\right]\left[\begin{array}{l}{q}_{i0}\\ \delta {\mathit{q}}_i\end{array}\right]+\frac{1}{2}\left[\begin{array}{cc}0& -\Delta {\mathit{\omega }}_i\\ \Delta {\mathit{\omega }}_i& -⌊\Delta {\mathit{\omega }}_i\times ⌋\end{array}\right]\left[\begin{array}{l}{q}_{i0}\\ \delta {\mathit{q}}_i\end{array}\right]\\ \approx \left[\begin{array}{l}0\\ -⌊{\widehat{\mathit{\omega }}}_i\times ⌋\delta {\mathit{q}}_i\end{array}\right]-\left[\begin{array}{l}0\\ \frac{1}{2}{q}_{i0}\Delta {\mathit{\omega }}_i\end{array}\right]\end{array},$$

(15)

Equation (15) can be simplified with the small angle assumption as follows:

$$\stackrel{.}{\delta {\mathit{\theta }}_i}=-⌊{\widehat{\mathit{\omega }}}_i\times ⌋\delta {\mathit{\theta }}_i-\Delta {\mathit{\omega }}_i=-⌊{\widehat{\mathit{\omega }}}_i\times ⌋\delta {\mathit{\theta }}_i-\Delta {\mathit{b}}_{\mathit{\omega }i}-{\mathit{n}}_{{\mathit{\omega }}_{mi}},$$

(16)

The derivatives of the remaining terms in $\tilde{\mathit{X}}$ are as follows:

$${\stackrel{.}{\Delta \mathit{v}}}_c={\mathit{n}}_{{\mathit{v}}_c},\stackrel{.}{\Delta {\mathit{\omega }}_c}={\mathit{n}}_{{\mathit{\omega }}_c},\stackrel{.}{\Delta {\mathit{\omega }}_i}=\frac{d}{dt}\left(\Delta {\mathit{b}}_{\omega i}+{\mathit{n}}_{{\mathit{\omega }}_{mi}}\right)={\mathit{n}}_{\omega i},\stackrel{.}{\Delta {\mathit{b}}_{ai}}={\mathit{n}}_{{\mathit{b}}_{ai}},\stackrel{.}{\Delta {\mathit{b}}_{\omega i}=}{\mathit{n}}_{{\mathit{b}}_{\omega i}},\stackrel{.}{\Delta \lambda }=n_{\lambda },$$

(17)

3. Propagation and Update of the ICEKF

The propagation and update of the ICEKF are described in detail in this section. The key matrices in the propagation step determine the internal transition process of the filter. The update step rectifies the filtered outcomes in reference to the measurement results.

3.1. Propagation

For the linearized continuous-time errors of an ICEKF state, the following equation exists [24]:

$$\widetilde{\dot{\mathit{X}}}={\mathit{F}}_c\tilde{\mathit{X}}+{\mathit{G}}_c\mathit{n},$$

(18)

where $\mathit{n}={[{\mathit{n}}_{{\mathit{\omega }}_c}^T {\mathit{n}}_{{\mathit{a}}_{mi}}^T {\mathit{n}}_{{\mathit{\omega }}_c}^T {\mathit{n}}_{{\mathit{\omega }}_{mi}}^T {\mathit{n}}_{{\mathit{b}}_{\mathit{a}i}}^T {\mathit{n}}_{{\mathit{b}}_{\mathit{\omega }i}}^T]}^T$ is the propagation noise vector for the ICEKF following the Gaussian distribution. ${\mathit{F}}_c$ and ${\mathit{G}}_c$ are considered constant during every iteration.

To discretize Equation (18) during the period $\Delta t$, we write the description of the discrete state transition matrix ${\mathit{F}}_d$ and the discrete noise covariance matrix ${\mathit{Q}}_d$ as follows [24]:

$$\left\{\begin{array}{l}{\mathit{F}}_d=\exp ({\mathit{F}}_c\Delta t)={\mathbf{I}}_d+{\mathit{F}}_c\Delta t+\frac{1}{2!}{\mathit{F}}_c^2\Delta t^2+\dots \\ {\mathit{Q}}_d={\displaystyle {\int }_t^{t+\Delta t}{\mathit{F}}_d(\tau ){\mathit{G}}_c{\mathit{Q}}_c{\mathit{G}}_c^T{\mathit{F}}_d{(\tau )}^{T}d\tau }\end{array}\right.,$$

(19)

where ${\mathit{Q}}_c=diag({\mathit{\sigma }}_{{\mathit{n}}_{{\mathit{v}}_c}}^2,{\mathit{\sigma }}_{{\mathit{n}}_{{\mathit{a}}_i}}^2,{\mathit{\sigma }}_{{\mathit{n}}_{{\mathit{\omega }}_c}}^2,{\mathit{\sigma }}_{{\mathit{n}}_{{\mathit{\omega }}_i}}^2,{\mathit{\sigma }}_{{\mathit{n}}_{{\mathit{b}}_{ai}}}^2,{\mathit{\sigma }}_{{\mathit{n}}_{{\mathit{b}}_{\omega \mathrm{i}}}}^2)$ is the diagonal matrix converted from Gaussian noise.

Section 2.5 presents the specific expression of $\widetilde{\dot{\mathit{X}}}$. Considering that the algorithm operates at a high frequency, only the first-order expansion in Equation (19) is considered. The complete expression of ${\mathit{F}}_d$ is derived as follows:

$${\mathit{F}}_d={\left[\begin{array}{l}{\mathit{I}}_3 A_1 A_2\hspace{1em}\hspace{1em}\hspace{1em}\dots \hspace{1em}\hspace{1em}{\mathbf{0}}_{3\times 19}\\ {\mathbf{0}}_3 {\mathit{I}}_3 \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\dots \hspace{1em}\hspace{1em}{\mathbf{0}}_{3\times 22}\\ {\mathbf{0}}_3 {\mathbf{0}}_3 {\mathit{I}}_3 {\mathbf{0}}_3 A_3 {\mathbf{0}}_3 {\mathbf{0}}_3 A_4 {\mathbf{0}}_3 0_{3\times 1}\\ {\mathbf{0}}_3 {\mathbf{0}}_3 {\mathbf{0}}_3 A_5 {\mathbf{0}}_3 A_6 A_7 {\mathbf{0}}_3 {\mathbf{0}}_3 0_{3\times 1}\\ {\mathbf{0}}_3 {\mathbf{0}}_3 {\mathbf{0}}_3 {\mathbf{0}}_3 A_8 {\mathbf{0}}_3 {\mathbf{0}}_3 {\mathbf{0}}_3 A_9 0_{3\times 1}\\ {\mathbf{0}}_{13\times 15} \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\dots \hspace{1em}\hspace{1em}{\mathit{I}}_{13\times 13} \end{array}\right]}_{28\ast 28},$$

(20)

where $A_1=\lambda (1-{\mu }_v){\mathit{R}}_c^i\Delta t$, $A_2={\mu }_v{\mathit{I}}_3\Delta t$, $A_3=-{\widehat{\mathit{R}}}_i⌊{\widehat{\mathsf{a}}}_i\times ⌋\Delta t$, $A_4=-{\widehat{\mathit{R}}}_i\Delta t$, $A_5={\mathit{I}}_3-\left({\mu }_{\omega }⌊\left({{\mathit{R}}_{{\mu }_{\omega }c}}^T{\widehat{\mathit{\omega }}}_i\right)\times ⌋+\left(1-{\mu }_{\omega }\right)⌊{\widehat{\mathit{\omega }}}_c\times ⌋\right)\Delta t$, $A_6=\left(1-{\mu }_{\omega }\right){\mathit{I}}_3\Delta t$, $A_7={\mu }_{\omega }{{\mathit{R}}_{{\mu }_{\omega }c}}^T\Delta t$, $A_8={\mathit{I}}_3-⌊{\widehat{\mathit{\omega }}}_{\mathit{I}}\times ⌋\Delta t$, and $A_9=-{\mathit{I}}_3\Delta t$.

${\mathit{Q}}_d$ can be further derived by combining Equation (20) and ${\mathit{G}}_c$, and the explicit form of ${\mathit{G}}_c$ is recovered according to Equation (18) as follows:

$${\mathit{G}}_c={\left[\begin{array}{l}{0}_3 0_3 0_3 0_3 0_3 0_3\\ I_3 0_3 0_3 0_3 0_3 0_3\\ 0_3-{\widehat{\mathit{R}}}_i 0_3 0_3 0_3 0_3\\ 0_3 0_3 0_3 0_3 0_3 0_3\\ 0_3 0_3 0_3-I_3 0_3 0_3\\ 0_3 0_3 I_3 0_3 0_3 0_3\\ 0_3 0_3 0_3 0_3 I_3 0_3\\ \hspace{1em}\hspace{1em}{0}_{7\times 18}\hspace{1em}\hspace{1em}\end{array}\right]}_{28\ast 18}.$$

(21)

3.2. Measurement

Let the state measurement matrix at the $k$-th step of the ICEKF be ${\mathit{z}}_k$. After the measured linear translation ${\mathit{z}}_P$, the attitude quaternion ${\overline{\mathit{z}}}_q$, the linear velocity ${\mathit{z}}_v$, and the angular velocity ${\mathit{z}}_{\omega }$ are obtained by comparing the results from the keyframes of the visual measurement with the expectation of the ICEKF, ${\mathit{z}}_k$ is obtained as follows:

$${\tilde{\mathit{z}}}_k={\left[{{\tilde{\mathit{z}}}_p}^T{{\widetilde{\overline{\mathit{z}}}}_q}^T{{\tilde{\mathit{z}}}_{\mathit{v}}}^T{{\tilde{\mathit{z}}}_{\omega }}^T\right]}^T,$$

(22)

For the error of the linear translation, the following holds:

$$\begin{array}{l}{\tilde{\mathit{z}}}_p=({\mathit{p}}_{ic}-{\mathit{R}}_{ic}{\mathit{p}}_i^c)\lambda +{\mathit{n}}_p-({\widehat{\mathit{p}}}_{ic}-{\widehat{\mathit{R}}}_{ic}{\mathit{p}}_i^c)\widehat{\lambda }\\ =\left({\mathit{p}}_{ic}\lambda -{\widehat{\mathit{p}}}_{ic}\lambda \right)+\left({\widehat{\mathit{p}}}_{ic}\lambda -{\widehat{\mathit{p}}}_{ic}\widehat{\lambda }\right)+\left[{\widehat{\mathit{R}}}_{ic}{\mathit{p}}_i^c\lambda -{\widehat{\mathit{R}}}_{ic}\left({\mathit{I}}_3+⌊\delta {\mathit{\theta }}_{ic}\times ⌋\right){\mathit{p}}_i^c\lambda \right]+\left(-{\widehat{\mathit{R}}}_{ic}{\mathit{p}}_i^c\lambda +{\widehat{\mathit{R}}}_{ic}{\mathit{p}}_i^c\widehat{\lambda }\right)+{\mathit{n}}_p\\ =\lambda \Delta \mathit{p}+{\widehat{\mathit{R}}}_{ic}⌊\left({\mathit{p}}_i^c\lambda \right)\times ⌋\delta {\mathit{\theta }}_{ic}+\left({\widehat{\mathit{p}}}_{ic}-{\widehat{\mathit{R}}}_{ic}{\mathit{p}}_i^c\right)\Delta \lambda +{\mathit{n}}_p\end{array},$$

(23)

For the error of the attitude quaternion considering the small angle assumption, the following equation is used:

$${\widetilde{\overline{\mathit{z}}}}_{\mathit{q}}={\widehat{\overline{\mathit{q}}}}_{ic}^{\ast }\otimes \left({\overline{\mathit{q}}}_c^i\otimes {\overline{\mathit{q}}}_c\right)=\delta {\overline{\mathit{q}}}_{ic}\approx \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathit{\theta }}_{ic}+{\mathit{n}}_{\mathit{\theta }}\end{array}\right],$$

(24)

For the error of the linear velocity, the following holds:

$$\begin{array}{l}{\tilde{\mathit{z}}}_{\mathit{v}}={\mathit{v}}_{ic}-{\widehat{\mathit{v}}}_{ic}\\ ={\mu }_{\mathit{v}}\stackrel{.}{\Delta {\mathit{v}}_i}\Delta t+\lambda (1-{\mu }_{\mathit{v}}){\mathit{R}}_c^i\Delta {\mathit{v}}_c\\ =-{\mu }_{\mathit{v}}\Delta t{\widehat{\mathit{R}}}_i⌊{\widehat{\mathit{a}}}_i\times ⌋\delta {\mathit{\theta }}_i-{\mu }_{\mathit{v}}\Delta t{\widehat{\mathit{R}}}_i\Delta {\mathit{b}}_a+\lambda (1-{\mu }_{\mathit{v}}){\mathit{R}}_c^i\Delta {\mathit{v}}_c+{\mathit{n}}_{\mathit{v}}\end{array},$$

(25)

Since the angular velocities in different coordinate frames cannot be directly subtracted, the measurement error can be indirectly obtained according to Equation (14) as follows:

$$\begin{array}{l}{\tilde{\mathit{z}}}_{\omega }=\stackrel{.}{\delta {\mathit{\theta }}_{ic}}\Delta t+{\mathit{n}}_{\omega }\\ =-\Delta t\left[{\mu }_{\omega }⌊\left({{\mathit{R}}_{{\mu }_{\omega }c}}^T{\widehat{\mathit{\omega }}}_i\right)\times ⌋+\left(1-{\mu }_{\mathit{\omega }}\right)⌊{\widehat{\mathit{\omega }}}_c\times ⌋\right]\delta {\mathit{\theta }}_{ic}+\left(1-{\mu }_{\omega }\right)\Delta t\Delta {\mathit{\omega }}_c+{\mu }_{\omega }\Delta t{{\mathit{R}}_{{\mu }_{\omega }c}}^T\Delta {\mathit{\omega }}_i+{\mathit{n}}_{\omega }\end{array},$$

(26)

Based on the description of $\tilde{\mathit{X}}$ in Equation (8) and the above expansion of the measurement process, under the small angle assumption, the measurement error is reformulated as follows:

$${\tilde{\mathit{z}}}_k\simeq {\mathit{H}}_k{\tilde{\mathit{X}}}_k+{\mathit{n}}_m,$$

(27)

where ${\mathit{H}}_k$ is the measurement matrix, and the measurement noise is simplified as ${\mathit{n}}_m={\left[{{\mathit{n}}_p}^T{{\mathit{n}}_{\mathit{\theta }}}^T{{\mathit{n}}_{\mathit{v}}}^T{{\mathit{n}}_{\omega }}^T\right]}^T$. Thus, ${\mathit{H}}_k$ can be recovered from Equations (22)–(26) as follows:

$${\mathit{H}}_k={\left[\begin{array}{l}\lambda {\mathit{I}}_{3\times 3}\hspace{1em}\hspace{1em}\hspace{1em}{\mathbf{0}}_{3\times 6}\hspace{1em}\hspace{1em}\hspace{1em}{\mathit{B}}_1\hspace{1em}\hspace{1em}\hspace{1em}{\mathbf{0}}_{3\times 15}\hspace{1em}{\mathit{B}}_2\\ {\mathbf{0}}_{3\times 9}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\frac{1}{2}{I}_{3\times 3}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{\mathbf{0}}_{3\times 16}\\ {\mathbf{0}}_{3\times 3} {\mathit{B}}_3 {\mathbf{0}}_{3\times 6} {\mathit{B}}_4\hspace{1em}\hspace{1em}{\mathbf{0}}_{3\times 6}\hspace{1em}\hspace{1em}-{\mu }_{\mathit{v}}\Delta t {\widehat{\mathit{R}}}_i\hspace{1em}\hspace{1em}{\mathbf{0}}_{3\times 4}\\ {\mathbf{0}}_{3\times 9} {\mathit{B}}_5 {\mathbf{0}}_{3\times 3} \left(1-{\mu }_{\omega }\right)\Delta t{\mathit{I}}_{3\times 3}\hspace{1em}{\mu }_{\mathit{\omega }}\Delta t{{\mathit{R}}_{{\mu }_{\omega }c}}^T\hspace{1em}{\mathbf{0}}_{3\times 7} \end{array}\right]}_{12\ast 28},$$

(28)

where ${\mathit{B}}_1={\widehat{\mathit{R}}}_{ic}⌊\left({\mathit{p}}_i^c\lambda \right)\times ⌋$, ${\mathit{B}}_2={\widehat{\mathit{p}}}_{ic}-{\widehat{\mathit{R}}}_{ic}{\mathit{p}}_i^c$, ${\mathit{B}}_3=\lambda \Delta t(1-{\mu }_v){\mathit{R}}_c^i$, ${\mathit{B}}_4=-{\mu }_v\Delta t{\widehat{\mathit{R}}}_i⌊{\widehat{\mathit{a}}}_i\times ⌋$ and ${\mathit{B}}_5=-\Delta t\left[{\mu }_{\omega }⌊\left({{\mathit{R}}_{{\mu }_{\omega }c}}^T{\widehat{\mathit{\omega }}}_i\right)\times ⌋+\left(1-{\mu }_{\omega }\right)⌊{\widehat{\mathit{\omega }}}_c\times ⌋\right]$.

3.3. Entire ICEKF Process

The entire process of the ICEKF in the k-th iteration is presented as follows:

Step 1. By calculating ${\mathit{F}}_d$ and ${\mathit{Q}}_d$ according to Equation (19), the prior covariance matrix of errors ${\mathit{P}}_{k+1|k}$ can be obtained from the following:

$${\mathit{P}}_{k+1|k}={\mathit{F}}_d{\mathit{P}}_{k|k}{\mathit{F}}_d+{\mathit{Q}}_d,$$

(29)

Step 2. The Kalman gain matrix ${\mathit{K}}_k$ is updated as follows:

$$\left\{\begin{array}{l}{\mathit{S}}_k={\mathit{H}}_k{\mathit{P}}_{k+1|k}{\mathit{H}}_k^T+{\mathit{R}}_k\\ {\mathit{K}}_k={\mathit{P}}_{k+1|k}{\mathit{H}}_k^T{\mathit{S}}_k^{-1}\end{array}\right.,$$

(30)

where ${\mathit{R}}_k$ is the measurement noise matrix.

Step 3. The current state ${\widehat{\mathit{X}}}_{k+1|k+1}={\widehat{\mathit{X}}}_{k+1|k}+{\widehat{\tilde{\mathit{X}}}}_k$ is calculated according to ${\widehat{\tilde{\mathit{X}}}}_k={\mathit{K}}_k{\tilde{\mathit{z}}}_k$, where ${\tilde{\mathit{z}}}_k$ is obtained via Equation (27).

Step 4. The posterior covariance matrix of errors ${\mathit{P}}_{k+1|k+1}$ is updated, and Step 1 is carried out again according to the following:

$${\mathit{P}}_{k+1|k+1}=({\mathit{I}}_{28\times 28}-{\mathit{K}}_k{\mathit{H}}_k){\mathit{P}}_{k+1|k}{({\mathit{I}}_{28\times 28}-{\mathit{K}}_k{\mathit{H}}_k)}^T+{\mathit{K}}_k{\mathit{R}}_k{\mathit{K}}_k^T,$$

(31)

During Step 2, the measurement error model of the visual measurement can be preestablished as described in [25,26]. While updating the attitude during Step 3, the angular error of rotation in ${\widehat{\tilde{X}}}_k$ is achieved with the small angle assumption. Therefore, the quaternion form of the attitude expectation ${\widehat{\overline{\mathit{q}}}}_{k+1}$ must be recovered as follows [23]:

$$\left\{\begin{array}{l}\delta {\widehat{\mathit{q}}}_k\approx \frac{1}{2}\delta {\widehat{\mathit{\theta }}}_k\\ {\widehat{\overline{\mathit{q}}}}_{k+1}=\left\{\begin{array}{l}{\left[\sqrt{1-\delta {{\widehat{\mathit{q}}}_k}^T\delta {\widehat{\mathit{q}}}_k},\delta {{\widehat{\mathit{q}}}_k}^T\right]}^T \mathrm{if} \delta {{\widehat{\mathit{q}}}_k}^T\delta {\widehat{\mathit{q}}}_k\le 1 \\ \frac{1}{\sqrt{1+\delta {{\widehat{\mathit{q}}}_k}^T\delta {\widehat{\mathit{q}}}_k}}{\left[1,\delta {{\widehat{\mathit{q}}}_k}^T\right]}^T \mathrm{if} \delta {{\widehat{\mathit{q}}}_k}^T\delta {\widehat{\mathit{q}}}_k>1 \end{array}\right.\end{array}\right.,$$

(32)

For a clearer view of the entire process, the data flow during the k-th iteration is illustrated by Figure 3.

4. Nonlinear Observability Analysis

The nonlinear system with an ICEKF can function properly if it has local weak observability, as described in [27]. To simplify the observability analysis with angular velocity layer fusion, a pair of virtual coupled measurement variables are defined as angular velocity ${\mathit{\omega }}_{mic}$ with bias ${\mathit{b}}_{{\mathit{\omega }}_{ic}}$, which can be regarded as the yields obtained from correcting ${\mathit{\omega }}_{mi}$ and ${\mathit{b}}_{{\mathit{\omega }}_i}$ with visual measurement. Thus, referring to the observation system modeling in [16], the nonlinear system representing the fusion measurement results can be expressed as follows:

$$\dot{\mathit{X}}=\left[\begin{array}{l}{\dot{\mathit{p}}}_{ic}\\ {\dot{\mathit{v}}}_c\\ {\dot{\mathit{v}}}_i\\ {\dot{\overline{\mathit{q}}}}_{ic}\\ {\dot{\overline{\mathit{q}}}}_i\\ {\dot{\mathit{\omega }}}_c\\ {\dot{\mathit{\omega }}}_i\\ {\dot{\mathit{b}}}_{{\mathit{a}}_i}\\ {\dot{\mathit{b}}}_{{\mathit{\omega }}_i}\\ \dot{\lambda }\end{array}\right]=\stackrel{f_0}{\overbrace{\left[\begin{array}{l}{\mu }_{\mathit{v}}{\mathit{v}}_i+\lambda (1-{\mu }_{\mathit{v}}){\mathit{R}}_c^i{\mathit{v}}_c\\ 0_{3\times 1}\\ -{\mathit{R}}_i{\mathit{b}}_{{\mathit{a}}_i}-\mathit{g}\\ \frac{1}{2}\Xi \left({\dot{\overline{\mathit{q}}}}_{ic}\right){\mathit{b}}_{{\mathit{\omega }}_{ic}}\\ \frac{1}{2}\Xi \left({\dot{\overline{\mathit{q}}}}_i\right){\mathit{b}}_{{\mathit{\omega }}_i}\\ 0_{3\times 1}\\ 0_{3\times 1}\\ 0_{3\times 1}\\ 0_{3\times 1}\\ 0\end{array}\right]}}+\stackrel{f_1}{\overbrace{\left[\begin{array}{l}{0}_{3\times 3}\\ 0_{3\times 3}\\ 0_{3\times 3}\\ 0_{4\times 3}\\ \frac{1}{2}\Xi \left({\dot{\overline{\mathit{q}}}}_i\right)\\ 0_{3\times 3}\\ 0_{3\times 3}\\ 0_{3\times 3}\\ 0_{3\times 3}\\ 0\end{array}\right]}}{\mathit{\omega }}_{mi}+\stackrel{f_2}{\overbrace{\left[\begin{array}{l}{0}_{3\times 3}\\ 0_{3\times 3}\\ 0_{3\times 3}\\ \frac{1}{2}\Xi \left({\dot{\overline{\mathit{q}}}}_{ic}\right)\\ 0_{4\times 3}\\ 0_{3\times 3}\\ 0_{3\times 3}\\ 0_{3\times 3}\\ 0_{3\times 3}\\ 0\end{array}\right]}}{\mathit{\omega }}_{mic}+\stackrel{f_3}{\overbrace{\left[\begin{array}{l}{0}_{3\times 3}\\ 0_{3\times 3}\\ {\mathit{R}}_i\\ 0_{4\times 3}\\ 0_{4\times 3}\\ 0_{3\times 3}\\ 0_{3\times 3}\\ 0_{3\times 3}\\ 0_{3\times 3}\\ 0\end{array}\right]}}{\mathit{a}}_{mi},$$

(33)

where for a general unit quaternion $\overline{\mathit{q}}$, there is the following:

$$\left\{\begin{array}{l}\dot{\overline{\mathit{q}}}=0.5\Xi \left(\dot{\overline{\mathit{q}}}\right)\mathit{\omega }\\ \Xi \left(\overline{\mathit{q}}\right)=\left[\begin{array}{c}-{\mathit{q}}^T\\ {\mathit{q}}_0{I}_3-⌊\mathit{q}\times ⌋\end{array}\right]\end{array}\right.,$$

(34)

The measurement functions are designed as $\mathit{h}\left(\dot{\mathit{X}}\right)={\left[{{\mathit{h}}_1}^T,\dots ,{{\mathit{h}}_8}^T\right]}^T$, with ${\mathit{h}}_1=({\mathit{p}}_{ic}-{\mathit{R}}_{ic}{\mathit{p}}_i^c)\lambda$, ${\mathit{h}}_2={\overline{\mathit{q}}}_{ic}$, ${\mathit{h}}_3={\overline{\mathit{q}}}_i$, ${\mathit{h}}_4={{\overline{\mathit{q}}}_{ic}}^T{\overline{\mathit{q}}}_{ic}$, ${\mathit{h}}_5={{\overline{\mathit{q}}}_i}^T{\overline{\mathit{q}}}_i$, ${\mathit{h}}_6={\mathit{v}}_c$, ${\mathit{h}}_7={\mathit{\omega }}_c$, and ${\mathit{h}}_8={\mathit{\omega }}_i$.

According to the detailed deduction in Appendix C, the observability matrix of the system is constructed with the Lie derivative as follows:

$$\mathbf{\Omega }=\left[\begin{array}{l}\nabla L^0{\mathit{h}}_1\\ \nabla L^0{\mathit{h}}_2\\ \nabla L^0{\mathit{h}}_3\\ \nabla L^0{\mathit{h}}_4\\ \nabla L^0{\mathit{h}}_5\\ \nabla L^0{\mathit{h}}_6\\ \nabla L^0{\mathit{h}}_7\\ \nabla L^0{\mathit{h}}_8\\ \nabla L_{f_0}^1{\mathit{h}}_1\\ \nabla L_{f_0}^1{\mathit{h}}_3\\ \nabla L_{f_0}^2{\mathit{h}}_1\\ \nabla L_{f_3}^1{L}_{f_0}^1{\mathit{h}}_1\end{array}\right]={\left[\begin{array}{cccccccccc}\stackrel{{\mathit{p}}_{ic}}{\overbrace{{\mathit{U}}_1}}& \stackrel{{\mathit{v}}_c}{\overbrace{\mathbf{0}}}& \stackrel{{\mathit{v}}_i}{\overbrace{\mathbf{0}}}& \stackrel{{\overline{\mathit{q}}}_{ic}}{\overbrace{{\mathit{U}}_2}}& \stackrel{{\overline{\mathit{q}}}_i}{\overbrace{\mathbf{0}}}& \stackrel{{\mathit{\omega }}_c}{\overbrace{\mathbf{0}}}& \stackrel{{\mathit{\omega }}_i}{\overbrace{\mathbf{0}}}& \stackrel{{\mathit{b}}_{\mathit{a}}}{\overbrace{\mathbf{0}}}& \stackrel{{\mathit{b}}_{\omega }}{\overbrace{\mathbf{0}}}& \stackrel{\lambda }{\overbrace{{\mathit{U}}_3}}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{U}}_4& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{U}}_5& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{U}}_6& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{U}}_6& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\mathit{U}}_8& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{U}}_9& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{U}}_{10}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\mathit{U}}_{11}& {\mathit{U}}_{12}& {\mathit{G}}_{[9,4]}& \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{G}}_{[9,8]}& \mathbf{0}& {\mathit{G}}_{[9,10]}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{U}}_{13}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{G}}_{[11,4]}& {\mathit{G}}_{[11,5]}& \mathbf{0}& \mathbf{0}& {\mathit{U}}_{15}& {\mathit{G}}_{[11,9]}& {\mathit{G}}_{[11,10]}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{G}}_{[12,4]}& {\mathit{G}}_{[12,5]}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{U}}_{16}\end{array}\right]}_{38\times 30},$$

(35)

where each column corresponds to the entries in the state vector of the ICEKF, the matrices $\mathit{G}$ with indices represent blocks that are irrelevant to the rank analysis, and the matrices $\mathit{U}$ with subscripts are blocks contributing to the column rank of $\mathbf{\Omega }$, as follows: ${\mathit{U}}_1={\mathbf{I}}_3\lambda$, ${\mathit{U}}_4,{\mathit{U}}_5={\mathbf{I}}_4$, ${\mathit{U}}_8,{\mathit{U}}_9,{\mathit{U}}_{10}={\mathbf{I}}_3$, ${\mathit{U}}_{12}=\lambda {\mu }_{\mathit{v}}{\mathbf{I}}_3$, ${\mathit{U}}_{14}=0.5\Xi \left({\dot{\overline{\mathit{q}}}}_i\right)$, ${\mathit{U}}_{15}=-\lambda {\mu }_{\mathit{v}}{\mathit{R}}_i$, and ${\mathit{U}}_{16}=\mathit{U}\left({\mathit{R}}_i\right)$.

To prove that $\mathbf{\Omega }$ has full rank, block Gaussian elimination is applied. The rows of blocks can be rearranged so that only one block in each row is allowed to determine whether the corresponding column of blocks has full column rank. Following this process, all the block columns in $\mathbf{\Omega }$, with the exception of the last one corresponding to ${\mathit{U}}_{16}$, have full column rank when $\lambda ,{\mu }_{\mathit{v}}$ are nonzero.

Expanding the rotation matrix ${\mathit{R}}_i$ as three columns yields the following:

$$L_{f_3}^1{L}_{f_0}^1{\mathit{h}}_1=\lambda {\mu }_{\mathit{v}}{\mathit{R}}_i=\lambda {\mu }_{\mathit{v}}\left[{\mathit{r}}_x{\mathit{r}}_y{\mathit{r}}_z\right],$$

(36)

An explicit form is achieved by inspecting the Lie derivative of Equation (36) as ${\mathit{U}}_{16}={\mu }_{\mathit{v}}{{\left[{{\mathit{r}}_x}^T{{\mathit{r}}_y}^T{{\mathit{r}}_z}^T\right]}^T}_{9\times 1}$. As described in [16], if the linear acceleration was excited on at least one axis, one of ${\mathit{r}}_x$, ${\mathit{r}}_y$, or ${\mathit{r}}_z$ would be a nonzero vector.

In accordance with the analysis above, it can be demonstrated that for the visual–inertial system with the ICEKF described in this paper, when $\lambda ,{\mu }_{\mathit{v}}$ are nonzero and the IMU is excited in any direction, the observability matrix has full column rank, which means that the system has local weak observability [27]. The above conditions are readily fulfilled during practical applications.

5. Simulation and Experiments

Three-dimensional motion simulation and dataset experiments are conducted in this section to analyze the performance of the ICEKF.

5.1. Simulation

Assuming that the coordinate frames of the IMU and the camera are aligned, the pose transformation between them can be omitted. To observe the convergence process, we deliberately assign random initial values to the prior covariance matrix ${\mathit{P}}_{k|k}$. The starting point is [1,0,0]. The covariance matrix of measurement error ${\mathit{R}}_k$ is designed as a skew-symmetric matrix with small random values. Gaussian noise is introduced to all the virtual measurement values. The coefficients are set as ${\mu }_v=0.5$, ${\mu }_{\omega }=0.5$, and $\lambda =1$. The virtual IMU-camera rig is directed to move along a virtual helical trajectory, as described by the following equation:

$$\left\{\begin{array}{l}x=\cos \left(\frac{1}{2}\pi t\right)\\ y=\sin \left(\frac{1}{2}\pi t\right)\\ z=t\end{array}\right.,$$

(37)

The three-dimensional position curves of the ICEKF and the ground truth are shown in Figure 4, which reveals that the estimated results align closely with the true values. The position errors of the ICEKF estimation against the ground truth of the IMU-camera rig along the three axes are depicted in Figure 5, and Figure 6 shows the orientation errors. The curve of $\lambda$ is shown in Figure 7.

The norm errors between the ground truth and the simulation results, including the statistical values of the RMSE (root mean square error), mean error, and STD (standard deviation) for both coupled translation ${\mathit{p}}_{ic}$ and attitude ${\overline{\mathit{q}}}_{ic}$ across all three axes, are detailed in

Table 2. Norm errors of the translation and attitude between the ground truth and the simulation results.

Translation RMSE	Translation Mean Error	Translation STD	Attitude RMSE	Attitude Mean Error	Attitude STD
0.207 m	0.1447 m	0.1408 m	0.1684 rad	0.1348 rad	0.1008 rad

. The errors of the attitude are obtained by converting ${\overline{\mathit{q}}}_{ic}$ to roll-pitch-yaw.

The curves and numerical statistics show that despite the randomized ${\mathit{P}}_{k|k}$ and ${\mathit{R}}_k$ alongside a high yaw angle velocity disturbing the initial convergence, the ICEKF ultimately converges without much specific parameter tuning. This indicates that the ICEKF possesses strong robustness while yielding reliable motion estimates.

To further test the filter’s ability to converge, an initial state estimate set is designed which conducts the virtual rig starting from the different three-dimensional points to the actual desired starting point [1,0,0]. The initial estimate set, varying from [0,−1,−1] to [1,0,0], and the converging process are illustrated by Figure 8, which depicts how even when facing uncertain initial estimates, the filter can converge with a decent performance.

5.2. Dataset Experiment

To inspect the effectiveness of the algorithm in real-world scenarios, this paper employs the EuRoC ROOM01 and MainHall02 datasets [28] for validation. Since this study investigates the performance of the loosely coupled visual–inertial algorithm itself instead of visual odometry, one of the state-of-the-art visual SLAM algorithms, monocular ORB-SLAM V3 [29], serves as the visual black-box approach. Given that this algorithm has been confirmed to exhibit exceptional ATE performance, this paper focuses on RPE comparisons of different algorithms by employing EVO tools [30] to analyze trajectories. The ICEKF operates with the IMU data at 200 Hz, and meanwhile the visual measurement runs at 20 Hz. To facilitate the comparison, each frame from the visual measurement with ORB-SLAM is considered a keyframe.

The coefficients are ${\mu }_v=0.9$ and ${\mu }_{\omega }=0.5$, and the initial scale factor is determined based on the prior results of the visual algorithm as $\lambda =2.04$. Given that the hardware setup and scenario are fixed in the dataset, the initial covariance matrix ${\mathit{P}}_{k|k}$ of the ICEKF can be easily adjusted via multiple runs. In the first run, the initial ${\mathit{P}}_{k|k}$ is selected randomly, and eventually stabilizes as the algorithm progresses. The final updated ${\mathit{P}}_{k|k}$ is utilized to run the scenario test again, which greatly increases the convergence speed and provides high resilience against noise. For applications in unknown environments, initial covariance estimation can be conducted in a similar scenario to further improve convergence efficiency.

Dataset experiments with monocular ORB-SLAM, its IMU variant, and the monocular MSCKF with an IMU were performed. The entire experiment processes are shown in Figure 9 and Figure 10.

The contrast between the three-dimensional position output of the ICEKF and the ground truth is shown in Figure 11 and Figure 12. The three-dimensional position errors and the orientation errors between the output of the ICEKF and the ground truth after applying alignment using Umeyama’s method [30] from the two dataset tests are shown in Figure 13, Figure 14, Figure 15 and Figure 16.

Trajectory analysis using EVO is conducted, in which the ICEKF with the results from ORB-SLAM and its IMU variant, as well as the data obtained from the monocular MSCKF [31], are compared against the ground truth, with the statistical RPE results presented in

Table 3. Norm errors of the translations between the ground truth and the results of the dataset experiments (ROOM01).

	Translation RMSE	Translation Mean Error	Translation STD
ICEKF	0.04153 m	0.00574 m	0.04112 m
Monocular ORB-SLAM V3	0.0393 m	0.0355 m	0.01678 m
Monocular ORB-SLAM V3 with IMU	0.003841 m	0.002973 m	0.002433 m
Monocular MSCKF	0.1305 m	0.05366 m	0.119 m

and

Table 4. Norm errors of the translations between the ground truth and the results of the dataset experiments (MainHall02).

	Translation RMSE	Translation Mean Error	Translation STD
ICEKF	0.08922 m	0.009765 m	0.08869 m
Monocular ORB-SLAM V3	0.735 m	0.533 m	0.506 m
Monocular ORB-SLAM V3 with IMU	0.001366 m	0.004981 m	0.01272 m
Monocular MSCKF	0.2689 m	0.09905 m	0.25 m

.

The comparative results indicate that the ICEKF ensures real-time measurement through high-frequency IMU updates and achieves an accuracy comparable to that of the monocular ORB-SLAM and its IMU variant while outperforming the monocular MSCKF. Although the trajectory does not reach the high accuracy of ORB-SLAM-based methods owing to the lack of posterior batch optimization processing, the computational complexity of the coupled part of the algorithm is only $\mathit{O}\left(n\right)$, which benefits localization applications. Moreover, the algorithm framework can be directly utilized with other localization algorithms to improve their measurement stability in complex environments.

To assess the robustness of the ICEKF with regard to leap noise from visual measurements, this study incorporates random stimulations with an amplitude of 0.5 m into the visual measurement result to imitate harsh visual measurement failure. Figure 17 shows a comparison of the partial trajectory from the ICEKF against the corrupted visual measurement. This indicates that the ICEKF can significantly mitigate the impact of sporadic instability from visual measurements.

6. Conclusions

This study introduces a novel weighted loosely coupled algorithm for fusing data from IMUs and visual measurements and demonstrates its observability. The algorithm, of which the coupled part possesses a computational complexity of $\mathit{O}\left(n\right)$, retains the high-frequency update capability inherent to the EKF framework, thus enabling high-accuracy active localization. Both simulation and dataset tests reveal that the ICEKF achieves deep integration of the IMU and the visual data on the velocity layer throughout the updating process, which benefits the localization performance. By conveniently tuning the weights assigned to different data sources, the framework improves both the fusion accuracy and the resilience to abrupt noise. In future studies, we will aim to apply the approach across multiple devices by analyzing the sensitivity of the coupling coefficients in challenging real-life scenarios.