Robust Outlier-Adaptive Filtering for Vision-Aided Inertial Navigation

Abstract

With the advent of unmanned aerial vehicles (UAVs), a major area of interest in the research field of UAVs has been vision-aided inertial navigation systems (V-INS). In the front-end of V-INS, image processing extracts information about the surrounding environment and determines features or points of interest. With the extracted vision data and inertial measurement unit (IMU) dead reckoning, the most widely used algorithm for estimating vehicle and feature states in the back-end of V-INS is an extended Kalman filter (EKF). An important assumption of the EKF is Gaussian white noise. In fact, measurement outliers that arise in various realistic conditions are often non-Gaussian. A lack of compensation for unknown noise parameters often leads to a serious impact on the reliability and robustness of these navigation systems. To compensate for uncertainties of the outliers, we require modified versions of the estimator or the incorporation of other techniques into the filter. The main purpose of this paper is to develop accurate and robust V-INS for UAVs, in particular, those for situations pertaining to such unknown outliers. Feature correspondence in image processing front-end rejects vision outliers, and then a statistic test in filtering back-end detects the remaining outliers of the vision data. For frequent outliers occurrence, variational approximation for Bayesian inference derives a way to compute the optimal noise precision matrices of the measurement outliers. The overall process of outlier removal and adaptation is referred to here as “outlier-adaptive filtering”. Even though almost all approaches of V-INS remove outliers by some method, few researchers have treated outlier adaptation in V-INS in much detail. Here, results from flight datasets validate the improved accuracy of V-INS employing the proposed outlier-adaptive filtering framework.

1. Introduction

The most widely used algorithms for estimating the states of a dynamic system are a Kalman Filter [1,2] and its nonlinear versions such as an extended Kalman filter (EKF) [3,4]. After the NASA Ames Research Center implemented the Kalman filter into navigation computers to estimate the trajectory of the Apollo program, engineers have developed a myriad of applications of the Kalman filter in navigation system research areas [5]. For example, Magree and Johnson [6] developed a simultaneous localization and mapping (SLAM) architecture with improved numerical stability based on the UD factored EKF, and Song et al. [7] proposed a new EKF system that loosely fuses both absolute state measurements and relative state measurements. Furthermore, Mostafa et al. [8] integrated radar odometry and visual odometry via EKF to help overcome their limitations in navigation. Despite the development of numerous applications of the Kalman filter in various fields, it suffers from inaccurate estimation when required assumptions fail.

Estimation using a Kalman filter is optimal when process and measurement noise are Gaussian. However, sensor measurements are often corrupted by unmodeled non-Gaussian or heavy-tailed noise. An abnormal value relative to an overall pattern of the nominal Gaussian noise distribution is called an outlier. In other words, in statistics, an outlier is an observation that deviates so much from other observations as to arouse suspicion that it is generated by a different mechanism [9]. Such outliers have many anomalous causes. They arise due to unanticipated changes in system behavior (e.g., temporary sensor failure or transient environmental disturbance) or unmodeled factors (e.g., human errors or unknown characteristics of intrinsic noise). As an example of measurement outliers in many navigation systems, either computer vision data contaminated by outliers or sonar data corrupted by phase noise lead to erroneous measurements. Process outliers also occur by chance. Inertial measurement unit (IMU) dead reckoning and wheel odometry as a proxy often generate inaccurate dynamic models in visual-inertial odometry (VIO) and SLAM algorithms, respectively. Without accounting for outliers, the accuracy of the estimator significantly degrades, and control systems that rely on high-quality estimation can diverge.

1.1. Related Work

1.1.1. State Estimation for Measurements with Outliers

As the performance of the Kalman filter degrades at the presence of measurement outliers, many researchers have investigated other approaches to mitigate the impact of the outliers. Mehra [10] created adaptive filtering with the identification of noise covariance matrices and showed the asymptotic convergence of the estimates towards their true values. Maybeck [11] and Stengel [12] found other noise-adaptive filtering such as covariance matching. However, all of these filters performed only offline and required filter tuning. To estimate parameter values in unknown covariances without the need for manual parameter tuning, Ting et al. [13] used a variational expectation–maximization (EM) framework. That is, they introduced a scalar weight for each observed data sample and modeled the weights to be Gamma distributed random variables. However, it assumed that noise characteristics are homogeneous across all measurements even though sensors have distinct properties. Särkkä and Nummenmaa [14] provided the online learning of the parameters of the measurement noise variance, but to simultaneously track the system states and the levels of sensor noise, they additionally defined a heuristic transition model for the noise parameters. Piché et al. [15] developed Gaussian assumed density filtering and smoothing framework for nonlinear systems using the multivariate Student t-distribution, and Roth et al. [16] included an approximation step for heavy-tailed process noise, but these filters are not applicable in high dimensions. Next, Solin and Särkkä [17] found that the added flexibility of Student-t processes over Gaussian processes robustifies inference in outlier-contaminated noisy data, but they treated only analytic solutions enabled by the noise entanglement.

Recently, Agamennoni et al. developed the outlier-robust Kalman filter (ORKF) [18,19] to obtain the optimal precision matrices of measurement outliers by variational approximation for Bayesian inference [20]. However, this method requires iterations at every time, even when observed data contain no outliers. Graham et al. also established the ${\mathcal{l}}_1$-norm filter [21] for both types of sparse outliers. However, the filter might not work for nonlinear systems since they derived the constraint of ${\mathcal{l}}_1$-norm optimization based on only linear system equations. Similar to the ORKF, the ${\mathcal{l}}_1$-norm filter needs the constrained optimization at all times, even when no additional noise present as outliers. Therefore, these two approaches demand some extensive computational complexity for either iterations or optimization. As outliers do not always arise (i.e., are rare), we reduce such computation cost if a test detects the time when outliers occur. All of the above methods were not validated for complicated systems such as unmanned aerial vehicles or vision-aided inertial navigation [6,22,23] or with sequential measurement updates [24,25].

1.1.2. Outlier Rejection Techniques

One of the primary problems in vision-aided inertial navigation systems (V-INS) is incorrect vision data correspondence or association. Matched features between two different camera views are corrupted by outliers because of image noise, occlusions, and illumination changes that are not modeled by the feature matching techniques. To provide cleaned measurement data to the filter, outlier removal in image processing front-end is essential. One of standard outlier rejection techniques is RANdom SAmple Consensus (RANSAC) [26]. RANSAC is an iterative approach to estimate the parameters of a mathematical model from a set of observed data contaminated by outliers. An underlying assumption is that the data consist of inliers whose distribution is described by some set of the parameters of the model and outliers that do not fit the model. The generated parameters are then verified on the remaining subset of the data, and the model with the highest consensus is a selected solution. In particular, 2-point RANSAC [27,28] is an extended RANSAC-based method for two consecutive views of a camera rigidly mounted on a vehicle platform. Given gyroscopic data from IMU measurements, randomly selected two-feature correspondences hypothesize an ego-motion of the vehicle. This motion constraint discards wrong data associations in the feature matching processes.

