Monocular Visual-Inertial Odometry with an Unbiased Linear System Model and Robust Feature Tracking Front-End

Abstract

The research field of visual-inertial odometry has entered a mature stage in recent years. However, unneglectable problems still exist. Tradeoffs have to be made between high accuracy and low computation for users. In addition, notation confusion exists in quaternion descriptions of rotation; although not fatal, this may results in unnecessary difficulties in understanding for researchers. In this paper, we develop a visual-inertial odometry which gives consideration to both precision and computation. The proposed algorithm is a filter-based solution that utilizes the framework of the noted multi-state constraint Kalman filter. To dispel notation confusion, we deduced the error state transition equation from scratch, using the more cognitive Hamilton notation of quaternion. We further come up with a fully linear closed-form formulation that is readily implemented. As the filter-based back-end is vulnerable to feature matching outliers, a descriptor-assisted optical flow tracking front-end was developed to cope with the issue. This modification only requires negligible additional computation. In addition, an initialization procedure is implemented, which automatically selects static data to initialize the filter state. Evaluations of proposed methods were done on a public, real-world dataset, and comparisons were made with state-of-the-art solutions. The experimental results show that the proposed solution is comparable in precision and demonstrates higher computation efficiency compared to the state-of-the-art.

1. Introduction

The fusion of monocular cameras and inertial measurement units (IMUs) is very popular recently, thanks to great improvements in the computation capacity of computers with low energy costs and low weight, and the increasing demand for accurate motion tracking or positioning in unmanned aerial vehicles (UAVs), augmented reality, and driverless cars. This fusion problem has been studied by brilliant scientists for years [1,2,3,4], and as a result, one can now build his own visual-inertial odometry (VIO) module simply with cheap sensors and open-source software [2,5,6,7,8,9].

Fusion frameworks are divided into two main branches: filter-based and optimization- based [10,11]. Optimization-based methods are so far widely recognized performing better in terms of precision [12] due to their iterating mechanism, which is essentially solving a noted bundle adjustment (BA) problem [13]. The BA problem was considered to be computational costly in earlier years, until the literature recognized and revealed its sparse structure [13,14] so as to develop real-time algorithms. Kümmerle et al. [7] modeled BA as a graph optimization problem. Kaess et al. [8] introduced a factor graph model to further illustrate the Bayesian nature of BA. Kaess et al. [8] also found that the incremental fact of sophiscated Hessian matrix in normal equation can be utilized for solving BA, thus speeding up the calculation further. With these profound insights, researchers also made efforts to overcome the inconsistency in fixed-lag fusion algorithms, which has the advantages of having bounded computation with less information loss and maintaining the sparse structure [15]. Several open-source libraries are available for building the back-end for algorithms of this branch, based on different mathematical descriptions listed above and providing convenient application program interfaces (APIs) [7,8,9,16]. Although enabling reduced computation by leveraging sparse matrix factorization, optimization-based VIO systems still need to be tailored sometimes in order to be deployed on a computation-limited platform. Sometimes, this leads to a downgraded performance [17].

Before the flowering of optimization-based methods, the solving of fusion problems was dominated by filtering [18]. The ordinary procedure is to include IMU pose and map point positions in the filter state and recursively propagate and update as IMU and camera measurements respectively arrive [10]. The accurate estimation of map point positions is the key to bring about an unbiased IMU pose updating. In traditional filter-based solutions, the filter state would invariably have a very large dimension since it always preserves a lot of map points, resulting in enhanced computation requirements [1]. The use of a multi-state constraint Kalman filter (MSCKF) was proposed as an effective and optimal filter-based solution that does not maintain map points in filter state [19]. By properly handling the camera measurement, MSCKF can achieve as competitive a performance as optimization-based algorithms and demands far less computation [17].

By correcting observability properties [19,20,21] and incorporating camera–IMU extrinsic parameters into the filter state [22], the performance of MSCKF was further improved. Many follow-up works emerged, including an open-source monocular implementation [23], expansion to stereo camera rig [24], and schemes using direct visual front-ends [25] or adding line features [26].

It should be emphasized that all members of the MSCKF family so far have been developed based on Shuster’s notation of quaternion [27], whereas most of the community utilizes the traditional Hamilton notation, which results in unnecessary trouble in understanding for researchers [28].

Visual front-ends apparently play an important role in VIOs. There are typically of two categories. Feature-based methods use descriptors to match features between consecutive images [6], while direct methods seek a minimization of photometric residuals to accomplish data correlation [5,25]. Sparse optical flow tracking is an efficient direct method that is widely used [2,23,24]. It provides sub-pixel accuracy but contains more outliers than feature descriptor matching [29]. An optimization-based back-end would eliminate outliers during iteration [30]. Filter-based back-ends are meanwhile vulnerable to the outliers if only one-off updating is applied [23]. Using an iterated update scheme would mitigate this situation while introducing additive computation [31].

To recap, in order to make VIO algorithms more practical, it is desirable to develop algorithms with lower computation while maintaining high precision.

In this paper, we developed a filter-based monocular visual-inertial odometry which can be regarded as a member of MSCKF family, giving consideration to both high precision and computation efficiency. The main contributions of this paper are as follows:

• We deduced a closed-form IMU error state transition equation based on the more cognitive Hamilton notation of quaternion. By solving integration terms analytically, a novel fully linear formulation was further obtained, which is also closed-form, and furthermore, is readily implemented.
• By analyzing the statistical properties of ORB descriptor [32] distances of matched and unmatched feature points, we introduced a novel descriptor-assisted sparse optical flow tracking technique, which enhances the feature tracking robustness and barely adds any computation complexity.
• More improvements are made to improve the usability and performance of the filter. An initialization procedure is developed that automatically detects stationary scenes by analyzing tracked features and initializes the filter state based on static IMU data. The feature triangulation mechanism is carefully refined to provide efficient measurement updates.
• A filter-based monocular VIO using the proposed state transition equation, visual front-end, and initialization procedure under Sun et al.’s [24] framework is implemented. The performances of our VIO and MSCKF-MONO [23], an open-source monocular implementation of MSCKF, are compared with parameters setup as similarly as possible. Ours is also compared with other state-of-the-art open-source VIOs including ROVIO [5], OKVIS [6], and VINS-MONO [2]. In addition, we analyze the process time of our algorithm. All of the evaluations above are done on EuRoC datasets [33]. Detailed evaluations are reported.

The rest of this paper is organized as follows. The problem of quaternion notation confusion is illustrated in Section 2. Section 3 deduces the error state differential equation based on Hamilton’s notation. Section 4 gives a closed-form error state transition formulation and then solves the integration terms in it, obtaining a fully linear closed-form formulation. Section 5 presents the descriptor-assisted sparse optical flow tracking front-end. Other implementation details and improvements are presented in Section 6, including the overall filter model, automatic initialization procedure, and refined feature triangulation mechanism. Section 7 presents the experimental results in detail. Finally, conclusions are made in Section 8.

2. Quaternion Notation Confusion

Quaternion is one of the widely used representations of rotation in numerical calculations [34]. In the related literature, there are mainly two different notations: Hamilton’s notation and Shuster’s notation [35]. The difference between them lies in their flipped rule for the multiplication of imaginary parts $\mathbf{i}$, $\mathbf{j}$, and $\mathbf{k}$. Hamilton utilizes $\mathbf{ij}=\mathbf{k}$, while Shuster advocates for $\mathbf{ij}=-\mathbf{k}$ to maintain the order of chain rule when transferring to direction cosine matrices (DCMs). Sommer et al. [28] surveyed this notation confusion problem in detail and argue for entirely abandoning Shuster’s notation. In this section, we present the original problem that Shuster’s notation is designed to solve and a solution for maintaining chain rule order while still using Hamilton’s notation.

A quaternion of rotation $\mathbf{q}$ is basically a unit quaternion; it can be defined as

$$\begin{array}{c}\hfill \mathbf{q}=cos\frac{\theta }{2}+{\mathbf{u}}^A·sin\frac{\theta }{2}\end{array}$$

(1)

where ${\mathbf{u}}^A$ is the unit vector of rotation axis in frame A, and $\theta$ is the angle of rotation. In the rest of this article, the term “quaternion” will be used to refer to a quaternion of rotation, for the sake of simplicity.

Equation (1) shows how to construct a quaternion $\mathbf{q}$ from an axis-angle $\theta {\mathbf{u}}^A$, which describes the anticlockwise rotation of an angle $\theta$ about the axis $\mathbf{u}$. If the original frame A is rotated to a new frame B after this rotation, as illustrated in Figure 1, then we can use a quaternion ${\mathbf{q}}_A^B$ or a DCM ${\mathbf{R}}_A^B$ to describe this rotation.

Note that ${\mathbf{R}}_A^B$ can be used to compute the coordinate of a vector $\mathbf{v}$ in frame B given its coordinate in frame A, that is ${\mathbf{v}}^B={\mathbf{R}}_A^B{\mathbf{v}}^A$. ${\mathbf{R}}_A^B$ can be written as a function of ${\mathbf{q}}_A^B$

$$\begin{array}{c}\hfill {\mathbf{R}}_A^B=C_S\left({\mathbf{q}}_A^B\right)\end{array}$$

