Observability-Constrained Resampling-Free Cubature Kalman Filter for GNSS/INS with Measurement Outliers

Abstract

Integrating global navigation satellite systems (GNSSs) with inertial navigation systems (INSs) has been widely recognized as an ideal solution for autonomous vehicle navigation. However, GNSSs suffer from disturbances and signal blocking inevitably, making the performance of GNSS/INSs degraded in the occurrence of measurement outliers. It has been proven that the sigma points-based Kalman filter (KF) performs better than an extended KF in cases where large prior uncertainty is present in the state estimation of a GNSS/INS. By modifying the sigma points directly, the resampling-free sigma point update framework (SUF) propagates more information excepting Gaussian moments of prescribed accuracy, based on which the resampling-free cubature Kalman filter (RCKF) was developed in our previous work. In order to enhance the adaptivity and robustness of the RCKF, the resampling-free SUF depending on dynamic prediction residue should be improved by suppressing the time-varying measurement outlier. In this paper, a robust observability-constrained RCKF (ROCRCKF) is proposed based on adaptive measurement noise covariance estimation and outlier detection, where the occurrence of measurement outliers is modelled by the Bernoulli variable and estimated with the state simultaneously. Experiments based on car-mounted GNSS/INSs are performed to verify the effectiveness of the ROCRCKF, and result indicates that the proposed algorithm outperforms the RCKF in the presence of measurement outliers, where the heading error and average root mean square error of the position are reduced from 1.96° and 6.38 m to 0.27° and 5.95 m, respectively. The ROCRCKF is robust against the measurement outliers and time-varying model uncertainty, making it suitable for the real-time implementation of GNSS/INSs in GNSS-challenged environments.

1. Introduction

Reliable and accurate position and orientation information plays a vital role in the autonomous navigation of land vehicles, e.g., unmanned agricultural machinery and self-driving cars [1,2]. Global navigation satellite systems (GNSSs) have become the default tool for land vehicle navigation, employing real-time kinematic technology to achieve centimeter-level positioning accuracy. Inertial navigation systems (INSs) represent a self-contained navigation technology which updates the solution to a given problem by calculating the integrals of inertial sensor output given the initial state. Because the drift errors of INSs are accumulated with time, the integration of GNSSs with INSs has been widely studied and applied in autonomous navigation. The Kalman filter (KF) and its variants have dominated the information fusion of navigation and target tracking since the celebrated work of Kalman and Schmidt [3,4]. With precise prior knowledge on the underlying state and noise statistical properties, KF achieves optimal estimation under the linear minimum mean square error (LMMSE). However, GNSS suffers from multipath disturbances and signal blocking inevitably, making the hypotheses on the Gaussian distribution noise invalid and thus degrading the performance of the KF heavily.

Because there is always nonlinearity in practical state space models (SSMs), many sub-optimal filters have been developed to approximate the KF framework. The extended Kalman filter (EKF) is by far the most popular method for solving nonlinear filtering problems and linearizes nonlinear functions to the best available state by using first-order Taylor series expansion. In order to reduce the nonlinear approximation error, sigma-point KFs based on deterministic sampling and quadrature rules are proposed, e.g., unscented Kalman filters [5], central difference filters [6], and cubature Kalman filters (CKFs) [7], which not only avoid calculating the derivatives of functions but also showcase better convergence than EKFs in cases of large initial uncertainty [8]. By using the spherical-radial cubature rule, CKF approximates the Gaussian moments at third-order accuracy in terms of Taylor series and shows better numerical stability than UKFs in high-dimensional systems [9,10]. Particle filters (PFs) approximate the state posterior probability density function (PDF) by using a large number of weighted particles, and they make no assumptions concerning the state distribution and SSM, which is suitable for nonlinear and non-Gaussian filtering problems [11]. Although improved PFs have been proposed [12,13], their huge computational complexity still hinders their application in real-time GNSS/INSs. In the presence of time-varying noise statistics, model mismatches, and measurement outliers, the SSM of a GNSS/INS appears to have non-Gaussian and nonlinear properties [14,15,16]. Because there are no closed-form solutions for nonlinear and non-Gaussian filtering problems, the suboptimal filter design for navigation and target tracking is an active area of research [17,18].

The unknown process and measurement noise of KF-type filters can be estimated online by using maximum a posterior, or variational Bayesian (VB) technology [19,20,21]. However, the derivation of the KF is under the LMMSE, which makes it sensitive to measurement outliers and non-Gaussian noise. Other robust KFs include Huber-based KFs, maximum correntropy criterion (MCC)-based KFs [22,23,24], and finite impulse response KFs [25,26]. Although these filters showcase better performance than their LMMSE-based counterparts in the presences of measurement outliers, they don’t make full use of the feature of non-Gaussian distribution. Unlike [21], which estimates the measurement noise under a Gaussian SSM, the skew-t distribution is used to capture the skewness and heavy-tailedness of asymmetric distributions [27]. However, the occurrences of state and measurement outliers are time-varying, and there are non-stationary heavy-tailed processes and measurement noise. Recently, the mixture distribution has been employed to handle time-varying outliers, e.g., Gaussian mixtures [28] and Gaussian-Student’s mixtures [29], where both the prediction covariance and measurement noise covariance are estimated by VB after constructing the linear hierarchical Gaussian SSM. These robust KFs import extra parameters to formulate a robust model, which not only needs suitable hierarchical prior selection but also increases the computational complexity [30]. What is more, the above-mentioned robust KFs focus on constructing apposite models of non-Gaussian noise, whereas their uncertainty propagation is less effective when the prior uncertainty is high and the observation is accurate [31].

In the information fusion of a GNSS/INS, nonlinearity is negligible, provided that precision prior knowledge on noise and state statistics is provided; however, this randomly occurs in practical implementation. Furthermore, the under-estimation of VB-based parameters and inconsistent covariance in the KF framework degrade the state estimation of GNSS/INSs [27]. The main cause of inconsistent estimation is the linearization error of dynamic models, which produces spurious information and reduces the uncertainty incorrectly by coupling with the KF gain update [31,32,33]. Minimizing the linearization error of the dynamic model resulting from actual state approximation is an effective approach to relieving covariance inconsistency. A resampling-free SUF that modifies the sigma points directly based on model prediction residue was proposed in our previous work [31], where a resampling-free CKF (RCKF) was developed to handle the GNSS outage of GNSS/INSs. Based on the RCKF, the MCC-enhanced and VB-based resampling-free SUFs were then developed to handle the time-varying and non-Gaussian measurement noise [34,35]. However, the parameter selection of resampling-free SUFs is pre-determined by tuning attempts, and the effect of non-stationary heavy-tailed noise has not been fully taken into consideration.

Inspired by works on measurement outlier detection [36,37], an improved RCKF with enhanced robustness and adaptability is developed in this paper, where the time-varying measurement outlier is addressed by VB-based outlier detection and the sigma points are updated based on the prediction residue of the dynamic model. Consequently, a robust observability-constrained RCKF (ROCRCKF) is developed for land vehicle navigation based on outlier-robust measurement updates. The main contributions of this work are two-fold: (1) an improved RCKF is developed by employing a model indicator and a scale parameter to handle non-stationary measurement outliers, (2) the ROCRCKF is verified based on a car-mounted GNSS/INS with different maneuver modes, and the performance of a resampling-free SUF under different state observability is investigated.

The remainder of this paper is organized as follows. In Section 2, CKFs and resampling-free CKFs are briefly reviewed. The ROCRCKF is then developed in Section 3 based on adaptive outlier detection. By employing the car-mount GNSS/INS field test, the proposed algorithm is verified in Section 4, and the result is discussed in Section 5. Section 6 concludes this paper.

2. Preliminaries

2.1. Cubature Kalman Filter

The discrete-time SSM of GNSS/INSs can be written as:

$${\mathit{x}}_k=\mathrm{f}\left({\mathit{x}}_{k-1}\right)+{\mathit{w}}_{k-1}$$

(1)

$${\mathit{z}}_k=\mathrm{h}\left({\mathit{x}}_k\right)+{\mathit{v}}_k$$

(2)

