Adaptive Maximum Correntropy Gaussian Filter Based on Variational Bayes

Abstract

In this paper, we investigate the state estimation of systems with unknown covariance non-Gaussian measurement noise. A novel improved Gaussian filter (GF) is proposed, where the maximum correntropy criterion (MCC) is used to suppress the pollution of non-Gaussian measurement noise and its covariance is online estimated through the variational Bayes (VB) approximation. MCC and VB are integrated through the fixed-point iteration to modify the estimated measurement noise covariance. As a general framework, the proposed algorithm is applicable to both linear and nonlinear systems with different rules being used to calculate the Gaussian integrals. Experimental results show that the proposed algorithm has better estimation accuracy than related robust and adaptive algorithms through a target tracking simulation example and the field test of an INS/DVL integrated navigation system.

1. Introduction

As the benchmark work in state estimation problems, the linear recursive Kalman filter (KF) has been applied in various applications, such as information fusion, system control, integrated navigation, target tracking, and GPS solutions [1,2,3,4]. It is then extended to nonlinear systems through different ways to approximate the nonlinear functions or filtering distributions. Using the Taylor series to linearize the nonlinear functions, the popular extended Kalman filter (EKF) [5] is obtained. To further improve the estimation accuracy of EKF, several sigma points based nonlinear filters have been proposed in recent decades, such as unscented Kalman filter (UKF) using unscented transform [6], cubature Kalman filter (CKF) according to cubature rules [7], and divided difference Kalman filter (DDKF) adopting the polynomial approximations [8]. All these filters can be regarded as special cases of Gaussian filter (GF) [9,10,11], where the noise distribution is assumed to be Gaussian.

However, when the measurements are polluted by non-Gaussian noise, such as impulsive inference or outliers, GF will have worse estimation results and even break down [12,13]. Besides the computation extensive methods including particle filter [14,15], Gaussian sum filter [16], and multiple model filters [17], the robust filters, such as Huber’s KF (HKF, also known as M-estimation) [18,19,20] and $H_{\infty }$ filter [21], are also intended for the contaminated measurements. Although the $H_{\infty }$ filter can obtain guaranteed bounded estimation error, it does not perform well under Gaussian noise [22]. The Huber’s M-estimation is a combined $l_1$ and $l_2$ norm filter that can effectively suppress the non-Gaussian noise [18,19,20]. Recently, the information theoretical measure correntropy has been used to incorporate the non-Gaussian noise [23,24,25,26,27,28]. According to the maximum correntropy criterion (MCC), a new robust filter known as the maximum correntropy Kalman filter (MCKF) has been proposed in [24], and it is also extended to nonlinear systems using EKF [27] and UKF [25,26,28]. Simulation results show that an MCC based GF (MCGF) may obtain better estimation accuracy than M-estimation when choosing a proper kernel bandwidth [23,24,25,26]. Even so, both MCGF and M-estimation still require the information of nominal measurement noise covariance, which may be unknown or time varying in some applications. In this situation, the performance of MCGF will degrade as shown in our simulation examples.

Traditionally, the unknown noise statistic can be estimated by the adaptive filter, such as the Sage–Husa filter [29] and fading memory filter [30]. One drawback of these recursion adaptive filters is that the previously estimated statistic of the last time instant will influence current estimation, which is not suitable for the case measurement noise having frequently changing statistics [31]. The recently proposed variational Bayes (VB) based adaptive filter avoids this limitation by VB approximation, and VB based GFs (VBGFs) for both linear [32] and nonlinear systems [9,33] have been proposed.

In this paper, we proposed an adaptive MCGF based on the VB approximation, which is especially useful for estimating the system state from the measurements with unknown covariance non-Gaussian noise. Typical applications include low-cost INS/GPS integrated navigation systems [32] and maneuvering target tracking [33]. To overcome the limitation of MCGF under unknown time varying measurement noise covariance, the VB method is utilised to improve the adaptivity of MCGF, which is achieved through the fixed-point iteration framework. As will be demonstrated in our simulation results, our proposed method has better estimation accuracy than related algorithms. Furthermore, various filters can be obtained by using different ways to calculate the Gaussian integrals.

The rest of this paper is given as follows. In Section 2, after briefly introducing the concept of correntropy, we give the general MCGF algorithm. In Section 3, we explain the main idea of the VB method and the procedure of embedding it into MCGF to obtain our proposed adaptive MCGF. Section 4 gives the experimental results of a typical target tracking model and an INS/DVL navigation system comparing with several related algorithms. Conclusions are made in the final section.

2. Gaussian Filter Based on the Maximum Correntropy Criterion

2.1. Correntropy

As a kind of similarity measure, the correntropy of random variables X and Y is defined as [24,25,26]

$$V(X,Y)=E\left[\kappa (X,Y)\right]=\int \int \kappa (x,y)\mathrm{d}{F}_{X,Y}(x,y),$$

(1)

where $E[·]$ denotes expectation, $F_{X,Y}(x,y)$ is the joint density function, and $\kappa (x,y)$ represents the Mercer kernel. The most popular Gaussian kernel is given as the following:

$$\kappa (x,y)=G_{\sigma }\left(e\right)=\exp (-\frac{e^2}{2{\sigma }^2}),$$

(2)

where $e=x-y$, and $\sigma >0$ is the kernel bandwidth.

Then, taking the Taylor series expansion on the Gaussian correntropy, we obtain that

$$V(X,Y)=\sum _{n=0}^{\infty }\frac{{(-1)}^n}{2^n{\sigma }^{2n}n!}\mathrm{E}[{(X-Y)}^{2n}].$$

(3)

Obviously, it contains all the even moments of $X-Y$ weighted by the kernel bandwidth $\sigma$. It enables us to capture high order information when applying the correntropy in signal processing. In practice, we can use the sampling data to estimate the real correntropy since the joint density function is usually unavailable.

2.2. Maximum Correntropy Gaussian Filter

In this paper, we consider the following nonlinear system with additive noise

