Adaptive Robust Unscented Kalman Filter via Fading Factor and Maximum Correntropy Criterion

Abstract

In most practical applications, the tracking process needs to update the data constantly. However, outliers may occur frequently in the process of sensors’ data collection and sending, which affects the performance of the system state estimate. In order to suppress the impact of observation outliers in the process of target tracking, a novel filtering algorithm, namely a robust adaptive unscented Kalman filter, is proposed. The cost function of the proposed filtering algorithm is derived based on fading factor and maximum correntropy criterion. In this paper, the derivations of cost function and fading factor are given in detail, which enables the proposed algorithm to be robust. Finally, the simulation results show that the presented algorithm has good performance, and it improves the robustness of a general unscented Kalman filter and solves the problem of outliers in system.

1. Introduction

In real-world applications, target tracking problems have attracted much attention such as maneuvering target tracking [1], ballistic target tracking [2] and multiple target tracking [3], etc. For getting better accuracy, efficiency and performance of tracking problems, the effect of noise needs to be reduced, especially of measurement noise. Therefore, the measurement noise needs to be restrained timely in the process of a tracking target. As is well known, filtering algorithms are powerful tools for suppressing the effect of noise. Kalman [4] first proposed the Kalman filter (KF) algorithm in 1960, and the filtering technique has developed quickly ever since. For the linear Gaussian process [4,5,6,7], the KF could get optimal recursive results based on the minimum mean square error (MMSE) estimation. It is well known that KF can employ the first two moments (mean and covariance) of state and measurement to obtain optimal estimates. It can be seen that the KF has been applied in numerous areas such as navigation, target tracking, communications [7], attitude determination [8], multiple sensors data [9] and many more. However, for amounts of data collected or sent by sensors, the KF experienced heavy computational load to get an optimal solution. In most practical applications, the systems are nonlinear. In this case, the performance of KF is unsatisfactory. Furthermore, when a dynamic model was contaminated by outliers [10], the KF degraded severely. Therefore, it is necessary to design an efficient algorithm that can suppress the outliers for the nonlinear problem.

In fact, for a nonlinear tracking problem, many nonlinear filtering algorithms were proposed to deal with the nonlinear systems—for example, extended Kalman filter (EKF), unscented Kalman filter (UKF), cubature Kalman filter (CKF), particle filter (PF), Gaussian sum filter (GSF), and so on. Particularly, it is well known that EKF deals with the nonlinear system by its first-order Taylor Series expansions and leads to a suboptimal solution. UKF, based on unscented transform (UT), is an improved filter compared to KF and EKF. It selects deliberately as sigma points to generalize the KF for both linear and nonlinear systems, and it can be used to propagate three order information for state estimation. Therefore, it can achieve better estimation accuracy and computational feasibility [11]. Zhan and Wan [12] developed the iterated unscented Kalman filter for passive target tracking. Based on the spherical-radial cubature rule, CKF is presented to approximate the Gaussian filter [13,14]. GSFs are based on the idea that a non-Gaussian probability density function (PDF) can be approximated by a sum of Gaussian PDFs [15]. Based on Bayesian estimation and a sequential Monte Carlo approach, Du et al. utilized PF to handle nonlinear and non-Gaussian problems, and the PF is applied in small target tracking in an optimal image sequence [16]. However, these nonlinear filters were susceptible to outliers and did not have robust property.

For measurement model contaminated by outliers, several filtering algorithms have been proposed [17,18,19,20,21,22,23]. Based on the Huber function, Wang et al. [17] presented the derivative-free filter to manage the measurement outliers, but it did not suppress the state outliers well. For measurement outliers, Durovic and Kovacevic [18] utilized the M-estimation to deal with measurement outliers in the presence of disturbance uncertainty. It can not deal with both state and measurement outliers. Their performance will diverge if the state and measurement models were contaminated by outliers simultaneously. To solve a nonlinear system with heavy-tailed noise, Wang et al. [19] studied a robust information filter to solve the measurements with a large error from the estimation process. However, it did not embed the fading factor into the framework of information filter. Under this case, Karlgaard [20] proposed an adaptive robust nonlinear filtering algorithm to resist the effects of outliers. For both the state and measurement outliers, Gandhi and Mili [21] introduced a generalized maximum likelihood type KF. However, the KF was limited to a linear system, and the evaluation of Jacobian matrices in EKF could be nontrivial and this leads to degraded performance. Chang et al. [22] investigated a robust filter to suppress the state and measurement outliers, and it utilized the robust property of the Huber function. The $H_{\infty }$ filter has robustness to minimize the estimation error [23], and it can not accommodate the outliers well for outliers occurring randomly. However, these filters have robustness to some extent.

In order to enhance the robustness of the aforementioned filters, some fading factors were proposed to embed into the above filters to keep them stable and effective [24,25,26]. Wang et al. [24] introduced a modifiable fading factor to tackle the nonlinear estimation problem. After that, several researchers also investigated this problem. Yang et al. [25] investigated the adaptive robust filtering via the robust maximum likelihood estimation, but it can not control the dynamics model biases. Safarinejadian and Yousefi [26] proposed an adaptive fading memory KF to deal with static alignment of inertial navigation systems. Furthermore, Geng and Wang [27] utilized multiple fading factors in KF to handle the filtering divergence with inaccurate system noise. In [25,26,27], the fading factors were embedded into KF, but not nonlinear filters. Therefore, in order to overcome their shortcomings, Karlgaard [20] and Wang et al. [24] embedded the fading factors into nonlinear filters to handle nonlinear problems.