For detecting remaining outliers that are not rejected in the image processing front-end, outlier detection tests are required in filtering back-end. Most of the statistical tests [29] that require access to the entire set of data samples for detecting outliers might not be a viable option in real-time applications. For example, the typicality and eccentricity data analysis [30,31] used in [32] is an inadequate measure in V-INS, as computing the means and the variances of each residual of sequential measurements is challenging. In V-INS, the tracking of some measured features is possibly lost due to out of sight, and new feature measurements are coming for initialization.

For the real-time outlier detection of sequential measurements in V-INS, the Mahalanobis gating test [33] is a useful measure based on the analysis of residual and covariance signals at each feature measurement. The approach builds upon each Mahalanobis distance [34] of residuals and compares each value against a threshold given by the quantile of the chi-squared distribution with a certain degree of freedom. The confidence level of the threshold is designated prior to examining the data. Most commonly, the 95% confidence level is used. This hypothesis testing, called goodness of fit, is a commonly used outlier detection method in practice. Because of such suitability of the Mahalanobis gating test to real-time detection in V-INS, this paper combines the test with the ORKF [18,32] to detect and handle measurement outliers in vision-aided estimation problems. Similar to the derivation of update steps for handling measurement outliers in the ORKF, for computing the optimal precision matrices of unmodeled outliers in V-INS, Section 4 will derive feasible update procedures by variational inference. In other words, whenever unexpected outliers appear, the outlier-adaptive filtering in this paper updates and marginalizes measurement outliers to improve the robustness of the navigation systems.

1.2. Summary of Contributions

This paper presents improving the use of outlier removal techniques in image processing front-end and the development of a robust and adaptive state estimation framework for V-INS when frequent outliers occur. For outlier removal in the image processing front-end of V-INS, feature correspondence constitutes the following three steps: tracking, stereo matching, and 2-point RANSAC. To estimate the states of V-INS in which vision measurements still contain remaining outliers, we propose a novel approach that combines a real-time outlier detection technique with an extended version of the ORKF in the filtering back-end of V-INS. Therefore, our approach does not restrict noise at either a constant or Gaussian level in filtering. The testing results of benchmark flight datasets show that our approach leads to greater improvement in accuracy and robustness under severe illumination environments.

Starting from the architecture of the existing vision-aided inertial navigation system, this paper more focuses on contributing to the development of red boxes in Figure 3.

1.3. A Guide to This Document

The remainder of this document contains the following sections. Section 2 introduces background for all of this study. To estimate the states of V-INS in which frequent outliers arise, Section 3 examines outlier rejection techniques in image processing front-end, and Section 4 and Section 5 formulate a novel implementation of robust outlier-adaptive filtering. Section 6 shows testing results of this study on benchmark flight datasets. The last section concludes and plans future work.

2. Preliminaries

2.1. The Extended Kalman Filter

The system equations with continuous-time dynamics and a discrete-time sensor are as follows,

$$\begin{array}{cc}\hfill \dot{x}\left(t\right)& =f\left(x\left(t\right),\eta \left(t\right)\right)\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill y\left(t_k\right)& =h\left(x\left(t_k\right)\right)+\zeta \left(t_k\right),\hfill \end{array}$$

(2)

where $x\in {\mathbb{R}}^n$ is the state and $y\in {\mathbb{R}}^m$ a measurement. $f(·)$ and $h(·)$ are the nonlinear dynamic and measurement functions, respectively. Let us assume that these functions are known based on each equation of motion and modeling. To clarify, t denotes continuous time, subscript k represents the k-th time step, and initial condition $x\left(0\right)=x_0$ is given. Moreover, let us assume that both propagation and measurements are corrupted by additive zero-mean white Gaussian noise; that is, $\eta \left(t\right)\sim \mathcal{N}(0,Q)$ and $\zeta \left(t_k\right)\sim \mathcal{N}(0,R)$.

2.1.1. Time Update

To estimate the state variables of the system, we design a hybrid EKF in the following steps. In the propagation step, state estimate $\widehat{x}:=\mathbb{E}\left[x\right]$ and its error-covariance $P:=\mathbb{E}\left[(x-\widehat{x}){(x-\widehat{x})}^{\mathrm{T}}\right]$ are integrated from time ${(k-1)}^{+}$ to time $k^{-}$ with respect to variable $\tau$

$${\widehat{x}}_k^{-}={\widehat{x}}_{k-1}^{+}+{\int }_{t_{k-1}}^{t_k}f\left(\widehat{x}\left(\tau \right)\right)d\tau ,$$

where let ${\widehat{x}}_k=\widehat{x}\left(t_k\right)$. Hat “ $\widehat{}$ ” denotes an estimate, and superscripts − and + represent a priori and a posteriori estimates, respectively. Here, for one numerical solution of the ordinary differential equation, we use the Heun’s method [35] that refers to the improved Euler’s method or a similar two-stage Runge–Kutta method. Jacobian A, B, and state transition matrix $\mathsf{\Phi }$ are defined by

$$\begin{array}{cc}\hfill A_{k-1}& =\frac{\partial f\left(x\right)}{\partial x}{|}_{{\widehat{x}}_{k-1}^{+}},B_{k-1}=\frac{\partial f\left(x\right)}{\partial \eta }{|}_{{\widehat{x}}_{k-1}^{+}}\hfill \\ \hfill {\mathsf{\Phi }}_{k-1}& =\exp \left(A_{k-1}\mathsf{\Delta }{t}_{k-1}\right)\approx I+A_{k-1}\mathsf{\Delta }{t}_{k-1},\hfill \end{array}$$

(3)

where $\mathsf{\Delta }{t}_{k-1}=t_k-t_{k-1}$. Letting $P_k=P\left(t_k\right)$, the time update of error covariance is

$$P_k^{-}={\mathsf{\Phi }}_{k-1}{P}_{k-1}^{+}{\mathsf{\Phi }}_{k-1}^{\mathrm{T}}+B_{k-1}Q{B}_{k-1}^{\mathrm{T}}\mathsf{\Delta }{t}_{k-1}.$$

(4)

2.1.2. Measurement Update

Using actual sensor measurements, the measurement update step of the EKF corrects state estimates and its corresponding error covariance after propagation. Letting $y_k=y\left(t_k\right)$, at every time k when $y_k$ is measured,

$$\begin{array}{cc}\hfill K_k& =P_k^{-}{C}_k^{\mathrm{T}}{(C_k{P}_k^{-}{C}_k^{\mathrm{T}}+R)}^{-1}\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill {\widehat{x}}_k^{+}& ={\widehat{x}}_k^{-}+K_k(y_k-h\left({\widehat{x}}_k^{-}\right))\hfill \end{array}$$