(2)

where $C_S\left(•\right)$ is an operator mapping ${\mathbf{q}}_A^B$ to ${\mathbf{R}}_A^B$.

Let ${\mathbf{q}}_B^C$ be the quaternion describing the rotation from frame B to frame C and ${\mathbf{q}}_A^C$ the rotation from frame A to frame C. Then, according to Equation (2), we have

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathbf{R}}_B^C=& C_S\left({\mathbf{q}}_B^C\right)\hfill \\ \hfill {\mathbf{R}}_A^C=& C_S\left({\mathbf{q}}_A^C\right).\hfill \end{array}\end{array}$$

(3)

Coordinate transformation of vectors can also be done by applying the triple product of quaternions:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathbf{v}}^B=& {\left({\mathbf{q}}_A^B\right)}^{-1}\otimes {\mathbf{v}}^A\otimes {\mathbf{q}}_A^B\hfill \\ \hfill {\mathbf{v}}^C=& {\left({\mathbf{q}}_B^C\right)}^{-1}\otimes {\mathbf{v}}^B\otimes {\mathbf{q}}_B^C.\hfill \end{array}\end{array}$$

(4)

Here we abuse the notation of ${\mathbf{v}}^A$, ${\mathbf{v}}^B$, and ${\mathbf{v}}^C$ to describe quaternions with zero real part such that ${\mathbf{v}}^A\to {\left[\begin{array}{cc}0& {\left({\mathbf{v}}^A\right)}^T\end{array}\right]}^T$. Combining the two equations above yields:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathbf{v}}^C=& {\left({\mathbf{q}}_B^C\right)}^{-1}\otimes {\left({\mathbf{q}}_A^B\right)}^{-1}\otimes {\mathbf{v}}^A\otimes {\mathbf{q}}_A^B\otimes {\mathbf{q}}_B^C\hfill \\ \hfill =& {\left({\mathbf{q}}_A^B\otimes {\mathbf{q}}_B^C\right)}^{-1}\otimes {\mathbf{v}}^A\otimes \left({\mathbf{q}}_A^B\otimes {\mathbf{q}}_B^C\right).\hfill \end{array}\end{array}$$

(5)

Referring to Equation (5), there is

$$\begin{array}{c}\hfill {\mathbf{q}}_A^C={\mathbf{q}}_A^B\otimes {\mathbf{q}}_B^C.\end{array}$$

(6)

At the same time, by applying the chain rule in DCM production, it follows that

$$\begin{array}{c}\hfill {\mathbf{R}}_A^C={\mathbf{R}}_B^C{\mathbf{R}}_A^B.\end{array}$$

(7)

Now we can conclude that $C_S\left({\mathbf{q}}_A^B\otimes {\mathbf{q}}_B^C\right)=C_S\left({\mathbf{q}}_B^C\right)C_S\left({\mathbf{q}}_A^B\right)$, which means the mapping $C_S\left(•\right)$ is not a homomorphism. One would prefer a homomorphic mapping between DCM and quaternion to maintain the chain-rule order, which is convenient to manipulate. Shuster utilized a flipped multiplication rule to avoid this problem. This notation was adopted by the Jet Propulsion Laboratory (JPL) and thus introduced to spacecraft literatures, while other research fields were still using the traditional Hamilton notation. But as researchers have exchanged ideas between different research fields, Shuster’s notation has been utilized in robotics for rotation representation [28]. So far, all of the theories about MSCKF are deduced based on this notation [1].

As Sommer et al. [28] claimed, a homomorphic mapping could be obtained even under Hamilton’s notation. Let $C_H\left(•\right)$ be an operator that satisfies $C_H\left(\mathbf{q}\right)=C_S{\left(\mathbf{q}\right)}^T$. Thus, we have

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathbf{R}}_B^A=& C_H\left({\mathbf{q}}_A^B\right)\hfill \\ \hfill {\mathbf{R}}_C^B=& C_H\left({\mathbf{q}}_B^C\right)\hfill \\ \hfill {\mathbf{R}}_C^A=& C_H\left({\mathbf{q}}_A^C\right).\hfill \end{array}\end{array}$$

(8)

According to Equations (6) and (7), we now have $C_H\left({\mathbf{q}}_A^B\otimes {\mathbf{q}}_B^C\right)=C_H\left({\mathbf{q}}_A^B\right)C_H\left({\mathbf{q}}_B^C\right)$, which proves $C_H\left(•\right)$ to be a homomorphism.

Given a quaternion

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathbf{q}}_A^B=& cos\frac{\theta }{2}+{\mathbf{u}}^{A}sin\frac{\theta }{2}\hfill \\ \hfill =& q_0+q_1\mathbf{i}+q_1\mathbf{j}+q_2\mathbf{k},\hfill \end{array}\end{array}$$

(9)

the operator $C_H\left(•\right)$ is defined as a function mapping quaternion ${\mathbf{q}}_A^B$ to a DCM ${\mathbf{R}}_B^A$ as

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill C_H\left({\mathbf{q}}_A^B\right)=& {\mathbf{R}}_B^A\hfill \\ \hfill =& \left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_1{q}_3+q_0{q}_2\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_2{q}_3+q_0{q}_1\right)& q_0^2-q_1^2-q_2^2+q_3^2.\end{array}\right]\hfill \end{array}\end{array}$$

(10)

This is, in fact, the classical Rodrigues Rotation Formula. There is a more thorough discussion about this mapping in [36].

3. IMU Error State Differential Equation

In this section, we deduce the IMU error state differential equation based on Hamilton’s quaternion notation. The Earth’s rotation is ignored as low cost gyros cannot measure it. The static world assumption is employed, which means that gravity has a fixed direction. This is acceptable when a VIO is working in a limited region.

3.1. Notation

The east-north-up geographic coordinate system at initial position is selected as the reference world frame w. As the Earth’s rotation is omitted, w can be regarded as an inertial frame. Quaternion ${\mathbf{q}}_w^b$ is used to represent the rotation from frame w to body frame b. According to Equation (8), we obtain

$$\begin{array}{c}\hfill C_H\left({\mathbf{q}}_w^b\right)={\mathbf{R}}_b^w.\end{array}$$

(11)

The error quaternion is defined as

$$\begin{array}{c}\hfill \delta {\mathbf{q}}_w^b={\mathbf{q}}_w^b\otimes {\left({\widehat{\mathbf{q}}}_w^b\right)}^{-1},\end{array}$$

(12)

where ${\widehat{\mathbf{q}}}_w^b$ is the estimated quaternion of ${\mathbf{q}}_w^b$.

According to Equation (10), applying the $C_H\left(•\right)$ mapping to Equation (12) leads to

$$\begin{array}{c}\hfill {\mathbf{R}}_{w^{\prime }}^w={\mathbf{R}}_b^w{\left({\mathbf{R}}_b^{w^{\prime }}\right)}^{-1},\end{array}$$

(13)

where $w^{\prime }$ is the estimated world frame, and $\delta {\mathbf{q}}_w^b$ corresponds to the rotation between w and $w^{\prime }$.

$\delta {\mathbf{q}}_w^b$ can be expressed in axis-angle formulation as

$$\begin{array}{c}\hfill \delta {\mathbf{q}}_w^b=cos\frac{\left|\delta {\mathit{\theta }}^w\right|}{2}+\frac{\delta {\mathit{\theta }}^w}{\left|\delta {\mathit{\theta }}^w\right|}sin\frac{\left|\delta {\mathit{\theta }}^w\right|}{2},\end{array}$$

(14)

where $\delta {\mathit{\theta }}^w={\left[\begin{array}{ccc}{\delta \theta }_x^w& {\delta \theta }_y^w& {\delta \theta }_z^w\end{array}\right]}^T$ is an axis-angle in frame w that rotates frame $w^{\prime }$ to frame w. As $\left|\delta {\mathit{\theta }}^w\right|$ is a small angle, an approximate expression of Equation (14) is

$$\begin{array}{c}\hfill \delta {\mathbf{q}}_w^b\approx 1+\frac{1}{2}\delta {\mathit{\theta }}^w.\end{array}$$

(15)

Based on Equations (10) and (14), an approximate expression of ${\mathbf{R}}_{w^{\prime }}^w$ is formulated as

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathbf{R}}_{w^{\prime }}^w\approx & \left[\begin{array}{ccc}1& -\delta {\theta }_z^w& \delta {\theta }_y^w\\ \delta {\theta }_z^w& 1& -\delta {\theta }_x^w\\ -\delta {\theta }_y^w& \delta {\theta }_x^w& 1\end{array}\right]\hfill \\ \hfill =& \mathbf{I}+\left[\delta {\mathit{\theta }}^w\times \right],\hfill \end{array}\end{array}$$

(16)

where the operator $\left[•\times \right]$ is used to denote the skew matrix. For a given three-dimensional (3D) vector $\mathbf{v}={\left[\begin{array}{ccc}{v}_x& v_y& v_z\end{array}\right]}^T$, its skew matrix is

$$\begin{array}{c}\hfill \left[\mathbf{v}\times \right]=\left[\begin{array}{ccc}0& -v_z& v_y\\ v_z& 0& -v_x\\ -v_y& v_x& 0\end{array}\right].\end{array}$$

