Adaptive Generalized Robust Iterative Cubature Kalman filter

I.   Introduction

Kalman filter (KF) is a commonly used tool in state estimation problems. The KF is utilized in numerous fields including power systems [1], vehicle navigation [2], and radar tracking [3], etc. While the traditional KF is developed for linear systems [4], various variants of KF such as extend Kalman filter (EKF) [5], unscented Kalman filter (UKF) [4], [6] and cubature Kalman filter (CKF) [7], [8] are further developed for nonlinear systems [4].However, with the increasing nonlinearity of the system, it is difficult to obtain good state estimation results with the traditional nonlinear KF variant. Therefore, iterative algorithms such as iterative EKF [9], [10], iterative UKF [11] and iterative CKF [12], [13] etc are developed to further enhance the performance of KF when dealing with nonlinear systems. In addition, different algorithms have been proposed to improve the accuracy of nonlinear state estimation.

Various types of non-Gaussian noise problems in the observations can occur due to various failures that may occur in the sensors during the state estimation process, such as sudden changes in the measurements due to sensor damage. However, the KF and its nonlinear variants using the Minimum Mean Square Error (MMSE) criterion leads to a degradation of the filter’s performance in the face of non-Gaussian noise situations and do not allow for accurate state estimation [11].In order to address this problem, various variants of KF have been proposed in existing research using different robust criteria instead of MMSE. Among them, the Maximum Correntropy Criterion (MCC) [14] as well as the Minimum Error Entropy (MEE) criterion [4] have been used to replace the MMSE criterion to optimize the KF to develop the Maximum correntropy Kalman filtering (MCKF) [15] and Minimum Error Entropy Kalman Filter (MEEKF) [16] due to their strong performance in the face of non-Gaussian noise environments. Furthermore, the MCKF and MEEKF are improved so that the two algorithms can be applied to more complex systems and noisy environments [17] [18]. For instance, to investigate systems with state saturation as well as stochastic nonlinear systems, C-MCKF [19] is developed to solve the problem. And there is also iterative MCUKF [11] and improve iterative MCCKF [20] developed for nonlinear systems. In addition, the strong performance of M-estimation cost functions to deal with non-Gaussian noise is also noted, where cost functions such as Huber, Cauchy, Welsch, etc., show excellent performance in dealing with non-Gaussian noise. Various robust Kalman filters, such as Huber-UKF [21] M-estimation-based MCKF [22], Outlier Robust Iterative EKF [10], and so on [23], have been developed using the M-estimation cost function. Moreover, the impressive efficacy of a general robust loss function [24] has been widely used in the arenas of deep learning, state tracking, and image recognition [24], [25]. It adjusts the shape of the core in real time to the noise environment. And it can cope with complex noise environments relative to the general M-estimation cost function. However, its single kernel parameter tuning still leads to degradation of the algorithm’s performance when faced with more complex noise distribution.

Therefore, for various types of non-Gaussian noise problems caused by sensors damage and so on, in this paper, we improve the general robust loss function to propose a new generalized general robust loss function. And further considering the nonlinear state estimation problem of the robust Kalman filtering algorithm, an Adaptive Generalized Robust Iterative Cubature Kalman filter (AGRICKF) is then proposed. The main contributions of this paper are as follows

1. A new generalized general robust loss function is proposed by adopting a shape modification parameter on the basis of the general robust loss function, which effectively cope with more complex noise distributions.

2. An adaptive generalized robust iterative cubic Kalman filter (AGRICKF) is proposed. The algorithm can effectively handle non-Gaussian noise in state estimation as well as nonlinear state estimation problems. And an adaptive method for the parameters of the algorithm is given.

3. The experimental results validate the performance of the proposed algorithm.

The rest of the article is organized as follows. In Section 2 we give the form of the generalized general robust loss function. The main steps of the Adaptive Generalized Robust Iterative Cubature Kalman filter and parameter optimization are given in Section 3. The results of the experiments are given in Section IV and finally the conclusions are drawn in Section V.

II.   Generalized General Robust Loss Function