(6)

$$\begin{array}{cc}\hfill P_k^{+}& =P_k^{-}-K_k{C}_k{P}_k^{-},\hfill \end{array}$$

(7)

where K is called the Kalman gain and Jacobian C is defined as

$$C_k=\frac{\partial h\left(x\right)}{\partial x}{|}_{{\widehat{x}}_k^{-}}.$$

Equation (7) is the Joseph’s form [36] of the covariance measurement update, so this form preserves its symmetry and positive definite. For more details such as optimality and derivation, see References [24,37].

2.1.3. Sequential Measurement Update

When myriad measurements are observed at one time, sequential Kalman filtering is useful. In fact, we obtain N measurements, $y_1,y_2,\cdots ,y_N$, at time k; that is, we first measure $y_1$, then $y_2$, ⋯, and finally $y_N$, shown in Figure 1.

We first initialize a posteriori estimate and covariance after zero measurement is processed; that is, they are equal to the a priori estimate and covariance. For $i=1,\cdots ,N$, perform the general measurement update using the i-th measurement. We lastly assign the a posteriori estimate and covariance as ${\widehat{x}}_k^{+}\leftarrow {\left({\widehat{x}}_k\right)}_N$ and $P_k^{+}\leftarrow {\left(P_k\right)}_N$. Based on Simon [24]’s proof that the sequential Kalman filtering is equivalent formulation of the standard EKF, the order of updates does not affect overall performance of estimation.

2.2. Models of Vision-Aided Inertial Navigation

2.2.1. Vehicle Model

The nonlinear dynamics of a vehicle is driven by raw micro-electromechanical system (MEMS) IMU sensor data including specific force and angular velocity inputs. The estimated vehicle state is given by

$${\widehat{x}}_V={\left[\begin{array}{ccccc}{}^i{\widehat{p}}_{b/i}^{\mathrm{T}}& {}^i{\widehat{v}}_{b/i}^{\mathrm{T}}& {\delta \widehat{\theta }}^{\mathrm{T}}& {\widehat{b}}_a^{\mathrm{T}}& {\widehat{b}}_{\omega }^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}},$$

(8)

where $p_{b/i}$, $v_{b/i}$ are the position and velocity of the vehicle with respect to the inertial frame, respectively. $\delta \theta$ is the error quaternion of the attitude of the vehicle, and its more details are explained in [38,39,40]. $b_a$ and $b_{\omega }$ are the acceleration and gyroscope biases of the IMU, respectively. Left superscript i denotes a vector expressed in the inertial frame. The EKF propagates the vehicle state vector by dead reckoning with data from the IMU. Raw MEMS IMU sensor measurements $a_{\mathrm{raw}}$ and ${\omega }_{\mathrm{raw}}$ are corrupted by noise and bias as follows,

$$\begin{array}{cc}\hfill a_{\mathrm{raw}}& =a_{\mathrm{true}}-{\mathcal{T}}_{b/i}{}^{i}g+b_a+{\eta }_a\hfill \\ \hfill {\omega }_{\mathrm{raw}}& ={\omega }_{\mathrm{true}}+b_{\omega }+{\eta }_{\omega }\hfill \end{array}$$

$$\begin{array}{cc}\hfill {\dot{b}}_a& ={\eta }_{b_a}\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill {\dot{b}}_{\omega }& ={\eta }_{b_{\omega }},\hfill \end{array}$$

(10)

where $a_{\mathrm{true}}$ and ${\omega }_{\mathrm{true}}$ are the true acceleration and angular rate, respectively, and g is the gravitational acceleration in the inertial frame. ${\eta }_a$ and ${\eta }_{\omega }$ are zero-mean, white, Gaussian noise of the accelerometer and gyroscope measurement, and ${\mathcal{T}}_{b/i}={{\mathcal{T}}_{i/b}}^{\mathrm{T}}$ denotes the rotation matrix from the inertial frame to the body frame.

${\eta }_{b_a}$ and ${\eta }_{b_{\omega }}$ in Equations (9) and (10) are the random walk rate of the acceleration and gyroscope biases. From the works in [40,41,42], the MEMS accelerometer and gyroscope are subject to flicker noise in the electronics and other components susceptible to random flickering. The flicker noise causes their biases to wander over time. Such bias fluctuations are usually modeled as a random walk. In other words, slow variations in the bias of the IMU sensor are modeled with a “Brownian motion” process, also termed a random walk in discrete-time. In practice, the biases are constrained to be within some range, and thus the random walk model is only a good approximation to the true process for short periods of time.

The vehicle dynamics is given by

$$\begin{array}{cc}\hfill {}^i{\dot{\widehat{p}}}_{b/i}& ={}^i{\widehat{v}}_{b/i}\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill {}^i{\dot{\widehat{v}}}_{b/i}& ={\widehat{\mathcal{T}}}_{i/b}(a_{\mathrm{raw}}-{\widehat{b}}_a)+{}^{i}g\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill {\dot{\widehat{q}}}_{i/b^{\prime }}& =\frac{1}{2}\mathcal{Q}({\omega }_{\mathrm{raw}}-{\widehat{b}}_{\omega }){\widehat{q}}_{i/b^{\prime }}\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill \delta \dot{\widehat{\theta }}& =-⌈({\omega }_{\mathrm{raw}}-{\widehat{b}}_{\omega })\times ⌋\delta \widehat{\theta }\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill {\dot{\widehat{b}}}_a& =0\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill {\dot{\widehat{b}}}_{\omega }& =0,\hfill \end{array}$$

(16)

where $⌈\alpha \times ⌋$ is a skew symmetric matrix, and function $\mathcal{Q}(·)$ maps a 3 by 1 vector of the angular velocity into a 4 by 4 matrix [25]. The use of the 4 by 1 quaternion representation in state estimation causes the covariance matrix to become singular, so it requires considerable accounting for the quaternion constraints. To avoid these difficulties, engineers developed the error-state Kalman filter in which 3 by 1 infinitesimal error quaternion $\delta \theta$ is used instead of 4 by 1 quaternion q in the state vector. In other words, we use attitude error quaternion to express the incremental difference between tracked reference body frame $b^{\prime }$ and actual body frame b for the vehicle. Jacobian matrix $A=\frac{\partial \dot{x}}{\partial x}{|}_{\widehat{x}}$ and $B=\frac{\partial \dot{x}}{\partial \eta },\mathrm{where}\eta ={[{\eta }_a^{\mathrm{T}},{\eta }_{\omega }^{\mathrm{T}},{\eta }_{b_a}^{\mathrm{T}},{\eta }_{b_{\omega }}^{\mathrm{T}}]}^{\mathrm{T}},$ are computed in Appendix A.

2.2.2. Camera Model