(17)

3.2. IMU Measurement Model

An IMU includes a 3-axis gyroscope and a 3-axis accelerometer, whose axes are aligned with the body frame. The output of the gyroscope is modeled as

$$\begin{array}{c}\hfill {\mathit{\omega }}_m^b={\mathit{\omega }}_{wb}^b+{\mathbf{b}}_g+{\mathbf{n}}_g,\end{array}$$

(18)

where ${\mathit{\omega }}_{wb}^b$ is the true angular velocity, ${\mathbf{b}}_g$ denotes the gyroscope bias under the body frame, and ${\mathbf{n}}_g$ is the Gaussian white noise.

The accelerometer measures the specific force along a body-fixed axis, which includes an opposite gravity and is affected by bias and noise as well:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathbf{f}}_m^b=& {\mathbf{a}}^b-{\mathbf{R}}_w^b{\mathbf{g}}^w+{\mathbf{b}}_a+{\mathbf{n}}_a\hfill \\ \hfill =& {\mathbf{R}}_w^b\left({\mathbf{a}}^w-{\mathbf{g}}^w\right)+{\mathbf{b}}_a+{\mathbf{n}}_a,\hfill \end{array}\end{array}$$

(19)

where ${\mathbf{a}}^b$ is the true acceleration, and ${\mathbf{g}}^w={\left[\begin{array}{ccc}0& 0& -g\end{array}\right]}^T$ denotes the gravity under the world frame. ${\mathbf{b}}_a$ and ${\mathbf{n}}_a$ denote the bias and the Gaussian white noise under the body frame, respectively.

The biases ${\mathbf{b}}_g$ and ${\mathbf{b}}_a$ are modeled as random walk processes

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\dot{\mathbf{b}}}_g=& {\mathbf{n}}_{wg}\hfill \\ \hfill {\dot{\mathbf{b}}}_a=& {\mathbf{n}}_{wa},\hfill \end{array}\end{array}$$

(20)

where ${\mathbf{n}}_{wg}$ and ${\mathbf{n}}_{wa}$ are Gaussian white noises.

3.3. IMU Error State Definition

The IMU state includes the quaternion ${\mathbf{q}}_w^b$, velocity ${\mathbf{v}}_b^w$ and position ${\mathbf{p}}_b^w$ of the body frame origin in the world frame, and IMU biases ${\mathbf{b}}_g$ and ${\mathbf{b}}_a$. The IMU state can be defined as

$$\begin{array}{c}\hfill {\mathbf{x}}_{IMU}={\left[\begin{array}{ccccc}{{\mathbf{q}}_w^b}^T& {{\mathbf{v}}_b^w}^T& {{\mathbf{p}}_b^w}^T& {\mathbf{b}}_g^T& {\mathbf{b}}_a^T\end{array}\right]}^T.\end{array}$$

(21)

The filter is designed based on the error state because it is convenient to process by extended Kalman filter (EKF). Three dimensional angular error $\delta {\mathit{\theta }}^w$ rather than four dimensional quaternion error $\delta {\mathbf{q}}_w^b$ is utilized since it is accordance with the degree of freedom (DOF) of rotation, and thus a minimum parameterization.

Other error state components are simply defined as the Euclidean distances between true states and the estimated states, which lead to

$$\begin{array}{cc}\hfill \delta {\mathbf{v}}_b^w=& {\mathbf{v}}_b^w-{\widehat{\mathbf{v}}}_b^w,\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill \delta {\mathbf{p}}_b^w=& {\mathbf{p}}_b^w-{\widehat{\mathbf{p}}}_b^w,\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill \delta {\mathbf{b}}_g=& {\mathbf{b}}_g-{\widehat{\mathbf{b}}}_g,\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill \delta {\mathbf{b}}_a=& {\mathbf{b}}_a-{\widehat{\mathbf{b}}}_a.\hfill \end{array}$$

(25)

The overall IMU error state can now be concluded as

$$\begin{array}{c}\hfill \delta {\mathbf{x}}_{IMU}={\left[\begin{array}{ccccc}\delta {{\mathit{\theta }}^w}^T& \delta {{\mathbf{v}}_b^w}^T& \delta {{\mathbf{p}}_b^w}^T& {\mathbf{b}}_g^T& {\mathbf{b}}_a^T\end{array}\right]}^T.\end{array}$$

(26)

3.4. Differential Equation

The matrix form of the differential equation of the overall IMU error state is as follows.

$$\begin{array}{c}\hfill \delta {\dot{\mathbf{x}}}_{IMU}=\mathbf{F}\delta {\mathbf{x}}_{IMU}+\mathbf{G}{\mathbf{n}}_{IMU},\end{array}$$

(27)

where ${\mathbf{n}}_{IMU}$ denotes the IMU noise, given by

$$\begin{array}{cc}\hfill \delta {\mathbf{x}}_{IMU}=& {\left[\begin{array}{ccccc}\delta {\theta }^w& \delta {{\mathbf{v}}_b^w}^T& \delta {{\mathbf{p}}_b^w}^T& \delta {\mathbf{b}}_g^T& \delta {\mathbf{b}}_a^T\end{array}\right]}^T,\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill {\mathbf{n}}_{IMU}=& {\left[\begin{array}{cccc}{\mathbf{n}}_g^T& {\mathbf{n}}_a^T& {\mathbf{n}}_{wg}^T& {\mathbf{n}}_{wa}^T\end{array}\right]}^T,\hfill \end{array}$$

(29)

and the matrices $\mathbf{F}$ and $\mathbf{G}$ are as follows:

$$\begin{array}{cc}\hfill \mathbf{F}=& \left[\begin{array}{ccccc}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& -{\widehat{\mathbf{R}}}_b^w& {\mathbf{0}}_{3\times 3}\\ -\left[\left({\widehat{\mathbf{R}}}_b^w\widehat{\mathbf{a}}\right)\times \right]& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& -{\widehat{\mathbf{R}}}_b^w\\ {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right],\hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill \mathbf{G}=& \left[\begin{array}{cccc}-{\widehat{\mathbf{R}}}_b^w& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& -{\widehat{\mathbf{R}}}_b^w& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\right].\hfill \end{array}$$

(31)

4. Fully Linear State Transition Equation Formulation

A state transition equation is needed for the extended Kalman filter (EKF) to propagate the state and covariance. One commonly used method is to make a first-order approximation based on a continuous differential equation [37]. Li and Mourikis [19] proposed a closed-form error state transition equation that effectuated a system model with no information loss. However, there are still some tricky integration terms left behind. In this section, we first present the closed-form IMU error state transition equation based on the results of Section 3. Then, we solve the integration terms by two-sample fitting of the rotation matrix, resulting in a closed-form formulation that is fully linear.

4.1. Original Closed-Form Equation

Following the methodology of Li and Mourikis [19], the closed-form transition equation was deduced and presented in what follows. Noting that k and $k+1$ are consecutive discrete sampling instants of IMU, and $\Delta t$ is the sampling period,

$$\begin{array}{cc}\hfill \delta {\mathit{\theta }}_{k+1|k}^w=& {\mathsf{\Phi }}_{\theta \theta }\left(k+1,k\right)\delta {\mathit{\theta }}_{k|k}^w+{\mathsf{\Phi }}_{\theta b_g}\left(k+1,k\right)\delta {\mathbf{b}}_{gk|k}+{\mathbf{n}}_{\theta k},\hfill \end{array}$$

(32)

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \delta {\mathbf{v}}_{k+1|k}^w=& {\mathsf{\Phi }}_{v\theta }\left(k+1,k\right)\delta {\mathit{\theta }}_{k|k}^w+{\mathsf{\Phi }}_{vv}\left(k+1,k\right)\delta {\mathbf{v}}_{k|k}^w\hfill \\ & +{\mathsf{\Phi }}_{v{b}_g}\left(k+1,k\right)\delta {\mathbf{b}}_{gk|k}+{\mathsf{\Phi }}_{v{b}_a}\left(k+1,k\right)\delta {\mathbf{b}}_{ak|k}+{\mathbf{n}}_{vk},\hfill \end{array}\end{array}$$

(33)

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \delta {\mathbf{p}}_{k+1|k}^w=& {\mathsf{\Phi }}_{p\theta }\left(k+1,k\right)\delta {\mathit{\theta }}_{k|k}^w+{\mathsf{\Phi }}_{pv}\left(k+1,k\right)\delta {\mathbf{v}}_{k|k}^w+{\mathsf{\Phi }}_{pp}\left(k+1,k\right)\delta {\mathbf{p}}_{k|k}^w\hfill \\ & +{\mathsf{\Phi }}_{p{b}_g}\left(k+1,k\right)\delta {\mathbf{b}}_{gk|k}+{\mathsf{\Phi }}_{p{b}_a}\left(k+1,k\right)\delta {\mathbf{b}}_{ak|k}+{\mathbf{n}}_{pk},\hfill \end{array}\end{array}$$

(34)

$$\begin{array}{cc}\hfill \delta {\mathbf{b}}_{gk+1|k}=& {\mathsf{\Phi }}_{b_g{b}_g}\left(k+1,k\right)\delta {\mathbf{b}}_{gk|k}+{\mathbf{n}}_{b_{g}k},\hfill \end{array}$$

(35)

$$\begin{array}{cc}\hfill \delta {\mathbf{b}}_{ak+1|k}=& {\mathsf{\Phi }}_{b_a{b}_a}\left(k+1,k\right)\delta {\mathbf{b}}_{ak|k}+{\mathbf{n}}_{b_{a}k},\hfill \end{array}$$

(36)

where ${\mathsf{\Phi }}_{x_1{x}_2}$ is used to represent the transition matrix of the error state of $x_1$ with respect to the error state of $x_2$, and ${\mathbf{n}}_{*}$ terms represent noise. All of the ${\mathsf{\Phi }}_{*}$ and ${\mathbf{n}}_{*}$ terms are listed as follows:

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{\theta \theta }\left(k+1,k\right)=& {\mathbf{I}}_{3\times 3},\hfill \end{array}$$

(37)

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{\theta b_g}\left(k+1,k\right)=& -{\widehat{\mathbf{R}}}_{b_k}^w{\int }_{t_k}^{t_{k+1}}{\widehat{\mathbf{R}}}_{b_{\tau }}^{b_k}d\tau ,\hfill \end{array}$$

(38)

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{v\theta }\left(k+1,k\right)=& -\left[\left({\widehat{\mathbf{v}}}_{k+1|k}^w-{\widehat{\mathbf{v}}}_{k|k}^w-{\mathbf{g}}^w\Delta t\right)\times \right],\hfill \end{array}$$

(39)

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{vv}\left(k+1,k\right)=& {\mathbf{I}}_{3\times 3},\hfill \end{array}$$

(40)

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{v{b}_g}\left(k+1,k\right)=& {\int }_{t_k}^{t_{k+1}}\left\{\left[\left({\dot{\widehat{\mathbf{v}}}}_{\tau }^w-{\mathbf{g}}^w\right)\times \right]{\widehat{\mathbf{R}}}_{b_k}^w\left({\int }_{t_k}^{\tau }{\widehat{\mathbf{R}}}_{b_m}^{b_k}dm\right)\right\}d\tau ,\hfill \end{array}$$

(41)

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{v{b}_a}\left(k+1,k\right)=& -{\widehat{\mathbf{R}}}_{b_k}^w{\int }_{t_k}^{t_{k+1}}{\widehat{\mathbf{R}}}_{b_{\tau }}^{b_k}d\tau ,\hfill \end{array}$$

(42)

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{p\theta }\left(k+1,k\right)=& -\left[\left({\widehat{\mathbf{p}}}_{k+1|k}^w-{\widehat{\mathbf{p}}}_{k|k}^w-{\widehat{\mathbf{v}}}_{k|k}^w\Delta t-\frac{1}{2}{\mathbf{g}}^w\Delta t^2\right)\times \right],\hfill \end{array}$$

(43)

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{pv}\left(k+1,k\right)=& {\mathbf{I}}_{3\times 3}\Delta t,\hfill \end{array}$$

(44)

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{pp}\left(k+1,k\right)=& {\mathbf{I}}_{3\times 3},\hfill \end{array}$$

(45)

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{p{b}_g}\left(k+1,k\right)=& {\int }_{t_k}^{t_{k+1}}\left\{{\int }_{t_k}^{\tau }\left(\left[\left({\dot{\widehat{\mathbf{v}}}}_s^w-{\mathbf{g}}^w\right)\times \right]{\widehat{\mathbf{R}}}_{b_k}^w\left[{\int }_{t_k}^s{\widehat{\mathbf{R}}}_{b_m}^{b_k}dm\right]\right)ds\right\}d\tau ,\hfill \end{array}$$

(46)

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{p{b}_a}\left(k+1,k\right)=& -{\widehat{\mathbf{R}}}_{b_k}^w{\int }_{t_k}^{t_{k+1}}\left({\int }_{t_k}^{\tau }{\widehat{\mathbf{R}}}_{b_s}^{b_k}ds\right)d\tau ,\hfill \end{array}$$

(47)

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{b_g{b}_g}\left(k+1,k\right)=& {\mathbf{I}}_{3\times 3},\hfill \end{array}$$

(48)

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{b_a{b}_a}\left(k+1,k\right)=& {\mathbf{I}}_{3\times 3},\hfill \end{array}$$

(49)

$$\begin{array}{cc}\hfill {\mathbf{n}}_{\theta k}=& {\widehat{\mathbf{R}}}_{b_k}^w{\int }_{t_k}^{t_{k+1}}{\widehat{\mathbf{R}}}_{b_{\tau }}^{b_k}\left({\int }_{t_k}^{\tau }{\mathbf{n}}_{wgs}ds+{\mathbf{n}}_{g\tau }\right)d\tau ,\hfill \end{array}$$

(50)

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathbf{n}}_{vk}=& {\int }_{t_k}^{t_{k+1}}{\widehat{\mathbf{R}}}_{b_{\tau }}^w\left(-{\int }_{t_k}^{\tau }{\mathbf{n}}_{was}ds-{\mathbf{n}}_{a\tau }\right)d\tau \hfill \\ & -{\int }_{t_k}^{t_{k+1}}\left\{\left[\left({\dot{\widehat{\mathbf{v}}}}_{\tau }^w-{\mathbf{g}}^w\right)\times \right]{\widehat{\mathbf{R}}}_{b_k}^w\left[{\int }_{t_k}^{\tau }{\widehat{\mathbf{R}}}_{b_m}^{b_k}\left({\int }_{t_k}^m{\mathbf{n}}_{wgs}ds+{\mathbf{n}}_{gm}\right)dm\right]\right\}d\tau ,\hfill \end{array}\end{array}$$

(51)

$$\begin{array}{cc}\hfill {\mathbf{n}}_{pk}=& {\int }_{t_k}^{t_{k+1}}{\mathbf{n}}_{v\tau }d\tau ,\hfill \end{array}$$

(52)

$$\begin{array}{cc}\hfill {\mathbf{n}}_{b_{g}k}=& {\int }_{t_k}^{t_{k+1}}{\mathbf{n}}_{wg\tau }d\tau ,\hfill \end{array}$$

(53)

$$\begin{array}{cc}\hfill {\mathbf{n}}_{b_{a}k}=& {\int }_{t_k}^{t_{k+1}}{\mathbf{n}}_{wa\tau }d\tau .\hfill \end{array}$$

(54)

4.2. Fully Linear Closed-Form Formulation

Notice that in Equations (38), (41), (42), (46), and (47), although they are closed-form expressions, there are still some tricky integration terms that are not straightforward for implementation. One can solve these terms with numerical integration, but here we present a fully linear analytical expression that is readily implemented. The key is to solve ${\int }_{t_k}^{t_{k+1}}{\widehat{\mathbf{R}}}_{b_{\tau }}^{b_k}d\tau$. We first apply a two-sample fitting method to approximate the axis-angle representing ${\widehat{\mathbf{R}}}_{b_{\tau }}^{b_k}$, then Rodrigues’ rotation formula is applied to express the DCM as a linear function of $\tau$, thus making the integration easy to solve.

4.2.1. Two-Sample Fitting of Axis-Angle

Any DCM can be regarded as a single rotation about a fixed axis, and thus can be represented by an axis-angle. Let the axis-angle of ${\widehat{\mathbf{R}}}_{b_{k+1}}^{b_k}$ be $\mathit{\varphi }=\alpha {\mathbf{u}}^{b_k}$, where $\alpha$ is the angle of rotation and ${\mathbf{u}}^{b_k}$ is the rotation axis. Let $\tau$ be a time instant between $t_k$ and $t_{k+1}$ and $\epsilon =\tau -t_k$, then a linear model can be used to represent the axis-angle of ${\widehat{\mathbf{R}}}_{b_{\tau }}^{b_k}$:

$$\begin{array}{c}\hfill \begin{array}{c}\hfill {\mathit{\varphi }}_{\tau }=\mathit{\varphi }\left(\epsilon \right)=\frac{\epsilon }{\Delta t}\alpha {\mathbf{u}}^{b_k}=\frac{\epsilon }{\Delta t}\mathit{\varphi }.\end{array}\end{array}$$

(55)

Angular velocity measurements of $t_k$ and $t_{k+1}$ are available when calculating the transition matrix $\mathsf{\Phi }\left(k+1,k\right)$, so a two-sample fitting method can be used to approximate the axis-angle $\mathit{\varphi }$. We start from the differential equation of $\mathit{\varphi }\left(\epsilon \right)$ [36]:

$$\begin{array}{c}\hfill \dot{\mathit{\varphi }}\left(\epsilon \right)=\overline{\mathit{\omega }}+\frac{1}{2}\mathit{\varphi }\left(\epsilon \right)\times \overline{\mathit{\omega }}+\frac{1}{12}\mathit{\varphi }\left(\epsilon \right)\times \left(\mathit{\varphi }\left(\epsilon \right)\times \overline{\mathit{\omega }}\right),\end{array}$$

(56)

where $\overline{\mathit{\omega }}$ is the average angular velocity between $t_k$ and $t_{k+1}$. As two gyro measurements are available, we use a straight line model to fit $\overline{\mathit{\omega }}$ as

