Mid-State Kalman Filter for Nonlinear Problems

Abstract

When tracking very long-range targets, wide-band radars capable of measuring targets with high precision at ranges have severe measurement nonlinearities. The existing nonlinear filtering technology, such as the extended Kalman filter and untracked Kalman filter, will have significant consistency problems and loss in tracking accuracy. A novel mid-state Kalman filter is proposed to avoid loss and preserve the filtering consistency. The observed state and its first-order state derivative are selected as the mid-state vector. The update process is transformed into the measurement space to ensure the Gaussian measurement distribution and the linearization of the measurement equation. In order to verify the filter performance in comparison, an iterative formulation of Cramér-Rao Low Bound for the nonlinear system is further derived and given in this paper. Simulation results show that the proposed method has excellent performance of high filtering accuracy and fast convergence by comparing the filter state estimation accuracy and consistency.

1. Introduction

Target tracking is a process that uses sensors to estimate the characteristics of a moving object of interest. It is widely used in unmanned aerial vehicles (UAV), military strikes, and other fields [1,2]. As the core problem of target tracking, state estimation theory realizes the real-time online estimation of a target’s motion state by integrating the prior information of the target and online measurement information provided by sensors. When the system satisfies the linear Gaussian condition, a Kalman filter (KF) can recursively obtain the consistent minimum variance and linear unbiased estimate of the state and is the optimal solution [3]. However, the system model is nonlinear in multi-sensor fusion, radar maneuvering target tracking, and satellite communication systems [4]. For the nonlinear system model, a nonlinear filtering method is needed to improve the estimation accuracy.

The extended Kalman filter (EKF) is the most widely used nonlinear filter in practical engineering because of its simple algorithm and small computation. The EKF uses a Taylor series expansion to approximate the nonlinear system model. When the nonlinearity is severe, the filtering accuracy will be reduced or even diverged due to a high-order truncation error [5]. Therefore, a second order and higher-order EKF are proposed successively, but their computational burden increases significantly [6]. The iterated extended Kalman filter (IEKF) is also obtained by dividing the one-step update of the EKF into multiple steps in pseudo-time and gradually updating the states according to the nonlinear gradient of measurement function [7]. The scholars applied numerical integration approximation methods to nonlinear filtering. The Gauss Hermite filter (GHF), unscented Kalman filter (UKF), and cubature Kalman filter (CKF) were proposed successively. The GHF is a polynomial integral approximation filtering algorithm for nonlinear system models [8] which uses Gauss-Hermite polynomials to approximate the probability density in Gaussian filtering. The UKF takes the UT criterion to select deterministic Sigma sampling points in the original state distribution point set, inputs the sampling points into the nonlinear system, and obtains the mean and covariance of the posterior probability density function through the transformation of the point set [9,10]. The UKF has less computation and better approximation performance than the EKF. The CKF is based on the spherical-radial criterion and uses a group of cubature points with equal weight to calculate the mean and covariance of state variables [11]. Wang Shuo conducts a comparative analysis on the UKF and CKF for low-dimensional and high-dimensional models under nonlinear conditions. The simulation results show that the CKF has the optimal numerical stability and filtering accuracy under high dimensional conditions [12,13]. A Particle Filter (PF) is not limited by the linearization error or gaussian noise assumption and approximates the probability density function corresponding to the nonlinear function. However, the considerable amount of calculation is unbearable for a real-time target tracking system [14].

Due to the complexity and uncertainty of very long-range target tracking, the Gaussian noise distribution in the measurement space will become a severe non-Gaussian distribution when it is converted into the state space in the update process. This phenomenon is often encountered in wide-band radar systems with high-range accuracy [15]. Due to the nonlinear distribution of the measurement area, existing nonlinear filters have specific problems in the accuracy of state estimation and the consistency of the filter and even divergence of the filter may occur in the EKF [16]. Because of this, a mid-state Kalman filter (MSKF) is proposed in this paper. A MSKF takes the observed state and its first-order state derivative as the mid-state vector and converts the predicted state covariance into the covariance of the mid-state vector according to the third-order spherical-radial criterion. Then the KF update process is converted to the measurement space and realizes the linearization of the measurement equation. Therefore, information loss in updating will be reduced to a minimum and the MSKF will have a higher accuracy in state estimation. The MSKF was applied to very long-range tracking problems and the simulation results prove the superiority and applicability of the algorithm.

2. Traditional Nonlinear Filter

In practical engineering applications, discrete nonlinear systems are generally characterized using additive noise and the system model is expressed as