An intrinsically calibrated pinhole camera model [27,43] is given by

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{u}_j\\ v_j\end{array}\right]=y_j& =h_j\left(x\right)+{\zeta }_j=\left[\begin{array}{c}{f}_u\frac{{}^c{X}_j}{{}^c{Z}_j}+{\zeta }_{u_j}\\ f_v\frac{{}^c{Y}_j}{{}^c{Z}_j}+{\zeta }_{v_j}\end{array}\right]\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill {\left[\begin{array}{ccc}{}^c{X}_j,& {}^c{Y}_j,& {}^c{Z}_j\end{array}\right]}^{\mathrm{T}}={}^c{p}_{f_j/c}& ={\mathcal{T}}_{c/i}\left({}^i{p}_{f_j/i}-{}^i{p}_{c/i}\right)\hfill \\ & \hfill ={\mathcal{T}}_{c/b}\mathcal{T}\left(q_{i/b}\right)\left({}^i{p}_{f_j/i}-{}^i{p}_{b/i}\right)-{\mathcal{T}}_{c/b}{}^b{p}_{c/b},\end{array}$$

(18)

where x is the state vector including the vehicle state and the feature state, and measurement $y_j$ is the j-th feature 2D location on the image plane. $f_u$ and $f_v$ are the horizontal and vertical focal lengths, respectively, and ${\zeta }_u$ and ${\zeta }_v$ are additive, zero-mean, white, Gaussian noise of the measurement. Vectors $p_{f_j/c}$, $p_{f_j/i}$ are the j-th feature 3D position with respect to the camera frame and the inertial frame, respectively. Extrinsic parameter ${\mathcal{T}}_{c/b}$ and ${}^b{p}_{c/b}$ are known and constant. Jacobian matrix $C_j=\frac{\partial y_j}{\partial x}{|}_{\widehat{x}}$ is computed in Appendix A. In addition, from Equation (19), if j-th measurement $y_j$ on an image is a new feature, then ${}^i{p}_{f_j/i}$ is unknown so need to be initialized. Details of feature initialization are explained in Appendix B.

3. Outlier Rejection in Image Processing Front-End

3.1. Feature Correspondence

In this study, a feature detector using the Features from Accelerated Segment Test (FAST) algorithm [44,45] maintains a minimum number of features in each image. For each new image, a feature extractor using the Kanade–Lucas–Tomasi (KLT) sparse optical flow algorithm [46] tracks the existing features. Even though Paul et al. [47] proved that descriptor-based methods for temporal feature tracking are more accurate than KLT-based methods, as Sun et al. [48] found that descriptor-based methods require much more computing resource with a small gain in accuracy, we employ the KLT optical flow algorithm in the image processing front-end of this study. Next, our stereo matching using a fixed baseline stereo configuration also applies to the KLT optical flow algorithm for saving computational loads compared to other stereo matching approaches. With the matched features, a 2-point RANSAC [26] is applied to remove remaining outliers by utilizing the RANSAC step in the fundamental matrix test [27]. In the scope of this study, we implement the 2-point RANSAC algorithm by simply running one of open source codes.

Similar to [48,49], our outlier rejection is composed of three steps, shown in Figure 2. We assume that features from previous $c_1$ and $c_2$ images are outlier-rejected points, where $c_1$ and $c_2$ are left and right camera frames of a stereo camera, respectively. The three steps form a closed loop of previous and current frames of left and right cameras. The first step is the stereo matching of tracked features on current $c_1$ image to $c_2$ image. The next steps are applying 2-point RANSAC between previous and current images of the left camera and another 2-point RANSAC between previous and current images of the right camera. For steps 2 and 3, stereo-matched features are directly used in each RANSAC.

3.2. Algorithm of Feature Correspondence

Algorithm 1 summarizes the feature correspondence for outlier rejection. For the scope of this paper, the OpenCV library [50] and open source codes of RANSAC are extremely useful and directly applied.

Algorithm 1 Feature Correspondence for Outlier Rejection

Require: Pyramids and outlier-rejected points of previous $c_1$, $c_2$ images

1: Feature Tracking: 2: functionbuildOpticalFlowPyramid(current $c_1$ or $c_2$ image)                           ▹ OpenCV [50] 3:  return pyramid of current $c_1$ or $c_2$ 4: end function 5: functionpredictFeatures(outlier-rejected points of previous $c_1$, ${\widehat{\mathcal{T}}}_{\mathrm{curr}\leftarrow \mathrm{prev}}$ of $c_1$, Intrinsic $c_1$) 6:  return predicted features of current $c_1$ 7: end function 8: functioncalcOpticalFlowPyrLK(pyramids of previous and current $c_1$, outlier-rejected points of previous $c_1$, predicted features of current $c_1$)                                            ▹ OpenCV [50] 9:  return tracked points of previous $c_1$ and $c_2$, tracked features of current $c_1$ 10: end function 11: Stereo Matching: 12: functionstereoMatching(tracked points of previous $c_1$ and $c_2$, tracked features of current $c_1$) 13:  Initialize $c_2$ points by projecting the tracked features of current $c_1$ to $c_2$ using the rotation from stereo extrinsic 14:  function calcOpticalFlowPyrLK(pyramid of current $c_1$ and $c_2$, tracked features of current $c_1$, initialized $c_2$ points)                                                    ▹ OpenCV [50] 15:  end function 16:  Further remove outliers based on the essential matrix 17:  return matched points of previous $c_1$ and $c_2$, matched features of current $c_1$ and $c_2$ 18: end function 19: 2-Point RANSAC: 20: functiontwoPointRansac(matched points of previous $c_1$ or $c_2$, matched features of current $c_1$ or $c_2$, ${\widehat{\mathcal{T}}}_{\mathrm{curr}\leftarrow \mathrm{prev}}$ of $c_1$ or $c_2$, Intrinsic of $c_1$ or $c_2$) 21:  return outlier-rejected points of current $c_1$ or $c_2$ 22: end function 23: Addition of Newly Detected Features: 24: Create a mask to avoid re-detecting existing features 25: functionfastFeatureDetector(current $c_1$ image, mask) 26:  return new features on current $c_1$ 27: end function 28: functionstereoMatching(new features on current $c_1$) 29:  return matched new features on current $c_2$ 30: end function 31: Group all of outlier-rejected features

In Algorithm 1, Pyramid is a type of multi-scale signal representation in which an image is subject to repeated smoothing and sub-sampling.

4. Outlier Adaptation in Filtering Back-End

Even though image processing front-end removes outliers by tracking, stereo matching, and 2-point RANSAC, some outlier features still survive and enter the filter as inputs. This section explains the outlier rejection procedure in filtering back-end.

4.1. Outlier Removal in Feature Initialization

