2D and 3D Angles-Only Target Tracking Based on Maximum Correntropy Kalman Filters

Abstract

In this paper, angles-only target tracking (AoT) problem is investigated in the non Gaussian environment. Since the conventional minimum mean square error criterion based estimators tend to give poor accuracy in the presence of large outliers or impulsive noises in measurement, a maximum correntropy criterion (MCC) based framework is presented. Accordingly, three new estimation algorithms are developed for AoT problems based on the conventional sigma point filters, termed as MC-UKF-CK, MC-NSKF-GK and MC-NSKF-CK. Here MC-NSKF-GK represents the maximum correntropy new sigma point Kalman filter realized using Gaussian kernel and MC-NSKF-CK represents realization using Cauchy kernel. Similarly, based on the unscented Kalman filter, MC-UKF-CK has been developed. The performance of all these estimators is evaluated in terms of root-mean-square error (RMSE) in position and % track loss. The simulations were carried out for 2D as well as 3D AoT scenarios and it was inferred that, the developed algorithms performed with improved estimation accuracy than the conventional ones, in the presence of non Gaussian measurement noise.

1. Introduction

In state estimation, Kalman filter (KF) is a recursive solution used in various applications, such as information fusion, system control, integrated navigation, target tracking, and GPS solutions [1,2,3,4]. Kalman filter gives optimal estimates provided the dynamical system is linear and the noises assumed are Gaussian. However, it is extended to nonlinear systems through suitable approximation of the nonlinear functions. Using the Taylor series to linearize the nonlinear functions, the popular extended Kalman filter (EKF) [5] was derived. Also various sigma point filters have been proposed in the literature such as unscented Kalman filter (UKF) [6], cubature Kalman filter (CKF) [7], new sigma point Kalman filter (NSKF) [8], to obtain improved estimation accuracy than the EKF.

Since these filters are based on minimum mean square error criterion, their performance is likely to get deteriorated in the presence of non Gaussian noises such as heavy tailed and impulsive noises [9]. This makes state estimation a very challenging problem in the presence of nonlinear models and non Gaussian noise. Other possible solutions that can provide robust state estimates are Gaussian sum filter (GSF) [10,11], particle filter (PF) [12], Huber’s KF (HKF, also known as M-estimation) [13], $H_{\infty }$ filter [14] etc.

In order to improve the robustness of nonlinear state estimators in the presence of non Gaussian noise, a local similarity measure called correntropy [15,16], based filter called correntropy filter (C-Filter) was first proposed in [17]. Since it was developed by replacing the minimum mean square error (MMSE) criterion with maximum correntropy criterion (MCC), it proved beneficial for non Gaussian systems, but only for linear systems [18]. This algorithm made use of least squares and fixed point iteration, but failed to incorporate covariance estimation. In order to avoid this, a maximum correntropy Kalman filter (MCKF) involving fixed point iteration and covariance propagation was proposed in [19]. Similar issue was also addressed in [20], which used a cost function consisting of weighted least square (WLS) and Gaussian kernel function, and hence was named as maximum correntropy criterion-Kalman filter (MCC-KF).

To deal with nonlinear systems, extensions to the existing conventional algorithms based on MCC criterion were also developed and were named as maximum correntropy EKF (MC-EKF) [21], maximum correntropy UKF (MC-UKF) [22] and maximum correntropy sparse grid Gauss-Hermite quadrature filter (MC-SGHQF) [23]. But in the presence of large outliers in measurements, these filters incurred analytical problems in calculating inverse of matrices. Thus, new algorithms involving new cost function, WLS and statistical linearisation were proposed in [24], which were called as MC-UKF-constant and MC-UKF-adaptive [25]. In developing the above mentioned estimators, Gaussian kernel played an important role in suppressing the non Gaussian measurement noise. In target tracking applications, we may receive measurements which have larger outliers. This could prove to be a challenging task in successful estimation of states using Gaussian kernel as it may be difficult to select a proper kernel bandwidth. Hence, Gaussian kernel may not always prove to be the best choice for a kernel function. To overcome this drawback, a Cauchy kernel function is constructed which gives reasonable estimation accuracy for a wide range of kernel bandwidth [26,27].

This paper deals with angles only target tracking problem in 2D and 3D. The literature presents with many variations of this tracking problem such as when the target is a curvilinear manoeuvring target [28,29]. However, as is common in passive sonar target tracking applications, the objective here is to estimate the states of a moving constant velocity target with the help of angles-only measurements, but corrupted with non Gaussian noise. The observer continuously monitors for the signals, that are generated due to the sound radiated by the target. The AoT can also be performed with other measurement sources like IRST sensor [4], radar [30] and also through video tracking [31]. Any irregularities in these signals received by the observer can be considered as glint noise. A mixture model of two zero-mean Gaussian for glint noise has been proposed in [32]. This consists of one Gaussian density with high probability and small variance while the other has small probability of occurrence and large variance. Alternatively, it is also modelled in [33] as a mixture of zero mean with small variance. In this work, the non Gaussian noise in angular measurements is modelled as a mixture of Gaussian densities plus shot noise.

The main contribution of this paper is the development of three new nonlinear filters for AoT problem, MC-UKF-CK, MC-NSKF-GK and MC-NSKF-CK, and their performance evaluation in the context of angles-only tracking. Accordingly, conventional filters UKF and NSKF have been reformulated based on maximum correntropy criterion. MC-UKF and MC-NSKF based on Gaussian kernel (MC-UKF-GK, MC-NSKF-GK) and Cauchy kernel (MC-UKF-CK, MC-NSKF-CK) functions have been derived. The performance evaluation of these estimators are conducted considering RMSE in position and track loss as the two performance metrics and a comparative discussion is presented. The simulation results highlight that the existing solutions behave poorly in comparison to the proposed algorithms.

The rest of the paper is organised as follows. Section 2 describes the problem formulation for AoT in 2D as well as 3D. Section 3 illustrates the correntropy, its properties for two random variables and MCC. In Section 4, the already existing Gaussian kernel based MC state estimation framework is revisited. In Section 5, the Cauchy kernel based MC state estimation framework for nonlinear systems is derived. Section 6 briefly discuss about the state estimators on which the developed MCC framework is incorporated. Section 7 describes the realization of non Gaussian noise, followed by simulation study in Section 8. Finally, the concluding remarks are given in Section 9.

2. Problem Formulation

The aim of the angles only tracking problem is to track the target trajectory using the noise corrupted angular measurements. The dynamics of the target is assumed to be a constant velocity motion. The observer motion is deterministic, implying that the position and velocity of the observer is known to us. The 2D and 3D target observer dynamics is illustrated below.

2.1. Process Model

The target and observer state vector with position and velocity as its states is given as

$$\begin{array}{cccc}\hfill {\mathbf{X}}_k^{\mathbf{t}}& ={\left[\begin{array}{cccc}{\mathtt{x}}_k^{\mathtt{t}}& {\mathtt{y}}_k^{\mathtt{t}}& {\dot{\mathtt{x}}}_k^{\mathtt{t}}& {\dot{\mathtt{y}}}_k^{\mathtt{t}}\end{array}\right]}^{\prime }\hfill & \hfill {\mathbf{X}}_k^{\mathbf{o}}& ={\left[\begin{array}{cccc}{\mathtt{x}}_k^{\mathtt{o}}& {\mathtt{y}}_k^{\mathtt{o}}& {\dot{\mathtt{x}}}_k^{\mathtt{o}}& {\dot{\mathtt{y}}}_k^{\mathtt{o}}\end{array}\right]}^{\prime }.\hfill \end{array}$$

The discrete time linear process model representing the target motion is given as

$${\mathbf{X}}_k^{\mathbf{t}}=\mathbf{F}{\mathbf{X}}_{k-1}^{\mathbf{t}}+{\mathtt{w}}_{k-1}.$$

(1)

Now, the relative state vector dynamics is

$${\mathbf{X}}_k=\mathbf{F}{\mathbf{X}}_{k-1}+{\mathtt{w}}_{k-1}-{\mathbf{X}}_{k-1}^{\mathbf{o}}+\mathbf{F}{\mathbf{X}}_{k-1}^{\mathbf{o}}.$$

(2)

where ${\mathbf{X}}_k$, the relative vector is defined as

$$\begin{array}{cc}\hfill {\mathbf{X}}_k& ={\mathbf{X}}_k^{\mathbf{t}}-{\mathbf{X}}_k^{\mathbf{o}}\hfill \\ \hfill & ={\left[\begin{array}{cccc}{\mathtt{x}}_k^{\mathtt{t}}-{\mathtt{x}}_k^{\mathtt{o}}& {\mathtt{y}}_k^{\mathtt{t}}-{\mathtt{y}}_k^{\mathtt{o}}& {\dot{\mathtt{x}}}_k^{\mathtt{t}}-{\dot{\mathtt{x}}}_k^{\mathtt{o}}& {\dot{\mathtt{y}}}_k^{\mathtt{t}}-{\dot{\mathtt{y}}}_k^{\mathtt{o}}\end{array}\right]}^{\prime }\hfill \\ \hfill & ={\left[\begin{array}{cccc}{\mathtt{x}}_k& {\mathtt{y}}_k& \dot{{\mathtt{x}}_k}& \dot{{\mathtt{y}}_k}\end{array}\right]}^{\prime }.\hfill \end{array}$$

