Online IMU Self-Calibration for Visual-Inertial Systems

Abstract

Low-cost microelectro mechanical systems (MEMS)-based inertial measurement unit (IMU) measurements are usually affected by inaccurate scale factors, axis misalignments, and g-sensitivity errors. These errors may significantly influence the performance of visual-inertial methods. In this paper, we propose an online IMU self-calibration method for visual-inertial systems equipped with a low-cost inertial sensor. The goal of our method is to concurrently perform 3D pose estimation and online IMU calibration based on optimization methods in unknown environments without any external equipment. To achieve this goal, we firstly develop a novel preintegration method that can handle the IMU intrinsic parameters error propagation. Then, we frame IMU calibration problem into general factors so that we can easily integrate the factors into the current graph-based visual-inertial frameworks and jointly optimize the IMU intrinsic parameters as well as the system states in a big bundle. We evaluate the proposed method with a publicly available dataset. Experimental results verify that the proposed approach is able to accurately calibrate all the considered parameters in real time, leading to significant improvement of estimation precision of visual-inertial system (VINS) compared with the estimation results with offline precalibrated IMU measurements.

1. Introduction

Vision-based motion estimation and 3D reconstruction is a very active field of research in robotics and computer vision communities over the last decades due to its potential applications, such as robot navigation, autonomous driving, augmented reality (AR) and virtual reality (VR). Different sensor setups can be used for vision-based motion estimation: monocular [1,2], stereo [3,4], RGB-D [5,6] and visual-inertial [7,8]. Among these, the combination of cameras and inertial sensors, also known as visual-inertial system (VINS), has attracted more and more attention in recent years [7,8,9,10,11,12,13,14,15,16,17,18,19,20,21]. On the one hand, camera and inertial measurements offer complementary nature, and the combination of these two sensors can lead to significant improvements in accuracy of the estimation results and robustness of the system. On the other hand, with the development of microelectro mechanical systems (MEMS) inertial sensors, the cost of inertial measurement unit (IMU) steadily decline and an increasing number of robot platforms, drones and consumer electronics, especially mobile devices, are equipped with low-cost MEMS-based IMUs.

Due to the imperfections of the manufacturing and physical characteristics of the sensors, real IMU measurements are usually affected by systematic errors as well as random noise [22,23]. The process of identifying the intrinsic parameters of sensors for compensating these errors is known as IMU calibration. Although VINS have seen tremendous improvements in accuracy, robustness and efficiency over recent years, most visual-inertial methods either treat these measurement errors as sensor noise or assume the IMU that used is precalibrated offline. For high performance inertial sensors, which generally are manufactured precisely or factory calibrated carefully with each sensor is sold with its own calibration parameters stored into the firmware, providing accurate measurements off the shelf [23], this assumption is reasonable. However, for low-cost MEMS-based inertial sensors, which are usually used for consumer mobile robots, neglecting these errors may significantly influence the performance of VINS or even breakdown the whole system. Moreover, offline IMU calibration have certain shortcomings. Traditional high precision calibration process heavily rely on special external equipment such as robot manipulator [24,25,26], high accuracy turntable [27], motion tracked system [28]. Such equipment is usually very expensive. As a result, the cost of one calibration process often exceeds the cost of MEMS-based IMU. Secondly, the IMU measurement models used for calibration are usually simplified linear models, the intrinsic parameters of which may vary with mechanical shocks, temperature and other factors. Treating these parameters as constant would lead to unmodeled errors and degenerate the performance of the VINS. Therefore, even high quality offline-calibration is performed, this process still needs to be repeated periodically. Moreover, performing offline calibration often needs special operators and is time consuming, which hinders the rapid deployment of VINS to consumer devices.

To this end, we propose an online IMU self-calibration method for VINS. Specifically, we are interested in concurrently performing 3D pose estimation and online IMU calibration in unknown environment, using only camera and inertial measurements, and without any knowledge about the structure of the scene or any external equipment.

We implement the method with the extension of an open source monocular VINS pipeline, VINS-Mono [8], and demonstrate that, in this setting, it is possible to concurrently localize and perform online calibration of all of the following quantities: the axis misalignments and scale factors of the IMU sensors, the g-sensitivity (also called linear acceleration dependence) of the gyroscopes. What we have to claim is that although our implementation is based on VINS-Mono, the proposed online IMU self-calibration method can be easily integrated into other graph-based visual/visual-inertial frameworks as well as the multi-camera cases. To the best of our knowledge, this is the first paper to present a VINS pipeline with online IMU self-calibration based on optimization methods. We highlight our main contributions as follows:

1. An online IMU self-calibration method for visual-inertial system that can perform in real time in unknown environment without any external equipment.

2. We frame the IMU calibration problem into general factors so that the proposed method can be easily integrated into other graph-based visual/visual-inertial frameworks.

3. An extensive evaluation on publicly available dataset showing that the online calibration can obtain accurate IMU intrinsics estimation and significantly improve the estimation precision of the VINS method compared with the estimation results with offline precalibrated IMU measurements.

The rest of this paper is organized as follows. In Section 2, we discuss the relevant literature. The algorithm as well as the implementation details are introduced in Section 3. The experimental results are shown in Section 4. Finally, the paper is concluded with the discussion and possible future research directions in Section 5.

2. Related Work

Over the last decade, there has been tremendous progress in VINS. Existing approaches can be classified into filtering-based methods [9,10,11,12,13,14,15,16] and graph-based methods [7,8,17,18,19,20,21]. Filtering-based methods usually process the fusion with the extended Kalman filter (EKF) and its variants, in which, the measurements from IMU are used for state propagation and observations from vision are used for update. Filtering-based methods have the advantage of fast processing since it contentiously marginalizes past states. However, their performance are usually sub-optimization due to early fix of linearization points. Compared to filter-based methods, graph-based methods have attracted more attention in the recent years, as they obtain more accurate estimation results by maintaining measurements of long term history and perform batch optimization to obtain the optimal estimate. To achieve real time operation, graph-based methods usually apply keyframe-based techniques and optimize over a bounded-size sliding window of recent states by marginalizing past states and measurements. Nevertheless, all of these methods above either treat the IMU measurement errors as sensor noise or assume the IMU that used is precalibrated offline except the work in [13], which, to the best of our knowledge, is the only VINS method that implement IMU and camera online self-calibration in a filter-based framework. However, only simulation results were shown in this paper. Besides, their method cannot be applied for graph-based VINS methods.

For offline IMU calibration, numerous methods have been proposed over the last several decades, with the development of strapdown inertial navigation system or AHRS [22]. As we are concerned with online self-calibration, here we only give a brief review of these methods. Traditionally IMU calibration methods are usually done by using special external equipment that could provide known reference acceleration or rotational velocity of the inertial sensor. The measurements of the sensor are compared directly with the known reference value to determine the intrinsic parameters. Apparently, the accuracy of these methods strongly relies on the accuracy of the kinematic reference. To obtain highly accurate results, expensive mechanical platforms, such as robot manipulator or high accuracy turntable [24,25,26,27] are usually used, resulting in a calibration cost that often exceeds the cost of the IMU‘s hardware. In [28] the authors exploited using a marker-based optical tracking system to provide the reference value, while in [29], the GPS readings are used to calibrate initial biases and misalignments. In order to achieve in-situ calibration, the multi-position method was firstly introduced in [30], using the fact that the magnitude of the static acceleration must equal to the gravity‘s magnitude. However, this method can only calibrate the bias and scale factors of the accelerometer. This technique has been further extended by [31], in which, a method with two calibration schemes was presented. The accelerometer is firstly calibrated by exploiting the high local stability of the gravity vector‘s magnitude, and then the gyroscope is calibrated by comparing the gravity vector sensed by the calibrated accelerometer with the gravity vector obtained by integrating the angular velocities. The main advantage of this method is that it do not require any external mechanical equipment. A similarly approach also can be found in [23]. A self-calibration method based on an iterative matrix factorization was proposed by Hwangbo et al. [32]. this method use gravity as accelerometers reference, and a camera as gyroscopes reference. A noteworthy work in the VINS domain is the kalibr toolbox by Rehder et al. [33], in which, multiple camera-IMU extrinsic as well as the IMU intrinsics are jointly optimized in a single estimator with the continues-time batch optimization framework [33,34]. Impressive results were demonstrated in this work. However, their method cannot be integrated directly in the presented discrete-time graph-based VINS methods.

Motivated by IMU preintegration theories developed by [8,35,36,37,38], we firstly develop a novel preintegration method to handle the IMU intrinsic parameters error propagation. Then we frame the IMU preintegration measurements and IMU intrinsic parameters into general factors. In such a way, we can implement an optimization method to jointly estimate the IMU intrinsic parameters as well as the system state in a big bundle, which makes our method totally distinguished from the method in [13]. Compared to the the offline IMU calibration methods, our method integrates into the current VINS method and performs online IMU self-calibration, without any special equipment.

3. Methodology

3.1. Visual-Inertial System

Consider a sensing system (e.g., a mobile robot or a hand-held device) equipped with a monocular camera and a low-cost uncalibrated IMU. Our goal is to concurrently estimate the states of the sensing system, as well as the IMU intrinsics in a tightly-coupled graph-based framework. A general visual-inertial system is illustrated in Figure 1, in which, several keyframes and IMU measurements are maintained in a sliding window. When a new keyframe is inserted into the sliding window, a local bundle adjustment (BA) would be implemented to jointly optimize the camera and IMU states, as well as the feature location. After optimization, one of the old keyframe as well as its corresponding states and measurements would be marginalized from the sliding windows, limiting the sliding windows size and reducing computation complexity.

Figure 1.

Illustration of visual-inertial system. Several keyframes and inertial measurement unit (IMU) measurements are maintained in a sliding window. When a new keyframe is inserted into the sliding window, a local bundle adjustment (BA) would be implemented to jointly optimize the camera and IMU states, as well as the feature location.

The notations and frame definitions used in this paper are briefly described as follows. We denote the world frame and the body frame (BF) as ${(·)}^w$ and ${(·)}^b$, respectively. The z-axis of the world frame is aligned with the direction of the gravity. The BF is defined to be the same as the accelerometer orthogonal frame (AOF). We primarily use quaternions $\mathit{q}$ with Hamilton convention to represent rotation, but rotation matrices $\mathit{R}$ are also used for the convenient representation of 3D vectors rotation. ${\mathit{q}}_b^w$ represents the orientation of body frame with respect to the word frame. ${\mathit{R}}_b^w$ is the corresponding rotation matrix of ${\mathit{q}}_b^w$. The parameter ${\mathit{p}}_b^w$ is the translation from the world frame to the body frame, being expressed in word frame, $b_k$ is the body frame while taking the k-th image, ${\mathit{g}}^w$ and ${\mathit{g}}^b$ are the gravity vectors in the world frame and body frame, respectively.

The full state vector in the sliding window is defined as

$$\begin{array}{cc}\hfill \mathcal{X}& =[{\mathit{x}}_0,{\mathit{x}}_1,\cdots ,{\mathit{x}}_n,{\mathit{x}}_{\mathcal{I}},{\mathit{f}}_0,{\mathit{f}}_1,\cdots ,{\mathit{f}}_m]\hfill \\ \hfill {\mathit{x}}_k& =[{\mathit{p}}_{b_k}^w,{\mathit{q}}_{b_k}^w,{\mathit{v}}_{b_k}^w,{\mathit{b}}_a,{\mathit{b}}_{\omega }],\hfill \end{array}$$ (1)

where ${\mathit{x}}_k$ is the IMU state at the k-th image. It contains position, orientation and velocity of the IMU as well as the accelerometer bias ${\mathit{b}}_a$ and gyroscope bias ${\mathit{b}}_{\omega }$. The parameter ${\mathit{x}}_{\mathcal{I}}$ is the IMU intrinsic parameters, ${\mathit{f}}_l$ is the feature location, which can be parameterized by either the 3D position or inverse depth of the feature, n is the number of keyframes in the sliding window, m is the number of features which have been observed at least by two frames in the sliding window.

After the VINS was successfully initialized with the method proposed in [8,39], we were able to use a sliding window based framework to jointly optimize both the full system states as described in Equation (1). We minimize the sum of prior and the Mahalanobis norm of all measurement residuals to obtain a maximum posteriori estimation as

$$\underset{\mathcal{X}}{min}\left\{{∥{\mathit{r}}_p-{\mathit{H}}_p\mathcal{X}∥}^2+\sum _{k\in \mathcal{B}}{∥{\mathit{r}}_{\mathcal{B}}\left({\widehat{\mathit{z}}}_{b_{k+1}}^{b_k},\mathcal{X}\right)∥}_{{\mathit{P}}_{b_{k+1}}^{b_k}}^2+\sum _{(l,j)\in \mathcal{C}}{∥{\mathit{r}}_{\mathcal{C}}\left({\widehat{\mathit{z}}}_l^{c_j},\mathcal{X}\right)∥}_{{\mathit{P}}_l^{c_j}}^2+{∥{\mathit{r}}_{\mathcal{I}}\left(\mathit{x}\right)∥}_{{\mathit{P}}_{\mathcal{I}}}^2\right\},$$ (2)