If a measurement is a new feature, our system initializes its 3D position with respect to the inertial frame. In feature initialization, Gaussian–Newton least-squares minimization first estimates the depth of left $c_1$ camera. If either the estimated depth of the left or right camera is negative, then the solution of the minimization is invalid since features are always in front of both camera frames observing them. This process of removing features that has the invalid depth is referred to as outlier removal in feature initialization.

4.2. Outlier Detection by Chi-Squared Statistical Test

Before operating navigation systems, we initialize the chi-squared test table with a 95% confidence level. While the systems estimate the state variable, if j-th measurement $y_j$ at time k is the existing feature, then its residual $r_j$ and Jacobian $C_j$ are computed. Next, we proceed a Mahalanobis gating test [33] for residual $r_j$ to detect remaining outliers. In fact, Mahalanobis distance [34] ${\gamma }_j$ is a measure of the distance between residual $r_j$ and covariance matrix $S_j=C_j{\left(P_k\right)}_{j-1}{C}_j^{\mathrm{T}}+R$

$${\gamma }_j=r_j^{\mathrm{T}}{\left(C_j{\left(P_k\right)}_{j-1}{C}_j^{\mathrm{T}}+R\right)}^{-1}{r}_j.$$

(19)

In the statistic test, we compare ${\gamma }_j$ value against a threshold given by the 95-th percentile of the ${\chi }^2$ distribution with ${\nu }_j$ degree of freedom. Here, ${\nu }_j$ is the number of observations of the j-th feature minus one. If the feature passes the test, the EKF uses residual $r_j$ to process the measurement update.

4.3. Outlier-Adaptive Filtering

Unlike the extended ORKF (EORKF) [32], for a practical estimation approach in V-INS, this study investigates only measurement outliers due to the following reasons. As the measurement update is not the process performed at every time step, the outlier detection by each residual value cannot directly detect the outliers of IMU measurements. Furthermore, in the sequential measurement update, multiple residuals are computed to update at one IMU time stamp. In other words, as only rare observations among feature measurements from one image are corrupted by the remaining outliers, hypothesizing that the outliers come from the IMU may be faulty. Therefore, in the scope of this paper, we handle only measurement outliers.

4.3.1. Student’s t-Distribution

Despite the true system with outliers, the classical EKF assumes that each model in the filter is corrupted with additive white Gaussian noise. The levels of the noise are assumed to be constant and encoded by sensor covariance matrices Q and R (i.e., ${\eta }_k\sim \mathcal{N}(0,Q),{\left({\zeta }_j\right)}_k\sim \mathcal{N}(0,R)$). However, as outliers arise in the realistic system, now we do not restrict noise at either a constant or Gaussian level. Instead, their levels vary over time, or noise have heavier tails than the normal distribution as follows,

$${\zeta }_j{|}_k\sim \mathrm{ST}(0,{\tilde{R}}_j,{\nu }_j),\mathrm{where}{\tilde{R}}_j\sim {\mathcal{W}}^{-1}\left({\nu }_j{\Lambda }_j,{\nu }_j\right),$$

(20)

where $\mathrm{ST}(·)$ denotes a Student’s t-distribution, and ${\nu }_k>m-1$ is degrees of freedom. Covariance matrix ${\tilde{R}}_j$ follows the inverse-Wishart distribution, denoted as ${\mathcal{W}}^{-1}(·)$. ${\Lambda }_j\succ 0$ is $m\times m$ precision matrix.

In Bayesian statistics, the inverse-Wishart distribution is used as the conjugate prior for the covariance matrix of a multivariate normal distribution [20]. The probability density function (pdf) of the inverse-Wishart is

$$\begin{array}{cc}\hfill p\left({\tilde{R}}_j\right|{\nu }_j,{\Lambda }_j)& \propto |{\tilde{R}}_j{|}^{-\frac{{\nu }_j+m+1}{2}}exp\left[-\frac{{\nu }_j}{2}\mathrm{tr}\left({\Lambda }_j{\tilde{R}}_j^{-1}\right)\right],\hfill \end{array}$$

(21)

where $\mathrm{tr}(·)$ denotes the trace of a square matrix in linear algebra. Moreover, in probability and statistics, a Student’s t-distribution is any member of a family of continuous probability distributions that arises when estimating the mean of a normally distributed population in situations where the standard deviation of the population is unknown [51]. Whereas a normal distribution describes a full population, a t-distribution describes samples drawn from a full population; thus, the larger the sample, the more the distribution resembles a normal distribution. Indeed, as the degree of freedom goes to infinity, the t-distribution approaches the standard normal distribution. In other words, when the variance of a normally distributed random variable is unknown and a conjugate prior placed over it that follows an inverse-Wishart distribution, the resulting marginal distribution of the variable follows a Student’s t-distribution [52]. Then, the Student-t, a sub-exponential distribution with much heavier tails than the Gaussian, is more prone to producing outlying values that fall far from its mean.

4.3.2. Variational Inference

The purpose of filtering is generally to find the approximations of posterior distributions $p\left(x_k\right|y_{1:k})$, where $y_{1:k}=[y_1,y_2,\cdots ,y_k]$ is the histories of sensor measurements obtained up to time k. For systems with the heavy-tailed noise, we also wish to produce another inference about covariance matrices whose priors follow the inverse-Wishart distribution. Hence, our goal in this section is to find both approximations for posterior distribution $p(x_{1:k},{\tilde{R}}_{1:k}|y_{1:k})$ and model evidence $p\left(y_{1:k}\right)$. Compared to sampling methods, the variational Bayesian method performs approximate posterior inference at low computational cost for a wide range of models [20,52]. In the method, we decompose log marginal probability

$$lnp\left(y_{1:k}\right)=\mathrm{KL}\left[q\parallel p\right]+\mathcal{L}\left[q\right],$$

(22)

where

$$\begin{array}{cc}\hfill \mathrm{KL}\left[q\parallel p\right]& =\int q(x_{1:k},{\tilde{R}}_{1:k})ln\frac{q(x_{1:k},{\tilde{R}}_{1:k})}{p(x_{1:k},{\tilde{R}}_{1:k}|y_{1:k})}\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill \mathcal{L}\left[q\right]& =\int q(x_{1:k},{\tilde{R}}_{1:k})ln\frac{p(x_{1:k},{\tilde{R}}_{1:k},y_{1:k})}{q(x_{1:k},{\tilde{R}}_{1:k})}.\hfill \end{array}$$

(24)

p is the true distribution that is intractable for non-Gaussian noise models, and q is a tractable approximate distribution.