$$\begin{array}{c}\hfill \overline{\mathit{\omega }}\left(t_k+\epsilon \right)=\mathbf{a}+2\mathbf{b}\epsilon ,0\le \epsilon \le \Delta t.\end{array}$$

(57)

Considering ${\overline{\mathit{\omega }}}^b\left(t_k\right)={\widehat{\mathit{\omega }}}_{wb}^b\left(t_k\right)$ and ${\overline{\mathit{\omega }}}^b\left(t_k+\Delta t\right)={\widehat{\mathit{\omega }}}_{wb}^b\left(t_{k+1}\right)$, leads to

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \mathbf{a}=& {\widehat{\mathit{\omega }}}_{wb}^b\left(t_k\right)\hfill \\ \hfill \mathbf{b}=& \left({\widehat{\mathit{\omega }}}_{wb}^b\left(t_{k+1}\right)-{\widehat{\mathit{\omega }}}_{wb}^b\left(t_k\right)\right)/\phantom{\left({\widehat{\omega }}_{wb}^b\left(k+1\right)-{\widehat{\omega }}_{wb}^b\left(k\right)\right)\left(2\Delta t\right)}\left(2\Delta t\right).\hfill \end{array}\end{array}$$

(58)

According to Equation (55), $\mathit{\varphi }$ is equal to ${\mathit{\varphi }}_{t_{k+1}}$, then using Taylor expansion to expand $\mathit{\varphi }$ at linearized point $t_k$ yields

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \mathit{\varphi }=& {\mathit{\varphi }}_{t_{k+1}}\hfill \\ \hfill =& {\mathit{\varphi }}_{t_k}+\Delta t{\dot{\mathit{\varphi }}}_{t_k}+\frac{\Delta t^2}{2!}{\ddot{\mathit{\varphi }}}_{t_k}+\cdots \hfill \\ \hfill =& \mathit{\varphi }\left(0\right)+\Delta t\dot{\mathit{\varphi }}\left(0\right)+\frac{\Delta t^2}{2!}\ddot{\mathit{\varphi }}\left(0\right)+\cdots .\hfill \end{array}\end{array}$$

(59)

Now define a new function of $\epsilon$ as

$$\begin{array}{c}\hfill \Delta \mathit{\theta }\left(\epsilon \right)={\int }_0^{\epsilon }{\widehat{\mathit{\omega }}}_{wb}^b\left(t_k+\epsilon \right)d\epsilon .\end{array}$$

(60)

It can be pointed out that $\mathit{\varphi }\left(\epsilon \right)\approx \Delta \mathit{\theta }\left(\epsilon \right)$. Derivatives of $\Delta \mathit{\theta }\left(0\right)$ are defined as

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \Delta \mathit{\theta }\left(0\right)=& \mathbf{0},\hfill \\ \hfill \Delta \dot{\mathit{\theta }}\left(0\right)=& {\widehat{\mathit{\omega }}}_{wb}^b\left(t_k\right)=\mathbf{a},\hfill \\ \hfill \Delta \ddot{\mathit{\theta }}\left(0\right)=& {\dot{\widehat{\mathit{\omega }}}}_{wb}^b\left(t_k\right)=2\mathbf{b},\hfill \\ \hfill \Delta {\mathit{\theta }}^{\left(i\right)}\left(0\right)=& \widehat{\mathit{\omega }}{{}_{wb}^b}^{\left(i-1\right)}\left(t_k\right)=\mathbf{0},i=3,4,5,\cdots .\hfill \end{array}\end{array}$$

(61)

The third term in Equation (56) is a high-order small quantity that can be omitted. By substituting $\mathit{\varphi }\left(\epsilon \right)$ as $\Delta \mathit{\theta }\left(\epsilon \right)$, Equation (56) turns into

$$\begin{array}{c}\hfill \dot{\mathit{\varphi }}\left(\epsilon \right)=\overline{\mathit{\omega }}\left(t_k+\epsilon \right)+\frac{1}{2}\Delta \mathit{\theta }\left(\epsilon \right)\times \overline{\mathit{\omega }}\left(t_k+\epsilon \right).\end{array}$$

(62)

Now the high-order derivatives of $\mathit{\varphi }\left(\epsilon \right)$ can be obtained:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \ddot{\mathit{\varphi }}\left(\epsilon \right)=& \dot{\overline{\mathit{\omega }}}\left(t_k+\epsilon \right)+\frac{1}{2}\Delta \dot{\mathit{\theta }}\left(\epsilon \right)\times \overline{\mathit{\omega }}\left(t_k+\epsilon \right)+\frac{1}{2}\Delta \mathit{\theta }\left(\epsilon \right)\times \dot{\overline{\mathit{\omega }}}\left(t_k+\epsilon \right),\hfill \\ \hfill {\mathit{\varphi }}^{\left(3\right)}\left(\epsilon \right)=& \frac{1}{2}\Delta \ddot{\mathit{\theta }}\left(\epsilon \right)\times \overline{\mathit{\omega }}\left(t_k+\epsilon \right)+\Delta \dot{\mathit{\theta }}\left(\epsilon \right)\times \dot{\overline{\mathit{\omega }}}\left(t_k+\epsilon \right),\hfill \\ \hfill {\mathit{\varphi }}^{\left(4\right)}\left(\epsilon \right)=& \frac{3}{2}\Delta \ddot{\mathit{\theta }}\left(\epsilon \right)\times \dot{\overline{\mathit{\omega }}}\left(t_k+\epsilon \right),\hfill \\ \hfill {\mathit{\varphi }}^{\left(i\right)}\left(\epsilon \right)=& \mathbf{0},i=5,6,7,\cdots .\hfill \end{array}\end{array}$$

(63)

Let $\epsilon =0$, and considering Equation (61), we have

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \mathit{\varphi }\left(0\right)=& \mathbf{0},\hfill \\ \hfill \dot{\mathit{\varphi }}\left(0\right)=& \mathbf{a},\hfill \\ \hfill \ddot{\mathit{\varphi }}\left(0\right)=& 2\mathbf{b},\hfill \\ \hfill {\mathit{\varphi }}^{\left(3\right)}\left(0\right)=& \mathbf{a}\times \mathbf{b},\hfill \\ \hfill {\mathit{\varphi }}^{\left(i\right)}\left(0\right)=& \mathbf{0},i=4,5,6,\cdots .\hfill \end{array}\end{array}$$

(64)

Substituting the equations above into Equation (59) yields

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \mathit{\varphi }=& \mathbf{a}\Delta t+\mathbf{b}\Delta t^2+\frac{1}{6}\left(\mathbf{a}\times \mathbf{b}\right)\Delta t^3\hfill \\ \hfill =& \frac{1}{2}\left({\widehat{\omega }}_{wb}^b\left(k\right)+{\widehat{\mathit{\omega }}}_{wb}^b\left(k+1\right)\right)\Delta t+\frac{1}{12}\left({\widehat{\mathit{\omega }}}_{wb}^b\left(k\right)\times {\widehat{\mathit{\omega }}}_{wb}^b\left(k+1\right)\right)\Delta t^2.\hfill \end{array}\end{array}$$

(65)

This is how the axis-angle between two consecutive sampling time instants $t_k$ and $t_{k+1}$ can be computed.

According to Rodrigues’ rotation formula,

$$\begin{array}{c}\hfill {\widehat{\mathbf{R}}}_{b_{k+1}}^{b_k}=\mathbf{I}+sin\alpha \left[{\mathbf{u}}^{b_k}\times \right]+\left(1-cos\alpha \right){\left[{\mathbf{u}}^{b_k}\times \right]}^2.\end{array}$$

(66)

As $\alpha$ is a small angular, since $\Delta t$ is small, Equation (66) has an approximation

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\widehat{\mathbf{R}}}_{b_{k+1}}^{b_k}\approx & \mathbf{I}+\alpha \left[{\mathbf{u}}^{b_k}\times \right]\hfill \\ \hfill =& \mathbf{I}+\left[\mathit{\varphi }\times \right].\hfill \end{array}\end{array}$$

(67)

Now, substituting Equation (55) into Equation (67) leads to

$$\begin{array}{c}\hfill {\widehat{\mathbf{R}}}_{b_{\tau }}^{b_k}\approx \mathbf{I}+\frac{\tau -t_k}{\Delta t}\left[\mathit{\varphi }\times \right].\end{array}$$

(68)

Finally, the general procedure to solve the integration term ${\int }_{t_k}^{t_{k+1}}{\widehat{\mathbf{R}}}_{b_{\tau }}^{b_k}d\tau$ can be summarized as follows:

• Compute the axis-angle between $t_k$ and $t_{k+1}$ according to Equation (65).
• Express ${\widehat{\mathbf{R}}}_{b_{\tau }}^{b_k}$ as Equation (68).
• Easily solve the ${\int }_{t_k}^{t_{k+1}}{\widehat{\mathbf{R}}}_{b_{\tau }}^{b_k}d\tau$ term, as it becomes an integration about a linear analytic expression.

4.2.2. Solve Integration Terms in ${\mathsf{\Phi }}_{*}$