Motivated by the above discussion, this work proposes a new adaptive robust UKF scheme based on both fading factor and maximum correntropy criterion (MCC) to focus on the state estimation problems with measurement outliers. To the best knowledge of the authors, for the nonlinear tracking problem, the filtering algorithm based on MCC and fading factor had not been studied before. In our research, tracking a moving target by a sensor has been carried out to compare with UKF and an adaptive Huber unscented Kalman filter [24]. Furthermore, the prime dedications of this paper are as follows: (1) the cost function of the proposed filter is proposed, the fading factor is applied in the state model and the maximum correntropy criterion is used in the measurement model; (2) the proposed filter can suppress the effect of outliers effectively in the dynamics system (state or/and observation equation); and (3) the proposed filter is easy to perform and has better performance in the presence of outliers.

The structure of this paper is organized as follows. The Section 2 briefly introduces the fundamentals of the proposed filter. Section 3 proposes the adaptive robust unscented Kalman filter based on fading factor and MCC for a nonlinear system. Simulation results and comparisons are provided to confirm the feasibility and superiority of the proposed filter in Section 4. Finally, conclusions are shown in Section 5.

2. Fundamentals of the Proposed Filter

2.1. Maximum Correntropy Criterion

For any random variables X and Y, the correntropy is defined as

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

(1)

where $E[·]$ represents the mean value, and $\rho (·)$ is a real-valued continuous, symmetric and nonnegative definite kernel function, respectively. $F_{X,Y}(x,y)$ is the joint probability density function of X and Y. However, $F_{X,Y}(x,y)$ is usually unknown, and numbers of data samples, $(x_i,y_i),$ $(i=1,2,\dots ,N)$ can be obtained. Therefore, the correntropy can be computed as follows:

$$\tilde{V}(X,Y)=\frac{1}{N}\sum _{i=1}^N\rho (x_i,y_i).$$

(2)

In theory, various kernel functions can be used. In general, the Gaussian kernel function is selected as follows:

$${\kappa }_G(e_i)={\kappa }_G(x_i-y_i)=\frac{1}{\sqrt{2\pi \sigma }}exp\{-\frac{e_i^2}{2{\sigma }^2}\},$$

(3)

where $e_i=x_i-y_i$, and $\sigma$ is the kernel width of correntropy, respectively. If $e_i=0$, ${\kappa }_G(e_i)$ can reach its maximum value. Therefore, the cost function of MCC is expressed by

$$J_{MCC}=min(\sum _{i=1}^N({\kappa }_G(0)-{\kappa }_G(e_i))).$$

(4)

2.2. Cost Function of Adaptive Robust Kalman Filter

In this section, the cost function of the adaptive robust Kalman filter is derived by analyzing the following linear state model and measurement model:

$$x_k={\mathsf{\Phi }}_{k|k-1}{x}_{k-1}+w_{k-1}{y}_k=H_k{x}_k+v_k,$$

(5)

where k denotes discrete time, $x_k\in {\mathbb{R}}^n$ represents $n\times 1$ system state vector at time k, ${\mathsf{\Phi }}_{k|k-1}$ is the state transition matrix, $y_k\in {\mathbb{R}}^m$ is $m\times 1$ system measurement vector at time k, $H_k$ is observation matrix, $w_{k-1}$ is the process noise with the covariance $Q_{k-1}$, $v_k$ is the measurement noise with the covariance $R_k$, and they are uncorrelated zero-mean Gaussian white noises.

For simplicity, the problem of state and measurement outliers is focused on in this work. For the linear state space model (5), a combined cost function is used to perform two different criterions to the state model and measurement model. Utilizing the Bayesian maximum likelihood, the posterior mean estimate is derived by minimizing the following function

$${\widehat{x}}_{k|k}=argmin(\parallel x_k-{\widehat{x}}_{k|k-1}{\parallel }_{P_{k|k-1}^{-1}}^2+\parallel H_k{x}_k-y_k{\parallel }_{R_k^{-1}}^2),$$

(6)

where ${\parallel x\parallel }_A^2=x^{T}Ax$ is the quadratic form with respect to A (A is nonnegative definite matrix ); ${\widehat{x}}_{k|k}$ is the posteriori estimate, ${\widehat{x}}_{k|k-1}$ is the priori estimate, $P_{k|k-1}$ is the covariance matrix of ${\widehat{x}}_{k|k-1}$.

Denoting ${\xi }_k=R_k^{-1/2}(H_k{x}_k-y_k)$ and utilizing MCC in Equation (4), the cost function of the filtering algorithm in Equation (6) is formulated as follows:

$${\widehat{x}}_{k|k}=argmin(\parallel x_k-{\widehat{x}}_{k|k-1}{\parallel }_{P_{k|k-1}^{-1}}^2+\sum _{j=1}^m(k_G(0)-k_G({\xi }_{k,j}))),$$

(7)

where the term m is the dimension of the measurement model. Furthermore, the fading factor is embedded to strengthen the robustness property of the KF with model error. Then, the cost function in Equation (7) can be rearranged as

$$\begin{array}{c}{\widehat{x}}_{k|k}=argmin(\parallel x_k-{\widehat{x}}_{k|k-1}{\parallel }_{{({\alpha }_k{P}_{k|k-1})}^{-1}}^2+\sum _{j=1}^m(k_G(0)-k_G({\xi }_{k,j}))).\hfill \end{array}$$

(8)

Differencing Equation (8) with respect to $x_k$, one has that

$${({\alpha }_k{P}_{k|k-1})}^{-1}(x_k-{\widehat{x}}_{k|k-1})-\sum _{i=1}^n\frac{\partial k_G({\xi }_{k,i})}{\partial {\xi }_{k,i}}\frac{\partial {\xi }_{k,i}}{\partial x_k}=0.$$

(9)

Substituting Equation (3) into Equation (9), it can be obtained that

$${({\alpha }_k{P}_{k|k-1})}^{-1}(x_k-{\widehat{x}}_{k|k-1})+\frac{1}{\sqrt{2\pi }{\sigma }^3}\sum _{i=1}^{m}exp(-\frac{{\xi }_{k,i}^2}{2{\sigma }^2}){\xi }_{k,i}\frac{\partial {\xi }_{k,i}}{\partial x_k}=0.$$

