Accurate IMU Preintegration Using Switched Linear Systems For Autonomous Systems

TL;DR

This paper introduces a novel IMU preintegration method using switched linear systems, improving accuracy for autonomous vehicle localization in GPS-denied environments by modeling constant acceleration and angular velocity between measurements.

Contribution

A new IMU integration model based on switched linear systems that assumes constant acceleration and angular velocity between measurements, outperforming existing models.

Findings

Outperforms state-of-the-art IMU integration models

Enhances localization accuracy for high-speed autonomous vehicles

Effective in GPS-denied environments

Abstract

Employing an inertial measurement unit (IMU) as an additional sensor can dramatically improve both reliability and accuracy of visual/Lidar odometry (VO/LO). Different IMU integration models are introduced using different assumptions on the linear acceleration from the IMU. In this paper, a novel IMU integration model is proposed by using switched linear systems. The proposed approach assumes that both the linear acceleration and the angular velocity in the body frame are constant between two consecutive IMU measurements. This is more realistic in real world situation compared to existing approaches which assume that linear acceleration is constant in the world frame while angular velocity is constant in the body frame between two successive IMU measurements. Experimental results show that the proposed approach outperforms the state-of-the-art IMU integration model. The proposed model is thus important for localization of high speed autonomous vehicles in GPS denied environments.

Figures

Tables

Table 1. TABLE I: Coefficients of the matrices E ​ ( θ ) 𝐸 𝜃 E(\theta) , Γ ​ ( θ ) Γ 𝜃 \Gamma(\theta) and Λ ​ ( θ ) Λ 𝜃 \Lambda(\theta)

Matrix	Coefficient of $I$	Coefficient of ${\theta }^{\wedge }$	Coefficient of ${\left({\theta }^{\wedge }\right)}^2$
$E\left(\theta \right)$	1	$\frac{h_1\left(\Vert \theta \Vert \right)}{\Vert \theta \Vert }$	$\frac{h_2\left(\Vert \theta \Vert \right)}{{\Vert \theta \Vert }^2}$
$\Gamma \left(\theta \right)$	1	$\frac{h_2\left(\Vert \theta \Vert \right)}{{\Vert \theta \Vert }^2}$	$\frac{h_3\left(\Vert \theta \Vert \right)}{{\Vert \theta \Vert }^3}$
$\Lambda \left(\theta \right)$	$\frac{1}{2}$	$\frac{h_3\left(\Vert \theta \Vert \right)}{{\Vert \theta \Vert }^3}$	$\frac{h_4\left(\Vert \theta \Vert \right)}{2{\Vert \theta \Vert }^4}$

Table 2. TABLE II: RMSE On Position For Different Sequences

Sequence	Forster [19] [cm]	Proposed [cm]	Percentage [$\%$]
MH_02_easy	16.70	16.05	3.89
MH_05_difficult	30.32	26.95	11.11
V1_01_easy	10.16	10.29	-1.28
V1_03_difficult	31.12	21.21	31.84
V2_01_easy	7.33	7.16	2.32
V2_03_difficult	28.10	22.51	19.89

Equations

