Mixed-Degree Cubature H_∞ Information Filter-Based Visual-Inertial Odometry

Abstract

Visual–inertial odometry is an effective system for mobile robot navigation. This article presents an egomotion estimation method for a dual-sensor system consisting of a camera and an inertial measurement unit (IMU) based on the cubature information filter and H_∞ filter. The intensity of the image was used as the measurement directly. The measurements from the two sensors were fused with a hybrid information filter in a tightly coupled way. The hybrid filter used the third-degree spherical-radial cubature rule in the time-update phase and the fifth-degree spherical simplex-radial cubature rule in the measurement-update phase for numerical stability. The robust H_∞ filter was combined into the measurement-update phase of the cubature information filter framework for robustness toward non-Gaussian noises in the intensity measurements. The algorithm was evaluated on a common public dataset and compared to other visual navigation systems in terms of absolute and relative accuracy.

1. Introduction

Visual odometry (VO) is the process of estimating the egomotion of an agent with a single or multiple visual sensors in mobile robot navigation. The aim of VO is to estimate the pose incrementally. Compared with the simultaneous localization and mapping (SLAM) method seeking globally consistent estimation, VO is concerned with the local consistency of the pose estimation [1]. As an inexpensive method independent of external reference systems such as global positioning systems (GPS) and motion capture systems, VO is widely applied in domains including robotics, automotive, and wearable computing [2]. In robotic applications, VO is the basis of studies on visual control [3], obstacle avoidance in robot navigation [4,5], and unmanned aerial vehicle navigation [6].

In practical applications, VO is commonly combined with the inertial measurement units (IMUs) with the aid of Kalman filters. As the visual–inertial integrated navigation system serves as an egomotion estimator and is similar to VO functionally, we used the term of visual–inertial odometry (VIO) in this paper. The IMU generally operates independently of the environmental conditions at a higher rate than cameras. Therefore, the robustness and accuracy of the visual odometry can be enhanced by fusing the IMU measurements.

The VO methods are grouped into indirect methods and direct methods, depending on the usage of the images. The indirect methods preprocess the images of the camera to extract features that are used for motion estimation with a matching process. Direct methods use the raw measurements of the visual sensors without feature extraction and matching processes.

Indirect methods have dominated the research field for a long time, and many effective descriptors of features have been developed as shown in [7]. Guang [8] applied a line-feature extractor in the pre-processing of images and estimated the attitude error with line features. The IMU measurements were fused with the visual measurements loosely with the conventional Kalman filter. Mostafa [9] constructed a multi-sensor fusion system based on the extended Kalman filter (EKF). An indirect VO algorithm with speeded up robust features (SURF) was applied to process the visual measurements. A tightly coupled nonlinear optimization-based indirect VO was applied on the micro aerial vehicle (MAV) in [10,11]. Forster developed a hybrid approach with a direct method for initial alignment and an indirect method for joint optimization. Aladem [12] built a light-weight VO using the adaptive and generic accelerated segment test (AGAST) corner detector and the binary robust independent elementary features (BRIEF) descriptor.

Recently, direct methods have gained more attention due to their independence of extra feature extraction and matching processes. Bloesch [13] used the intensity errors of image patches as measurements and fused the measurements of IMU with the direct VO with EKF. The resulting VIO method was applied in the navigation of a MAV in [14]. Furthermore, Bloesch used the iterated EKF method that operates in a recursive form like the Gauss–Newton optimization to reduce the linearization errors in the regular EKF. A binocular vision system was used to construct a direct VIO in [15]. Engel designed a direct sparse odometry (DSO) using a monocular camera [16]. The even sampling and keyframe management methods used in Engle’s DSO contributed to improving the accuracy and robustness of the photometric-error optimization.

The Jacobian-based EKF method suffers from linearization errors in nonlinear systems. To improve its accuracy, Arasaratnam designed the cubature Kalman filter (CKF) by applying the third-degree spherical-radial cubature rule on the nonlinear Bayes filter [17]. The resulting CKF outperformed the EKF and unscented Kalman filter (UKF) on nonlinear systems. Based on the third-degree CKF, Jia designed a fifth-degree CKF for higher accuracy [18]. Wang developed another CKF method with the regular simplex and moment matching method [19]. The developed fifth-degree spherical simplex-radial cubature Kalman filter (SSRCKF) required fewer cubature points than the fifth-degree CKF in [18] in high-dimensional applications. Zhang developed an arbitrary degree interpolatory cubature Kalman filter (ICKF) with additional free parameters [20]. By adjusting the free parameters, CKF and UKF are special cases of ICKF. An estimator with an online measuring of non-linearity was developed in [21]. The method adaptively switches between cubature rules with different degrees. The CKF was used in [22] for a loosely coupled nonlinear attitude estimator. Tseng [23] embedded the Huber M-estimation method with CKF in [17] to deal with the outliers and non-Gaussian noises in the measurements.

As an equivalent description of the Kalman filter, the information filter has also been extended with the cubature rules for nonlinear systems. Pakki directly applied the technique used in [17] on the information filter and obtained a third-degree spherical-radial cubature rule-based cubature information filter (CIF). Jia applied the fifth-degree spherical-radial cubature rule on the information filter for multi-sensor fusion [24,25]. Yin developed the third-degree spherical simplex-radial cubature information filter. CIFs with different cubature rules have been used in the state estimation for the biased nonlinear system [26], bearing-only tracking with multi sensors [27], and the trajectory estimation for the ballistic missile [28].

The H_∞ filter was developed for minimizing the estimation errors in the presence of unsatisfactory noises. Yang developed a solution of the H_∞ filter in the form of the regular Kalman filter [29]. Chandra directly embedded Yang’s H_∞ filter in the CKF and CIF according to the computational procedures of the filters [30,31] to improve the CKF and CIF in the presence of non-Gaussian noises.

In this paper, motivated by the recently developed cubature information filters for nonlinear systems, a mixed-degree cubature H_∞ information filter-based VIO (MCH_∞IF-VIO) was designed by fusing the measurements of IMU and intensity errors in a tightly coupled manner. Unlike previous studies that have focused on refining the feature descriptions, we developed the MCH_∞IF-VIO system according to the system characteristics and used a simple frame-to-frame alignment model. The proposed VIO method mainly focuses on the nonlinearity and non-Gaussian noises in the VIO system, and is suitable for the camera-IMU system consisting of an IMU and an RGBD camera. The main contributions of this paper are as follows.

• A mixed-degree nonlinear filter framework was proposed for fusing the IMU measurements and the intensity measurements of the camera. The proposed filter applied two cubature rules with different degrees to guarantee numerical stability with high accuracy.
• The H_∞ filter was used in the measurement update phase for robustness toward the non-Gaussian noises in intensity errors.

The remainder of this paper is organized as follows. The model of the visual–inertial odometry is presented in Section 2. The VIO system based on a mixed-order cubature H_∞ information filter is detailed in Section 3. The test results with a public dataset are presented in Section 4. The conclusions are presented in Section 5.

2. Model of Visual–Inertial Odometry

In this section, we present the model of a keyframe-based VIO system consisting of the motion model, the keyframe management, and the measurement model. The motion model was formulated in form of classical rigid-body kinematics with the modified Rodrigues parameter representing the attitude. The keyframe management was mainly inspired by Engle’s DSO system with some simplifications. The measurement model was formulated in the form of the simple frame-to-frame alignment with the intensity as the measurement.

2.1. Coordinate Systems

Two moving coordinate systems connected with the sensors were used in this paper: (1) The IMU coordinate system (ICS) with its origin at the center of the IMU, where the linear accelerations and angular rates are measured; and (2) The camera coordinate system (CCS) with its origin at the optical center of the camera and the z-axis aligned with the optical axis of the lens.

Another two reference coordinate systems were defined correspondingly: The static IMU coordinate system (SICS) with the same origin and axes as the ICS at the time of constructing the current keyframe; and the static camera coordinate system (SCCS) with the same origin and axes as the CCS at the time of constructing the current keyframe. The SICS and SCCS were static relative to the inertial space in the egomotion estimation.