(3)

$\mathbf{F}$ is the state transition matrix and ${\mathtt{w}}_{k-1}$ is zero mean Gaussian process noise with $\mathbf{Q}$ as the covariance matrix. For problem formulation in the two dimensional space (let n = 2), $\mathbf{F}$, $\mathbf{Q}$ matrices are defined as,

$$\begin{array}{cccc}\hfill \mathbf{F}& =\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\hfill & \hfill \mathrm{and}\mathbf{Q}& =\left[\begin{array}{cccc}\frac{T^3}{3}{\mathtt{q}}_x& 0& \frac{T^2}{2}{\mathtt{q}}_x& 0\\ 0& \frac{T^3}{3}{\mathtt{q}}_y& 0& \frac{T^2}{2}{\mathtt{q}}_y\\ \frac{T^2}{2}{\mathtt{q}}_x& 0& T{\mathtt{q}}_x& 0\\ 0& \frac{T^2}{2}{\mathtt{q}}_y& 0& T{\mathtt{q}}_y\end{array}\right].\hfill \end{array}$$

The target observer dynamics in 2D for a moderately nonlinear scenario, is shown in Figure 1. Similarly for n = 3, the state and the associated matrices are

$$\begin{array}{cccc}\hfill {{\mathbf{X}}^{\mathbf{t}}}_k& ={\left[\begin{array}{cccccc}{\mathtt{x}}_k^{\mathtt{t}}& {\mathtt{y}}_k^{\mathtt{t}}& {\mathtt{z}}_k^{\mathtt{t}}& {\dot{\mathtt{x}}}_k^{\mathtt{t}}& {\dot{\mathtt{y}}}_k^{\mathtt{t}}& {\dot{\mathtt{z}}}_k^{\mathtt{t}}\end{array}\right]}^{\prime }\hfill & \hfill {\mathbf{X}}^{\mathbf{o}}& ={\left[\begin{array}{cccccc}{\mathtt{x}}_k^{\mathtt{o}}& {\mathtt{y}}_k^{\mathtt{o}}& {\mathtt{z}}_k^{\mathtt{o}}& {\dot{\mathtt{x}}}_k^{\mathtt{o}}& {\dot{\mathtt{y}}}_k^{\mathtt{o}}& {\dot{\mathtt{z}}}_k^{\mathtt{o}}\end{array}\right]}^{\prime }\hfill \end{array}$$

$$\begin{array}{cccc}\hfill {\mathbf{F}}_{k-1}& =\left[\begin{array}{cccccc}1& 0& 0& T& 0& 0\\ 0& 1& 0& 0& T& 0\\ 0& 0& 1& 0& 0& T\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],\hfill & \hfill {\mathbf{Q}}_{k-1}& =\left[\begin{array}{cccccc}\frac{T^3}{3}{\mathtt{q}}_x& 0& 0& \frac{T^2}{2}{\mathtt{q}}_x& 0& 0\\ 0& \frac{T^3}{3}{\mathtt{q}}_y& 0& 0& \frac{T^2}{2}{\mathtt{q}}_y& 0\\ 0& 0& \frac{T^3}{3}{\mathtt{q}}_z& 0& 0& \frac{T^2}{2}{\mathtt{q}}_z\\ \frac{T^2}{2}{\mathtt{q}}_x& 0& 0& T{\mathtt{q}}_x& 0& 0\\ 0& \frac{T^2}{2}{\mathtt{q}}_y& 0& 0& T{\mathtt{q}}_y& 0\\ 0& 0& \frac{T^2}{2}{\mathtt{q}}_z& 0& 0& T{\mathtt{q}}_z\end{array}\right],\hfill \end{array}$$

where T is the sampling time interval and ${\mathtt{q}}_x,{\mathtt{q}}_y,{\mathtt{q}}_z$ are the power spectral densities of the process noise along the X, Y, and Z axes respectively.

The 3D target observer trajectory referred in the problem is given by Figure 2.

2.2. Measurement Model

2D AoT problem: The only available measurements are the bearing angles through which the states of the relative state vector can be estimated. The measurement model and the true angle measurements for the problem can be represented as

$$\begin{array}{cccc}\hfill z_k& =h\left({\mathbf{X}}_k\right)+{\mathtt{v}}_k\hfill & \hfill h\left({\mathbf{X}}_k\right)={\beta }_k& ={tan}^{-1}({\mathtt{x}}_k,{\mathtt{y}}_k).\hfill \end{array}$$

(4)

where ${\mathtt{v}}_k$ shall be modelled as the non Gaussian noise.

3D AoT problem:Figure 3 represents the target observer dynamics in Cartesian coordinate.

The range vector $\mathbf{r}$ is defined as

$$\begin{array}{cc}\hfill \mathbf{r}& ={\left[\begin{array}{ccc}{\mathtt{x}}_k^{\mathtt{t}}-{\mathtt{x}}_k^{\mathtt{o}}& {\mathtt{y}}_k^{\mathtt{t}}-{\mathtt{y}}_k^{\mathtt{o}}& {\mathtt{z}}_k^{\mathtt{t}}-{\mathtt{z}}_k^{\mathtt{o}}\end{array}\right]}^{\prime }\hfill \\ \hfill & ={\left[\begin{array}{ccc}{\mathtt{x}}_k& {\mathtt{y}}_k& {\mathtt{z}}_k\end{array}\right]}^{\prime }.\hfill \end{array}$$

From Figure 3, $\mathbf{r}$ can be expressed in terms of bearing ($\beta$) and elevation ($ϵ$) as

$$\mathbf{r}={[rcosϵsin\beta rcosϵcos\beta rsinϵ]}^{\prime },$$

and the actual range is $r_k=\sqrt{{\mathtt{x}}_k^2+{\mathtt{y}}_k^2+{\mathtt{z}}_k^2}$. Here, the nonlinear noise corrupted measurements are bearing ($\beta$) and elevation $\left(ϵ\right)$ angles respectively, where $\beta \in [0,2\pi ]$ and $ϵ\in [-\frac{\pi }{2},\frac{\pi }{2}]$. The measurement model involving the bearing and elevation angle is

$$\begin{array}{c}\hfill {\mathbf{z}}_k=\mathbf{h}\left({\mathbf{X}}_k\right)+{\mathbf{v}}_k,\end{array}$$

(5)

where,

$$\begin{array}{c}\hfill \mathbf{h}\left({\mathbf{X}}_k\right)=\left[\begin{array}{c}{\beta }_k\\ {ϵ}_k\end{array}\right]=\left[\begin{array}{c}{tan}^{-1}({\mathtt{x}}_k,{\mathtt{y}}_k)\\ {tan}^{-1}\left(\frac{{\mathtt{z}}_k}{\sqrt{{{\mathtt{x}}_k}^2+{{\mathtt{y}}_k}^2}}\right)\end{array}\right].\end{array}$$

Here, ${\mathbf{v}}_k$ is to be modelled as the non Gaussian noise.

3. Correntropy Measure

Correntropy is directly related to the probability of how similar two random variables are in the joint space controlled by the kernel bandwidth. The kernel bandwidth controls the window in which the similarity has to be assessed, and hence provides a way to eliminate the detrimental effect of outliers [16]. If X and Y are random variables, correntropy is defined as

$$V_{\sigma }(X,Y)=E\left[k_{\sigma }(X,Y)\right]=\int \int k_{\sigma }(x,y)p_{XY}(x,y)dxdy,$$

where $k_{\sigma }$ denotes a positive definite kernel function, $p_{XY}(.)$ denotes the joint PDF of X and Y and E is the expectation operator. Since the joint density is not accessible and if only a finite number of data points N are available, a sample estimator can be defined as

$${\widehat{V}}_{\sigma }(X,Y)=\frac{1}{N}\sum _{i=1}^N{G}_{\sigma }(x_i-y_i).$$

Here $G_{\sigma }(·)$ is the Gaussian kernel, defined as

$$G_{\sigma }(x_i-y_i)=exp\left(-\frac{\parallel x_i-y_i{\parallel }^2}{2{\sigma }^2}\right),$$

(6)

which is bounded, positive and reaches its maximum only when $X=Y$, leading to the maximum correntropy criterion (MCC). By taking the Taylor series expansion of the Gaussian kernel, correntropy can also be expressed as a weighted sum of all even order moments of $(x_i-y_i)$, i.e.,

$$V_{\sigma }(X,Y)=\sum _{k=0}^{\infty }\frac{{(-1)}^k}{2^k{\sigma }^{2k}k!}E\left[{(X-Y)}^{2k}\right].$$

On the other hand, Cauchy kernel based non-linear state estimators can be developed using Cauchy kernel instead of Gaussian kernel function. It is defined as [34]