$$\left\{\begin{array}[]{lcr}\dot{P}_{W}=V_{W}\\ \dot{V}_{W}=a_{W}\\ \dot{R}_{BW}=R_{BW}\omega_{B}^{\wedge}\end{array}\right.,$$

$$\omega_{B}^{\wedge}=\left[\begin{array}[]{lcr}0&-\omega_{3}&\omega_{2}\\ \omega_{3}&0&-\omega_{1}\\ -\omega_{2}&\omega_{1}&0\\ \end{array}\right].$$

$$\left\{\begin{array}[]{l}(\omega_{B}^{\wedge})^{2}=\left[\begin{array}[]{lcr}-\omega_{2}^{2}-\omega_{3}^{2}&\omega_{1}\omega_{2}&\omega_{1}\omega_{3}\\ \omega_{1}\omega_{2}&-\omega_{1}^{2}-\omega_{3}^{2}&\omega_{2}\omega_{3}\\ \omega_{1}\omega_{3}&\omega_{2}\omega_{3}&-\omega_{1}^{2}-\omega_{2}^{2}\\ \end{array}\right]\\ (\omega_{B}^{\wedge})^{3}=-\|\omega_{B}\|^{2}\omega_{B}^{\wedge}\\ \end{array}\right.,$$

$$\left\{\begin{array}[]{lcr}P_{W}(t+{\Delta}t)=\displaystyle P_{W}(t)+\int_{t}^{t+{\Delta}t}\limits V_{W}(\tau)d\tau+\int\int_{t}^{t+{\Delta}t}\limits a_{W}(\tau)d\tau^{2}\\ V_{W}(t+{\Delta}t)=\displaystyle V_{W}(t)+\int_{t}^{t+{\Delta}t}\limits a_{W}(\tau)d\tau\\ R_{BW}(t+{\Delta}t)=\displaystyle R_{BW}(t)exp(\int_{t}^{t+{\Delta}t}\limits\omega_{B}(\tau)d\tau)\\ \end{array}\right..$$

$$\left\{\begin{array}[]{lcr}P_{W}(t+{\Delta}t)=\displaystyle P_{W}(t)+V_{W}\Delta t+\frac{1}{2}a_{W}\Delta t^{2}\\ V_{W}(t+{\Delta}t)=\displaystyle V_{W}(t)+a_{W}\Delta t\\ R_{BW}(t+{\Delta}t)=\displaystyle R_{BW}(t)E(\theta_{B}(t))\\ \end{array}\right.,$$

$$\left\{\begin{array}[]{l}h_{1}(z)=\sin z\\ h_{2}(z)=1-\cos z\\ h_{3}(z)=z-\sin z\\ h_{4}(z)=2\cos z-2+z^{2}\\ \end{array}\right..$$

$$\left\{\begin{array}[]{l}\theta^{\wedge}\Gamma(\theta)+I=E(\theta)\\ \theta^{\wedge}\Lambda(\theta)+I=\Gamma(\theta)\\ \end{array}\right..$$

$$a_W(t)=R_{BW}(t)a_B+g_W,$$

$$a_B=\frac{u_B{F}_B}{m},$$

$$\left\{\begin{array}[]{lcr}P_{W}(t+{\Delta}t)=\displaystyle P_{W}(t)+V_{W}\Delta t+\frac{1}{2}g_{W}\Delta t^{2}+\frac{1}{2}R_{BW}(t)a_{B}\Delta t^{2}\\ V_{W}(t+{\Delta}t)=\displaystyle V_{W}(t)+g_{W}\Delta t+R_{BW}(t)a_{B}\Delta t\\ R_{BW}(t+{\Delta}t)=\displaystyle R_{BW}(t)E(\theta_{B}(t))\\ \end{array}\right..$$

$$\left\{\begin{array}[]{lcr}P_{W}(j)=P_{W}(i)+\Theta(i,j)+R_{BW}(i)\zeta_{1}(i,j)\\ V_{W}(j)=V_{W}(i)+g_{W}{\displaystyle\sum_{k=i}^{j-1}}\Delta t+R_{BW}(i)\mu_{1}(i,j)\\ R_{BW}(j)=R_{BW}(i)F(i,j)\\ \end{array}\right.,$$

$$\left\{\begin{array}[]{ll}\Theta(i,j)=V_{W}(i){\displaystyle\sum_{k=i}^{j-1}}\Delta t+\frac{g_{W}}{2}({\displaystyle\sum_{k=i}^{j-1}}\Delta t)^{2}\\ F(i,j)={\displaystyle\prod_{k=i}^{j-1}}E(\theta_{B}(k))\\ \end{array}\right.,$$

$$\left\{\begin{array}[]{ll}\zeta_{1}(i,j)={\displaystyle\sum_{k=i}^{j-1}}(\frac{1}{2}F(i,k)a_{B}(k)\Delta t^{2}+\mu_{1}(i,k)\Delta t)\\ \mu_{1}(i,j)={\displaystyle\sum_{k=i}^{j-1}}F(i,k)a_{B}(k)\Delta t\\ \end{array}\right..$$

$$T(i,j)=\left[\begin{array}[]{ll}F^{T}(i,j)&t(i,j)\\ 0&1\\ \end{array}\right],$$

$$\left[\begin{array}[]{l}\dot{P}_{W}\\ \dot{V}_{W}\\ \dot{R}_{1}\\ \dot{R}_{2}\\ \dot{R}_{3}\\ \end{array}\right]=\left[\begin{array}[]{c}V_{W}\\ R_{BW}a_{B}+g_{W}\\ -\omega_{B}^{\wedge}R_{1}\\ -\omega_{B}^{\wedge}R_{2}\\ -\omega_{B}^{\wedge}R_{3}\\ \end{array}\right],$$

$$\displaystyle R_{BW}a_{B}=\left[\begin{array}[]{l}R_{1}^{T}a_{B}\\ R_{2}^{T}a_{B}\\ R_{3}^{T}a_{B}\\ \end{array}\right]=\left[\begin{array}[]{l}a^{T}_{B}R_{1}\\ a^{T}_{B}R_{2}\\ a^{T}_{B}R_{3}\\ \end{array}\right].$$

$$\left[\begin{array}[]{l}\dot{P}_{W}\\ \dot{V}_{W}\\ \dot{R}_{1}\\ \dot{R}_{2}\\ \dot{R}_{3}\\ \end{array}\right]=A(1,a_{B},\omega_{B})\left[\begin{array}[]{l}P_{W}\\ V_{W}\\ R_{1}\\ R_{2}\\ R_{3}\\ \end{array}\right]+\left[\begin{array}[]{l}0\\ g_{W}\\ 0\\ 0\\ 0\\ \end{array}\right],$$

$$\displaystyle A(s,a,\omega)=\left[\begin{array}[]{lllll}0&sI&0&0&0\\ 0&0&\hat{A}_{1}(a)&\hat{A}_{2}(a)&\hat{A}_{3}(a)\\ 0&0&-\omega^{\wedge}&0&0\\ 0&0&0&-\omega^{\wedge}&0\\ 0&0&0&0&-\omega^{\wedge}\\ \end{array}\right],$$

$$\left\{\begin{array}[]{lll}\hat{A}_{1}(a)=\left[\begin{array}[]{lll}a&0&0\\ \end{array}\right]^{T}\\ \hat{A}_{2}(a)=\left[\begin{array}[]{lll}0&a&0\\ \end{array}\right]^{T}\\ \hat{A}_{3}(a)=\left[\begin{array}[]{lll}0&0&a\\ \end{array}\right]^{T}\\ \end{array}\right..$$

$$\left[\begin{array}[]{l}P_{W}(k+1)\\ V_{W}(k+1)\\ R_{1}(k+1)\\ R_{2}(k+1)\\ R_{3}(k+1)\\ \end{array}\right]=e^{A(\Delta t,\Delta ta_{B},\theta_{B})}\left[\begin{array}[]{l}P_{W}(k)\\ V_{W}(k)\\ R_{1}(k)\\ R_{2}(k)\\ R_{3}(k)\\ \end{array}\right]+\left[\begin{array}[]{l}g_{W}\frac{\Delta t^{2}}{2}\\ g_{W}\Delta t\\ 0\\ 0\\ 0\\ \end{array}\right],$$

$$\left[\begin{array}[]{lllll}I&sI&s\hat{A}_{1}(a)\Lambda(-\theta)&s\hat{A}_{2}(a)\Lambda(-\theta)&s\hat{A}_{3}(a)\Lambda(-\theta)\\ 0&I&\hat{A}_{1}(a)\Gamma(-\theta)&\hat{A}_{2}(a)\Gamma(-\theta)&\hat{A}_{3}(a)\Gamma(-\theta)\\ 0&0&E(-\theta)&0&0\\ 0&0&0&E(-\theta)&0\\ 0&0&0&0&E(-\theta)\\ \end{array}\right],$$

$$\left\{\begin{array}[]{l}a_{B}^{T}(k)\Lambda(-\theta_{B}(k))R_{i}(k)=R_{i}^{T}(k)\Lambda(\theta_{B}(k))a_{B}(k)\\ a_{B}^{T}(k)\Gamma(-\theta_{B}(k))R_{i}(k)=R_{i}^{T}(k)\Gamma(\theta_{B}(k))a_{B}(k)\\ (E(-\theta_{B}(k))R_{i})^{T}=R_{i}^{T}E(\theta_{B}(k))\end{array}\right.,$$

$$\left\{\begin{array}[]{lccr}P_{W}(k+1)=\displaystyle P_{W}(k)+V_{W}(k)\Delta t\\ \hskip 48.36967pt+(\frac{1}{2}g_{W}+R_{BW}(k)\Lambda(\theta_{B}(k))a_{B}(k))\Delta t^{2}\\ V_{W}(k+1)=\displaystyle V_{W}(k)+(g_{W}+R_{BW}(k)\Gamma(\theta_{B}(k))a_{B}(k))\Delta t\\ R_{BW}(k+1)=\displaystyle R_{BW}(k)E(\theta_{B}(k))\\ \end{array}\right..$$

$$\hat{A}(k)={\exp }^{A(\Delta t,\Delta t{a}_B(k),{\theta }_B(k))},$$

$$\hat{b}=\left[\begin{array}[]{lllll}g_{W}\frac{\Delta t^{2}}{2}&g_{W}\Delta t&0&0&0\end{array}\right]^{T},$$

$$\left[\begin{array}[]{l}P_{W}(k+1)\\ V_{W}(k+1)\\ R_{1}(k+1)\\ R_{2}(k+1)\\ R_{3}(k+1)\\ \end{array}\right]=\hat{A}(k)\left[\begin{array}[]{l}P_{W}(k)\\ V_{W}(k)\\ R_{1}(k)\\ R_{2}(k)\\ R_{3}(k)\\ \end{array}\right]+\hat{b},$$

$$\left\{\begin{array}[]{lcr}P_{W}(j)=P_{W}(i)+\Theta(i,j)+R_{BW}(i)\zeta_{2}(i,j)\\ V_{W}(j)=V_{W}(i)+g_{W}{\displaystyle\sum_{k=i}^{j-1}}\Delta t+R_{BW}(i)\mu_{2}(i,j)\\ R_{BW}(j)=R_{BW}(i)F(i,j)\\ \end{array}\right.,$$

$$\left\{\begin{array}[]{lll}\zeta_{2}(i,j)={\displaystyle\sum_{k=i}^{j-1}}(F(i,k)\Lambda(\theta_{B}(k))a_{B}(k)\Delta t^{2}+\mu_{2}(i,k)\Delta t)\\ \mu_{2}(i,j)={\displaystyle\sum_{k=i}^{j-1}}F(i,k)\Gamma(\theta_{B}(k))a_{B}(k)\Delta t\\ \end{array}\right..$$

Full text

I INTRODUCTION

State estimation is definitely one of the most essential modules in many applications such as an autonomous driving, self-localization systems and precise navigation [1], [2]. Environment which has poor or lack of global positioning system (GPS) represents a great challenge for motion estimation. Accordingly, motion estimation of an agent such as an unmanned aerial vehicle (UAV) or an unmanned ground vehicle (UGV) is crucial in such environment [3], [4]. Employing an inertial measurement unit (IMU) as an additional sensor could allow low latency high update rate state output required for low-level control [5] and simplify visual odometry (VO) [6], [7] and lidar odometry (LO) [8], [9], [10]. The estimated position, velocity and orientation can be obtained by integrating both the linear acceleration and angular velocity from the IMU assuming that the discrete outputs are constant between two successive sampling points. However, the accumulated error due to the integration process induces substantial drift and inaccuracy in attitude estimation. Sensor fusion can be adopted to correct the drift whereby the estimated state obtained by the motion integration using the IMU data is used as a prediction state to update or correct the estimated state using another onboard sensor [11], [12].

Sensor fusion techniques are undoubtedly crucial for online state estimation. Graph based optimization approaches give higher state estimation accuracy than the filtering based approaches [13], [14], [15], [16] and [17]. However, the fact that the IMU has a higher sampling rate than other sensors is used in existing motion estimation represents a great challenge for online estimation using graph based optimization approaches. Whereby, the trajectory grows rapidly and the online estimation becomes infeasible. Lupton and Sukkarieh [18] established the pre-integration theory whereby the linear acceleration and angular velocity are integrated in the body frame before they are transferred to the world frame between two consecutive keyframes. However, previously both the linear acceleration and the angular velocity were transferred to the world frame at each time stamp to estimate the state before being integrated. Forster et al. [19] has introduced the IMU preintegration on the manifold SO(3) on top of the work in [18]. The IMU preintegration theory is widely used nowadays because it integrates the IMU mesaurements into a single value between two consecutive frames. Thus, the online optimization can be done because the graph-based optimization have been simplified. However, the IMU integration model has been obtained under different assumptions to solve the differential equations whereby the linear acceleration in the world frame and the angular velocity in the body frame are assumed to be constant between two consecutive IMU measurements in [19]. Consequently, the agent orientation has also been assumed to be constant for each integration interval between two successive IMU measurements. However, this assumption is not always true. For example, the linear acceleration of a multi-copter system has a higher chance to be constant in the body frame rather than in the world frame between two consecutive IMU measurements. Even though higher-order numerical methods [20], [21] could be used to improve the IMU integration method in [19], the complexity of the high-order solution could be an issue. On the other hand, it is very difficult to take into account the IMU biases and to compute the covariance of the high-order IMU integration. It is thus necessary to set up a new IMU integration model which based on more realistic assumptions.

In this paper, a novel IMU Preintegration model is introduced by using switched linear systems [22]. The new model assumes that both the linear acceleration and the angular velocity in the body frame are constant between two consecutive IMU measurements. The assumption that the linear acceleration in the body frame is constant between two consecutive IMU measurements is reasonable because external forces, such as thrust, are of fixed orientation in the body frame [23]. A simple discrete linear system is first developed to represent the kinematic dynamics between two successive IMU measurements. Since both the linear acceleration and the angular velocity between two successive visual frames are usually time-varying, the overall system between two successive visual frames is a switched linear system [22]. The IMU preintegration between two video frames is then obtained by integrating the switched linear system using all the IMU measurements between the two video frames. The proposed approach provides an analytic solution to the motion integration rather than an approximation solution as in [19]. Thus, it presents a more accurate motion estimation regardless of the value of angular velocity between two successive IMU measurements. Experimental results show that the proposed model outperforms the classical model in [19]. Overall, the main contribution of this paper is a novel IMU preintegration model which gives a better motion accuracy because the kinematics between two video frames is analyzed using switched linear systems. Since the proposed model can be applied to improve visual odmetry and Lidar odometry, it is useful for localization of high speed autonomous vehicles in GPS denied environments.

This paper is organized as follow. The classical IMU integration model is presented in section II. A new model is introduced in section III. The performance of the new model is evaluated in section IV using both real datasets. Finally, section V concludes this paper.

II Related Works on IMU Preintegration

In this section, the detail on the IMU preintegration model in [19] is provided. An IMU provides both linear acceleration and angular velocity in the body frame in the form of a 3-axis accelerometer and 3-axis gyroscope data denoted by $a_{B}$ and $\omega_{B}$. The body frame is expressed by the suffix B notation while the world frame is expressed by the suffix W notation. The linear acceleration vector and the angular velocity vector are transformed into the world frame using the rotational matrix from the body frame to the world frame. The gravity vector in the world frame is denoted by $g_{W}$.

Let P_{W}=\left[\begin{array}[]{lll}P_{1}&P_{2}&P_{3}\end{array}\right]^{T} and V_{W}=\left[\begin{array}[]{lll}V_{1}&V_{2}&V_{3}\end{array}\right]^{T} be the position of the camera center and the linear velocity of the agent having the camera in the world reference frame and $R_{BW}$ be the rotation matrix from the body frame to the world reference one. The IMU integration can be obtained by introducing the IMU kinematic model as [19, 24]:

[TABLE]

where \omega_{B}=\left[\begin{array}[]{lll}\omega_{1}&\omega_{2}&\omega_{3}\end{array}\right]^{T} is the angular velocity of the agent measured by a gyroscope in the body frame. The matrix $\omega_{B}^{\wedge}$ is

[TABLE]

It can be easily derived that

[TABLE]

where $\|\omega_{B}\|^{2}$ is $(\omega_{1}^{2}+\omega_{2}^{2}+\omega_{3}^{2})$.

The state estimate at $t+{\bigtriangleup}t$ is obtained by the integration of kinematic model (1) from $t$ to $t+{\bigtriangleup}t$ as

[TABLE]

Generally, equation (4) does not have an exact solution for measurements with time varying linear acceleration and angular velocity. By assuming that the linear acceleration in the world frame $a_{W}$ and the angular velocity in the body frame $\omega_{B}$ are constant in the time interval [$t$, $t+{\Delta}t$] [19], the kinematic model can be expressed by

[TABLE]

where $\theta_{B}(t)$ is $\omega_{B}(t)\Delta t$, and the matrix $E(\theta_{B}(t))$ is defined as in Table I. When the values of $\omega_{i}(i=1,2,3)$ are zero’s, the matrix $E(\theta)$ becomes the $3\times 3$ identity matrix $I$.

The functions $h_{i}(z)(i=1,2,3,4)$ in Table I are defined as

[TABLE]

It can be easily verified that

[TABLE]

Notice that $a_{W}$ can be expressed by:

[TABLE]

where a_{B}=\left[\begin{array}[]{lll}a_{1}&a_{2}&a_{3}\end{array}\right]^{T} is the linear acceleration in the body frame that is measured with an accelerometer, and its value is [23]

[TABLE]

$u_{B}$ is the magnitude of thrust applied to the agent, $F_{B}$ is a constant unit vector in the body frame representing the fixed orientation of the thruster, and $m$ is the mass of the agent. g_{W}=\left[\begin{array}[]{lll}0&0&-9.81\end{array}\right]^{T} with $g_{W}$ being the gravity acceleration in the world frame.

Substituting (8) in (5), the IMU kinematic model becomes

[TABLE]

The principle of IMU preintegration theory is to integrate all IMU measurements between two visual keyframes in a single compound measurement. The IMU and camera can provide measurements synchronously at the discrete time $k$. Suppose that two video frames are captured at the time instances $k=i$ and $k=j$. The IMU integrated model can be obtained by integration of (10) between the two frames as shown in the following equation:

[TABLE]

where the vector $\Theta(i,j)$ and the matrix $F(i,j)$ are computed as

[TABLE]

and the vectors $\zeta_{1}(i,j)$ and $\mu_{1}(i,j)$ are given as

[TABLE]

The translation matrix between the two video frames $i$ and $j$, $T(i,j)$ is finally computed as

[TABLE]

where the translation vector $t(i,j)$ is $(-F^{T}(i,j)(\zeta_{1}(i,j)+R_{WB}(i)\Theta(i,j)))$.

The equations (11) and (12) can be regarded as the classical IMU integration model. It can be shown from the equations (8) and (9) that the linear acceleration can be assumed to be constant in the world frame if the value of $\omega_{B}$ is zero between two successive IMU measurements. As such, the agent orientation $R_{BW}(t)$ is constant in the time interval $[t,t+\Delta t]$. All the integrations in the equation (10) are accurate. It was pointed out in [19] that this assumption is usually true if the the sampling rate of IMU is high. But it could be an issue if 1) the sampling rate is slow or 2) the value of $\omega_{B}$ is not zero between two successive IMU measurements at a high sampling rate. According to the equation (9), $a_{B}$* intuitively has more chance than $a_{W}$ to be constant in the small interval $[t,t+\Delta t]$*.

III A New IMU Preintegration Model Using Switched Linear Systems

In this section, a new IMU integration model is derived under an assumption that both the linear acceleration and the angular velocity in the body frame are constant between two successive IMU measurements.

Let R_{WB}=\left[\begin{array}[]{lll}R_{1}&R_{2}&R_{3}\end{array}\right] be the rotation matrix from the world reference frame to the body frame. A new kinematics model will be built up using the new state vector whereby new state vector is introduced as \left[\begin{array}[]{lllll}P_{W}^{T}&V_{W}^{T}&R_{1}^{T}&R_{2}^{T}&R_{3}^{T}\end{array}\right]^{T}. Let $I$ be a $3\times 3$ identity matrix. Using the equation $R_{WB}R_{BW}=I$, it can be derived from the equations (1) and (8) that

[TABLE]

Using the equality that $R_{BW}=R_{WB}^{T}$ and the matrix theory [25], it can be derived that

[TABLE]

It follows that

[TABLE]

where the matrix $A(s,a,\omega)$ is given by

[TABLE]

and the matrices $\hat{A}_{i}(a)(1\leq i\leq 3)$ are

[TABLE]

The new kinematics model (23) is a time varying linear system. Assuming that the values of $a_{B}$ and $\omega_{B}$ are fixed in the small interval $[k\Delta t,(k+1)\Delta t]$, the matrix $A(1,a_{B},\omega_{B})$ is a constant matrix in the interval. Subsequently, the system in the equation (23) is a linear time invariant system in the interval. Using the linear control system theory [26], a discrete model can be derived from the equation (23) as [25, 26]

[TABLE]

where the matrix $e^{A(s,a,\theta)}$ is given as

[TABLE]

and the matrices $\Gamma(\theta)$ and $\Lambda(\theta)$ are defined as in Table I.

Using the following equation

[TABLE]

it can be shown that the equation (31) is equivalent to

[TABLE]

Similar to [19], we also assume that two video frames are captured at the time instances $k=i$ and $k=j$. Defining a matrix $\hat{A}(k)$ as

[TABLE]

and a vector $\hat{b}$ as

[TABLE]

the kinematics of the agent between the two video frames can be represented by the following time variant system:

[TABLE]

where $i\leq k\leq j$.

The values of $a_{B}$ and $\omega_{B}$ may be changed in the next small time interval $[(k+1)\Delta t,(k+2)\Delta t]$. This implies that the matrix $\hat{A}(k)$ could be different from the matrix $\hat{A}(k+1)$. Thus, the time variant system in the equation (37) is a switched linear system [22]. The discrete dynamic model in the equation (37) will be used to integrate all the IMU data between the two video frames as follows:

[TABLE]

where the vectors $\zeta_{2}(i,j)$ and $\mu_{2}(i,j)$ a are computed as

[TABLE]

The rotation matrix for the proposed model is $F^{T}(i,j)$, and the translation vector $t(i,j)$ for the proposed model is $(-F^{T}(i,j)(\zeta_{2}(i,j)+R_{WB}(i)\Theta(i,j)))$.

IV Experimental Results

In this section, the proposed model is compared with the classical model in [19] using real public datasets. These datasets include different conditions in flight dynamics, texture and illumination. For the fair comparison, we have implemented both models on VINS_MONO benchmark which it is an open source code [27]. The evaluation has been done using high sampling IMU 200Hz and all key frames have been selected at 10Hz. The loop closure has also been enabled and used in this evaluation test. The estimation output from VINS_MONO benchmark has a different reference from the ground truth reference. Thus, the estimation raw data has been aligned first with the ground truth reference using the trajectory evaluation toolbox in [28]. It is worth noting that the alignment process has been done using the full trajectory states.

The EuRoC dataset has been used in this test where it provides the measurement using a micro aerial vehicle (MAV) in different places with different conditions in flight dynamics, texture and illumination. The dataset has been classified into different sequences as easy, medium and difficult. The measurements have been recorded at 200Hz for IMU and 20Hz for images. The dataset has been synchronized, corrected and matched with the IMU measurements output [29]. As we evaluate both models on monocular visual inertial odometry, the images from the left camera have been used only.

IV-A Difficult Conditions Sequences

In this subsection, the comparison between the state of the art and the proposed approach has been done on difficult conditions sequences which presents the most challenging conditions in the terms of lighting and flights dynamics and texture-less areas. Accordingly, a V1_03_difficult sequence has been selected to perform this analysis. Fig. 1 shows the estimated trajectories of the state of the art and proposed approaches compared to the ground-truth. In addition, the accuracy of the proposed approach is evaluated against model in [19]. The RMSE and position errors in x-direction, y-direction and z-direction have been illustrated in Fig. 2. The proposed model estimates the position more accurately at x, y and z directions whereby the proposed model outperforms the model in [19] by 31.84$\%$ over the full path of V1_03_difficult sequence. Not only the proposed model outperforms the classical model on V1_03_difficult sequence, but also it outperforms the classical model on both sequences V2_03_difficult and MH_05_difficult by 19.89$\%$ and 11.11$\%$, respectively. The reason that, the proposed can handle the dramatic dynamic changes and gives a reliable estimation over the classical one, the proposed model takes into consideration the dynamic changes between measurements. The metric RMSE errors of both sequences have been illustrated at Table II.

IV-B Comparison on Different Sequences

In this subsection, the proposed model has been evaluated using different sequences to test the accuracy of the proposed model over the classical one in different environment. Table II shows the root mean square error (RMSE) over the full path of all sequences which have been used in this evaluation. The proposed model outperforms the classical model on MH_02_easy and V2_01_easy sequences by 3.89$\%$ and 2.32$\%$, respectively. On the other hand, the classical model outperforms the proposed model on V1_01_easy sequence but with a very small difference by 1.3 $mm$ only. When the condition of motion becomes aggressive and significant changing in illumination with texture-less area, the proposed model can gives much better estimation. This implies that the proposed model achieves higher accuracy than the state of the art especially in these cases when high dynamic changes in angular velocity or significant illumination changing environment with texture-less area.

V CONCLUSIONS

A new IMU preintegration model has been proposed with formulation using switched linear systems. The proposed model assumes constant linear acceleration in the body frame. Doing so allow our proposed model to be closer to real world condition. Our proposed model is compared with state-of-the-art model on real datasets with different conditions in flight dynamics, texture and illumination. Experimental results show that the proposed model outperforms the state-of-the-art model.

There are many applications of the proposed model. For example, it can utilized to improve visual odmetry [3, 19] and Lidar odometry [1, 30]. These applications are important for high speed autonomous vehicles. For a potential contact-based inspection operation within UAV framework [31, 32], it can be a significant stage to leverage such algorithms in real time. All these works will be studied in our future research.

ACKNOWLEDGMENT

This research is partially supported by SERC grant No. 162 25 00036 from the National Robotics Programme (NRP) and No. A1715a0064 from Aerospace Program, Singapore.