A Lightweight and Accurate Localization Algorithm Using Multiple Inertial Measurement Units

09/11/2019 ∙ by Ming Zhang, et al. ∙ 0

In this paper, a lightweight and accurate localization algorithm is proposed using measurements from multiple inertial measurement units (IMUs). IMU is a low-cost motion sensor which provides measurements on rotational velocity and gravity compensated linear acceleration of a moving platform, and it is widely used in localization problems. To date, most existing work that employs IMU sensor for localization focuses on algorithms or applications based on a single IMU. While this category of design yields acceptable accuracy and robustness for different use cases, the overall performance can be further improved by using multiple IMUs. To this end, we propose a lightweight and accurate algorithm for IMU-assisted localization, which is able to obtain noticeable performance gain without incurring additional computational cost. To achieve this, we first probabilistically map measurements from all IMUs onto a virtual IMU. This step is performed by stochastic estimation with least-squares estimators and probabilistic marginalization of inter-IMU rotational accelerations. Subsequently, propagation equations of both state vector and error state of the virtual IMU are also derived, which enables the use of the classical filter-based or optimization-based localization algorithms via the virtual IMU. Finally, results from both simulation and real-world tests are provided, which demonstrate that the proposed algorithm outperforms competing algorithms by noticeable margins.

READ FULL TEXT VIEW PDF
POST COMMENT

Comments

There are no comments yet.

Authors

page 1

page 7

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

In recent years, the technology of using inertial measurement units for commercial applications has been under fast development. On one hand, the maturity of MEMS manufacturing process significantly reduces the size, price, and power consumption of the IMU hardware, and makes it a popular sensor used in robotics, personal electronic devices, wearable devices, and so on [koccer2013development]. On the other hand, significant progress has also been made in both software and algorithm design for IMUs, ranging from sensor characterization and calibration [flenniken2005characterization, li2014high, rehder2016extending, schneider2017visual], measurement integration [trawny2005indirect, Li2013high, forster2017manifold], sensor fusion[qin2018vins, skog2016inertial, li2013real, schneider2018maplab, huai2018robocentric], and so on.

In this work, we focus on the inertial-aided localization problem, which is to estimate the 6D pose (3D position and 3D orientation) of a moving platform. Since localization with IMU only inevitably suffers from long-term pose drift, measurements from other sensors, e.g., wheel encoders, laser range finders, or cameras, are typically used in combination with IMUs to provide performance guarantees [schneider2018maplab, zhang2019localization, ye2019tightly]. To formulate probabilistic estimators, the majority of existing work use measurements from IMU to perform pose prediction, which is followed by nonlinear refinement using measurements from other sensors[qin2018vins, skog2016inertial, li2013real, schneider2018maplab, Li2013high, forster2017manifold]. To ensure real-time and high-precision performance, IMU based pose prediction must yield low-drift and consistent pose estimates via efficient computational cost, which is one of the key research topics in the robotic research community [trawny2005indirect, Li2013high, forster2017manifold].

Fig. 1: The IMU array board used in this work, which contains nine ST LSM6DSOX IMUs marked by red rectangles and a processor interface to connect cameras.

To date, most algorithms on inertial-aided localization are designed based on a single IMU. Although those algorithms are successfully deployed in different applications, using additional IMU sensors creates new possibilities for further improving the system accuracy and robustness. To the best of the authors’ knowledge, the only work in recent years on robotic localization using multiple IMUs is [eckenhoff2019sensor], which proposed an approach for vision-aided inertial navigation using measurements from multiple IMUs. However, the proposed algorithm is of significantly increased computational cost, which makes real-world deployment on low-cost platforms infeasible. On the other hand, in global navigation satellite system (GNSS) community, there are a few approaches designed for using multiple IMUs [skog2016inertial, jafari2015optimal, shahri2017implementation]. However, GNSS signals are not reliable in indoor environments, underground caves, or even urban streets with densely constructed high-rise buildings. This inevitably limits the application of those approaches for general commercial platforms, e.g., robots, mobile devices, and so on.