Before proposing the generalized general robust loss function, we first review the general robust loss function, which has the following general form [24], [25]

When $\alpha$ is the shape parameter that determines the robustness of this function. When $\alpha = 2$, it is $L_2$ loss; when $\alpha = 0$, it becomes Cauchy; when $\alpha = -2$, it becomes Geman-McClure loss; and when $\alpha = -\infty$, it is Correntropy induced. $\beta$ is then a proportionality parameter that controls the size of the loss quadratic bowl for errors near 0. Considering that a single parameter tuning does not allow the cost function to face more complex noise distributions, the generalized general robust loss function is defined as:

where $\gamma$ is a shape modification parameter, which is further extended to make the loss function more effective in achieving better performance in more complex noise environments. When $\gamma = 2$, the loss function degenerates to an ordinary general robust loss function. And our proposed cost function also has the scalability of a general robust loss function, which takes the following form for different values of $\alpha$ for the generalized general robust loss function.

To further illustrate the advantages of our proposed cost function over general robust loss functions, we compare the shapes of the cost function and its gradient for different parameter $\gamma$.As can be seen in Figure 1 and Figure 2, the shape of the function and its gradient changes when the parameter $\gamma$ takes on different values. Figure 1 and Figure 2 show that when the size of the error and the parameter $\alpha$ are determined, further adjustment of the parameter $\gamma$ will make the size of the weight on the error change further, thus achieving a more flexible weighting for the error point. Therefore, when facing more diversified noise problems, our proposed cost function will be handled more flexibly, so that the robust Kalman filtering algorithm we obtained using this cost function will cope with more complex noise environments.

Figure  1.  Generalized general robust loss function with different parameters $\gamma$.

DownLoad: Full-Size Img PowerPoint

Figure  2.  The Gradient of Generalized general robust loss function with different parameters $\gamma$.

DownLoad: Full-Size Img PowerPoint

Remark 1: Although general robust cost function can be adapted to different noise distributions by changing the kernel parameter $\alpha$, it still leads to degradation of the cost function’s performance when confronted with more complex noise distributions. Therefore, the shape factor $\gamma$ is further introduced to modify the kernel shape rather than changing alone, which better enable the cost function to cope with different distributions of noise types.

Remark 2: For different noise distributions to be processed, the generalized general robust loss function will assign different weights to the errors at different time points, and when all the parameters are determined, the size of the weights is determined by the error size. However, the rule of weighting only according to the error size is often difficult to perform optimally when facing various types of noise distributions with different distributions in the filter. Therefore, when confronted with different noise distributions, by continuously adjusting the shape parameters to find the optimal weights to assign to the error points, the performance of the filter will be effectively improved.

III.   Adaptive Generalized Robust Iterative Cubature Kalman filter

In this subsection, a new Adaptive Generalized Robust Iterative Cubature Kalman filter (AGRICK) is given. And the shape parameters of the Generalized general robust loss function are determined.

1   Derivation of AGRICKF

Consider the following nonlinear system state model

where ${\bf{v}}_t$ and ${\bf{z}}_t$ represent n-dimensional and m-dimensional state vectors and observation vectors, respectively. ${\bf{f}}(\cdot)$ and ${\bf{g}}(\cdot)$ represent the state transfer function and observation function, respectively. ${\bf{w}}_t-1$ and ${\bf{r}}_t$ are the state noise and observation noise with covariance matrices ${\bf{W}}_t-1$ and ${\bf{R}}_t$, respectively, that have zero mean and are uncorrelated. Considering the CKF algorithm has a higher desired accuracy as well as a moderate computational cost [8]. Therefore, the CKF is used to compute the priori state estimates $\hat{{\bf{v}}}_{t|t-1}$ and the priori covariance matrix ${\bf{P}}_{t|t-1}$. Letting ${\bf{P}}_{t-1} = \Sigma_{t-1} \Sigma_{t-1}^T$.

Calculating Sampling Points:

where

where $a(k)$ denotes the vector whose k-th element is 1 and other elements are 0. Next, calculate the cubature points after the transfer