In probability theory, a measure of the difference between two probability distributions p and q is the Kullback–Leibler divergence, denoted as $\mathrm{KL}[·]$. If we allow any possible choice for q such as the Gaussian distribution, then lower bound $\mathcal{L}\left[q\right]$ is maximum when the KL divergence vanishes; that is, $q(x_{1:k},{\tilde{R}}_{1:k})=p(x_{1:k},{\tilde{R}}_{1:k}|y_{1:k})$. To minimize the KL divergence, we seek the member of a restricted family of $q(x_{1:k},{\tilde{R}}_{1:k})$. Indeed, maximizing $\mathcal{L}\left[q\right]$ is equivalent to minimizing another new KL divergence [52], and thus the minimum occurs when factorized distributions $q(x_{1:k},{\tilde{R}}_{1:k})=q\left(x_{1:k}\right)q\left({\tilde{R}}_{1:k}\right)$ and the following Equations (25) and (26) hold,

$$\begin{array}{cc}\hfill lnq\left(x_{1:k}\right)& =lnp\left(x_1\right)+\sum _{t=2}^k{\mathbb{E}}_{q\left({\tilde{R}}_{1:t}\right)}[lnp\left(x_t\right|x_{t-1})]+\sum _{t=1}^k{\mathbb{E}}_{q\left({\tilde{R}}_{1:t}\right)}[lnp\left(y_t\right|x_t,{\tilde{R}}_t)]+\cdots \hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill lnq\left({\tilde{R}}_k\right)& ={\mathbb{E}}_{q\left(x_{1:k}\right)}[lnp\left(y_k\right|x_k,{\tilde{R}}_k)]+lnp\left({\tilde{R}}_k\right)+\cdots ,\hfill \end{array}$$

(26)

where ${\mathbb{E}}_{q(·)}$ represents the expectation with respect to $q(·)$. With assuming that initial state $x_1$ is Gaussian, the measurement update with varying noise covariance $\mathbb{E}\left[{\tilde{R}}_t^{-1}\right]={\Lambda }_t^{-1}$, which closely resemble the EKF updates, solve Equation (25). Algorithm 2 describes the details of the updates.

Now let us assume that the true priors are IID noise models as the case in this study; that is, $p\left({\tilde{R}}_k\right)$ follows ${\mathcal{W}}^{-1}(\nu R,\nu )$ distribution. Then, second term $lnp\left({\tilde{R}}_k\right)$ in the right-hand side of Equation (26) is computed using the pdf of the inverse-Wishart distribution in Equation (21) with its prior noise model.

$$lnp\left({\tilde{R}}_k\right)=-\frac{\nu +m+1}{2}ln\left|{\tilde{R}}_k\right|-\frac{\nu }{2}\mathrm{tr}\left(R{\tilde{R}}_k^{-1}\right).$$

(27)

As the term is conjugate prior for Equation (20), the approximations of $q\left({\tilde{R}}_k\right)$ have same mathematical forms as priors; that is, $q\left({\tilde{R}}_k\right)$ also follows ${\mathcal{W}}^{-1}({\tilde{\nu }}_k{\Lambda }_k,{\tilde{\nu }}_k)$ distribution.

$$lnq\left({\tilde{R}}_k\right)=-\frac{{\tilde{\nu }}_k+m+1}{2}ln\left|{\tilde{R}}_k\right|-\frac{{\tilde{\nu }}_k}{2}\mathrm{tr}\left({\Lambda }_k{\tilde{R}}_k^{-1}\right).$$

(28)

As $y_t|\{x_t,{\tilde{R}}_t\}\sim \mathcal{N}(h\left(x_t\right),{\tilde{R}}_t)$,

$$\mathbb{E}\left[lnp\left(y_k\right|x_k,{\tilde{R}}_k)\right]=-\frac{1}{2}ln\left|{\tilde{R}}_k\right|-\frac{1}{2}\mathrm{tr}\left(\mathbb{E}\left[{\zeta }_k{\zeta }_k^{\mathrm{T}}\right]{\tilde{R}}_k^{-1}\right).$$

(29)

From Equations (26)–(29), to handle measurement outliers, similar to Agamennoni et al. [18,19]’s derivation, we derive how to compute precision matrix ${\Lambda }_k$ of approximate distribution $q\left({\tilde{R}}_k\right)$ of ${\tilde{R}}_k$ as follows,

$$\begin{array}{cc}\hfill {\tilde{\nu }}_k& =1+\nu ,{\tilde{\nu }}_k{\Lambda }_k=\mathbb{E}\left[{\zeta }_k{\zeta }_k^{\mathrm{T}}\right]+\nu R\hfill \\ \hfill \Rightarrow {\Lambda }_k& =\frac{\nu R+\mathbb{E}\left[{\zeta }_k{\zeta }_k^{\mathrm{T}}\right]}{\nu +1},\hfill \end{array}$$

(30)

where each feature from one image is independent and

$$\begin{array}{cc}\hfill {\left({\zeta }_j\right)}_k& ={\left(y_j\right)}_k-h\left(x\left(t_{\mathrm{img}}\right)\right)\hfill \\ \hfill & ={\left(y_j\right)}_k-h\left({\left({\widehat{x}}_k\right)}_j+e_j\right)\hfill \\ \hfill & \approx {\left(y_j\right)}_k-h\left({\left({\widehat{x}}_k\right)}_j\right)-C_j{e}_j.\hfill \end{array}$$

Next, in Equation (30),

$$\begin{array}{cc}\hfill \mathbb{E}\left[{\left({\zeta }_j\right)}_k{\left({\zeta }_j\right)}_k^{\mathrm{T}}\right]& =\left[{\left(y_j\right)}_k-h\left({\left({\widehat{x}}_k\right)}_j\right)\right]{\left[{\left(y_j\right)}_k-h\left({\left({\widehat{x}}_k\right)}_j\right)\right]}^{\mathrm{T}}\dots \hfill \\ \hfill & +C_j\mathbb{E}\left[e_j{e}_j^{\mathrm{T}}\right]C_j^{\mathrm{T}}\hfill \\ \hfill & =r_j{r}_j^{\mathrm{T}}+C_j{\left(P_k\right)}_j{C}_j^{\mathrm{T}},\hfill \end{array}$$

(31)

where estimation error $e_j=x_k-{\left({\widehat{x}}_k\right)}_j$ and the Jacobian $C_j=\frac{\partial h_j}{\partial x}{|}_{{\left({\widehat{x}}_k\right)}_j}$. In the sequential measurement update, ${\left({\widehat{x}}_k\right)}_j$ and ${\left(P_k\right)}_j$ are corrected by Kalman gain $K_j$ that is a function of ${\left({\Lambda }_j\right)}_k$, so these update steps are coupled. Hence no a closed-form solution exists, and we can only solve iteratively. The purpose of the iteration seems to be similar to that of the online learning of unknown variances of each noise [10]. In addition, similar to Agamennoni et al.’s interpretation [19], the convergence and optimality of the derived update steps for outliers are guaranteed since the variational lower bound is convex with respect to ${\left({\widehat{x}}_k\right)}_j$, ${\left(P_k\right)}_j$, and ${\left({\Lambda }_j\right)}_k$. In particular, as the j-th feature is observed countless times (i.e., ${\nu }_j$→∞), ${\Lambda }_j$ converges to R in the limit of an infinitely precise noise distribution, so the iterative update steps reduce to the standard sequential measurement update of the EKF.