In this paper, we propose a systematic framework of using multiple IMUs for localizing a moving platform. Compared to methods of using a single IMU, the proposed method is able to achieve better localization accuracy without incurring noticeable extra computational cost. In addition, our method can be integrated with any type of sensors, e.g., wheel encoders, laser range finders, and cameras, for designing different types of estimators. Our main contributions are as follows:

  • A mapping algorithm is proposed to map measurements from multiple IMUs onto a virtual IMU without information loss. This is achieved by both linear least-squares estimation and probabilistic marginalization of inter-IMU rotational accelerations.

  • Equations for propagating state estimate and error state vector are derived for the virtual IMU in closed-form. Those equations can be integrated into most existing algorithms for performing inertial-aided localization without additional modifications.

  • Results from both simulations and real-world experiments show that the proposed method outperforms competing methods by a wide margin.

We also point out that, in this work, experimental analysis on localization accuracy with different number of IMUs is also provided, based on our customized sensor platform consisting of nine IMUs (see Fig. 1).

Ii Related Work

In this work, we group the related work into two categories: the methods for inertial-aided localization and the ones using multiple IMUs.

Ii-a Inertial Aided Localization

The IMU sensor typically generates measurements at very high frequencies (e.g., - Hz) and provides direct 6D motion estimates of a moving platform. These properties make IMU an important complementary sensor for designing localization systems [Aided_INS].

Camera is another widely used sensor in robotic applications. However, when used for localization, the methods of relying on cameras only have theoretical drawbacks. They are unable to uniquely determine the roll and pitch angles as well as the scale parameter (in monocular camera setup). This limits the localization accuracy and robustness. However, when an IMU is added to the camera based localization system, those problems can be theoretically resolved, and the performance is largely improved. As a result, there are a variety of algorithms proposed in recently years on visual-inertial localization, and their contributions range from theoretical estimator design [qin2018vins][leutenegger2015keyframe], camera cost function design [zheng2017photometric][von2018direct], optimization of both the computational cost and accuracy [Li2013high][forster2017manifold], IMU to camera calibration [li2014high][rehder2016extending], and so on.

IMU is also commonly used in combination with both wheel encoders and laser range finders (LRFs). When combined with wheel encoders, IMU can help improve the dead reckoning and general localization precision [zhang2019localization], as well as detect wheel slippery [yi2007imu]. On the other hand, LRF measures intensity and range from each laser beam at typically low frequencies. Thus, the high-FPS measurements from an IMU can be used for estimating the motion between LRF scans and deriving estimators for robust localization [ye2019tightly].

Ii-B Methods of Using Multiple IMUs

Early work on processing measurements from multiple IMUs seek to reduce the measurement noises via least-squares estimation [colomina2004redundant, waegli2008redundant]. However, the important correlation terms between accelerometer measurements and rotational accelerations are ignored, and the system accuracy becomes not optimal. Those algorithms are later improved by [skog2016inertial], which presents a two-stage method by firstly computing error-reduced rotational velocities and subsequently obtaining the rotational accelerations and specific force vectors. Another category of methods on using multiple IMUs is to design centralized estimators with a ‘stacked’ state vector, concatenating the state of each IMU [bancroft2008twin, bancroft2009multiple]. This type of methods does not require approximations of sensors’ measurements, and is typically of high accuracy. However, the size of the concatenated state vector grows along with the number of IMU and consequently the computation efficiency decreases. The third type of methods is to design federated filters, using both local and master filters [carlson2001federated, bancroft2011data]. Shared states are estimated in both the local and master filter, while other states such as biases are only estimated in the local filters.

However, all previously mentioned methods focus on IMU signal processing or GNSS localization only. To the best of our knowledge, in existing literature, only [eckenhoff2019sensor] focuses on general localization using multiple IMUs. [eckenhoff2019sensor] proposes to add IMU poses of each IMU into the state vector in a vision-aided inertial navigation system, and derives relative pose-to-pose constraints between different IMUs. This approach is shown to generate results with higher accuracy, while at a cost of significantly increased computational complexity especially when many IMUs are used. In addition, although the general sensor fusion accuracy can be improved by [eckenhoff2019sensor], the improvement will be only available after each update. In other words, for applications that require IMU based forward integration (e.g., real-time control for drones or high-fps rendering in virtual reality headsets), [eckenhoff2019sensor] has limitations. By contrast, our proposed method is able to improve both overall localization accuracy and forward integration accuracy without incurring additional computational cost, which are the desired properties for a variety of applications.