The fully linear closed-form transition matrix of Equations (38), (41), (42), (46), and (47) can now be obtained by simply solving the integration terms. The results are listed below.

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathsf{\Phi }}_{\theta b_g}\left(k+1,k\right)=& -{\widehat{\mathbf{R}}}_{b_k}^w\left(\Delta t\mathbf{I}+\frac{1}{2}\Delta t\left[\mathit{\varphi }\times \right]\right),\hfill \\ \hfill {\mathsf{\Phi }}_{v{b}_g}\left(k+1,k\right)=& \left[\left(-{\widehat{\mathbf{p}}}_{k+1|k}^w+{\widehat{\mathbf{p}}}_{k|k}^w+{\widehat{\mathbf{v}}}_{k+1|k}^w\Delta t-\frac{1}{2}{\mathbf{g}}^w\Delta t^2\right)\times \right]{\widehat{\mathbf{R}}}_{b_k}^w\hfill \\ & +\left[\left(-\frac{1}{2}{\widehat{\mathbf{p}}}_{k+1|k}^w+\frac{1}{2}{\widehat{\mathbf{p}}}_{k|k}^w+\frac{1}{2}{\widehat{\mathbf{v}}}_{k+1|k}^w\Delta t-\frac{1}{6}{\mathbf{g}}^w\Delta t^2\right)\times \right]{\widehat{\mathbf{R}}}_{b_k}^w\left[\mathit{\varphi }\times \right],\hfill \\ \hfill {\mathsf{\Phi }}_{v{b}_a}\left(k+1,k\right)=& -{\widehat{\mathbf{R}}}_{b_k}^w\left(\Delta t\mathbf{I}+\frac{1}{2}\Delta t\left[\mathit{\varphi }\times \right]\right),\hfill \\ \hfill {\mathsf{\Phi }}_{p{b}_g}\left(k+1,k\right)=& \left[\left(-\frac{1}{6}{\mathbf{g}}^w\Delta t^3\right)\times \right]{\widehat{\mathbf{R}}}_{b_k}^w\hfill \\ & +\left[\left(\frac{1}{4}{\widehat{\mathbf{p}}}_{k+1|k}^w\Delta t-\frac{1}{4}{\widehat{\mathbf{p}}}_{k|k}^w\Delta t-\frac{1}{24}{\mathbf{g}}^w\Delta t^3\right)\times \right]{\widehat{\mathbf{R}}}_{b_k}^w\left[\mathit{\varphi }\times \right],\hfill \\ \hfill {\mathsf{\Phi }}_{p{b}_a}\left(k+1,k\right)=& -\frac{1}{6}{\widehat{\mathbf{R}}}_{b_k}^w\Delta t^2\left(3\mathbf{I}+\left[\mathit{\varphi }\times \right]\right).\hfill \end{array}\end{array}$$

(69)

Notice that all of the variables needed are available at the time of calculating the $\mathsf{\Phi }$ terms above. This model is unbiased up to the information loss of the two-sample fitting of DCM, which is small due to the utilization of all related measurement data.

4.2.3. Process Noise Terms

The property of noise terms in Equations (50)–(54) should be acquired to compute the process noise covariance matrix in a Kalman filter. The process noise covariance at $t_k$ can be computed as [37]:

$$\begin{array}{c}\hfill \mathbf{Q}\left(t_k\right)={\int }_{t_k}^{t_{k+1}}\mathsf{\Phi }\left(t_{k+1},\tau \right)\mathbf{G}\left(\tau \right)\mathbf{q}{\mathbf{G}}^T\left(\tau \right){\mathsf{\Phi }}^T\left(t_{k+1},\tau \right)d\tau .\end{array}$$

(70)

We temporarily abuse symbol $\mathbf{q}$ here to represent the noise intensity matrix. $\mathsf{\Phi }$ is the overal; IMU error state transition matrix. As $\Delta t$ is a small quantity, an approximate expression of Equation (70) is formulated as

$$\begin{array}{c}\hfill \mathbf{Q}\left(t_k\right)\approx \mathsf{\Phi }\left(t_{k+1},t_k\right)\mathbf{G}\left(t_k\right)\mathbf{q}{\mathbf{G}}^T\left(t_k\right){\mathsf{\Phi }}^T\left(t_{k+1},t_k\right)\Delta t.\end{array}$$

(71)

The discrete form, which will be preferable for a discrete filter implementation, is

$$\begin{array}{c}\hfill \mathbf{Q}\left(k\right)\approx \mathsf{\Phi }\left(k+1,k\right)\mathbf{G}\left(k\right)\mathbf{q}{\mathbf{G}}^T\left(k\right){\mathsf{\Phi }}^T\left(k+1,k\right)\Delta t.\end{array}$$

(72)

4.3. Summarization

According to the derivation above, the proposed fully linear closed-form IMU error state transition equation is as follows:

$$\begin{array}{c}\hfill \delta {\mathbf{x}}_{IMU}\left(k+1\right)=\mathsf{\Phi }\left(k+1,k\right)\delta {\mathbf{x}}_{IMU}\left(k\right)+{\mathbf{n}}_{IMU}\left(k\right),\end{array}$$

(73)

where

$$\begin{array}{cc}\hfill \mathsf{\Phi }\left(k+1,k\right)=& \left[\begin{array}{ccccc}{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathsf{\Phi }}_{\theta b_g}& {\mathbf{0}}_{3\times 3}\\ {\mathsf{\Phi }}_{v\theta }& {\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathsf{\Phi }}_{v{b}_g}& {\mathsf{\Phi }}_{v{b}_a}\\ {\mathsf{\Phi }}_{p\theta }& \Delta t{\mathbf{I}}_{3\times 3}& {\mathbf{I}}_{3\times 3}& {\mathsf{\Phi }}_{p{b}_g}& {\mathsf{\Phi }}_{p{b}_a}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\right],\hfill \end{array}$$

(74)

$$\begin{array}{cc}\hfill {\mathbf{n}}_{IMU}\left(k\right)=& {\left[\begin{array}{ccccc}{\mathbf{n}}_{\theta k}^T& {\mathbf{n}}_{vk}^T& {\mathbf{n}}_{pk}^T& {\mathbf{n}}_{b_{g}k}^T& {\mathbf{n}}_{b_{a}k}^T\end{array}\right]}^T,\hfill \end{array}$$

(75)

and the covariance matrix of ${\mathbf{n}}_{IMU}\left(k\right)$ is $E\left[{\mathbf{n}}_{IMU}\left(k\right){\mathbf{n}}_{IMU}^T\left(k\right)\right]=\mathbf{Q}\left(k\right)$.

The integration terms are solved using a fitting rule of DCMs by utilizing all of the related measurements, so we claim that the obtained formulation is an unbiased model up to the numerical integration resolution.

5. ORB Descriptor-Assisted Optical Flow Front-End

In this section, we propose a sparse visual front-end using descriptor-assisted optical flow feature tracking.

Different kinds of feature descriptors are used in several VIOs to accomplish feature extraction and matching [1,6,30]. In contrast, other solutions choose optical flow feature tracking as their front-end solution since it is not that time-consuming compared to the descriptor-based methods [2,23,24]. However, there are more wrong matches in optical flow tracking than in descriptor-based methods, and these wrong matches exist even after eliminating algorithms such as random sample consensus (RANSAC). Filter-based VIOs are very sensitive to feature outliers since they don’t eliminate outliers in their iterations as the optimization-based ones do. Wrong matches left behind will participate in measurement updates, which may result in deteriorating estimates or even failure. As a conclusion, a robust front-end is needed to achieve stable performance for filter-based VIOs, while a real-time solution also calls for fast data correlation.

Yang et al. [29], refined ORB-SLAM [38] by using a sparse optical flow algorithm. The key idea was to correct the image coordinates of ORB features by optical flow tracking results to achieve sub-pixel precision. The proposed method here is a bit different since we use optical flow to first conduct a fast tracking, then compute descriptor distance between matched feature pair members and justify whether they are a good match-up.

There exist plenty of feature descriptor algorithms. We chose the ORB descriptor in our proposed method for two reasons:

• The ORB descriptor is a binary string, so the distance between two descriptors can be expressed as a Hamming distance, which can be computed efficiently.
• The rotation between consecutive images in a real-time application is usually very gentle, so invariance to rotation is not very important for a descriptor.

Descriptor Distance Analysis for General Corner Features

The basic visual front-end is based on Shi-Tomasi corner detection [39] and optical flow tracking [40]. It is important to figure out whether the ORB descriptor is meaningful for general Shi-Tomasi corner features. An experiment was done and proved that it is indeed meaningful statistically. We calculated the feature angle for a Shi-Tomasi feature and then used it to compute the ORB descriptor [32]. Several tests were conducted in the experiment. For each test, feature pairs from every two adjacent images of a continuous image stream were stored separately in two sequences. These tests basically analyzed the statistical properties of ORB descriptor distances of feature pairs, including

• Coarsely matched feature pairs based on Shi-Tomasi corner detection and optical flow tracking.
• Relatively strictly matched feature pairs based on ORB descriptor matching and RANSAC.
• Randomly constructed feature pairs.
• Unmatched feature pairs generated by inverse order of one of the strictly matched feature sequences.

One feature sequence from strictly matched pairs was inverted to generate strictly unmatched feature pairs. The experimental result is shown in Figure 2.