where $\{{\mathit{r}}_p,{\mathit{H}}_p\}$ are the prior information from marginalization. ${\mathit{r}}_B({\widehat{\mathit{z}}}_{b_{k+1}}^{b_k}\mathcal{X})$ and ${\mathit{r}}_C({\widehat{\mathit{z}}}_l^{c_j},\mathcal{X})$ are residuals for the IMU and visual measurements respectively, where $\mathcal{B}$ is the set of all IMU measurements and $\mathcal{C}$ is the set of features. The parameter ${\mathit{r}}_{\mathcal{I}}(\mathit{x})$ is the residuals for IMU intrinsic parameters.

As our goal is to estimate the IMU intrinsic parameters online, in this paper, we assume the translation and orientation between camera and IMU are fixed and known from prior calibration. Furthermore, we assume there is a vision front-end for feature detection and tracking, providing the pixel measurements of the features. Therefore, we will neglect the residuals of vision measurements ${\mathit{r}}_C({\widehat{\mathit{z}}}_l^{c_j},\mathcal{X})$ in this paper and only detail the residuals of IMU measurements ${\mathit{r}}_B({\widehat{\mathit{z}}}_{b_{k+1}}^{b_k}\mathcal{X})$ as well as the IMU intrinsics ${\mathit{r}}_{\mathcal{I}}(\mathit{x})$ in the following.

3.2. IMU Model

A six-degree-of-freedom IMU composes by a triple axis accelerometer and a triple axis gyroscope. Ideally, the two triple-axis define a single, shared, orthogonal 3D frame. The accelerometer senses the acceleration of the sensor along of each input axis, while gyroscope measures the angular velocity around each input axis. The scale factors convert analog or digital output of the sensor to real physical quantity measurements. Unfortunately, due to assembly imperfections, each axis of the accelerometer and gyroscope deviates by a small angle from its designed mounting direction, as a result, introducing axis misalignment and cross-axis sensitivity errors to the measurements. Also, real scale factor of each axis is unique and different from the nominal value provided by the manufacturer. In additional, the angular velocities measurements are affected by the accelerations that the sensor is subjected. Moreover, all of the measurements are almost always affected by variable bias. In order to model and compensate these errors, we employ the IMU measurement model as described in [33].

$$\begin{array}{cc}\hfill {\mathit{a}}_m& ={\mathit{S}}_a{\mathit{M}}_a({\mathit{a}}_{wb}^b-{\mathit{g}}^b)+{\mathit{b}}_a+{\mathit{n}}_a\hfill \end{array}$$ (3)

$$\begin{array}{cc}\hfill {\mathit{\omega }}_m& ={\mathit{S}}_{\omega }{\mathit{M}}_{\omega }{\mathit{\omega }}_{wb}^b+{\mathit{A}}_{\omega }({\mathit{a}}_{wb}^b-{\mathit{g}}^b)+{\mathit{b}}_{\omega }+{\mathit{n}}_{\omega },\hfill \end{array}$$ (4)

where ${\mathit{a}}_m$ and ${\mathit{\omega }}_m$ are the measurement of accelerometer and gyroscope, respectively, ${\mathit{S}}_a$ is a $3\times 3$ diagonal matrix constructed by the scale factors of the triple axis, ${\mathit{M}}_a$ is a $3\times 3$ lower triangular matrix representing the axis misalignments and the cross-coupling terms, and ${\mathit{S}}_{\omega }$ and ${\mathit{M}}_{\omega }$ are defined analogously to ${\mathit{S}}_a$ and ${\mathit{M}}_a$. Each row of ${\mathit{M}}_a$ and ${\mathit{M}}_{\omega }$ is a unit vector. The parameter ${\mathit{A}}_{\omega }$ is a full populated matrix representing the g-sensitivity coefficients of gyroscope, ${\mathit{a}}_{wb}^b$ and ${\mathit{\omega }}_{wb}^b$ are the body frame acceleration and angular velocity with respect to the word frame, being expressed in the body frame, ${\mathit{b}}_a$ and ${\mathit{b}}_{\omega }$ is the measurement bias of accelerometer and gyroscope respectively, ${\mathit{n}}_a$, ${\mathit{n}}_{\omega }$ is the measurement noise. We assume that ${\mathit{n}}_a$, ${\mathit{n}}_{\omega }$ are Gaussian white noise, ${\mathit{n}}_a\sim \mathcal{N}(0,{\mathit{\sigma }}_a^2)$, ${\mathit{n}}_{\omega }\sim \mathcal{N}(0,{\mathit{\sigma }}_{\omega }^2)$. For the derivations of Equations (3) and (4), please refer to [33].

The accelerometer and gyroscope bias are modeled as random walks, the derivatives of which are Gaussian white noise, ${\mathit{n}}_{b_a}\sim \mathcal{N}(0,{\mathit{\sigma }}_{b_a}^2)$, ${\mathit{n}}_{b_{\omega }}\sim \mathcal{N}(0,{\mathit{\sigma }}_{b_{\omega }}^2)$.

$${\dot{\mathit{b}}}_a={\mathit{n}}_{b_a},{\dot{\mathit{b}}}_{\omega }={\mathit{n}}_{b_{\omega }}.$$ (5)

It is inconvenient to optimize the ${\mathit{M}}_a$ and ${\mathit{M}}_{\omega }$ as each row of these two matrices is constrained to a unit vector. Besides, when integrating the angular velocity and acceleration to obtain the motion of the system, we need to compute the inverse matrix of ${\mathit{S}}_a$, ${\mathit{M}}_a$, ${\mathit{S}}_{\omega }$, ${\mathit{M}}_{\omega }$ at every step to compensate the measurement errors, which is a waste of computing resources. If defining ${\mathit{T}}_a={({\mathit{S}}_a{\mathit{M}}_a)}^{-1}$, ${\mathit{T}}_{\omega }={({\mathit{S}}_{\omega }{\mathit{M}}_{\omega })}^{-1}$, we could get the compensated acceleration and angular velocity from the measurements more effectively:

$$\begin{array}{cc}\hfill {\mathit{a}}_{wb}^b& ={\mathit{T}}_a({\mathit{a}}_m-{\mathit{b}}_a-{\mathit{n}}_a)+{\mathit{g}}^b\hfill \end{array}$$ (6)

$$\begin{array}{cc}\hfill {\mathit{\omega }}_{wb}^b& ={\mathit{T}}_{\omega }\left({\mathit{\omega }}_m-{\mathit{A}}_{\omega }({\mathit{a}}_{wb}^b-{\mathit{g}}^b)-{\mathit{b}}_{\omega }-{\mathit{n}}_{\omega }\right),\hfill \end{array}$$ (7)

where, ${\mathit{T}}_a$ is still a lower triangular matrix, ${\mathit{T}}_{\omega }$ is a full populated matrix without constraint. For an uncalibrated IMU, ${\mathit{T}}_a$, ${\mathit{T}}_{\omega }$ is roughly close to identify matrix with some uncertainty. ${\mathit{A}}_{\omega }$ is close to $\mathbf{0}$.

3.3. IMU Preintegration

The IMU preintegration method was firstly introduced in [35], which parameterize the rotation error using Euler angles. This method was further improved in [8] with the continuous-time quaternion-based derivation and including the handling of IMU biases. The intuitive concept of IMU preintegration is illustrated in Figure 2. The proposed an IMU preintegration method that is motivated by [8], but is different from the previous work as we introduce the IMU intrinsic parameters in the measurement model. Therefore, we need to re-derive all of the equations from scratch.

Figure 2.

Illustration of the concept of IMU preintegration. The IMU measures the acceleration and angular velocity at discrete time. Generally, The IMU measurement frequency is much larger than the frame rate of camera. Giving two time consecutive frames $b_k$ and $b_{k+1}$, there exists several measurements in time interval $[t_k,t_{k+1}]$. The IMU measurements between image frames k and $k+1$ are pre-integrated into a single compound measurement. From an optimization perspective, it can be seen as the observations or measurements with which we can construct the error terms and adjust the corresponding state values to minimize the errors.

Theoretically, giving two time instants at images frames k and $k+1$, we can compute the states at $t_{k+1}$ as a function of IMU measurements between two frames:

$$\begin{array}{cc}\hfill {\mathit{p}}_{b_{k+1}}^w& ={\mathit{p}}_{b_k}^w+{\mathit{v}}_{b_k}^w\Delta t_k+\frac{1}{2}{\mathit{g}}^w\Delta t_k^2+{\int \int }_{t\in [k,k+1]}{R}_{b_t}^w{\mathit{T}}_a({\mathit{a}}_{m_t}-{\mathit{b}}_{a_t}-{\mathit{n}}_{a_t})\mathrm{d}{t}^2\hfill \end{array}$$ (8)

$$\begin{array}{cc}\hfill {\mathit{v}}_{b_{k+1}}^w& ={\mathit{v}}_{b_k}^w+{\mathit{g}}^w\Delta t_k+{\int }_{t\in [k,k+1]}{\mathit{R}}_{b_t}^w{\mathit{T}}_a({\mathit{a}}_{m_t}-{\mathit{b}}_{a_t}-{\mathit{n}}_{a_t})\mathrm{d}t\hfill \end{array}$$ (9)

$$\begin{array}{cc}\hfill {\mathit{q}}_{b_{k+1}}^w& ={\mathit{q}}_{b_k}^w\otimes {\int }_{t\in [k,k+1]}\frac{1}{2}{\mathit{q}}_{b_t}^w\otimes {\mathit{T}}_{\omega }\left({\mathit{\omega }}_{m_t}-{\mathit{A}}_{\omega }{T}_a({\mathit{a}}_{m_t}-{\mathit{b}}_{a_t}-{\mathit{n}}_{a_t})-{\mathit{b}}_{{\omega }_t}-{\mathit{n}}_{{\omega }_t}\right)\mathrm{d}t,\hfill \end{array}$$ (10)

where $\Delta t_k$ is the duration of time interval $[t_k,t_{k+1}]$.

While Equations (8)–(10) provide an estimation of motion between time $t_k$ and $t_{k+1}$, it can be seen that the IMU state propagation requires the rotation, position and velocity of frame k as the starting states. In the optimization-based algorithm, we adjust the full system states at every iteration step, which means these starting states may change. In such cases, we need to re-integrate the IMU measurements, which is computation consuming. To avoid this, we adopt the preintegration method.

Premultiply ${\mathit{q}}_w^{b_k}$ or ${\mathit{R}}_w^{b_k}$ to both sides of Equations (8)–(10) to change the reference frame for the IMU propagation to the local frame $b_k$:

$$\begin{array}{cc}\hfill {\mathit{R}}_w^{b_k}{\mathit{p}}_{b_{k+1}}^w& ={\mathit{R}}_{b_k}^w({\mathit{p}}_{b_k}^w+{\mathit{v}}_{b_k}^w\Delta t+\frac{1}{2}{\mathit{g}}^w\Delta t^2)+{\mathit{\alpha }}_{b_{k+1}}^{b_k}\hfill \end{array}$$ (11)

$$\begin{array}{cc}\hfill {\mathit{R}}_w^{b_k}{\mathit{v}}_{b_{k+1}}^w& ={\mathit{R}}_w^{b_k}{\mathit{v}}_{b_k}^w+{\mathit{R}}_w^{b_k}{\mathit{g}}^w\Delta t_k+{\mathit{\beta }}_{b_{k+1}}^{b_k}\hfill \end{array}$$ (12)

$$\begin{array}{cc}\hfill {\mathit{q}}_w^{b_k}\otimes {\mathit{q}}_{b_{k+1}}^w& ={\mathit{\gamma }}_{b_{k+1}}^{b_k},\hfill \end{array}$$ (13)

where

$$\begin{array}{cc}\hfill {\mathit{\alpha }}_{b_{k+1}}^{b_k}& ={\int \int }_{t\in [k,k+1]}{\mathit{R}}_{b_t}^{b_k}{\mathit{T}}_a({\mathit{a}}_{m_t}-{\mathit{b}}_{a_t}-{\mathit{n}}_{a_t})\mathrm{d}{t}^2\hfill \end{array}$$ (14)

$$\begin{array}{cc}\hfill {\mathit{\beta }}_{b_{k+1}}^{b_k}& ={\int }_{t\in [k,k+1]}{\mathit{R}}_{b_t}^{b_k}{\mathit{T}}_a({\mathit{a}}_{m_t}-{\mathit{b}}_{a_t}-{\mathit{n}}_{a_t})\mathrm{d}t\hfill \end{array}$$ (15)

$$\begin{array}{cc}\hfill {\mathit{\gamma }}_{b_{k+1}}^{b_k}& ={\int }_{t\in [k,k+1]}\frac{1}{2}{\mathit{\gamma }}_{b_t}^{b_k}\otimes {\mathit{T}}_{\omega }({\mathit{\omega }}_{m_t}-{\mathit{A}}_{\omega }{T}_a({\mathit{a}}_{m_t}-{\mathit{b}}_{a_t}-{\mathit{n}}_{a_t})-{\mathit{b}}_{{\omega }_t}-{\mathit{n}}_{{\omega }_t})\mathrm{d}t.\hfill \end{array}$$ (16)