The organization of the rest of this paper is as follows. Sec. III discusses the IMU measurement models and the standard approach of using a single IMU. Subsequently, we derive the method of generating the virtual IMU measurement from multiple IMUs in Sec. IV. After that, detailed analysis on virtual IMU propagation equations is provided in Sec. V. Finally, Sec. VI presents the performance of the proposed method by using datasets from both simulated and real-world experiments.

Iii Standard Method of Using A Single IMU

In this section, we first describe a standard method of using measurements from a single IMU, which will be extended to multiple IMU case in the following sections.

Iii-a IMU Measurement Equations

Assuming an IMU, , moves with respect to a global frame , the IMU’s measurements can be described by:

(1)
(2)

where and are the gyroscope and accelerometer measurements, and the angular velocity and linear acceleration of the IMU expressed in frames and respectively, the rotation from to , and the white Gaussian noises, and the measurement biases modeled as random walk processes, and the known gravity vector.

Iii-B IMU State Vector and Error State

Following [trawny2005indirect, Li2013high, schneider2018maplab], the IMU state is defined as:

(3)

where presents rotation from IMU frame to global frame (i.e. ) in quaternion [trawny2005indirect], and and stand for the IMU’s position and velocity. The continuous-time motion dynamics of the IMU are described as:

(4)

whose detailed form is

(5)
(6)
(7)

where , and

(8)

In the above equation,

represents the skew-symmetric matrix of vector

. Similar to [trawny2005indirect, Li2013high, schneider2018maplab], the IMU error state is defined:

(9)

in which the standard additive error definition is used for the position, velocity, and biases. The quaternion local error is defined:

(10)

where represents the estimate of .

Iii-C IMU Propagation Equations

Since is not unknown, to implement probabilistic estimators, the general method is to derive a nonlinear equation for predicting the state estimate and a linearized equation for characterizing the error state :

(11)
(12)

where and represent the continuous-time linearized state-transition matrix and noise Jacobian matrix, and is the noise term. Once Eq. 11 and 12 are defined, they can be straightforwardly integrated in to different types of localization estimators for performing motion estimation [li2014high, trawny2005indirect, Li2013high, forster2017manifold, qin2018vins, li2013real, schneider2018maplab, zhang2019localization, ye2019tightly].

Iv Virtual IMU Generation

Our high-level design choice of formulating multiple-IMU algorithm is to derive IMU propagation equations whose structure is as same as Eq. 11 and 12, without non-probabilistic assumptions. Once this is achieved, the proposed algorithm can also be directly integrated into a variety of existing estimators without requiring complicated software re-design. To achieve this, we rely on measurements from multiple IMUs to compute measurements for a ‘virtual’ IMU, which are used to replace and in Eq. 11. The entire process is described in this section. In the next section, we derive the detailed equations for and in Eq. 12, after the virtual IMU replacement. We also note that, for the rest this section, we discuss the representative case when two IMUs are involved, which can be straightforwardly extended to the case when there are more IMUs.

We begin by introducing our method of generating a ‘virtual’ IMU from real IMUs.

Virtual IMU Measurement.

Suppose that synchronized measurements , , and from IMU A and B are given, along with their extrinsic calibration . By arbitrarily picking the extrinsics for the virtual IMU (reference frame ) relative to IMU A, the corresponding virtual gyroscope and accelerometer measurements are given as:

(13)
(14)

where

(15)
(16)

is left nullspace projection of , and is the Moore-Penrose inverse of a real matrix , defined as:

(17)

With the generated virtual IMU measurements (Eq. 13 and 14), the prediction equation of the IMU state estimate (Eq. 11) can be used without any modification. The following two subsections discuss the methodology and derivations on generating the virtual IMU measurements.

Iv-a Virtual Gyroscope Model

In this section, we derive the measurement equations for the virtual gyroscope. To start with, we have the identity transformation:

(18)

Combination of Eq. 1 and 18 leads to:

(19)

Based on the measurements from IMUs and , the optimal estimate of the virtual IMU angular velocity is given by:

(20)

Due to limited space in this paper, to simplify the analysis, we here assume for our derivations throughout the paper. We also note that, if an IMU array is designed using the same type of IMUs, the assumption becomes exact. Solving for Eq. 20 leads to the linear least-squares estimate:

(21)

where is defined in Eq. 15. In addition, by substituting Eq. 19 into Eq. 21, we obtain:

(22)

With Eq. 21 and 22 being defined, we are able to formulate the virtual gyroscope measurements, similar to the the original IMU measurements (i.e. Eq. 1), as:

(23)

where , , and

are the measurement, bias vector, and noise for the virtual IMU respectively, given by:

(24)

We also note that, taking the expectation operator on both sides of Eq. 23 leads to Eq. 21, and further calculating the error terms results in Eq. 22. The noise covariance matrix in Eq. 23 can be computed as:

(25)

The above equation clearly indicates that, by using multiple IMUs, the overall IMU measurement noises can be reduced, and the localization precision should be improved. When , the expression in Eq. 25 becomes slightly different, but the conclusion on noise reduction still holds.

Additionally, based on Eq. 24, the continuous time dynamics of can be described by:

(26)

and the above noises are modeled as

(27)

Similar to Eq. 25, Eq. 27 also indicates that by generating the virtual measurements, the corresponding gyroscope bias vector drifts slower.

Iv-B Virtual Accelerometer Model

Similar to the derivation on virtual gyroscope measurements, to derive the model for virtual accelerometer, we start with identity equation:

(28)

Take the first and second derivative of the above equation leads to:

(29)

where are the angular velocity and the rotational acceleration of virtual IMU frame in global frame.

Substituting Eq. 2 into Eq. 29 leads to

(30)

where we have used the equality , and represents the specific force vector .

Similar to computing the optimal estimate of in Eq. 20, stacking accelerometer measurements from both IMUs via Eq. 30 leads to:

(31)

where matrix and operator are defined in Eq. 15 and 16 respectively. In Eq. 31, the rotational acceleration is unknown, and also not directly measured by the IMU sensor. If is not properly handled, can not be accurately represented. In this work, we adopt a method similar to [Li2013high], in which unknown visual features are probabilistically marginalized. By denoting the left nullspace projection of the matrix , Eq. 31 is equivalent to:

(32)

Solving for the above equation, we obtain:

(33)

with also defined in Eq. 16. Eq. 33 allows us to define the virtual accelerometer measurements, which can be given by:

(34)

Combining Eq. 30,  33 and 34, we are able to write:

(35)

with

(36)

and

(37)

where can be expanded as

(38)
(39)

In the above equation, we have used the quadratic error approximation .

Eq. 35 and 39 clearly demonstrate that, the virtual accelerometer measurements are affected by both gyroscope biases, accelerometer biases, gyroscope noises, and accelerometer noises from original IMUs. This is unlike the original IMU measurement equations, i.e., Eq. 1 and 2, in which measurement noises are not correlated and biases are independent.

Intuitively thinking, any rotation not along the axis of the IMU-to-IMU extrinsic position will create centripetal acceleration on the virtual IMU, which is a function of both extrinsic parameters and the rotational motion. This causes dependency of accelerometer measurements on rotational velocity component. Since rotational velocity measurements are also subject to biases and noises, the expression of accelerometer measurements becomes slightly more complicated. As shown in the next section this also causes the state-transition matrix and noise Jacobian matrix in propagation equations to have more fill-ins. However, since one of the most important role of using IMU measurements is to accurately determine the rotational components (integrating accelerometer measurements also requires rotating specific force by rotational estimates), this gives us another chance to further refine gyroscope bias estimates via accelerometer measurements, to improve the localization accuracy.

V Virtual IMU Propagation

In this section, we present a method to using virtual IMU measurements, i.e. Eq. 13 and 14, to integrate both the IMU state vector and error state. Similar to the definition for the case of single IMU, i.e. Eq. 3 and 9, we define the state and error state of the virtual IMU as:

(40)
(41)

whose continuous-time propagation equations are given by

(42)
(43)

with

(44)

To implement Eq. 42, we first generate the virtual IMU measurement from all IMUs via Eq. 13 and 14, and the remaining step of pose integration is as same as that of Eq. 11. By denoting

(45)
(46)

the state-transition matrix and noise Jacobian matrix in Eq. 43 can be computed, and the detailed expression is given by Eq. 47.

(47)

Compared to the single IMU case, and contain the following additional non-zero terms:

(48)

where

(49)

The detailed derivations of and are shown in the Appendix. In addition to those two terms, the expressions of all other components in and are identical to that of the single IMU case. Once detailed expression of each matrix in Eq. 43 is given, a computer programmable discrete-time estimator can be derived accordingly, similar to existing work on IMU integration [trawny2005indirect, forster2017manifold, schneider2018maplab]. We omit the details. Eq. 42 and 43 provide core equations for performing state and error state integration based on the virtual IMU. Those equations can be straightforwardly integrated into different types of tightly-coupled localization algorithms, identical to the process of using a single IMU.