If true state $x\left(t_{\mathrm{img}}\right)$ is significantly different from its estimate ${\left({\widehat{x}}_k\right)}_j$, then statistics $\mathbb{E}\left[{\left({\zeta }_j\right)}_k{\left({\zeta }_j\right)}_k^{\mathrm{T}}\right]$ dominates, and ${\left({\Lambda }_j\right)}_k$ becomes much larger than R. This ${\zeta }_j$ is regarded as a measurement outlier at time k. As Kalman gain $K_j$ is a function of the inverse of precision matrix ${\left({\Lambda }_j\right)}_k$, the larger ${\left({\Lambda }_j\right)}_k$ values, the smaller the Kalman gain. Therefore, to deal with situations where measurement outliers occur, the iteration for Equations (30) and (31) corrects the state estimates and its covariance with low weights.

5. Implementation

5.1. Marginalization of Feature States

If measurement outliers often occur, a few numbers of sequential updates in the EKF are proceeded to correct the state estimates. Without a sufficient number of measurement updates, the EKF is not robust and even diverges. Hence, outlier-adaptive filtering introduced in Section 4.3 performs the modified measurement update even when a residual is detected as an outlier. Indeed, to save computation resources, this study operates the outlier-adaptive filtering for only features detected frequently outliers. For implementation, we count how many numbers of features augmented in state variables are detected as outliers. Once updating feature outliers by the outlier-adaptive filtering approach, we prune the used feature states from the state vector (Figure 3). In addition, similar to that mentioned in Appendix B, to maintain a certain size of the state vector, after the feature initialization, we marginalize the features with the least number of observations among tracked features.

5.2. Summarized Algorithm

This section summarizes and describes an implementation of the proposed method. Figure 4 and Algorithm 2 illustrate a flow chart and the pseudocode of the overall process of the outlier-adaptive filtering approach for V-INS, respectively. For the robust outlier-adaptive filter presented in this paper, the blue boxes in Figure 4 are extended from the figure in [25].

Algorithm 2 The Outlier-Adaptive Filtering

Require: ${\widehat{x}}_0^{+}(={\widehat{x}}_{V_0}^{+}),P_0^{+},Q,R,{\chi }^2$

1: for$k=1:T$do 2:                 ▹ Image processing front-end in different thread 3:  if new image capture then 4: Image Processing:                       ▹ Algorithm 1 5:   Stereo matching between current images of left camera $c_1$ and right camera $c_2$ 6:   RANSAC between previous and current images of camera $c_1$ 7:   RANSAC between previous and current images of camera $c_2$ 8:  end if 9:                    ▹ Filtering back-end in different thread 10:  if new IMU packet arrival then 11: Time Update: 12:   State prediction 13:  end if 14:  if new vision data packet arrival then 15:   for $j=1:♯$ of observed features N do 16:    if new feature then 17: Feature Initialization: 18:     If any depth of $c_1$ or $c_2$ is negative, j-th feature is outlier 19:    else                        ▹ tracked feature 20: Outlier Gating Test: 21:     $r_j=y_j-h_j\left({\left({\widehat{x}}_k\right)}_{j-1}\right)$ 22:     $S_j={\left(C_j\right)}_k{\left(P_k\right)}_{j-1}{\left(C_j\right)}_k^{\mathrm{T}}+R$         ▹${\left(C_j\right)}_k=\frac{\partial h_j}{\partial x}{|}_{{\left({\widehat{x}}_k\right)}_{j-1}}$ 23:     $\gamma =r_j^{\mathrm{T}}{S}_j^{-1}{r}_j\stackrel{?}{<}{\chi }_j^2$ 24: Measurement Updates: 25:     if outlier detected then 26:      ${\tilde{x}}_j\leftarrow {\left({\widehat{x}}_k\right)}_{j-1},{\tilde{P}}_j\leftarrow {\left(P_k\right)}_{j-1}$    ▹${\left({\widehat{x}}_k\right)}_0={\widehat{x}}_k^{-},{\left(P_k\right)}_0=P_k^{-}$ 27:      while until converged do 28:       Update measurement noise given the state 29:       ${\tilde{r}}_j=y_j-h_j\left({\tilde{x}}_j\right)$ 30:       ${\tilde{C}}_j=\frac{\partial h_j}{\partial x}{|}_{{\tilde{x}}_j}$ 31:       $W_j={\tilde{r}}_j{\tilde{r}}_j^{\mathrm{T}}+{\tilde{C}}_j{\tilde{P}}_j{\tilde{C}}_j^{\mathrm{T}}$ 32:       ${\Lambda }_j\leftarrow {\displaystyle \frac{{\nu }_{j}R+W_j}{{\nu }_j+1}}$ 33:       Update the posteriori state given noise 34:       ${\tilde{S}}_j={\left(C_j\right)}_k{\left(P_k\right)}_{j-1}{\left(C_j\right)}_k^{\mathrm{T}}+{\Lambda }_j$ 35:       ${\tilde{K}}_j={\left(P_k\right)}_{j-1}{\left(C_j\right)}_k^{\mathrm{T}}{\tilde{S}}_j^{-1}$ 36:       ${\tilde{x}}_j\leftarrow {\left({\widehat{x}}_k\right)}_{j-1}+{\tilde{K}}_j{r}_j$ 37:       ${\tilde{P}}_j\leftarrow {\left(P_k\right)}_{j-1}-{\tilde{K}}_j{\left(C_j\right)}_k{\left(P_k\right)}_{j-1}$ 38:      end while 39:      ${\left({\widehat{x}}_k\right)}_j={\tilde{x}}_j,{\left(P_k\right)}_j={\tilde{P}}_j$ 40:     else             ▹ standard sequential EKF in Section 2 41:      $K_j={\left(P_k\right)}_{j-1}{\left(C_j\right)}_k^{\mathrm{T}}{S}_j^{-1}$ 42:      ${\left({\widehat{x}}_k\right)}_j={\left({\widehat{x}}_k\right)}_{j-1}+K_j{r}_j$ 43:      ${\left(P_k\right)}_j={\left(P_k\right)}_{j-1}-K_j{\left(C_j\right)}_k{\left(P_k\right)}_{j-1}$ 44:     end if 45:    end if 46:   end for                 ▹${\widehat{x}}_k^{+}={\left({\widehat{x}}_k\right)}_N,P_k^{+}={\left(P_k\right)}_N$ 47:  end if 48: end for

6. Flight Datasets Test Results