$$\{\begin{array}{l}{x}_k=f(x_{k-1})+w_{k-1}\\ z_k=h(x_k)+v_k\end{array}$$

(1)

where process noise $w_k$ and observation noise $v_k$ are independent white zero-mean Gaussian noise.

The EKF obtains linear approximation of nonlinear system by using Taylor series expansion and then uses a KF to deal with the filtering problem of nonlinear system. The EKF approximates the nonlinear functions $f(x_{k-1})$ and $h(x_k)$ in the state space to the first-order Taylor polynomials near ${\widehat{x}}_k$ and ${\widehat{x}}_{k|k-1}$, respectively:

$$f(x_k)\approx f({\widehat{x}}_{k-1})+F_{k-1}[x_{k-1}-{\widehat{x}}_{k-1}]$$

(2)

$$h(x_k)\approx h({\widehat{x}}_{k|k-1})+H_k[x_k-{\widehat{x}}_{k|k-1}]$$

(3)

where $F_{k-1}$ and $H_k$ are the Jacobi matrices as follows:

$$F_{k-1}={\frac{\partial f(x_{k-1})}{\partial x}|}_{x={\widehat{x}}_{k-1}}$$

(4)

$$H_k={\frac{\partial h(x_k)}{\partial x}|}_{x={\widehat{x}}_{k|k-1}}$$

(5)

The UKF is different from the EKF in that it directly calculates the mean and covariance of the target distribution, avoiding the approximation of nonlinear functions. The UKF uses the mean and covariance of initial distribution to generate a series of determined sigma sampling points according to Formulas (6) and (7). These sigma sampling points are propagated through nonlinear functions to get the estimated mean and covariance [17].

$$\{\begin{array}{l}{\chi }_0=x_k\hfill \\ {\chi }_i=x_k+e_i\sqrt{(n_x+\kappa )P_x}\hfill \\ {\chi }_{i+n}=x_k-e_i\sqrt{(n_x+\kappa )P_x}\hfill \end{array},i=1,2,\cdots ,n_x$$

(6)

The corresponding weight is

$$\{\begin{array}{c}{w}_0=\frac{\kappa }{n_x+\kappa }\hfill \\ w_i=w_{i+n}=\frac{1}{2(n_x+\kappa )}\end{array}$$

(7)

where $\kappa$ is a fine-tuned scalar parameter, $n_x$ is the dimension of the state vector $x_k$, and $e_i$ is the $i$-th Cartesian fundamental vector. The size of $\kappa$ is related to the size of the sample moments and controls the distance from the sigma point to the mean point.

Under the Gaussian assumption, the state estimation of the nonlinear KF can ultimately be equated to the calculation of multidimensional vector integral, which can be summarized in the form of a nonlinear function $\times$ Gaussian probability density function:

$$I(g)={\displaystyle \underset{R^n}{\int }g(x)N(x;\overline{x},P_x)dx}$$

(8)

where $g(·)$ is any nonlinear function. To solve the integral numerically, the CKF transforms it into spherical-radial form and then carries out numerical integration according to the spherical-radial criterion

$${\displaystyle {\int }_0^{\infty }f(r)r^{n-1}\exp (-r^2)dr}={\displaystyle \sum _{i=1}^m{w}_{i}f(r_i)}$$

(9)

where $r$ is the radial scalar obtained by spherical-radial transformation of the vector $x$ in a cartesian coordinate system. The CKF filtering process is similar to that of the UKF. They all transform the sampled points with weight through the equations of the nonlinear system to calculate the first and second order moments required by filtering, but they have essential differences in obtaining sampling points. In a high-dimensional system, the weights of the UKF sigma points are prone to be negative which will cause the problem of unstable estimated values. The weights of the CKF sampling points are always positive, which solves the problem of instability. However, the filtering accuracy will be affected due to the increasing distance between the sampling point and the center point.

3. Mid-State Kalman Filter for Nonlinear Problems

The KF is proposed based on the least mean square error criterion. Under the assumption that the observation noise obeys the Gaussian distribution, the KF is a constant minimum variance and linear unbiased estimate that can recursively obtain the state. In this paper, a Mid-state Kalman Filter is proposed based on the KF method. A MSKF selects the observed state and its first-order state derivative as the mid-state vector and transforms the filter update process into the measurement space. The proposed method guarantees the Gaussian distribution of the measurement and correspondingly transforms the measurement equation into a linear form.

Mid-State Kalman Filter

Consider a nonlinear discrete system for target tracking as shown below:

$$\{\begin{array}{l}{x}_k=F_k{x}_{k-1}+w_{k-1}\hfill \\ z_k=H_k(x_k)+v_k\hfill \end{array}$$

(10)

where, $x_k\in {\mathbf{R}}^{\mathrm{n}}$ and $z_k\in {\mathbf{R}}^{\mathrm{m}}$ are the system state vector and the measurement vector; $F_k(·):{\mathbf{R}}^{\mathrm{n}}\to {\mathbf{R}}^{\mathrm{n}}$ and $H_k(·):{\mathbf{R}}^{\mathrm{n}}\to {\mathbf{R}}^{\mathrm{m}}$ respectively represent the system state function and nonlinear measurement function; and the process noise $w_k$ and observation noise $v_k$ are independent zero-mean Gaussian noise with covariance $Q_k$ and $R_k$.

In the typical two-dimensional radar target tracking, the target is assumed to be in uniform linear motion. Its state vector at time $k$ is $x_k={[x_k,v_{x_k},y_k,v_{y_k}]}^T$, including the position ${[x_k,y_k]}^T$ and velocity vector ${[v_{x_k},v_{y_k}]}^T$. In the observation vector $z_k={[r_k,{\theta }_k]}^T$, $r_k$ and ${\theta }_k$ are the distance measurement and angle measurement, respectively. The system state function and nonlinear measurement function in Equation (10) are respectively expressed as:

$$F=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],H_k(x_k)=\left[\begin{array}{c}\sqrt{x_k^2+y_k^2}\\ \arctan (\frac{y_k^2}{x_k^2})\end{array}\right]$$

(11)

The MSKF selects $x_{M_k}={[z_k,{\dot{z}}_k]}^T={[r_k,{\dot{r}}_k,{\theta }_k,{\dot{\theta }}_k]}^T$ as mid-state vector and linearizes the measurement equation in the measurement space as:

$$z_k=H_z{x}_{M_k}+v_k=[\begin{array}{cc}{I}_m& 0_{m\times m}\end{array}]\times {[z_k,{\dot{z}}_k]}^T+v_k$$

(12)

In this case, the nonlinear system in Equation (10) can be transformed into the following linear system:

$$\{\begin{array}{l}{x}_k=F_k{x}_{k-1}+w_{k-1}\hfill \\ z_k=H_z{x}_{M_k}+v_k\hfill \end{array}$$

(13)

This avoids the linearization process of the measurement function $H_k(x_k)$. Because the system satisfies the linear Gaussian condition of the KF, the mid-state can be updated by the KF. As the optimal linear filter, KF has less information loss and higher filtering accuracy.

In the mid-state, ${[{\dot{r}}_k,{\dot{\theta }}_k]}^T$ is not only the first-order state derivative of ${[r_k,{\theta }_k]}^T$, but also has practical physical significance. ${\dot{r}}_k$ and ${\dot{\theta }}_k$, respectively, represent the radial velocity $v_r$ and angular velocity $\omega$ of the target movement in polar coordinates. Therefore, the linear velocity is $v_{\theta }=\omega r$. As shown in Figure 1, the relative radar distance of the target is $r$, the angle is $\theta$, and the velocity at time $k$ is $v$. In the cartesian coordinate system, the velocity vector $v$ can be decomposed into $x$ axial velocity $v_x$ and $y$ axial velocity $v_y$. In the polar coordinate system, the velocity vector $v$ can be decomposed into radial velocity $v_r$ and linear velocity $v_{\theta }$. It should be noted that the radial velocity $v_r$ only changes the magnitude of the velocity $v$ without changing its direction, while the linear velocity $v_{\theta }$ only changes the direction of the velocity $v$ without changing its magnitude.

The mid-state is introduced into the Kalman filter to form the tracking filter algorithm MSKF, which is suitable for dealing with nonlinear problems. The algorithm steps are given as follows.

Initialization: Given the initial state estimate ${\widehat{x}}_0$ and covariance matrix $P_0$, set the time $k=1$.

Prediction: Assume that the input of filter in prediction step is the filtering result ${\widehat{x}}_k$ and $P_k$ at time $k$, and obtain the state prediction result ${\widehat{x}}_{k+1|k}$ and $P_{k+1|k}$ through the following formula:

$${\widehat{x}}_{k+1|k}=F_k{x}_k$$

(14)

$$P_{k+1|k}=F_k{P}_k{F}_k^T+Q_k$$

(15)

State transformation: The predicted state vector ${\widehat{x}}_{k+1|k}$ is converted to the mid-state $x_{M_{k+1|k}}={[z_{k+1|k},{\dot{z}}_{k+1|k}]}^T$, and the predicted covariance $P_{k+1|k}$ in the state space is converted to the covariance $P_{M_{k+1|k}}$ in the measurement space according to the third-order spherical-radial criterion.

According to the geometric relation and velocity decomposition in Figure 1, the transformation relationship from state vector ${\widehat{x}}_{k+1|k}={[x_{k+1|k},v_{x_{k+1|k}},y_{k+1|k},v_{y_{k+1|k}}]}^T$ to mid-state vector $x_{M_{k+1|k}}={[r_{M_{k+1|k}},{\dot{r}}_{M_{k+1|k}},{\theta }_{M_{k+1|k}},{\dot{\theta }}_{M_{k+1|k}}]}^T$ is shown in the following formula:

$$\{\begin{array}{c}{r}_{M_{k+1|k}}=\sqrt{x_{k+1|k}{}^2+y_{k+1|k}{}^2}\hfill \\ {\dot{r}}_{M_{k+1|k}}=\frac{1}{r}(x_{k+1|k}{v}_{x_{k+1|k}}+y_{k+1|k}{v}_{y_{k+1|k}})\\ {\theta }_{M_{k+1|k}}=\arctan (\frac{y_{k+1|k}}{x_{k+1|k}})\hfill \\ {\dot{\theta }}_{M_{k+1|k}}=\frac{1}{r^2}(x_{k+1|k}{v}_{y_{k+1|k}}-y_{k+1|k}{v}_{x_{k+1|k}})\end{array}$$

(16)

The predicted covariance $P_{k+1|k}$ in the state space was converted to the covariance $P_{M_{k+1|k}}$ in the measurement space according to the third-order spherical-radial criterion:

(1)

Obtain $2n$ sampling points through ${\widehat{x}}_{k+1|k}$ and $P_{k+1|k}$:

$$\{\begin{array}{l}{w}_i=w_{i+n_x}=\frac{1}{2n_x}\hfill \\ {\zeta }_i={\widehat{x}}_{k+1|k}+\sqrt{n_x{P}_{k+1|k}}{e}_i\hfill \\ {\zeta }_{i+n}={\widehat{x}}_{k+1|k}-\sqrt{n_x{P}_{k+1|k}}{e}_i\hfill \end{array}, i=1,2,\cdots ,n_x$$

(17)

where $e_i$ represents the unit vector with the $i$-th element being 1.

(2)

The value of the sampling point after conversion is ${ℂ}_i=l({\zeta }_i), i=1,2,\cdots ,2n_x$.

(3)

Then the covariance $P_{M_{k+1|k}}$ in the intermediate states in the measurement space is:

$$\overline{c}={\displaystyle \sum _{i=1}^{2n}{w}_i{ℂ}_i}$$

(18)

$$P_{M_{k+1|k}}={\displaystyle \sum _{i=1}^{2n}{w}_i({ℂ}_i-\overline{c}){({ℂ}_i-\overline{c})}^T}$$

(19)

The state transformation represents the target state transformation relationship under different coordinates. This module can be quickly replaced according to the actual situation without modifying the filter prediction and update module. This also improves the practical engineering applicability of the MSKF.

Update: The state estimation results $x_{M_{k+1}}$ and $P_{M_{k+1}}$ in the measurement space are obtained from the mid-states $x_{M_{k+1|k}}$ and their covariance $P_{M_{k+1|k}}$.

$$S_{M_{k+1}}=H_z{P}_{M_{k+1|k}}{H}_z^T+R_{k+1}$$

(20)

$$K_{M_{k+1}}=P_{M_{k+1|k}}{H}_z{S}_{M_{k+1|k}}^{-1}$$

(21)

$$x_{M_{k+1}}=x_{M_{k+1|k}}+K_{M_{k+1}}(z_{k+1}-H_z{x}_{M_{k+1|k}})$$

(22)

$$P_{M_{k+1}}=(I_{2m}-K_{M_{k+1}}{H}_z)P_{M_{k+1|k}}$$

(23)

where $H_z=[\begin{array}{cc}{I}_m& 0_{m\times m}\end{array}]$ is the observation matrix of the mid-state vector.

Extraction of state estimation: Obtain the state estimation ${\widehat{x}}_{k+1}$ and $P_{k+1}$ at time $k$ from the filtering results $x_{M_{k+1}}$ and $P_{M_{k+1}}$ in measurement space.

This process is the reverse process of state transformation. Therefore, the conversion relationship from the mid-state vector $x_{M_{k+1}}={[r_{M_{k+1}},{\dot{r}}_{M_{k+1}},{\theta }_{M_{k+1}},{\dot{\theta }}_{M_{k+1}}]}^T$ to the state vector ${\widehat{x}}_{k+1}={[x_{k+1},v_{x_{k+1}},y_{k+1},v_{y_{k+1}}]}^T$ is

$$\{\begin{array}{l}{x}_{k+1}=r_{M_{k+1}}\cos {\theta }_{M_{k+1}}\hfill \\ v_{x_{k+1}}=v_{r_{M_{k+1}}}\cos {\theta }_{M_{k+1}}-v_{{\theta }_{M_{k+1}}}\sin {\theta }_{M_{k+1}}={\dot{r}}_{M_{k+1}}\cos {\theta }_{M_{k+1}}-r_{M_{k+1}}{\dot{\theta }}_{M_{k+1}}\sin {\theta }_{M_{k+1}}\hfill \\ y_{k+1}=r_{M_{k+1}}\sin {\theta }_{M_{k+1}}\hfill \\ v_{y_{k+1}}=v_{r_{M_{k+1}}}\sin {\theta }_{M_{k+1}}+v_{{\theta }_{M_{k+1}}}\cos {\theta }_{M_{k+1}}={\dot{r}}_{M_{k+1}}\sin {\theta }_{M_{k+1}}+r_{M_{k+1}}{\dot{\theta }}_{M_{k+1}}\cos {\theta }_{M_{k+1}}\hfill \end{array}$$

(24)

At the same time, $P_{k+1}$ is obtained from $P_{M_{k+1}}$ according to the third-order spherical-radial criterion.

The algorithm flow of the estimator is shown in Figure 2.

4. Nonlinear Filter Performance Evaluation

In order to better verify the performance of nonlinear filters, it is necessary to analyze and evaluate the filtering accuracy, credibility, stability, and other aspects of the filter which largely depend on reasonable evaluation metrics. In addition to the common evaluation metrics of state accuracy, such as root mean square error, this paper also deduces and gives the Cramér-Rao Low Bound iteration formula for nonlinear systems and analyzes the consistency of a filter by using a normalized error square.

4.1. Root Mean Square Error

Root Mean Square Error (RMSE) is widely used in performance comparison and evaluation of nonlinear filtering algorithms. It is based on the estimation error set obtained by the Monte Carlo simulation to evaluate the accuracy of the algorithm [18], defined as follows:

$$RMS{E}_x=\sqrt{\frac{1}{N_m}{\displaystyle \sum _{l=1}^{N_m}{({\widehat{x}}_{k|k}^l-x_k)}^2}}$$

(25)

where $N_m$ is the Monte Carlo simulation times, ${\widehat{x}}_{k|k}^l$ represents the estimation result of the target motion state at time $k$ in the $l$-th Monte Carlo simulation, and $x_k$ is the actual state of the target. Although this metric has a definite physical meaning, it is easily dominated by considerable error values.

4.2. Cramér-Rao Low Bound for Nonlinear Systems

For nonlinear systems, the optimal Bayesian nonlinear filter is impossible [19], so there are many approximate suboptimal algorithms. They have a theoretical optimal performance lower bound called Cramér-Rao Low Bound (CRLB). It is not only a benchmark for performance analysis, but also can be used to design the parameters of a suboptimal algorithm. It has important theoretical and application value.

Literature [20,21] provides the CRLB iterative formula for discrete nonlinear systems, but it only provides the definition containing the expected operation without the final equations. Therefore, this paper deduces the CRLB for nonlinearity systems with Gaussian white noise.

For the nonlinear system in Equation (1), the unbiased estimate of the state $x_k$ is ${\widehat{x}}_k$ and its covariance matrix is $P_k$. The matrix should satisfy:

$$P_k=E[({\widehat{x}}_k-x_k){({\widehat{x}}_k-x_k)}^T]\ge J_k^{-1}$$

(26)

where $J_k=E[-\frac{{\partial }^2\ln p(x_k,z_k)}{\partial x_k^2}]$ known as the information matrix, $J_k^{-1}$ is the CRLB at time $k$. The iteration formula is

$$J_{k+1}=D_k^{22}-D_k^{21}{(J_k+D_k^{11})}^{-1}{D}_k^{12}$$

(27)

Among them,

$$\begin{array}{l}{D}_k^{11}=E[-\frac{{\partial }^2\ln p(x_{k+1}|x_k)}{\partial x_k\partial x_k}]\\ D_k^{12}=E[-\frac{{\partial }^2\ln p(x_{k+1}|x_k)}{\partial x_k\partial x_{k+1}}]\\ D_k^{21}=E[-\frac{{\partial }^2\ln p(x_{k+1}|x_k)}{\partial x_{k+1}\partial x_k}]={[D_k^{12}]}^T\\ D_k^{22}=E[-\frac{{\partial }^2\ln p(x_{k+1}|x_k)}{\partial x_{k+1}\partial x_{k+1}}]+E[-\frac{{\partial }^2\ln p(z_{k+1}|x_{k+1})}{\partial x_{k+1}\partial x_{k+1}}]\end{array}$$

(28)

If both the system noise $v_k$ and the measured noise $w_k$ are zero-mean Gaussian white noise and their covariance matrices are $Q_k$ and $R_{k+1}$, respectively, then it can be obtained from the Formula (28):

$$\frac{\partial \ln p(x_{k+1}|x_k)}{\partial x_k}=\frac{\partial f_k^T(x_k)}{\partial x_k}{Q}_k^{-1}[x_{k+1}-f_k(x_k)]$$

(29)

$$D_k^{11}=E\{\frac{\partial f_k^T(x_k)}{\partial x_k}{Q}_k^{-1}[x_{k+1}-f_k(x_k)]{[x_{k+1}-f_k(x_k)]}^T{(Q_k^{-1})}^T{[\frac{\partial f_k^T(x_k)}{\partial x_k}]}^T\}=E[{\tilde{F}}_k^T{Q}_k^{-1}{\tilde{F}}_k]$$

(30)

$$D_k^{12}=-E[{\tilde{F}}_k^T]Q_k^{-1}={[D_k^{21}]}^T$$

(31)

$$D_k^{22}=Q_k^{-1}+E[{\tilde{H}}_{k+1}^T{R}_{k+1}^{-1}{\tilde{H}}_{k+1}]$$

(32)

Defining ${\tilde{F}}_k=\frac{\partial f_k(x_k)}{\partial x_k}$ and ${\tilde{H}}_{k+1}=\frac{\partial h_{k+1}(x_{k+1})}{\partial x_{k+1}}$. The initial information matrix $J_0$ is the inverse of the initial state covariance matrix $P_{0|0}^{}$:

$$J_0=P_{0|0}^{-1}$$

(33)

Through the above derivation process, the CRLB provides the lower bound of the mean square error of unbiased estimation and gives the mean square error of an ideal nonlinear filter. In this paper, the RMSE curves of all filters are compared with the CRLB curves of the nonlinear model. The degree of approaching the CRLB curve reflects the accuracy and performance of the corresponding algorithm.

4.3. Normalized Estimation Error Squared

Normalized Estimation Error Squared (NEES) gives a more accurate quantitative evaluation of the consistency of the filtering algorithm. Literature [22] gives a relatively standard definition: for the confidence level $\alpha (0\le \alpha \le 1)$, when the difference between the real error information and the error information calculated by the filter is not statistically significant, the filter can be considered to be consistent at the level $\alpha$. The definition is as follows:

$${\epsilon }_k=\mathrm{NEES}={(x_k-{\widehat{x}}_{k|k})}^T{P}_{k|k}^{-1}(x_k-{\widehat{x}}_{k|k})$$

(34)

When $P_{k|k}$ is equal to the actual mean square error matrix, ${\epsilon }_k$ follows the distribution ${\chi }^2$ with degree of freedom $n_x$, and $n_x$ is the state dimension. In this case, the consistency of state estimation is transformed into a ${\chi }^2$ test problem. Most of the existing filter performance evaluation is based on the Monte Carlo simulation and the Arithmetic-mean Normalized Estimation Error Square (ANEES) using $N_m$ simulation results defined as follows:

$${\zeta }_k=\mathrm{ANEES}=\frac{1}{N_m}{\displaystyle \sum _{i=1}^{N_m}\raisebox{1ex}{${\epsilon }_k^i$}\!\left/ \!\raisebox{-1ex}{$n_x$}\right.}=\frac{1}{N_m{n}_x}{\displaystyle \sum _{i=1}^{N_m}{\epsilon }_k^i}$$

(35)

5. Simulation

5.1. Problem Analysis

When the wide-band radar tracks a very long-range target, a typical problem caused by the measurement nonlinearity is the “Contact Lens” problem. It is directly related to the filter update process and is the leading cause of filter inconsistency and divergence.

The wide-band radar has high range accuracy and low angle accuracy when tracking very long-range targets. The uncertainty region of measurement will present a severe non-Gaussian distribution when the state in the Cartesian coordinate system is updated with nonlinear measurements from different coordinate systems. The uncertainty region presents a curved shape, similar to a banana shape in two dimensions and a contact lens shape in three dimensions. Therefore, this distribution is called the “Contact Lens” distribution and this kind of nonlinear problem is called the CL problem [23]. Figure 3 describes the reason for the uncertainty region of measurement from a geometric point of view. In the case of constant range accuracy, the range of cross-angle error $R{\sigma }_{\theta }^{}$ increases with the radar detection range. As it is far greater than the range error ${\sigma }_r^{}$, the uncertainty region of measurement gradually presents a curve. It should be noticed that in order to describe the distribution characteristics, the curve segment of constant thickness is used in Figure 3 to emphasize the overall curvature of the uncertainty region. However, the curvature of the actual space varies from place to place. The curvature is related to the probability of the measurement distribution at that point in the space. In Figure 4, the intensity of the color is proportional to the probability of the measurement distribution in the space.

Bar-shalom uses a parametric expression to describe the uncertainty region $CL(x;r,{\sigma }_r^2,{\sigma }_{\theta }^2)$, but he did not give the specific expression of the distribution. The bias significance metric is put forward to measure the degree of its distortion from a Gaussian distribution [24]:

$$\beta =\frac{r{\delta }_{\theta }^2}{2{\delta }_r}$$

(36)

where $r$ is the distance from the target to the radar, ${\delta }_{\theta }^2$ is the radar angle variance, and ${\delta }_r^2$ is the radar range variance. Figure 5 shows the measurement distribution characteristics corresponding to different values of $\beta$ when the target is $500 \mathrm{km}$ away from the radar. When $\beta$ is small, the measurement distribution shows a typical Gaussian distribution and the traditional filter can track targets well. However, as the value increases, the nonlinear degree of measurement gradually increases. At this time, the nonlinear filters EKF and UKF will have consistency problems, resulting in the decrease of filtering accuracy and even the problem of filter divergence. This problem can be explained by theoretical analysis.

The EKF needs to linearize the measurement equation in Equation (10). Assume $(x,y)$ is the position coordinate of the target relative to the Cartesian coordinate system with the radar as the origin. The vector $(x_0,y_0)$ is an initial unbiased estimate of $(x,y)$ with the covariance matrix $P_0$. The range and angle observation model of the radar is $r={(x^2+y^2)}^{1/2}+v_r$, $\theta =\arctan (y/x)+v_{\theta }$. Updating $(x_0,y_0)$ with the measurement $r$ and $\theta$ through the EKF, obtains a new estimate $(x_e,y_e)$ with a covariance matrix $P_e$. The EKF valuation equation is then expressed as

$$\left[\begin{array}{c}{x}_e\\ y_e\end{array}\right]=\left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]+P_e{H}_0^T{R}^{-1}\left[\begin{array}{c}r-r_0\\ \theta -{\theta }_0\end{array}\right]$$

(37)

where

$$P_e^{-1}=P_0^{-1}+H_0^T{R}^{-1}{H}_0$$

(38)

$$r_0=(\begin{array}{c}{x_0^2+y_0^2)}^{1/2}\end{array}$$

(39)

$${\theta }_0=\arctan \left(y_0/x_0\right)$$

(40)

$$H_0=\left[\begin{array}{cc}\cos {\theta }_0& \sin {\theta }_0\\ -\sin {\theta }_0/r_0& \cos 0_0/r_0\end{array}\right]$$

(41)

$$R=\left[\begin{array}{cc}{\sigma }_r^2& 0\\ 0& {\sigma }_{\theta }{}^2\end{array}\right]$$

(42)

Carrying out the matrix multiplication operation of Formula (37), we can get

$$\left[\begin{array}{c}{x}_e\\ y_e\end{array}\right]=\left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]+P_e\left[\begin{array}{c}\cos {\theta }_0\\ \sin {\theta }_0\end{array}\right][(r-r_0)/{\sigma }_r^2]+P_e\left[\begin{array}{c}-\sin {\theta }_0\\ \cos {\theta }_0\end{array}\right][(\theta -{\theta }_0)/r_0{\sigma }_{\theta }^2]$$

(43)

For high-resolution wide-band radar, $P_0^{-1}\ll R^{-1}$, so $P_0^{-1}$ can be ignored, then

$$P_e^{}\approx H_0^{-1}R{(H_0^T)}^{-1}$$

(44)

Thus,

$$\left[\begin{array}{c}{x}_e\\ y_e\end{array}\right]\approx \left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]+\left[\begin{array}{c}\cos {\theta }_0\\ \sin {\theta }_0\end{array}\right](r-r_0)+\left[\begin{array}{c}-\sin {\theta }_0\\ \cos {\theta }_0\end{array}\right]r_0(\theta -{\theta }_0)$$

(45)

From the above formula,

$$x_e=r\cos {\theta }_0-r_0\left(\theta -{\theta }_0\right)\sin {\theta }_0$$

(46)

$$y_e=r\sin {\theta }_0+r_0\left(\theta -{\theta }_0\right)\cos {\theta }_0$$

(47)

Therefore, the following formula can be obtained:

$$r_e={\left(x_e^2+y_e^2\right)}^{1/2}$$

(48)

$${\theta }_e=\arctan \left(y_e/x_e\right)$$

(49)

Assuming that $(r-r_0)/r$ can be ignored, then the estimation error caused by the linearization of the measurement equation in EKF is

$$r_e-r\approx r{[1+{(\theta -{\theta }_0)}^2]}^{1/2}-r$$

(50)

$${\theta }_e-\theta \approx -(\theta -{\theta }_0)+\arctan (\theta -{\theta }_0)$$

(51)

The above formula shows that when the target is far away from the radar, the estimation error may exceed the distance measurement error ${\sigma }_r^{}$, so the solution of the EKF will tend to diverge.

The UKF is a numerical method to calculate the statistical characteristics of random variables after nonlinear transformation. Its essence is to use multiple deterministic sampling points to approximate the Gaussian probability density function. However, in the CL problem, the probability distribution of measurement in the Cartesian coordinate system obviously cannot be approximated simply by Gaussian distribution, so the filtering accuracy of the UKF is poor. Figure 6 shows the error ellipse of the EKF and UKF estimation results which shows that the Gaussian distribution estimated by the two filters cannot cover the measurement distribution well, so the performance of the two filters is degraded or even divergent.

MSKF uses ${[x_k,{\dot{x}}_k,y_k,{\dot{y}}_k]}^{\mathrm{T}}$ as the state vector as the prediction in Cartesian coordinates. The mid-state $[r,\dot{r},\theta ,\dot{\theta }]$ is selected to make sure the update process is carried out in the measurement coordinates when updating. In the measurement space, the measurement distribution conforms to the Gaussian distribution assumption of the Kalman filter which ensures the consistency of the filter.

5.2. Simulations Results

The following two very long-range target tracking scenarios are designed for different radar performances:

(1)

$r=2700 \mathrm{km},{\delta }_r=10 \mathrm{m},{\delta }_{\theta }={0.3}^{\circ },\beta =3.7;$

(2)

$r=2700 \mathrm{km},{\delta }_r=0.1 \mathrm{m},{\delta }_{\theta }={0.3}^{\circ },\beta =371$

Figure 7 shows the measurement distribution of two simulation scenarios. Simulation scenario 1 uses narrowband radar to track very long-range targets. This is set according to the situation of missile target tracking by narrowband radar in engineering. The measurement distribution is Gaussian distribution. Simulation scenario 2 uses wide-band radar to track very long-range targets. In scenario 2, the typical CL problem occurs.

The experiment uses 100 Monte Carlo simulations, the initial target position is $x_0=1050 \mathrm{km}$, $y_0=2500 \mathrm{km}$, and the initial velocity is $v_x=200 m/s$,$v_y=300 m/s$. The traditional nonlinear filters EKF, UKF, and CKF are used for performance comparison simulation. The sampling time interval of the four filters is $\mathrm{T}=0.1 s$ and the tracking time is $80 s$. RMSE, CRLB theory, and consistency metric ANEES are used to test the filtering performance. The convergence speed of the filter is greatly related to the initial state, so the two-point starting method is adopted. The initial state and covariance are obtained by differentiating the two measurements at the first time and the second time.

Table 1. Different filtering accuracy and time consumption in scenario (1).

Filter	Range RMSE/m	Angle RMSE/°	ASETC/s
EKF	2.487	0.2224	0.0295
UKF	2.506	0.0986	0.0450
CKF	2.502	0.0266	0.0426
MSKF	2.598	0.0208	0.0691

lists the range and velocity estimation RMSE of different filters in the tracking scenario (1) when the measurement uncertainty region is Gaussian distribution. In order to compare the computational complexity, the table also lists the Average State Estimation Time Consumption (ASETC) of different filters. It can be seen from Table 1 that the range measurement accuracy of the MSKF is similar to that of the UKF and CKF, which shows that the MSKF is also suitable for nonlinear target tracking in general. The angle measurement accuracy of the CKF and MSKF is higher than the EKF and UKF. The time cost of the MSKF is slightly higher than that of the other filters, but it increases the accuracy of radar angle measurement. In

Table 2. Different filtering accuracy and time consumption in scenario (2).

Filter	Range RMSE/m	Angle RMSE/°	ASETC/s
EKF	1.6700	17.7700	0.03155
UKF	0.0956	1.2730	0.04779
CKF	0.1139	0.1977	0.04578
MSKF	0.0926	0.0781	0.07162

, the MSKF improves the angle measurement accuracy and achieves accurate tracking of a very long-range target compared with other filters. In summary, the MSKF can be used in various situations and, for special nonlinear scenarios, it can also effectively improve radar tracking accuracy.

Figure 8, Figure 9, Figure 10 and Figure 11 shows the simulation results of tracking very long-range targets with filters in scenario (2). At the time $\beta =371$, the uncertainty region of measurement is CL distribution. It can be seen from Figure 8 and Figure 9 that for very long-range targets, the measurement uncertainty region presents a severe non-Gaussian shape and the three filters except EKF can accurately track the target. In order to achieve the linear approximation of the system model, the EKF ignores the higher-order terms of Taylor’s expansion. Therefore, the EKF causes a significant error in the estimated state posterior distribution due to the truncation error. The UKF and CKF design a small number of deterministic sampling points and approximate the posterior probability of the state by calculating the statistical characteristics of these sampling points after passing through the nonlinear system so the target state is estimated accurately. For the tracking scenario (2), the angle estimation accuracy of the CKF is higher than that of the UKF. As shown in Table 2, the MSKF proposed in this paper has better filtering performance and higher target tracking accuracy than the CKF when the uncertainty region of measurement is CL distribution. Meanwhile, since the MSKF performs linear updates in the measurement space and is more affected by measures, the convergence speed of the MSKF is faster under the same two-point starting mode.

The state component estimation RMSE and theoretical performance lower bound CRLB is given in Figure 10. Due to the non-Gaussian measurement distribution, the initial RMSE of the four filters and CRLB are all large. As the iteration progresses, the UKF, CKF, and MSKF proposed in this paper gradually converge to steady-state, while the range accuracy of the EKF gradually diverges. The steady-state value indicates the approximation performance of the algorithm to nonlinear filtering. The convergence rate of RMSE represents the speed at which the algorithm finds the center of the non-Gaussian distribution. Figure 10a,b clearly show that the CKF and MSKF gradually converge to CRLB, and the MSKF has a faster convergence rate, a smaller steady-state value, and is closer to the theoretical boundary.

Figure 11 shows the ANEES distribution of the four filters. Since the consistency metric ANEES measures the difference between the real error and the estimated covariance matrix, the smaller the value, the better the consistency and the higher the robustness. It can be seen that the ANEES of EKF increases gradually, which means that the covariance matrix of the state vector increases gradually in the estimation process, eventually leading to the divergence. The ANEES of UKF gradually converges to a constant value, but its robustness is poor which leads to a significant estimation error. However, the ANEES of the CKF and MSKF gradually converge to achieve high-precision and stable target tracking. The MSKF has better state covariance estimation and higher robustness.

6. Conclusions

A typical nonlinear CL problem occurs when high-resolution wide-band radar tracks very long-range targets. To solve this problem, a novel mid-state Kalman filter is proposed in this paper. The MSKF uses the observed state and its first state derivative as the mid-state vector to transform the update process of the filter into the measurement space and rederives the update equation of the Kalman filter. The state transition module can be replaced according to the actual situation without modifying the filter prediction and update module. This improves the practical engineering applicability of the MSKF. When the CL problem occurs, the MSKF has the characteristics of high state estimation accuracy and fast filter convergence compared with other filters. This will significantly help the wide-band radar system to form the target trajectory quickly and accurately.