(10)

Define the function

$${\mathsf{\Psi }}_j=\frac{1}{\sqrt{2\pi }{\sigma }^3}\sum _{i=1}^{m}exp(-\frac{{\xi }_{k,i}^2}{2{\sigma }^2})$$

(11)

and the matrix

$$\mathsf{\Psi }=\mathrm{diag}\left[{\mathsf{\Psi }}_j\right]j=1,\dots ,m.$$

(12)

Thus, we find out that Equation (10) can be redescribed as follows:

$${({\alpha }_k{P}_{k|k-1})}^{-1}(x_k-{\widehat{x}}_{k|k-1})+H_k^T{R}^{-T/2}\mathsf{\Psi }{\xi }_k=0.$$

(13)

Substituting ${\xi }_k$ into Equation (13), we arrive at

$${({\alpha }_k{P}_{k|k-1})}^{-1}(x_k-{\widehat{x}}_{k|k-1})+H_k^T{R}^{-T/2}\mathsf{\Psi }{R}_k^{-1/2}(H_k{x}_k-y_k)=0.$$

(14)

Equation (14) satisfies the minimization solution of the cost function as follows:

$${\widehat{x}}_{k|k}=argmin(\parallel x_k-{\widehat{x}}_{k|k-1}{\parallel }_{{\tilde{P}}_{k|k-1}^{-1}}^2+\parallel H_k{x}_k-y_k{\parallel }_{{\tilde{R}}_k^{-1}}^2),$$

(15)

where ${\tilde{P}}_{k|k-1}={\alpha }_k{P}_{k|k-1}$ and ${\tilde{R}}_k=R_k^{T/2}{\mathsf{\Psi }}_k^{-1}{R}_k^{1/2}$. Comparing Equation (15) with Equation (6), it can be seen that covariance matrixes of them are different, and others are identical. According to the iterative equations of KF, the explicit solution of Equation (15) can be performed as

$$K_k={\tilde{P}}_{k|k-1}{H}_k^T{(H_k{\tilde{P}}_{k|k-1}{H}_k^T+{\tilde{R}}_k)}^{-1},$$

(16)

$${\widehat{x}}_{k|k}={\widehat{x}}_{k|k-1}+K_k(y_k-H_k{\widehat{x}}_{k|k-1}),$$

(17)

$${\tilde{P}}_{k|k}=(I_n-K_k{H}_k){\tilde{P}}_{k|k-1}.$$

(18)

Remark 1.

From an engineering viewpoint, it can be seen that the real state vector $x_k$ can not be obtained. However, some methods are proposed to change the proportion of state vector, and they can not utilize the state vector—for example, maximum correntropy criterion, Huber function, and so on. Although the state vector was coped with in [22], the real state $x_k$ was replaced by estimation vector ${\widehat{x}}_{k|k-1}$ in the process.

2.3. Formation of the Fading Factor

The state estimate of Kalman filter depends on the ratio of new measurements and the ones which are based on predicted state vector, dynamics model, and all previous measurements. If the state model error is much larger than that of the measurement model, it is obtained that the information from new measurements will be ignored. Thus, the result of the filter will become poor. The fading factor in Equation (9) is adopted to ensure the performance of the filter in the presence of dynamics model error. For improving the utilization rate of the new measurements, the fading factor ${\alpha }_k$ in Equation (8) becomes greater than 1. Subsequently, the variance matrix $P_{k|k-1}$ is inflated. Therefore, it is obtained that the contribution of ${\widehat{x}}_{k|k-1}$ to ${\widehat{x}}_{k|k}$ is reduced, and the impact of dynamics model error would be small.

Next, the fading factor ${\alpha }_k$ in Equation (5) is analyzed and derived via the innovation sequence orthogonal principle [28]

$$E\left\{v_{k+j}{v}_k^T\right\}=0j=0,1,2,\dots k=1,2,\dots ,$$

(19)

where $E\{.\}$ and $v_k$ are expected value and innovation sequence, respectively.

For the state model and measurement model stated by Equation (5), the lemma in [28] is given as follows:

Lemma 1.

If $\parallel x_k-{\widehat{x}}_{k|k}\parallel$ is significantly smaller than $\parallel x_k\parallel$, then, for any j,

$$\begin{array}{cc}\hfill C_{j,k}& =E\left\{v_{k+j}{v}_k\right\}\hfill \\ & =\mathsf{\Theta }(k+j,\dots ,k,{\widehat{x}}_{k+j|k+j-1},\dots ,{\widehat{x}}_{k|k-1})(P_{x_k{y}_k}-K_k{C}_{0,k})\hfill \\ & =0,\hfill \end{array}$$

(20)

where $P_{x_k{y}_k}$ is the cross covariance between state and measurement, $K_k$ represents the Kalman gain, $C_{0,k}$ equals to $E\left\{v_k{v}_k\right\}$, and $\mathsf{\Theta }(k+j,\dots ,k,{\widehat{x}}_{k+j|k+j-1},\dots ,{\widehat{x}}_{k|k-1})$ can be written as follows:

$$\mathsf{\Theta }(k+j,\dots ,k,{\widehat{x}}_{k+j|k+j-1},\dots ,{\widehat{x}}_{k|k-1})=H_{k+j}\prod _{l=k+1}^{k+j-1}{\mathsf{\Phi }}_l(I-K_l{H}_l){\mathsf{\Phi }}_k.$$

(21)

The proof of Lemma 1 is omitted here. Please refer to Zhou et al. [28] if needed.

It can be shown that Lemma 1 holds strictly for a linear system, and it is approximately true for a nonlinear system. A sufficient condition of Equation (20) is given as follows:

$$P_{x_k{y}_k}-K_k{C}_{0,k}=0.$$