To examine the influence of outliers in V-INS and validate the reliability and robustness of the proposed outlier-adaptive approach for navigation systems with outliers, we test one of benchmark flight datasets, the so-called “EuRoC MAV datasets” [53]. The visual-inertial sequences of the datasets were recorded onboard a micro aerial vehicle (MAV) while a pilot manually flew around the indoor motion capture environment. For more details, see Appendix C. To articulate the significance of outliers, we select two datasets of the bright scene, called “EuRoC V1 Easy”, and motion blur, called “EuRoC V1 Difficult.” As the images in the difficult dataset are dark scene or motion blur, we hypothesize that outliers occur more frequently in the difficult dataset.

The EKF estimates the relative location and orientation from a starting point. As we do not know the exact absolute location of the origin of given datasets, to compare with ground-truth data given in the datasets, we require certain evaluation error metrics such as so-call “absolute trajectory error [54]”. For more details, see Appendix D. The absolute trajectory error as an evaluation error metric yields the following various comparison plots. Figure 5 illustrates the top-down view of the estimated flight trajectory of the difficult dataset.

Figure 6 depict the estimated x, y, z position and their respective estimation errors. All the estimation errors are bounded within each standard deviation $\sigma$ envelope, so the proposed approach is reliable vision-aided inertial navigation under even poor illumination environments. Conceptually, the position error gets locked in, and it tends to increase with the length of the trajectory until new features are being mapped.

Similar to the analysis presented in [25], the adaptive filter is a well-tuned estimator since the performance of doing runs with ×3 or ×10 ( /3 or /10) multiplier on the R matrix used in the filter is worse for all of those, shown in

Table 1. Indication that the outlier-adaptive filter is well-tuned for the EuRoC V1 difficult dataset.

Multiplier on R		/10	/3	1	×3	×10
RMS error [m]		0.9240	0.3801	0.1700	0.5153	0.5610

. That is, the fact that using the multipliers reveals larger root mean square (RMS) estimation errors indicates that our filter is well-tuned.

Figure 7 shows the advantages of the addition of the outlier adaptation proposed in this paper by comparing it with a baseline, the standard EKF in V-INS.

As Lee [25] and other researchers showed that the standard EKF is a basic filter in V-INS, we choose the method as a baseline here. The baseline only rejects outliers whenever the chi-squared test fails, whereas the outlier-adaptive filtering follows all proposed adaptive approaches in Algorithm 2. Although the iteration in the outlier-adaptive filtering might increase computational resources, it significantly improves the accuracy of estimation. Fortunately, the “while” loop iteration in Algorithm 2 rapidly converges to the optimal noise covariance by twice or three times iterations. For sensitivity analysis, RMS position errors resulting from the baseline and the outlier-adaptive filtering are compiled in

Table 2. Sensitivity analysis in RMS position error [m] of the outlier-adaptive filtering.

Dataset		EuRoC V1 Easy		EuRoC V1 Difficult
		Slow Motion 0.41 m/s, 16.0 deg/s		Fast Motion 0.75 m/s, 35.5 deg/s
Method		Bright Scene		Motion Blur
Baseline		0.2558		0.3656
Outlier-Adaptive		0.2237		0.2264

.

Motion blur datasets are more sensitive to outliers as the improvement is larger when applied to those datasets. Although the outlier-adaptive filtering is the best choice for motion blur datasets, we can select an adequate mode depending on computation margin and cost.

Although a number of researchers have investigated V-INS of the EuRoC datasets [55], only a few of them thoroughly has focused on vision measurements with outliers.

Table 3. Comparison with other methods in RMS position error [m] of the outlier-adaptive filtering.

Dataset		EuRoC V1 Easy	EuRoC V1 Difficult
Method		Bright Scene	Motion Blur
Outlier-Adaptive Filter		0.2237	0.2264
SVO+MSF [56,57] (loosely coupled)		0.40	×
S-MSCKF [48,58] (stereo-filter)		0.34	0.67

reveals that the proposed estimator, the outlier-adaptive filter, outperforms other state-of-the-art V-INS techniques, called “SVO+MSF [56,57]” and “S-MSCKF [48,58] ” in which stereo is available. As SVO+MSF is loosely coupled, its algorithm actually gets diverged.

7. Discussion

This paper has presented practical outlier-adaptive filtering for a vision-aided inertial navigation systems (V-INS) and evaluated its performance with flight datasets testing. In other words, this study develops a robust and adaptive state estimation framework for V-INS under frequent outliers occurrence. In the image processing front-end of the framework, we propose the improved utilization of outlier removal techniques. In filtering back-end, for estimating the states of V-INS with measurement outliers, we implement a novel approach of the outlier-robust extended Kalman filter (EKF) to V-INS, for which we derive iterative update steps for computing the precision noise matrices of vision outliers when the Mahalanobis gating test detects remaining outliers.

To validate the accuracy of the proposed approach and compare it with other state-of-the-art V-INS algorithms, we test the performance of V-INS employing the outlier-adaptive filtering algorithm in the realistic benchmark flight datasets. In particular, to show more improvements of our method over the others’ approaches, we use the fast motion and motion blur flight datasets. Results from the flight datasets testing show that the novel navigation approach in this study improves the accuracy and reliability of state estimation in V-INS with frequent outliers. Using the outlier-adaptive filtering reduces the root mean square (RMS) error of the estimates and accelerates the robustness of the estimates, especially for the motion blur datasets.

The primary goals of future work are listed as follows. Since an inertial measurement unit (IMU) is also a sensor, it could generate outliers in V-INS. With accounting for the process outliers, the accuracy and robustness of the estimator would be improved. If we distinguish process outliers from IMU sensors with measurement outliers from vision data, the extended outlier-robust EKF [32] may be an impressive and innovate approach for this case. Furthermore, the investigation of color noise in V-INS is another possible future work. One of the required assumptions of the Kalman filter is the whiteness of measurement noise. As an illustration, during sampling and transmission in image processing, color noise that may be originated from a multiplicity of sources could degrade the quality of images [59]. The vibrational effects of camera sensors might also produce colored measurement noise [60]. That is, if the residuals of vision data are correlated with themselves at different timestamp, then colored measurement noise occurs in V-INS. Therefore, the images with color noise would be filtered for ensuring the accuracy of locating landmarks. As modeling noise without additional prior knowledge of the noise statistics is typically difficult, the machine learning techniques-based state estimator for colored noise [61,62] may handle the unknown correlations in V-INS.

This study showcases the approaches using stereo cameras but is also suitable for monocular V-INS and employable to other filter-based V-INS frameworks. We test benchmark flight datasets to validate the reliability and robustness of this study, but additional validating with other flight datasets or real-time flight tests would help to prove its more robustness. In addition, we can operate unmanned aerial vehicles (UAVs) stacked the navigation algorithms in this study and a controller-in-the-loop. The use of the controller-in-the-loop could be a more important validation criteria due to the potential for navigation-controller coupling. The research objectives and contributions presented here will remarkably advance the state-of-the-art techniques of vision-aided inertial navigation for UAVs.