Calculate the predicted states and the a priori covariance matrix

Then an augmentation model is constructed to fuse the prediction error with the measurement error. The model is as follows [10], [11]

where

Easily we can get

where ${\bf{U}}_t$, ${\bf{U}}^p_{t|t-1}$ and ${\bf{U}}_t^r$ are obtained by Cholesky decomposition of $E\left[{\bf{Q}}_t {\bf{Q}}_t^\top\right]$, ${\bf{P}}_{t|t-1}$ and ${\bf{R}}_t$, respectively. By left-multiplying (11) by ${\bf{U}}_t^{-1}$, we get

where

It suggests that $E\left[{\bf{e}}_t {\bf{e}}_t^\top\right] = {\bf{I}}_{n+m}.$ According to (1), the cost function of GRKF is

where $e_t(i)$ represents the i-th element of $e_t$. Then the optimal value of the state is obtained by minimizing (18).

To find the gradient of the cost function, we have

Then consider the following diagonal matrix

where

where $\psi(e_{t,n+1}) = \frac{|e_{t,n+1}(i)|^{\gamma-2}}{|\beta|^\gamma} \left( \frac{|e_{t,n+1}(i)/\beta|^\gamma}{|\alpha - \gamma|} + 1 \right)^{\alpha/\gamma - 1}$, and $diag(\cdot)$ represents the transformation of a vector into a diagonal matrix. Letting ${\bf{S}}_t = \frac{\partial {\bf{e}}_t}{\partial {\bf{v}}_t}.$, (20) can be written as

Making the gradient equal to 0, we have

where i represents iteration index. According to the matrix inversion lemma [15], [16], one can obtain

where

The posterior covariance matrix is then updated by

Remark 3: The generalized general robust loss function used in our proposed AGRICKF algorithm cope with more complex noise distributions, and the algorithm degenerates to Adaptive Robust Iterative Cubature Kalman filter (ARICKF) when the shape parameter $\gamma=2$.

Remark 4: The AGRICKF algorithm proposed in this paper processes the noise at each moment when confronted with measurements containing Gaussian noise as well as non-Gaussian noise. Although the filter may not have fully processed the error at the previous moment, resulting in the state estimation results still not accurate enough. However, the construction of the error generalization model in (11) helps the filter to process the state error in real time as well, and although there is a certain amount of accumulated error, the accumulated error will continue to be processed at the next moment, which will help the filter’s state estimation error to become smaller and smaller, thus achieving the convergence of the error behavior.

2   Optimization of the shape Parameters in AGRICKF

Since the parameter $\alpha$ and $\gamma$ determines the shape of the kernel of the general robust loss function. In this subsection, we discuss the optimal values of the parameter $\alpha$ and $\gamma$. Consider $\alpha$ and $\gamma$ as additional location parameters to minimize the generalized loss.

A value of $\alpha$ and $\gamma$ can be obtained using (32), but this value of $\alpha$ essentially minimizes all errors without affecting ${\bf{v}}$, i.e., treating all data as outliers. To avoid this, a generalized loss function [24] is constructed by

where

where $\theta$ is the truncation limit of the approximate integral. The optimal value of the parameter $\alpha$ can then be obtained by minimizing the negative logarithm of the generalized loss function.

Remark 5: During the experiment, the algorithm does not work when the parameter $\alpha$ is larger than 1, and the effect on the performance of the algorithm is not obvious when the parameter $\alpha$ is smaller than -10. And due to the gradient calculation is very complicated, the optimal values of the algorithm parameters $\alpha$ and $\gamma$ are determined by using a grid search, the optimization range are set to [-10, 1] with a resolution of 0.1 and [1.5, 2.5] with a resolution of 0.01, respectively.

IV.   Performance Analysis

In this section, we analyze the error behavior of AGRICKF and give the convergence analysis of AGRICK’s mean error behavior and mean square error behavior, respectively.

First define the error of the posteriori state estimation

According to (4), (5), (26), (36) can be written as