The preintegration parts ${\mathit{\alpha }}_{b_{k+1}}^{b_k}$, ${\mathit{\beta }}_{b_{k+1}}^{b_k}$, ${\mathit{\gamma }}_{b_{k+1}}^{b_k}$ can be obtained solely with IMU measurements and its intrinsic parameters. These preintegrated measurements construct the constraints between the states at time $t_k$ and the states at time $t_{k+1}$. Therefore, we can construct the error terms with these preintegrated IMU measurements, and adjust the corresponding state values to minimize the errors base on optimization method.

It can be seen that the preintegration measurements in Equations (14)–(16) are also the function of the IMU intrinsic parameters, ${\mathit{T}}_a$, ${\mathit{T}}_{\omega }$, ${\mathit{A}}_{\omega }$ as well as the bias ${\mathit{b}}_a$, ${\mathit{b}}_{\omega }$. As a result, the imperfection of these parameters would introduce errors into the measurements. This also means that we can concurrently optimize the IMU intrinsic parameters to minimize the errors constrained by the preintegration measurements. To achieve this goal, two problems need to be dealt with:

1. Compute the Jacobian matrix of ${\mathit{\alpha }}_{b_{k+1}}^{b_k}$, ${\mathit{\beta }}_{b_{k+1}}^{b_k}$, ${\mathit{\gamma }}_{b_{k+1}}^{b_k}$ with respect to the IMU intrinsic parameters. The Jacobian matrix will be used in two ways: (1) for the optimization method, the Jacobian matrix would be used to evaluate the increment of the input arguments; (2) at the end of each iteration step, we use Jacobian as well as the new adjusted states will be used to compute the first-order approximation of preintegration measurement so that we can avoid re-integrating the IMU measurements.

2. Estimate the uncertainty of the preintegration measurements. All of the IMU measurements contain random noises. These noises, as well as the the inaccuracy of the IMU intrinsic parameters, introduce the uncertainty to the preintegration results. In order to get more precise optimization results, we need to estimate the uncertainty of the preintegration measurements in the form of a covariance matrix or information matrix and use these uncertainty information to weight the error terms during optimization.

These two problems will be tackled in the next section.

3.4. Jacobian and Noise Propagation

In order to compute the Jacobian matrix and estimate the uncertainty of preintegration results, we divide the true state value of preintegration result $\mathit{x}$ into two components, the nominal state $\widehat{\mathit{x}}$ and the error state $\delta \mathit{x}$. The nominal state is estimated by integrating the IMU measurements directly, without taking into account the noise terms and imperfect intrinsic parameters or unmodeled error. As a results, it will accumulate errors. These errors are collected into the error state. In this way, the true state can be expressed as a composition of the nominal state and error state, that is,

$$\begin{array}{c}\hfill \mathit{x}=\widehat{\mathit{x}}\oplus \delta \mathit{x},\end{array}$$ (17)

where ⊕ is the composition operator. Since the quaternion $\mathit{q}$ is over-parameterized, we define its error term with the minimal representation $\delta \mathit{\theta }$. As a result, for quaternion, ⊕ is defined as

$$\mathit{q}=\widehat{\mathit{q}}\oplus \delta \mathit{\theta }\approx \widehat{\mathit{q}}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathit{\theta }\end{array}\right]\Rightarrow \delta \mathit{\theta }=2{[{\widehat{\mathit{q}}}^{-1}\otimes \mathit{q}]}_{xyz},$$ (18)

where ${[·]}_{xyz}$ extracts the vector part of a quaternion $\mathit{q}$. For other states, $\widehat{\mathit{x}}\oplus \delta \widehat{\mathit{x}}\triangleq \widehat{\mathit{x}}+\delta \mathit{x}$.

The concept of nominal-state and error-state is similar to the error-state Kalman filter (ESKF) [40,41]. The main difference is that we need to tackle the imperfection of the IMU intrinsic parameters. This is a nontrivial extension as it introduces 24 more parameters into the state space, which makes the equations more complex compared to the general ESKF and preintegration methods showed in [8,37,38].

The nominal-state is computed as follows:

$$\begin{array}{cc}\hfill {\widehat{\mathit{\alpha }}}_{b_{k+1}}^{b_k}& ={\int \int }_{t\in [k,k+1]}{\widehat{\mathit{R}}}_{b_t}^{b_k}{\widehat{\mathit{T}}}_a({\mathit{a}}_{m_t}-{\widehat{\mathit{b}}}_{a_t})\mathrm{d}{t}^2\hfill \end{array}$$ (19)

$$\begin{array}{cc}\hfill {\widehat{\mathit{\beta }}}_{b_{k+1}}^{b_k}& ={\int }_{t\in [k,k+1]}{\widehat{\mathit{R}}}_{b_t}^{b_k}{\widehat{\mathit{T}}}_a({\mathit{a}}_{m_t}-{\widehat{\mathit{b}}}_{a_t})\mathrm{d}t\hfill \end{array}$$ (20)

$$\begin{array}{cc}\hfill {\widehat{\mathit{\gamma }}}_{b_{k+1}}^{b_k}& ={\int }_{t\in [k,k+1]}\frac{1}{2}{\widehat{\mathit{\gamma }}}_{b_t}^{b_k}\otimes {\widehat{\mathit{T}}}_{\omega }\left({\mathit{\omega }}_{m_t}-{\widehat{\mathit{A}}}_{\omega }{\widehat{T}}_a({\mathit{a}}_{m_t}-{\widehat{\mathit{b}}}_{a_t})-{\widehat{\mathit{b}}}_{{\omega }_t}\right)\mathrm{d}t.\hfill \end{array}$$ (21)

As the real IMU measures the acceleration and angular velocity at discrete time, Equations (19)–(21) can be computed by numberical integraion methods such as Euler, mid-point or Runge—Kutta methods. The mid-point integration is used in our implementation code, which is detailed in the Appendix A.2. At the beginning of integration, ${\mathit{\alpha }}_{b_k}^{b_k}$, ${\mathit{\beta }}_{b_k}^{b_k}$ is set to $\mathbf{0}$, and ${\mathit{\gamma }}_{b_k}^{b_k}$ is set to identity quaternion.

With the computation above, all the large-signal dynamics have been integrated in the nominal-state. As a result, the error-state is always small. We can solve its kinematics and neglect all second-order infinitesimals to get the continuous-time linearized dynamics.

$$\begin{array}{cc}\hfill \delta {\dot{\mathit{\alpha }}}_{b_t}^{b_k}& =\delta {\mathit{\beta }}_{b_t}^{b_k}\hfill \end{array}$$ (22)

$$\begin{array}{cc}\hfill \delta {\dot{\mathit{\beta }}}_{b_t}^{b_k}& =-{\widehat{\mathit{R}}}_{b_t}^{b_k}{[{\widehat{T}}_a\widehat{\mathit{a}}]}_{\times }\delta {\theta }_{b_t}^{b_k}-{\widehat{\mathit{R}}}_{b_t}^{b_k}{\widehat{\mathit{T}}}_a\delta {\widehat{\mathit{b}}}_a+{\widehat{\mathit{R}}}_{b_t}^{b_k}\delta {\mathit{T}}_a{\widehat{\mathit{a}}}_B-{\widehat{\mathit{R}}}_{b_t}^{b_k}{\mathit{T}}_a{\mathit{n}}_a\hfill \end{array}$$ (23)

$$\begin{array}{cc}\hfill \delta {\dot{\mathit{\theta }}}_{b_t}^{b_k}=& -{[{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B]}_{\times }\delta {\mathit{\theta }}_{b_t}^{b_k}+{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }{\widehat{\mathit{T}}}_a\delta {\mathit{b}}_a-{\widehat{\mathit{T}}}_{\omega }\delta {\mathit{b}}_{\omega }-{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }\delta {\mathit{T}}_a{\widehat{\mathit{a}}}_B\hfill \\ & +\delta {\mathit{T}}_{\omega }{\widehat{\mathit{\omega }}}_B-{\widehat{\mathit{T}}}_{\omega }\delta {\mathit{A}}_{\omega }{\mathit{T}}_a{\widehat{\mathit{a}}}_B+{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }{\widehat{\mathit{T}}}_a{\mathit{n}}_a-{\widehat{\mathit{T}}}_{\omega }{\mathit{n}}_{\omega },\hfill \end{array}$$ (24)

where ${\widehat{\mathit{R}}}_{b_t}^{b_k}$ is the corresponding rotation matrix of ${\mathit{\gamma }}_{b_t}^{b_k}$, ${\widehat{\mathit{a}}}_B=({\mathit{a}}_{m_t}-{\widehat{\mathit{b}}}_{a_t})$, ${\widehat{\mathit{\omega }}}_B={\mathit{\omega }}_{m_t}-{\widehat{\mathit{A}}}_{\omega }{\widehat{T}}_a({\mathit{a}}_{m_t}-{\widehat{\mathit{b}}}_{a_t})-{\widehat{\mathit{b}}}_{{\omega }_t}$, ${[·]}_{\times }$ is the skew symmetry matrix operator,

$${[\mathit{\omega }]}_{\times }=\left[\begin{array}{ccc}0& -{\omega }_x& {\omega }_y\\ {\omega }_x& 0& -{\omega }_z\\ -{\omega }_y& {\omega }_z& 0\end{array}\right].$$ (25)

Details about how to get the error-state kinematic representation above is given in the Appendix A.1.

The error-state dynamics of the IMU intrinsic parameters are as follows

$$\begin{array}{cc}\hfill \delta {\dot{\mathit{b}}}_a& ={\mathit{n}}_{b_a},\delta {\dot{\mathit{b}}}_{\omega }={\mathit{n}}_{b_{\omega }}\hfill \end{array}$$ (26)

$$\begin{array}{cc}\hfill \delta {\dot{\mathit{T}}}_a& =\mathbf{0},\delta {\dot{\mathit{T}}}_{\omega }=\mathbf{0},\delta {\dot{\mathit{A}}}_{\omega }=\mathbf{0}.\hfill \end{array}$$ (27)

In order to compute the the Jacobian matrix and covariance recursively, we need to convert Equations (22)–(24), (26) and (27) to matrix representation. This takes a little trick.

Define

$$\begin{array}{cc}\hfill {\mathcal{X}}_{pre}& =[{\mathit{\alpha }}_{b_t}^{b_k},{\mathit{\beta }}_{b_t}^{b_k},{\mathit{\gamma }}_{b_t}^{b_k},{\mathit{b}}_a,{\mathit{b}}_{\omega },{\mathit{t}}_a,{\mathit{t}}_{\omega },{\mathit{a}}_{\omega }],\hfill \end{array}$$ (28)

where

$$\begin{array}{cc}\hfill {\mathit{t}}_a& ={[T_{a_{11}},T_{a_{21}},T_{a_{22}},T_{a_{31}},T_{a_{32}},T_{a_{33}}]}^T\hfill \\ \hfill {\mathit{t}}_{\omega }& ={[T_{{\omega }_{11}},T_{{\omega }_{12}},T_{{\omega }_{13}},T_{{\omega }_{21}},T_{{\omega }_{22}},T_{{\omega }_{23}},T_{{\omega }_{31}},T_{{\omega }_{32}},T_{{\omega }_{33}}]}^T\hfill \\ \hfill {\mathit{a}}_{\omega }& ={[A_{{\omega }_{11}},A_{{\omega }_{12}},A_{{\omega }_{13}},A_{{\omega }_{21}},A_{{\omega }_{22}},A_{{\omega }_{23}},A_{{\omega }_{31}},A_{{\omega }_{32}},A_{{\omega }_{33}}]}^T.\hfill \end{array}$$

With $T_{a_{ij}}$, $T_{{\omega }_{ij}}$, $A_{{\omega }_{ij}}$ are the corresponding elements of the matrix ${\mathit{T}}_a$, ${\mathit{T}}_{\omega }$, ${\mathit{A}}_{\omega }$, respectively.

Assume ${\mathit{r}}_{3\times 1}={[r_1,r_2,r_3]}^T$. Now we define the ${[·]}_T$ operator as

$${[{\mathit{r}}_{3\times 1}]}_T={\left[\begin{array}{cccccc}{r}_1& 0& 0& 0& 0& 0\\ 0& r_1& r_2& 0& 0& 0\\ 0& 0& 0& r_1& r_2& r_3\end{array}\right]}_{3\times 6},\mathrm{such}\mathrm{that}{\mathit{T}}_a{\mathit{r}}_{3\times 1}={[{\mathit{r}}_{3\times 1}]}_T{\mathit{t}}_a$$

$$\mathrm{and}{[{\mathit{r}}_{3\times 1}]}_T={\left[\begin{array}{ccc}{\mathit{r}}^T& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\mathit{r}}^T& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& {\mathit{r}}^T\end{array}\right]}_{3\times 9},\mathrm{such}\mathrm{that}{\mathit{T}}_{\omega }{\mathit{r}}_{\mathbf{3}\times \mathbf{1}}={[{\mathit{r}}_{\mathbf{3}\times \mathbf{1}}]}_T{\mathit{t}}_{\omega },{\mathit{A}}_{\omega }{\mathit{r}}_{\mathbf{3}\times \mathbf{1}}={[{\mathit{r}}_{\mathbf{3}\times \mathbf{1}}]}_T{\mathit{a}}_{\omega }.$$