$$\begin{array}{c}{\mathit{x}}_i={\mathit{f}}_{i-1}\left({\mathit{x}}_{i-1}\right)+{\mathit{w}}_{i-1},\hfill \\ {\mathit{z}}_i={\mathit{h}}_i\left({\mathit{x}}_i\right)+{\mathit{v}}_i,\hfill \end{array}$$

(4)

where ${\mathit{x}}_i\in {\mathbb{R}}^n$ is the system state at time i and ${\mathit{z}}_i\in {\mathbb{R}}^m$ denotes the measurement. ${\mathit{f}}_i(·)$ and ${\mathit{h}}_i(·)$ represent the known nonlinear functions. In standard GF, the process noise ${\mathit{w}}_i$ and the measurement noise ${\mathit{v}}_i$ are assumed to be zero mean Gaussian noise sequences with known covariance ${\mathit{Q}}_i$ and ${\mathit{R}}_i$. The initial state ${\mathit{x}}_0$ has known mean ${\overline{\mathit{x}}}_0$ and covariance ${\mathit{P}}_0$.

In both GF and MCGF, the one step estimation ${\widehat{\mathit{x}}}_i^{-}$ and its estimation covariance ${\mathit{P}}_i^{-}$ are obtained through:

$${\widehat{\mathit{x}}}_i^{-}=\int {\mathit{f}}_{i-1}\left({\mathit{x}}_{i-1}\right)\mathcal{N}\left({\mathit{x}}_{i-1}\right|{\widehat{\mathit{x}}}_{i-1},{\mathit{P}}_{i-1})\mathrm{d}{\mathit{x}}_{i-1},$$

(5)

$${\mathit{P}}_i^{-}=\int ({\mathit{f}}_{i-1}\left({\mathit{x}}_{i-1}\right)-{\widehat{\mathit{x}}}_i^{-}){({\mathit{f}}_{i-1}\left({\mathit{x}}_{i-1}\right)-{\widehat{\mathit{x}}}_i^{-})}^{\mathrm{T}}\mathcal{N}\left({\mathit{x}}_{i-1}\right|{\widehat{\mathit{x}}}_{i-1},{\mathit{P}}_{i-1})\mathrm{d}{\mathit{x}}_{i-1}+{\mathit{Q}}_{i-1}.$$

(6)

To further improve the robustness of GF, MCC has been applied on the derivation of measurement update of MCGF. Consider the following regression model based on Equations (4)–(6) [25]:

$$\left[\begin{array}{c}{\widehat{\mathit{x}}}_i^{-}\\ {\mathit{z}}_i\end{array}\right]=\left[\begin{array}{c}{\mathit{x}}_i\\ {\mathit{h}}_i\left({\mathit{x}}_i\right)\end{array}\right]+\left[\begin{array}{c}\delta {\mathit{x}}_i\\ {\mathit{v}}_i\end{array}\right]$$

(7)

where $\delta {\mathit{x}}_i={{\widehat{\mathit{x}}}_i^{-}-\mathit{x}}_i$. The covariance of ${\left[\delta {\mathit{x}}_i{\mathit{v}}_i\right]}^{\mathrm{T}}$ is

$$\left[\begin{array}{cc}{\mathit{P}}_i^{-}& 0\\ 0& {\mathit{R}}_i\end{array}\right]=\left[\begin{array}{cc}{\mathit{S}}_{p,i}{\mathit{S}}_{p,i}^{\mathrm{T}}& 0\\ 0& {\mathit{S}}_{r,i}{\mathit{S}}_{r,i}^{\mathrm{T}}\end{array}\right]={\mathit{S}}_i{\mathit{S}}_i^{\mathrm{T}}.$$

(8)

Multiplying ${\mathit{S}}_i^{-1}$ on both sides of Equation (7), we obtain

$${\mathit{e}}_i={\mathit{D}}_i-{\mathit{W}}_i,$$

(9)

where ${\mathit{D}}_i={\mathit{S}}_i^{-1}\left[\begin{array}{c}{\widehat{\mathit{x}}}_i^{-}\\ {\mathit{z}}_i\end{array}\right]$ and ${\mathit{W}}_i={\mathit{S}}_i^{-1}\left[\begin{array}{c}{\mathit{x}}_i\\ {\mathit{h}}_i\left({\mathit{x}}_i\right)\end{array}\right]$.

Then, the optimal estimation ${\widehat{\mathit{x}}}_i$ under the MCC can be obtained through the following optimization problem:

$${\widehat{\mathit{x}}}_i=\underset{{\mathit{x}}_i}{argmax}{\mathit{J}}_L=\underset{{\mathit{x}}_i}{argmax}\sum _{k=1}^{m+n}{G}_{\sigma }\left(e_i\left(k\right)\right),$$

(10)

where $e_i\left(k\right)$ is the k-th element of ${\mathit{e}}_i$.

Equation (10) can be solved by:

$$\frac{\partial {\mathit{J}}_L}{\partial {\mathit{x}}_i}={\left(\frac{\partial {\mathit{h}}_i\left({\mathit{x}}_i\right)}{\partial {\mathit{x}}_i}\right)}^{\mathrm{T}}{\mathit{C}}_i({\mathit{D}}_i-{\mathit{W}}_i)=0,$$

(11)

where

$${\mathit{C}}_i=\left[\begin{array}{cc}{\tilde{\mathit{C}}}_{x,i},& \mathbf{0},\\ \mathbf{0},& {\tilde{\mathit{C}}}_{y,i},\end{array}\right],$$

(12)

$${\tilde{\mathit{C}}}_{x,i}=diag\left(G_{\sigma }\left(e_i\left(1\right)\right),\dots ,G_{\sigma }\left(e_i\left(n\right)\right)\right),$$

(13)

$${\tilde{\mathit{C}}}_{y,i}=diag\left(G_{\sigma }\left(e_i(n+1)\right),\dots ,G_{\sigma }\left(e_i(n+m)\right)\right).$$

