4.1. Process model

The state equation in continuous time form is as follows:

(2) \begin{equation} {}^{I}\!\!{}_{G}{\dot{{\textbf{q}}}}{}=\frac{1}{2}\boldsymbol{\Omega }\left({\unicode{x1D6DA}}\right){}^{I}\!\!{}_{G}{{\textbf{q}}}{} \end{equation}

(3) \begin{equation} \dot{{\textbf{b}}}_{g}={\textbf{n}}_{{{{{\unicode{x03C9}}}}} g} \end{equation}

where ${\unicode{x1D6DA}}={\unicode{x1D6DA}}_{m}-{\textbf{b}}_{g}-{\textbf{n}}_{g}$ is the true value of the angular velocity of the IMU, ${\textbf{n}}_{g}$ is the Gaussian noise of the gyroscope. ${\unicode{x1D6DA}}_{m}=\left[\begin{array}{l@{\quad}l@{\quad}l} {{{{\unicode{x03C9}}}}} _{x} & {{{{\unicode{x03C9}}}}} _{y} & {{{{\unicode{x03C9}}}}} _{z} \end{array}\right]^{T}$ represents the angular velocity measured by the gyroscope. $\boldsymbol{\Omega }({\unicode{x1D6DA}})=\left[\begin{array}{l@{\quad}l} -\lfloor {\unicode{x1D6DA}}\times \rfloor & {\unicode{x1D6DA}}\\[5pt] -{\unicode{x1D6DA}}^{T} & 0 \end{array}\right]$ with $\lfloor {\unicode{x1D6DA}}\times \rfloor$ denotes the skew-symmetric matrix of ${\unicode{x1D6DA}}$ .

The approximate state equation can be obtained as follows by applying the expectation operator in the above formula

(4) \begin{equation} {}^{I}\!\!{}_{G}{\dot{\hat{{\textbf{q}}}}}{}=\frac{1}{2}\boldsymbol{\Omega }\left(\hat{{\unicode{x1D6DA}}}\right){}^{I}\!\!{}_{G}{\hat{{\textbf{q}}}}{} \end{equation}

(5) \begin{equation} \dot{\hat{{\textbf{b}}}}_{g}=\textbf{0}_{3\times 1} \end{equation}

where $\hat{{\unicode{x1D6DA}}}={\unicode{x1D6DA}}_{m}-\hat{{\textbf{b}}}_{g}$ represents the estimated results of actual angular velocity information of the gyroscope. To propagate state uncertainty, the state transition matrix is calculated as follows:

(6) \begin{equation} \boldsymbol{\Phi }_{{k_{q}}}=\exp \left(\int _{t_{k}}^{t_{k+1}}\frac{1}{2}\boldsymbol{\Omega }\left(\hat{{\unicode{x1D6DA}}}\left(\tau \right)\right)d\tau \right)_{4\times 4}\approx \left({\textbf{I}}+\frac{{\Delta} t}{2}\boldsymbol{\Omega }\right) \end{equation}

(7) \begin{equation} \boldsymbol{\Phi }_{{k_{b}}}=\textbf{0}_{3\times 3} \end{equation}

where ${{\Delta}} t$ is the time interval between two IMU data frames. From the above formula, the discrete state transition equation of the system can be obtained as

(8) \begin{equation} {\textbf{X}}_{pose}\!\left(k+1\right)=\left[\begin{array}{l} {}^{I}\!\!{}_{G}{{\textbf{q}}}{}\!\left(k+1\right)\\[5pt] {\textbf{b}}_{g}\!\left(k+1\right) \end{array}\right]=\left[\begin{array}{l} \boldsymbol{\Phi }_{{k_{q}}}\quad {}^{I}\!\!{}_{G}{{\textbf{q}}}{}\left(k\right)\\[5pt] \boldsymbol{\Phi }_{{k_{b}}}\quad {\textbf{b}}_{g}\!\left(k\right) \end{array}\right]+{\textbf{y}}\!\left(k\right) \end{equation}

(9) \begin{equation} {\textbf{X}}_{pose}\left(k+1\right)\approx {\textbf{H}}_{p}{\textbf{X}}_{pose}\left(k\right)+{\textbf{y}}\left(k\right) \end{equation}