We strongly suspect the very long tail in Figure 2a may be due to wrong matches because no further outlier rejection method was applied after optical flow tracking in this test. In Figure 2b, except for the massive Guassian-like distribution, a little bump centered at about 17 appeared, which is framed by a red rectangular border. This is because the random pairs were constructed in two adjacent images and thus, two matched features have a considerable probability of being coincidentally formed into a pair. These two experiments prove that ORB descriptors and descriptor distances are meaningful for general Shi-Tomasi corners, from a statistical standpoint.

In order to clearly analyze the statistical properties of matched and unmatched pairs, two further tests were conducted. First, a descriptor-based matching and RANSAC mechanism were applied to obtain relatively strictly matched feature pairs. Then, the order of one of the feature sequences was reversed, which is a simple yet effective way to make two sequences unmatched. Descriptor distances before and after order reversion were computed, and statistical results are shown in Figure 2c,d. It can be seen from the figures that the long tail and little bump disappear because of the relatively strict pairing rule. They are plotted together in Figure 3 to make a clear comparison.

The experimental results show that the descriptor distances of unmatched and matched feature pairs possess significantly different statistical properties. As shown in Figure 3, descriptor distances of unmatched features approximately follow the Gaussian distribution with a mean, or we can say peak, at about $124.7$ and with a standard deviation of $21.8$. For matched pairs, the distribution shows a sharper peak at about $18.5$. There is still a tail in the matched distribution, but it is much smaller than the one in Figure 2a. The difference between matched and unmatched pairs is significant enough to design a strategy to filter out wrong matches.

We use a heuristic to complete the mission:

• For feature pairs with distances lower than the smaller peak value, classify them as inliers.
• For feature pairs with distances higher than the bigger peak value, classify them as outliers.
• For feature pairs whose distances are between two peaks, calculate and compare the Mahalanobis distances to both peak to decide their classification.

6. EKF-Based VIO Implementation Details and Improvements

In this section, implementation details and improvements of the proposed EKF-based VIO are presented, including filtering scheme, automatic initialization procedure, and refined feature triangulation mechanism. An overall flow chart of the implemented VIO algorithm is shown in Figure 4. Red sections highlight novelties proposed in this paper.

6.1. Filter State and Measurement Model

A VIO following the scheme of Mourikis and Roumeliotis [1] is implemented. The system state includes a sliding window of N historical IMU poses and camera-IMU extrinsic as proposed by Li and Mourikis [22]. The overall system state is formulated as

$$\begin{array}{c}\hfill {\mathbf{x}}_{all}={\left[\begin{array}{cccccccc}{\mathbf{x}}_{IMU}& {\mathbf{q}}_b^c& {\mathbf{p}}_c^b& {\mathbf{q}}_w^{b_1}& {\mathbf{p}}_{b_1}^w& \cdots & {\mathbf{q}}_w^{b_N}& {\mathbf{p}}_{b_N}^w\end{array}\right]}^T.\end{array}$$

(76)

Therefore, the overall error state of the filter is

$$\begin{array}{c}\hfill \delta {\mathbf{x}}_{all}={\left[\begin{array}{cccccccc}\delta {\mathbf{x}}_{IMU}& \delta {\mathit{\theta }}^b& \delta {\mathbf{p}}_c^b& \delta {\mathit{\theta }}_1^w& \delta {\mathbf{p}}_{b_1}^w& \cdots & \delta {\mathit{\theta }}_N^w& \delta {\mathbf{p}}_{b_N}^w\end{array}\right]}^T.\end{array}$$

(77)

The measurement residual is a linearized residual about historical IMU pose errors and camera-IMU extrinsic errors. The original reprojection error is manipulated firstly by left nullspace multiplication to marginalize out the feature position, and secondly by applying QR decomposition to decrease the residual dimensions without information loss [1]. Furthermore, only residuals passing through the Mahalanobis gating test would be used in measurement updating.

6.2. Automatic Initialization Procedure

An automatic initialization procedure is developed. Firstly, a stationary scene is automatically detected by only using image stream. Secondly, stationary IMU data is used to initialize the system state. The detailed procedure is described in Algorithm 1.

The algorithm identifies a stationary scene by continuously detecting almost no motion of tracked features. Then, static gyro data is used to initialize gyro bias. Rotation matrix ${\widehat{\mathbf{R}}}_w^b$ is computed by aligning gravity in frame b, which is the mean static accelerometer data, with gravity in frame w. This initialization procedure is a rough one since accelerometer bias has not been eliminated, but its uncertainty can be modeled by the initial covariance matrix of the filter state.

Algorithm 1 Automatic initialization procedure

Detect stationary scene $Counter=0$ for each image do   ${\mathbf{pix}}_{curr}=TrackFeatures\left(\right)$   if $0==Counter$ then     $Counter++$     ${\mathbf{pix}}_{prev}={\mathbf{pix}}_{curr}$     continue   end if   $\mathbf{diff}={\mathbf{pix}}_{curr}-{\mathbf{pix}}_{prev}$   if $max\left(\mathbf{diff}\right)$ is small enough then     $Counter++$   else     $Counter=0$     ${\mathbf{pix}}_{prev}={\mathbf{pix}}_{curr}$     continue   end if   if $Counter$ is big enough then     break   end if end for Initialize system state Save stationary acc data in ${\mathbf{arry}}_{acc}$ Save stationary gyro data in ${\mathbf{arry}}_{gyro}$ ${\mathbf{g}}_b=mean\left({\mathbf{arry}}_{acc}\right)$ ${\mathbf{g}}_w={[0,0,-9.8]}^T$ ${\widehat{\mathbf{R}}}_w^b=FromTwoVectors({\mathbf{g}}_b,-{\mathbf{g}}_w)$ ${\widehat{\mathbf{b}}}_g$ = $mean\left({\mathbf{arry}}_{gyro}\right)$ ${\widehat{\mathbf{v}}}^w=\mathbf{0};{\widehat{\mathbf{p}}}^w=\mathbf{0};{\widehat{\mathbf{b}}}_a=\mathbf{0}$;

6.3. Refined Feature Triangulation Mechanism

In Mourikis and Roumeliotis’s work [1], features are triangulated only if they are no longer being tracked; however, we found that this mechanism does not perform well, especially when using cheap IMUs. To conduct frequent and effective measurement updates, which is crucial to correct biased IMU propagation, a maximum feature tracking length is set. This means each feature would be triangulated when it has been tracked for a certain number of frames, even if it is still being tracked. In the latter situation, the current observation would not be used in triangulation.

Generally, features that failed in triangulation would be discarded directly. While the proposed mechanism is that if a feature fails in triangulation while it is still being tracked, it will have another chance to triangulate when the next image is coming. This mechanism improves the performance when the camera is moving slowly, where features in adjacent images exhibit a small parallax that would easily result in triangulation failure.

7. Experimental Results

The public dataset EuRoC [33] was used to evaluate the performance of the proposed VIO. It includes 11 sequences that were collected by a UAV in three different scenes. One is a machine hall and the other two are rooms equipped with motion capture systems and different manual layout arrangements. The extrinsic and intrinsic parameters of sensors are carefully calibrated, and ground truths of UAV poses are provided. It is one of the widely used benchmarks for evaluating algorithms of different configurations, including monocular-visual, stereo-visual and monocular/stereo-visual-inertial setups. All of the experiments below were performed on an Ubuntu 16.04 virtual machine powered by MacBook Pro Mid 2015 assigned with two core and 8 GB RAM. Our implementation is a real-time algorithm based on ROS nodelet [41].

The estimated trajectories and corresponding ground truths are shown in Figure 5. Estimated trajectories are aligned with ground truths by a 6-DOF $Sim\left(3\right)$ transformation without adjusting the scale [42].

7.1. Front-End Improvement

For implementations of front-ends with and without ORB descriptor assistance, we run each EuRoC sequence 50 times. A boxplot summary is shown in Figure 6. The corresponding means and standard deviations are listed in

Table 1. Mean and standard deviation of RMSEs in Figure 6. For each sequence, the one with an obviously better performance is highlighted.

Sequence	MH_01		MH_02		MH_03		MH_04		MH_05		V1_01		V1_02		V1_03		V2_01		V2_02		V2_03
	mean	std	mean	std	mean	std	mean	std	mean	std	mean	std	mean	std	mean	std	mean	std	mean	std	mean	std
pure optical flow	0.309	0.076	0.297	0.065	0.381	0.050	0.435	0.071	0.393	0.051	0.108	0.026	0.082	0.012	0.130	0.018	0.162	0.057	0.137	0.019	0.248	0.047
ORB assisted	0.294	0.055	0.273	0.056	0.330	0.048	0.366	0.058	0.391	0.046	0.104	0.018	0.082	0.010	0.131	0.017	0.127	0.030	0.134	0.019	0.231	0.039

.

After adding ORB descriptor assistance, the estimator performs better in most sequences, since boxes became narrow and their position lower in Figure 6. The statistics in Table 1 give a numerical display of the results. Obvious improvement can be observed in seven sequences. In the other four sequences, performance are similar with or without ORB descriptor assistance. This may be due to the small quantity of outliers of optical flow tracking in these sequences.