$$C_{\delta }(x_i-y_i)=\frac{1}{1+{\displaystyle \frac{\parallel x_i-y_i{\parallel }^2}{\delta }}}.$$

Here $\delta$ is a positive scalar, representing the Cauchy kernel bandwidth. Similar to the Gaussian kernel, it can be shown that the Cauchy kernel also incorporates the higher order moments, given as

$$V_{\delta }(X,Y)=\sum _{k=0}^{\infty }\frac{{(-1)}^k}{{\delta }^k}\left(\genfrac{}{}{0pt}{}{N+k-1}{k}\right)E\left[{(X-Y)}^{2k}\right].$$

A detailed derivation of the above equation is given in Appendix A.

4. Gaussian Kernel Based Maximum Correntropy Estimation Framework

Let us consider the process model described in Equations (1) and (5). To accommodate for the large outliers in measurements, the noise ${\mathbf{v}}_k$ is considered non-Gaussian. Hence for MC based estimation framework [24], the Gaussian assumption of ${\mathbf{v}}_k$ is relaxed.

In order to deal with the non Gaussian noises in the measurement update step, a statistical linearisation approach is employed. Consider that the nonlinear function $\mathbf{h}(·)$, operating on vector random variables ${\mathbf{X}}_k$ is evaluated at $\mathsf{N}$-points ${\chi }_k,k=1,\cdots ,\mathsf{N}$, with ${\mathbf{z}}_k=\mathbf{h}\left({\chi }_k\right)+{\mathbf{v}}_k$. Suppose that the weighted mean of ${\chi }_k$ is given by ${\widehat{\mathbf{X}}}_{k|k-1}={\sum }_{k=1}^{\mathsf{N}}{W}_k{\chi }_k$, with ${\sum }_{k=1}^{\mathsf{N}}{W}_k=1$. Similarly, ${\widehat{\mathbf{z}}}_{k|k-1}={\sum }_{k=1}^{\mathsf{N}}{W}_k{\mathbf{z}}_k$. Then the prior and cross covariance ${\mathbf{P}}_{k|k-1}$ and ${\mathbf{P}}_{\mathbf{X}\mathbf{z}}$ are given as

$$\begin{array}{cc}\hfill {\mathbf{P}}_{k|k-1}& =\sum _{k=1}^{\mathsf{N}}{W}_k\left[({\chi }_k-{\widehat{\mathbf{X}}}_{k|k-1}){({\chi }_k-{\widehat{\mathbf{X}}}_{k|k-1})}^{\prime }\right]and\hfill \\ \hfill {\mathbf{P}}_{\mathbf{X}\mathbf{z}}& =\sum _{k=1}^{\mathsf{N}}{W}_k\left[({\chi }_k-{\widehat{\mathbf{X}}}_{k|k-1}){({\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k|k-1})}^{\prime }\right].\hfill \end{array}$$

The nonlinear measurement function is represented in terms of measurement slope matrix ${\overline{\mathbf{H}}}_k$, and a constant term ${\overline{c}}_k$ as $\mathbf{h}\left(X_k\right)\approx {\overline{\mathbf{H}}}_k{\mathbf{X}}_k+{\overline{c}}_k$. Here ${\overline{\mathbf{H}}}_k$ and ${\overline{c}}_k$ are computed by minimizing the weighted least squares,

$$\underset{{\overline{H}}_k,{\overline{c}}_k}{\arg \min }{W}_k{\parallel {\overline{\mathbf{v}}}_k\parallel }^2,where{\overline{\mathbf{v}}}_k={\mathbf{z}}_k-{\overline{\mathbf{H}}}_k{\mathbf{X}}_k-{\overline{c}}_k.$$

Then the solutions are ${\overline{\mathbf{H}}}_k={\left({\mathbf{P}}_{k|k-1}^{-1}{\mathbf{P}}_{\mathbf{X}\mathbf{z}}\right)}^{\prime }and{\overline{c}}_k={\widehat{\mathbf{z}}}_{k|k-1}-{\overline{\mathbf{H}}}_k{\widehat{\mathbf{X}}}_{k|k-1}$. As the mean of ${\overline{\mathbf{v}}}_k$ is zero, that is $E\left[{\overline{\mathbf{v}}}_k\right]={\widehat{\mathbf{z}}}_{k|k-1}-{\overline{\mathbf{H}}}_k{\widehat{\mathbf{X}}}_{k|k-1}-{\overline{c}}_k=0$, the covariance matrix ${\mathbf{R}}_k$ can be calculated as

$$\begin{array}{cc}\hfill {\mathbf{R}}_k=& \sum _{k=1}^{\mathsf{N}}{W}_k\left[{\overline{\mathbf{v}}}_k{\overline{\mathbf{v}}}_k^{\prime }\right]\hfill \\ \hfill =& \sum _{k=1}^{\mathsf{N}}{W}_k\left[({\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k|k-1})-{\overline{\mathbf{H}}}_k({\mathbf{X}}_k-{\widehat{\mathbf{X}}}_{k|k-1})\right]{\left[({\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k|k-1})-{\overline{\mathbf{H}}}_k({\mathbf{X}}_k-{\widehat{\mathbf{X}}}_{k|k-1})\right]}^{\prime }\hfill \\ \hfill =& {\mathbf{P}}_{\mathbf{z}\mathbf{z}}-{\overline{\mathbf{H}}}_k{\mathbf{P}}_{\mathbf{X}\mathbf{z}}-{\mathbf{P}}_{\mathbf{X}\mathbf{z}}^{\prime }{\overline{\mathbf{H}}}_k^{\prime }+{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}{\overline{\mathbf{H}}}_k^{\prime }\hfill \\ \hfill =& {\mathbf{P}}_{\mathbf{z}\mathbf{z}}-{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}{\overline{\mathbf{H}}}_k^{\prime }.\hfill \end{array}$$

(7)

Thus, the linearised measurement equation is given as

$${\mathbf{z}}_k={\widehat{\mathbf{z}}}_{k|k-1}+{\overline{\mathbf{H}}}_k\left({\mathbf{X}}_k-{\widehat{\mathbf{X}}}_{k|k-1}\right)+{\overline{\mathbf{v}}}_k\mathrm{with}{\overline{\mathbf{v}}}_k\sim \mathcal{N}(0,{\mathbf{R}}_k).$$

(8)

Accordingly, a cost function is formulated with the help of weighted least squares (WLS) to handle Gaussian process noise. To handle non-Gaussian measurement noise, statistical linearisation approach was used to define WLS function which in turn is used in MCC. Hence the cost function can be defined as

$$\mathbf{J}=\wp {\parallel {\mathbf{X}}_k-{\widehat{\mathbf{X}}}_{k|k-1}\parallel }_{{\mathbf{P}}_{k|k-1}^{-1}}^2-\varrho exp\left(-\frac{{\aleph }^{\prime }{\mathbf{R}}^{-1}\aleph }{2{\sigma }^2}\right),$$

where $\aleph ={\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k|k-1}-{\overline{\mathbf{H}}}_k({\mathbf{X}}_k-{\widehat{\mathbf{X}}}_{k|k-1})$, ℘ and $\varrho$ are adjustable weights. In order to find the optimal estimate of ${\mathbf{X}}_k$, the cost function has to be minimized i.e.,

$${\widehat{\mathbf{X}}}_k=\underset{{\mathbf{X}}_k}{\arg \min }\mathbf{J},$$

and the solution can be obtained as $\frac{\partial \mathbf{J}}{\partial {\mathbf{X}}_k}=0$. This implies that

$$\begin{array}{cc}\hfill \frac{\partial \mathbf{J}}{\partial {\mathbf{X}}_k}& =\wp {\mathbf{P}}_{k|k-1}^{-1}({\mathbf{X}}_k-{\widehat{\mathbf{X}}}_{k|k-1})+{\displaystyle \frac{\varrho }{2{\sigma }^2}}{G}_{\sigma }\left({\aleph }_{\mathbf{R}}\right){\overline{\mathbf{H}}}_k^{\prime }{\mathbf{R}}_k^{-1}\aleph \hfill \\ \hfill & =\wp {\mathbf{P}}_{k|k-1}^{-1}({\mathbf{X}}_k-{\widehat{\mathbf{X}}}_{k|k-1})+\frac{\varrho {\mathbf{L}}_k^G{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{R}}_k^{-1}}{2{\sigma }^2}\aleph =0,\hfill \end{array}$$

(9)

where

$$\begin{array}{c}\hfill {\mathbf{L}}_k^G=G_{\sigma }\left({\aleph }_{\mathbf{R}}\right)=exp\left(-\frac{{\aleph }^{\prime }{\mathbf{R}}^{-1}\aleph }{2{\sigma }^2}\right).\end{array}$$

(10)

In order to guarantee the convergence of the algorithm to a corresponding state estimator that follows a complete Gaussian assumption (when the kernel bandwidth $\sigma$ becomes infinity), the values for weights in $\mathbf{J}$ are taken as $\wp =1$ and $\varrho =-2{\sigma }^2$. Then Equation (9) becomes