With the definitions above, Equations (22)–(24), (26) and (27) can be converted to matrix representation form

$$\begin{array}{c}\hfill \delta {\dot{\mathcal{X}}}_{pre}={\mathit{F}}_t\delta {\mathcal{X}}_{pre}+{\mathit{G}}_t\mathit{n}\end{array}$$ (29)

where

$${\mathit{F}}_t=\left[\begin{array}{c}\begin{array}{c}\begin{array}{cccccccc}\mathbf{0}& \mathit{I}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& -{\widehat{\mathit{R}}}_{b_t}^{b_k}{[{\widehat{\mathit{T}}}_a\widehat{\mathit{a}}]}_{\times }& -{\widehat{\mathit{R}}}_{b_t}^{b_k}{\widehat{\mathit{T}}}_a& \mathbf{0}& {\widehat{\mathit{R}}}_{b_t}^{b_k}{[{\widehat{\mathit{a}}}_t]}_T& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& {[{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_t]}_{\times }& {\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }{\widehat{\mathit{T}}}_a& {\widehat{\mathit{T}}}_{\omega }& -{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }{[{\widehat{\mathit{a}}}_t]}_T& {[\widehat{\mathit{\omega }}]}_T& {\widehat{\mathit{T}}}_{\omega }{[{\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_t]}_T\end{array}\\ \hfill {\mathbf{0}}_{30\times 39}\hfill \end{array}\end{array}\right]$$

$${\mathit{G}}_t=\left[\begin{array}{c}\begin{array}{c}\begin{array}{cccc}\mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ {\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }{\widehat{\mathit{T}}}_a& -{\widehat{\mathit{R}}}_{b_t}^{b_k}{\widehat{\mathit{T}}}_a& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& -{\widehat{\mathit{T}}}_{\omega }& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathit{I}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathit{I}\end{array}\\ {\mathbf{0}}_{24\times 12}\end{array}\end{array}\right],$$

and $\mathit{n}=[{\mathit{n}}_a,{\mathit{n}}_{\omega },{\mathit{n}}_{b_a},{\mathit{n}}_{b_{\omega }}]$.

With the Equation (29), it is convenient to compute the the covariance matrix ${\mathit{P}}_{b_{k+1}}^{b_k}$ recursively by the first order discrete-time covariance update

$${\mathit{P}}_{b_{t+\delta t}}^{b_k}=(\mathit{I}+{\mathit{F}}_t\delta t){\mathit{P}}_{b_t}^{b_k}{(\mathit{I}+{\mathit{F}}_t\delta t)}^T+({\mathit{G}}_t\delta t)\mathit{Q}{({\mathit{G}}_t\delta t)}^T,$$ (30)

where $\delta t$ is the time between two IMU measurements, and $\mathit{Q}$ is the diagonal covariance matrix of noise $({\mathit{\sigma }}_a^2,{\mathit{\sigma }}_g^2,{\mathit{\sigma }}_{b_a}^2,{\mathit{\sigma }}_{b_g}^2)$. The initial covariance value corresponding to $\mathit{\alpha }$, $\mathit{\beta }$, $\mathit{\gamma }$, ${\mathit{b}}_a$, ${\mathit{b}}_{\omega }$ is set to 0. For ${\mathit{T}}_a$, ${\mathit{T}}_{\omega }$, ${\mathit{A}}_{\omega }$, the initial covariance can be set to 0 or a constant uncertainty, which will be discussed in Section 3.6.

Now we deal with the Jacobian matrix. Neglecting the noise term, Equation (29) can be converted to a discrete-time representation as following:

$$\begin{array}{c}\hfill \delta {\mathcal{X}}_{pre}(t+\delta t)=(1+{\mathit{F}}_t\delta t)\delta {\mathcal{X}}_{pre}(t).\end{array}$$ (31)

As a result, the Jacobian of $\delta {\mathcal{X}}_{pre}(t+\delta t)$ with respect to $\delta {\mathcal{X}}_{pre}(t)$ can be obtained conveniently:

$${\mathit{J}}_t^{t+\delta t}=\frac{\partial \delta {\partial \mathcal{X}}_{pre}(t+\delta t)}{\delta {\mathcal{X}}_{pre}(t)}=1+{\mathit{F}}_t\delta t.$$ (32)

We can compute $J_k^{k+1}$ recursively:

$$\begin{array}{cc}\hfill {\mathit{J}}_k^{k+1}& =\frac{\partial \delta {\mathcal{X}}_{pre}(j)}{\partial \delta {\mathcal{X}}_{pre}(j-1)}\frac{\partial \delta {\mathcal{X}}_{pre}(j-1)}{\partial \delta {\mathcal{X}}_{pre}(j-2)}\cdots \frac{\partial \delta {\mathcal{X}}_{pre}(i+1)}{\partial \delta {\mathcal{X}}_{pre}(i)}\hfill \\ & =(I+{\mathit{F}}_{t,j-1}\delta t_{j-1})(I+{\mathit{F}}_{t,j-2}\delta t_{j-2})\cdots (I+{\mathit{F}}_i\delta t_i),\hfill \end{array}$$ (33)

where i and j represent the i-th and j-th IMU measurement obtained at the time instant of frame k and $k+1$, respectively, as illustrated in Figure 2.

The first order approximation of ${\mathit{\alpha }}_{b_{k+1}}$, ${\mathit{\beta }}_{b_{k+1}}$, ${\mathit{\gamma }}_{b_{k+1}}$ with respect to the IMU intrinsic parameters can be computed as:

$$\begin{array}{cc}\hfill {\mathit{\alpha }}_{b_{k+1}}^{b_k}& ={\widehat{\mathit{\alpha }}}_{b_{k+1}}^{b_k}+{\mathit{J}}_{b_a}^{\alpha }\delta {\mathit{b}}_{a_k}+{\mathit{J}}_{b_{\omega }}^{\alpha }\delta {\mathit{b}}_{{\omega }_k}+{\mathit{J}}_{t_a}^{\alpha }\delta {\mathit{t}}_{a_k}+{\mathit{J}}_{t_{\omega }}^{\alpha }\delta {\mathit{t}}_{{\omega }_k}+{\mathit{J}}_{a_{\omega }}^{\alpha }\delta {\mathit{a}}_{{\omega }_k}\hfill \end{array}$$ (34)

$$\begin{array}{cc}\hfill {\mathit{\beta }}_{b_{k+1}}^{b_k}& ={\widehat{\mathit{\beta }}}_{b_{k+1}}^{b_k}+{\mathit{J}}_{b_a}^{\beta }\delta {\mathit{b}}_{a_k}+{\mathit{J}}_{b_{\omega }}^{\beta }\delta {\mathit{b}}_{{\omega }_k}+{\mathit{J}}_{t_a}^{\beta }\delta {\mathit{t}}_{a_k}+{\mathit{J}}_{t_{\omega }}^{\beta }\delta {\mathit{t}}_{{\omega }_k}+{\mathit{J}}_{a_{\omega }}^{\beta }\delta {\mathit{a}}_{{\omega }_k}\hfill \end{array}$$ (35)

$$\begin{array}{cc}\hfill {\mathit{\gamma }}_{b_{k+1}}^{b_k}& ={\widehat{\mathit{\gamma }}}_{b_{k+1}}^{b_k}\otimes ({\mathit{J}}_{b_a}^{\gamma }\delta {\mathit{b}}_{a_k}+{\mathit{J}}_{b_{\omega }}^{\gamma }\delta {\mathit{b}}_{{\omega }_k}+{\mathit{J}}_{t_a}^{\gamma }\delta {\mathit{t}}_{a_k}+{\mathit{J}}_{t_{\omega }}^{\gamma }\delta {\mathit{t}}_{{\omega }_k}+{\mathit{J}}_{a_{\omega }}^{\gamma }\delta {\mathit{a}}_{{\omega }_k}),\hfill \end{array}$$ (36)

where ${\mathit{J}}_{b_a}^{\alpha }$ is the sub-block matrix of ${\mathit{J}}_{b_{k+1}}$ whose location is corresponding to $\frac{\partial \delta {\mathit{\alpha }}_{b_{k+1}}^{b_k}}{{\mathit{b}}_{a_k}}$. The same meaning is also used for other $\mathit{J}$ in the above.

At each step of optimization, we can use Equations (34)–(36) to update the value of $\mathit{\alpha }$, $\mathit{\beta }$, $\mathit{\gamma }$ with the new estimated IMU intrinsic parameters and recompute the corresponding residuals.

3.5. IMU Measurement Factor

Considering two consecutive frames k and $k+1$ in the window, the residual of preintegrated IMU measurement can be constructed from Equations (11)–(13):

$${\mathit{r}}_B({\widehat{\mathit{z}}}_{b_{k+1}}^{b_k},\mathcal{X})=\left[\begin{array}{c}\delta {\widehat{\mathit{\alpha }}}_{b_{k+1}}^{b_k}\\ \delta {\widehat{\mathit{\beta }}}_{b_{k+1}}^{b_k}\\ \delta {\widehat{\mathit{\gamma }}}_{b_{k+1}}^{b_k}\\ \delta {\mathit{b}}_a\\ \delta {\mathit{b}}_{\omega }\end{array}\right]=\left[\begin{array}{c}{\mathit{R}}_w^{b_k}({\mathit{p}}_{b_{k+1}}^w-{\mathit{p}}_{b_k}^w+\frac{1}{2}{\mathit{g}}^w\Delta t^2-{\mathit{v}}_{b_k}^w\Delta t)-{\widehat{\mathit{\alpha }}}_{b_{k+1}}^{b_k}\\ {\mathit{R}}_w^{b_k}({\mathit{v}}_{b_{k+1}}^w+{\mathit{g}}^w\Delta t-{\mathit{v}}_{b_k}^w)-{\widehat{\mathit{\beta }}}_{b_{k+1}}^{b_k}\\ 2{[{({\widehat{\mathit{\gamma }}}_{b_{k+1}}^{b_k})}^{-1}\otimes {({\mathit{q}}_{b_k}^w)}^{-1}\otimes {\mathit{q}}_{b_{k+1}}^w]}_{xyz}\\ {\mathit{b}}_{a,k+1}-{\mathit{b}}_{a,k}\\ {\mathit{b}}_{\omega ,k+1}-{\mathit{b}}_{\omega ,k}\end{array}\right].$$ (37)

Please note that the accelerometer and gyroscope bias are also included in the residual terms for online correction. Equation (37) seems the same as the IMU measurements residual in the paper [8]. However, as our preintegrated IMU measurements $\mathit{\alpha }$, $\mathit{\beta }$, $\mathit{\gamma }$ handle the imperfection of the IMU intrinsic parameters, which made it different from the one of [8]. Besides, the Jacobian of ${\mathit{r}}_B({\widehat{\mathit{z}}}_{b_{k+1}}^{b_k},\mathcal{X})$ with respect to the input arguments is completely different, too.

3.6. IMU Intrinsic Factor

There are several possible but different strategies for defining the IMU intrinsic parameters residual:

1. The parameters ${\mathit{T}}_a$, ${\mathit{T}}_{\omega }$, ${\mathit{A}}_{\omega }$ are modeled as random-walk processes, but are constant between two frames in the sliding window, and are handled with the same strategy as the IMU bias.

2. The parameters ${\mathit{T}}_a$, ${\mathit{T}}_{\omega }$, ${\mathit{A}}_{\omega }$ are assumed to be uncertain but constant between two frames in the sliding window. With this strategy, we can set the covariance value of ${\mathit{P}}_{b_k}^{b_k}$ in Equation (30) corresponding to ${\mathit{T}}_a$, ${\mathit{T}}_{\omega }$, ${\mathit{A}}_{\omega }$ with a constant value and optimize them at every frame.

3. The parameters ${\mathit{T}}_a$, ${\mathit{T}}_{\omega }$, ${\mathit{A}}_{\omega }$ are assumed to be uncertain, but constant in the timespan of the sliding window.

From the perspective of optimization method, the strategies 1 and 2 are nearly same, except that the covariance value is different, with strategy 1 computing the covariance during preintegration. However, for every frame these two strategies would add additional 24 parameters to the estimator. Assuming the size of sliding window is 10, this means that we will need to optimize an additional 240 parameters at each step, leading to a computational problem for realtime implementation. Besides, compared to the accelerometer and gyroscope bias which vary with time and temperature, the scale factor and misalignment parameters seem more stable, which means that we do not need to optimize them at every frame. Moreover, the scale factors and axis misalignments may couple with the measurement bias under degenerate motions such as rectilinear trajectory, motion with zero acceleration or rotation. In such a case, it is impossible to distinguish all these parameters in one or two frames. With these reasons, in our algorithm, we would choose the strategy 3.

The residual of ${\mathit{T}}_a$, ${\mathit{T}}_{\omega }$, ${\mathit{A}}_{\omega }$ in our implementation is defined as follows:

$${\mathit{r}}_{\mathcal{I}}(\mathcal{X})=\left[\begin{array}{c}{\mathit{t}}_a-{\widehat{\mathit{t}}}_{a,k}\\ {\mathit{t}}_{\omega }-{\widehat{\mathit{t}}}_{\omega ,k}\\ {\mathit{a}}_{\omega }-{\widehat{\mathit{a}}}_{\omega ,k}\end{array}\right],$$ (38)

where ${\widehat{\mathit{t}}}_{a,k}$, ${\widehat{\mathit{t}}}_{\omega ,k}$ and ${\widehat{\mathit{a}}}_{\omega ,k}$ are the current estimation of IMU intrinsic parameters. Please note that the residual is weighted by a constant covariance matrix ${\mathit{P}}_{\mathcal{I}}$ and add to the cost function, Equation (2).

3.7. Discussion and Implementation Details

The considered IMU intrinsic parameters in our method include axis misalignments, scale factors and g-sensitivity. In practice, however, it should be noted that online g-sensitivity calibration is a difficult task. This my be caused by various reasons. First, the errors caused by g-sensitivity are generally smaller than other errors, especially in cases that the IMU suffers from motion with low acceleration or constant speed, or remains static. Second, the g-sensitivity coefficients of a real IMU may vary with the frequency of vibration. As a result, the errors caused by g-sensitivity are more prone to couple with the gyroscope bias or manifest as random noise. In some worse cases, online g-sensitivity calibration may lead to less accuracy of the motion estimation of VINS. Moreover, g-sensitivity calibration would introduce an extra nine variables to the state space, which would consume more computational resource. Therefore, in practice application, whether or not calibrating the g-sensitivity online depends on the IMU used and the motion that the sensor system are subjected. Fortunately, it is very easy to calibrate without g-sensitivity by setting the g-sensitivity coefficients to zeros and keep them constant. The implementation details would be discussed in the following.

We implemented the proposed online IMU self-calibration method with the extension of VINS-Mono [8], as depicted in Figure 3. The system started with measurement preprocessing. For each new image, features were detected by the Shi–Tomasi corner detector [42] and tracked by a KLT tracker [43]. Meanwhile, IMU measurements were locally pre-integrated. The initialization procedure provided all necessary values for bootstrapping the subsequent nonlinear optimization. After the system has been successfully initialized, the tightly-coupled nonlinear optimization would be implemented when every frame is added to the sliding window. After that, keyframe management module marginalized one of the frames from the sliding window in order to bound the computation complexity. We add the IMU intrinsic parameters into the state vector and replace the IMU preintegration block with our method, which are detailed in Section 3.3 and Section 3.4. The proposed IMU measurement factor (Section 3.5) and IMU intrinsic factor (Section 3.6) were used in the nonlinear optimization block. The optimization computation is based on the Ceres Solver [44]. In the implementation, we separate the ${\mathit{T}}_a$, ${\mathit{T}}_{\omega }$, ${\mathit{A}}_{\omega }$ into individual parameter blocks so that we can conveniently set any blocks to constant when optimization, depending the real application. In such way, we can implement our method with and without g-sensitivity calibration, as discussed above.

Figure 3.

Block diagram illustrating of a monocular visual-inertial system (VINS-Mono) [8], based on which, we implement the proposed online IMU self-calibration method. We add the IMU intrinsic parameters into the state vector and replace the IMU Preintegration block with our method, which are detailed in Section 3.3 and Section 3.4. The proposed IMU measurement factor (Section 3.5) and IMU intrinsic factor (Section 3.6) are used in the nonlinear optimization block.

4. Experimental Results

We performed dataset experiments to evaluate the proposed online IMU self-calibration method. At present there are several publicly available datasets for VINS evaluation, such as UMich NCLT [45], EuRoc [46], PennCOSYVIO [47], Zurich Urban MAV [48], TUM VI [49], etc., in which, EuRoc is a very popular dataset and used by many papers [8,20,38,39,50,51,52]. For a completely comparison of these datasets, please refer to [49]. In this paper we adopt the TUM VI dataset [49] for two main reasons: (1) the IMU used in EuRoc dataset is an ADIS16448, a high-end industrial grade IMU with precisely factory calibrated. In contrast, the TUM VI uses a low-cost consumer grade IMU, Bosch BMI160, which makes it more suitable for evaluating our algorithm; (2) The dataset provides both the raw IMU measurements and the precalibrated IMU measurements that compensated for proper IMU scaling and axis alignment. The value of scale factor and axis alignment parameters used for compensation are also provided with the dataset. As a result, we can easily run our algorithm with the raw IMU measurements and compare the estimated intrinsic parameters with the provided reference value.

The TUM VI dataset provides several categories of sequences for the evaluation of VINS method, including three corridor sequences, six magistrale sequences, eight outdoor sequences, six room sequences, and three slider sequence. The outdoor sequences are neglected in our experiments as we focus on the IMU self-calibration but not long time drift evaluation in challenging environments. To perform the comparison experiments, we firstly packed the the quarter resolution images ($512\times 512$ pixels) of the left camera, precalibred IMU measurements as well as the raw IMU measurements with different topic names into one single ROS bag file for each sequence so that we can easily evaluate the algorithm with different type of IMU measurements by just modifying the topic name.

Please note that we deduct the large IMU bias, which is coarsely reproducible between sensor restarts, from the raw measurements. This is reasonably explained in [49]. We also compensate the timestamp of IMU data so that the timestamp is consistent for camera, IMU and ground truth pose.

4.1. IMU Intrinsics Estimation Results

As discussed in Section 3.7, it is difficult to calibrate the g-sensitivity online. In the experiments, we ran the algorithm two times on each sequence of dataset, with and without ${\mathit{A}}_{\omega }$ estimation, respectively. Then we compared the IMU intrinsics estimation results with the the reference values. In all experiments, the initial value of ${\mathit{T}}_a$ and ${\mathit{T}}_{\omega }$ were set to the identity matrix, with the covariance as $2.0\times {10}^{-4}$, and the initial value ${\mathit{A}}_{\omega }$ was set as a zeros matrix with covariance as $5.0\times {10}^{-6}$.

4.1.1. IMU Intrinsics Estimation without G-Sensivity

Figure 4 and Figure 5 show the estimation results on the first sequence of each categories, corridor 1, magistrale 1, room 1, slides 1, respectively, in detail. We can see that all parameters converge to a certain range close to the ref value, but slightly vary with time after that. For each sequence, the convergence speed is different. Generally, the parameters of the gyroscope converge in about 15–20 s and the estimation results is more stable. In contrast, it takes 50–100 s for the parameters of accelerometer to converge. In Figure 4b and Figure 5b, the estimation results of $T_{a_{21}}$ and $T_{a_{32}}$ show larger differences compared with the results of Figure 4a and Figure 5a.

Figure 4.

IMU intrinsics estimation in corridor 1 (a) and magistrale 1 (b). Solid lines are the estimation results and the dotted lines with the same color are the corresponding reference value.

Figure 5.

IMU intrinsics estimation in room 1 (a) and slider 1 (b). Solid lines are the estimation results and the dotted lines with the same color are the corresponding reference value.

It is not surprising for the results mentioned above, as many factors would affect the accuracy of the estimation results. Firstly, the VINS problem has the characteristics of high nonlinearity and local weak observability. To achieve full observability of the IMU intrinsic parameters as well as the measurement bias, motion with sufficient excitation was required. It is certain that this condition would not be always satisfied at any time in real applications. Therefore, the result shows fluctuation under insufficient excitation motion. Besides, the convergence speeds were affected by different motions, which shows a different convergence speed for each sequence. Secondly, as the accelerometer measures the acceleration and the gyroscope can measure the angular velocity directly, this also means that different conditions are required for the accelerometer intrinsics and gyroscope intrinsics to achieve observability. For the accelerometer, sufficient acceleration on each axis is required, and for the gyroscope, only sufficient rotation of the system is required. For a moving sensing system, keeping sufficient acceleration all the time is infeasible in real applications. In contrast, keeping sufficient rotation is a more easily satisfied condition. As a result, the convergence speed of gyroscope shows faster and more stable than that of accelerometer. Thirdly, the measurements of vision sensor and IMU are affected by random noise, which is coupled with the erros caused by inaccuracy intrinsics and bias, and further affects the accuracy of the estimation results. Generally, the measurement noise of gyroscopes is much smaller than that of accelerometers, which means the estimated results of gyroscopes are more accurate than accelerometers.

From Figure 4 and Figure 5, it seems that the estimation results of intrinsic parameters located at the diagonal of matrix ${\mathit{T}}_a$ and ${\mathit{T}}_{\omega }$ are more stable and accurate than other parameters located at the lower or upper triangle. This is because parameters of the lower or upper triangle themselves are much smaller than those of diagonal, and any small challenges would show large fluctuations in the figures. To further evaluate the estimate results, we take the estimation values at the end of each sequence as the final estimation results and calculate their mean value, standard deviation as well as the differences compared with the corresponding reference values. The statistics are shown in the Table 1 and Table 2. We can see that: (1) the standard deviation of diagonal parameters and that of off-diagonal parameters do not show obvious differences; (2) The standard deviation of gyroscope intrinsics is much smaller than that of accelerometer intrinsics. In Table 1, the standard deviation of results was about $0.002$. In contrast, five of nine parameters’ standard deviations in Table 2 were less than $0.001$. This means that the results of the gyroscope were more accurate, which concludes the analysis above. Generally, the axis misalignments of low-cost MEMS-based IMU was 0.5–1%, and the standard deviations of our estimation results was in the order of magnitude ${10}^{-3}$. Therefore, we can conclude that the proposed method can works well with the low-cost MEMS-based IMU.

Table 1.

Statistics of the estimation results of accelerometer intrinsics.

	${\mathit{T}}_{{\mathit{a}}_{11}}$	${\mathit{T}}_{{\mathit{a}}_{21}}$	${\mathit{T}}_{{\mathit{a}}_{22}}$	${\mathit{T}}_{{\mathit{a}}_{31}}$	${\mathit{T}}_{{\mathit{a}}_{32}}$	${\mathit{T}}_{{\mathit{a}}_{33}}$
Reference	1.0042	$-0.0001$	1.0014	$-0.0098$	$-0.0010$	0.9705
Mean	1.0060	$-0.0019$	1.0040	$-0.0090$	0.0006	0.9700
Standard deviations	0.0017	0.0024	0.0020	0.0010	0.0028	0.0021
|Mean-reference|	0.0018	0.0018	0.0026	0.0008	0.0016	0.0004

Table 2.

Statistics of the estimation results of gyroscope intrinsics.

	${\mathit{T}}_{{\mathit{\omega }}_{11}}$	${\mathit{T}}_{{\mathit{\omega }}_{12}}$	${\mathit{T}}_{{\mathit{\omega }}_{13}}$	${\mathit{T}}_{{\mathit{\omega }}_{21}}$	${\mathit{T}}_{{\mathit{\omega }}_{22}}$	${\mathit{t}}_{{\mathit{\omega }}_{23}}$	${\mathit{t}}_{{\mathit{\omega }}_{31}}$	${\mathit{t}}_{{\mathit{\omega }}_{32}}$	${\mathit{t}}_{{\mathit{\omega }}_{33}}$
Reference	0.9436	0.0015	0.0008	0.0004	1.0941	$-0.0027$	$-0.0018$	0.0083	1.0159
Mean	0.9416	0.0023	0.0009	$-0.0005$	1.0919	$-0.0012$	$-0.0015$	0.0064	1.0160
Standard deviations	0.0009	0.0009	0.0004	0.0006	0.0018	0.0022	0.0004	0.0030	0.0012
|Mean-reference|	0.0020	0.0008	0.0000	0.0009	0.0022	0.0015	0.0003	0.0019	0.0002

4.1.2. IMU Intrinsics Estimation with G-Ensitivity

As the g-sensitivity coefficients are unknown, here we only show the results of sequence of corridor 1 and magistrale 1 in detail, as in Figure 6a,b. Comparing these two figures with the estimation results without g-sensitivity showed in Figure 4a,b, we can find that the reuslts of ${\mathit{T}}_a$ and ${\mathit{T}}_{\omega }$ are nearly same, regardless of with or without g-sensivity estimation. The results of g-sensivity ${\mathit{A}}_{\omega }$ do not show that they are very stable. However, this does not mean that we obtained the wrong estimations, or that the calibration of g-sensitivity is totally failed. We would further discuss this in the next section with the quantitative results of VINS to indirectly evaluation to g-sensitivity estimation.

Figure 6.

IMU intrinsics estimation with g-sensitivity in corridor 1 (a) and magistrale1 (b). Solid lines are the estimation results and the dot lines with the same color are the corresponding reference value.

4.2. Quantitative Evaluation

The goal of the IMU calibration is to estimate the IMU intrinsics and compensate the measurements of IMU, and to improve the performance of VINS. Therefore, the accuracy of the IMU intrinsics calibration would directly affects the results of motion estimation. In this section, we quantify the accuracy of the estimation result of system pose with absolute trajectory error (ATE) and relative pose error (RPE) [53]. The RPE is only available for the sequences of rooms 1–6, as for other sequences only accurate pose ground truth at the start and end are available. For comparison, we also ran the VINS-Mono with the same camera image sequences and configuration (IMU noise, camera intrinsics, etc.), but with the precalibrated IMU measurements provided by the dataset. We did not run VINS-Mono directly with the raw IMU measurements as it fails after a few seconds on all sequences. For each sequence, we run the proposed algorithm and the VINS-Mono five times. In each run of the proposed algorithm, the IMU intrinsic parameters estimated online are saved after each nonlinear optimization step and used as the initial values of the next run. The median values obtained by the five times running were used as the final result. The trajectory estimation results of sequence rooms 3 and 5 are showed in Figure 7. To simplify the notation, in the following figures and tables, we use VINS to denote the results of VINS-Mono, our results and our results with ${\mathit{A}}_{\omega }$ to denote the results of our proposed method without and with g-sensitivity estimation, respectively.

Figure 7.

Trajectory of (a) rooms 3 and (b) room 5.

The root-mean-square-error (RMSE) ATE of all sequences is shown in Table 3. Compared the results of our method without g-sensitivity estimation (column 3) against those of VINS-Mono (column 2), 13/20 of the our results method show improvement performance, with the rest of results offering near performance to those of VINS-Mono except the sequence magistrales 3 and 6. What interested us is that, although the estimation results of $T_{a_{21}}$ and $T_{a_{32}}$ in sequence magistrale 1 and slider 1 show larger differences compared to the reference values, the ATE of these two sequences show obvious improvement. This means there exist some errors in the precalibrated reference values or the intrinsic parameter values vary with time, as we mentioned in Section 1. Therefore, our VINS implementaion with online IMU self-calibration shows superior performance compared against the result of VINS-Mono with offline precalibrated IMU measurements.

Table 3.

Root-mean-square-error (RMSE) absolute trajectory error (ATE) comparison between the proposed method with raw inertial measurement unit (IMU) measurements and monocular visual-inertial system (VINS-Mono) with precalibrated IMU measurements in all of the sequences. Downarrow means better performance compared to VINS-Mono. The bold values indicate the best results.

Sequence	VINS	Ours	Ours with ${\mathit{A}}_{\mathit{\omega }}$	Length (m)
corridor1	0.618	0.453 ↓	0.851	305
corridor2	1.174	1.157 ↓	0.936 ↓	322
corridor3	1.309	0.797 ↓	0.447 ↓	300
corridor4	0.309	0.195 ↓	0.136 ↓	114
corridor5	0.674	0.547 ↓	0.647 ↓	270
magistrale1	2.385	2.043 ↓	1.072 ↓	918
magistrale2	3.205	3.105 ↓	3.132 ↓	561
magistrale3	0.358	0.521	1.035	566
magistrale4	4.443	3.919 ↓	4.223 ↓	688
magistrale5	0.542	0.603	0.738	458
magistrale6	2.078	2.825	1.210 ↓	771
room1	0.089	0.095	0.094	146
room2	0.048	0.051	0.049	142
room3	0.145	0.076 ↓	0.084 ↓	135
room4	0.042	0.048	0.052	68
room5	0.196	0.049 ↓	0.043 ↓	131
room6	0.059	0.057 ↓	0.048 ↓	67
slides1	0.517	0.273 ↓	0.193 ↓	289
slides2	1.007	1.047	1.209	299
slides3	1.005	0.781 ↓	0.764 ↓	383

Now we further focus on the ATE results of our method with g-sensitivity estimation (column 4). 9/20 of the results show best performance between the three algorithms, most of which show more than $50\%$ improvements. However, some of the results show worse performance compared to the estimation results without g-sensitivity calibration. This also concludes our discussion in Section 3.7 that online g-sensitivity calibration is a difficult task and we would not always get a better performance with g-sensitivity estimation.

Figure 8 shows the overall relative pose error (RTE) in room1–room6, caculated by the toolbox [54]. In Figure 8a, we can find obvious improvements compared to VINS-Mono. In contrast, nearly same relative orientation errors are showed in Figure 8b.

Figure 8.

Overall relative pose error in rooms 1–6. (a) Relative translation error; (b) Relative orientation error

4.3. Runtime Evaluation

The proposed online IMU self-calibration method adds additional states to the system state vector, and estimates the optimal value of these states based on optimization method. As a result, online calibration would consume more computation resource. Figure 9 shows the average processing time per frame on each sequence. All experiments were performed using a computer equipped with an Intel Core(TM) i7-7700 CPU at 3.60 GHz and 8 GB RAM.

Figure 9.

Average processing time per frame.

It is obvious that the proposed method consumes an additional 3–5 ms to estimate the IMU intrinsics. We further compute the average processing time for all sequences and the results are 27.7 ms, 31.8 ms, 33.5 ms, respectively. The processing time of our method without and with g-sensitivity estimation increase $14.9\%$ and $20.9\%$, respectively, compared with VINS-Mono. Nevertheless, we can conclude that our method can run in real time.

Calibration process with g-sensitivity would introduce extra nine variables to the state space and consumes about additional 2 ms times the cost. Therefore, in practice applications, it is necessary to decide whether or not calibrating the g-sensitivity online, according to the IMU used and the trade-off between estimation accuracy and real time performance. Beside, the motion of the sensing system that are subjected should be considered, too. For example, if the general motion of the system is in low acceleration most of the time, g-sensitivity calibration may not significantly improve the performance of VINS.

5. Conclusions

In this paper, we propose an online IMU self-calibration method for visual-inertial system with low-cost inertial sensors. The main advantage of our method is that it performs online IMU self-calibration based on optimization method in unknown environment, using only camera and inertial measurements, and without any knowledge about the structure of scene or any external equipment. Specifically, the estimated parameters include the axis misalignments and scale factors of the IMU, as well as the g-sensitivity of the gyroscope. We perform extensive evaluation on public TUM VI dataset, with and without g-sensitivity estimation, respectively. Experimental results verify that the proposed method is able to accurately calibrate all the considered parameters in real time, and show superior performance compared with the results of VINS with offline precalibrated IMU measurements. It should be noted that the calibration process with g-sensitivity would introduce extra nine variables to the state space and consume more computation resource. Therefore, in practical application, it is necessary to decide whether or not calibrating the g-sensitivity online, according to the IMU used and the trade-off between estimation accuracy and real time performance.

The proposed method is dedicated to low-cost IMUs. As a result, it is very suitable for the consumer grade mobile robots, drones and cellphones which are broadly equipped with low-cost MEMS-based IMUs. Moreover, our method would promote the rapid deployment of VINS in various applications base on these platforms, such as robot navigation, autonomous driving, AR and VR.

In this paper we adopt quaternion as global parameterization for rotations. In the future we will extend our method to work with the special orthogonal group SO(3), which would make it more easily be employed in existing SO(3)-based visual/visual-inertial frameworks.

Abbreviations

The following abbreviations are used in this manuscript:

IMU	Inertial measurement unit
MEMS	Microelectro mechanical systems
AHRS	Attitude and heading reference system

Appendix A. Quaternion-Based IMU Preintegration

Appendix A.1. The Error-State Kinematics

This section details how to obtain the the error-state kinematic in Section 3.4. Our goal is to determine $\delta \dot{\mathit{\alpha }}$, $\delta \dot{\mathit{\beta }}$, $\delta \dot{\mathit{\gamma }}$. For readable and simplicity purpose, we will drop the coordinate frame subscripts in the following equations, which would not cause ambiguous.

From Equations (14)–(16), we can get the true state kinematics of preintegration:

$$\begin{array}{cc}\hfill \dot{\mathit{\alpha }}& =\mathit{\beta }\hfill \end{array}$$ (A1)

$$\begin{array}{cc}\hfill \dot{\mathit{\beta }}& =\mathit{R}{\mathit{T}}_a({\mathit{a}}_m-{\mathit{b}}_a-{\mathit{n}}_a)\hfill \end{array}$$ (A2)

$$\begin{array}{cc}\hfill \dot{\mathit{\gamma }}& =\frac{1}{2}\mathit{\gamma }\otimes {\mathit{T}}_{\omega }({\mathit{\omega }}_m-{\mathit{A}}_{\omega }{T}_a({\mathit{a}}_m-{\mathit{b}}_a-{\mathit{n}}_a)-{\mathit{b}}_{\omega }-{\mathit{n}}_{\omega }).\hfill \end{array}$$ (A3)

From Equations (19)–(21), the nominal-states kinematic is obtained as follows:

$$\begin{array}{cc}\hfill \dot{\widehat{\mathit{\alpha }}}& =\widehat{\mathit{\beta }}\hfill \end{array}$$ (A4)

$$\begin{array}{cc}\hfill \dot{\widehat{\mathit{\beta }}}& =\widehat{\mathit{R}}{\widehat{\mathit{T}}}_a({\mathit{a}}_m-{\widehat{\mathit{b}}}_a)\hfill \end{array}$$ (A5)

$$\begin{array}{cc}\hfill \dot{\widehat{\mathit{\gamma }}}& =\frac{1}{2}\widehat{\mathit{\gamma }}\otimes {\widehat{\mathit{T}}}_{\omega }\left({\mathit{\omega }}_m-{\widehat{\mathit{A}}}_{\omega }{\widehat{T}}_a({\mathit{a}}_m-{\widehat{\mathit{b}}}_a)-{\widehat{\mathit{b}}}_{\omega }\right).\hfill \end{array}$$ (A6)

A. The Translation Error

With Equations (A3) and (A6), it is easy to obtain:

$$\delta \dot{\mathit{\alpha }}=\mathit{\beta }-\widehat{\mathit{\beta }}=\delta \mathit{\beta }$$ (A7)

B. The Linear Velocity Error

We start with the following relations:

$$\begin{array}{cc}\hfill \mathit{R}& =\widehat{\mathit{R}}(I+{[\delta \mathit{\theta }]}_{\times })+\mathcal{O}({\parallel \delta \mathit{\theta }\parallel }^2)\hfill \end{array}$$ (A8)

$$\begin{array}{cc}\hfill \mathit{a}& ={\mathit{T}}_a({\mathit{a}}_m-{\mathit{b}}_a-{\mathit{n}}_a)\hfill \\ & ={\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B+{\widehat{\mathit{T}}}_a\delta {\mathit{a}}_B+\delta {\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B+\delta {\widehat{\mathit{T}}}_a\delta {\mathit{a}}_B,\hfill \end{array}$$ (A9)

where ${\widehat{\mathit{a}}}_B={\mathit{a}}_m-{\mathit{b}}_a$, $\delta {\mathit{a}}_B=-\delta {\mathit{b}}_a-{\mathit{n}}_a$.

Substituting Equations (A8) and (A9) into Equations (A2) and (A5) yields:

$$\begin{array}{cc}\hfill \dot{\mathit{\beta }}& \approx \widehat{\mathit{R}}(I+{[\delta \mathit{\theta }]}_{\times })({\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B+{\widehat{\mathit{T}}}_a\delta {\mathit{a}}_B+\delta {\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B+\delta {\widehat{\mathit{T}}}_a\delta {\mathit{a}}_B)\hfill \\ & =\widehat{\mathit{R}}{\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B+\widehat{\mathit{R}}{\widehat{\mathit{T}}}_a\delta {\mathit{a}}_B+\widehat{\mathit{R}}\delta {\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B+\widehat{\mathit{R}}\delta {\widehat{\mathit{T}}}_a\delta {\mathit{a}}_B+\widehat{\mathit{R}}{[\delta \mathit{\theta }]}_{\times }{\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B\hfill \\ \hfill \dot{\widehat{\mathit{\beta }}}& =\widehat{\mathit{R}}{\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B\hfill \end{array}$$

$$\begin{array}{cc}\hfill \Rightarrow \delta \dot{\mathit{\beta }}& =\dot{\mathit{\beta }}-\dot{\widehat{\mathit{\beta }}}\hfill \\ & \approx \widehat{\mathit{R}}{\widehat{\mathit{T}}}_a\delta {\mathit{a}}_B+\widehat{\mathit{R}}\delta {\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B+\widehat{\mathit{R}}{[\delta \mathit{\theta }]}_{\times }{\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B\hfill \\ & =-\widehat{\mathit{R}}{[{\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B]}_{\times }\delta \mathit{\theta }-\widehat{\mathit{R}}{\widehat{\mathit{T}}}_a\delta {\widehat{\mathit{b}}}_a+\widehat{\mathit{R}}\delta {\mathit{T}}_a{\widehat{\mathit{a}}}_B-\widehat{\mathit{R}}{\mathit{T}}_a{\mathit{n}}_a.\hfill \end{array}$$ (A10)

C. The Orientation Error

We start with the following relations

$$\begin{array}{cc}\hfill \mathit{\omega }& ={\mathit{T}}_{\omega }\left({\mathit{\omega }}_m-{\mathit{A}}_{\omega }{T}_a({\mathit{a}}_m-{\mathit{b}}_a-{\mathit{n}}_a)-{\mathit{b}}_{\omega }-{\mathit{n}}_{\omega }\right)\hfill \\ & ={\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B+{\widehat{\mathit{T}}}_{\omega }\delta {\mathit{\omega }}_B+\delta {\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B+\delta {\widehat{\mathit{T}}}_{\omega }\delta {\mathit{\omega }}_B,\hfill \end{array}$$ (A11)

where, ${\widehat{\mathit{\omega }}}_B={\mathit{\omega }}_m-{\widehat{\mathit{A}}}_{\omega }{\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B-{\widehat{\mathit{b}}}_{\omega }$, $\delta {\mathit{\omega }}_B=-\delta {\mathit{A}}_{\omega }{\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B+{\widehat{\mathit{A}}}_{\omega }{\widehat{\mathit{T}}}_a\delta {\mathit{b}}_a+({\widehat{\mathit{A}}}_{\omega }+\delta {\mathit{A}}_{\omega }){\widehat{\mathit{T}}}_a{\mathit{n}}_a-{\mathit{A}}_{\omega }\delta {\widehat{\mathit{T}}}_a({\widehat{\mathit{a}}}_B-\delta {\mathit{b}}_a-{\mathit{n}}_a)-\delta {\mathit{b}}_{\omega }-{\mathit{n}}_{\omega }$.

Substituting Equation (A11) in Equations (A3) and (A6) yields:

$$\begin{array}{c}\dot{\mathit{\gamma }}=\frac{1}{2}\mathit{\gamma }\otimes \mathit{\omega },\dot{\widehat{\mathit{\gamma }}}=\frac{1}{2}\widehat{\mathit{\gamma }}\otimes {\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B\hfill \\ \Rightarrow \dot{\mathit{\gamma }}=\dot{(\widehat{\mathit{\gamma }}\otimes \delta \mathit{\gamma })}=\frac{1}{2}(\widehat{\mathit{\gamma }}\otimes \delta \mathit{\gamma })\otimes \mathit{\omega }\hfill \\ \Rightarrow \dot{\widehat{\mathit{\gamma }}}\otimes \delta \mathit{\gamma }+\widehat{\mathit{\gamma }}\otimes \delta \dot{\mathit{\gamma }}=\frac{1}{2}(\widehat{\mathit{\gamma }}\otimes \delta \mathit{\gamma })\otimes \mathit{\omega }\hfill \\ \Rightarrow \frac{1}{2}\widehat{\mathit{\gamma }}\otimes {\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B\otimes \delta \mathit{\gamma }+\widehat{\mathit{\gamma }}\otimes \delta \dot{\mathit{\gamma }}=\frac{1}{2}(\widehat{\mathit{\gamma }}\otimes \delta \mathit{\gamma })\otimes \mathit{\omega }\hfill \\ \Rightarrow {(\widehat{\mathit{\gamma }})}^{-1}(\frac{1}{2}\widehat{\mathit{\gamma }}\otimes {\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B\otimes \delta \mathit{\gamma }+\widehat{\mathit{\gamma }}\otimes \delta \dot{\mathit{\gamma }})={(\widehat{\mathit{\gamma }})}^{-1}(\frac{1}{2}(\widehat{\mathit{\gamma }}\otimes \delta \mathit{\gamma })\otimes \mathit{\omega })\hfill \\ \Rightarrow \frac{1}{2}{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B\otimes \delta \mathit{\gamma }+\delta \dot{\mathit{\gamma }}=\frac{1}{2}\delta \mathit{\gamma }\otimes \mathit{\omega }\hfill \\ \Rightarrow \delta \dot{\mathit{\gamma }}=\frac{1}{2}\delta \mathit{\gamma }\otimes \mathit{\omega }-\frac{1}{2}{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B\otimes \delta \mathit{\gamma }.\hfill \end{array}$$

Remember that $\delta \mathit{\gamma }=\left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathit{\theta }\end{array}\right]$. Substitute it to the last equation above:

$$\begin{array}{cc}\hfill \left[\begin{array}{c}1\\ \delta \dot{\mathit{\theta }}\end{array}\right]& =2\delta \dot{\mathit{\gamma }}=\delta \mathit{\gamma }\otimes \mathit{\omega }-{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B\otimes \delta \mathit{\gamma }\hfill \\ & ={\mathsf{\Omega }}_R(\mathit{\omega })\delta \mathit{\gamma }-{\mathsf{\Omega }}_L({\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B)\delta \mathit{\gamma }\hfill \\ & =\left[\begin{array}{cc}0& -(\mathit{\omega }-{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B)\\ (\mathit{\omega }-{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B)& -{[\mathit{\omega }+{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B]}_{\times }\end{array}\right]\left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathit{\theta }\end{array}\right]+\mathcal{O}({\parallel \delta \mathit{\theta }\parallel }^2)\hfill \\ & =\left[\begin{array}{cc}0& -({\widehat{\mathit{T}}}_{\omega }\delta {\mathit{\omega }}_B+\delta {\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B)\\ ({\widehat{\mathit{T}}}_{\omega }\delta {\mathit{\omega }}_B+\delta {\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B)& -{[2{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B+{\widehat{\mathit{T}}}_{\omega }\delta {\mathit{\omega }}_B+\delta {\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B]}_{\times }\end{array}\right]\left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathit{\theta }\end{array}\right]+\mathcal{O}({\parallel \delta \mathit{\theta }\parallel }^2),\hfill \end{array}$$

where, ${\mathsf{\Omega }}_L(\mathit{\omega })=\left[\begin{array}{cc}0& -{\mathit{\omega }}^T\\ \mathit{\omega }& {[\mathit{\omega }]}_{\times }\end{array}\right]$, ${\mathsf{\Omega }}_R(\mathit{\omega })=\left[\begin{array}{cc}0& {\mathit{\omega }}^T\\ -\mathit{\omega }& -{[\mathit{\omega }]}_{\times }\end{array}\right]$.

Now we obtain $\delta \dot{\mathit{\theta }}$ as:

$$\begin{array}{cc}\hfill \delta \dot{\mathit{\theta }}& =({\widehat{\mathit{T}}}_{\omega }\delta {\mathit{\omega }}_B+\delta {\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B)-{[2{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B+{\widehat{\mathit{T}}}_{\omega }\delta {\mathit{\omega }}_B+\delta {\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B]}_{\times }\frac{\delta \mathit{\theta }}{2}+\mathcal{O}({\parallel \delta \mathit{\theta }\parallel }^2)\hfill \\ & \approx {\widehat{\mathit{T}}}_{\omega }\delta {\mathit{\omega }}_B+\delta {\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B-{[{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B]}_{\times }\delta \mathit{\theta }\hfill \\ & =-{[{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B]}_{\times }\delta \mathit{\theta }+\delta {\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B+{\widehat{\mathit{T}}}_{\omega }(-\delta {\mathit{A}}_{\omega }{\widehat{\mathit{T}}}_a{\widehat{\mathit{a}}}_B+{\widehat{\mathit{A}}}_{\omega }{\widehat{\mathit{T}}}_a\delta {\mathit{b}}_a\hfill \\ & +({\widehat{\mathit{A}}}_{\omega }+\delta {\mathit{A}}_{\omega }){\widehat{\mathit{T}}}_a{\mathit{n}}_a-{\mathit{A}}_{\omega }\delta {\widehat{\mathit{T}}}_a({\widehat{\mathit{a}}}_B-\delta {\mathit{b}}_a-{\mathit{n}}_a)-\delta {\mathit{b}}_{\omega }-{\mathit{n}}_{\omega })\hfill \\ & \approx -{[{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{\omega }}}_B]}_{\times }\delta \mathit{\theta }+{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }{\widehat{\mathit{T}}}_a\delta {\mathit{b}}_a-{\widehat{\mathit{T}}}_{\omega }\delta {\mathit{b}}_{\omega }-{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }\delta {\mathit{T}}_a{\widehat{\mathit{a}}}_B\hfill \\ & +\delta {\mathit{T}}_{\omega }{\widehat{\mathit{\omega }}}_B-{\widehat{\mathit{T}}}_{\omega }\delta {\mathit{A}}_{\omega }\widehat{{\mathit{T}}_a}{\widehat{\mathit{a}}}_B+{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }{\widehat{\mathit{T}}}_a{\mathit{n}}_a-{\widehat{\mathit{T}}}_{\omega }{\mathit{n}}_{\omega }.\hfill \end{array}$$ (A12)

Equations (A7), (A10) and (A12) form the error-state kinematic which is same as in Equations (22)–(24).

Appendix A.2. Mid-Point Integration for the IMU Preintegration

We perform the Mid-point numerical integration in our implementation code. This section shows the computation details.

As showed in the Figure 2, we assume that the i-th IMU measurement is obtained at the time instant of frame k, and j-th measurement at frame $k+1$. For a real sensor suite, there may be no IMU measurement at the time instant of a image frame is captured. In this case, linear interpolation method can be used to estimate a IMU measurement at the corresponding time instant. As showed in Section 3.6, the ${\mathit{T}}_a$, ${\mathit{T}}_{\omega }$, ${\mathit{A}}_{\omega }$ is also assumed constant in the timespan of sliding window, so we will not explicitly distinguish the ${\mathit{T}}_{a,i}$, ${\mathit{T}}_{\omega ,i}$, ${\mathit{A}}_{\omega ,i}$ and ${\mathit{T}}_{a,i+1}$, ${\mathit{T}}_{\omega ,i+1}$, ${\mathit{A}}_{\omega ,i+1}$ in the following equations.

A. Nominal-State Preintegration

Integrating Equations (19)–(21) is to get the nominal-state estimation:

$$\begin{array}{cc}\hfill {\widehat{\mathit{\alpha }}}_{i+1}^{b_k}& ={\widehat{\mathit{\alpha }}}_i^{b_k}+{\widehat{\mathit{\beta }}}_{i+1}^{b_k}\delta t+\frac{1}{2}{\widehat{\mathit{\gamma }}}_i^{b_k}{\widehat{\mathit{T}}}_a(\frac{1}{2}({\mathit{a}}_{m_i}+{\mathit{a}}_{m_{i+1}})-{\widehat{\mathit{b}}}_a)\delta t^2\hfill \end{array}$$ (A13)

$$\begin{array}{cc}\hfill {\widehat{\mathit{\beta }}}_{i+1}^{b_k}& ={\widehat{\mathit{\beta }}}_i^{b_k}+{\widehat{\mathit{\gamma }}}_i^{b_k}{\widehat{\mathit{T}}}_a(\frac{1}{2}({\mathit{a}}_{m_i}+{\mathit{a}}_{m_{i+1}})-{\widehat{\mathit{b}}}_a)\delta t\hfill \end{array}$$ (A14)

$$\begin{array}{cc}\hfill {\widehat{\mathit{\gamma }}}_{i+1}^{b_k}& ={\widehat{\mathit{\gamma }}}_i^{b_k}\otimes {\widehat{\mathit{T}}}_{\omega }(\frac{1}{2}({\mathit{\omega }}_{m_i}+{\mathit{\omega }}_{m_{i+1}})-{\widehat{\mathit{A}}}_{\omega }{\widehat{T}}_a(\frac{1}{2}({\mathit{a}}_{m_i}+{\mathit{a}}_{m_{i+1}})-{\widehat{\mathit{b}}}_{a_t})-{\widehat{\mathit{b}}}_{{\omega }_t})\delta t,\hfill \end{array}$$ (A15)

where $\delta t$ is the time interval between $t_i$ and $t_{i+1}$. At the beginning of integration, ${\mathit{\alpha }}_{b_{k+1}}^{b_k}$, ${\mathit{\beta }}_{b_{k+1}}^{b_k}$ is 0, and ${\mathit{\gamma }}_{b_{k+1}}^{b_k}$ is identity quaternion. The prorogation starts with $i=i$ and repeats step by step until $i+1=j$.

In the following we will detail how to obtain the error-state Jacobian and covariance using the Mid-point integration.

B. Error-State of Orientation Preintegration

Define ${\overline{\mathit{\omega }}}_B=\frac{1}{2}({\widehat{\mathit{\omega }}}_{B,i}+{\widehat{\mathit{\omega }}}_{B,i+1})$, ${\overline{\mathit{a}}}_B=\frac{1}{2}({\widehat{\mathit{a}}}_{B,i}+{\widehat{\mathit{a}}}_{B,i+1})$. Integration Equation (A12) in discrete time:

$$\begin{array}{cc}\hfill \delta {\mathit{\theta }}_{i+1}& =\left(\mathit{I}-{[{\widehat{\mathit{T}}}_{\omega }{\overline{\mathit{\omega }}}_B]}_{\times }\delta t\right)\delta {\mathit{\theta }}_i+{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }{\widehat{\mathit{T}}}_a\delta t\delta {\mathit{b}}_{a,i}-{\widehat{\mathit{T}}}_{\omega }\delta t\delta {\mathit{b}}_{\omega ,i}-{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }\delta {\mathit{T}}_a{\overline{\mathit{a}}}_B\delta t\hfill \\ & +\delta {\mathit{T}}_{\omega }{\overline{\mathit{a}}}_B\delta t-{\widehat{\mathit{T}}}_{\omega }\delta {\mathit{A}}_{\omega }{\widehat{\mathit{T}}}_a{\overline{\mathit{a}}}_B\delta t+{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }{\widehat{\mathit{T}}}_a\delta t{\mathit{n}}_a-{\widehat{\mathit{T}}}_{\omega }\delta t{\mathit{n}}_{\omega }\hfill \\ & ={\mathit{f}}_{33}\delta {\mathit{\theta }}_i+{\mathit{f}}_{34}\delta {\mathit{b}}_{a,i}+{\mathit{f}}_{35}\delta {\mathit{b}}_{\omega ,i}+{\mathit{f}}_{36}\delta {\mathit{t}}_{a,i}+{\mathit{f}}_{37}\delta {\mathit{t}}_{\omega ,i}+{\mathit{f}}_{38}\delta {\mathit{a}}_{\omega ,i}+{\mathit{n}}_{31}{\mathit{n}}_a+{\mathit{n}}_{32}{\mathit{n}}_{\omega },\hfill \end{array}$$ (A16)

where,

$$\begin{array}{c}{\mathit{f}}_{33}=I-{[{\widehat{\mathit{T}}}_{\omega }{\overline{\mathit{\omega }}}_B]}_{\times }\delta t,{\mathit{f}}_{34}={\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }{\widehat{\mathit{T}}}_a\delta t,{\mathit{f}}_{35}={\widehat{\mathit{T}}}_{\omega }\delta t,{\mathit{f}}_{36}=-{\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }{[{\overline{\mathit{a}}}_B]}_T\delta t\hfill \\ {\mathit{f}}_{37}={[{\overline{\mathit{a}}}_B]}_T\delta t,{\mathit{f}}_{38}=-{\widehat{\mathit{T}}}_{\omega }{[{\widehat{\mathit{T}}}_a{\overline{\mathit{a}}}_B]}_T\delta t,{\mathit{n}}_{31}={\widehat{\mathit{T}}}_{\omega }{\widehat{\mathit{A}}}_{\omega }{\widehat{\mathit{T}}}_a\delta t,{\mathit{n}}_{32}=-{\widehat{\mathit{T}}}_{\omega }\delta t.\hfill \end{array}$$

C. Error-State of Velocity Preintegration

Define

$$\begin{array}{cc}\hfill {\mathit{R}}_{a0}& =\frac{1}{2}{\widehat{\mathit{R}}}_i{[{\widehat{\mathit{T}}}_a{\mathit{a}}_{B,i}]}_{\times },{\mathit{R}}_{a1}=\frac{1}{2}{\widehat{\mathit{R}}}_{i+1}{[{\widehat{\mathit{T}}}_a{\mathit{a}}_{B,i+1}]}_{\times }\hfill \\ \hfill \overline{\mathit{R}}& =\frac{1}{2}({\widehat{\mathit{R}}}_i+{\widehat{\mathit{R}}}_{i+1})\hfill \\ \hfill {\mathit{R}}_{a_T}& =\frac{1}{2}({\widehat{\mathit{R}}}_i{[{\mathit{a}}_{B,i}]}_T+{\widehat{\mathit{R}}}_{i+1}[{\mathit{a}}_{{B,i+1]}_T}).\hfill \end{array}$$

Integration Equation (A10) in discrete time:

$$\begin{array}{cc}\hfill \delta {\mathit{\beta }}_{i+1}& =\delta {\mathit{\beta }}_i-({\mathit{R}}_{a0}\delta {\mathit{\theta }}_i+{\mathit{R}}_{a1}\delta {\mathit{\theta }}_{i+1})\delta t-\overline{\mathit{R}}{\widehat{\mathit{T}}}_a\delta t\delta {\widehat{\mathit{b}}}_{a,i}+{\mathit{R}}_{a_T}\delta t\delta {\mathit{t}}_{a,i}-\overline{\mathit{R}}{\widehat{\mathit{T}}}_a\delta t{\mathit{n}}_a\hfill \\ & =\delta {\mathit{\beta }}_i-({\mathit{R}}_{a0}\delta {\mathit{\theta }}_i+{\mathit{R}}_{a1}({\mathit{f}}_{33}\delta {\mathit{\theta }}_i+{\mathit{f}}_{34}\delta {\mathit{b}}_{a,i}+{\mathit{f}}_{35}\delta {\mathit{b}}_{\omega ,i}+{\mathit{f}}_{36}\delta {\mathit{t}}_{a,i}+{\mathit{f}}_{37}\delta {\mathit{t}}_{\omega ,i}+{\mathit{f}}_{38}\delta {\mathit{a}}_{\omega ,i}\hfill \\ & +{\mathit{n}}_{31}{\mathit{n}}_a+{\mathit{n}}_{32}{\mathit{n}}_{\omega }))\delta t-\overline{\mathit{R}}{\widehat{\mathit{T}}}_a\delta t\delta {\widehat{\mathit{b}}}_a+{\mathit{R}}_{a_T}\delta t\delta {\mathit{t}}_a-\overline{\mathit{R}}{\widehat{\mathit{T}}}_a\delta t{\mathit{n}}_a\hfill \\ & =\delta {\mathit{\beta }}_i+{\mathit{f}}_{23}\delta {\mathit{\theta }}_i+{\mathit{f}}_{24}\delta {\mathit{b}}_{a,i}+{\mathit{f}}_{25}\delta {\mathit{b}}_{\omega ,i}+{\mathit{f}}_{26}\delta {\mathit{t}}_{a,i}+{\mathit{f}}_{27}\delta {\mathit{t}}_{\omega ,i}+{\mathit{f}}_{28}\delta {\mathit{a}}_{\omega ,i}+{\mathit{n}}_{21}{\mathit{n}}_a+{\mathit{n}}_{22}{\mathit{n}}_{\omega },\hfill \end{array}$$ (A17)

where,

$$\begin{array}{c}{\mathit{f}}_{23}=-({\mathit{R}}_{a0}+{\mathit{R}}_{a1}{\mathit{f}}_{33})\delta t,{\mathit{f}}_{24}=-({\mathit{R}}_{a1}{\mathit{f}}_{33}+\overline{\mathit{R}}{\widehat{\mathit{T}}}_a)\delta t,{\mathit{f}}_{25}=-{\mathit{R}}_{a1}{\mathit{f}}_{35}\delta t,\hfill \\ {\mathit{f}}_{26}=(-{\mathit{R}}_{a1}{\mathit{f}}_{36}+{\mathit{R}}_{a_T})\delta t,{\mathit{f}}_{27}=-{\mathit{R}}_{a1}{\mathit{f}}_{37}\delta t,{\mathit{f}}_{28}=-{\mathit{R}}_{a1}{\mathit{f}}_{38}\delta t,\hfill \\ {\mathit{n}}_{21}=-({\mathit{R}}_{a1}{\mathit{n}}_{21}+\overline{\mathit{R}}{\widehat{\mathit{T}}}_a)\delta t,{\mathit{n}}_{22}=-{\mathit{R}}_{a1}{\mathit{n}}_{22}\delta t.\hfill \end{array}$$

D. Error-State of Translation Preintegration

$$\begin{array}{cc}\hfill \delta {\mathit{\alpha }}_{i+1}& =\delta {\mathit{\alpha }}_i+\delta {\mathit{\beta }}_i\delta t+\frac{1}{2}\delta {\dot{\mathit{\beta }}}_i\delta t^2\hfill \\ & =\delta {\mathit{\alpha }}_i+\delta {\mathit{\beta }}_i\delta t+\frac{\delta t}{2}({\mathit{f}}_{23}\delta {\mathit{\theta }}_i+{\mathit{f}}_{24}\delta {\mathit{b}}_{a,i}+{\mathit{f}}_{25}\delta {\mathit{b}}_{\omega ,i}+{\mathit{f}}_{26}\delta {\mathit{t}}_{a,i}\hfill \\ & +{\mathit{f}}_{27}\delta {\mathit{t}}_{\omega ,i}+{\mathit{f}}_{28}\delta {\mathit{a}}_{\omega ,i}+{\mathit{n}}_{21}{\mathit{n}}_a+{\mathit{n}}_{22}{\mathit{n}}_{\omega }).\hfill \end{array}$$ (A18)

Equations (A16)–(A18) can be represent in matrix form:

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\mathit{\alpha }}_{i+1}\\ {\mathit{\beta }}_{i+1}\\ {\mathit{\gamma }}_{i+1}\\ {\mathit{b}}_{a,i+1}\\ {\mathit{b}}_{\omega ,i+1}\\ {\mathit{t}}_{a,i+1}\\ {\mathit{t}}_{\omega ,i+1}\\ {\mathit{a}}_{\omega ,i+1}\end{array}\right]& =\underset{{\mathit{F}}_{m,i}}{\underbrace{\left[\begin{array}{cccccccc}{\mathit{I}}_3& \frac{\delta t}{2}{\mathit{f}}_{23}& {\mathit{I}}_3\delta t& \frac{\delta t}{2}{\mathit{f}}_{24}& \frac{\delta t}{2}{\mathit{f}}_{25}& \frac{\delta t}{2}{\mathit{f}}_{26}& \frac{\delta t}{2}{\mathit{f}}_{27}& \frac{\delta t}{2}{\mathit{f}}_{28}\\ \mathbf{0}& {\mathit{f}}_{22}& {\mathit{I}}_3& {\mathit{f}}_{24}& {\mathit{f}}_{25}& {\mathit{f}}_{26}& {\mathit{f}}_{27}& {\mathit{f}}_{28}\\ \mathbf{0}& {\mathit{f}}_{32}& \mathbf{0}& {\mathit{f}}_{34}& {\mathit{f}}_{35}& {\mathit{f}}_{36}& {\mathit{f}}_{37}& {\mathit{f}}_{38}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{I}}_3& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{I}}_3& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{I}}_6& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{I}}_9& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{I}}_9\end{array}\right]}}\left[\begin{array}{c}{\mathit{\alpha }}_i\\ {\mathit{\beta }}_i\\ {\mathit{\gamma }}_i\\ {\mathit{b}}_{a,i}\\ {\mathit{b}}_{\omega ,i}\\ {\mathit{t}}_{a,i}\\ {\mathit{t}}_{\omega ,i}\\ {\mathit{a}}_{\omega ,i}\end{array}\right]\hfill \\ & +\underset{{\mathit{G}}_{m,i}}{\underbrace{\left[\begin{array}{cccc}\frac{\delta t}{2}{\mathit{n}}_{21}& \frac{\delta t}{2}{\mathit{n}}_{22}& \mathbf{0}& \mathbf{0}\\ {\mathit{n}}_{21}& {\mathit{n}}_{22}& \mathbf{0}& \mathbf{0}\\ {\mathit{n}}_{31}& {\mathit{n}}_{32}& \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}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\end{array}\right]}}\left[\begin{array}{c}{\mathit{n}}_a\\ {\mathit{n}}_{\omega }\\ {\mathit{n}}_{ba}\\ {\mathit{n}}_{b_{\omega }}\end{array}\right].\hfill \end{array}$$ (A19)

Again, the Jacobian and covariance matrix can propagate recursively:

$${\mathit{J}}_{i+1}={\mathit{F}}_{m,i}{\mathit{J}}_i$$ (A20)

$${\mathit{P}}_{i+1}={\mathit{F}}_{m,i}{\mathit{P}}_i{\mathit{F}}_{m,i}^T+{\mathit{G}}_{m,i}\mathit{Q}{\mathit{G}}_{m,i}^T.$$ (A21)

Author Contributions

Conceptualization, Y.X.; funding acquisition, X.R. and X.Z. (Xiaoqing Zhu); methodology, Y.X.; project administration, X.R. and X.Z. (Xiaoqing Zhu); software, Y.X.; supervision, X.R.; writing—original draft, Y.X.; writing—review and editing, X.Z. (Xiaoqing Zhu), X.Z. (Xiaoping Zhang) and J.C.

Funding

This research has been funded by the National Natural Science Foundation of China (No. 61773027), Key Project of S and T Plan of Beijing Municipal Commission of Education (No. KZ201610005010) and Beijing Natural Science Foundation (4174083).

Conflicts of Interest

The authors declare no conflict of interest.