We also analyzed the processing time of the proposed ORB descriptor-assisted outlier elimination procedure. The maximum feature number is set as 150. The results are listed in

Table 2. Mean of the processing time (ms) of the proposed ORB descriptor-assisted outlier elimination procedure for every image.

Sequence	MH_01	MH_02	MH_03	MH_04	MH_05	V1_01	V1_02	V1_03	V2_01	V2_02	V2_03
process time	1.3942	1.6480	1.3373	1.3983	1.0870	1.3297	1.0410	0.9574	1.2506	1.0525	0.7465

.

As shown in Table 2, the proposed ORB descriptor-assisted outlier elimination procedure introduces little computation. The processing time varies among sequences, mostly due to the motion speed. Sequences with aggressive motion tend to take less processing time than those with slow motion since fewer features are tracked in the former case, and fewer ORB descriptor distances need to be calculated.

7.2. Comparison with MSCKF-MONO

We compare our proposed monocular MSCKF with the open-source monocular MSCKF implementation MSCKF-MONO [23]. MSCKF-MONO has a visual front-end based on optical flow and utilizes first-order approximation state transition equations in filtering. It also applies observability-constrained Kalman filter (OC-KF) [21] to fix the observability problem, which would fix the wrong observability properties and improve filter performance. Note that ours does not apply any similar techniques.

In our experiment, we removed the coarse initialization and forbid the reset module in MSCKF-MONO because for some reason, MSCKF-MONO did not work properly on nearly half of sequences under the original coarse initialization, and reset does not help if there is no stop during running. The initial state was assigned by noisy ground truth for both our algorithm and MSCKF-MONO in this experiment. To make a fair comparison, we tried to run with same setup for common parameters in both algorithms, such as noise densities for sensors measurement, sliding window size, and maximum or minimum track lengths for features. However, MSCKF-MONO barely worked in any sequences under a similar setup as ours. This is mainly due to the different state transition model and visual front-end implementations. As we explored further and could not find a setup which generally performed better than the original setup for MSCKF-MONO, we left the original parameters unaltered. The comparison results are listed in

Table 3. Comparison results for proposed algorithm and MSCKF-MONO using the EuRoC dataset. The means of positioning RMSEs (m) of 10 runs for both algorithms are calculated.

	MH_01	MH_02	MH_03	MH_04	MH_05	V1_01	V1_02	V1_03	V2_01	V2_02	V2_03
MSCKF-MONO	1.015	0.534	0.427	2.102	0.968	0.169	0.275	1.551	0.281	0.341	×
Proposed	0.299	0.280	0.342	0.350	0.384	0.096	0.078	0.132	0.121	0.137	0.224

.

The results show that the proposed monocular MSCKF is far more accurate than MSCKF-MONO. We claimed that this is due to a more accurate state transition model and a robust visual front-end.

7.3. Comparison with the State-Of-The-Art

The results of proposed VIO algorithm are compared with several state-of-the-art open-source monocular VIOs using the EuRoC dataset, including OKVIS [6], ROVIO [5], and VINS-MONO [2]. To make a fair comparison between pure VIOs, we turned off the closure detection in VINS-MONO. The proposed VIO automatically selects stationary IMU data to initialize the rotation and gyro bias at the beginning of every sequence, while other states are initialized as zeros. In addition, a unique parameter configuration is applied in all sequences. Results are listed in

Table 4. Results of proposed and state-of-the-art VIOs using EuRoC dataset. Ten runs on each sequence and the means of positioning RMSEs (m) are calculated.

	MH_01	MH_02	MH_03	MH_04	MH_05	V1_01	V1_02	V1_03	V2_01	V2_02	V2_03
VINS-MONO	0.159	0.182	0.199	0.350	0.313	0.090	0.110	0.188	0.089	0.163	0.305
ROVIO	0.250	0.653	0.449	1.007	1.448	0.159	0.198	0.172	0.299	0.642	0.190
OKVIS	0.376	0.378	0.277	0.323	0.451	0.087	0.157	0.224	0.132	0.185	0.305
Proposed	0.289	0.258	0.331	0.394	0.423	0.117	0.089	0.134	0.097	0.140	0.211

.

As shown above, the proposed VIO algorithm is comparable in accuracy to the state-of-the-art. Notice that VINS-MONO generally performs best out of all four algorithms, and the proposed algorithm has a similar performance in vicon rooms, which is due to good feature triangulation results in a limited area. In addition, the proposed algorithm and ROVIO perform better in V1_03 and V2_03 than others. There are aggressive motions in these two sequences that might result in tracking failure in the front-end; the proposed algorithm and ROVIO are filter-based methods that can utilize IMU measurements to propagate for a short period in this situation, while VINS-MONO and OKVIS sometimes fail and have to lean on relocalization in this circumstance. Notice that the machine hall is a relatively large-scale scenario [33], where triangulations in the proposed method mostly deal with points of large depth. This results in a relatively downgraded performance of the proposed method in the machine hall, even in sequences with mild motions.

7.4. Processing Time

As mentioned by Delmerico and Scaramuzza [17], the better performance of VINS-MONO is a trade-off requiring more computer resources than others. In contrast, the proposed method has a similar architecture to MSCKF-MONO, which is a light-weight solution. The average processing time of the visual front-end and EKF/optimization back-end of our implementation and the state-of-the-art are listed in

Table 5. Average processing time (ms) and rate (Hz) of visual front-end and EKF/optimization back-end of our implementation and the state-of-the-art using the EuRoC dataset.

	Sequence	MH_01		MH_02		MH_03		MH_04		MH_05		V1_01		V1_02		V1_03		V2_01		V2_02		V2_03
		Time	Rate	Time	Rate	Time	Rate	Time	Rate	Time	Rate	Time	Rate	Time	Rate	Time	Rate	Time	Rate	Time	Rate	Time	Rate
VINS-MONO	front-end	18.0	55	18.3	55	18.6	54	19.3	52	21.3	47	20.2	49	21.4	47	23.2	43	22.3	45	23.8	42	30.6	33
	back-end	50.2	20	50.9	20	50.1	20	50.1	20	53.0	19	53.1	19	45.9	22	37.9	26	54.4	18	48.3	21	33.4	30
ROVIO	front-end	2.0	505	1.9	526	2.0	497	2.1	476	2.0	490	1.9	538	2.0	508	2.1	481	2.0	503	2.0	510	2.0	478
	back-end	15.9	63	15.9	63	15.9	63	15.9	63	15.7	63	15.9	63	15.9	63	15.9	63	15.9	63	15.9	63	15.9	63
OKVIS	front-end	46.7	21	45.3	22	47.4	21	40.9	24	41.4	24	38.5	26	38.8	26	31.3	32	38.8	26	37.3	27	31.4	32
	back-end	39.8	25	39.4	25	39.9	25	32.1	31	33.1	30	30.6	33	25.5	39	19.2	52	29.6	34	27.9	36	18.0	56
Proposed	front-end	16.2	62	16.5	61	15.9	63	16.1	62	15.7	64	15.7	64	15.3	65	16.4	61	15.8	63	15.9	63	17.3	58
	back-end	5.5	182	5.9	169	6.1	164	5.5	181	6.0	166	5.7	174	5.4	185	4.9	203	5.7	176	5.6	178	4.6	218

.

The results show that, the proposed method has higher processing speed than the listed optimization-based methods. ROVIO is the fastest solution among all listed solutions, but as shown in Table 4, its precision is generally the worst. In proposed method, the visual front-end can process images at about 60 Hz. Notice that V2_03 is a little bit slower than others, because aggressive motions in this sequence result in a short feature tracking length, and thus, the front-end will take more time to extract new features. The EKF-based back-end run at more than 160Hz and the difference between each sequence is due to the difference in the number of features used in measurement updating. As can be concluded from Table 4 and Table 5, the proposed method is a VIO solution which has comparable precision and generally required less computation resources than the state-of-the-art.

8. Conclusions

In this paper, we first deduced a highly closed-form IMU error state transition equation from scratch. By using Hamilton’s notation of quaternion, we tried to eliminate notation ambiguity. We then managed to solve the integration terms left behind in the transition equation by introducing a two-sample fitting method to approximate the axis-angle, resulting in a fully linear closed-form formulation that is unbiased up to the fitting resolution. This formulation also has potential to incorporate IMU intrinsics into the filter state, since it is a linear function of IMU measurements. An automatic initialization procedure is developed and the feature triangulation mechanism is carefully refined. The ORB descriptor distance between Shi-Tomasi corner pairs was analyzed, and we found that there is a statistical difference in descriptor distances between matched and unmatched feature pairs. As outliers are sometimes fatal for filter-based VIOs, this inspired us to propose a visual front-end based on optical flow tracking and additionally, to use ORB descriptors to eliminate outliers. We implement a monocular VIO under the framework of MSCKF with proposed novelties.

Through a comparison between estimation results with and without the proposed outlier eliminating method, we demonstrate its effectiveness. Furthermore, an experiment was done to compare the proposed method with several state-of-the-art VIOs, both in terms of precision and computation. Results show that the proposed VIO is a visual inertial fusion solution with comparable precision to the state-of-the-ar but which demands less computation resources.

Future works include adding a robust initialization procedure adapting to versatile scenes and analyzing the point selection mechanism in detail.