where ${\textbf{H}}_{p}$ represents the state transition matrix of the gyroscope state. The specific values are shown in ref. [Reference Sun, Mohta, Pfrommer, Watterson, Liu, Mulgaonkar, Taylor and Kumar18]. ${\textbf{y}}(k)$ obeys a Gaussian distribution with a mean of 0 and a covariance matrix ${\textbf{Q}}_{k}$ .

(10) \begin{equation} {\textbf{y}}\left(k\right)\sim N\left(0;\;{\textbf{Q}}_{k}\right),{\textbf{Q}}_{k}=\left[\begin{array}{l@{\quad}l} \sigma _{q}^{2}{\textbf{I}}_{4} & \textbf{0}_{4\times 3}\\[5pt] \textbf{0}_{3\times 4} & \sigma _{bg}^{2}{\textbf{I}}_{3} \end{array}\right] \end{equation}

where $\sigma _{q}$ and $\sigma _{bg}$ are constants.

Finally, the propagated covariance of the gyroscope state can be expressed as follows:

(11) \begin{equation} {\textbf{P}}_{pos{e_{k+1|k}}}={\textbf{H}}_{p}{\textbf{P}}_{pos{e_{k|k}}}{\textbf{H}}_{p}^{T}+{\textbf{Q}}_{k} \end{equation}

4.2. Observation model

The actual value of the accelerometer is

(12) \begin{equation} {\boldsymbol{a}}={\boldsymbol{a}}_{{\boldsymbol{m}}}-{\textbf{b}}_{a} \end{equation}

where ${\textbf{{a}}}_{{\textbf{{m}}}}=\left[\begin{array}{l@{\quad}l@{\quad}l} a_{{\textbf{{x}}}} & a_{{\textbf{{y}}}} & a_{{\textbf{{z}}}} \end{array}\right]^{{\textbf{{T}}}}$ represents the acceleration measured by accelerometer, ${\textbf{b}}_{a}$ represents the bias of the accelerometer. According to the attitude information predicted by the gyroscope, we can obtain the $\hat{{\textbf{{a}}}}$

(13) \begin{equation} \hat{{\textbf{{a}}}}={}^{I}\!\!{}_{G}{{\textbf{R}}}{}\left[\begin{array}{c} 0\\[5pt] 0\\[5pt] -{\textbf{g}} \end{array}\right]=-{\textbf{g}}\left[\begin{array}{c} 2q_{{\textbf{{x}}}}q_{{\textbf{{z}}}}+2q_{{\textbf{{y}}}}q_{{\textbf{{w}}}}\\[5pt] 2q_{{\textbf{{y}}}}q_{{\textbf{{z}}}}-2q_{{\textbf{{x}}}}q_{{\textbf{{w}}}}\\[5pt] q_{{\textbf{{w}}}}^{2}-q_{{\textbf{{x}}}}^{2}-q_{{\textbf{{y}}}}^{2}+q_{{\textbf{{z}}}}^{2} \end{array}\right] \end{equation}

where ${\textbf{g}}$ is the gravity vector. The residual of the accelerometer can be expressed as

(14) \begin{equation} {\textbf{r}}={\textbf{{a}}}-\hat{{\textbf{{a}}}}={\textbf{H}}_{{\boldsymbol{g}}}{\textbf{X}}_{{\boldsymbol{pose}}}+{\textbf{{v}}}\!\left({\boldsymbol{k}}\right) \end{equation}

where ${\textbf{H}}_{g}$ represents the Jacobian matrix of the acceleration measurement with respect to the state of the gyroscope, and the specific values are shown in ref. [Reference Sun, Mohta, Pfrommer, Watterson, Liu, Mulgaonkar, Taylor and Kumar18]. ${\textbf{v}}(k)$ obeys a Gaussian distribution with mean 0 and covariance ${\textbf{R}}_{k}$ .

(15) \begin{equation} {\textbf{v}}\left(k\right)\sim N\left(0;\;{\textbf{R}}_{k}\right)\!,{\textbf{R}}_{k}=\sigma _{a}^{2}{\textbf{I}}_{3} \end{equation}

where $\sigma _{a}$ is a constant. Finally, update ${\textbf{X}}_{pose}$ by the residual