Since ${\bf{w}}_t$ and ${\bf{r}}_t$ are both noisy with mean 0. Therefore, the mathematical expectation of the $\sigma_t$ is

Since the state transfer function ${\bf{f}}(\cdot)$ and the measurement function ${\bf{g}}(\cdot)$ in the state-space equations considered in this paper are both time-invariant, both ${\bf{F}}$ and ${\bf{B}}_t$ are stable. According to (20)-(30), and since

which is a positive definite matrix, the Kalman gain ${\bf{G}}_t$ is stable.

Further considering the mean square error behavior of AGRICKF, the error covariance matrix of the algorithm is first given

where

Letting

(39) can be rewritten as

Similarly, since the noise covariance matrices ${\bf{W}}_t$ and ${\bf{R}}_t$ of the Kalman filtering algorithm are set invariant in advance, and the algorithm in this paper does not take into account the case where ${\bf{W}}_t$ and ${\bf{R}}_t$ are unknown. Thus $\hbar_t$ and $\ell_t$ are time-invariant, so the error covariance matrix $\lambda_t$ of the algorithm converges. We give the limiting case of (44)

Letting $\lim_{t \to \infty} \lambda_t=\lambda$, $\lim_{t \to \infty} \hbar_{t}=\hbar$, $\lim_{t \to \infty} \ell_t= \ell$. We simplify (45) to a real discrete time Lyapunov equation

with

where $\otimes$ represents the Kronecker product.

At this point, we can conclude that the mean square error behaviour of the proposed algorithm AGRICKF converges and the proposed algorithm is stable.

V.   Numerical Results

In this section, the performance of the proposed AGRICKF algorithm is validated. The robustness of AGRICKF is demonstrated through comparison with CKF [7], ICKF, MCCKF [15], MEECKF [16], HuberICKF [10], CauchyICKF [10] and ARICKF. The iterative convergence threshold was set to. The kernel width those are set in Table I. To enhance the statistical characterization of the experimental outcomes, we executed M=100 independent Monte Carlo experiments. RMSE is used as metrics for performance comparison. RMSE is defined as

where ${\bf{v}}_t^k$ and $\hat{{\bf{v}}}_t^k$ represents the true state and estimated state value at t-th moment and k-th Monte Carlo experiments.

Considering the following nonlinear Land Vehicle Navigation system [16]

where ${\bf{v}}_t = \begin{bmatrix} v_{t}(1) & v_{t}(2) & v_{t}(3) & v_{t}(4) \end{bmatrix}^\top$ is state vector consisting of the north position, the east position, the north velocity and the east velocity. $T=0.1$ is the sampling interval. The real state value, the initial state value, and the initial covariance matrix are set as ${\bf{v}}_0 = \begin{bmatrix} 0 & 10 & 5 & 10 \end{bmatrix}^\top$, ${\bf{v}}_{1|0} = \begin{bmatrix} 1 & 1 & 1 & 1 \end{bmatrix}^\top$, ${\bf{P}}_{1|0}={\bf{eye}}(n)$.

Table  2.  KERNEL WIDTH OF DIFFERENT ALGORITHM

	Kernel width	Threshold $\varepsilon$
MCCKF	25	$10^{-6}$
MEECKF	2	$10^{-6}$
HuberICKF	1.345	$10^{-6}$
CauchyICKF	1.345	$10^{-6}$

 | Show Table

DownLoad: CSV

Case A Impact of parameters

We begin by evaluating the AGRICKF parameters in the scenario of Gaussian noise with random outliers, and assess the RMSEs with varying parameters. As the observer of the navigation system receives various types of non-Gaussian noise, such as anomalous data, loss of observation data, etc. We use a mixture of two Gaussian noises to simulate the possible non-Gaussian noise problem in this section. The Gaussian distribution with large variance is used to simulate the possible outlier data, and the Gaussian distributed noise with a smaller variance is used to model the interference of the measurements by various types of environmental noise during transmission. The noise of Gaussian noise with random outliers is given by