(14)

Note here that the $diag(·)$ is used to denote the diagonal matrix.

Based on the above equation and using ${\widehat{\mathit{x}}}_i^{-}$ to replace the ${\mathit{x}}_i$ contained in Equation (9), MCGF can be written in a similar way as GF except a modified measurement noise covariance [25]:

$${\tilde{\mathit{R}}}_i={\mathit{S}}_{r,i}{\tilde{\mathit{C}}}_{y,i}^{-1}{\mathit{S}}_{r,i}^{\mathrm{T}}.$$

(15)

Then, ${\widehat{\mathit{x}}}_i$ and its covariance ${\mathit{P}}_i$ can be obtained through

$${\widehat{\mathit{x}}}_i={\widehat{\mathit{x}}}_i^{-}+{\mathit{K}}_i({\mathit{z}}_i-{\widehat{\mathit{z}}}_i),$$

(16)

$${\mathit{P}}_i={\mathit{P}}_i^{-}-{\mathit{K}}_i{\mathit{P}}_{zz}{\mathit{K}}_i^{\mathrm{T}},$$

(17)

where

$${\mathit{K}}_i={\mathit{P}}_{xz}{\mathit{P}}_{zz}^{-1},$$

(18)

$${\widehat{\mathit{z}}}_i=\int {\mathit{h}}_i\left({\mathit{x}}_i\right)\mathcal{N}({\mathit{x}}_i|{\widehat{\mathit{x}}}_i^{-},{\mathit{P}}_i^{-})\mathrm{d}{\mathit{x}}_i,$$

(19)

$${\mathit{P}}_{zz}=\int ({\mathit{h}}_i\left({\mathit{x}}_i\right)-{\widehat{\mathit{z}}}_i){({\mathit{h}}_i\left({\mathit{x}}_i\right)-{\widehat{\mathit{z}}}_i)}^{\mathrm{T}}\mathcal{N}({\mathit{x}}_i|{\widehat{\mathit{x}}}_i^{-},{\mathit{P}}_i^{-})\mathrm{d}{\mathit{x}}_i+{\tilde{\mathit{R}}}_i,$$

(20)

$${\mathit{P}}_{xz}=\int ({\mathit{x}}_i-{\widehat{\mathit{x}}}_i^{-}){({\mathit{h}}_i\left({\mathit{x}}_i\right)-{\widehat{\mathit{z}}}_i)}^{\mathrm{T}}\mathcal{N}({\mathit{x}}_i|{\widehat{\mathit{x}}}_i^{-},{\mathit{P}}_i^{-})\mathrm{d}{\mathit{x}}_i.$$

(21)

We easily find that the main difference between MCGF and GF is the modified measurement noise covariance, and MCGF shows excellent estimation performance when measurement is polluted by outliers or shot noise [24,25,26]. However, it still requires the knowledge of measurement noise covariance. When the covariance changes over time (which implies the true covariance is different from the known covariance), the MCGF algorithm does not perform well. Therefore, we adopt the adaptive method to further improve the performance of MCGF in this case.

3. Variation Beysian Maximum Correntropy Gaussian Filter

The main idea under state estimation is to obtain the posterior probability density function $p\left({\mathit{x}}_i\right|{\mathit{z}}_{1:i})$. For GF, we obtain it through the Gaussian approximation $p\left({\mathit{x}}_i\right|{\mathit{z}}_{1:i})\approx \mathcal{N}\left({\mathit{x}}_i\right|{\widehat{\mathit{x}}}_i,{\mathit{P}}_i)$. However, if the measurement noise covariance ${\mathit{R}}_i$ is unavailable, we need to estimate the joint posterior distribution $p({\mathit{x}}_i,{\mathit{R}}_i|{\mathit{z}}_{1:i})$. This distribution can be solved by the free form VB approximation [9,32]:

$$p({\mathit{x}}_i,{\mathit{R}}_i|{\mathit{z}}_{1:i})\approx Q\left({\mathit{x}}_i\right)Q\left({\mathit{R}}_i\right),$$

(22)

where $Q\left({\mathit{x}}_i\right)$ and $Q\left({\mathit{R}}_i\right)$ are unknown approximation densities, which can be calculated by minimizing the Kullback–Leibler (KL) divergence between the true one and its corresponding approximation [9,32]:

$$Q\left({\mathit{x}}_i\right)\propto exp\left(\int logp({\mathit{z}}_i,{\mathit{x}}_i,{\mathit{R}}_i|{\mathit{z}}_{1:i-1})Q\left({\mathit{R}}_i\right)\mathrm{d}{\mathit{R}}_i\right),$$

(23)

$$Q\left({\mathit{R}}_i\right)\propto exp\left(\int logp({\mathit{z}}_i,{\mathit{x}}_i,{\mathit{R}}_i|{\mathit{z}}_{1:i-1})Q\left({\mathit{x}}_i\right)\mathrm{d}{\mathit{x}}_i\right).$$

(24)

According to the VB method, $p({\mathit{x}}_i,{\mathit{R}}_i|{\mathit{z}}_{1:i})$ can be approximated as a product of Gaussian distribution and inverse Wishart (IW) distribution [9,32]:

$$p({\mathit{x}}_i,{\mathit{R}}_i|{\mathit{z}}_{1:i})\approx \mathcal{N}\left({\mathit{x}}_i\right|{\widehat{\mathit{x}}}_i,{\mathit{P}}_i)\mathrm{IW}\left({\mathit{R}}_i\right|v_i,{\mathit{V}}_i),$$

(25)

where

$$\mathcal{N}\left({\mathit{x}}_i\right|{\widehat{\mathit{x}}}_i,{\mathit{P}}_i)\propto \mid {\mathit{P}}_i{\mid }^{-1/2}exp\left(-\frac{1}{2}{({\mathit{x}}_i-{\widehat{\mathit{x}}}_i)}^{\mathrm{T}}{\mathit{P}}_i^{-1}({\mathit{x}}_i-{\widehat{\mathit{x}}}_i)\right)$$