In this paper, the superscript at the top-left corner of a vector represents the coordinate system the vector is expressed in. Specifically, ${}^{\mathrm{I}}p$ represents the vector $p$ in the ICS, ${}^{\mathrm{C}}p$ represents the vector $p$ in the CCS, ${}^{\mathrm{SI}}p$ represents the vector $p$ in the SICS, and ${}^{\mathrm{SC}}p$ represents the vector $p$ in the SCCS.

2.2. Motion Model of Camera-IMU System

A 9-dimension system was built to represent the motion of the camera–IMU system. The state vector is defined as follows:

$$x={\left[\begin{array}{ccc}{}_{\mathrm{SI}}{}^{\mathrm{I}}\rho ^{\mathrm{T}}& {}^{\mathrm{SI}}p^{\mathrm{T}}& {}^{\mathrm{SI}}v^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$$

(1)

where ${}_{\mathrm{SI}}{}^{\mathrm{I}}\rho$ is the modified Rodrigues parameter (MRP) representing the rotation of the ICS relative to the SICS; ${}^{\mathrm{SI}}p$ is the position of the origin of the ICS in the SICS; and ${}^{\mathrm{SI}}v$ is the velocity of the origin of the ICS relative to the SICS. A discrete motion model was constructed according to the six degrees of the freedom kinematic model of a rigid body as follows:

$$x_k=F{x}_{k-1}+G\left(x_{k-1}\right)u_{k-1}+C$$

(2)

where $u_{k-1}={\left[\begin{array}{cc}{}^{\mathrm{I}}\omega _{k-1}^{\mathrm{T}}& {}^{\mathrm{I}}a_{k-1}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$ is the input vector of the navigation system; ${}^{\mathrm{I}}\omega _{k-1}$ and ${}^{\mathrm{I}}a_{k-1}$ are the measurements of the gyroscope and the accelerator at time $k-1$, respectively; $C={\left[\begin{array}{cc}{0}_{1\times 6}& \Delta t{}^{\mathrm{SI}}g^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$ is a constant vector; $\Delta t$ is the sampling period; ${}^{\mathrm{SI}}g={}_{\mathrm{E}}{}^{\mathrm{SI}}R{}^{\mathrm{E}}g$ is the gravity acceleration vector in the SICS; ${}_{\mathrm{E}}{}^{\mathrm{SI}}R$ is the transformation matrix from the earth-fixed coordinate system to the SICS, of which initial value determined by a simple attitude and heading reference system (AHRS) [32]; and $F$ and $G\left(x_{k-1}\right)$ are the state-transition matrix and input matrix, respectively, formulated as follows:

$$\begin{array}{l}F=\left[\begin{array}{ccc}{\mathcal{I}}_3& 0_3& 0_3\\ 0_3& {\mathcal{I}}_3& \Delta t{\mathcal{I}}_3\\ 0_3& 0_3& {\mathcal{I}}_3\end{array}\right]\\ G\left(x_{k-1}\right)=\left[\begin{array}{cc}A\left({}_{\mathrm{SI}}{}^{\mathrm{I}}\rho _{k-1}\right)& 0_3\\ 0_3& 0_3\\ 0_3& {}_{\mathrm{SI}}{}^{\mathrm{I}}R{\left({}_{\mathrm{SI}}{}^{\mathrm{I}}\rho _{k-1}\right)}^{\mathrm{T}}\end{array}\right]\end{array}$$

(3)

where ${\mathcal{I}}_3$ is the three-dimensional identity matrix; and $0_3$ is the three-dimensional zero matrix. $A\left({}_{\mathrm{SI}}{}^{\mathrm{I}}\rho _{k-1}\right)$ and ${}_{\mathrm{SI}}{}^{\mathrm{I}}R\left({}_{\mathrm{SI}}{}^{\mathrm{I}}\rho _{k-1}\right)$ are formulated as follows. The right subscript $k-1$, left subscript ‘SI’ and left prescript ‘I’ in the right side of the equations are neglected to simplify the description

$$A\left({}_{\mathrm{SI}}{}^{\mathrm{I}}\rho _{k-1}\right)=\frac{1}{4}\left[\begin{array}{ccc}{\rho }_1^2-{\rho }_2^2-{\rho }_3^2+1& 2\left({\rho }_1{\rho }_2-{\rho }_3\right)& 2\left({\rho }_1{\rho }_3+{\rho }_2\right)\\ 2\left({\rho }_1{\rho }_2+{\rho }_3\right)& {\rho }_2^2-{\rho }_1^2-{\rho }_3^2+1& 2\left({\rho }_2{\rho }_3-{\rho }_1\right)\\ 2\left({\rho }_1{\rho }_3-{\rho }_2\right)& 2\left({\rho }_2{\rho }_3+{\rho }_1\right)& {\rho }_3^2-{\rho }_1^2-{\rho }_2^2+1\end{array}\right]$$

(4)

$${}_{\mathrm{SI}}{}^{\mathrm{I}}R\left({}_{\mathrm{SI}}{}^{\mathrm{I}}\rho _{k-1}\right)=\left[\begin{array}{ccc}1-\frac{8\left({\rho }_2^2+{\rho }_3^2\right)}{{\left({\left|\rho \right|}^2+1\right)}^2}& -\frac{4\left({\left|\rho \right|}^2{\rho }_3-2{\rho }_1{\rho }_2-{\rho }_3\right)}{{\left({\left|\rho \right|}^2+1\right)}^2}& \frac{4\left({\left|\rho \right|}^2{\rho }_2+2{\rho }_1{\rho }_3-{\rho }_2\right)}{{\left({\left|\rho \right|}^2+1\right)}^2}\\ \frac{4\left({\left|\rho \right|}^2{\rho }_3+2{\rho }_1{\rho }_2-{\rho }_3\right)}{{\left({\left|\rho \right|}^2+1\right)}^2}& 1-\frac{8\left({\rho }_1^2+{\rho }_3^2\right)}{{\left({\left|\rho \right|}^2+1\right)}^2}& -\frac{4\left({\left|\rho \right|}^2{\rho }_1-2{\rho }_2{\rho }_3-{\rho }_1\right)}{{\left({\left|\rho \right|}^2+1\right)}^2}\\ -\frac{4\left({\left|\rho \right|}^2{\rho }_2-2{\rho }_1{\rho }_3-{\rho }_2\right)}{{\left({\left|\rho \right|}^2+1\right)}^2}& \frac{4\left({\left|\rho \right|}^2{\rho }_1+2{\rho }_2{\rho }_3-{\rho }_1\right)}{{\left({\left|\rho \right|}^2+1\right)}^2}& 1-\frac{8\left({\rho }_1^2+{\rho }_2^2\right)}{{\left({\left|\rho \right|}^2+1\right)}^2}\end{array}\right]$$

(5)

In fact, ${}_{\mathrm{SI}}{}^{\mathrm{I}}R\left({}_{\mathrm{SI}}{}^{\mathrm{I}}\rho _{k-1}\right)$ is the direct cosine matrix representing the rotation of the IMU coordinate system relative to the static IMU coordinate system.

2.3. Keyframe Management

A keyframe consisting of a RGB image and a depth image represents the reference for the egomotion estimation. The very first RGB image and depth image are used to set up the first keyframe. A point set is sampled from the images of the keyframe together with the intensity. Unlike the indirect VO, no extraction or matching process of the distinct features is needed in the direct VO. As a result, the direct VO is generally faster than the indirect VO. However, the accuracy and robustness are hard to ensure without carefully designed features. To ensure the accurate estimation, the point sampling in the direct VO should follow two basic criteria: (1) The sampled points are distributed as evenly as possible in the image; and (2) The absolute intensity gradient at the sampled point is significantly higher than its neighbors. The absolute intensity gradient g at point j with the pixel coordinate of $\left[u_{{\mathrm{p}}_j},v_{{\mathrm{p}}_j}\right]$ can be evaluated with the intensity differences as follows:

$$g=\sqrt{{\left(\frac{I\left(u_{{\mathrm{p}}_j}+1,v_{{\mathrm{p}}_j}\right)-I\left(u_{{\mathrm{p}}_j}-1,v_{{\mathrm{p}}_j}\right)}{2}\right)}^2+{\left(\frac{I\left(u_{{\mathrm{p}}_j},v_{{\mathrm{p}}_j}+1\right)-I\left(u_{{\mathrm{p}}_j},v_{{\mathrm{p}}_j}-1\right)}{2}\right)}^2}$$

(6)

where $I(·)$ is the intensity at the corresponding pixel coordinate. For even sampling, the RGB image in the keyframe is divided into patches with the size of $b\times b$. The pixel with the highest absolute intensity gradient among the pixels with valid depth measurements in each patch is chosen as a candidate. If the absolute intensity gradient of a candidate is no less than a predesigned threshold $g_{\mathrm{thres}}$, the 3D coordinate of the candidate pixel is constructed. We used a simple pin-hole camera model for the 3D construction.

$${}^{\mathrm{SC}}p_{{\mathrm{p}}_j}=\left[\begin{array}{c}{}^{\mathrm{SC}}x_{{\mathrm{p}}_j}\\ {}^{\mathrm{SC}}y_{{\mathrm{p}}_j}\\ {}^{\mathrm{SC}}z_{{\mathrm{p}}_j}\end{array}\right]=d\left(u_{{\mathrm{p}}_j},v_{{\mathrm{p}}_j}\right)\left[\begin{array}{c}\frac{u_{{\mathrm{p}}_j}-c_{\mathrm{x}}}{f_{\mathrm{x}}}\\ \frac{v_{{\mathrm{p}}_j}-c_{\mathrm{y}}}{f_{\mathrm{y}}}\\ 1\end{array}\right]$$

(7)

where $d\left(u_{{\mathrm{p}}_j},v_{{\mathrm{p}}_j}\right)$ is the depth measurement of corresponding point; and $f_{\mathrm{x}}$, $f_{\mathrm{y}}$, $c_{\mathrm{x}}$, and $c_{\mathrm{y}}$ are the intrinsic parameters of the RGB camera. To simplify the measurement function, the constructed points were transformed into static IMU coordinate system. The transformed 3-dimensional coordinate was added into the point set of the keyframe.

$${}^{\mathrm{SC}}p_{{\mathrm{p}}_j}={}_{\mathrm{I}}{}^{\mathrm{C}}R^{\mathrm{T}}\left({}^{\mathrm{SC}}p_{{\mathrm{p}}_j}-{}^{\mathrm{C}}p\right)$$

(8)

where ${}_{\mathrm{I}}{}^{\mathrm{C}}R$ and ${}^{\mathrm{C}}p$ represent the transformation between the ICS and the CCS. We assumed that the two sensors were mounted on a rigid body so that the transformation did not change over time. The camera–IMU transformation and the intrinsic parameters were calibrated in advance and the calibration methods are not included in this paper.

The absolute intensity gradient threshold $g_{\mathrm{thres}}$ was designed according to the intensity characteristic of each patch and evaluated adaptively online. We treated $g_{\mathrm{thres}}$ as the sum of two parts:

$$g_{\mathrm{thres}}=\overline{g}+g_{\mathrm{b}}$$

(9)

where $\overline{g}$ is the average intensity of the image patch and $g_{\mathrm{b}}$ is a positive value to ensure that the absolute intensity gradient of the candidate is sufficiently high. It is easy to understand that the absolute intensity gradient of the candidate should exceed more pixels in a larger patch, which means that $g_{\mathrm{thres}}$ is higher with a larger patch and $g_{\mathrm{b}}$ has a positive correlation with the patch size b. We used a simple linear function to represent the positive correlation and formulated the lower-bound constraint of the absolute intensity gradient as follows:

$$g\ge \overline{g}+\lambda \left(b-1\right),\lambda >0$$

(10)

where $\lambda$ is a positive constant. Particularly in the case of $b=1$, i.e., only one pixel exits in each patch, the above lower bound constraint degrades into $g\ge g$, which means that all points with valid depth measurements are constructed. Figure 1 shows an example of point sampling on a 640 × 480 image. In the example, the above method was able to sample points not only from the edges of the objects, but also from regions with weak intensity variations such as the computer screen and the floor. The points were distributed more densely with a smaller patch. The numbers of sampled points with different patch sizes in this example are shown in

Table 1. The number of sampled points with different patch sizes.

b	Number of Sampled Points
2	55752
4	14445
8	3707
16	964
32	242

.

The above point sampling method adjusts the number of the points by tuning b and guarantees evenly distributed points. The usage of images approximated the dense visual odometry with a small b. The usage of images approximated the sparse visual odometry with a large b. The keyframe may not sufficient to estimate the egomotion as the camera moves, so we used two judging criteria to change the keyframe:

• A new keyframe is needed if there are not enough points in the current view. This is measured via the ratio of points in the current view.
• Observing the same area from different locations far from each other is likely to lead to large differences in occlusion and illumination. In such cases, new keyframes are needed even if enough points are in the current view. This criterion is quantified with the mean square optical flow ignoring rotational motion $f=\sqrt{\frac{1}{m}{\displaystyle {\sum }_{i=1}^m\left({\left(u_{i,0}-u_{i,\mathrm{t}}\right)}^2+{\left(v_{i,0}-v_{i,\mathrm{t}}\right)}^2\right)}}$. $\left[u_{i,0},v_{i,0}\right]$ is the pixel coordinate of point i in the keyframe, and $\left[u_{i,\mathrm{t}},v_{i,\mathrm{t}}\right]$ is the pixel coordinate transformed only by the translational motion.

A new keyframe is constructed with the current RGBD images if any of the two criteria are satisfied. After a new keyframe is constructed, the previous keyframe will be replaced by the new one, and the reference coordinate systems are changed in turn. The estimations of ${}^{\mathrm{SI}}v$ and ${}_{\mathrm{E}}{}^{\mathrm{SI}}R$ are transformed with ${}_{\mathrm{SI}}{}^{\mathrm{I}}\rho$. Then, the estimations of ${}_{\mathrm{SI}}{}^{\mathrm{I}}\rho$ and ${}^{\mathrm{SI}}p$ are set to 0.

The above keyframe and point sampling method mainly refers to Engel’s DSO method, except for several simplifications as follows:

• Only one keyframe is maintained for the sake of complexity.
• The depth measurements are generated with an RGBD camera instead of being estimated in the optimization scheme.
• The gradient constraint in the sampling points is evaluated according to the patch size.

2.4. Measurement Model

A direct VO was used as the measurement model where the intensity was used as the measurement directly without feature extraction and matching processes. The position of each point j was transformed from the SICS into the CCS by the motion state as follows:

$${}^{\mathrm{C}}p_{{\mathrm{p}}_j}={}_{\mathrm{I}}{}^{\mathrm{C}}R{}_{\mathrm{SI}}{}^{\mathrm{I}}R\left({}_{\mathrm{SI}}{}^{\mathrm{I}}\rho \right)\left({}^{\mathrm{SI}}p_{{\mathrm{p}}_j}-{}^{\mathrm{SI}}p\right)+{}^{\mathrm{C}}p$$

(11)

The pixel coordinate of point j in the current view was evaluated according to the pin-hole camera model.

$$\left[\begin{array}{c}{u}_{{\mathrm{p}}_j}\\ v_{{\mathrm{p}}_j}\end{array}\right]=\pi \left({}^{\mathrm{C}}p_{{\mathrm{p}}_j}\right)=\left[\begin{array}{c}{}^{\mathrm{C}}x_{{\mathrm{p}}_j}\frac{f_{\mathrm{x}}}{{}^{\mathrm{C}}z_{{\mathrm{p}}_j}}+c_{\mathrm{x}}\\ {}^{\mathrm{C}}y_{{\mathrm{p}}_j}\frac{f_{\mathrm{y}}}{{}^{\mathrm{C}}z_{{\mathrm{p}}_j}}+c_{\mathrm{y}}\end{array}\right]$$

(12)

The intensity $I_k\left(u_{{\mathrm{p}}_j},v_{{\mathrm{p}}_j}\right)$ of point j in the RGB image at time k was used as the measurement

$$z_{j,k}=I_k\left(u_{{\mathrm{p}}_j},v_{{\mathrm{p}}_j}\right)+n_{j,k}$$

(13)

where $n_{j,k}$ is the measurement noise and assumed to obey a Gaussian distribution with covariance $R_{j,k}$. Combining Equations (11)–(13), one can obtain the following measurement equation.

$$z_k=h\left({}_{\mathrm{SI}}{}^{\mathrm{I}}\rho _k,{}^{\mathrm{SI}}p_k\right)$$

(14)

3. Mixed-Degree Cubature H_∞ Information Filter-Based VIO

The above keyframe management method and measurement model of intensity led to a simple frame-to-frame alignment-based egomotion estimator. However, the simple estimator with linearization-based solvers such as the Gaussian–Newton optimization and EKF performed poorly as the models were highly nonlinear and the measurement noises vary dramatically. The linear approximations and Gaussian assumptions used in the solvers were not valid practically. For a robust estimator, the previous studies on VO generally maintained a slide window of multiple keyframes or a local map. In this section, we present a nonlinear filter-based VIO method for the frame-to-frame alignment problem. The designed hybrid cubature H_∞ information filter used two cubature rules with different degrees to reduce the linearization error in a numerically stable way and the H_∞ filter to estimate the states in the presence of non-Gaussian noises.

3.1. Bayes Filter for Nonlinear Visual–Inertial Navigation System

Consider the following nonlinear system

$$\begin{array}{l}{x}_k=f\left(x_{k-1}\right)+w_{k-1}\\ z_k=h\left(x_k\right)+n_k\end{array}$$

(15)

where $f(·)$ and $h(·)$ are arbitrary nonlinear functions and $w_{k-1}$ and $n_k$ are the process noise and the measurement noise, respectively. Under the Gaussian assumption of noises, the Bayes filter for nonlinear system described by the information vector and information matrix was formulated as an iterated process of a time-update phase and a measurement-update phase.

(1) Time-update phase

$$\begin{array}{l}{\widehat{\mathit{x}}}_{k|k-1}={\displaystyle {\int }_{{\mathrm{ℝ}}^{\mathrm{n}}}f\left(x\right)\mathcal{N}\left(x;{\widehat{\mathit{x}}}_{k-1|k-1},P_{k-1|k-1}\right)dx}\\ P_{k|k-1}={\displaystyle {\int }_{{\mathrm{ℝ}}^{\mathrm{n}}}f\left(x\right)f^{\mathrm{T}}\left(x\right)\mathcal{N}\left(x;{\widehat{\mathit{x}}}_{k-1|k-1},P_{k-1|k-1}\right)dx}-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{\mathit{x}}}_{k|k-1}^{\mathrm{T}}\\ Y_{k|k-1}=P_{k|k-1}^{-1}\\ y_{k|k-1}=Y_{k|k-1}{\widehat{x}}_{k|k-1}\end{array}$$

(16)

(2) Measurement-update phase

$$\begin{array}{l}{\widehat{\mathit{z}}}_{k|k-1}={\displaystyle {\int }_{{\mathrm{ℝ}}^{\mathrm{n}}}h\left(x\right)\mathcal{N}\left(x;{\widehat{\mathit{x}}}_{k|k-1},P_{k|k-1}\right)dx}\\ P_{xz}={\displaystyle {\int }_{{\mathrm{ℝ}}^{\mathrm{n}}}{xh}^{\mathrm{T}}\left(x\right)\mathcal{N}\left(x;{\widehat{\mathit{x}}}_{k|k-1},P_{k|k-1}\right)dx-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}}\\ {\mathit{H}}_k={\left(Y_{k|k-1}{P}_{xz}\right)}^{\mathrm{T}}\\ {\widehat{\mathit{y}}}_{k|k}={\widehat{\mathit{y}}}_{k|k-1}+{\mathit{H}}_k^T{\mathit{R}}_k^{-1}({\mathit{z}}_k-{\widehat{\mathit{x}}}_{k|k-1})+{\mathit{H}}_k{\widehat{\mathit{x}}}_{k|k-1}\\ Y_{k|k}=Y_{k|k-1}+{\mathit{H}}_k^T{\mathit{R}}_k^{-1}{\mathit{H}}_k\end{array}$$

(17)

The state estimation could be recovered from the information state

$$\begin{array}{l}{P}_{k|k}=Y_{k|k}^{-1}\\ {\widehat{\mathit{x}}}_{k|k}=P_{k|k}{\widehat{\mathit{y}}}_{k|k}\end{array}$$

(18)

According to the measurement model in Equation (14), only a part of the state vector appeared explicitly in the measurement equation. We defined the state vector explicitly shown in the measurement model as follows:

$$x_1=\left[\begin{array}{c}{}_{\mathrm{SI}}{}^{\mathrm{I}}\rho \\ {}^{\mathrm{SI}}p\end{array}\right]\in {\mathrm{ℝ}}^{n_1}$$

(19)

where $n_1=6$. The rest state vector is

$$x_2={}^{\mathrm{SI}}v\in {\mathrm{ℝ}}^{n_2}.$$

(20)

Correspondingly, the covariance matrix $\mathit{P}$ of the state vector is composed of four blocks as follows:

$$P=\left[\begin{array}{cc}{P}_{11}& P_{12}\\ P_{21}& P_{22}\end{array}\right]$$

(21)

Applying the above partition of the state vector to the evaluation of the predicted measurements (the first equation in Equation (17)), one can obtain.

$$\begin{array}{ll}{\widehat{\mathit{z}}}_{k|k-1}& ={\displaystyle {\int }_{{\mathrm{ℝ}}^{\mathrm{n}}}h\left(x\right)\mathcal{N}\left(x;{\widehat{\mathit{x}}}_{k|k-1},P_{k-1|k-1}\right)dx}\\ & ={\displaystyle {\int }_{{\mathrm{ℝ}}^{\mathrm{n}1}}h\left(x\right)\mathcal{N}\left(x_1;{\widehat{\mathit{x}}}_{1,k|k-1},P_{11,k|k-1}\right)d{x}_1}\end{array}$$

(22)

As shown by Equation (22), the original 9-dimensional integral is simplified to a 6-dimensional integral. The evaluation of the cross-covariance matrix (the second equation in Equation (17)) can also be simplified in a similar way.

$$\begin{array}{ll}{P}_{xz}& ={\displaystyle {\int }_{{\mathrm{ℝ}}^{\mathrm{n}}}{xh}^{\mathrm{T}}\left(x\right)\mathcal{N}\left(x;{\widehat{\mathit{x}}}_{k-1|k-1},P_{k-1|k-1}\right)dx}-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}\\ & ={\displaystyle {\int }_{{\mathrm{ℝ}}^{n_1}}{\displaystyle {\int }_{{\mathrm{ℝ}}^{n_2}}\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]h^{\mathrm{T}}\left(x_1\right)p\left(x_1{x}_2\right)d{x}_{2}d{x}_1}-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{z}}_{k|k-1}^{\mathrm{T}}}\end{array}$$

(23)

where $p\left(x_1,x_2\right)$ is the joint probability density function of two random vectors $x_1$ and $x_2$ i.e., the probability density function of the Gaussian distribution. Based on the formula of conditional probability, one can obtain

$$p\left(x_1,x_2\right)=p\left(x_2|x_1\right)p\left(x_1\right)$$

(24)

Substituting Equation (24) into Equation (23) yields

$$P_{xz}={\displaystyle {\int }_{{\mathrm{ℝ}}^{n_1}}{\displaystyle {\int }_{{\mathrm{ℝ}}^{n_2}}\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]h^{\mathrm{T}}\left(x_1\right)p\left(x_2|x_1\right)d{x}_2}p\left(x_1\right)d{x}_1}-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}$$