$${\mathbf{P}}_{k|k-1}^{-1}({\mathbf{X}}_k-{\widehat{\mathbf{X}}}_{k|k-1})={\mathbf{L}}_k^G{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{R}}_k^{-1}\aleph .$$

Rearranging, we get

$$\left({\mathbf{P}}_{k|k-1}^{-1}+{\mathbf{L}}_k^G{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{R}}_k^{-1}{\overline{\mathbf{H}}}_k\right){\mathbf{X}}_k={\mathbf{L}}_k^G{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{R}}_k^{-1}\left({\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k|k-1}\right)+\left({\mathbf{L}}_k^G{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{R}}_k^{-1}{\overline{\mathbf{H}}}_k+{\mathbf{P}}_{k|k-1}^{-1}\right){\widehat{\mathbf{X}}}_{k|k-1}.$$

(11)

Since ${\mathbf{L}}_k^G$ is related to ${\mathbf{X}}_k$, Equation (11) represents a fixed point equation that can be solved using the fixed point iteration algorithm considering ${\mathbf{X}}_k$ equal to ${\widehat{\mathbf{X}}}_{k|k-1}$ in Equation (10). But as mentioned in [19,22,24], for a satisfactory estimation performance, a single iteration is sufficient. Hence, adopting the same approach leads to the modification of Equation (11) as

$${\widehat{\mathbf{X}}}_{k|k}={\widehat{\mathbf{X}}}_{k|k-1}+{\mathbf{K}}_k^G\left({\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k|k-1}\right),$$

where

$$\begin{array}{cc}\hfill {\mathbf{K}}_k^G& ={\left({\mathbf{P}}_{k|k-1}^{-1}+{\mathbf{L}}_k^G{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{R}}_k^{-1}{\overline{\mathbf{H}}}_k\right)}^{-1}{\mathbf{L}}_k^G{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{R}}_k^{-1}\mathrm{and}\hfill \\ \hfill {\mathbf{L}}_k^G& =exp\left(-\frac{{({\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k|k-1})}^{\prime }{\mathbf{R}}_k^{-1}({\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k|k-1})}{2{\sigma }^2}\right).\hfill \end{array}$$

A more appropriate form for ${\mathbf{K}}_k^G$, in terms of reduced computational complexity, can be derived using the matrix inversion lemma (detailed derivation is given in Appendix B) as

$${\mathbf{K}}_k^G={\mathbf{P}}_{k|k-1}{\mathbf{L}}_k^G{{\overline{\mathbf{H}}}_k}^{\prime }{\left({\mathbf{R}}_k+{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}{\mathbf{L}}_k^G{\overline{\mathbf{H}}}_k^{\prime }\right)}^{-1}.$$

(12)

Now, the corresponding posterior error covariance matrix is given as

$${\mathbf{P}}_{k|k}=\left(\mathbf{I}-{\mathbf{K}}_k^G{\overline{\mathbf{H}}}_k\right){\mathbf{P}}_{k|k-1}{\left(\mathbf{I}-{\mathbf{K}}_k^G{\overline{\mathbf{H}}}_k\right)}^{\prime }+{\mathbf{K}}_k^G{\mathbf{R}}_k{\mathbf{K}}_k^{G^{\prime }}.$$

5. Cauchy Kernel Based Maximum Correntropy Estimation Framework

In this section, we derive a maximum correntropy estimation framework using Cauchy kernel for potential improvement in estimation accuracy, in the presence of large multi dimensional non Gaussian noise. Hence the cost function becomes

$${\mathbf{J}}_C={\wp }^C{\parallel {\mathbf{X}}_k-{\widehat{\mathbf{X}}}_{k|k-1}\parallel }_{{\mathbf{P}}_{k|k-1}^{-1}}^2-{\varrho }^C{C}_{\delta }\left({\aleph }_{\mathbf{R}}\right)$$

where ${\wp }^C$ and ${\varrho }^C$ are adjustable weights, and

$$C_{\delta }\left({\aleph }_{\mathbf{R}}\right)={\displaystyle \frac{1}{1+{\displaystyle \frac{{\aleph }^{\prime }{\mathbf{R}}_k^{-1}\aleph }{\delta }}}},$$

with ℵ being the same as that mentioned in Section 4. To obtain the optimal estimate of ${\mathbf{X}}_k$, we equate ${\displaystyle \frac{\partial {\mathbf{J}}_C}{\partial {\mathbf{X}}_k}}=0$, giving ${\wp }^C{\mathbf{P}}_{k|k-1}^{-1}\left({\mathbf{X}}_k-{\widehat{\mathbf{X}}}_{k|k-1}\right)-{\displaystyle \frac{{\varrho }^C{\mathbf{L}}_k^C}{\delta }}{\overline{\mathbf{H}}}^{\prime }{\mathbf{R}}_k^{-1}\aleph =0$, where

$${\mathbf{L}}_k^C=C_{\delta }^2\left({\aleph }_{\mathbf{R}}\right)=C_{\delta }^2\left({\parallel {\mathbf{z}}_k-{\overline{\mathbf{H}}}_k{\mathbf{X}}_k-{\widehat{\mathbf{z}}}_{k|k-1}+{\overline{\mathbf{H}}}_k{\widehat{\mathbf{X}}}_{k|k-1}\parallel }_{{\mathbf{R}}_k^{-1}}\right).$$

We set ${\wp }^C=1$ and ${\varrho }^C=\delta$ so as to guarantee the convergence of the estimator when kernel bandwidth $\delta$ tends to ∞. Rearranging,

$$\left({\mathbf{P}}_{k|k-1}^{-1}+{\mathbf{L}}_k^C{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{R}}_k^{-1}{\overline{\mathbf{H}}}_k\right){\mathbf{X}}_k={\mathbf{L}}_k^C{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{R}}_k^{-1}\left({\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k|k-1}\right)+\left({\mathbf{L}}_k^C{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{R}}_k^{-1}{\overline{\mathbf{H}}}_k+{\mathbf{P}}_{k|k-1}^{-1}\right){\widehat{\mathbf{X}}}_{k|k-1}.$$

(13)

Here also, ${\mathbf{L}}_k^C$ is related to ${\mathbf{X}}_k$ and hence Equation (13) is a fixed point equation that is to be solved using fixed point iteration algorithm, considering ${\mathbf{X}}_k$ equal to ${\widehat{\mathbf{X}}}_{k|k-1}$. Using the same justification that was adopted in Gaussian kernel case that only a single iteration is required, the expression for posterior mean is obtained as

$${\widehat{\mathbf{X}}}_{k|k}={\widehat{\mathbf{X}}}_{k|k-1}+{\mathbf{K}}_k^C\left({\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k|k-1}\right),$$

where

$${\mathbf{K}}_k^C={\left({\mathbf{P}}_{k|k-1}^{-1}+{\mathbf{L}}_k^C{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{R}}_k^{-1}{\overline{\mathbf{H}}}_k\right)}^{-1}{\mathbf{L}}_k^C{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{R}}_k^{-1}\mathrm{and}{\mathbf{L}}_k^C=C_{\delta }^2\left({\parallel {\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k|k-1}\parallel }_{{\mathbf{R}}_k^{-1}}\right).$$

As per the proof given in Appendix B, Kalman gain can be modified as

$${\mathbf{K}}_k^C={\mathbf{P}}_{k|k-1}{\mathbf{L}}_k^C{\overline{\mathbf{H}}}_k^{\prime }{\left({\mathbf{R}}_k+{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}{\mathbf{L}}_k^C{\overline{\mathbf{H}}}_k^{\prime }\right)}^{-1}.$$

(14)

Then the posterior error covariance matrix shall be calculated as

$${\mathbf{P}}_{k|k}=\left(\mathbf{I}-{\mathbf{K}}_k^C{\overline{\mathbf{H}}}_k\right){\mathbf{P}}_{k|k-1}{\left(\mathbf{I}-{\mathbf{K}}_k^C{\overline{\mathbf{H}}}_k\right)}^{\prime }+{\mathbf{K}}_k^C{\mathbf{R}}_k{\mathbf{K}}_k^{C^{\prime }}.$$

(15)

Theorem 1.

As the kernel bandwidth $\delta \to \infty$, the Cauchy kernel based MC estimator reduces to the standard nonlinear state estimation algorithm.

Proof. 

As the time update is the same for the developed algorithms with respect to the standard nonlinear state estimators, the prior mean and covariance is unchanged. Hence the focus shall be on the posterior mean and covariance. This implies that the Kalman gain equation has to be revisited. When $\delta \to \infty$,

$$\underset{\delta \to \infty }{lim}{\mathbf{L}}_k^C=\underset{\delta \to \infty }{lim}{C}_{\delta }^2\left({\parallel {\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k|k-1}\parallel }_{{\mathbf{R}}_k^{-1}}\right)=\underset{\delta \to \infty }{lim}\frac{1}{{\left(1+{\displaystyle \frac{{\aleph }^{\prime }{\mathbf{R}}_k^{-1}\aleph }{\delta }}\right)}^2}=1.$$