It is also important to note that compared to processing measurements of a single IMU, the proposed algorithm requires neglect-able additional computational cost. The main additional computational operations include i) computing nullspace projection of matrix and obtaining in Eq. 15 and ii) computing discrete time error state propagation by handling extra fill-ins in and . The added computational cost of the first task is linear in the number of IMUs, while the second one is constant. By contrast, the computational cost of the method in [eckenhoff2019sensor] is cubic in the summation of number of camera poses and number of IMUs, and thus adding additional IMUs incurs significant extra computing time. This is not the case in our proposed method.

Fig. 2: IMU integration errors when different number of IMUs used for varying time durations. Left: Errors in predicted position; Middle: Errors in predicted orientation; Right: Errors in predicted velocity.

Vi Experiments

Vi-a Methodology

To demonstrate the performance of our proposed algorithm on processing measurements from multiple IMUs, we integrate it into a visual-inertial odometry (VIO) approach [zhang2019localization]111In our implementation of [zhang2019localization], only measurements from the IMU and cameras are used.. The performance of VIO is tested by using different number of IMUs. When there is only one IMU used, the VIO algorithm is identical to [zhang2019localization]. When more IMUs are used, the proposed method of generating virtual IMU measurements and performing virtual IMU propagation are implemented to replace the single-IMU pipeline. In addition, the method of [eckenhoff2019sensor] is implemented in our work, as a competing algorithm to compare against.

Vi-B Simulation Tests

We first show results from simulation tests. To ensure realistic simulation tests, we generated synthetic poses and sensor measurements based on real-world experiments, similar to the simulation method in [li2014vision]. In the tests, we used IMUs and a monocular camera, with perfectly known sensor intrinsic and extrinsic parameters. During data generation, each IMU was sampled at Hz and the camera captured measurements at Hz. During the simulation, the layout of the multiple IMU array is identical to that of Fig. 1, and all IMUs were also synchronized.

Vi-B1 Pose Prediction Error

The first test is to demonstrate the pose integration accuracy by using different number of IMUs for varying time durations. Specifically, we started this test with perfectly known IMU pose, i.e., position, orientation, velocity, biases, and used sensor measurements to predict poses in future timestamps. We also note that [eckenhoff2019sensor] is identical to the single IMU case in this test, since the formulation in [eckenhoff2019sensor] is not able to improve the forward IMU prediction accuracy.

Fig. 3: Localization errors in real-world experiments. Three methods are tested: The original VIO algorithm using a single IMU, the modified VIO algorithm using the proposed method with IMUs (method V, V = Virtual), and [eckenhoff2019sensor] with 9 IMUs (method C, C = Centralized). The left plot represents the final drifts in indoor tests, and the right plot denotes the positional RMS errors in urban street tests.
 Num. of IMUs 1 2 4 6 9
Environment 1
 Pos. err. (m) 0.7042 0.2768 0.2441 0.1740 0.1709
 Rot. err. (rad.) 0.0327 0.0026 0.0028 0.0052 0.0027
Environment 2
 Pos. err. (m) 1.0867 0.4830 0.3670 0.2550 0.2488
 Rot. err. (rad.) 0.0719 0.0080 0.0076 0.0076 0.0041
TABLE I: Pose RMS errors as a function of number of IMUs.

The testing results are shown in Fig. 2, which clearly demonstrate that by using more IMUs for pose integration the accuracy can be largely improved. This validates our theoretical analysis, and our design motivation of using more IMUs. In addition, we emphasize that, in all cases, using multiple IMUs for pose prediction is consistently better than the original method of using a single IMU and that of [eckenhoff2019sensor].

Vi-B2 VIO Localization Error

The second test is to demonstrate the localization accuracy when the proposed method is integrated into VIO. Similar to the previous tests, we conducted statistical comparison between the cases when different number of IMUs involved. For all methods, we computed the root-mean-square (RMS) errors for both 3D position and rotation over 50 Monte-Carlos simulation tests, under two sets of representative simulation environments.

Table I shows the results when , , , , and IMUs are used. The most important result from table I is that when multiple IMUs are used via the proposed method the localization accuracy is consistently better that that of the single-IMU based method. In addition, we also observe that when more IMUs are used, the accuracy can be further improved, in terms of both rotational estimates and positional estimates.