(26)

$$\mathrm{IW}\left({\mathit{R}}_i\right|v_i,{\mathit{V}}_i)\propto \mid {\mathit{R}}_i{\mid }^{-(v_i+n+1)/2}exp\left(-\frac{1}{2}\mathrm{tr}\left({\mathit{V}}_i{\mathit{R}}_i\right)\right),$$

(27)

where $\mathrm{tr}(·)$ is the trace of a matrix, and $v_i$ and ${\mathit{V}}_i$ are the degree of freedom parameter and the inverse scale matrix, respectively.

The integrals in (22) and (23) can be computed as follows [9,32]:

$$\begin{array}{c}\int logp({\mathit{z}}_i,{\mathit{x}}_i,{\mathit{R}}_i|{\mathit{z}}_{1:i-1})Q\left({\mathit{R}}_i\right)\mathrm{d}{\mathit{R}}_i\hfill \\ =-\frac{1}{2}{({\mathit{z}}_i-{\mathit{h}}_i\left({\mathit{x}}_i\right))}^{\mathrm{T}}{\langle {\mathit{R}}_i^{-1}\rangle }_R({\mathit{z}}_i-{\mathit{h}}_i\left({\mathit{x}}_i\right))-\frac{1}{2}{({\mathit{x}}_i-{\widehat{\mathit{x}}}_i^{-})}^{\mathrm{T}}{\left({\mathit{P}}_i^{-}\right)}^{-1}({\mathit{x}}_i-{\widehat{\mathit{x}}}_i^{-})+C_1,\hfill \end{array}$$

(28)

$$\begin{array}{c}\int logp({\mathit{z}}_i,{\mathit{x}}_i,{\mathit{R}}_i|{\mathit{z}}_{1:i-1})Q\left({\mathit{x}}_i\right)\mathrm{d}{\mathit{x}}_i\hfill \\ =-\frac{1}{2}(v_i^{-}+n+2)log\mid {\mathit{R}}_i\mid -\frac{1}{2}\mathrm{tr}\left({\mathit{V}}_i^{-}{\mathit{R}}_i^{-1}\right)-\frac{1}{2}{\langle {({\mathit{z}}_i-{\mathit{h}}_i\left({\mathit{x}}_i\right))}^{\mathrm{T}}{\mathit{R}}_i^{-1}({\mathit{z}}_i-{\mathit{h}}_i\left({\mathit{x}}_i\right))\rangle }_x+C_2,\hfill \end{array}$$

(29)

where ${\langle ·\rangle }_R=\int (·)Q\left({\mathit{R}}_i\right)\mathrm{d}{\mathit{R}}_i$, ${\langle ·\rangle }_x=\int (·)Q\left({\mathit{x}}_i\right)\mathrm{d}{\mathit{x}}_i$, and $C_1$, $C_2$ are some constants. Due to the fact that $Q\left({\mathit{R}}_i\right)=\mathrm{IW}\left({\mathit{R}}_i\right|v_i,{\mathit{V}}_i)$, we obtain

$${\langle {\mathit{R}}_i^{-1}\rangle }_R=(v_i-n-1){\mathit{V}}_i^{-1}.$$

(30)

Besides this, the expectation can be rewritten as

$${\langle {({\mathit{z}}_i-{\mathit{h}}_i\left({\mathit{x}}_i\right))}^{\mathrm{T}}{\mathit{R}}_i^{-1}({\mathit{z}}_i-{\mathit{h}}_i\left({\mathit{x}}_i\right))\rangle }_x=\mathrm{tr}\{{\langle {({\mathit{z}}_i-{\mathit{h}}_i\left({\mathit{x}}_i\right))}^{\mathrm{T}}({\mathit{z}}_i-{\mathit{h}}_i\left({\mathit{x}}_i\right))\rangle }_x{\mathit{R}}_i^{-1}\}.$$

(31)

Substituting (30) and (31) into (28) and (29), and matching the parameters in (26) and (27), we can obtain the following results:

$$\begin{array}{c}{v}_i=v_i^{-}+1,\hfill \end{array}$$

(32)

$$\begin{array}{c}{\mathit{T}}_i=\int ({\mathit{h}}_i\left({\mathit{x}}_i\right)-{\widehat{\mathit{z}}}_i){({\mathit{h}}_i\left({\mathit{x}}_i\right)-{\widehat{\mathit{z}}}_i)}^{\mathrm{T}}\mathcal{N}({\mathit{x}}_i|{\widehat{\mathit{x}}}_i^{-},{\mathit{P}}_i^{-})\mathrm{d}{\mathit{x}}_i+{(v_i-n-1)}^{-1}{\mathit{V}}_i,\hfill \end{array}$$

(33)

$$\begin{array}{c}{\mathit{K}}_i={\mathit{P}}_{xz}{\left({\mathit{T}}_i\right)}^{-1},\hfill \end{array}$$

(34)

$$\begin{array}{c}{\widehat{\mathit{x}}}_i={\widehat{\mathit{x}}}_i^{-}+{\mathit{K}}_i({\mathit{z}}_i-{\widehat{\mathit{z}}}_i),\hfill \end{array}$$

(35)

$$\begin{array}{c}{\mathit{P}}_i={\mathit{P}}_i^{-}-{\mathit{K}}_i{\mathit{T}}_i{{\mathit{K}}_i}^{\mathrm{T}},\hfill \end{array}$$

(36)

$$\begin{array}{c}{\mathit{V}}_i={\mathit{V}}_i^{-}+\int ({\mathit{z}}_i-{\mathit{h}}_i\left({\mathit{x}}_i\right)){({\mathit{z}}_i-{\mathit{h}}_i\left({\mathit{x}}_i\right))}^{\mathrm{T}}\mathcal{N}\left({\mathit{x}}_i\right|{\widehat{\mathit{x}}}_i,{\mathit{P}}_i)\mathrm{d}{\mathit{x}}_i,\hfill \end{array}$$