(16)

Substituting the Equations (7) and (16) and ${\overline{\mathbf{H}}}_k$ in ${\mathbf{K}}_k^C$, we have

$${\mathbf{K}}_k^C={\mathbf{P}}_{k|k-1}\left({\mathbf{P}}_{k|k-1}^{-1}{\mathbf{P}}_{\mathbf{X}\mathbf{z}}\right){({\mathbf{P}}_{\mathbf{z}\mathbf{z}}-{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}{\overline{\mathbf{H}}}_k^{\prime }+{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}{\overline{\mathbf{H}}}_k^{\prime })}^{-1}={\mathbf{P}}_{\mathbf{X}\mathbf{z}}{{\mathbf{P}}_{\mathbf{z}\mathbf{z}}}^{-1}.$$

Since the expression of ${\mathbf{K}}_k^C$ is similar to the Kalman gain of standard nonlinear state estimator, posterior mean is also the same.

Now, for the posterior covariance ${\mathbf{P}}_{k|k}$, consider Equation (15),

$${\mathbf{P}}_{k|k}={\mathbf{P}}_{k|k-1}-{\mathbf{P}}_{k|k-1}{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{K}}_k^{C^{\prime }}-{\mathbf{K}}_k^C{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}+{\mathbf{K}}_k^C\left({\mathbf{R}}_k+{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}{\mathbf{L}}_k^C{\overline{\mathbf{H}}}_k^{\prime }\right){\mathbf{K}}_k^{C^{\prime }}.$$

(17)

Post multiplying Equation (14) by $({\mathbf{R}}_k+{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}{\mathbf{L}}_k^C{\overline{\mathbf{H}}}_k^{\prime })$ on both sides give

$${\mathbf{K}}_k^C({\mathbf{R}}_k+{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}{\mathbf{L}}_k^C{\overline{\mathbf{H}}}_k^{\prime })={\mathbf{P}}_{k|k-1}{\mathbf{L}}_k^C{\overline{\mathbf{H}}}_k^{\prime }.$$

(18)

Using Equations (16) and (18)

$${\mathbf{P}}_{k|k}={\mathbf{P}}_{k|k-1}-{\mathbf{P}}_{k|k-1}{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{K}}_k^{C^{\prime }}-{\mathbf{K}}_k^C{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}+{\mathbf{P}}_{k|k-1}{\overline{\mathbf{H}}}_k^{\prime }{\mathbf{K}}_k^{C^{\prime }}={\mathbf{P}}_{k|k-1}-{\mathbf{K}}_k^C{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}.$$

Substituting ${\overline{\mathbf{H}}}_k$, we get ${\mathbf{P}}_{k|k}={\mathbf{P}}_{k|k-1}-{\mathbf{K}}_k^C{\mathbf{P}}_{\mathbf{X}\mathbf{z}}^{\prime }$. For the given condition, ${\mathbf{K}}_k^C={\mathbf{P}}_{\mathbf{X}\mathbf{z}}{{\mathbf{P}}_{\mathbf{z}\mathbf{z}}}^{-1}$, then ${\mathbf{P}}_{\mathbf{X}\mathbf{z}}^{\prime }={{\mathbf{P}}_{\mathbf{z}\mathbf{z}}}^{\prime }{\mathbf{K}}_k^{C^{\prime }}$. Thus ${\mathbf{P}}_{k|k}$ will become ${\mathbf{P}}_{k|k}={\mathbf{P}}_{k|k-1}-{\mathbf{K}}_k^C{{\mathbf{P}}_{\mathbf{z}\mathbf{z}}}^{\prime }{\mathbf{K}}_k^{C^{\prime }}$, which matches with the posterior error covariance of standard nonlinear estimator.    □

Remark 1.

For systems with non-Gaussian noise with large probability of abnormal noise, small value of δ is likely to provide more robustness. If the occurrence of abnormal noise is less, large value of δ could be considered.

Remark 2.

Cauchy kernel based nonlinear estimator with different δ performs with more estimation accuracy than Gaussian kernel based nonlinear estimator with different σ. Hence it is easier to select a value for δ that can provide accurate and robust estimates in the presence of abnormal noise.

6. Nonlinear State Estimators

This section deals with the nonlinear state estimators UKF and NSKF with a generalized algorithm for kernel based MC estimator.

6.1. Unscented Kalman Filter (UKF)

In the Bayesian framework, when the functions are nonlinear, the integrals encountered are intractable in nature and has to be evaluated using suitable numerical approximation methods. The UKF, through its unscented transformation, provides a way to numerically evaluate these integrals. Assuming that the integral to be approximated is

$$\mathtt{I}\left(\mathbf{X}\right)=\int \mathbf{h}\left(\mathbf{X}\right)p_{\mathbf{X}}\left(\mathbf{X}\right)d\mathbf{X},$$

and $\mathbf{X}\sim \mathcal{N}(\widehat{\mathtt{X}},\mathbf{P})$, the unscented transformation defines a set of sigma points (${\widehat{\mathbf{X}}}_i$) and weights ($W_i$) such that [35]

$\mathtt{I}\left(\mathbf{X}\right)\approxeq \sum _{i=1}^{\mathbf{N}}{W}_i\mathbf{h}\left({\widehat{\mathbf{X}}}_i\right)$, where n is the dimension of the state space and N = 2n + 1.

The sigma points and weights are defined as

$$\begin{array}{cc}\hfill {\widehat{\mathbf{X}}}_1& =\widehat{\mathtt{X}},W_1={\displaystyle \frac{\kappa }{\mathit{n}+\kappa }},\hfill \\ \hfill {\widehat{\mathbf{X}}}_i& =\widehat{\mathtt{X}}+{\left(\sqrt{(\mathit{n}+\kappa )\mathbf{P}}\right)}_i{W}_i={\displaystyle \frac{1}{2(\mathit{n}+\kappa )}},i=1,\cdots ,\mathit{n}\hfill \\ \hfill {\widehat{\mathbf{X}}}_i& =\widehat{\mathtt{X}}-{\left(\sqrt{(\mathit{n}+\kappa )\mathbf{P}}\right)}_i{W}_i={\displaystyle \frac{1}{2(\mathit{n}+\kappa )}},i=1,\cdots ,\mathit{n},\hfill \end{array}$$

(19)

with $\kappa$ being the tuning parameter and $\widehat{\mathtt{X}}$ is the mean.

6.2. New Sigma Point Kalman Filter (NSKF)

From Equation (19), it can observed that in the unscented transformation, the maximum weight is assigned to the mean value. All the other sigma points are assigned equal weights, i.e., same probability of occurrence. In NSKF, a new approach was considered such that the sigma points closer to the mean will have more probability of occurrence. To realize this, a new method was formulated for defining the sigma points and weights, stated as [8]

$$\begin{array}{cc}\hfill & {\widehat{\mathbf{X}}}_1=\widehat{\mathtt{X}},W_1=1-\frac{{\sum }_{i=1}^{\mathit{n}}{\alpha }_i}{2({\sum }_{i=1}^{\mathit{n}}{\alpha }_i+\mathit{b})}\hfill \\ \hfill & {\widehat{\mathbf{X}}}_{i+1}=\widehat{\mathtt{X}}+\sqrt{\frac{{\sum }_{i=1}^{\mathit{n}}{\alpha }_i+\mathit{b}}{\mathit{m}{\alpha }_i}}{S}_i,W_{i+1}=\frac{\mathit{m}{\alpha }_i}{4({\sum }_{i=1}^{\mathit{n}}{\alpha }_i+\mathit{b})},i=1,\cdots ,\mathit{n}\hfill \\ \hfill & {\widehat{\mathbf{X}}}_{i+1}=\widehat{\mathtt{X}}-\sqrt{\frac{{\sum }_{i=1}^{\mathit{n}}{\alpha }_i+\mathit{b}}{\mathit{m}{\alpha }_{i-\mathit{n}}}}{S}_{i-\mathit{n}},W_{i+1}=\frac{\mathit{m}{\alpha }_{i-\mathit{n}}}{4({\sum }_{i=1}^{\mathit{n}}{\alpha }_i+\mathit{b})},i=\mathit{n}+1,\cdots ,2\mathit{n}\hfill \\ \hfill & {\widehat{\mathbf{X}}}_{i+1}=\widehat{\mathtt{X}}+\sqrt{\frac{{\sum }_{i=1}^{\mathit{n}}{\alpha }_i+\mathit{b}}{(1-\mathit{m}){\alpha }_{i-2\mathit{n}}}}{S}_{i-2\mathit{n}},W_{i+1}=\frac{(1-\mathit{m}){\alpha }_{i-2\mathit{n}}}{4({\sum }_{i=1}^{\mathit{n}}{\alpha }_i+\mathit{b})},i=2\mathit{n}+1,\cdots ,3\mathit{n}\hfill \\ \hfill & {\widehat{\mathbf{X}}}_{i+1}=\widehat{\mathtt{X}}-\sqrt{\frac{{\sum }_{i=1}^{\mathit{n}}{\alpha }_i+\mathit{b}}{(1-\mathit{m}){\alpha }_{i-3\mathit{n}}}}{S}_{i-3\mathit{n}},W_{i+1}=\frac{(1-\mathit{m}){\alpha }_{i-3\mathit{n}}}{4({\sum }_{i=1}^{\mathit{n}}{\alpha }_i+\mathit{b})},i=3\mathit{n}+1,\cdots ,4\mathit{n}.\hfill \end{array}$$