(16) \begin{equation} \left\{\begin{array}{l} {\textbf{X}}_{pos{e_{k+1|k+1}}}={\textbf{X}}_{pos{e_{k+1|k}}}+\textbf{Kr}\\[5pt] \textbf{K}={\textbf{P}}_{pos{e_{k+1|k}}}{\textbf{H}}_{g}^{T}\left({\textbf{H}}_{g}{\textbf{P}}_{pos{e_{k+1|k}}}{\textbf{H}}_{g}^{T}+{\textbf{R}}_{k}\right)^{-1}\\[5pt] {\textbf{P}}_{pos{e_{k+1|k+1}}}=\left({\textbf{I}}-\textbf{K}{\textbf{H}}_{g}\right){\textbf{P}}_{pos{e_{k+1|k}}} \end{array}\right. \end{equation}

where $\textbf{K}$ is the Kalman gain, ${\textbf{X}}_{pos{e_{k+1|k+1}}}$ and ${\textbf{P}}_{pos{e_{k+1|k+1}}}$ are the state vectors and covariance matrices estimated at time $k+1$ , respectively. By fusing the measured values of the accelerometer, we can calculate the precise IMU pose ${}^{I}\!\!{}_{G}{{\textbf{q}}}{}$ and gyroscope bias ${\textbf{b}}_{g}$ for state vector of the IMU in the second-stage filter.

5.1. Process model

The state equation of the IMU in continuous time form is

\begin{equation*} \begin{array}{l@{\quad}l} {}^{I}\!\!{}_{G}{\dot{{\textbf{q}}}}{}=\dfrac{1}{2}\boldsymbol{\Omega }\!\left({\unicode{x1D6DA}}_{m}-{\textbf{b}}_{g}-{\textbf{n}}_{g}\right){}^{I}\!\!{}_{G}{{\textbf{q}}}{},\, & \dot{{\textbf{b}}}_{g}={\textbf{n}}_{{{{{\unicode{x03C9}}}}} g} \end{array} \end{equation*}

\begin{equation*} {}^{G}{\dot{{\textbf{v}}}}{}={\textbf{C}}\!\left({}^{I}\!\!{}_{G}{{\textbf{q}}}{}\right)^{{\textbf{{T}}}}\left({\textbf{{a}}}_{{\textbf{{m}}}}-{\textbf{b}}_{a}-{\textbf{n}}_{a}\right)+{}^{G}{{\textbf{g}}}{} \end{equation*}

\begin{equation*} \begin{array}{l@{\quad}l} \dot{{\textbf{b}}}_{a}={\textbf{n}}_{{{{{\unicode{x03C9}}}}} a},\, & {}^{G}{\dot{{\textbf{p}}}}{_{I}^{}}={}^{G}{{\textbf{v}}}{} \end{array}_{I} \end{equation*}

(22) \begin{equation} \begin{array}{l@{\quad}l} {}_{C}^{I}\dot{{\textbf{q}}}=0,\, & \begin{array}{l@{\quad}l} {}^{I}{\dot{{\textbf{p}}}}{_{C}^{}}=0,\, & \dot{t}_{d} \end{array}=0 \end{array} \end{equation}

where $C(\!\cdot\!)$ is the function that converts the quaternion into the rotation matrix. We can get the propagation model of the IMU state through the linear continuous kinematics model of the IMU error state, and it is shown as follows:

(23) \begin{equation} \dot{\tilde{{\textbf{X}}}}_{I}={\textbf{F}}\tilde{{\textbf{X}}}_{I}+{\textbf{G}}{\textbf{n}}_{I} \end{equation}

where ${\textbf{n}}_{I}=\left(\begin{array}{l@{\quad}l@{\quad}l@{\quad}l} {\textbf{n}}_{g}^{T} & {\textbf{n}}_{{{{{\unicode{x03C9}}}}} g}^{T} & {\textbf{n}}_{a}^{T} & {\textbf{n}}_{{{{{\unicode{x03C9}}}}} a}^{T} \end{array}\right)^{T}$ . The vectors ${\textbf{n}}_{g}$ and ${\textbf{n}}_{a}$ are the Gaussian noise of the gyroscope and accelerometer measurements. ${\textbf{n}}_{{{{{\unicode{x03C9}}}}} g}$ and ${\textbf{n}}_{{{{{\unicode{x03C9}}}}} a}$ are the random walk rates of the gyroscope and accelerometer measurement bias. The details of ${\textbf{F}}$ and ${\textbf{G}}$ are shown in ref. [Reference Sun, Mohta, Pfrommer, Watterson, Liu, Mulgaonkar, Taylor and Kumar18]. A fourth-order Runge–Kutta numerical integration is used to estimate the state of the IMU The discrete-time state transition matrix $\boldsymbol{\Phi }_{k}$ and the discrete-time noise covariance matrix ${\textbf{Q}}_{k}$ are shown as follows:

(24) \begin{equation} \boldsymbol{\Phi }_{k}=\exp \left(\int _{t_{k}}^{t_{k+1}}{\textbf{F}}d\tau \right) \end{equation}

(25) \begin{equation} {\textbf{Q}}_{k}=\int _{t_{k}}^{t_{k+1}}\boldsymbol{\Phi }_{k}{\textbf{G}}{\textbf{n}}_{I}{\textbf{G}}^{T}\boldsymbol{\Phi }_{k}^{T}d\tau \end{equation}

The covariance prediction matrix of the VIO system at time $k$ can be described as

(26) \begin{equation} {\textbf{P}}_{k+1|k}=\left[\begin{array}{c@{\quad}c} \boldsymbol{\Phi }_{k}{\textbf{P}}_{I{I_{k|k}}}\boldsymbol{\Phi }_{k}^{T}+{\textbf{Q}}_{k} & \boldsymbol{\Phi }_{k}{\textbf{P}}_{I{C_{k|k}}}\\[5pt] {\textbf{P}}_{IC_{k|k}}^{T}\boldsymbol{\Phi }_{k}^{T} & {\textbf{P}}_{C{C_{k|k}}} \end{array}\right] \end{equation}

The pose of the newly added camera state ${\textbf{x}}_{{C_{i}}}$ in the state vector can be calculated according to the IMU pre-integration result and the relative position between the camera and the IMU.

(27) \begin{equation} {}_{G}^{C_{i}}{{\textbf{q}}}{}={}_{I}^{C}{{\textbf{q}}}{}\otimes {}^{I}\!\!{}_{G}{{\textbf{q}}}{},{}^{G}{{\textbf{p}}}{_{Ci}^{}}={}^{G}{{\textbf{p}}}{_{I}^{}}+C\left({}^{I}\!\!{}_{G}{{\textbf{q}}}{}\right)^{T}{}^{I}{\hat{{\textbf{p}}}}{_{C}^{}} \end{equation}

The augmented covariance matrix is as follows:

(28) \begin{equation} {\textbf{P}}_{k|k}=\left[\begin{array}{c} {\textbf{I}}_{22+6N}\\[5pt] \textbf{J} \end{array}\right]{\textbf{P}}_{k|k}\left[\begin{array}{c} {\textbf{I}}_{22+6N}\\[5pt] \textbf{J} \end{array}\right]^{T} \end{equation}

where $\textbf{J}$ is shown in ref. [Reference Sun, Mohta, Pfrommer, Watterson, Liu, Mulgaonkar, Taylor and Kumar18].

5.2. Measurement model

The image data are used to correct the state of IMU. At time $t$ , there is a time offset $t_{d}$ between the camera and IMU. Assuming that the left and right cameras can track the same feature point $f_{i}$ at the same time, where the pose of the left camera is $\left[\begin{array}{l@{\quad}l} {}_{G}^{C1}{{\textbf{q}}}{}(t+t_{d}) & {}^{G}{{\textbf{p}}}{_{C1}^{}}(t+t_{d}) \end{array}\right]$ and the pose of the right camera is $\left[\begin{array}{l@{\quad}l} {}_{G}^{C2}{{\textbf{q}}}{}(t+t_{d}) & {}^{G}{{\textbf{p}}}{_{C2}^{}}(t+t_{d}) \end{array}\right]$ . The observation of the stereo camera is

(29) \begin{equation} \textbf{z}_{i}\!\left(t\right)=\left[\begin{array}{l} u_{1}^{i}\!\left(t\right)\\[5pt] v_{1}^{i}\!\left(t\right)\\[5pt] u_{2}^{i}\!\left(t\right)\\[5pt] v_{2}^{i}\!\left(t\right) \end{array}\right]=\left[\begin{array}{c@{\quad}c} \dfrac{1}{{}^{C1}{z}{_{i}^{}}\!\left(t+t_{d}\right)} & \textbf{0}_{2\times 2}\\[5pt] \textbf{0}_{2\times 2} & \dfrac{1}{{}^{C2}{z}{_{i}^{}}\!\left(t+t_{d}\right)} \end{array}\right]\times \left[\begin{array}{l} {}^{C1}{x}{_{i}^{}}\!\left(t+t_{d}\right)\\[5pt] {}^{C1}{y}{_{i}^{}}\!\left(t+t_{d}\right)\\[5pt] {}^{C2}{x}{_{i}^{}}\!\left(t+t_{d}\right)\\[5pt] {}^{C2}{y}{_{i}^{}}\!\left(t+t_{d}\right) \end{array}\right] \end{equation}

where $\left[\begin{array}{l@{\quad}l@{\quad}l} {}^{Ck}{x}{_{i}^{}}(t+t_{d}) & {}^{Ck}{y}{_{i}^{}}(t+t_{d}) & {}^{Ck}{z}{_{i}^{}}(t+t_{d}) \end{array}\right]^{T},k=1,2$ represents the position of the feature point in the left and right images and is shown as follows:

(30) \begin{equation} {}^{C_{k}}{{\textbf{p}}}{_{fi}^{}}\left(t+t_{d}\right)=\left[\begin{array}{l} {}^{Ck}{x}{_{i}^{}}\left(t+t_{d}\right)\\[5pt] {}^{Ck}{y}{_{i}^{}}\left(t+t_{d}\right)\\[5pt] {}^{Ck}{z}{_{i}^{}}\left(t+t_{d}\right) \end{array}\right]=C\left({}_{G}^{Ck}{{\textbf{q}}}{}\left(t+t_{d}\right)\right)\left({}^{G}{{\textbf{p}}}{_{fi}^{}}-{}^{G}{{\textbf{p}}}{_{I}^{}}\left(t+t_{d}\right)\right)+{}^{G}{{\textbf{p}}}{_{I}^{}} \end{equation}

where ${}^{G}{{\textbf{p}}}{_{fi}^{}}$ is the 3D point position of the feature point $f_{i}$ in the global frame, which can be calculated through the least square method of multiple feature points. After obtaining the ${}^{G}{{\textbf{p}}}{_{fi}^{}}$ , we can calculate the reprojection error of the feature point observed in the current frame, and it is shown as follows:

(31) \begin{equation} {\textbf{r}}_{i,t}=\textbf{z}_{i}\!\left(t\right)-\hat{\textbf{z}}_{{f_{i}}}\left(t+t_{d}\right)\approx {\textbf{H}}_{X,t}^{i}\tilde{{\textbf{X}}}\left(t+t_{d}\right)+{\textbf{H}}_{{f_{i}},t}{}^{G}{\tilde{{\textbf{p}}}}{_{fi}^{}}\left(t+t_{d}\right)+{\textbf{n}}_{{f_{i}}} \end{equation}

where ${\textbf{n}}_{{f_{i}}}$ is the measurement noise. The measurement Jacobian matrices ${\textbf{H}}_{C}^{i}$ and ${\textbf{H}}_{{f_{i}}}$ are shown in ref. [Reference Sun, Mohta, Pfrommer, Watterson, Liu, Mulgaonkar, Taylor and Kumar18].

The above is the observation residual of a feature point in a camera frame, and the residual block obtained by superimposing multiple observations of the feature point $f_{i}$ is shown as follows:

(32) \begin{equation} {\textbf{r}}_{i}={\textbf{H}}_{X}^{i}\tilde{{\textbf{X}}}+{\textbf{H}}_{{f_{i}}}{}^{G}{\tilde{{\textbf{p}}}}{_{fi}^{}}+{\textbf{n}}_{{f_{i}}} \end{equation}

Since ${}^{G}{\tilde{{\textbf{p}}}}{_{fi}^{}}$ is computed using the camera pose, its uncertainty is related to the camera pose in that state. To ensure that the uncertainty of ${}^{G}{\tilde{{\textbf{p}}}}{_{fi}^{}}$ will not affect the residual, the residual in Eq, (36) ${\textbf{r}}_{i}$ is projected onto the left null space of ${\textbf{H}}_{{f_{i}}}$

(33) \begin{equation} {\textbf{r}}_{0}^{i}=\textbf{A}^{T}{\textbf{r}}_{i}\approx \textbf{A}^{T}{\textbf{H}}_{X}^{i}\tilde{{\textbf{X}}}+\textbf{A}^{T}{\textbf{n}}_{{f_{i}}}={\textbf{H}}_{X,0}^{i}\tilde{{\textbf{X}}}+{\textbf{n}}_{0}^{i} \end{equation}

$\textbf{A}$ represents a unitary matrix composed of column vectors of ${\textbf{H}}_{{f_{i}}}$ . The state of the IMU is updated through the gyroscope and accelerometer measurements. Then, the new camera state is estimated according to the IMU preintegration result and then added to the state vector of VIO system to calculate the augmented covariance matrix. Finally, the VIO system state is updated using the reprojection errors of the multi-constrained feature points.

(34) \begin{equation} \left\{\begin{array}{l} {\textbf{X}}_{k+1|k+1}={\textbf{X}}_{k+1|k}+\textbf{K}'{\textbf{r}}'\\[5pt] \textbf{K}'={\textbf{P}}_{k+1|k}{\textbf{H}}^{T}\left({\textbf{H}}{\textbf{P}}_{k+1|k}{\textbf{H}}^{T}+{\textbf{R}}'\right)^{-1}\\[5pt] {\textbf{P}}_{k+1|k+1}={\textbf{P}}_{k+1|k}-\textbf{K}'{\textbf{H}}{\textbf{P}}_{k+1|k} \end{array}\right. \end{equation}

where ${\textbf{r}}'$ is the reprojection error of all points in the current frame, composed of different ${\textbf{r}}_{0}^{i}$ . ${\textbf{H}}$ is the observation matrix, composed of different ${\textbf{H}}_{X,0}^{i}$ .

5.3. Schur complement marginalization

With the continuous operation of the VIO system, the number of newly added camera states grows, and the matrix’s dimension becomes larger, which increases the computational cost of camera pose estimation. To meet the real-time requirements, it is necessary for us to marginalize some camera states with just a small amount of information. If such a camera state is deleted directly, it will cause the prior information of some camera states to be lost. The Schur complement model is utilized to marginalize old camera states to reduce the computational cost and preserve the prior information of this kind of camera state.

Figure 3.

The marginalization process. The change process of the covariance matrix when the marginalized state ${\textbf{C}}_{1}$ and added state ${\textbf{C}}_{7}$ .

Figure 3 shows the process of adding new states and deleting old states in the sliding window where ${\textbf{X}}_{I}$ is the IMU state, ${\textbf{C}}_{1}$ is the old camera state that needs to be deleted, and ${\textbf{C}}_{7}$ is the new camera state that needs to be added to the sliding window. First, the state of ${\textbf{C}}_{7}$ is added to the system state vector. At the same time, the cross-covariance part between ${\textbf{C}}_{7}$ and ${\textbf{C}}_{6}$ , and ${\textbf{X}}_{I}$ is added to the system covariance matrix. Then, the state ${\textbf{C}}_{1}$ needs to be deleted, but ${\textbf{C}}_{1}$ is connected to ${\textbf{C}}_{0}$ and ${\textbf{C}}_{2}$ . Deleting it directly will cause the loss of prior information, so the marginalization is performed through the Schur complement model.

(35) \begin{equation} {\textbf{P}}_{AA}^{new}={\textbf{P}}_{AA}-{\textbf{P}}_{AB}{\textbf{P}}_{BB}^{-1}{\textbf{P}}_{AB}^{T} \end{equation}

where ${\textbf{P}}_{BB}$ is the covariance matrix of state ${\textbf{C}}_{1}, {\textbf{P}}_{AA}$ is the covariance matrix of the state to be preserved, and ${\textbf{P}}_{AB}$ is the covariance matrix between state ${\textbf{C}}_{1}$ and the state to be preserved. In the process of marginalization, state ${\textbf{C}}_{1}$ is first moved to the top of the covariance matrix and then the covariance matrices ${\textbf{P}}_{AB}$ and ${\textbf{P}}_{AB}^{T}$ are obtained. Finally, a new covariance matrix named ${\textbf{P}}_{AA}^{new}$ was obtained through the Schur complement model, preserving prior information between ${\textbf{C}}_{1}$ and ${\textbf{C}}_{0}$ , and ${\textbf{C}}_{2}$ , and adding new constraints between ${\textbf{C}}_{0}$ and ${\textbf{C}}_{2}$ .

The algorithm flow of this paper is shown in Algorithm 1.

Algorithm 1

Overview system process

6.1. EuRoC data set

The EuRoC dataset is collected by an unmanned aerial vehicle containing stereo image pairs at 20 Hz and IMU measurements at 200 Hz. In the original dataset [Reference Burri, Nikolic, Gohl, Schneider, Rehder, Omari, Achtelik and Siegwart33], due to the high-precision cameras and IMUs, the image stream and the IMU data sequence are fully time-synchronized. In order to verify the performance of our proposed method in this article, the time stamp of the image stream is uniformly modified. The open-source toolbox provided in ref. [Reference Zhang and Scaramuzza34] is used to evaluate the VIO performance quantitatively. The RMSE of the absolute trajectory error is selected as the evaluation standard of the algorithm performance. All algorithms run 10 Monte Carlo experiments on the custom EuRoC dataset.

Figure 4.

The estimation process of the time offset of the algorithm in (a) V1_01, (b) V2_01, (c) MH_04, and (d) MH_05 four sequences, the time offsets are 10 ms.

Figure 4 shows the time offset estimation results for the Proposed, OPEN-VINS, and VINS-MONO on four sequences (V1_01, V2_01, MH_04, and MH_05) with a time offset of 10 ms. From the results in Figure 4, it can be observed that our proposed method achieves the highest precision in time offset estimation. VINS-MONO relying on minimizing the reprojection error of feature points, depends on high-quality image features, leading to inaccurate estimation results. On the other hand, OPEN-VINS employs a Kalman filter framework to predict the time offset, but it fails to sufficiently consider the impact of imprecise gyroscope attitudes on system accuracy, resulting in inaccurate estimation as well. Calculate the average time offset estimated by each algorithm and compare it with the preset time offset to evaluate the time estimation performance of each algorithm. Combining the information from Figure 4 and Table 2, it is evident that our proposed method online time calibration accurately predicts the specified time offset with estimation errors within 0.5 ms. By incorporating the time offset into sensor timestamps and achieving synchronization between sensors through software implementation, the effectiveness of our propoed method has been demonstrated.

Figure 5 shows the trajectory estimation accuracy comparison of our proposed method, MSCKF, C-MSCKF, VINS-MONO, and OPEN-VINS on the EuRoC dataset with 20 ms time offset. The gray dotted line represents the groundtruth. Figure 5(a), (b), (c), and (d) are the XY plane projection diagrams of the trajectories of the five algorithms on the four sequences of the custom EuRoC dataset V1_01, V2_01, MH_04, and MH_05, respectively. Table 2 shows the time offset estimation results of our proposed method and the trajectory RMSE of the five algorithms in the four sequences V1_01, V2_01, MH_04, and MH_05, It can be seen from Table 2 and Figure 5 that our proposed method has higher algorithm accuracy. The possible reasons for the advantage of our proposed method over the other method are mainly manifested in two points: first, the inclusion of the first-stage complementary Kalman filter, which corrects the position estimation results of the gyroscope by using accelerometer measurements, enables the IMU to output a more accurate position and provides good position estimation results for the VIO system, which is a part that other methods fail to achieve. Second, the problem that the time offsets between different sensors lead to the time unsynchronization between the estimated and observed states of the Kalman filter is fully considered, and a one-dimensional time offset is added to the state vector of the VIO, which is continuously updated using the camera observation, so as to achieve the smallest error between the estimated state of the IMU and the observed state of the camera, thus improving the positioning accuracy of the system.

Figure 6 shows the average running time of the back end for the four algorithms. The running times of our proposed method, MSCKF, OPEN-VINS, and VINS-MONO algorithms are 8.72 ms, 7.73 ms, 18.14 ms, and 29.9 ms, respectively. It can be seen from the figure that the optimization-based algorithm VINS-MONO has the highest time complexity, more than three times that of our proposed method, mainly because VINS-MONO jointly optimizes all the state vectors that can be optimized. When new images arrive, the data size will grow, thus increasing the time complexity of the algorithm. The complexity of the OPEN-VINS algorithm based on EKF is also relatively high because it combines a variety of modules that can improve the accuracy of the algorithm, resulting in an increase in overall performance and complexity. Compared with the MSCKF algorithm, our proposed method has a first-order complementary Kalman filter structure, and the time offset is added to the VIO system state vector. Therefore, the problem size is slightly larger than the original MSCKF algorithm, and the running time is slightly higher than the MSCKF algorithm, but our proposed method is more accurate than MSCKF. The positioning accuracy of VIO is significantly higher than that of MSCKF algorithm.

Table II.

Time offset calibration results on euRoC datasets.

Figure 5.

The trajectories of MSCKF, C-MSCKF, VINS-MONO, OPEN-VINS, and proposed on EuRoC datasets V1_01, V2_01, MH_04, and MH_05 sequences with 20 ms time offset.

Figure 6.

The average back-end processing time of the three algorithms on the EuRoC dataset.

6.2. Experimental verfication of real scenes

In real experiment, the camera is fixed on a mobile robot platform to make the robot move in a circular motion. Meanwhile, select the VICON motion capture system to record the robot’s 3D position as ground truth. The experimental environment is shown in Figure 7. The ZED2 Stereo camera is used in real experiment. This sensor contains two global shutter cameras with the field of vision of $120^{\circ}\times 110^{\circ}\times 70^{\circ}$ (diagonal, horizontal, and vertical, respectively) and a six-axis IMU. The camera frequency is set to be 20 Hz, the image resolution is set to be 1280 × 720, and the IMU frequency is 200 Hz.

Figure 7.

Area where the real-world motion experiment took place. A VICON motion capture system is used to record the real 3D position as ground truth.

In Figure 8(a), the gray dotted line represents the ground truth, and the blue, green, and red lines represent VINS-MONO (no loop), OPEN-VINS, and our proposed method, respectively. Figure 8(b) shows the unstable temporal offset estimated by our proposed method during motion. It can be seen from Figure 8(a) that the trajectory accuracy of our proposed method is higher and closer to the ground truth. This shows that our proposed method uses the accelerometer to dynamically correct the gyroscope bias, which improves the initial attitude and trajectory accuracy of the VIO system. At the same time, this paper defines the time offset in different sensor fusion processes as an unknown variable and uses a MSCKF for dynamic estimation, which further improves the trajectory accuracy of the VIO system.

Figure 8.

Trajectory comparisons of the real-world motion experiment. (a) Trajectories of ground truth, OPEN-VINS, VINS-MONO, and proposed algorithms. (b) Unstable time offset estimated by proposed.

In Figure 9, we compare the final trajectories of our proposed method and MSCKF in a closed environment. Figure 9(a) is the indoor experimental scene, Figure 9(b) is our proposed method and MSCKF trajectory, where the blue line belongs to the MSCKF algorithm, the red line belongs to our proposed method, and Figure 9(c) is the unstable time offset calculated by our proposed method. From Figure 9(b), it can be seen that due to the IMU rotation, there is a large deviation in the start and end points of the MSCKF algorithm trajectory estimation. Our proposed method corrects the rotation obtained by the gyroscope through the accelerometer. Fully consider the unstable time offset in the fusion process, so that the IMU rotation data are more stable, the image constraints are more accurate, and the starting point and the ending point coincide. The above experiments fully prove that the VIO algorithm based on the two-stage EKF proposed in this paper has higher positioning accuracy under the condition of increasing a small amount of calculation cost.

Figure 9.

Trajectory of a moving vehicle in an enclosed space. (a) Experimental environment. (b) Trajectories of MSCKF and proposed. (c) Proposed estimated unstable time offset.