(37)

where ${(v_i-n-1)}^{-1}{\mathit{V}}_i$ is the estimated measurement covariance.

The VB based GF works well for unknown measurement noise covariance. However, when the measurement contains outliers or shot noise, their estimation will degrade, as will be shown in our simulation results. To overcome the shortcomings of MCGF and VBGF, we take the advantages of VB and MCC by the fixed-point iteration method, and design the so called VBMCGF algorithm, which is summarized as follows:

Step 1:

Predict:

${\widehat{\mathit{x}}}_i^{-}$ and ${\mathit{P}}_i^{-}$ are obtained through (5) and (6), and

$$v_k^{-}=\rho (v_{k-1}-n-1)+n+1,$$

(38)

$${\mathit{V}}_k^{-}=\mathit{B}{\mathit{V}}_{k-1}{\mathit{B}}^{\mathrm{T}},$$

(39)

where $0<\rho \le 1$, $0<\mid \mathit{B}\mid \le 1$, and a reasonable choice is $\mathit{B}=\sqrt{\rho }\mathit{I}$.

Step 2:

Update:

First, set ${\widehat{\mathit{x}}}_i^{\left(1\right)}={\widehat{\mathit{x}}}_i^{-}$, ${\mathit{P}}_i^{\left(1\right)}={\mathit{P}}_i^{-}$, $v_i=1+v_i^{-}$, and ${\mathit{V}}_i^{\left(1\right)}={\mathit{V}}_i^{-}$. Calculate ${\widehat{\mathit{z}}}_i$ and ${\mathit{P}}_{xz}$ by (19) and (21).

For $j=1,\dots ,N$, iterate the following equations:

$$\begin{array}{c}{\mathit{R}}_i^{\left(j\right)}={\mathit{S}}_{r,i}^{\left(j\right)}{({\mathit{S}}_{r,i}^{\left(j\right)})}^{\mathrm{T}}={(v_i-n-1)}^{-1}{\mathit{V}}_i^{\left(j\right)},\hfill \end{array}$$

(40)

$$\begin{array}{c}{e}_{r,i}^{\left(j\right)}={({\mathit{S}}_{r,i}^{\left(j\right)})}^{-1}({\mathit{z}}_i-{\widehat{\mathit{z}}}_i),\hfill \end{array}$$

(41)

$$\begin{array}{c}{\tilde{\mathit{C}}}_{r,i}^{\left(j\right)}=diag\left(G_{\sigma }(e_{r,i}^{\left(j\right)}\left(1\right)),\dots ,G_{\sigma }(e_{r,i}^{\left(j\right)}\left(m\right))\right),\hfill \end{array}$$

(42)

$$\begin{array}{c}{\tilde{\mathit{R}}}_i^{\left(j\right)}={\mathit{S}}_{r,i}^{\left(j\right)}{({\tilde{\mathit{C}}}_{r,i}^{\left(j\right)})}^{-1}{({\mathit{S}}_{r,i}^{\left(j\right)})}^{\mathrm{T}},\hfill \end{array}$$

(43)

$$\begin{array}{c}{\mathit{T}}_i^{(j+1)}=\int ({\mathit{h}}_i\left({\mathit{x}}_i\right)-{\widehat{\mathit{z}}}_i){({\mathit{h}}_i\left({\mathit{x}}_i\right)-{\widehat{\mathit{z}}}_i)}^{\mathrm{T}}\mathcal{N}({\mathit{x}}_i|{\widehat{\mathit{x}}}_i^{-},{\mathit{P}}_i^{-})\mathrm{d}{\mathit{x}}_i+{\tilde{\mathit{R}}}_i^{\left(j\right)},\hfill \end{array}$$

(44)

$$\begin{array}{c}{\mathit{K}}_i^{(j+1)}={\mathit{P}}_{xz}{({\mathit{T}}_i^{(j+1)})}^{-1},\hfill \end{array}$$

(45)

$$\begin{array}{c}{\widehat{\mathit{x}}}_i^{(j+1)}={\widehat{\mathit{x}}}_i^{-}+{\mathit{K}}_i^{(j+1)}({\mathit{z}}_i-{\widehat{\mathit{z}}}_i),\hfill \end{array}$$

(46)

$$\begin{array}{c}{\mathit{P}}_i^{(j+1)}={\mathit{P}}_i^{-}-{\mathit{K}}_i^{(j+1)}{\mathit{T}}_i^{(j+1)}{({\mathit{K}}_i^{(j+1)})}^{\mathrm{T}},\hfill \end{array}$$

(47)

$$\begin{array}{c}{\mathit{V}}_i^{(j+1)}={\mathit{V}}_i^{-}+\int ({\mathit{z}}_i-{\mathit{h}}_i\left({\mathit{x}}_i\right)){({\mathit{z}}_i-{\mathit{h}}_i\left({\mathit{x}}_i\right))}^{\mathrm{T}}\mathcal{N}({\mathit{x}}_i|{\widehat{\mathit{x}}}_i^{(j+1)},{\mathit{P}}_i^{(j+1)})\mathrm{d}{\mathit{x}}_i,\hfill \end{array}$$

(48)

End For. In addition, set ${\mathit{V}}_i={\mathit{V}}_i^{(N+1)}$, ${\widehat{\mathit{x}}}_i={\widehat{\mathit{x}}}_i^{(N+1)}$, and ${\mathit{P}}_i={\mathit{P}}_i^{(N+1)}$.

The main difference between the proposed VBMCGF and existing GFs lies in the modified estimation error covariance ${\tilde{\mathit{R}}}_i^{\left(j\right)}$, where VB iterations are used to estimate its value and MCC is used to modify it in the presence of non-Gaussian noises. The kernel bandwidth $\sigma$ plays an important role in reducing the effect of non-Gaussian noise or outliers. A smaller $\sigma$ will make the filter more sensitive to outliers, but it may affect the convergence performance. In addition, a too large $\sigma$ may cause the VBMCGF to perform more like VBGF (It can be proved that, if $\sigma \to \infty$, the proposed VBMCGF will reduce to VBGF). One possible way to select it is by the trial and error method [24,25,26]. Another important issue is the number of fixed-point iterations. In fact, only a few iterations (e.g., 2 or 3) are enough [31,32].