(20)

Now the total number of sigma points $\mathbf{N}=4\mathit{n}+1$, ${\mathbf{P}}_i\mathrm{and}{S}_i$ denote the ith column of $\mathbf{P}andS$ respectively, and $S{S}^{\prime }=P$. The scalar variables are defined as $\mathit{b}>\{\frac{1}{4}max\left(m{\alpha }_i\right)-\frac{1}{2}{\sum }_{i=1}^{\mathit{n}}{\alpha }_i\}$, $m\in (0.5,1)$ and ${\alpha }_i=\frac{\mid <\widehat{\mathtt{X}},{\mathbf{P}}_i>\mid }{\Vert \widehat{\mathtt{X}}{\Vert }_2{\Vert {\mathbf{P}}_i\Vert }_2}$.

The Algorithm 1 for the developed estimators, both Gaussian kernel and Cauchy kernel based is given below. In this algorithm, ${\mathbf{K}}_k$ and ${\mathbf{L}}_k$ can be defined as per the chosen kernel function. ${\mathtt{R}}_k$ is the noise covariance matrix which is assumed to be known in case there are no measurement outliers.

Algorithm 1: For MC-UKF-CK and MC-NSKF-CK

$\mathtt{Initialise}$${\widehat{\mathbf{X}}}_{k-1|k-1}$$\mathtt{and}$${\mathbf{P}}_{k-1|k-1}$ ${\widehat{\mathbf{X}}}_{k|k-1}={\mathbf{F}}_{k-1}{\widehat{\mathbf{X}}}_{k-1|k-1}-{\mathbf{X}}_k^{\mathbf{o}}+{\mathbf{F}}_{k-1}{\mathbf{X}}_{k-1}^{\mathbf{o}}$ ${\mathbf{P}}_{k|k-1}={\mathbf{F}}_{k-1}{\mathbf{P}}_{k-1|k-1}{\mathbf{F}}_{k-1}^{\prime }+\mathbf{Q}$ $\mathrm{Calculate}$${\widehat{\mathbf{X}}}_i$ and $W_i$$\mathrm{using}$ (19) $\mathrm{or}$ (20), $i=1,\cdots ,\mathbf{N}$ ${\mathtt{Z}}_{i,k|k-1}=\mathbf{h}\left({\widehat{\mathbf{X}}}_i\right)$ ${\widehat{\mathbf{z}}}_k={\sum }_{i=1}^{\mathbf{N}}{W}_i{\mathtt{Z}}_{i,k|k-1}$ ${\mathbf{P}}_{\mathbf{z}\mathbf{z}}={\sum }_{i=1}^{\mathbf{N}}{W}_i[{\mathtt{Z}}_{i,k|k-1}-{\widehat{\mathbf{z}}}_k]{[{\mathtt{Z}}_{i,k|k-1}-{\widehat{\mathbf{z}}}_k]}^{\prime }+{\mathtt{R}}_k$ ${\mathbf{P}}_{\mathbf{X}\mathbf{z}}={\sum }_{i=1}^{\mathbf{N}}{W}_i[{\widehat{\mathbf{X}}}_{i,k|k-1}-{\widehat{\mathbf{X}}}_{k|k-1}]{[{\mathtt{Z}}_{i,k|k-1}-{\widehat{\mathbf{z}}}_k]}^{\prime }$ ${\overline{\mathbf{H}}}_k={\left({\mathbf{P}}_{k|k-1}^{-1}{\mathbf{P}}_{\mathbf{X}\mathbf{z}}\right)}^{\prime }$ ${\mathbf{R}}_k={\mathbf{P}}_{\mathbf{z}\mathbf{z}}-{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}{\overline{\mathbf{H}}}_k^{\prime }$ ${\mathbf{K}}_k={\mathbf{P}}_{k|k-1}{\mathbf{L}}_k{\overline{\mathbf{H}}}_k^{\prime }{({\mathbf{R}}_k+{\overline{\mathbf{H}}}_k{\mathbf{P}}_{k|k-1}{\mathbf{L}}_k{\overline{\mathbf{H}}}_k^{\prime })}^{-1}.$ Posterior mean: ${\widehat{\mathbf{X}}}_{k|k}={\widehat{\mathbf{X}}}_{k|k-1}+{\mathbf{K}}_k\left({\mathbf{z}}_k-{\widehat{\mathbf{z}}}_k\right)$. Posterior covariance: ${\mathbf{P}}_{k|k}=(\mathbf{I}-{\mathbf{K}}_k{\overline{\mathbf{H}}}_k){\mathbf{P}}_{k|k-1}{(\mathbf{I}-{\mathbf{K}}_k{\overline{\mathbf{H}}}_k)}^{\prime }+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\prime }$.

7. Modelling of Non Gaussian Noise in Angular Measurements

As mentioned in [36], a suitable way of modelling glint noise is to assume a Gaussian mixture. It is observed that the glint is more like Gaussian around the mean but has a non-Gaussian nature towards the tail region. The tail region represents the outliers, termed as glint spikes [32]. But shot noise, on the other hand, is modelled as an impulse with fixed amplitude at specific time steps. The mixture density of glint noise is modelled as $f\left(x\right)=(1-\mu )f_{g1}\left(x\right)+\mu f_{g2}\left(x\right)$, where $\mu$ is the glint probability and $f_{g1}\left(x\right)\sim \mathcal{N}(0,{\sigma }_1^2)$, $f_{g2}\left(x\right)\sim \mathcal{N}(0,{\sigma }_2^2)$ with ${\sigma }_1\ne {\sigma }_2$. The non Gaussian noises for angular measurements have been modelled by taking appropriate values for $\mu$, ${\sigma }_1$ and ${\sigma }_2$.

8. Simulation Results

The scenario for angles-only tracking problem in 2D as well as 3D Cartesian coordinate frame is considered in this section. The parameters required for generating the target-observer dynamics, and simulation results are discussed. For simulations, moderately nonlinear tracking scenario for 2D as well as for 3D is considered. The simulation is carried for 1000 Monte Carlo runs with sampling time interval denoted as T. The entire tracking scenario is implemented and simulated in MATLAB software.

8.1. 2D Scenario and Filter Initialisation

Figure 4 shows the tracking performance of MC-NSKF-CK, where the estimated target path is plotted along with the truth target path, and the observer path for a single Monte Carlo run. It should be noted that for each run, observer path remains the same where as the target path varies due to the process noise. Further, the filter initialisation is also changing because of the randomness introduced in each run, as mentioned in Equation (21).

The filter is initialised as given in [10]. It is to be noted that for filter initialisation, we need an initial guess for speed, initial course and range of the target. Considering the problem at hand, these estimates have to be obtained from the initial angle measurement received. From these initial guess for parameters, the initial estimate for the states are obtained which are the positions and velocities. Accordingly, the initial range, target course and speed values are considered and mentioned in the

Table 1. Tracking parameters for 2D scenario.

Parameters	Values
Initial Target Position	$\left[\begin{array}{cc}4.9286& 0.8420\end{array}\right]$ (km)
Initial Observer Position	$\left[\begin{array}{cc}0& 0\end{array}\right]$ (km)
Initial Target Speed (s)	4 (knots)
Initial Observer Speed	5 (knots)
Target Course	$-135.4^{\circ }$
Observer manoeuvre	From 780 to 1020 (s)
Initial Range (r)	5 (km)
Observation time	1800 (s)
${\mathtt{q}}_x$, ${\mathtt{q}}_y$	9 $\times {10}^{-12}$ (${\mathrm{km}}^2/{\mathrm{s}}^3$)
${\sigma }_{\beta }$	$1.5^{\circ }$
${\sigma }_r$	2 (km)
${\sigma }_s$	2 (knots)
Sampling time	$T=10$ (s)
Initial Observer Course	${140}^{\circ }$
Final Observer Course	${20}^{\circ }$
${\sigma }_c$	$\pi /\sqrt{12}$

. They are defined as $\overline{s}=\mathcal{N}(s,{\sigma }_s^2)$, $c_r=\mathcal{N}({\overline{c}}_r,{\sigma }_c^2)$ and $\overline{r}=\mathcal{N}(r,{\sigma }_r^2)$ where ${\overline{c}}_r$ can be defined as ${\overline{c}}_r={\mathit{z}}_0+\pi$ with ${\mathit{z}}_0$ as the first bearing measurement. Finally the initial state vector ${\widehat{\mathbf{X}}}_{0|0}$ and the initial covariance ${\mathbf{P}}_{0|0}$ is calculated as