Vi-C Real World Experiments

Vi-C1 Testing Platforms and Localization Environment

To evaluate the performance of the proposed method, we also conducted experiments by using datasets from our customized sensor platform. Specifically, our sensor platform consists of a stereo camera system with ON AR0144 imaging sensors, an IMU array of ST LSM6DSOX IMUs and a RTK-GPS system. The IMU array is shown in Fig. 1 and the integrated sensor platform is shown in Fig 4. The intrinsic and extrinsic sensor calibration parameters were all calibrated offline via [rehder2016extending]. In our implementation of this experiment, we chose the virtual IMU frame to be identical to the central IMU in the IMU array.

To test the proposed method in different environments, we collected three datasets in indoor environments and three datasets on urban streets. During the data collection, images were recorded at Hz with pixels resolution in indoor environments and pixels resolution in outdoor environments. Measurements from all IMUs were with Hz. For indoor datasets, the trajectory lengths are about to meters. For the urban street datasets, the lengths are about km. For the tests on urban streets, we also recorded the poses from the RTK-GPS, which are used as ground truth for computing RMS errors for different algorithms. In indoor environments, due to lack of precise ground truth, we started and stopped the motion of our sensor platform at exactly the same location, to enable computing the final drift as the error metric. Similar to the simulation tests, we here also implemented VIO algorithms by using different number of IMUs as well as the competing algorithm [eckenhoff2019sensor].

Fig. 4: Sensor platform used to collect datasets. GPS receiver is marked by a blue rectangle, the stereo camera system by a green one and the IMU array board by a red one. Left figure: The front view. Right figure: The side view.

Vi-C2 Qualitative Results

We here report the qualitative results of all methods tested in both indoor and urban street tests, which are shown in Fig. 3. For all datasets, the proposed method consistently outperforms the original VIO algorithm with a single IMU. In fact, the average error reduction is about , which is significant and can be used as guidance when designing hardware sensor platform and software package for performing high-precision localization. In addition, when compared against [eckenhoff2019sensor], the proposed method yields similar localization accuracy, since both methods utilize measurement information from multiple IMUs without non-probabilistic approximations.

Moreover, we compare the computational efficiency of the proposed method and [eckenhoff2019sensor], by computing the following metric:

(50)

In fact, represents the gain in localization accuracy per computational cost, and the results are shown in Table II. The results demonstrate that, the proposed method is significantly more efficient when seeking for accuracy gain. In addition, the average time per update for tested algorithms is also given by Table III, which shows that the computational cost of the proposed method is almost identical to that of the single-IMU method and [eckenhoff2019sensor] is noticeable slower. This verifies our theoretical analysis that since [eckenhoff2019sensor] models all IMUs’ poses in the state vector of an estimator, the computational cost grows significantly when the number of IMU grows. By contrast, in our proposed method, since we generate measurements for a virtual IMU before performing update (with linear computational cost in the number of IMU, and constant cost in the number of VIO keyframes), the computational cost almost keeps unchanged.

 Num. Seq 1 2 3
[eckenhoff2019sensor] 0.2545 0.1186 0.2865
 Proposed 0.6894 0.3795 0.8198
TABLE II: Average accuracy improvement per computational cost, computed by Eq. 50.
 Num. Seq 1 2 3
 Single-IMU (msec) 9.9380 9.4280 10.6717
[eckenhoff2019sensor] (msec) 39.5155 40.9543 42.0586
 Proposed (msec) 10.0381 9.8014 11.6740
TABLE III: Time (msec) per update for competing algorithms

Vii Conclusions

In this paper, we present a lightweight and accurate localization algorithm using multiple inertial measurement units. Specifically, we propose a method of optimally generating a virtual IMU from the measurements of multiple IMUs, which is followed by closed-form derivation of virtual IMU propagation equations. By integrating the proposed method into a visual-inertial odometry algorithm, we show that it significantly improves the localization accuracy and outperforms competing algorithms by a wide margin.

We here provide detailed derivation for and :

(51)

To start with, we write from Eq. 39 as:

(52)

Subtracting Eq. 39 from Eq. 52 leads to

(53)

Therefore, for any vector , we can write:

(54)

As a result:

(55)

where

(56)

This completes our derivation.

References