As the general framework, our filter can be easily implemented according to the real requirements. For linear systems that are described by ${\mathit{x}}_i={\mathit{F}}_{i-1}{\mathit{x}}_{i-1}+{\mathit{w}}_{i-1}$ and ${\mathit{z}}_i={\mathit{H}}_i{\mathit{x}}_i+{\mathit{v}}_i$, the predation update in the VBMCKF is the same as KF:

$${\widehat{\mathit{x}}}_i^{-}={\mathit{F}}_{i-1}{\widehat{\mathit{x}}}_{i-1},$$

(49)

$${\mathit{P}}_i^{-}={\mathit{F}}_{i-1}{\mathit{P}}_{i-1}{\mathit{F}}_{i-1}^{\mathrm{T}}+{\mathit{Q}}_{i-1}.$$

(50)

In addition, the ${\widehat{\mathit{z}}}_i$, ${\mathit{P}}_{xz}$, ${\mathit{T}}_i^{(j+1)}$, and ${\mathit{V}}_i^{(j+1)}$ that appeared in VBMCKF will reduce to the following equations:

$${\widehat{\mathit{z}}}_i={\mathit{H}}_i{\widehat{\mathit{x}}}_i^{-},$$

(51)

$${\mathit{P}}_{xz}={\mathit{P}}_i^{-}{\mathit{H}}_i^{\mathrm{T}},$$

(52)

$${\mathit{T}}_i^{(j+1)}={\mathit{H}}_i{\mathit{P}}_i^{-}{\mathit{H}}_i^{\mathrm{T}}+{\tilde{\mathit{R}}}_i^{\left(j\right)},$$

(53)

$${\mathit{V}}_i^{(j+1)}={\mathit{V}}_i^{-}+{\mathit{H}}_i{\mathit{P}}_i^{(j+1)}{\mathit{H}}_i^{\mathrm{T}}+({\mathit{z}}_i-{\mathit{H}}_i{\widehat{\mathit{x}}}_i^{(j+1)}){({\mathit{z}}_i-{\mathit{H}}_i{\widehat{\mathit{x}}}_i^{(j+1)})}^{\mathrm{T}},$$

(54)

while other steps are the same as the general framework.

When it comes to the nonlinear systems, the Gaussian integrals contained in ${\widehat{\mathit{x}}}_i^{-}$, ${\mathit{P}}_i^{-}$, ${\widehat{\mathit{z}}}_i$, ${\mathit{P}}_{xz}$, ${\mathit{T}}_i^{(j+1)}$, and ${\mathit{V}}_i^{(j+1)}$ can be calculated according to Taylor series, unscented transform, or cubature rules, and the corresponding filters are called VBMCEKF, VBMCUKF, and VBMCCKF, respectively.

4. Experimental Results

4.1. Simulation Results of the Target Tracking Model

To illustrate the performance of the proposed algorithm, we first give the simulation results using a typical target tracking model, where cubature rules are used to calculate the integrals. We compare the estimation accuracy of seven filters: CKF [7], MCCKF-1 [26], MCCKF-2 [25], VBCKF [9], HCKF [18], VBHCKF (which adopts Huber’s function) and the proposed VBMCCKF under various kinds of measurement noise. The target tracking example is modeled as [2]:

$${\mathit{x}}_i=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]{\mathit{x}}_{i-1}+\left[\begin{array}{cc}\frac{T^2}{2}& 0\\ T& 0\\ 0& \frac{T^2}{2}\\ 0& T\end{array}\right]{\mathit{w}}_{i-1},$$

(55)

$${\mathit{z}}_i=\left[\begin{array}{c}\sqrt{{({\xi }_x-{\xi }_0)}^2+{({\eta }_y-{\eta }_0)}^2}\\ arctan\frac{{\eta }_x-{\eta }_0}{{\xi }_y-{\xi }_0}\end{array}\right]+{\mathit{v}}_i,$$

(56)

where ${\mathit{x}}_i={\left[\begin{array}{cccc}{\xi }_{x,i}& {\dot{\xi }}_{x,i}& {\eta }_{y,i}& \dot{{\eta }_{y,i}}\end{array}\right]}^{\mathrm{T}}$ is the system state with $({\xi }_{x,i},{\eta }_{y,i})$ and $({\dot{\xi }}_{x,i},{\dot{\eta }}_{y,i})$ being the position and velocity in x and y directions. The position of radar is set as $({\xi }_0,{\eta }_0)$ = (−100 m, −100 m). We set the sampling period $T=0.1\mathrm{s}$, ${\mathit{Q}}_i=\mathrm{diag}\left(\begin{array}{cc}0.04{\mathrm{m}}^2{\mathrm{s}}^{-3},& 0.04{\mathrm{s}}^{-3}\end{array}\right)$. The initial state is ${\widehat{\mathit{x}}}_0={\overline{\mathit{x}}}_0={[-40\mathrm{m}3{\mathrm{ms}}^{-1}10\mathrm{m}1{\mathrm{ms}}^{-1}]}^{\mathrm{T}}$ and the covariance is ${\mathit{P}}_0=\mathrm{diag}\left(\begin{array}{cccc}4{\mathrm{m}}^2,& 0.01{\mathrm{m}}^2{\mathrm{s}}^{-2},& 4{\mathrm{m}}^2,& 0.01{\mathrm{m}}^2{\mathrm{s}}^{-2}\end{array}\right)$. The root mean square error (RMSE) and average RMSE (ARMSE) in position or velocity are used to describe the estimation accuracy—for example, the RMSE and ARMSE in position are defined as [2]:

$${\mathrm{RMSE}}_{pos}\left(i\right)=\sqrt{\frac{1}{M}\sum _{c=1}^M({({\xi }_{x,i}^c-{\widehat{\xi }}_{x,i}^c)}^2+{({\eta }_{y,i}^c-{\widehat{\eta }}_{y,i}^c)}^2)},$$

(57)

$${\mathrm{ARMSE}}_{pos}=\frac{1}{L}\sum _{i=1}^L{\mathrm{RMSE}}_{pos}\left(i\right),$$

(58)

where $({\xi }_{x,i}^c,{\eta }_{y,i}^c)$ and $({\widehat{\xi }}_{x,i}^c,{\widehat{\eta }}_{y,i}^c)$ are the true and estimated position in the cth Monte Carlo experiment, respectively. The RMSE and ARMSE of velocity are similar.

We here consider the following five kinds of measurement noises:

Case A: Gaussian distribution

$${\mathit{v}}_i\sim \mathcal{N}(\mathbf{0},\mathrm{diag}[{\left(0.2\mathrm{m}\right)}^2,{\left(0.015\mathrm{rad}\right)}^2]),$$

(59)

Case B: Time varying measurement noise covariance

$${\mathit{v}}_i\sim \mathcal{N}(\mathbf{0},{\alpha }_i^2\mathrm{diag}[{\left(0.2\mathrm{m}\right)}^2,{\left(0.015\mathrm{rad}\right)}^2]),$$

(60)

Case C: Gaussian mixture noise with time varying measurement noise covariance

$${\mathit{v}}_i\sim 0.8\mathcal{N}(\mathbf{0},{\alpha }_i^2\mathrm{diag}[{\left(0.2\mathrm{m}\right)}^2,{\left(0.015\mathrm{rad}\right)}^2])+0.2\mathcal{N}(\mathbf{0},\mathrm{diag}[{\left(5\mathrm{m}\right)}^2,{\left(0.75\mathrm{rad}\right)}^2]),$$

(61)

Case D: Time varying measurement noise covariance and shot noise

$${\mathit{v}}_i\sim {[{\beta }_i,{\gamma }_i]}^{\mathrm{T}}+\mathcal{N}(\mathbf{0},{\alpha }_i^2\mathrm{diag}[{\left(0.2\mathrm{m}\right)}^2,{\left(0.015\mathrm{rad}\right)}^2]),$$

(62)

Case E: Gaussian mixture noise with time varying measurement noise covariance and shot noise

$${\mathit{v}}_i\sim {[{\beta }_i,{\gamma }_i]}^{\mathrm{T}}+0.8\mathcal{N}(\mathbf{0},{\alpha }_i^2\mathrm{diag}[{\left(0.2\mathrm{m}\right)}^2,{\left(0.015\mathrm{rad}\right)}^2])+0.2\mathcal{N}(\mathbf{0},\mathrm{diag}[{\left(5\mathrm{m}\right)}^2,{\left(0.75\mathrm{rad}\right)}^2]),$$

(63)

where the parameters ${\alpha }_i$, ${\beta }_i$, and ${\gamma }_i$ are given in Figure 1.

In these simulations, we use $\sigma =8$ for MCC based algorithms, the commonly used threshold $h=1.345$ for Huber’s function, and we set $\rho =0.8$ and $N=3$ for VB approximations. We perform $L=100$ steps and run $M=100$ Monte Carlo experiments for each case. The simulation results are plotted in Figure 2, Figure 3, Figure 4, Figure 5 and Figure 6.

Under the Gaussian measurement noise with known noise covariance, as given in Figure 2, both MCCKF and HCKF have nearly similar estimation accuracy to CKF, since they will reduce to CKF if choosing proper free parameters (e.g., the $\sigma$ and h are infinity). VBCKF and VBMCCKF work slightly worse as compared with CKF because they only use their online estimated measurement noise covariance instead of the real one. In particular, the VBHCKF has the worst performance since the commonly used parameter $h=1.345$ for Huber’s function doesn’t fit the Gaussian noise situation when using the inaccurate online estimated measurement noise covariance. The proposed VBMCCKF works well with the same kernel bandwidth under both Gaussian and non-Gaussian noise situations, as will be shown in the following cases.

Figure 3, Figure 4, Figure 5 and Figure 6 show the estimation performances of different algorithms under Cases B–E. It can be seen obviously that CKF has the worst estimation accuracy since it requires the measurement noise satisfying Gaussian distribution with known covariance, which is violated in these situations. MCCKF-1 and MCCKF-2 have similar estimation performance but are slightly worse than HCKF when using this kernel bandwidth. As demonstrated in [23,24,25,26,27], MCCKF is able to obtain better estimation accuracy than HCKF with a suitable $\sigma$. The estimation results of MCCKF and HCKF do not change too much when Gaussian mixture noise or shot noise are added, since they are robust filters. The VBHCKF has better estimation results in velocity but worse accuracy than HCKF. The VBCKF has much better estimation in Cases B and C, as it is able to online estimate the time varying measurement noise covariance. However, its performance will degrade once shot noise is injected in Cases D and E. Among these algorithms, our VBMCCKF has the best estimation accuracy as compared with other algorithms under Cases B–E. It shows the adaptivity to unknown changing measurement noise covariance and robustness to Gaussian mixture noise and shot noise. Its estimation results are also much better than VBHCKF since the MCC has the potential to capture high order information than Huber’s function. The ARMSEs of these filters under different noises are also given in

Table 1. ARMSEs of different filters under five cases.

Algorithms	Case A		Case B		Case C		Case D		Case E
	Pos.	Vel.	Pos.	Vel.	Pos.	Vel.	Pos.	Vel.	Pos.	Vel.