$$\begin{array}{cccc}\hfill {\widehat{\mathbf{X}}}_{0|0}& =\left[\begin{array}{c}rsin\left({\mathit{z}}_0\right)\\ rcos\left({\mathit{z}}_0\right)\\ ssin\left({\overline{c}}_r\right)-{\dot{\mathtt{x}}}_0^{\mathtt{o}}\\ scos\left({\overline{c}}_r\right)-{\dot{\mathtt{y}}}_0^{\mathtt{o}}\end{array}\right]\hfill & \hfill {\mathbf{P}}_{0|0}& =\left[\begin{array}{cccc}{\mathtt{P}}_{\mathtt{xx}}& {\mathtt{P}}_{\mathtt{xy}}& 0& 0\\ {\mathtt{P}}_{\mathtt{yx}}& {\mathtt{P}}_{\mathtt{yy}}& 0& 0\\ 0& 0& {\mathtt{P}}_{\dot{\mathtt{x}}\dot{\mathtt{x}}}& {\mathtt{P}}_{\dot{\mathtt{x}}\dot{\mathtt{y}}}\\ 0& 0& {\mathtt{P}}_{\dot{\mathtt{y}}\dot{\mathtt{x}}}& {\mathtt{P}}_{\dot{\mathtt{y}}\dot{\mathtt{y}}}\end{array}\right]\hfill \end{array}$$

(21)

where

$$\begin{array}{cccc}\hfill {\mathtt{P}}_{\mathtt{xx}}& =r^2{\sigma }_{\beta }^2{cos}^2\left({\mathit{z}}_0\right)+{\sigma }_r^2{sin}^2\left({\mathit{z}}_0\right)\hfill & \hfill {\mathtt{P}}_{\mathtt{yy}}& =r^2{\sigma }_{\beta }^2{sin}^2\left({\mathit{z}}_0\right)+{\sigma }_r^2{cos}^2\left({\mathit{z}}_0\right)\hfill \\ \hfill {\mathtt{P}}_{\mathtt{xy}}& ={\mathtt{P}}_{\mathtt{yx}}=({\sigma }_r^2-r^2{\sigma }_{\beta }^2)sin\left({\mathit{z}}_0\right)cos\left({\mathit{z}}_0\right)\hfill & \hfill {\mathtt{P}}_{\dot{\mathtt{x}}\dot{\mathtt{x}}}& =s^2{\sigma }_c^2{cos}^2\left({\overline{c}}_r\right)+{\sigma }_s^2{sin}^2\left({\overline{c}}_r\right)\hfill \\ \hfill {\mathtt{P}}_{\dot{\mathtt{y}}\dot{\mathtt{y}}}& =s^2{\sigma }_c^2{sin}^2\left({\overline{c}}_r\right)+{\sigma }_s^2{cos}^2\left({\overline{c}}_r\right)\hfill & \hfill {\mathtt{P}}_{\dot{\mathtt{x}}\dot{\mathtt{y}}}& ={\mathtt{P}}_{\dot{\mathtt{y}}\dot{\mathtt{x}}}=({\sigma }_s^2-s^2{\sigma }_c^2)sin\left({\overline{c}}_r\right)cos\left({\overline{c}}_r\right).\hfill \end{array}$$

8.2. 3D Scenario and Filter Initialisation

Figure 5 shows the estimated target path obtained from MC-NSKF-CK and truth target path with observer trajectory. The initial parameter values required for generating the 3D scenario is given in the

Table 2. Target & Observer Initial Parameters.

Parameters	Values
Initial Target Position	$\left[\begin{array}{ccc}138/\sqrt{2}& 138/\sqrt{2}& 9\end{array}\right]$ (km)
Initial Observer Position	$\left[\begin{array}{ccc}0& 0& 10\end{array}\right]$ (km)
Initial Target Speed (s)	0.297 (km/s)
Initial Observer Speed (s)	0.297 (km/s)
Target Course	$-{135}^{\circ }$
Observer manoeuvre	From 70 to 370 (s)
Initial Range (r)	150 (km)
Observation time	420 (s)
${\mathtt{q}}_x$, ${\mathtt{q}}_y$	${10}^{-8}{\mathrm{km}}^2/{\mathrm{s}}^3$
${\mathtt{q}}_z$	${10}^{-10}{\mathrm{km}}^2/{\mathrm{s}}^3$
${\sigma }_{\beta }$, ${\sigma }_{ϵ}$	$0.{057}^{\circ }$
${\sigma }_r$	13.6 (km)
${\sigma }_s$	41.6 (m/s)
Elevation Angle	$0.{415}^{\circ }$
Sampling time	$T=10$ (s)

. Assuming that there are no outliers in the measurement, ${\mathtt{R}}_k$ is defined as ${\mathtt{R}}_k=diag({\sigma }_{\beta },{\sigma }_{ϵ})$. The bearing angle $\beta$ is calculated with reference to the true North.

For each Monte Carlo run, according to the new measurement received, initial range r and speed of the target s is assumed as mentioned in Table 2. According to these values, the relative state is initialised using the range estimate $\overline{r}\sim \mathcal{N}(r,{{\sigma }_r}^2)$, initial bearing and elevation estimate ${\widehat{\beta }}_1$ and ${\widehat{ϵ}}_1$ with headings ${\overline{\alpha }}_1={\beta }_1+\pi$ rad/s and ${\overline{\gamma }}_1=0$ rad/s, and the initial speed estimate $\overline{s}\sim \mathcal{N}(s,{{\sigma }_s}^2)$ with s as 0.258 km/s. The ${\sigma }_{{\overline{\alpha }}_1}=\pi /\sqrt{12}$ and ${\sigma }_{{\overline{\gamma }}_1}=\pi /60$ respectively [37].

The initial relative state vector ${\widehat{\mathbf{X}}}_{0|0}$ is given as [38]

$${\widehat{\mathbf{X}}}_{0|0}=\left[\begin{array}{c}\overline{r}{\zeta }_{1,0}({\widehat{ϵ}}_1,{{\sigma }_{ϵ}}^2){\zeta }_{0,1}({\widehat{\beta }}_1,{{\sigma }_{\beta }}^2)\\ \overline{r}{\zeta }_{1,0}({\widehat{ϵ}}_1,{{\sigma }_{ϵ}}^2){\zeta }_{1,0}({\widehat{\beta }}_1,{{\sigma }_{\beta }}^2)\\ \overline{r}{\zeta }_{0,1}({\widehat{ϵ}}_1,{{\sigma }_{ϵ}}^2)\\ \overline{s}{\zeta }_{1,0}({\overline{\gamma }}_1,{{\sigma }_{\gamma }}^2){\zeta }_{0,1}({\overline{\alpha }}_1,{{\sigma }_{\alpha }}^2)-{\dot{\mathtt{x}}}_1^{\mathtt{o}}\\ \overline{s}{\zeta }_{1,0}({\overline{\gamma }}_1,{{\sigma }_{\gamma }}^2){\zeta }_{1,0}({\overline{\alpha }}_1,{{\sigma }_{\alpha }}^2)-{\dot{\mathtt{y}}}_1^{\mathtt{o}}\\ \overline{s}{\zeta }_{0,1}({\overline{\gamma }}_1,{{\sigma }_{\gamma }}^2)-{\dot{\mathtt{z}}}_1^{\mathtt{o}}\end{array}\right],$$

where ${\zeta }_{1,0}(\mu ,{\sigma }^2)=cos\mu exp(-{\sigma }^2/2)\mathrm{and}{\zeta }_{0,1}(\mu ,{\sigma }^2)=sin\mu exp(-{\sigma }^2/2)$.

The initial covariance matrix ${\mathbf{P}}_{0|0}$, whose entries are considered as mentioned in [38], is defined as

$${\mathbf{P}}_{0|0}=\left[\begin{array}{cccccc}{P}_{\mathtt{xx}}& P_{\mathtt{xy}}& P_{\mathtt{xz}}& 0& 0& 0\\ P_{\mathtt{xy}}& P_{\mathtt{yy}}& P_{\mathtt{yz}}& 0& 0& 0\\ P_{\mathtt{xz}}& P_{\mathtt{yz}}& P_{\mathtt{zz}}& 0& 0& 0\\ 0& 0& 0& P_{\dot{\mathtt{x}}\dot{\mathtt{x}}}& P_{\dot{\mathtt{x}}\dot{\mathtt{y}}}& P_{\dot{\mathtt{x}}\dot{\mathtt{z}}}\\ 0& 0& 0& P_{\dot{\mathtt{x}}\dot{\mathtt{y}}}& P_{\dot{\mathtt{y}}\dot{\mathtt{y}}}& P_{\dot{\mathtt{y}}\dot{\mathtt{z}}}\\ 0& 0& 0& P_{\dot{\mathtt{x}}\dot{\mathtt{z}}}& P_{\dot{\mathtt{y}}\dot{\mathtt{z}}}& P_{\dot{\mathtt{z}}\dot{\mathtt{z}}}\end{array}\right].$$

8.3. Performance Metrics