(22)

Using the common criterion of UKF leads to $K_k=P_{x_k{y}_k}{P}_{y_k{y}_k}^{-1}$, and

$$P_{x_k{y}_k}(I-P_{y_k{y}_k}^{-1}{C}_{0,k})=0,$$

(23)

where $P_{y_k{y}_k}$ is the error covariance matrix of measurement, and a sufficient condition of Equation (23) is given as follows:

$$P_{y_k{y}_k}=C_{0,k}.$$

(24)

According to equations of KF that $P_{y_k{y}_k}=H_k{P}_{k|k-1}{H}_k^T+R_k$, Equation (24) can be rewritten as

$$H_k{P}_{k|k-1}{H}_k^T+R_k-C_{0,k}=0.$$

(25)

If there exists a fading factor such that Equation (25) holds, the innovations $v_{k+j}$ and $v_k$ become approximately orthogonal. Thus, Equation (25) can be redescribed as

$$H_k{\alpha }_0{P}_{k|k-1}{H}_k^T=C_{0,k}-R_k.$$

(26)

Noting that the terms $H_k,P_k,C_{0,k}$ and $R_k$ are full rank, by using the property of matrix trace, we have

$$tr\left[H_k{\alpha }_0{P}_{k|k-1}{H}_k^T\right]=tr[C_{0,k}-R_k].$$

(27)

Because ${\alpha }_0$ is a scalar, then it can be expressed as follows:

$${\alpha }_0=\frac{tr[C_{0,k}-R_k]}{tr\left[H_k{P}_{k|k-1}{H}_k^T\right].}$$

(28)

It seems that Equation (28) can only be calculated if the state model (5) is a linear or linearized system. However, the linearization term $H_k{P}_{k|k-1}{H}_k^T$ can be readily approximated by the other nonlinear methods—for example, EKF and UKF, and so on. The focused UKF is utilized in this work. Therefore, the fading factor ${\alpha }_0$ can be applied to the UKF framework, and it is redescribed as follows:

$${\alpha }_0=\frac{tr[C_{0,k}-R_k]}{tr[P_{y_k{y}_k}-R_k]},$$

(29)

where $C_{0,k}$ can be expressed as follows:

$$C_{0,k}=\left\{\begin{array}{cc}{v}_k{v}_k^T,\hfill & k=1,\hfill \\ \frac{\lambda C_{0,k-1}+v_k{v}_k^T}{1+\lambda },\hfill & k>1,\hfill \end{array}\right.$$

(30)

where the forgetting factor $\lambda$ is commonly set as 0.95 [19].

Remark 2.

In the framework of the proposed filter, the fading factor ${\alpha }_0$ is a single fading factor. For the complicated multivariable systems in actual applications, it is not enough to use a fading factor. To overcome the shortcomings, Yang et al. [25] presented a multiple fading factor Kalman filter to deal with multivariable systems, and ${\alpha }_0$ is replaced by a matrix of fading factors $\mathsf{\Lambda }=diag({\alpha }_0,{\alpha }_1,\dots ,{\alpha }_n)$.

3. The New Adaptive Robust Unscented Kalman Filter

In this section, we will focus on the proposed unscented Kalman filter based on fading factor and maximum correntropy criterion. The cost function of the proposed filter in Equation (15) has been derived, and it is embedded into the structure of UKF. In addition, it is well known that UKF has some good properties: easy implementation, appropriate performance and computational feasibility. Therefore, it is very popular in the nonlinear system.

Suppose that the nonlinear discrete-time system can be modelled as follows:

$$x_k=f(x_{k-1})+{\omega }_{k-1}{y}_k=h(x_k)+r_k,$$

(31)

where $x_k$ is the state vector with the covariance matrix $P_k$, and the other terms have the same meaning as the above terms in Equation (5), respectively. The initial estimate state and its covariance matrix are given as ${\widehat{x}}_0=E(x_0)$ and $P_0=E\left[(x_0-{\widehat{x}}_0){(x_0-{\widehat{x}}_0)}^T\right]$. Similarly, the estimate state and its covariance matrix at time $k-1$ are given as

$${\widehat{x}}_{k-1}=E(x_{k-1}),$$

(32)

$$P_{k-1}=E\left[(x_{k-1}-{\widehat{x}}_{k-1}){(x_{k-1}-{\widehat{x}}_{k-1})}^T\right].$$

(33)

The procedure of the proposed adaptive robust unscented Kalman filter depicted in

Table 1. AMUKF. AMUKF is short for Adaptive Robust Unscented Kalman Filter.

First set ${\mathit{x}}_{0,\mathit{k}|\mathit{k}}$ = [0 m, 1400 m, 2 m/s, −10 m/s]${}^{\mathit{T}}$.

$P_{0,k|k}=diag[1,1,1,1]$.

Then iterate the follow,

for $i=0:N-1$

Reformulate the augmented covariance

time update

${\chi }_{i,k|k-1}=f({\chi }_{i,k-1})$

${\widehat{x}}_{k|k-1}={\sum }_{i=0}^{2n}{\omega }_i^m{\chi }_{i,k|k-1}$

Measurement update:

${\widehat{x}}_{i,k|k-1}=[{\chi }_{i,k|k-1}{\chi }_{i,k|k-1}+\mu (\sqrt{P_{i,k|k-1}}){\chi }_{i,k|k-1}-\mu (\sqrt{P_{i,k|k-1}})]$

$y_{i,k|k-1}=h({\widehat{x}}_{i,k|k-1})$

${\widehat{y}}_{k|k-1}={\Sigma }_{i=0}^{2n}{\omega }_k{y}_{i,k|k-1}$

Calculate ${\alpha }_k$

$v_k=y_k-{\widehat{y}}_{k|k-1}$

$C_{0,k}=\left\{\begin{array}{cc}{v}_k{v}_k^T\hfill & k=1\hfill \\ \frac{\lambda C_{0,k-1}+v_k{v}_k^T}{1+\lambda }\hfill & k>1\hfill \end{array}\right.$

${\alpha }_k=\left\{\begin{array}{cc}{\alpha }_0\hfill & {\alpha }_0>1\hfill \\ 1\hfill & {\alpha }_0\le 1\hfill \end{array}\right.$

$P_{k|k-1}={\alpha }_k\mathrm{tr}\left[{\sum }_{i=0}^{2n}{\omega }_i^c(E_i){(E_i)}^T+Q_k\right]$

$P_{y_k{y}_k}={\alpha }_k{\sum }_{i=0}^{2n}{\omega }_i^c(y_{i,k|k-1}-{\widehat{y}}_{k|k-1}){(y_{i,k|k-1}-{\widehat{y}}_{k|k-1})}^T+{\tilde{R}}_k$

$P_{x_k{y}_k}={\alpha }_k{\sum }_{i=0}^{2n}{\omega }_i^c({\chi }_{i,k|k-1}-{\widehat{y}}_{k|k-1}){(y_{i,k|k-1}-{\widehat{y}}_{k|k-1})}^T$

$K_k=P_{x_k{y}_k}{P}_{y_k{y}_k}^{-1}$${\widehat{x}}_{k|k}={\widehat{x}}_{k|k-1}+K_k(y_k-{\widehat{y}}_{k|k-1})$

$P_{k|k}=P_{k|k-1}-K_k{P}_{y_k{y}_k}{K}_k^T$

end

where $E_i={\chi }_{i,k|k-1}-{\widehat{x}}_{k|k-1}$, $\mu =\sqrt{n+\kappa }$,

$W_0=\kappa /(n+\kappa )$, $W_i=\kappa /(n+\kappa ),$

$i=1,2,\cdots ,2n$ and $\kappa$ is a tune parameter.

More details about the selection of $\kappa$ can be seen in [11,22].

is described as follows:

Step 1: The Unscented Transform with 2n + 1 symmetric sigma points ${\chi }_{i,k-1}$ and weights update:

$$\begin{array}{cc}\hfill {\chi }_{0,k-1}& ={\widehat{x}}_{k-1},\hfill \\ \hfill {\chi }_{i,k-1}& ={\widehat{x}}_{k-1}+\sqrt{n+\delta }{(\sqrt{P_{k-1}})}_i,\hfill \\ & i=1,2,\dots ,n,\hfill \\ \hfill {\chi }_{i,k-1}& ={\widehat{x}}_{k-1}-\sqrt{n+\delta }{(\sqrt{P_{k-1}})}_i,\hfill \\ & i=n+1,n+2,\dots ,2n,\hfill \end{array}$$

(34)

where $\delta ={\phi }^2(n+\kappa )-n$, $\phi$, which ranges from 0 to 1, controls the distribution of sigma points. $\kappa$ equals $3-n$, and $\sqrt{P_{k-1}}$ is the Cholesky factor of $P_{k-1}$, respectively.

The corresponding weights ${\omega }_0^m$ and covariance ${\omega }_0^c$ are as follows:

$$\begin{array}{cc}\hfill {\omega }_0^m& =\frac{\delta }{n+\delta },\hfill \\ \hfill {\omega }_0^c& =\frac{\delta }{n+\delta }+(1-{\phi }^2+\eta ),\hfill \\ \hfill {\omega }_i^m& ={\omega }_i^c=\frac{1}{2(n+\delta )}i=1\dots .,2n,\hfill \end{array}$$

(35)

where $\eta$ is set as 2 generally for Gaussian distribution and relates with the prior distribution of state.

Step 2: Time update

$${\chi }_{i,k|k-1}=f({\chi }_{i,k-1}){\widehat{x}}_{k|k-1}=\sum _{i=0}^{2n}{\omega }_i^m{\chi }_{i,k|k-1}.$$

(36)

Step 3: Modified measurement covariance update

$$y_{i,k|k-1}=h({\chi }_{i,k-1}){\widehat{y}}_{k|k-1}=\sum _{i=0}^{2n}{\omega }_i^m{y}_{i,k|k-1},$$

(37)

$${\xi }_k=R_k^{-1/2}({\widehat{y}}_{k|k-1}-y_k),$$

(38)

$$\mathsf{\Psi }=\mathrm{diag}\left[{\psi }_{k,j}({\xi }_{k,j})\right]j=1,2,\dots ,m,$$

(39)

$${\tilde{R}}_k=R_k^{T/2}{\mathsf{\Psi }}^{-1}{R}_k^{1/2}.$$

(40)

Step 4: Fading factor update

$$v_k=y_k-{\widehat{y}}_{k|k-1},$$

(41)

$$C_{0,k}=\left\{\begin{array}{cc}{v}_k{v}_k^T,\hfill & k=1,\hfill \\ \frac{\lambda C_{0,k-1}+v_k{v}_k^T}{1+\lambda },\hfill & k>1,\hfill \end{array}\right.$$

(42)

$${\alpha }_0=\frac{tr[C_{0,k}-R_k]}{tr\left[{\sum }_{i=0}^{2n}{\omega }_i^c(y_{k,i}-{\widehat{y}}_{k|k-1}){(y_k-{\widehat{y}}_{k|k-1})}^T\right]},$$

(43)

$${\alpha }_k=\left\{\begin{array}{cc}{\alpha }_0,\hfill & {\alpha }_0>1,\hfill \\ 1,\hfill & {\alpha }_0\le 1.\hfill \end{array}\right.$$

(44)

Step 5: Measurement update

$$P_{k|k-1}={\alpha }_k\left[\sum _{i=0}^{2n}{\omega }_i^c({\chi }_{i,k|k-1}-{\widehat{x}}_{k|k-1}){({\chi }_{i,k|k-1}-{\widehat{x}}_{k|k-1})}^T+Q_k\right],$$

(45)

$$P_{y_k{y}_k}={\alpha }_k\sum _{i=0}^{2n}{\omega }_i^c(y_{k,i}-{\widehat{y}}_{k|k-1}){(y_{k,i}-{\widehat{y}}_{k|k-1})}^T+{\tilde{R}}_k,$$

(46)

$$P_{x_k{y}_k}={\alpha }_k\sum _{i=0}^{2n}{\omega }_i^c({\chi }_{i,k|k-1}-{\widehat{x}}_{k|k-1}){(y_{i,k|k-1}-{\widehat{y}}_{k|k-1})}^T,$$

(47)

$$P_{k|k}=P_{k|k-1}-K_k{P}_{y_k{y}_k}{K}_k^T,$$

(48)

$$K_k=P_{x_k{y}_k}{P}_{y_k{y}_k}^{-1},$$

(49)

$${\widehat{x}}_{k|k}={\widehat{x}}_{k|k-1}+K_k(y_k-{\widehat{y}}_{k|k-1}).$$

(50)

Remark 3.

In the process of nonlinear filter, it can be seen that both fading factor and maximum correntropy criterion are applied in the cost function of the new filter. We can obtain that the estimate state ${\widehat{x}}_k$ is more accurate than estimate state ${\widehat{x}}_k$ using other nonlinear filters such as EKF, UKF and other common nonlinear filters, etc. First, the proposed filter has better adaptive ability to balance the contribution between the process model information and measurement on the state vector. Second, the proposed filter can retain the effect of outliers. Particularly, when the state or/and measurement model are contaminated by outliers, the effectiveness of the proposed filter is much better than that of UKF.

4. Simulation and Comparison

To illustrate the practical applicability of the proposed nonlinear filtering algorithm, two classical filtering applications are employed in this section.

4.1. Radar Tracking System

We track a moving object by a radar which utilizes measurements of distances information. The two-dimensional system uses a single station for tracking targets, and the state vector includes position and velocity. The dynamics model moves within two dimensional plane according to the standard dynamics model [29]

$$X_k=\mathsf{\Phi }{X}_{k-1}+\Gamma u_k,$$

(51)

where $X_k=[x,\dot{x},y,\dot{y}]$, x and y are Cartesian coordinates of the moving target, and $\dot{x}$ and $\dot{y}$ are correlative velocities of the moving target, respectively. $u_k$ is zero mean Gaussian white noise with the covariance $Q=diag([{10}^{-4},{10}^{-4}])$. The transition matrix and noise excitation matrix are as follows:

$$\mathsf{\Phi }=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]\mathrm{and}\Gamma =\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right],$$

(52)

where $T=1$ is the time interval, and the measurement equation is

$$Z_k=\sqrt{{(x_k-o_x)}^2+{(k_k-o_y)}^2}+v_k,$$

(53)

where $(o_x,o_y)$ = (0, 1000) is land station, the variance of $r_k$ is $R=5$, and $v_k$ is tuning parameter satisfying $v\ll 1$.

Next, we will analyze and compare estimation performance of the following nonlinear filters that are conventional unscented Kalman filter (UKF), adaptive Huber unscented Kalman filter (AHUKF) [24], and the proposed filtering algorithm (AMUKF). Those three filters are utilized to track position and velocity of the maneuvering vehicle.

The initial position and velocity of the maneuvering vehicle are set as [0 m, 1400 m] and [2 m/s, −10 m/s], respectively. The value of initial variance $P_0$ is $diag[1,1,1,1]$. Simulation time lasts 50 s, the parameter $\sigma$ of Gaussian kernel function is set as 0.8 here. In this simulation, two cases are considered: (1) measurement outliers and (2) state and measurement outliers simultaneously.

For the first case, we set the measurement outliers ${\widehat{Z}}_{20}=Z_{20}+2$ (m) and ${\widehat{Z}}_{35}=Z_{35}-1$ (m). Under the same condition, Figure 1, Figure 2 and Figure 3 show the tracking performance of UKF, AHUKF and the proposed filter (AMUKF). Figure 1 shows the tracking results of the three filters above, and the effects of AMUKF are better than that of other filters. The performance of AHUKF and AMUKF is much better than that of UKF because the UKF is a nonlinear extension of the KF, and it is susceptible to outliers. In Figure 1, we can see that AHUKF has a robust property to some extent, and it can be seen that the impact of state outlier is to be suppressed to a certain extent. However, comparing with AMUKF, it is not better than that of AMUKF. In summary, we can draw the conclusion that AMUKF can suppress the outliers and has better robust properties. Thus, it has a better tracking result for true state. The tracking errors obtained by AHUKF are bigger than that obtained by AMUKF. Figure 2 and Figure 3 show the position errors and velocity errors about the three filters, respectively. In contrast, under the measurement outlier, the estimation position and velocity errors for AMUKF are always smaller than that of other filters.

For the second case, we also estimate position and velocity like the first case. The state outliers are set as ${\widehat{X}}_{10}=X_{10}$ + 0.2 × [5,1,5,1]${}^T$ (m), ${\widehat{X}}_{25}=X_{25}$ − 0.4 × [5,1,5,1] ${}^T$ (m), and the measurement outliers are set as ${\widehat{Z}}_{20}=Z_{20}+2$ (m)$,{\widehat{Z}}_{35}=Z_{35}-5$ (m), respectively. From Figure 4, Figure 5 and Figure 6, we can obtain that the effect of the three different filters when the process and measurement have outliers simultaneously. From Figure 4, it can be seen that the tracking results of the three filters in case 2, and the performance of AMUKF is better than that of other filters. The results of AHUKF and AMUKF are much better than that of UKF because AHUKF has robust properties, the effectiveness of AHUKF is approaching that of AMUKF, but it is not better than that of AMUKF. The tracking errors obtained by AHUKF are bigger than that obtained by AMUKF. Figure 5 and Figure 6 show the position errors and velocity errors about the three filters, respectively. Finally, it can be concluded that the errors of AMUKF are smaller than that of AHUKF. From Figure 1 and Figure 4, we also see that the behaviours of three filters are convergent. Finally, the root mean square error (RMSE) of state is shown in

Table 2. RMSE of State. RMSE is short for root mean square error.

Filter	RMSE of x	RMSE of $\dot{\mathit{x}}$	RMSE of y	RMSE of $\dot{\mathit{y}}$
UKF	28.1	0.315	145	0.117
AHUKF	26.7	0.017	139	0.037
AMUKF	25.8	0.011	135	0.029

, which shows that the presented filtering algorithm outperforms both UKF and AHUKF.

4.2. Mars Entry Model

In this subsection, the Mars entry model is considered. During Mars entry, the vehicle is affected by some uncertainty disturbances such as lift, drag, gravity, and measurement outliers, etc. Measurement outliers are investigated. The dynamics of the entry vehicle are described by [30]

$$\begin{array}{c}\hfill \left[\begin{array}{c}\dot{\mathbf{r}}\\ \dot{\mathbf{v}}\end{array}\right]=\left[\begin{array}{c}\mathbf{v}\\ -D\frac{\mathbf{v}}{\parallel \mathbf{v}\parallel }-Lcos\tilde{\sigma }\frac{\mathbf{v}}{\parallel \mathbf{v}\parallel }\times \frac{\mathbf{v}\times \mathbf{r}}{\parallel \mathbf{v}\times \mathbf{r}\parallel }+Lsin\tilde{\sigma }\frac{\mathbf{v}\times \mathbf{r}}{\parallel \mathbf{v}\times \mathbf{r}\parallel }-g\frac{\mathbf{r}}{\parallel \mathbf{r}\parallel }\end{array}\right],\end{array}$$

(54)

where $\mathbf{r}={[r_x,r_y,r_z]}^T$ and $\mathbf{v}={[v_x,v_y,v_z]}^T$ denote position vector and velocity vector, and $\tilde{\sigma }$ is bank angle. The lift acceleration L, drag acceleration D and gravitational acceleration g are

$$L=\frac{1}{2}\tilde{\rho }{\parallel \mathbf{v}\parallel }^{2}s/C_{L}MD=\frac{1}{2}\tilde{\rho }{\parallel \mathbf{v}\parallel }^{2}s/C_{D}Mg=\frac{\tilde{g}}{R^2},$$

(55)

where $\parallel \mathbf{r}\parallel$ and $\parallel \mathbf{v}\parallel$ are the scalar values of $\mathbf{r}$ and $\mathbf{v}$, M is the mass of the vehicle, and $\tilde{g}$ is the planet’s gravitational constant. Atmosphere density $\tilde{\rho }$ is given by

$$\begin{array}{c}\hfill \tilde{\rho }={\tilde{\rho }}_{0}exp(\frac{r_0-\parallel \mathbf{r}\parallel }{h_s}),\end{array}$$

(56)

where ${\tilde{\rho }}_0=2\times {10}^{-4}$ kg/m${}^3$ is the nominal reference density, $r_0=3.4372\times {10}^6$ m is the reference radius of Mars (40 km above Mars surface), and $h_s=7500$ m is the constant scale height.

For the measurement model, the two Mars orbiters–Mars Reconnaissance Orbiter (MRO) and Mars Express (MEX) used by Curiosity for relay communications are employed. The movement of the orbiter in polar coordinate can be constructed [31]:

$$\begin{array}{c}\hfill r_{ob}=\frac{a(1-e^2)}{1+ecos(\theta -\overline{\omega })}{r}_{ob}^2\dot{\theta }=\sqrt{\tilde{g}a(1-e^2)},\end{array}$$

(57)

where $r_{ob}$ is the distance between the orbiter and the center of Mars, $\theta$ is the angle between the ascending node and the orbiter, a is the semi-major axis of the movement ellipse, e is the eccentricity ratio, $\overline{\omega }$ is the argument of perigee, and $\tilde{g}$ is the gravity constant of Mars. The ecliptic longitude and latitude of the orbiter are:

$${\mathsf{\Omega }}_{ob}=\left\{\begin{array}{cc}-{cos}^{-1}\left[\frac{cos\theta }{cos{\varphi }_{ob}}\right]+\mathsf{\Omega },& {\varphi }_{ob}\le 0,\\ {cos}^{-1}\left[\frac{cos\theta }{cos{\varphi }_{ob}}\right]+\mathsf{\Omega },& {\varphi }_{ob}>0,\end{array}\right.$$

(58)

where ${\varphi }_{ob}={sin}^{-1}\left[\frac{sin\theta sini}{sink}\right]$, ${\mathsf{\Omega }}_{ob}$ is ecliptic longitude, and ${\varphi }_{ob}$ is the ecliptic latitude, and i is orbital inclination (angle between longitude and latitude, $k=\frac{\pi }{2}$). Thus, the relative range measurement between the vehicle and the orbiter can be constructed:

$$\begin{array}{c}\hfill \begin{array}{c}{R}_{ob}=\sqrt{{(\mathbf{r}-{\mathbf{r}}_{ob})}^T(\mathbf{r}-{\mathbf{r}}_{ob})},\hfill \\ {\tilde{R}}_{ob}=R_{ob}+{\epsilon }_{ob},\hfill \end{array}\end{array}$$

(59)

where ${\mathbf{r}}_{ob}={(cos{\varphi }_{ob}cos{\mathsf{\Omega }}_{ob}cos{\varphi }_{ob}sin{\mathsf{\Omega }}_{ob}sin{\varphi }_{ob})}^T$, and ${\epsilon }_{ob}$ is the range measurement noise, whose prior covariance is set to be 100, meaning the range measurement error is 10 m.

The relative range measurements between Mars surface beacons (MSBs) and the vehicle can be obtained by:

$$\begin{array}{c}\hfill \begin{array}{c}{\tilde{R}}_B^{\widehat{i}}=R_B^{\widehat{i}}+{\epsilon }_R^{\widehat{i}},\hfill \end{array}\end{array}$$

(60)

where $R_B^{\widehat{i}}=\sqrt{{(x_B^{\widehat{i}}-r_x)}^2+{(y_B^{\widehat{i}}-r_y)}^2+{(z_B^{\widehat{i}}-r_z)}^2}$ refers to the true range from the entry vehicle to the ith beacon, $\widetilde{R_B}$ is the measurement range, and $x_B^{\widehat{i}},y_B^{\widehat{i}}$ and $z_B^{\widehat{i}}$ are the position coordinates of MSBs. ${\epsilon }_R^{\widehat{i}}$ is the range measurement noise.

The observation of rate measurement can be given as:

$$\begin{array}{c}\hfill \begin{array}{c}{\tilde{V}}_B^{\widehat{i}}=V_{\widehat{i}}+{\epsilon }_V^{\widehat{i}}{V}_B^{\widehat{i}}=\frac{d{R}_{\widehat{i}}}{dt}\widehat{i}=1,\cdots ,N,\hfill \end{array}\end{array}$$

(61)

where $V_B^{\widehat{i}}$ is the true rate, ${\tilde{V}}_B^{\widehat{i}}$ is the measured rate, and ${\epsilon }_V^{\widehat{i}}$ is the rate measurement noise, whose prior covariance is set to be 1, meaning that the measurement error rate is 1 m/s.

From the above measurement models, the overall observation model can be obtained as

$$\mathit{y}=\left(\begin{array}{c}{\tilde{\mathbf{R}}}_{ob}\\ {\tilde{\mathbf{R}}}_B\\ {\tilde{\mathbf{V}}}_B\end{array}\right).$$

In this simulation, the measurement outliers are considered. We utilize UKF and AHUKF [24] to compare with the proposed filtering algorithm too. Their performance is to be analyzed as follows. First, the initial values are listed in

Table 3. Parameters setting of initial conditions. MSBs is short for Mars surface beacons.

Initial Setting	Notation	Values
Initial position	$(r_{x_0},r_{y_0},r_{z_0})$	(−3.92 km, −3099.09 km, −1663.11 km)
Initial velocity	$(v_{x_0},v_{y_0},v_{z_0})$	(463.25 m/s, −1528.75 m/s, 5268.14 m/s)
MSBs’ locations (1)	$(x_B^1,y_B^1,z_B^1)$	(875.35 km, −2914.43 km, −1509.77 km)
MSBs’ locations (2)	$(x_B^2,y_B^2,z_B^2)$	(410.25 km, −2955.32 km, −1624.04 km)
Vehicle mass	M	2804 kg
Vehicle cross-section	s	15.9 m${}^2$

. The parameters of the two orbiters are listed in

Table 4. Parameters of the orbiters. MRO and MEX are short for Mars Reconnaissance Orbiter and Mars Express, respectively.

	MRO	MEX
semi-major axis a	$3663.7$ km	$8572.2$ km
eccentricity ratio e	$0.0089$ rad	$0.5770$ rad
argument of perigee $\overline{\omega }$	$4.7124$ rad	$2.7911$ rad
orbital inclination i	$1.6154$ rad	$1.5055$ rad

. The simulation time is 500 s.

The initial position and velocity of maneuvering vehicle are also showed in Table 3. The initial errors matrix $P_0=diag([1000;1000;1000;1000;10;10])$. $Q={10}^{-5}\times diag([0.1;0.1;0.1;0.1;0.1;0.1])$; $R=diag([100;100;100;1;1;1])$. The parameter $\sigma$ of Gaussian kernel function is set as 10 here. The measurement outliers are assumed as ${\tilde{\mathit{y}}}_{100}={\mathit{y}}_{100}$ + 0.3 $\times {[800,800,800,800,30,30]}^T$ and ${\tilde{\mathit{y}}}_{200}={\mathit{y}}_{200}-0.4\ast {[800,800,800,800,30,30]}^T$, respectively. From Figure 7, Figure 8 and Figure 9, obviously, it can be seen that the state errors of UKF become very large for measurement outliers. while others’ errors are very small. Therefore, we can determine that the performance of AHUKF and AMUKF is far better than that of UKF, especially AMUKF. Even when the measurement outliers occur, AMUKF can effectively track the movement of the vehicle. Finally, from Figure 7, Figure 8 and Figure 9, we also see that the behaviours of three filters are convergent.

5. Conclusions

In this paper, a new adaptive robust unscented Kalman filter is obtained by combining both the robust property of maximum correntropy criterion and the adaptive property of the fading factor with the superiority of UKF. The cost function of the proposed filter includes both the fading factor to the state model and the maximum correntropy criterion to the measurement model. The maximum correntropy criterion can enhance the ability of being insensitive to outliers, and the fading factor can improve the ability of strongly tracking the state estimate. Therefore, the proposed filtering algorithm can track the state strongly and is insensitive to outliers. After that, the process of fading factor is derived in detail. Two numerical examples are given to illustrate the effectiveness of the proposed filter. UKF and AHUKF are selected for comparison with the proposed filtering algorithm in simulation. When state or/and measurement outliers occur, the proposed filter can track the target effectively and suppress the effect of outliers. Therefore, from the theoretical and numerical simulation results, it can be concluded that the proposed filter has not only the robustness of the fading factor but also the accuracy and flexibility of nonlinear systems. In the future, we will study the stability of the adaptive robust unscented Kalman filter based on the maximum correntropy criterion.