CKF	0.4097	0.0855	2.2930	0.3121	2.2050	0.3053	3.7720	0.7809	3.2960	0.6923
MCCKF-1	0.4077	0.0854	1.4960	0.1959	1.5680	0.2052	1.4090	0.2077	1.5680	0.2100
MCCKF-2	0.4079	0.0853	1.5280	0.1989	1.5990	0.2058	1.4500	0.2098	1.6010	0.2114
HCKF	0.4045	0.0847	1.0990	0.1304	1.1360	0.1390	1.0100	0.1384	1.1030	0.1365
VBHCKF	0.6500	0.1296	1.7180	0.1185	1.6360	0.1260	1.6460	0.1207	1.7590	0.1251
VBCKF	0.4362	0.0880	0.8761	0.0845	0.7828	0.0914	1.1240	0.0977	1.3100	0.1000
VBMCCKF	0.4350	0.0878	0.8633	0.0843	0.7752	0.0909	0.7424	0.0933	0.8236	0.0931

to clearly show the differences.

4.2. Field Results of Integrated Navigation

To further illustrate the effectiveness of the proposed algorithm, we compare our algorithm and existing related methods using the real data collected by a self-made fiber optical gyroscope inertial navigation system (INS) together with a doppler velocity logger (DVL). The integrated navigation results of photonics inertial navigation system (PHINS) and GPS are used as the reference system. We adopt the loosely coupled method to fusion the information of INS and DVL. The state vector is chosen as $\mathit{x}={\left[\delta L\delta \lambda \delta V_E\delta V_N{\phi }_x{\phi }_y{\phi }_z{\nabla }_x{\nabla }_y{\nabla }_z{\epsilon }_x{\epsilon }_y{\epsilon }_z\right]}^{\mathrm{T}}$, where $\delta L$ and $\delta \lambda$ are the latitude and longitude error, $\{\delta V_j,{\phi }_j,{\nabla }_j,{\epsilon }_j\}$ are the velocity error, attitude error, accelerometer bias and gyroscope constant drift, respectively. j denotes the subscribe $\{e,n,x,y,z\}$, where e and n present the east and north directions in the local-level frame, and x, y, and z are the directions of three axises in the body frame. Then, the continuous system model is given as follows:

$$\dot{\mathit{x}}\left(t\right)=\mathit{A}\left(t\right)\mathit{x}\left(t\right)+\mathit{B}\left(t\right)\mathit{w}\left(t\right),$$

(64)

where t is the continuous system time, and $\mathit{w}\left(t\right)={\left[{\mathbf{0}}_{1\times 2}{w}_{ax}{w}_{ay}{w}_{gx}{w}_{gy}{w}_{gz}{\mathbf{0}}_{1\times 5}\right]}^{\mathrm{T}}$ is the process noise, which contains the Gaussian noise of both accelerometers and gyroscopes. The detailed elements of matrix $\mathit{A}\left(t\right)$ and $\mathit{B}\left(t\right)$ can refer to [34]. The measurement equation is

$$\mathit{z}\left(t\right)=\mathit{H}\mathit{x}\left(t\right)+\mathit{v}\left(t\right),$$

(65)

where $\mathit{z}\left(t\right)={\left[\begin{array}{cc}(V_e^{INS}-V_e^{DVL})& (V_n^{INS}-V_n^{DVL})\end{array}\right]}^{\mathrm{T}}$, $\mathit{H}=\left[{\mathbf{0}}_{2\times 2}{\mathit{I}}_{2\times 2}{\mathbf{0}}_{2\times 8}\right]$, and $\mathit{v}\left(t\right)={\left[V_e^{DVL}{V}_n^{DVL}\right]}^{\mathrm{T}}$. Then, the discretization process is performed before running filtering algorithms.

We compare the estimation results of KF, VBKF, HKF, MCKF, and VBMCKF, where we choose $h=1.345$, $\sigma =4$, $\rho =0.96$, and $N=3$. We set ${\widehat{\mathit{x}}}_0={\left[{\mathbf{0}}_{1\times 12}\right]}^{\mathrm{T}}$, and the covariance is P₀ = diag($\begin{array}{cc}\frac{500}{Re}\mathrm{rad}/\mathrm{s},& \frac{500}{Re}\mathrm{rad}/\mathrm{s},\end{array}$ 0.5°, 0.5°, 3°, 1 × 10⁻⁴ g, 1 × 10⁻⁴ g, 0.01°/h, 0.01°/h, 0.01°/h), where $Re$ is the radius of the earth, and g is the gravitational acceleration. The process noise covariance and measurement noise covariance are set as ${\mathit{Q}}_i$ = diag(0, 0, 1 × 10⁻³ g, 1 × 10⁻³ g, 0.025°/h, 0.025 °/h, 0.025 °/h, 0, 0, 0, 0, 0) and ${\mathit{R}}_i$ = diag(0.1 m/s, 0.1 m/s) according to the parameters of INS and DVL.

In this experiment, the system is first tested in anchorage for about 50 min, then the ship starts to move. The real velocities of the ship are shown in Figure 7 provided by the commercial INS/GPS integrated navigation system. The collected data is processed using MATLAB (R2014a by MathWorks, Inc., Natick, MA, USA) on a computer with 2.50 GHz Intel Core i5-7300HQ CPU and 8 GB memory. The total computational time of KF, VBKF, HKF, MCKF, and VBMCKF are 0.1900 s, 0.4800 s, 0.2640 s, 0.2310 s, and 0.6340 s, respectively. The position and velocity errors of different filters are given in Figure 8 and Figure 9. The differences between attitude and heading errors are quite similar so we omit them.

It can be seen from Figure 8 and Figure 9 that when the motion state changes sharply, the proposed VBMCKF algorithm has the smallest estimation errors with slightly increased computational time as compared with other estimation methods.

5. Conclusions

In this paper, a novel adaptive MCGF based on VB approximation is proposed. The MCC is used to reduce the effect of non-Gaussian measurement noise and outliers, while we use VB to estimate the unknown measurement noise covariance. Experimental results based on simulation examples and real data show that the proposed algorithm has better estimation accuracy than related robust and adaptive filters.