Performance analysis of the estimators formulated is evaluated by considering the below mentioned error statistics.

1.

RMSE: Root-mean-square error in resultant target position is computed as follows

$$\begin{array}{cc}\hfill {\mathrm{RMSE}}_k& ={\sqrt{\frac{1}{M}\sum _{j=1}^M[{({\mathtt{x}}_{j,k}^{\mathtt{t}}-{\widehat{\mathtt{x}}}_{j,k}^{\mathtt{t}})}^2+{({\mathtt{y}}_{j,k}^{\mathtt{t}}-{\widehat{\mathtt{y}}}_{j,k}^{\mathtt{t}})}^2]}}_{n=2}\hfill \\ \hfill {\mathrm{RMSE}}_k& ={\sqrt{\frac{1}{M}\sum _{j=1}^M[{({\mathtt{x}}_{j,k}^{\mathtt{t}}-{\widehat{\mathtt{x}}}_{j,k}^{\mathtt{t}})}^2+{({\mathtt{y}}_{j,k}^{\mathtt{t}}-{\widehat{\mathtt{y}}}_{j,k}^{\mathtt{t}})}^2+{({\mathtt{z}}_{j,k}^{\mathtt{t}}-{\widehat{\mathtt{z}}}_{j,k}^{\mathtt{t}})}^2]}}_{n=3}\hfill \end{array}$$

where k denotes the time steps and M the total number of Monte Carlo runs.

2.

Track Divergence: In order to identify if a track is divergent or not, a certain threshold value (${\mathcal{T}}_b$) is set according to the position error computed at the final time instant of observation ($k_{max}$) as

$$\begin{array}{cc}\hfill {\mathtt{pos}}_{\mathtt{err}}& ={\sqrt{{({\mathtt{x}}_{j,k_{max}}^{\mathtt{t}}-{\widehat{\mathtt{x}}}_{j,k_{max}}^{\mathtt{t}})}^2+{({\mathtt{y}}_{j,k_{max}}^{\mathtt{t}}-{\widehat{\mathtt{y}}}_{j,k_{max}}^{\mathtt{t}})}^2}}_{n=2}\hfill \\ \hfill {\mathtt{pos}}_{\mathtt{err}}& ={\sqrt{{({\mathtt{x}}_{j,k_{max}}^{\mathtt{t}}-{\widehat{\mathtt{x}}}_{j,k_{max}}^{\mathtt{t}})}^2+{({\mathtt{y}}_{j,k_{max}}^{\mathtt{t}}-{\widehat{\mathtt{y}}}_{j,k_{max}}^{\mathtt{t}})}^2+{({\mathtt{z}}_{j,k_{max}}^{\mathtt{t}}-{\widehat{\mathtt{z}}}_{j,k_{max}}^{\mathtt{t}})}^2}}_{n=3}\hfill \\ \hfill & forj=1,2,\cdots ,M.\hfill \end{array}$$

So, if the difference between estimated and truth target position is more than the threshold value (${\mathcal{T}}_b$), then we can say that the estimated path is moving away from the truth path. Thus, the track is considered to be divergent, and the number of such tracks are counted over M Monte Carlo runs.

8.4. Performance Analysis

The performance analysis of the developed filters is evaluated in the presence of glint plus shot noise in angle measurements. The accuracy of the estimators are evaluated by computing root mean square error (RMSE) in position at the end of the simulation period by imposing a track loss condition of 1 km.

$${\mathtt{v}}_k=\left\{\begin{array}{cc}0.2\mathcal{N}(0,{{\sigma }_{{\theta }_1}}^2)+0.8\mathcal{N}(0,{{\sigma }_{{\theta }_2}}^2)+{10}^{\circ },\hfill & \mathrm{when}k=1200\mathrm{and}900s\hfill \\ 0.2\mathcal{N}(0,{{\sigma }_{{\theta }_1}}^2)+0.8\mathcal{N}(0,{{\sigma }_{{\theta }_2}}^2),\hfill & \mathrm{otherwise}.\hfill \end{array}\right.$$

(22)

The measurement noise ${\mathbf{v}}_k$ for both the scenarios are given in Equations (22) and (23), respectively. Here, ${\sigma }_{{\theta }_1}=0.5^{\circ }$, ${\sigma }_{{\theta }_2}=5^{\circ }$, (${\sigma }_{{\beta }_1},{\sigma }_{{ϵ}_1}$) as 0.0001 rad and (${\sigma }_{{\beta }_2},{\sigma }_{{ϵ}_2}$) as 0.01 rad. The noise corrupted angle measurement for 2D is plotted as Figure 6. For 3D, the noise corrupted bearing and elevation angle are as shown in Figure 7 and Figure 8 respectively. For illustration, in the figures, we have also plotted the angle measurements with Gaussian noise.

$${\mathbf{v}}_k=\left\{\begin{array}{c}0.8\mathcal{N}(0,diag\left(\left[{{\sigma }_{{\beta }_1}}^2{{\sigma }_{{ϵ}_1}}^2\right]\right))+0.2\mathcal{N}(0,diag\left(\left[{{\sigma }_{{\beta }_2}}^2{{\sigma }_{{ϵ}_2}}^2\right]\right))+{\left[{10}^{\circ }{1}^{\circ }\right]}^T,\hfill \\ \mathrm{when}k=270\mathrm{and}390s\hfill \\ 0.8\mathcal{N}(0,diag\left(\left[{{\sigma }_{{\beta }_1}}^2{{\sigma }_{{ϵ}_1}}^2\right]\right))+0.2\mathcal{N}(0,diag\left(\left[{{\sigma }_{{\beta }_2}}^2{{\sigma }_{{ϵ}_2}}^2\right]\right)),\mathrm{otherwise}.\hfill \end{array}\right.$$

(23)

With track loss condition of less than 1 km, the observed RMSE in position at the last time instant and percentage track loss for 2D as well as 3D is given in the

Table 3. 2D: RMSE in position and % Track Loss.

Filters	% Track Loss	RMSE (m)
UKF	4.4	152.8
MC-UKF-GK	1.1	111.0
MC-UKF-CK	1.1	108.9
NSKF	2.8	151.1
MC-NSKF-GK	1.2	109.6
MC-NSKF-CK	0.5	108.8

and

Table 4. 3D: RMSE in position and % Track Loss.

Filters	% Track Loss	RMSE (m)
UKF	100	-
MC-UKF-GK	14	496.1
MC-UKF-CK	13.5	499.8
NSKF	100	-
MC-NSKF-GK	14	496.8
MC-NSKF-CK	13.5	498.9

respectively. Also, RMSE in resultant position (after excluding the diverged tracks) is evaluated and plotted in Figure 9 and Figure 10. From these figures it can be inferred that in the presence of non Gaussian noise the estimation accuracy of UKF deteriorates, whereas filters based on MC framework performed with superior estimation accuracy. From the tabulation results it is evident that the Cauchy kernel based MC-UKF and MC-NSKF gives 108.9 m, 108.8 m RMSE and 1.1 and 0.5% track loss which is much less than that of the conventional UKF and NSKF which gives 152.8 m and 151.1 m RMSE with 4.4 and 2.8% track loss in 2D scenario. Similar observations can be made with respect to Gaussian kernel based MC-UKF and MC-NSKF giving much better accuracy but slightly less than Cauchy kernel MC framework. However, in 3D scenario, the MC based filters gave even superior estimation efficiency than that of the 2D scenario. UKF and NSKF in 3D with non Gaussian noise resulted in 100% track loss. Hence it can be inferred that for the given problem set up and noise statistics, UKF and NSKF failed to give estimates that met the track loss condition set, where as the Gaussian and Cauchy kernel based maximum correntropy filters gave more robust and accurate estimates. This can be inferred from the simulation results where the developed filters incurred only 13 to 14% track loss, with a final error in range of around 500 m. All these simulations are carried out by assuming the bandwidth ($\sigma ,\delta$) for 2D as (9,70) and for 3D as (11,75) such that the estimators can achieve maximum estimation accuracy. Also, the tuning parameter value of NSKF, $m=0.6$ is assumed for simulation.

9. Conclusions

Since, measurements obtained in target tracking scenarios are corrupted with non Gaussian noise, this paper presents a maximum correntropy framework for 2D as well as 3D angles-only target tracking problem. The reformulation of UKF and NSKF in terms of Gaussian and Cauchy kernel based MC framework was realized. The non Gaussian noise is modelled as a Gaussian mixture (glint noise) plus shot noise. Finally, the performance of the estimators were evaluated and a comparative analysis is presented on the basis of RMSE in position and % track loss. From the comparative analysis, it can be concluded that the Gaussian and Cauchy kernel based MC framework provides improved estimation accuracy than UKF and NSKF in non Gaussian noise environments. Thus, it can be inferred that MC based estimators have the potential to give accurate and robust state estimates in the presence of non Gaussian noises in angle measurements. As a future work, the proposed estimation framework can be extended to track a manoeuvring target in the presence of angles-only measurements corrupted with non Gaussian noise.