(25)

With the Gaussian assumption of the state vector, the distribution of $x_2$ conditional on $x_1$ is a multivariate Gaussian with the mean value of ${\widehat{\mathit{x}}}_2+P_{21}{P}_{11}^{-1}\left(x_1-{\widehat{\mathit{x}}}_1\right)$ and the covariance matrix of $P_{22}-P_{21}{P}_{11}^{-1}{P}_{12}$. Thus, the inner integral in Equation (25) is solved as

$${\displaystyle {\int }_{{\mathrm{ℝ}}^{n_2}}\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]h^{\mathrm{T}}\left(x_1\right)p\left(x_2|x_1\right)d{x}_2}=\left[\begin{array}{c}{x}_1\\ {\widehat{\mathit{x}}}_2+P_{21}{P}_{11}^{-1}\left(x_1-{\widehat{\mathit{x}}}_1\right)\end{array}\right]h^{\mathrm{T}}\left(x_1\right).$$

(26)

The cross-covariance matrix is evaluated as follows:

$$\begin{array}{ll}{P}_{xz}& ={\displaystyle {\int }_{{\mathrm{ℝ}}^{n_1}}\left[\begin{array}{c}{x}_1\\ {\widehat{\mathit{x}}}_{2,k|k-1}+P_{21,k|k-1}{P}_{11,k|k-1}^{-1}\left(x_1-{\widehat{\mathit{x}}}_{1,k|k-1}\right)\end{array}\right]h^{\mathrm{T}}\left(x_1\right)p\left(x_1\right)d{x}_1}-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}\\ & =\left[\begin{array}{c}{\displaystyle {\int }_{{\mathrm{ℝ}}^{n_1}}{x}_1{h}^{\mathrm{T}}\left(x_1\right)p\left(x_1\right)d{x}_1}\\ {\displaystyle {\int }_{{\mathrm{ℝ}}^{n_1}}\left({\widehat{x}}_{2,k|k-1}+P_{21,k|k-1}{P}_{11,k|k-1}^{-1}\left(x_1-{\widehat{\mathit{x}}}_{1,k|k-1}\right)\right)h^{\mathrm{T}}\left(x_1\right)p\left(x_1\right)d{x}_1}\end{array}\right]-{\widehat{\mathit{x}}}_{k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}\\ & =\left[\begin{array}{c}{\displaystyle {\int }_{{\mathrm{ℝ}}^{n_1}}{x}_1{h}^{\mathrm{T}}\left(x_1\right)p\left(x_1\right)d{x}_1}-{\widehat{x}}_{1,k|k-1}{\widehat{z}}_{k|k-1}^{\mathrm{T}}\\ P_{21,k|k-1}{P}_{11,k|k-1}^{-1}\left({\displaystyle {\int }_{{\mathrm{ℝ}}^{n_1}}{x}_1{h}^{\mathrm{T}}\left(x_1\right)p\left(x_1\right)d{x}_1}-{\widehat{\mathit{x}}}_{1,k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}\right)\end{array}\right]\end{array}$$