where ${\mathit{x}}_k\in {ℝ}^{n\times 1}$ and ${\mathit{z}}_k\in {ℝ}^{p\times 1}$ are the state and observations at time k; ${\mathit{w}}_{k-1}\in {ℝ}^{n\times 1}$ and ${\mathit{v}}_k\in {ℝ}^{p\times 1}$ are the process and measurement noise with the known covariance $Q_{k-1}\in {ℝ}^{n\times n}$ and $R_k\in {ℝ}^{p\times p}$; and $\mathrm{f}(·)$ and $\mathrm{h}(·)$ are the dynamic and measurement functions. The initial state ${\mathit{x}}_0$, ${\mathit{w}}_{k-1}$, and ${\mathit{v}}_k$ are mutually independent and subject to Gaussian distribution. By utilizing (1) and (2), the state prior PDF and likelihood function can be, respectively, written as $p\left({\mathit{x}}_k\right)=N\left({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k-1},P_{k|k-1}\right)$ and $p\left({\mathit{z}}_k\right)=N\left({\mathit{x}}_k;{\widehat{\mathit{z}}}_{k|k-1},P_{k|k-1}^{\mathit{z}\mathit{z}}\right)$, where $N\left(\mathit{x};\widehat{\mathit{x}},P\right)$ denotes the Gaussian distribution with the mean, $\widehat{\mathit{x}}$, and covariance, $P$. The sigma points are defined as follows:

$${\xi }_{i,k-1}={\widehat{\mathit{x}}}_{k-1|k-1}+\sqrt{n{P}_{k-1|k-1}}{\left[\mathbf{1}\right]}_i$$

(3)

$${\zeta }_{i,k}={\widehat{\mathit{x}}}_{k|k-1}+\sqrt{n{P}_{k|k-1}}{\left[\mathbf{1}\right]}_i$$

(4)

where ${\xi }_{i,k-1}$ and ${\zeta }_{i,k}$, respectively, correspond to the posterior PDF of time k−1 and the prior PDF at time k. Let ${\mathit{e}}_i$ denote the i-th column of identity matrix $I_{n\times n}$ and ${\left[\mathbf{1}\right]}_i$ is defined as:

$${\left[\mathbf{1}\right]}_i=\{\begin{array}{c}\begin{array}{cc}{\mathit{e}}_i& i=1,\cdots ,n\end{array}\\ \begin{array}{cc}-{\mathit{e}}_{i-n}& i=n+1,\cdots ,2n\end{array}\end{array}$$

Approximating the involved moments based on the third-degree spherical-radial cubature rule yields:

$${\widehat{\mathit{x}}}_{k|k-1}={{\displaystyle \sum }}_{i=1}^N{\varpi }_i\mathrm{f}\left({\xi }_{i,k-1}\right)$$

(5)

$$P_{k|k-1}={{\displaystyle \sum }}_{i=1}^N{\varpi }_i\left(\mathrm{f}\left({\xi }_{i,k-1}\right)-{\widehat{\mathit{x}}}_{k|k-1}\right){\left(\mathrm{f}\left({\xi }_{i,k-1}\right)-{\widehat{\mathit{x}}}_{k|k-1}\right)}^T+Q_{k-1}$$

(6)

$${\widehat{\mathit{z}}}_{k|k-1}={{\displaystyle \sum }}_{i=1}^N{\varpi }_i\mathrm{h}\left({\zeta }_{i,k}\right)$$

(7)

$$P_{k|k-1}^{\mathit{z}\mathit{z}}={{\displaystyle \sum }}_{i=1}^N{\varpi }_i\left(\mathrm{h}\left({\zeta }_{i,k}\right)-{\widehat{\mathit{z}}}_{k|k-1}\right){\left(\mathrm{h}\left({\zeta }_{i,k}\right)-{\widehat{\mathit{z}}}_{k|k-1}\right)}^T+R_k$$

(8)

$$P_{k|k-1}^{\mathit{x}\mathit{z}}={{\displaystyle \sum }}_{i=1}^N{\varpi }_i\left(\mathrm{f}\left({\xi }_{i,k-1}\right)-{\widehat{\mathit{x}}}_{k|k-1}\right){\left(\mathrm{h}\left({\zeta }_{i,k}\right)-{\widehat{\mathit{z}}}_{k|k-1}\right)}^T$$

(9)

where ${\varpi }_i=1/N$ is the weight of corresponding sigma points and N = 2n. Notably, ${\xi }_{i,k-1}$ and ${\zeta }_{i,k}$ are proportional to state dimension, n, leading to a non-local sampling problem in sigma points-based KFs. Once a new measurement, ${\mathit{z}}_k$, is received, the state posterior PDF can be written as $p\left({\mathit{x}}_k\right)=N\left({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k},P_{k|k}\right)$, which under Bayesian filter framework can be expressed as:

$${\widehat{\mathit{x}}}_{k|k}={\widehat{\mathit{x}}}_{k|k-1}+K_k\left({\mathit{z}}_k-{\widehat{\mathit{z}}}_{k|k-1}\right)$$

(10)

$$P_{k|k}=P_{k|k-1}-K_k{P}_{k|k-1}^{\mathit{z}\mathit{z}}{K}_k^T$$

(11)

$$K_k=P_{k|k-1}^{\mathit{x}\mathit{z}}{\left(P_{k|k-1}^{\mathit{z}\mathit{z}}\right)}^{-1}$$

(12)

The recursive sigma-point update framework of the CKF is based on approximated Gaussian moments, which are less efficient in cases where there is non-Gaussian noise or large uncertainty in the approximation of the state’s prior and posterior PDF. Moreover, reducing the covariance inconsistence by minimizing the linearization error of the dynamic function is suggested, especially when the nonlinearity of the dynamic function is strong [33]. Since the dynamics of function approximation errors are much slower than the instantiated function points, it is better to update the posterior points based on the model prediction residue.

2.2. Resampling-Free Sigma-Point Update Framework

Suppose that $\mathit{x}\in {ℝ}^{n\times 1}$ is a random variable subject to Gaussian distribution and its moments can be fully captured by $\left\{{\varsigma }_i, {\varpi }_i\right\}$, $i=1, \cdots , N$. The general transform of x can be formulated as:

$$\mathit{y}=g\left(\mathit{x}\right)+\mathit{ϵ}$$

(13)

where $p\left(\mathit{ϵ}\right)=N\left(\mathit{ϵ};\mathbf{0},P_{\mathit{ϵ}\mathit{ϵ}}\right)$ represents the noise of direct point modification, and the point corresponding to random variable $\mathit{y}$ is denoted as:

$${\mathit{Y}}_i=\overline{g}+T\left(g\left({\varsigma }_i\right)-\overline{g}\right)$$

(14)

where $\overline{g}={{\displaystyle \sum }}_{i=1}^N{\varpi }_{i}g\left({\varsigma }_i\right)$, $P_{gg}={{\displaystyle \sum }}_{i=1}^N{\varpi }_i\left(g\left({\varsigma }_i\right)-\overline{g}\right){\left(g\left({\varsigma }_i\right)-\overline{g}\right)}^T$, and the transform matrix are defined as:

$$T=\sqrt{P_{gg}+P_{\mathit{ϵ}\mathit{ϵ}}}{\left(\sqrt{P_{gg}}\right)}^{-1}$$

(15)

Let $\left\{X_{i,k-1|k-1}^{+}, {\varpi }_i\right\}$ denote the posterior sigma points and weights of ${\mathit{x}}_{k-1}$, and define ${\tilde{X}}_{i,k|k-1}^{-}=f\left(X_{i,k-1|k-1}^{+}\right)-{\widehat{\mathit{x}}}_{k|k-1}$, where ${\widehat{\mathit{x}}}_{k|k-1}={{\displaystyle \sum }}_{i=1}^N{\varpi }_{i}f\left(X_{i,k-1|k-1}^{+}\right)$. By applying (14) to (1), we obtain:

$$X_{i,k|k-1}={\widehat{\mathit{x}}}_{k|k-1}+\sqrt{P_{k|k-1}+Q_{k-1}}{\left(\sqrt{P_{k|k-1}}\right)}^{-1}{\tilde{X}}_{i,k|k-1}^{-}$$

(16)

Considering the linear measurement update step of CKF yields:

$$X_{i,k|k}^{-}=X_{i,k|k-1}+K_k\left(z_k-Z_{i,k|k}\right)$$

(17)

where $P_{k|k-1}={{\displaystyle \sum }}_{i=1}^N{\varpi }_i{\tilde{X}}_{i,k|k-1}^{-}{\left({\tilde{X}}_{i,k|k-1}^{-}\right)}^T$, $Z_{i,k|k}=h\left(X_{i,k|k-1}\right)$, and $X_{i,k|k}^{-}$ denote the sigma points after assimilating observation information. If we define ${\overline{\mathit{x}}}_{k|k}={{\displaystyle \sum }}_{i=1}^N{\varpi }_i{X}_{i,k|k}^{-}$,${\overline{\mathit{P}}}_{k|k}={{\displaystyle \sum }}_{i=1}^N{\varpi }_i\left(X_{i,k|k}^{-}-{\overline{\mathit{x}}}_{k|k}\right){\left(X_{i,k|k}^{-}-{\overline{\mathit{x}}}_{k|k}\right)}^T$ and suppose that there is an approximation error in the linear Bayesian update (17), then by employing (14) in (17), we obtain [35]:

$$X_{i,k|k}^{+}={\overline{\mathit{x}}}_{k|k}+\sqrt{{\overline{\mathit{P}}}_{k|k}+K_k{R}_k{K}_k^T}{\left(\sqrt{{\overline{\mathit{P}}}_{k|k}}\right)}^{-1}\left(X_{i,k|k}^{-}-{\overline{\mathit{x}}}_{k|k}\right)$$

(18)

where $K_k{R}_k{K}_k^T$ accounts for the variance increase in direct point modification from $X_{i,k|k}^{-}$ to $X_{i,k|k}^{+}$. Notably, importing the resampling-free SUF into CKF is straightforward, as it replaces (3) and (4) by setting ${\xi }_{i,k}=X_{i,k|k}^{+}$ and ${\zeta }_{i,k+1}=f\left(X_{i,k|k}^{+}\right)$.

2.3. Observability-Constrained Resampling-Free CKF

Because of time-varying measurement noise and outliers, the direct point modification from $X_{i,k|k}^{-}$ to $X_{i,k|k}^{+}$ cannot be formulated as (18), i.e., the $K_k$ approximates, 0 and $X_{i,k|k}^{-}$, should be replaced by $X_{i,k|k-1}$ in cases where there is a GNSS outage. Further, ${\tilde{X}}_{i,k|k}^{+}=X_{i,k|k}^{+}-{\widehat{\mathit{x}}}_{k|k}$, $\mathit{\varpi }={\left[\begin{array}{ccc}{\varpi }_1& \cdots & {\varpi }_N\end{array}\right]}^T$, and $W=\mathrm{diag}\left(\varpi \right)$, where $\mathrm{diag}\left(\mathit{\varpi }\right)$ denotes an operation that builds a diagonal matrix with the element of vector $\mathit{\varpi }$. Because the transformed points should at least match the first two moments of the prior PDF, we obtain [31]:

$${\tilde{X}}_{i,k|k-1}^{-}\mathit{\varpi }=\mathbf{0}$$

(19a)

$${\tilde{X}}_{i,k|k-1}^{-}W{\left({\tilde{X}}_{i,k|k-1}^{-}\right)}^{\mathit{T}}=P_{k|k-1}-Q_{k-1}$$

(19b)

Analogously, employing the constraints for the moment approximation of posterior PDF yields:

$${\tilde{X}}_{i,k|k}^{+}\mathit{\varpi }=\mathbf{0}$$

(20a)

$${\tilde{X}}_{i,k|k}^{+}W{\left({\tilde{X}}_{i,k|k}^{+}\right)}^{\mathit{T}}=P_{k|k}+\Delta {\rm E}$$

(20b)

Take the posterior sigma points error ${\tilde{X}}_{i,k|k}^{+}={\mathsf{\Xi }}_k{\tilde{X}}_{i,k|k-1}^{-}$, where ${\mathsf{\Xi }}_k$ is the transform matrix accounting for the propagation of sigma point residue and $\Delta {\rm E}$ denotes the uncertainty resulting from linearization errors and unmodeled dynamics. By setting $\Delta {\rm E}={\Lambda }_k{K}_k{R}_k{K}_k^T$, the spurious uncertainty reduction for unobservable states can be addressed, where ${\Lambda }_{\mathrm{k}}=\mathrm{diag}\left({\left[\begin{array}{ccc}{\eta }_1& \cdots & {\eta }_n\end{array}\right]}^{\mathrm{T}}\right)$ is the weight matrix accounting for different state observabilities and $\ell \in \left[1, n\right]$ is the index of state elements. For the information fusion of the GNSS/INS, the corresponding ${\eta }_{\ell }$ of sensor biases and attitude errors is set as 0 to prevent their posterior points from spurious uncertainty reduction. In order to ensure a variance increase in the calculation of the transform matrix while altering the points as little as possible [38], the ${\eta }_{\ell }$ corresponding to the position and velocity error is set as slightly larger than 1. Notably, (20a) follows directly from (19a), and substituting (19b) into (20b), yields:

$$\begin{gathered} {\tilde{X}}_{i,k|k}^{+}W{\left({\tilde{X}}_{i,k|k}^{+}\right)}^T={\Xi }_k{\tilde{X}}_{i,k|k}^{-}W{\left({\tilde{X}}_{i,k|k}^{-}\right)}^T{\mathsf{\Xi }}_{\mathrm{k}}^{\mathrm{T}} \\ ={\Xi }_k\left(P_{k|k-1}-Q_{k-1}\right){\Xi }_k^T \end{gathered}$$

(21)

The positive definiteness of $P_{k|k-1}-Q_{k-1}$ and $P_{k|k}+\Delta {\rm E}$ can be ensured by using third-degree spherical-radial cubature. Then, based on singular value decomposition (SVD), we obtain $P_{k|k-1}-Q_{k-1}=L_k^{-}S{\left(L_k^{-}\right)}^T$ and $P_{k|k}+\Delta {\rm E}=L_k^{+}\overline{S}{\left(L_k^{+}\right)}^T$, where $S$ and $\overline{S}$ are the diagonal matrices of singular values and $L_k^{-}$ and $L_k^{+}$ are orthonormal matrices. Based on (21), the transform matrix can be formulated as:

$${\Xi }_k=L_k^{+}{\overline{S}}^{\frac{1}{2}}{S}^{-\frac{1}{2}}{\left(L_k^{-}\right)}^{-1}$$

(22)

where ${\overline{S}}^{\frac{1}{2}}{S}^{-\frac{1}{2}}$ is element-wise multiplication, making the calculation of the transform matrix numerically stable. After that, we calculated ${\Xi }_k$, the posterior sigma points at time k, denoted as:

$$X_{i,k|k}^{+}={\widehat{\mathit{x}}}_{k|k}+{\Xi }_k{\tilde{X}}_{i,k|k-1}^{-}$$

(23)

Based on the above derivations, the steps of the RCKF can be summarized as Algorithm 1. Notably, the RCKF generates new sigma points based on ${\widehat{\mathit{x}}}_{k|k}$ and the transformed point residue ${\tilde{X}}_{i,k|k-1}^{-}$ from the dynamic function, which has no direct relation with state dimension n, relieving the non-local sampling problem. Regarding specific nonlinear transforms, the third-degree cubature rule approximates the mean more accurately than covariance, and there is extra information in $X_{i,k|k}^{+}$ except for Gaussian moments in cases where the dynamic function is of high unmodeled uncertainty.

Algorithm 1 The pseudocode of RCKF

Inputs:${\widehat{\mathit{x}}}_{k-1|k-1}$$,P_{k-1|k-1}$$,Q_{k-1}$$,R_k$ 1. Initialize ${\xi }_{i,k-1}$,${\zeta }_{i,k-1}$ by (3), (4) and update $X_{i,k|k}^{+}$ based on CKF and (18) Time update: 2. Let ${\xi }_{i,k}=X_{i,k|k}^{+}$, and calculate ${\widehat{\mathit{x}}}_{k+1|k}$,$P_{k+1|k}$ by (5) and (6) 3. Calculate ${\tilde{X}}_{i,k+1|k}^{-}=\mathrm{f}\left(X_{i,k|k}^{+}\right)-{\widehat{\mathit{x}}}_{k+1|k}$, and update $X_{i,k+1|k}$ by (16) Measurement update: 4. Calculate ${\widehat{\mathit{z}}}_{k+1|k}$, $P_{k+1|k}^{\mathit{z}\mathit{z}}$ and $P_{k+1|k}^{\mathit{x}\mathit{z}}$ as    ${\widehat{\mathit{z}}}_{k+1|k}={{\displaystyle \sum }}_{i=1}^N{\varpi }_i\mathrm{h}\left(X_{i,k+1|k}\right)$    $P_{k+1|k}^{\mathit{z}\mathit{z}}={{\displaystyle \sum }}_{i=1}^N{\varpi }_i\left(\mathrm{h}\left(X_{i,k+1|k}\right)-{\widehat{\mathit{z}}}_{k+1|k}\right){\left(\mathrm{h}\left(X_{i,k+1|k}\right)-{\widehat{\mathit{z}}}_{k+1|k}\right)}^T$    $P_{k+1|k}^{\mathit{x}\mathit{z}}={{\displaystyle \sum }}_{i=1}^N{\varpi }_i\left(\mathrm{f}\left({\xi }_{i,k}\right)-{\widehat{\mathit{x}}}_{k+1|k}\right){\left(\mathrm{h}\left(X_{i,k+1|k}\right)-{\widehat{\mathit{z}}}_{k+1|k}\right)}^T$ 5. Update ${\widehat{\mathit{x}}}_{k+1|k+1}$,$P_{k+1|k+1}$ by (10) and (11) 6. Calculate $X_{i,k+1|k+1}^{+}$ use (23) and return to Step 2 with $k=k+1$ Outputs: ${\widehat{\mathit{x}}}_{k+1|k+1}$, $P_{k+1|k+1}$, $X_{i,k+1|k+1}^{+}$

3. Methodology

By considering the unmodeled dynamic uncertainty and employing a dimension-independent SUF, the RCKF outperforms CKFs for high-dimensional state estimation problems. However, the RCKF inherits the LMMSE-based observation assimilation framework of the KF, which shows obviously degraded performance in the occurrence of time-varying measurement noise and sporadic outliers. In this section, in order to address the non-stationary heavy-tailed noise coming from time-varying GNSS measurement outliers, an improved RCKF is developed by employing the exponential multiplication form of two likelihood functions.

In the information fusion of the GNSS/INS, ${\mathit{v}}_k$ suffers from time-varying non-Gaussian noise, which can be formulated as:

$$p\left({\mathit{z}}_k|{\mathit{x}}_k,y_k\right)={\left(N\left({\mathit{z}}_k;{\widehat{\mathit{z}}}_{k|k-1},R_k\right)\right)}^{y_k}{\left(N\left({\mathit{z}}_k;{\widehat{\mathit{z}}}_{k|k-1},{\tilde{R}}_k\right)\right)}^{1-y_k}$$

(24)

where $y_k$ is a binary indicator variable, $y_k=1$ denotes there is no measurement outlier, and the measurement covariance $R_k$ satisfies $p\left(R_k|{\mathit{z}}_{1:k}\right)=IW\left(R_k;t_{k|k},T_{k|k}\right)$; otherwise, the measurement covariance is updated with a large-value ${\tilde{R}}_k=R_k/{\lambda }_k$, which indicates the occurrence of measurement outliers. $IW(·)$ denotes the inverse Wishart distribution and the prediction PDF of $R_k$ satisfies $p\left(R_k|{\mathit{z}}_{1:k-1}\right)=IW\left(R_k;t_{k|k-1},T_{k|k-1}\right)$, with $t_{k|k-1}=\rho \left(t_{k-1|k-1}-p-1\right)+p+1$, $T_{k|k-1}=\rho T_{k-1|k-1}$, where $\rho \in (0,1]$ coincides with the assumption that the covariance changes slowly. In order to estimate the hyper-parameters $y_k$ and ${\lambda }_k$, the following hierarchical priors are imported:

$$p\left(y_k|{\pi }_k\right)={\pi }_k^{y_k}{\left(1-{\pi }_k\right)}^{1-y_k}$$

(25)

$$p\left({\pi }_k\right)=Be\left({\pi }_k;e_0,1-e_0\right)$$

(26)

$$p\left({\lambda }_k\right)=G\left({\lambda }_k;\frac{{\nu }_k}{2},\frac{{\nu }_k}{2}\right)$$

(27)

where ${\pi }_k$ is the weight of different PDFs, ${\lambda }_k$ is the scale coefficient of $R_k$, and ${\lambda }_k\ll 1$, ${\nu }_k$ is the degree of freedom (dof) parameter;$Be(·)$ and $G(·)$ denote the Beta and Gamma distribution, respectively.

In the framework of Bayesian filtering, the posterior PDF can be calculated by $p\left({\Phi }_k|{\mathit{z}}_{1:k},{\Psi }_k\right)=p\left({\mathit{z}}_{1:k}|{\Phi }_k\right)p\left({\Phi }_k|{\Psi }_k\right)/p\left({\mathit{z}}_{1:k}|{\Psi }_k\right)$, where ${\Phi }_k=\left\{{\mathit{x}}_k,y_k,{\pi }_k,{\lambda }_k,R_k\right\}$ and ${\Psi }_k=\left\{t_{k|k-1},T_{k|k-1},e_0,{\nu }_k\right\}$. Suppose that $p\left({\Phi }_k|{\mathit{z}}_{1:k},{\Psi }_k\right)$ can be approximated by a tractable distribution $q\left({\Phi }_k\right)$; by importing (25)–(27) we obtain:

$$\begin{gathered} q\left({\Phi }_k\right)=q\left({\mathit{x}}_k|{\mathit{z}}_{1:k-1}\right)q\left({\mathit{z}}_k|{\mathit{x}}_k,y_k\right)q\left(R_k\right)q\left(y_k|{\pi }_k\right)q\left({\pi }_k\right)q\left({\lambda }_k\right) \\ =N\left({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k-1},P_{k|k-1}\right){\left(N\left({\mathit{z}}_k;{\widehat{\mathit{z}}}_{k|k-1},R_k\right)\right)}^{y_k}{\left(N\left({\mathit{z}}_k;{\widehat{\mathit{z}}}_{k|k-1},\frac{R_k}{{\lambda }_k}\right)\right)}^{1-y_k} \\ \times {\pi }_k^{y_k}{\left(1-{\pi }_k\right)}^{1-y_k}Be\left({\pi }_k;e_0,1-e_0\right)G\left({\lambda }_k;\frac{{\nu }_k}{2},\frac{{\nu }_k}{2}\right)IW\left(R_k;t_{k|k-1},T_{k|k-1}\right) \end{gathered}$$

(28)

Suppose that $\Delta$ is an arbitrary element of ${\Phi }_k$ and $q(\Delta )$ denotes the posterior PDF of $\Delta$. The approximation solution for $p(\Delta )$ is [28]:

$$\log q(\Delta )={\mathrm{E}}_{{\Phi }_k^{\left(-\Delta \right)}}\left[\log q\left({\Phi }_k\right)\right]+c_{\Delta }$$

(29)

where $\mathrm{E}[·]$ is referred to the expectation operation, ${\Phi }_k^{\left(-\Delta \right)}$ denotes the set of elements of ${\Phi }_k$ except for $\Delta$, and $c_{\Delta }$ represents the constant terms that are independent of $\Delta$.

Assume $\Delta ={\mathit{x}}_k$ and substitute (28) into (29) to obtain:

$$\begin{gathered} \log q^{\left(j+1\right)}\left({\mathit{x}}_k\right)=-0.5{\left({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1}\right)}^T{P}_{k|k-1}^{-1}\left({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1}\right)-0.5\{{\mathrm{E}}^{\left(j\right)}\left[y_k\right] \\ +\left(1-{\mathrm{E}}^{\left(j\right)}\left[y_k\right]\right){\mathrm{E}}^{\left(j\right)}\left[{\lambda }_k\right]\}{\left({\mathit{z}}_k-{\widehat{\mathit{z}}}_{k|k-1}\right)}^{\mathit{T}}{\mathrm{E}}^{\left(j\right)}\left[R_k^{-1}\right]\left({\mathit{z}}_k-{\widehat{\mathit{z}}}_{k|k-1}\right)+c_{{\mathit{x}}_k} \end{gathered}$$

(30)

where $j$ is the fixed-point iteration index in the variational parameters update. The posterior PDF of $q^{\left(j+1\right)}\left({\mathit{x}}_k\right)$ is subject to Gaussian distribution and can be calculated based on $q^{\left(j\right)}\left({\Phi }_k^{\left(-{\mathit{x}}_k\right)}\right)$. If $q^{\left(j+1\right)}\left({\mathit{x}}_k\right)=N\left({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k}^{\left(j+1\right)},P_{k|k}^{\left(j+1\right)}\right)$, then ${\widehat{\mathit{x}}}_{k|k}^{\left(j+1\right)}$, $P_{k|k}^{\left(j+1\right)}$ can be updated based on (14)-(16) with the moments calculated by (9)-(13), where $R_k$ is replaced with:

$${\overline{R}}_k^{\left(j+1\right)}={\left({\mathrm{E}}^{\left(j\right)}\left[R_k^{-1}\right]\right)}^{-1}/\left\{{\mathrm{E}}^{\left(j\right)}\left[y_k\right]+\left(1-{\mathrm{E}}^{\left(j\right)}\left[y_k\right]\right){\mathrm{E}}^{\left(j\right)}\left[{\lambda }_k\right]\right\}$$

(31)

Assume $\Delta =y_k$ and substitute (28) into (29) to yield:

$$\begin{gathered} \log q^{\left(j+1\right)}\left(y_k\right)=y_k\left\{{\mathrm{E}}^{\left(j\right)}\left[\log {\pi }_k\right]-0.5\mathrm{tr}\left(B_k^{\left(j+1\right)}{\mathrm{E}}^{\left(j\right)}\left[R_k^{-1}\right]\right)\right\}+\left(1-y_k\right) \\ \times \left\{{\mathrm{E}}^{\left(j\right)}\left[\log \left(1-{\pi }_k\right)\right]+0.5m{\mathrm{E}}^{\left(j\right)}\left[\log {\lambda }_k\right]-0.5{\mathrm{E}}^{\left(j\right)}\left[{\lambda }_k\right]\mathrm{tr}\left(B_k^{\left(j+1\right)}{\mathrm{E}}^{\left(j\right)}\left[R_k^{-1}\right]\right)\right\}+c_{y_k} \end{gathered}$$

(32)

where $B_k^{\left(j+1\right)}={\mathrm{E}}^{\left(j+1\right)}\left[\left({\mathit{z}}_k-{\widehat{\mathit{z}}}_{k|k-1}\right){\left({\mathit{z}}_k-{\widehat{\mathit{z}}}_{k|k-1}\right)}^{\mathit{T}}\right]$, with the result that:

$$\begin{gathered} P^{\left(j+1\right)}\left(y_k=0\right)=\mathrm{A}·\exp \{{\mathrm{E}}^{\left(j\right)}\left[\log \left(1-{\pi }_k\right)\right]+0.5m{\mathrm{E}}^{\left(j\right)}\left[\log {\lambda }_k\right] \\ -0.5{\mathrm{E}}^{\left(j\right)}\left[{\lambda }_k\right]\mathrm{tr}\left(B_k^{\left(j+1\right)}{\mathrm{E}}^{\left(j\right)}\left[R_k^{-1}\right]\right)\} \end{gathered}$$

(33)

$$P^{\left(j+1\right)}\left(y_k=1\right)=\mathrm{A}·\exp \left\{{\mathrm{E}}^{\left(j\right)}\left[\log {\pi }_k\right]-0.5\mathrm{tr}\left(B_k^{\left(j+1\right)}{\mathrm{E}}^{\left(j\right)}\left[R_k^{-1}\right]\right)\right\}$$

(34)

where A is the normalizing constant and $\mathrm{tr}(·)$ is the trace operation of a matrix. Since $q^{\left(j+1\right)}\left(y_k\right)$ is subject to Bernoulli distribution, the expectation of $y_k$ can be calculated as:

$${\mathrm{E}}^{\left(j+1\right)}\left[y_k\right]=\frac{P^{\left(j+1\right)}\left(y_k=1\right)}{P^{\left(j+1\right)}\left(y_k=0\right)+P^{\left(j+1\right)}\left(y_k=1\right)}$$

(35)

Assume $\Delta ={\pi }_k$, and substitute (28) into (29) to obtain:

$$\begin{gathered} \mathrm{Log}{q}^{\left(j+1\right)}\left({\pi }_k\right)=\left\{{\mathrm{E}}^{\left(j+1\right)}\left[y_k\right]+e_0-1\right\}\log {\pi }_k \\ +\left\{2-e_0-{\mathrm{E}}^{\left(j+1\right)}\left[y_k\right]-1\right\}\log \left(1-{\pi }_k\right)+c_{{\pi }_k} \end{gathered}$$

(36)

where the posterior PDF $q^{\left(j+1\right)}\left({\pi }_k\right)$ is defined as a Beta distribution, i.e., $q^{\left(j+1\right)}\left({\pi }_k\right)=Be\left({\pi }_k;e_k^{\left(j+1\right)},f_k^{\left(j+1\right)}\right)$, with the result that:

$$e_k^{\left(j+1\right)}=e_0+{\mathrm{E}}^{\left(j+1\right)}\left[y_k\right]$$

(37)

$$f_k^{\left(j+1\right)}=2-e_0-{\mathrm{E}}^{\left(j+1\right)}\left[y_k\right]$$

(38)

$${\mathrm{E}}^{\left(j+1\right)}\left[\log {\pi }_k\right]=\mathsf{\Psi }\left(e_k^{\left(j+1\right)}\right)-\mathsf{\Psi }\left(e_k^{\left(j+1\right)}+f_k^{\left(j+1\right)}\right)$$

(39)

$${\mathrm{E}}^{\left(j+1\right)}\left[\log \left(1-{\pi }_k\right)\right]=\mathsf{\Psi }\left(f_k^{\left(j+1\right)}\right)-\mathsf{\Psi }\left(e_k^{\left(j+1\right)}+f_k^{\left(j+1\right)}\right)$$

(40)

where $\mathsf{\Psi }(·)$ denotes the Digamma function.

Assume $\Delta ={\lambda }_k$ and substitute (28) into (29) to yield:

$$\begin{gathered} \mathrm{Log}{q}^{\left(j+1\right)}\left({\lambda }_k\right)=\left\{0.5p\left(1-{\mathrm{E}}^{\left(j+1\right)}\left[y_k\right]\right)+0.5{\nu }_k-1\right\}\log {\lambda }_k \\ -\left\{0.5\mathrm{tr}\left(B_k^{\left(j+1\right)}{\mathrm{E}}^{\left(j\right)}\left[R_k^{-1}\right]\right)\left(1-{\mathrm{E}}^{\left(j+1\right)}\left[y_k\right]\right)+0.5{\nu }_k\right\}{\lambda }_k+c_{{\lambda }_k} \end{gathered}$$

(41)

where the posterior PDF $q^{\left(j+1\right)}\left({\lambda }_k\right)=G\left({\lambda }_k;{\alpha }_k^{\left(j+1\right)},{\beta }_k^{\left(j+1\right)}\right)$, with the result that:

$${\alpha }_k^{\left(j+1\right)}=0.5p\left(1-{\mathrm{E}}^{\left(j+1\right)}\left[y_k\right]\right)+0.5{\nu }_k$$

(42)

$${\beta }_k^{\left(j+1\right)}=0.5\mathrm{tr}\left(B_k^{\left(j+1\right)}{\mathrm{E}}^{\left(j\right)}\left[R_k^{-1}\right]\right)\left(1-{\mathrm{E}}^{\left(j+1\right)}\left[y_k\right]\right)+0.5{\nu }_k$$

(43)

$${\mathrm{E}}^{\left(j+1\right)}\left[{\lambda }_k\right]=\frac{{\alpha }_k^{\left(j+1\right)}}{{\beta }_k^{\left(j+1\right)}}$$

(44)

$${\mathrm{E}}^{\left(j+1\right)}\left[\log {\lambda }_k\right]=\mathsf{\Psi }\left({\alpha }_k^{\left(j+1\right)}\right)-\log {\beta }_k^{\left(j+1\right)}$$

(45)

Assume $\Delta =R_k$ and substitute (28) into (29) to obtain:

$$\begin{gathered} \mathrm{Log}{q}^{\left(j+1\right)}\left(R_k\right)=-0.5\left(t_{k|k-1}+p+2\right)\log \left|R_k\right|-0.5\mathrm{tr}(\{T_{k|k-1} \\ +\left[{\mathrm{E}}^{\left(j+1\right)}\left[{\lambda }_k\right]+\left(1-{\mathrm{E}}^{\left(j+1\right)}\left[{\lambda }_k\right]\right){\mathrm{E}}^{\left(j+1\right)}\left[y_k\right]\right]B_k^{\left(j+1\right)}\}{R}_k^{-1})+c_{R_k} \end{gathered}$$

(46)

where the posterior PDF $q^{\left(j+1\right)}\left(R_k\right)=IW\left(R_k;t_{k|k}^{\left(j+1\right)},T_{k|k}^{\left(j+1\right)}\right)$, with the result that:

$$t_{k|k}^{\left(j+1\right)}=t_{k|k-1}+1$$

(47)

$$T_{k|k}^{\left(j+1\right)}=T_{k|k-1}+\left[{\mathrm{E}}^{\left(j+1\right)}\left[{\lambda }_k\right]+\left(1-{\mathrm{E}}^{\left(j+1\right)}\left[{\lambda }_k\right]\right){\mathrm{E}}^{\left(j+1\right)}\left[y_k\right]\right]B_k^{\left(j+1\right)}$$

(48)

$${\mathrm{E}}^{\left(j+1\right)}\left[R_k^{-1}\right]=t_{k|k}^{\left(j+1\right)}/T_{k|k}^{\left(j+1\right)}$$

(49)

The steps for the ROCRCKF based on measurement outlier adaptive detection can be summarized as Algorithm 2. Notably, the final output ${\mathrm{E}}^{\left(j+1\right)}\left[R_k^{-1}\right]$ and $q^{\left(j+1\right)}\left({\mathit{x}}_k\right)$ are employed in the calculation of ${\xi }_{i,k+1}$, making the resampling-free SUF insensitive to the time-varying noise and measurement outliers. Moreover, the observability-constrained resampling-free SUF combats the inconsistent estimation problem of outlier-robust filters by directly modifying the sigma points. The computational complexity of the RCKF has been discussed in our previous work [31], and is on the same order as the CKF in terms of floating-point operations. The difference between the ROCRCKF and the RCKF lies in the measurement update step only, and the increasing complexity of the ROCRCKF depends on the selection of $J_{ma\mathit{x}}$, which is a tradeoff between expected accuracy and efficiency.

Algorithm 2 The pseudocode of ROCRCKF

Inputs: ${\widehat{\mathit{x}}}_{k-1|k-1}$$,P_{k-1|k-1}$$,Q_{k-1}$$,R_k$$,e_0$$,v_k$$,t_{k-1|k-1}$$,T_{k-1|k-1}$$,J_{ma\mathit{x}}$ 1. Initialize ${\tilde{X}}_{i,k-1|k-1}^{+}$ based on CKF and update $X_{i,k-1|k-1}^{+}$ by (18) Time update: 2. Calculate ${\widehat{\mathit{x}}}_{k|k-1}$,$P_{k|k-1}$, $X_{i,k+1|k}$ follow RCKF and propagate $t_{k|k-1}$, $T_{k|k-1}$ Measurement update 3. Initialization: ${\widehat{\mathit{x}}}_{k|k}^{\left(1\right)}={\widehat{\mathit{x}}}_{k|k-1}$, ${\mathrm{E}}^{\left(1\right)}\left[{\lambda }_k\right]=1$, ${\mathrm{E}}^{\left(1\right)}\left[\log {\lambda }_k\right]=0$, ${\mathrm{E}}^{\left(1\right)}\left[R_k^{-1}\right]=t_{k-1|k-1}{T}_{k-1|k-1}^{-1}$   ${\mathrm{E}}^{\left(1\right)}\left[\log {\pi }_k\right]=\mathsf{\Psi }\left(e_0\right)-\mathsf{\Psi }\left(1\right)$, ${\mathrm{E}}^{\left(1\right)}\left[\log \left(1-{\pi }_k\right)\right]=\mathsf{\Psi }\left(1-e_0\right)-\mathsf{\Psi }\left(1\right)$, ${\mathrm{E}}^{\left(1\right)}\left[y_k\right]=1$ For $j=1:J_{ma\mathit{x}}$ 4. Update $q^{\left(j+1\right)}\left[{\mathit{x}}_k\right]$ as Gaussian distribution based on (30)   Calculate ${\overline{R}}_k^{\left(j+1\right)}$ by (31), and update ${\widehat{\mathit{x}}}_{k|k}^{\left(j+1\right)}$, $P_{k|k}^{\left(j+1\right)}$ by (10)–(12) 5. Update $q^{\left(j+1\right)}\left[y_k\right]$ as Bernoulli distribution based on (32)   Calculate $B_k^{\left(j+1\right)}$ and update ${\mathrm{E}}^{\left(j+1\right)}\left[y_k\right]$ by (35) 6. Update $q^{\left(j+1\right)}\left[{\pi }_k\right]$ as Beta distribution based on (36)   Calculate $a_k^{\left(j+1\right)}$, $f_k^{\left(j+1\right)}$ by (37) and (38)   Calculate ${\mathrm{E}}^{\left(j+1\right)}\left[\log {\pi }_k\right]$, ${\mathrm{E}}^{\left(j+1\right)}\left[\log \left(1-{\pi }_k\right)\right]$ by (39) and (40) 7. Update $q^{\left(j+1\right)}\left[{\lambda }_k\right]$ as Gamma distribution based on (41)   Calculate ${\alpha }_k^{\left(j+1\right)}$, ${\beta }_k^{\left(j+1\right)}$, ${\mathrm{E}}^{\left(j+1\right)}\left[{\lambda }_k\right]$ and ${\mathrm{E}}^{\left(j+1\right)}\left[\log {\lambda }_k\right]$ by (42)–(45) 8. Update $q^{\left(j+1\right)}\left[R_k\right]$ as inverse Wishart distribution based on (46)   Calculate $t_{k|k}^{\left(j+1\right)}$, $T_{k|k}^{\left(j+1\right)}$ by (47), (48) and update ${\mathrm{E}}^{\left(j+1\right)}\left[R_k^{-1}\right]$ by (49) End for 9. Update: ${\widehat{\mathit{x}}}_{k|k}={\widehat{\mathit{x}}}_{k|k}^{\left(j+1\right)}$, $P_{k|k}=P_{k|k}^{\left(j+1\right)}$, $t_{k|k}=t_{k|k}^{\left(j+1\right)}$, $T_{k|k}=T_{k|k}^{\left(j+1\right)}$ 10. Update $X_{i,k|k}^{+}$ by (23) and return to Step 2 with $k=k+1$ Outputs: ${\widehat{\mathit{x}}}_{k|k}$, $P_{k|k}$, $X_{i,k|k}^{+}$, $t_{k|k}$, $T_{k|k}$

4. Experiment Results and Analysis

Although the VB-based adaptive and robust filters for outlier detection and non-Gaussian noise have been widely tested by numerical simulation in existing works [28,29,36], they are seldom tested in practical application. We employ a car-mounted GNSS/INS to verify the effectiveness of the ROCRCKF in this section, where two trajectories with different maneuver modes and measurement outages are investigated.

4.1. Filter Model of the GNSS/INS

The dynamic model GNSS/INS is based on the error propagation model of the INS. In this paper, the navigation state is expressed in an Earth-centered, Earth-fixed frame (e-frame), the body frame (b-frame) is defined as right-forward-up, and the IMU is fixed to the vehicle axis, which is aligned with the b-frame. The state at time index k can be written as ${\mathit{x}}_k={\left[\begin{array}{ccc}\delta {\mathit{\psi }}^T& \delta {\mathit{v}}^T& \begin{array}{ccc}\delta {\mathit{p}}^T& {\mathit{b}}_a^T& {\mathit{b}}_g^T\end{array}\end{array}\right]}^T$, which denotes the error of attitude, velocity, position, and biases of the triaxial accelerometer and gyroscope, respectively. The corresponding error propagation model of the GNSS/INS is formulated as:

$$\delta \dot{\mathit{\psi }}=-{\mathit{\omega }}_{ie}^e\times \delta \mathit{\psi }+C_b^e{\mathit{b}}_g$$

(50)

$$\delta \dot{\mathit{v}}=-\left(C_b^e{\mathit{f}}_{ib}^b\right)\times \delta \mathit{\psi }-\mathbf{2}{\mathit{\omega }}_{ie}^e\times \delta \mathit{v}+\frac{2g_0}{r_{eS}^e}\frac{p}{{\left|p\right|}^2}{\mathit{p}}^T+C_b^e{\mathit{b}}_a$$

(51)

$$\delta \dot{\mathit{p}}=\delta \mathit{v},{\dot{\mathit{b}}}_g=0,{\dot{\mathit{b}}}_a=0$$

(52)

where the time is omitted for brevity; ${\mathit{\omega }}_{ie}^e$ is the angular rate of the Earth, which is expressed with respect to the geocentric inertial frame (i-frame) and resolved in the e-frame; $C_b^e$ denotes the rotation transform from the b-frame to the e-frame; ${\mathit{f}}_{ib}^b$ represents the specific force measured by accelerometers with respect to the i-frame resolved in the b-frame; $g_0$ indicates the local gravity and $r_{eS}^e$ is the geocentric radius at the Earth’s surface; $\mathit{a}\times$ denotes the skew symmetric matrix of vector $\mathit{a}$; and ${\mathit{a}}^{\mathit{T}}$ and $\left|\mathit{a}\right|$ denote the transpose and magnitude value of $\mathit{a}$, respectively.

The measurement model of the GNSS/INS can be written as:

$$\delta {\mathit{z}}_{k,GNSS/INS}=\left(\begin{array}{c}{\tilde{\mathit{p}}}_{G,k}-{\tilde{\mathit{p}}}_{I,k}-{\tilde{C}}_b^e{l}_{ba}^b\\ {\tilde{\mathit{v}}}_{G,k}-{\tilde{\mathit{v}}}_{I,k}-{\tilde{C}}_b^e\left({\tilde{\mathit{\omega }}}_{ib}^b\times l_{ba}^b\right)+{\mathit{\omega }}_{ie}^e\times {\tilde{C}}_b^{\mathit{e}}{l}_{ba}^b\end{array}\right)$$

(53)

where $\tilde{\mathit{a}}$ indicates $\mathit{a}$ contains a measurement error; ${\tilde{\mathit{p}}}_{G,k}$ and ${\tilde{\mathit{v}}}_{G,k}$ are the position and velocity measurements from the GNSS receiver resolved in the e-frame; ${\tilde{\mathit{p}}}_{I,k}$, ${\tilde{\mathit{v}}}_{I,k}$, ${\tilde{C}}_b^e$, and ${\tilde{\mathit{\omega }}}_{ib}^b$ are the solutions of the INS after state error correction; $l_{ba}^b$ is the lever arm of the antenna with respect to the IMU resolved in the b-frame. The dynamical model of the GNSS/INS can be formulated based on (50)–(52), while Equation (53) formulates the measurement model [39].

4.2. Experiment and Result Analysis

The experimental vehicle and setups for the field test are shown in Figure 1, where the raw sensor data of the NovAtel SPAN system were first recorded, then post-processed by the Inertial Explorer (IE) software of NovAtel and proposed as nonlinear filters, respectively. In cases where the SPAN system operates in the GNSS/INS tightly coupled mode, it outputs the position and heading continuously within 2 cm and 0.02 degrees, which are taken as the reference trajectories in this work. The IMU of the involved SPAN system includes a triaxial interferometric fiber optic gyroscope and a triaxial MEMS accelerometer; the specific parameters of the IMU are listed in

Table 1. Parameter information of the IMU.

Sensor	Characteristic	Value
Gyroscope	Rate bias	1 deg/h
	Rate scale factor	100 ppm
	Angular random walk	0.07 deg/$\sqrt{\mathrm{h}}$
Accelerometer	Bias	0.3 mg
	Scale factor	300 ppm

. The frequencies of the GNSS, IMU, and reference trajectory are 5 Hz, 200 Hz, and 100 Hz.

In this section, two trajectories are employed to verify the observability-constrained resampling-free SUF and outlier-robust measurement update of the ROCRCKF. The VBCKF [19], MCCKF [23], and RCKF [31] are employed as comparisons to verify the superiority of the ROCRCKF in terms of adaptivity and robustness. The forgetting factor is set as $\rho =1-\mathit{e}\mathit{x}p\left(-4\right)$ and the iteration number is $J_{ma\mathit{x}}=20$ for the VBCKF and ROCRCKF. The other parameters of the ROCRCKF are ${\mathit{e}}_0=0.85$, the dof ${\mathit{v}}_k=5$, $t_{0|0}=p+1+\tau$, $T_{0|0}=\tau ·R_0$, and $\tau =3$. $R_0$ denotes the initial measurement noise covariance for the VBCKF and ROCRCKF, and is also the constant noise covariance for the CKF, MCCKF, and RCKF. The kernel bandwidth and iteration number of the MCCKF are selected as 400 and 4. The same ${\Lambda }_k$ is set for the RCKF and ROCRCKF, where for position and velocity, the corresponding ${\eta }_{\ell }$ values are set as 1.01 and the other diagonal elements of ${\Lambda }_{\mathrm{k}}$ are set as 0. The other parameters in the filter settings, except for the above-mentioned parameters, share the same values, and all of the parameters are pre-determined by a series of tuning attempts to make a fair comparison.

The first trajectory is shown in Figure 2, where there are frequent GNSS outages in the test, during which the filters work on prediction mode and thus the SUF based on the approximated moments of the posterior PDF is less efficient. The raw IMU output is shown on the right panel, indicating that the vehicle is still at the initial stage of the test, where $\omega$ and $f$ denote the angular rate and specific force. Notably, because of the turning and vibration of the land vehicle there are obvious outliers in the observations of the IMU. Figure 3, Figure 4 and Figure 5 compare the attitude and position error of different nonlinear filters. Notably, the ROCRCKF showcases better performance than the RCKF in terms of attitude, which not only converges faster at the initial stage but also achieves smaller stability errors. It is well recognized that the gyroscope bias in the direction of gravity is unobservable when the land vehicle runs along a straight horizontal line at constant velocity [40]. Consequently, the heading error accumulates with time in cases where the land vehicle runs along a straight line at a constant speed.

The output trajectories of different nonlinear filters for trajectory I are shown in Figure 6. As we can see from the regional enlarged views in Figure 6, both the RCKF and ROCRCKF showcase better position results than the VBCKF and CKF during the turn, which is not only because the accelerometer bias is well compensated but also because the velocity propagation error is reduced by the small heading error. The observability of the heading is low even during steady turns, whereas its accuracy can be improved due to the enhanced observability of tilt angles. Notably, the RCKF performs better than the ROCRCKF in the occurrence of Outage #2, where the outage duration is short enough (less than 10 s) and there are sufficient observations for the RCKF converge to the steady state before another outage. However, in cases where the outage duration is increased, e.g., longer than 30 s, as shown in Outage #3, once the observations are occasionally provided, the RCKF cannot assimilate the innovation efficiently and shows degraded performance compared to the CKF and VBCKF. The sigma points of the CKF, VBCKF, and MCCKF depend on the time update stage of the KF framework, which leads to filtering divergence gradually because of the uncompensated dynamic uncertainty. However, this is not the case for resampling-free SUFs, which modify the sigma points based on (23) by considering the state observability and propagating more information by using the prediction residue of the dynamic model.

More specific estimation results of different filters are listed in

Table 2. The RMSEs of different nonlinear filters for trajectory I.

Method	Roll (°)	Pitch (°)	Heading (°)	ARMSEpos (m)
CKF	0.045	0.059	0.44	2.61
RCKF	0.045	0.065	0.17	2.50
MCCKF	0.059	0.080	0.38	5.48
VBCKF	0.046	0.059	0.48	2.53
ROCRCKF	0.030	0.041	0.04	2.55

. The root mean square error (RMSE) is formulated as:

$${\mathrm{RMSE}}_{\chi }=\sqrt{\frac{1}{M}{{\displaystyle \sum }}_{k=1}^M{\left({\widehat{\chi }}_k-{\chi }_k\right)}^2}$$

(54)

where M is the total number of data sets, $\chi$ indicates the corresponding navigation state, and ${\widehat{\chi }}_k$ and ${\chi }_k$ denote the estimated and reference values at time k, respectively. The average RMSE(ARMSE) is taken as the performance metric for the position result, which is defined as:

$${\mathrm{ARMSE}}_{pos}=\sqrt{\frac{1}{2}\left({\mathrm{RMSE}}_{EP}^2+{\mathrm{RMSE}}_{NP}^2\right)}$$

(55)

where ${\mathrm{RMSE}}_{EP}$ and ${\mathrm{RMSE}}_{NP}$ denote the RMSE of positions in the east and north directions. Notably, in the enlarged image of the left panel of Figure 5, the ROCRCKF converges quickly at the initial stage of the trajectory in cases where the vehicle does not move. By efficiently assimilating the innovation and relieving the covariance inconsistence problem, the ROCRCKF shows better attitude and heading results than its resampling SUF-based counterparts. However, as there is no extra correction information for the states of strong observability during GNSS outages, the ROCRCKF does not improve the position and velocity results compared with the RCKF.

The second trajectory is presented in Figure 7, where the navigation state observability is time-varying and the observability of the attitude is enhanced by frequent maneuver variation. The same parameters as that of trajectory I are employed in the filter configuration and only the observations are different for the comparative analysis of the observability-constrained resampling-free SUF. The results in terms of attitude and position are shown in Figure 8, Figure 9 and Figure 10. Notably, compared with the CKF, the VBCKF achieves slightly better position results but not better attitude results. Both the RCKF and ROCRCKF outperform the VBCKF in terms of attitude and position, and the MCCKF suffers from GNSS outages more obviously than the CKF and VBCKF, which coincides with the results shown in trajectory I. The MCCKF re-scales the uncertainty propagation of measurement updates based on the kernel function, whose performance heavily depends on the selection of kernel parameters, which is less effective in handling non-stationary non-Gaussian noise. Notably, in cases where the car is stopped, the observability constraint causes the direct observable state to approach the actual condition of the vehicle maneuver, which can be seen from the position result in Figure 10. However, the correction information of attitude error and sensor biases comes from prediction covariance, and both the RCKF and the ROCRCKF achieve better attitude results than their resampling-based SUF counterparts.

The output trajectories of different nonlinear filters for trajectory II are shown in Figure 11. As we can see from the regional enlarged views in Figure 11, the ROCRCKF showcases better position results than the RCKF and VBCKF during the turn (Outage #2), which coincides with the result presented in trajectory I. The MCCKF showcases better results than the RCKF, which may be because the MCCKF assimilates the occasionally provided observations efficiently, and the unmodeled dynamic model can be neglected when the vehicle runs in a straight line. The heading accuracy is improved due to the enhanced observability of tilt angles during the frequent turns shown in Figure 11, and thus during Outage #1 all the algorithms achieve acceptable position results. More specific results are listed in

Table 3. The RMSEs of different nonlinear filters for trajectory II.

Method	Roll (°)	Pitch (°)	Heading (°)	ARMSEpos (m)
CKF	0.091	0.16	2.60	7.30
RCKF	0.057	0.10	1.96	6.38
MCCKF	0.105	0.15	2.00	6.18
VBCKF	0.097	0.16	2.94	7.13
ROCRCKF	0.058	0.08	0.27	5.95

. Notably, the heading and position errors of the CKF are reduced from 2.6° and 7.3 m to 1.96° and 6.38 m by the RCKF, where the position improvement is more significant than that of the VBCKF. The RCKF reduces the ARMSE_pos of the VBCKF by 10.5%, where the corresponding value for the first trajectory is 1.2%, which confirms that the resampling-free SUF based on the dynamic prediction residue propagates more information than the SUF based on approximated Gaussian moments. Furthermore, the ROCRCKF outperforms the RCKF and VBCKF in both attitude and position results, where it reduces the position error by 6.7% and 16.5%, respectively. Compared with the results in Table 2, the attitude and heading results of the ROCRCKF shown in Table 3 do not degrade as heavily as those of the RCKF and VBCKF using the same filter configuration, which demonstrates the adaptivity and robustness of the ROCRCKF.

5. Discussion

Notably, the RCKF does not outperform the CKF and VBCKF in terms of roll and pitch in case the maneuver of vehicle is not high, whereas it achieves better position and heading results, especially when there is a GNSS outage. On one hand, the RCKF relieves the covariance inconsistence problem of the CKF by setting the ${\Lambda }_k$ when constructing the posterior sigma points, which makes the prediction covariance less sensitive to the change in innovation; on the other hand, it cannot assimilate the observation information efficiently once the measurement noise is small enough. The VBCKF improves the position result slightly compared with the CKF, which, however, does not outperform the CKF in terms of attitude and heading. This result coincides with the fact that the correction information of attitude comes from the off-diagonal element of prediction state covariance, which is updated at the prediction stage of nonlinear filters. The estimation results of the MCCKF have a direct relation to the duration of the outage, and in the field test based on trajectory I, it achieves similar heading results as the CKF but much worse position results, which indicates that the MCC-based measurement update cannot address the uncertainty propagation in cases of frequent GNSS outages. Generally, the measurement outliers are sparse in the GNSS/INS, and in cases where the noise is subject to Gaussian distribution, the MCC-based measurement update is sub-optimal and does not perform well. However, in Table 3, unlike the result listed in Table 2, the MCCKF achieves better heading and position results than the CKF and VBCKF, which indicates that the uncertainty or non-Gaussian process noise from the dynamic model has a significant effect on the state estimation of the GNSS/INS.

Notably, in Figure 5 and Figure 10 the position error of the ROCRCKF in the north direction are noisy, and the position error may increase abruptly when the observations reappear after a GNSS outage. The reason behind these phenomena may be that the measurement outlier in different direction behaves differently, whereas the same scalar scale ${\lambda }_k$ is applied for ${\mathrm{E}}^{\left(j\right)}\left[R_k\right]$. The estimation results of the binary indicator ${\mathrm{E}}^{\left(j\right)}\left[y_k\right]$ and corresponding noise covariance of the position for trajectory II are shown in Figure 12. Notably, the GNSS outages are detected accurately as they coincide with the outages indicated in the left panel of Figure 7. Moreover, the noise covariance diagonal element is increased in the presence of missing observations. Although there is no state update during the GNSS outage, the parameters in ${\Phi }_k$ are propagated in the prediction model during these periods, which is also helpful for the sigma point direct modification of the ROCRCKF. As we can see, based on the same filter model, filter configuration, and sensors, the state estimation result of trajectory II is worse than that of trajectory I, which indicates the unmodeled dynamic uncertainty has a significant effect on the state estimation of the GNSS/INS. The comparison analysis between trajectory I and II indicates that the ${\Lambda }_k$ should be adjusted according to the instantaneous state observability, and thus the observability quantifying analysis based on the real-time degree of observability calculation is under consideration.

6. Conclusions

This article presented an improved observability-constrained resampling-free CKF, the ROCRCKF, for the state estimation of a GNSS/INS with measurement outliers. The binary indicator of outliers, scale parameter of noise covariance, and state are estimated simultaneously based on variational Bayesian inferences. The proposed algorithm is verified by employing a car-mounted GNSS/INS, where two trajectories of different state observability are analyzed and compared. Experiment results demonstrate that, cases where there are frequent GNSS outages, the ROCRCKF outperforms the RCKF and VBCKF in terms of attitude without obvious position degradation. Moreover, when there is high uncertainty in the dynamic model prediction, the ROCRCKF achieves much better results than the RCKF and VBCKF in terms of heading and position results. The vehicle dynamic model-derived measurement should also be taken into consideration for the information fusion of land vehicle navigation, and the dynamic model prediction residue employed in the resampling-free SUF provided a new measure for the vehicle dynamic identification. In future works, multiple indicators of outliers should be employed, as the measurement outliers and noise characteristics may be independent in the observation domain.