where $N(a,b)$ represents Gaussian noise with a mean of a and a variance of b. The process noise is setting to ${\bf{w}}_t=N(0,0.01)$. In Figure 3 and Figure 4, the RMSE for different values of $\beta$ is compared. It can be concluded that the value of $\beta$ has a large impact on the accuracy of the algorithm, and when obtains a suitable value, it can make our algorithm have better robustness. In subsequent experiments, we set $\beta=1.0$.

Figure  3.  RMSE of velocity in different value of $\beta$.

DownLoad: Full-Size Img PowerPoint

Figure  4.  RMSE of position in different value of $\beta$.

DownLoad: Full-Size Img PowerPoint

In addition, the performance of the algorithm is compared for different values of the parameters $\alpha$ and $\gamma$ in order to validate our proposed method of adaptive parameter selection. $\text{ARMSE}=\frac{1}{N}\sum_{t=1}^{N} \text{RMSE}(t)$ is used as the evaluation index, where N is total time steps, and the comparison results are shown in Table II. It can be seen that the use of adaptive parameter selection helps the filter to achieve more accurate estimation results, which is due to the fact that the optimal parameters in this noisy environment are found by this method, effectively eliminating the influence of outliers.

Table  3.  ARMSE OF DIFFERENT VALUE OF $\alpha$ AND $\gamma$

$(\alpha,\gamma)$	ARMSE of position	ARMSE of velocity
adaptive parameter	0.4573	0.5810
(0,2.0)	0.5165	0.6222
(0,2.1)	0.6582	1.0205
(0,2.15)	1.0672	1.6147
(0,2.2)	1.6843	3.0534
(-2,2.1)	0.6537	0.9982
(1,2.1)	0.6877	1.1093
(2,2.1)	1.5721	2.6074

 | Show Table

DownLoad: CSV

Case B Algorithm Robustness Comparison

In this subsection, the enhanced robustness of our proposed AGRICKF algorithm is demonstrated through comparison with other robust algorithms. The process noise setting is still Gaussian noise, the same as Case A. A Gaussian mixture model is also used to model possible non-Gaussian noise problems. However, in this section the noise of the Laplace distribution is used to model the appearance of outliers. The algorithm is tested separately using the following two noisy environments.

where $L(0,1000)$ represents Laplace noise with a mean of 0 and a variance of 1000. Figure 5-10 show the RMSE in these three environments respectively. It is clearly concluded that our proposed AGRICKF algorithm exhibits superior robustness and maintains strong performance in a variety of complex noise environments. Thus, in the same way as the theoretical analysis, the AGRICKF shows a strong performance when dealing with complex non-Gaussian noise problems.

Figure  5.  RMSE of velocity of different algorithm in noise ${\bf{r}}_{1,t}$.

DownLoad: Full-Size Img PowerPoint

Figure  6.  RMSE of position of different algorithm in noise ${\bf{r}}_{1,t}$.

DownLoad: Full-Size Img PowerPoint

Figure  7.  RMSE of velocity of different algorithm in noise ${\bf{r}}_{2,t}$.

DownLoad: Full-Size Img PowerPoint

Figure  8.  RMSE of position of different algorithm in noise ${\bf{r}}_{2,t}$.

DownLoad: Full-Size Img PowerPoint

Figure  9.  RMSE of velocity of different algorithm in noise ${\bf{r}}_{3,t}$.

DownLoad: Full-Size Img PowerPoint

Figure  10.  RMSE of position of different algorithm in noise ${\bf{r}}_{3,t}$.

DownLoad: Full-Size Img PowerPoint

VI.   Conclusion

In this paper, an iterative Cubature Kalman filter based on a generalize general robust loss function (AGRICKF) is developed. As the generalize general robust loss function used can be converted into multiple M-estimation functions, the AGRICKF algorithm can handle various complex non-Gaussian noise environments. And the algorithm constructs a nonlinear augmented model ensemble of prediction errors and measurement errors to improve the accuracy of the algorithm in the face of nonlinear state estimation problems. The experimental results confirm the impact of the algorithm parameters and its enhanced robustness compared to other robust algorithms.