(27)

In this way, the integrals in 9-dimensional space are simplified into integrals in 6-dimensional space. This is practically useful when applying high-degree cubature rules and will be discussed later.

3.2. Mixed-Degree Cubature Information Filter

In general, the four integrals in Equations (16) and (17) cannot be solved directly. A group of numerical integration-based approximation methods named by cubature rules are used for the filter process and the resulting filters are called cubature filters. Using a cubature rule, a Gaussian weighted integral of a nonlinear function $g\left(x\right)$ is approximated with a weighted sum as follows:

$${\displaystyle {\int }_{{\mathrm{ℝ}}^{\mathrm{n}}}g\left(x\right)\mathcal{N}\left(x;{\widehat{\mathit{x}}}_{k-1|k-1},P_{k-1|k-1}\right)dx}\approx {\displaystyle \sum _{j=1}^{N_p}{w}_{j}g\left(X_j\right)}$$

(28)

where $X_j$ represents the cubature points; $w_j$ is the corresponding weight that satisfies ${\displaystyle \sum _{j=1}^{N_p}{w}_j}=1$; and $N_p$ is the number of the cubature points. The formulations of the cubature points and weights are different in different cubature rules. The spherical-radial (SR) cubature rules use the spherical-radial coordinates to transform the original integral into a double integral and select the cubature points on the spherical coordinate and the radial coordinate respectively. Based on the SR rules, the spherical simplex-radial (SSR) cubature rules use the n-simplex to evaluate the cubature points on the spherical coordinate.

The degree of a cubature rule is defined based on the degree of used monomials in the Tayler polynomial of the nonlinear function. A p-degree cubature rule integrates all of the monomials in the Tayler polynomial of $g\left(x\right)$ up to degree p exactly but not exactly for some monomials of degree p + 1. In general, the cubature rule with a higher degree achieves higher accuracy, but suffers from more computation with more cubature points and worse numerical stability with potential negative weights. Practically, third-degree cubature rules and fifth-degree cubature rules are commonly used in the Bayes filter. The numbers of cubature points of the four cubature rules are listed in

Table 2. Number of cubature points of different rules.

Cubature rules	Np
3rd-degree spherical-radial cubature rule (3-SR)	2n
5th-degree spherical-radial cubature rule (5-SR)	2n2 + 1
3rd-degree spherical simplex-radial cubature rule (3-SSR)	2n + 2
5th-degree spherical simplex-radial cubature rule (5-SSR)	n2 + 3n + 3

.

A cubature rule with all weights being positive is more desirable than one with both positive and negative weights for the sake of numerical stability. A stability parameter was defined in [33] as the sum of absolute values of the weights:

$$\mathsf{\Theta }={\displaystyle {\sum }_{i=0}^{N_p}\left|w_i\right|}$$

(29)

When $\mathsf{\Theta }$ is larger than 1, the truncation error will decrease the accuracy of the numerical integration. The curves of $\mathsf{\Theta }$ with respect to n for different cubature rules are shown in Figure 2. The stability parameters of 3-SR and 3-SSR were maintained at 1, while the stability parameter of 5-SR increased since n > 4, and the stability parameter of 5-SSR increased since n > 7.

According to the previous motion and measurement models, the time-update phase involves Gaussian-weighted integrals in 9-dimensional space while the measurement-update phase involves Gaussian-weighted integrals in 6-dimensional space. To obtain high precision and numerical stability with as few cubature points as possible, we applied the 3-SR rule in the time-update phase and the 5-SSR rule in the measurement-update phase. Substituting the chosen cubature rules, we formulated the VIO process based on the mixed-degree cubature information filter as follows.

(1) Time-update phase

Evaluate the cubature points based on the 3-SR rule:

$${\xi }_{k-1|k-1,j}=S_{k-1|k-1}{\gamma }_j^{\mathrm{SR}3}+{\widehat{\mathit{x}}}_{k-1|k-1},1\le j\le 2n$$

(30)

where $S_{k-1|k-1}$ is the square root of the covariance matrix $P_{k-1|k-1}$ so that $P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^{\mathrm{T}}$ and could be evaluated using the Cholesky decomposition; and ${\gamma }_j^{\mathrm{SR}3}$ is formulated as follows:

$${\gamma }_j^{\mathrm{SR}3}=\{\begin{array}{lll}{a}_j& ,& 1\le j\le n\\ -a_{j-n}& ,& n+1\le j\le 2n\end{array}$$

(31)

$$\begin{array}{l}{a}_j={\left[a_{j,1},a_{j,2},\cdots ,a_{j,n}\right]}^{\mathrm{T}}\\ a_{j,i}=\{\begin{array}{lll}\sqrt{9}& ,& i=j\\ 0& ,& i\ne j\end{array}\hspace{1em}\hspace{1em}\hspace{1em}1\le i\le n,1\le j\le n\end{array}$$

(32)

Evaluate the predicted state and the covariance matrix:

$$\begin{array}{l}{\widehat{\mathit{x}}}_{k|k-1}=\frac{1}{2n}{\displaystyle \sum _{j=1}^{2n}\left(F{\xi }_j+G\left({\xi }_j\right)u_{k-1}+c\right)}\\ P_{k|k-1}=\frac{1}{2n}{\displaystyle \sum _{j=1}^{2n}\left(F{\xi }_j+G\left({\xi }_j\right)u_{k-1}+c-{\widehat{\mathit{x}}}_{k|k-1}\right){\left(F{\xi }_j+G\left({\xi }_j\right)u_{k-1}+c-{\widehat{\mathit{x}}}_{k|k-1}\right)}^{\mathrm{T}}+Q}\end{array}$$

(33)

Evaluate the information matrix and the information vector:

$$\begin{array}{l}{Y}_{k|k-1}=P_{k|k-1}^{-1}\\ {\widehat{\mathit{y}}}_{k|k-1}=Y_{k|k-1}{\widehat{\mathit{x}}}_{k|k-1}\end{array}$$

(34)

(2) Measurement-update phase

Evaluate the cubature points based on the 5-SSR rule

$${\xi }_{k|k-1,j}^{\mathrm{m}}=S_{1,k|k-1}{\gamma }_j^{\mathrm{SSR}5}+{\widehat{\mathit{x}}}_{1,k|k-1}$$

(35)

where ${\gamma }_j^{\mathrm{SSR}5}$ is formulated as follows:

$${\gamma }_j^{\mathrm{SSR}5}=\{\begin{array}{lll}{\left[\begin{array}{ccc}0& \cdots & 0\end{array}\right]}^{\mathrm{T}}& ,& j=0\\ \sqrt{n_1+2}{c}_j& ,& j=1,2,\cdots ,n_1+1\\ -\sqrt{n_1+2}{c}_{j-\left(n_1+1\right)}& ,& j=n_1+2,\cdots ,2\left(n_1+1\right)\\ \sqrt{n_1+2}{b}_{j-2\left(n_1+1\right)}& ,& j=2n_1+3,\cdots ,\left(n_1^2+5n_1+4\right)/2\\ -\sqrt{n_1+2}{b}_{j-\left(n_1^2+5n_1+4\right)/2}& ,& j=\left(n_1^2+5n_1+6\right)/2,\cdots ,n_1^2+3n_1+2\end{array}$$

(36)

where $\left\{c_j\right\}$ are the vertexes of the $n_1$-simplex, and

$$\begin{array}{l}{c}_j={\left[c_{j,1},c_{j,2},\cdots ,c_{j,n_1}\right]}^{\mathrm{T}}\\ c_{j,i}=\{\begin{array}{lll}-\sqrt{\frac{n_1+1}{n_1\left(n_1-i+2\right)\left(n_1-i+1\right)}}& ,& i<j\\ \sqrt{\frac{\left(n_1+1\right)\left(n_1-j+1\right)}{n_1\left(n_1-j+2\right)}}& ,& i=j\\ 0& ,& i>j\end{array}\hspace{1em}\hspace{1em}1\le i\le n_1,1\le j\le n_1+1\end{array}$$

(37)

$\left\{b_i\right\}$ are the projections of the midpoints of the edges constructed by $\left\{c_j\right\}$.

$$\left\{b_i\right\}=\left\{\sqrt{\frac{n_1}{2\left(n_1-1\right)}}\left(c_l+c_m\right):l<m,1\le l\le n_1+1,1\le m\le n_1+1\right\}$$

(38)

The weights for the 5-SSR rule are as follows:

$$w_j=\{\begin{array}{lll}\frac{2}{n_1+2}& ,& j=0\\ \frac{\left(7-n_1\right)n_1^2}{2{\left(n_1+1\right)}^2{\left(n_1+2\right)}^2}& ,& j=1,\cdots ,2\left(n_1+1\right)\\ \frac{2{\left(n_1-1\right)}^2}{{\left(n_1+1\right)}^2{\left(n_1+2\right)}^2}& ,& j=2n_1+3,\cdots ,n_1^2+3n_1+2\end{array}$$

(39)

Evaluate the predicted measurement ${\widehat{\mathit{z}}}_{k|k-1}$ and the cross-covariance matrix $P_{xz}$:

$$\begin{array}{l}{\widehat{\mathit{z}}}_{k|k-1}={\displaystyle \sum _{j=1}^{n_1^2+3n_1+3}{\mathrm{w}}_{j}h\left({\xi }_{k|k-1,j}^{\mathrm{m}}\right)}\\ P_{xz}=\left[\begin{array}{c}{\displaystyle \sum _{j=1}^{n_1^2+3n_1+3}{\mathrm{w}}_j{\xi }_{k|k-1,j}^{\mathrm{m}}h{\left({\xi }_{k|k-1,j}^{\mathrm{m}}\right)}^{\mathrm{T}}}-{\widehat{\mathit{x}}}_{1,k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}\\ P_{21,k|k-1}{P}_{11,k|k-1}^{-1}\left({\displaystyle \sum _{j=1}^{n_1^2+3n_1+3}{\mathrm{w}}_j{\xi }_{k|k-1,j}^{\mathrm{m}}h{\left({\xi }_{k|k-1,j}^{\mathrm{m}}\right)}^{\mathrm{T}}}-{\widehat{\mathit{x}}}_{1,k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}\right)\end{array}\right]\end{array}$$

(40)

As the dimension of the measurement vector is generally large, the inversion of the matrix in Equation (17) is very time-consuming. To omit the inversion, we assumed that the m measurements were independent of each other, and the covariance matrix was a diagonal matrix with diagonal elements of $R_{j,k}(j=1,\dots ,m)$. Further assuming that $R_{j,k}$ was invariant with time and equal to the other diagonal elements, $R_{j,k}$ was treated as a constant $R$. Thus, the inverse of the m × m matrix $R$ was replaced by the division of the scalar $R$, and the measurement update of the information vector and matrix was obtained as follows.

$$\begin{array}{l}{\widehat{\mathit{y}}}_{k|k}={\widehat{\mathit{y}}}_{k|k-1}+R^{-1}{H}_k^{\mathrm{T}}\left(z_k-h\left({\widehat{\mathit{x}}}_{1,k|k-1}\right)+H_k{\widehat{\mathit{x}}}_{k|k-1}\right)\\ Y_{k|k}=Y_{k|k-1}+R^{-1}{H}_k^{\mathrm{T}}{H}_k\end{array}$$

(41)

Finally, the estimated state vector and covariance matrix were recovered based on Equation (18).

3.3. Robust H_∞ Filter

The aim of the H_∞ filter is to obtain the state estimation minimizing the following cost function [34].

$$J_{\infty }=\frac{{\displaystyle {\sum }_{k=0}^{N-1}{\Vert x_k-{\widehat{\mathit{x}}}_k\Vert }_{M_k}^2}}{{\Vert x_0-{\widehat{\mathit{x}}}_0\Vert }_{Y_0}^2+{\displaystyle {\sum }_{k=0}^{N-1}\left({\Vert w_k\Vert }_{Q_k}^2+{\Vert n_k\Vert }_{R_k}^2\right)}}$$

(42)

Based on game theory, the design of the H_∞ filter is a game between the filter designer and nature. Nature’s goal is to maximize the estimation error by properly choosing $x_0$, $w_k$, and $n_k$. The cost function is defined in a fractional form to create a fair game by preventing nature using extremely large noises.

The above optimization problem is difficult to solve directly. An alternative strategy is to choose an upper bound and estimate the state satisfying the following constraint.

$$J_{\infty }<{\gamma }^2$$

(43)

where $\gamma$ is the performance bound to be designed. According to the above equation, a new cost function is defined as follows:

$$J=-\frac{1}{\theta }{\Vert x_0-{\widehat{\mathit{x}}}_0\Vert }_{Y_0}^2+{\displaystyle {\sum }_{k=0}^{N-1}\left({\Vert x_k-{\widehat{\mathit{x}}}_k\Vert }_{M_k}^2-\frac{1}{\theta }\left({\Vert w_k\Vert }_{Q_k}^2+{\Vert n_k\Vert }_{R_k}^2\right)\right)}<1$$

(44)

Therefore, the original optimization problem is transformed into the following minimax problem.

$$J^{\ast }=\underset{{\widehat{\mathit{x}}}_k}{\min }\underset{x_0,w_k,n_k}{\max }J$$

(45)

Yang [29] solved the minimax problem and proposed an extended H_∞ filter (EH_∞F) for the nonlinear system, which was similar to EKF in form.

$$\begin{array}{l}{x}_{k|k-1}=f\left(x_{k-1|k-1}\right)\\ P_{k|k-1}=\nabla f{P}_{k-1|k-1}\nabla f^{\mathrm{T}}+Q\end{array}$$

(46)

$$\begin{array}{l}{K}_k=P_{k|k-1}\nabla h^{\mathrm{T}}{\left(\nabla h{P}_{k|k-1}\nabla h^{\mathrm{T}}+R_k\right)}^{-1}\\ x_{k|k}=x_{k|k-1}+K_k\left(z_k-h\left(x_{k-1|k-1}\right)\right)\\ P_{k|k}^{-1}=P_{k|k-1}^{-1}+\nabla h^{\mathrm{T}}{R}_k^{-1}\nabla h-{\gamma }^{-2}{\mathcal{I}}_n\end{array}$$

(47)

where ${\mathcal{I}}_n$ is the n-dimension identity matrix. The EH_∞F is a Jacobian-based filter, and can be extended with cubature rules for higher precision in nonlinear systems as Chandra did in [30]. The time-update phase of the cubature H_∞ filter (CH_∞F) is the same as the CKF and expressed as the equation. Therefore, we only provided the measurement-update equations as follows:

$$\begin{array}{l}{P}_{\mathrm{zz}}=H_k{P}_{k|k-1}{H}_k^{\mathrm{T}}+R_k\\ K_k=P_{\mathrm{xz}}{P}_{\mathrm{zz}}^{-1}\\ {\widehat{\mathit{x}}}_{k|k}={\widehat{x}}_{k|k-1}+K_k\left(z_k-h\left({\widehat{\mathit{x}}}_{k|k-1}\right)\right)\\ P_{k|k}^{-1}=P_{k|k-1}^{-1}+H_k^{\mathrm{T}}{R}_k^{-1}{H}_k-{\gamma }^{-2}{\mathcal{I}}_n\end{array}$$

(48)

where the cross-covariance matrix $P_{\mathrm{xz}}$ and the measurement matrix $H_k$ are evaluated using a cubature rule. It can be seen from Equations (47) and (48) that the main difference between the EH_∞F and the Kalman filter was the formula to update the covariance matrix $P_{k|k}$. In this way, the EH_∞F acts to reduce the confidence of the state estimation in case the unexpected noises negatively impact the performance. Applying the information-description of the Kalman filter on the above CH_∞F, a cubature H_∞ information filter (CH_∞IF) can be formulated as follows:

$$\begin{array}{l}{P}_{\mathrm{zz}}=H_k{P}_{k|k-1}{H}_k^{\mathrm{T}}+R_k\\ i_k=H_k^{\mathrm{T}}{R}_k^{-1}\left(z_k-h\left({\widehat{\mathit{x}}}_{k|k-1}\right)+H_k{\widehat{\mathit{x}}}_{k|k-1}\right)-{\gamma }^{-2}\left({\widehat{\mathit{x}}}_{k|k-1}+P_{\mathrm{xz}}{P}_{\mathrm{zz}}^{-1}\left(z_k-h\left({\widehat{\mathit{x}}}_{k|k-1}\right)\right)\right)\\ {\widehat{\mathit{y}}}_{k|k}={\widehat{\mathit{y}}}_{k|k-1}+i_k\\ Y_{k|k}=Y_{k|k-1}+H_k^{\mathrm{T}}{R}_k^{-1}{H}_k-{\gamma }^{-2}{\mathcal{I}}_n\end{array}$$

(49)

where the covariance matrix of measurement error vector $P_{\mathrm{zz}}$ is an m-dimension matrix. As previously stated, m is generally large in direct VO, which means that the inversion of $P_{\mathrm{zz}}$ in the above equation is not practical. We used the expressions of the gain matrix in the Kalman filter to simplify the inversion. In this way, the measurement-update phase in CH_∞IF was divided into two steps as follows:

(1) Evaluate the measurement matrix $H_k$ with a cubature rule and update the information matrix using the information filter.

$$Y_k{}^{\prime }=Y_{k|k-1}+H_k^{\mathrm{T}}{R}_k^{-1}{H}_k$$

(50)

(2) Evaluate the gain matrix $K_{\infty }$ and execute the measurement update of the H_∞ filter.

$$\begin{array}{l}{K}_{\infty }=Y_k{{}^{\prime }}^{-1}{H}_k^{\mathrm{T}}{R}_k^{-1}\\ i_k=H_k^{\mathrm{T}}{R}_k^{-1}\left(z_k-h\left({\widehat{\mathit{x}}}_{k|k-1}\right)+H_k{\widehat{\mathit{x}}}_{k|k-1}\right)-{\gamma }^{-2}\left({\widehat{\mathit{x}}}_{k|k-1}+K_{\infty }\left(z_k-h\left({\widehat{x}}_{k|k-1}\right)\right)\right)\\ {\widehat{\mathit{y}}}_{k|k}={\widehat{\mathit{y}}}_{k|k-1}+i_k\\ Y_{k|k}=Y_k{}^{\prime }-{\gamma }^{-2}{\mathcal{I}}_n\end{array}$$

(51)

It is easy to prove that the above two-step measurement-update process is equal to Equation (49). The inversion of the m-dimension matrix is replaced by the inversion of a 9-dimensional matrix $Y_k{}^{\prime }$.

It is worth noting that the measurement-update phase of the proposed CH_∞IF was different to the one with the same abbreviation that Chandra derived in [31]. Compared with Equation (49), Chandra’s method omitted the second term of $i_k$ when using the technique to analyze the estimation error in [35,36]. The error transition function of the measurement-update phase of our method is as follows:

$${\tilde{x}}_{k|k}={\tilde{\mathit{x}}}_{k|k-1}-K_k{\tilde{z}}_k$$

(52)

$$K_k=P_{k|k}\left(H_k^{\mathrm{T}}{R}_k^{-1}-{\gamma }^{-2}{K}_{\infty }\right)$$

(53)

where ${\tilde{z}}_k$ is the predicted error of the measurement $z_k$, and formulated as follows:

$${\tilde{\mathit{z}}}_k=z_k-h\left({\widehat{x}}_{k|k-1}\right)$$

(54)

While the error transition function of the measurement-update phase of Chandra’s method is related to the predicted mean value of the state vector ${\widehat{\mathit{x}}}_{k|k-1}$ as follows:

$${\tilde{x}}_{k|k}={\tilde{x}}_{k|k-1}-P_{k|k}{H}_k^{\mathrm{T}}{R}_k^{-1}{\tilde{\mathit{z}}}_k-{\gamma }^{-2}{P}_{k|k}{\widehat{\mathit{x}}}_{k|k-1}$$

(55)

This goes against the convergence of the filter. In contrast, the error transition function of our method is in the form of a weighted error function as used in [35,36], which makes a convergent filter.

3.4. Summary of MCH_∞IF-Based VIO

Combining the above methods, a mixed-order cubature H_∞ information filter (MOCH_∞IF) was designed for the nonlinear non-Gaussian VIO system. The time-update phase was the same as Equations (30)–(34). The measurement-update phase was performed in the following steps.

• Evaluate cubature points based on the 5-SSR rule with the time complexity of $O\left(N_p\right)$

  $${\xi }_{k|k-1,j}^{\mathrm{m}}=S_{1,k|k-1}{\gamma }_j^{\mathrm{SSR}5}+{\widehat{\mathit{x}}}_{1,k|k-1}$$

  (56)
• Evaluate the predicted measurements ${\widehat{\mathit{z}}}_{k|k-1}$ and the cross-covariance matrix $P_{xz}$ with the time complexity of $O\left(m{N}_p\right)$

  $$\begin{array}{l}{\widehat{\mathit{z}}}_{k|k-1}={\displaystyle \sum _{j=1}^{n_1^2+3n_1+3}{\mathrm{w}}_{j}h\left({\xi }_{k|k-1,j}^{\mathrm{m}}\right)}\\ P_{xz}=\left[\begin{array}{c}{\displaystyle \sum _{j=1}^{n_1^2+3n_1+3}{\mathrm{w}}_j{\xi }_{k|k-1,j}^{\mathrm{m}}h{\left({\xi }_{k|k-1,j}^{\mathrm{m}}\right)}^{\mathrm{T}}}-{\widehat{\mathit{x}}}_{1,k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}\\ P_{21,k|k-1}{P}_{11,k|k-1}^{-1}\left({\displaystyle \sum _{j=1}^{n_1^2+3n_1+3}{\mathrm{w}}_j{\xi }_{k|k-1,j}^{\mathrm{m}}h{\left({\xi }_{k|k-1,j}^{\mathrm{m}}\right)}^{\mathrm{T}}}-{\widehat{\mathit{x}}}_{1,k|k-1}{\widehat{\mathit{z}}}_{k|k-1}^{\mathrm{T}}\right)\end{array}\right]\end{array}$$

  (57)
• Evaluate the measurement matrix $H_k$ with the third equation in Equation (17) and update the information matrix using the information filter with the time complexity of $O\left(m\right)$

  $$Y_k{}^{\prime }=Y_{k|k-1}+R^{-1}{H}_k^{\mathrm{T}}{H}_k$$

  (58)
• Evaluate the gain matrix $K_{\infty }$ and execute the measurement update of the H_∞ filter with the time complexity of $O\left(m\right)$

  $$\begin{array}{l}{K}_{\infty }=R^{-1}{Y}_k{{}^{\prime }}^{-1}{H}_k^{\mathrm{T}}\\ i_k=R^{-1}{H}_k^{\mathrm{T}}\left(z_k-h\left({\widehat{\mathit{x}}}_{k|k-1}\right)+H_k{\widehat{\mathit{x}}}_{k|k-1}\right)-{\gamma }^{-2}\left({\widehat{\mathit{x}}}_{k|k-1}+K_{\infty }\left(z_k-h\left({\widehat{\mathit{x}}}_{k|k-1}\right)\right)\right)\\ {\widehat{\mathit{y}}}_{k|k}={\widehat{\mathit{y}}}_{k|k-1}+i_k\\ Y_{k|k}=Y_k{}^{\prime }-{\gamma }^{-2}{\mathbf{I}}_n\end{array}$$

  (59)

In conclusion, the time complexity of the measurement-update phase of MOCH_∞IF is $O\left(m{N}_p\right)$. In the case of $n_1=6$, we have $N_p=57$. Generally, the number of measurements is much larger than that i.e., $m\gg N_p$. Thus, the above measurement-update phase is a linear algorithm. In addition, the process of the measurement-update phase is highly parallelizable except for the Cholesky decomposition and the inverse of $Y_k{}^{\prime }$ as it mainly consists of matrix additions and multiplications.

4. Evaluation Results

As analyzed in the previous section, the measurement-update phase with operations of large matrixes is the bottleneck for real-time applications. However, the measurement-update phase of an information-based filter is parallelizable and can be implemented on a parallel processor. We used a laptop with the Geforce 940M GPU (NVIDIA, Santa Clara, CA, USA) and the i7-5600U CPU (Intel, Santa Clara, CA, USA) to test the proposed MOCH_∞IF-VIO system. We implemented our method with CUDA (in Supplementary Materials) (Version 8.0, NVIDIA, Santa Clara, CA, USA, 2016) language. The time-update phase was executed on the CPU and the measurement-update phase was executed on the GPU. We used the Technical University of Munich (TUM) RGBD dataset [37] for evaluation. The TUM RGBD dataset contains multiple sequences of videos collected using an RGBD camera under different environmental conditions. The RGB images and depth images are collected at the rate of 30 Hz with a resolution of 640 × 480. The TUM dataset does not contain IMU measurements, we simply used the differential and quadratic differential of the ground truth with the addition of random noises as the simulated IMU measurements.

We compared the proposed MCH_∞IF-VIO method with RGBDSLAM v2.0 in [38] and the lightweight visual tracking (LVT) in [12]. The results of RGBDSLAM were calculated with the corresponding ROS package, and the results of the LVT were directly obtained from the original paper. We conducted the evaluation for MCH_∞IF-VIO with five different patch sizes: 2, 4, 8, 16, and 32. The numbers were selected as powers of two for the convenience of CUDA implementation. The absolute trajectory (ATE) and relative pose error (RPE) defined in [37] were used as error metrics. The ATE measures the global consistency of the estimated trajectory and is suitable for visual SLAM methods. On the other hand, the RPE measures the local accuracy of the trajectory over a fixed time interval and is suited for visual odometry methods. We used five sequences in the TUM dataset for testing the three methods. The basic parameters of the five sequences are shown in

Table 3. Parameters of the used sequences.

Sequence	Duration (s)	Trajectory length (m)
fr1_desk	23.40	9.263
fr1_desk2	24.86	10.161
fr1_room	48.90	15.989
fr1_xyz	30.09	7.112
f3_office	87.09	21.455

. The test results are shown in Figure 3, Figure 4, Figure 5, Figure 6 and Figure 7. In these figures, MCH_∞IF-VIO(n) represents the MCH_∞IF-VIO with a patch size of n.

The RPE of MCH_∞IF-VIO increased along with the patch size in all of the tests. The change in the ATE of MCH_∞IF-VIO was similar to RPE with some exceptions. This is in line with the common knowledge that more measurements lead to better accuracy in visual navigation. The MCH_∞IF-VIO in this paper and LVT are dead-reckoning algorithms, while RGBDSLAM is a compete SLAM algorithm with joint optimization and loop-closure. Therefore, RGBDSLAM is more consistent globally than MCH_∞IF-VIO and LVT according to the ATE results. In our tests, MCH_∞IF-VIO achieved a smaller ATE than LVT with only one exception, where the ATE of MCH_∞IF-VIO(32) was larger than LVT in the test on fr1_room. MCH_∞IF-VIO(2) had the smallest RPE in all of the tests. The estimation errors caused by the IMU measurement noises increased over time. Moreover, the simple frame-to-frame alignment used in the measurement-update phase was less robust than the optimization with the global and local map used in RGBDSLAM and LVT. Therefore, the performance of MCH_∞IF-VIO in the test with long duration such as fr3_office was less distinctive.

5. Conclusions

In this paper, we presented our visual–inertial navigation system called MCH_∞IF-VIO, which uses a raw intensity-based measurement model. By analyzing the integrated navigation system, we constructed a numerically stable mixed-degree cubature information filter scheme for the state estimation problem. The H_∞ filter was combined for the non-Gaussian noises in the intensity measurements. The proposed VIO system was suitable for the RGBD camera–IMU system. The system was evaluated on the TUM RGBD dataset and compared with the RGBDSLAM and LVT systems.

Given the raw visual measurement model with intensities directly used as measurements in frame-to-frame alignment, the proposed MCH_∞IF-VIO system with consideration of the system nonlinearity and non-Gaussian measurement noises was shown to achieve good performance in our tests. Though the method prefers short durations, the implementation with a patch size of two outperformed the RGBDSLAM and LVT in the long-duration test. With the aid of adjustable patch size, the proposed method could be tuned from an accurate-dense algorithm to a fast-sparse algorithm.

This paper aimed to build a robust state-estimation framework according to the fundamental characteristics of the visual–inertial navigation system. The camera measurements were used in a raw and direct way without heuristics in selecting the feature descriptors or tracking a sliding window. Other well-designed intensity-based and feature-based measurement models can be applied in this hybrid filter-based framework.