A practical adaptive nonlinear tracking algorithm with range rate measurement

Abstract

It is difficult to answer the problem whether the range rate measurement should be adopted to track a target in a tracking scenario. A practical adaptive nonlinear tracking algorithm with the range rate measurement is proposed, which avoids this problem and achieves good accuracy of target state estimation. First, three popular nonlinear filtering algorithms only with the position measurement are surveyed. Second, three popular nonlinear filtering algorithms with the position and range rate measurements are surveyed. Then, a novel tracking algorithm with range rate measurement is proposed based on the cumulative sum detector and the above two kinds of nonlinear algorithms. The results of simulation experiment demonstrate that the range rate measurement could reduce accuracy of the target state estimation in mismatch tracking scenarios. The results of simulation experiment also verify that the performance of proposed algorithm is better than the current state and the art interacting multiple-model algorithm and can well follow the state estimation output of the measurement equation matching the tracking scenario.

Introduction

Target tracking algorithm with the range and bearing measurements from the radar is widely applied in practice. Normally, because of the range and bearing measurements in the radar coordinates and the target state in the Cartesian coordinates, the target tracking algorithm is nonlinear. At present, many nonlinear filtering algorithms are applied to solve this problem, such as the extended Kalman filter (EKF) algorithm, ¹ the unscented Kalman filter (UKF) algorithm, ² the cubature Kalman filter (CKF) algorithm, ³ the unbiased converted measurement Kalman filter (UCMKF) algorithm, ⁴ and the decorrelated unbiased converted measurement Kalman filter (DUCMKF) algorithm. ⁵ The EKF first widely used in this tracking problem uses the Taylor expansion of the measurement function to linearly approximate the nonlinear measurement function. If the nonlinearity of measurement function is strong, the effect of linear approximation is not ideal. The UKF and the CKF can be classified as the generalized Gaussian filtering. ⁶ The algorithm assumes that the state estimation and prediction obey the Gaussian distribution, and the numerical integration method is used to compute the mean and the covariance of the assumed Gaussian density function. Compared with the EKF, the UKF and the CKF have the advantages of not requiring differential operation or calculating the Jacobian matrix, but the disadvantage is that more computational steps are needed. The UCMKF and the DUCMKF are a class of measurement conversion algorithm whose idea is first to convert the raw radar measurement into Cartesian coordinate system before filtering and then use the linear Kalman filter (KF). ⁷ Lerro and Bar-Shalom ⁴ claim that the performance of the UCMKF is better than that of the EKF in the mixed coordinate system. On the basis of the UCMKF, the DUCMKF replaces the raw measurement from the radar with the state prediction from the filtering to eliminate the correlation between the measurement error covariance estimation and the measurement noise and then the estimation bias is reduced. However, if the state estimation error is large or the model mismatch occurs, the new prediction bias will be introduced due to the use of the state prediction.

In addition to range and bearing measurements called position measurement, the Doppler radar can also obtain the range rate measurement. Recently, the range rate measurement is used to estimate the parameters of target motion, because the range rate measurement includes the velocity information. An estimation method of turn rate is proposed by combining the range rate measurement and the coordinate point measurement for a fast curvilinear target. ⁸ In the passive location problem, the high-precision range rate measurement from single satellite using third-order Taylor expansion ensures the accuracy of location. ⁹ Harris and Whitcomb ¹⁰ report an observation model for range rate measurement with a delayed-state EKF to estimate the state of underwater vehicles in the communication and navigation. The modified approach of probability weight in the interacting multiple-model (IMM) is developed by the normal acceleration statistics derived from the range rate. ¹¹ In a multistatic radar, range and range rate measurement from multiple bistatic radars are adopted to determine the targets velocity and location. ¹² The kinematics and geometry relationships between range rate and target state have been developed in many applications gradually. In the above works, only basic nonlinear filters, such as the EKF, the UCMKF and the CKF, are used for the nonlinear estimation with range rate measurement. The strong nonlinear relationship between the state and the range rate measurement is ignored. Specially, the results of target tracking with the position and range rate measurements obtained by the EKF, the UKF, the CKF, and the UCMKF directly have potential inconsistencies and deviations. ¹³

In recent years, Lei and Han, ¹⁴ Jiao et al., ¹⁵ and Bordonaro et al. ¹⁶ propose several filtering algorithms with range rate measurement to reduce the stronger nonlinearity of the measurement equation containing the position and range rate measurement. The sequential unscented Kalman filter (SUKF) algorithm, the sequential extended Kalman filter (SEKF) algorithm and the converted measurement Kalman filter with range rate (CMKFRR) algorithm are accepted for the better stability and higher accuracy of state estimation than others. The SUKF ¹⁴ converts the position measurement into Cartesian coordinates and remains the raw range rate in the nonlinear measurement equation. Then, perform the KF for the converted position measurement and perform the UKF for the raw range rate measurement sequentially. The SEKF ¹⁵ converts the position measurement into Cartesian coordinate system and replaces the raw measurement with the pseudo-measurement that is the product of range and range rate in order to reduce the nonlinearity of the measurement equation. Then, perform the KF for the converted position measurement and perform the second-order EKF for the pseudo-measurement sequentially. The sequential filtering methods with the conversion measurement algorithm have better tracking performance than those of standard UKF and EKF. Bordonaro et al. ¹⁶ propose the CMKFRR that converts the range, bearing, and range rate measurements to the position and velocity in Cartesian coordinate system. However, the CMKFRR uses the cross-range rate, which needs the prior knowledge to model the expected probability distribution of cross-range rate measurement because the cross-range rate cannot be observed.

Although the above SUKF, SEKF, and CMKFRR can effectively reduce the nonlinearity of target tracking with the range rate measurement, it is not feasible to use the range rate measurement in any tracking scenario to improve accuracy of target state estimation. In other words, in some scenarios, the position measurement can be used to achieve the higher accuracy of target state estimation than the position and range rate measurements; in other scenarios, the position and range rate measurement are better. In order to obtain better accuracy of state estimation, it is necessary to distinguish where the position measurement should be used and where the position and range rate measurements should be used. However, this boundary is difficult to determine, because the accuracy of state estimation is influenced not only by the nonlinear measurement equation, also by the true movement of target, measurement noise, filtering algorithm, target motion model, and so on. In order to avoid the decrease in the target tracking performance due to the improper use of range rate measurement, a practical adaptive tracking algorithm with the range rate measurement based on the cumulative sum (CUSUM) detector is proposed in this article to achieve the good accuracy of target state estimation, which is close to the best performance of tracking algorithm with single measurement equation in different scenarios.

The content of this article is designed as follows. In section “Formulation of the problem,” the background of the problem is described and the measurement equation containing the position measurement and the measurement equation containing the position and range rate measurements are described. In section “Nonlinear tracking algorithms with the position measurement,” the popular nonlinear tracking algorithms only with the position measurement, namely, the UKF, the UCMKF, and the DUCMKF, are surveyed. In section “Nonlinear tracking algorithms with the position and range rate measurement,” the popular nonlinear tracking algorithms with the position and range rate measurements, namely, the SUKF, the SEKF, and the CMKFRR, are surveyed and the performance of the algorithms described in section “Nonlinear tracking algorithms with the position measurement” and the algorithms described in this section are compared by the simulation experiment. In section “A practical adaptive nonlinear tracking algorithm with the range rate measurement,” a practical adaptive nonlinear tracking algorithm with the range rate measurement is proposed and its performance is verified. In section “Conclusion and future works,” the conclusions and future works are given.

Formulation of the problem

For a target in two-dimensional (2D) plane, a coordinate decoupled dynamic model of the target is described in Cartesian coordinate system

$${\mathit{X}}_{k+1}={\mathbf{\Phi }}_k{\mathit{X}}_k+{\mathbf{\Gamma }}_k{\mathit{W}}_k$$

(1)

where ${\mathit{X}}_k=[x_k,y_k,{\stackrel{·}{x}}_k,{\stackrel{·}{y}}_k,s_{1\times (n-4)}]\prime$ is the target motion state at time $k$ ; $x_k$ , and $y_k$ are the target position components; ${\stackrel{·}{x}}_k$ and ${\stackrel{·}{y}}_k$ are the target velocity components; $s_{1\times (n-4)}$ is the other components, for example, the target acceleration components and jerk components; ${\mathbf{\Phi }}_k\in {\mathit{R}}^{n\times n}$ is the state transition matrix; ${\mathbf{\Gamma }}_k$ is the coefficient matrix of the corresponding dimension; and ${\mathit{W}}_k$ is the zero mean white Gauss process noise with the covariance matrix ${\mathit{Q}}_k$ .

When the position measurement only is adopted to track a target, the measurement vector is denoted as ${\mathit{Z}}_k^P=[r_k,{\theta }_k]\prime$ . Assuming the radar is located at the origin of the coordinate system, the measurement equation is described as

$${\mathit{Z}}_k^P={\mathit{h}}_k^P\left({\mathit{X}}_k\right)+{\mathit{V}}_k^P$$

(2)

where ${\mathit{h}}_k^P({\mathit{X}}_k)$ is the measurement function only with the position measurement

$${\mathit{h}}_k^P\left({\mathit{X}}_k\right)=\left[\begin{array}{c}\sqrt{x_k^2+y_k^2}\hfill \\ \multicolumn{1}{c}{\arctan (y_k/x_k)}\end{array}\right]$$

(3)

where $r_k$ is the range measurement; ${\theta }_k$ is the bearing measurement; ${\mathit{V}}_k^P$ is the zero mean white Gauss measurement noise vector with the covariance matrix

$${\mathit{R}}^P=\left[\begin{array}{cc}{\sigma }_r^2\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& {\sigma }_{\theta }^2\hfill \end{array}\right]$$

(4)

where ${\sigma }_r$ and ${\sigma }_{\theta }$ are the standard deviations of range and bearing measurements.

When the position and range rate measurement are adopted to track a target, the measurement vector is denoted as ${\mathit{Z}}_k^D=[r_k,{\theta }_k,{\stackrel{·}{r}}_k]\prime$ . The corresponding measurement equation is denoted as

$${\mathit{Z}}_k^D={\mathit{h}}_k^D\left({\mathit{X}}_k\right)+{\mathit{V}}_k^D$$

(5)

where ${\mathit{h}}_k^D({\mathit{X}}_k)$ is the measurement function with the position and range rate measurements

$${\mathit{h}}_k^D\left({\mathit{X}}_k\right)=\left[\begin{array}{c}\sqrt{x_k^2+y_k^2}\hfill \\ \multicolumn{1}{c}{\arctan (y_k^2/x_k)}\\ \multicolumn{1}{c}{\frac{x_k{\stackrel{·}{x}}_k+y_k{\stackrel{·}{y}}_k}{\sqrt{x_k^2+y_k^2}}}\end{array}\right]$$

(6)

where ${\stackrel{·}{r}}_k$ is the range rate measurement; ${\mathit{V}}_k^D$ is the zero mean white Gauss measurement noise vector with the covariance matrix

$${\mathit{R}}^D=\left[\begin{array}{ccc}{\sigma }_r^2\hfill & 0\hfill & \rho {\sigma }_r{\sigma }_{\stackrel{·}{r}}\hfill \\ \multicolumn{1}{c}{0}& {\sigma }_{\theta }^2\hfill & 0\hfill \\ \multicolumn{1}{c}{\rho {\sigma }_r{\sigma }_{\stackrel{·}{r}}}& 0\hfill & {\sigma }_{\stackrel{·}{r}}^2\hfill \end{array}\right]$$

(7)

where ${\sigma }_{\stackrel{·}{r}}$ is the standard deviation of the range rate measurement; $\rho$ is the correlation coefficient between the range rate measurement and the range measurement. ¹⁷ It can be seen from formulas (3) and (6) that ${\mathit{h}}_k^D({\mathit{X}}_k)$ has the stronger nonlinearity than ${\mathit{h}}_k^P({\mathit{X}}_k)$ .

Nonlinear tracking algorithms with the position measurement

When the position measurement is used to tracking a target, the target state equation and measurement equation are formulas (1) and (2), respectively. For the target tracking problem, the current popular methods are the UKF, the UCMKF, and the DUCMKF.

The UKF algorithm

The UKF is an approximate filtering algorithm based on the unscented transformation. ¹⁸ The filtering distribution is assumed to be a Gaussian distribution in the UKF

$$p({\mathit{X}}_k|{\mathit{Z}}_{1:k}^P)\approx \mathrm{N}({\mathit{X}}_k|{\mathit{m}}_k,{\mathit{P}}_k)$$

(8)

where ${\mathit{m}}_k$ and ${\mathit{P}}_k$ are the state estimation and the state estimation covariance. The UKF is as follow

Prediction

1. Form the sigma points

   $$\begin{array}{c}{\mathbf{\chi }}_{k-1}^{(0)}={\mathit{m}}_{k-1}\hfill \\ {\mathbf{\chi }}_{k-1}^{(i)}={\mathit{m}}_{k-1}+\sqrt{n+\lambda }{\left[\sqrt{{\mathit{P}}_{k-1}}\right]}_i\hfill \\ {\mathbf{\chi }}_{k-1}^{(i+n)}={\mathit{m}}_{k-1}-\sqrt{n+\lambda }{\left[\sqrt{{\mathit{P}}_{k-1}}\right]}_i\hfill \\ i=1,\dots ,n\hfill \end{array}$$

   (9)

   where $\lambda ={\alpha }^2(n+\kappa )-n$ ; the parameters $\alpha$ and $\kappa$ determine the coverage area of sigma points.

2. Propagate the sigma points through the state transform matrix

   $${\hat{\mathbf{\chi }}}_k^{(i)}={\mathbf{\Phi }}_{k-1}{\mathbf{\chi }}_{k-1}^{(i)},i=0,\dots ,2n$$

   (10)

3. Compute the predicted mean ${\mathit{m}}_k^{-}$ and the predicted covariance ${\mathit{P}}_k^{-}$

   $$\begin{array}{c}{\mathit{m}}_k^{-}=\sum _{i=0}^{2n}{W}_i^{(\mathrm{m})}{\hat{\mathbf{\chi }}}_k^{(i)}\hfill \\ {\mathit{P}}_k^{-}=\sum _{i=0}^{2n}{W}_i^{(\mathrm{c})}({\hat{\mathbf{\chi }}}_k^{(i)}-{\mathit{m}}_k^{-})({\hat{\mathbf{\chi }}}_k^{(i)}-{\mathit{m}}_k^{-})\prime +{\mathit{Q}}_{k-1}\hfill \end{array}$$

   (11)

where the weights $W_i^{(\mathrm{m})}$ and $W_i^{(\mathrm{c})}$ are calculated by

$$\begin{array}{c}{W}_0^{(m)}=\frac{\lambda }{n+\lambda }\hfill \\ W_0^{(\mathrm{c})}=\frac{\lambda }{n+\lambda }+(1-{\alpha }^2+\beta )\hfill \\ W_i^{(\mathrm{m})}=W_i^{(\mathrm{c})}=\frac{\lambda }{2(n+\lambda )},i=1,\dots ,2n\hfill \end{array}$$

(12)

where $\beta$ is an additional algorithm parameter that can be used for incorporating prior information.

Update

1. Form the sigma points

   $$\begin{array}{c}{\mathbf{\chi }}_k^{-(0)}={\mathit{m}}_k^{-}\hfill \\ {\mathbf{\chi }}_k^{-(i)}={\mathit{m}}_k^{-}+\sqrt{n+\lambda }{\left[\sqrt{{\mathit{P}}_k^{-}}\right]}_i\hfill \\ {\mathbf{\chi }}_k^{-(i+n)}={\mathit{m}}_k^{-}-\sqrt{n+\lambda }{\left[\sqrt{{\mathit{P}}_k^{-}}\right]}_i\hfill \\ i=1,\dots ,n\hfill \end{array}$$

   (13)

2. Propagate sigma points through the measurement function

   $${\hat{\mathbf{\xi }}}_k^{(i)}={\mathit{h}}_k^P\left({\mathbf{\chi }}_k^{-(i)}\right),i=0,\dots ,2n$$

   (14)

3. Compute the predicted mean ${\mathbf{\mu }}_k$ , the predicted covariance of the measurement ${\mathit{S}}_k$ , and the cross-covariance of the state and the measurement ${\mathit{C}}_k$

   $$\begin{array}{c}{\mathbf{\mu }}_k=\sum _{i=0}^{2n}{W}_i^{(\mathrm{m})}{\hat{\mathbf{\xi }}}_k^{(i)}\hfill \\ {\mathit{S}}_k=\sum _{i=0}^{2n}{W}_i^{(\mathrm{c})}({\hat{\mathbf{\xi }}}_k^{(i)}-{\mathbf{\mu }}_k)({\hat{\mathbf{\xi }}}_k^{(i)}-{\mathbf{\mu }}_k)\prime +{\mathit{R}}^P\hfill \\ {\mathit{C}}_k=\sum _{i=0}^{2n}{W}_i^{(\mathrm{c})}({\hat{\mathbf{\chi }}}_k^{-(i)}-{\mathit{m}}_k^{-})({\hat{\mathbf{\xi }}}_k^{(i)}-{\mathbf{\mu }}_k)\prime \hfill \end{array}$$

   (15)

4. Compute the filter gain ${\mathit{K}}_k$ , the filtered state mean ${\mathit{m}}_k$ , and the covariance, conditional on the measurement ${\mathit{P}}_k$

   $$\begin{array}{c}{\mathit{K}}_k={\mathit{C}}_k{\mathit{S}}_k^{-1}\hfill \\ {\mathit{m}}_k={\mathit{m}}_k^{-}+{\mathit{K}}_k[{\mathit{Z}}_k^P-{\mathbf{\mu }}_k]\hfill \\ {\mathit{P}}_k={\mathit{P}}_k^{-}-{\mathit{K}}_k{\mathit{S}}_k{\mathit{K}\prime }_k\hfill \end{array}$$

   (16)

The advantage of the UKF over the EKF is that the UKF is not based on a linear approximation at a single point, but uses further points in approximating the nonlinearity. The unscented transform is able to capture the higher order moments caused by the nonlinear transform better than Taylor series–based approximations. ¹⁹ The UKF can be classified as a special kind of Gaussian filtering, which includes the Gauss–Hermite Kalman filter and the CKF. The UKF performance is close to the Gauss–Hermite Kalman filter and the CKF. Although the mean estimate of the UKF is exact for polynomials up to third order, the covariance computation is only exact for polynomials up to the first order. ¹⁹

The UCMKF algorithm

The conversion measurement Kalman filter (CMKF) algorithm is a common tracking algorithm for the target state in Cartesian coordinate system and the measurement in the polar coordinate system. The CMKF has two bias sources. The first source is the conversion bias. If the bias conversion measurement is adopted in the KF, it will lead to performance degradation. The UCMKF is proposed in Longbin et al. ²⁰ to eliminate the bias by multiplicative compensation. The UCMKF with multiplicative compensation is as follows

$${\mathit{Z}}_k^{\mathrm{uP}}=\left[\begin{array}{c}{x}_k^u\hfill \\ \multicolumn{1}{c}{y_k^u}\end{array}\right]=\left[\begin{array}{c}{\lambda }_{\theta }^{-1}{r}_k\cos \left({\theta }_k\right)\hfill \\ \multicolumn{1}{c}{{\lambda }_{\theta }^{-1}{r}_k\sin \left({\theta }_k\right)}\end{array}\right],{\lambda }_{\theta }=e^{-{\sigma }_{\theta }^2/2}$$

(17)

The corresponding conversion measurement covariance matrix is

$${\mathit{R}}_u^P=\left[\begin{array}{cc}{R}_{u11}^P\hfill & R_{u12}^P\hfill \\ \multicolumn{1}{c}{R_{u21}^P}& R_{u22}^P\hfill \end{array}\right]$$

(18)

$$\begin{array}{cc}\multicolumn{1}{c}{R_{u11}^P=}& ({\lambda }_{\theta }^{-2}-2)r_k^2{\cos }^2{\theta }_k+\frac{1}{2}(r_k^2+{\sigma }_r^2)(1+{\lambda \prime }_{\theta }\cos 2{\theta }_k)\hfill \\ \multicolumn{1}{c}{R_{u12}^P=}& R_{u21}^P=({\lambda }_{\theta }^{-2}-2)r_k^2\sin {\theta }_k\cos {\theta }_k+\frac{1}{2}(r_k^2+{\sigma }_r^2){\lambda \prime }_{\theta }\sin 2{\theta }_k\hfill \\ \multicolumn{1}{c}{R_{u22}^P=}& ({\lambda }_{\theta }^{-2}-2)r_k^2{\sin }^2{\theta }_k\hfill \\ \multicolumn{1}{c}{}& +\frac{1}{2}(r_k^2+{\sigma }_r^2)(1-{\lambda \prime }_{\theta }\cos 2{\theta }_k),{\lambda \prime }_{\theta }=e^{-2{\sigma }_{\theta }^2}\hfill \end{array}$$

(18a)

The converted measurement ${\mathit{Z}}_k^{\mathrm{uP}}$ and the covariance matrix ${\mathit{R}}_u^P$ are combined with the KF to obtain the UCMKF. Although the UCMKF eliminates the conversion bias, the conversion measurement covariance matrix and measurement noise are correlated in formula (18a), which can lead to the second bias source called estimation bias.

The DUCMKF algorithm

The DUCMKF based on the UCMKF replaces the current measurement with the one-step state prediction to eliminate the correlation between the conversion measurement covariance matrix and measurement noise and then the estimation bias is reduced. Like the UCMKF, formula (17) is used to perform the unbiased converted measurement. The range and bearing measurement predictions are denoted as $r_{k+1|k}$ and ${\theta }_{k+1|k}$ at time $k+1$ , and their standard deviations are denoted as ${\sigma }_{r,k+1}$ and ${\sigma }_{\theta ,k+1}$ . The state prediction is denoted as ${\hat{\mathit{X}}}_{k+1|k}=[x_{k+1|k},y_{k+1|k},{\stackrel{·}{x}}_{k+1|k},{\stackrel{·}{y}}_{k+1|k}]\prime$ at time $k+1$ , and the state prediction covariance is denoted as ${\mathit{P}}_{k+1|k}^P$ . Then

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

(19)

The variables ${\sigma }_{r,k+1}$ and ${\sigma }_{\theta ,k+1}$ are calculated by

$$\begin{array}{c}{\sigma }_{r,k+1}^2=\frac{1}{r_{k+1|k}^2}\left[\begin{array}{cc}{x}_{k+1|k}\hfill & y_{k+1|k}\hfill \end{array}\right]{\mathit{P}}_{k+1|k}^P\left[\begin{array}{cc}{x}_{k+1|k}\hfill & y_{k+1|k}\hfill \end{array}\right]\prime \hfill \\ {\sigma }_{\theta ,k+1}^2=\frac{1}{r_{k+1|k}^4}\left[\begin{array}{cc}-y_{k+1|k}\hfill & x_{k+1|k}\hfill \end{array}\right]{\mathit{P}}_{k+1|k}^P\left[\begin{array}{cc}-y_{k+1|k}\hfill & x_{k+1|k}\hfill \end{array}\right]\prime \hfill \end{array}$$

(20)

The decorrelated conversion measurement error covariance matrix is denoted as

$${\mathit{R}}_{\mathrm{du}}^P=\left[\begin{array}{cc}{R}_{\mathrm{du}11}^P\hfill & R_{\mathrm{du}12}^P\hfill \\ \multicolumn{1}{c}{R_{\mathrm{du}21}^P}& R_{\mathrm{du}22}^P\hfill \end{array}\right]$$

(21)

$$\begin{array}{cc}\multicolumn{1}{c}{R_{\mathrm{du}11}^P=}& \frac{1}{2}(r_{k+1|k}^2+{\sigma }_r^2+{\sigma }_{k+1|k}^2)\hfill \\ \multicolumn{1}{c}{}& [1+\cos \left(2{\theta }_{k+1|k}\right)e^{-2{\sigma }_{\theta }^2}{e}^{-2{\sigma }_{\theta ,k+1|k}^2}]e^{{\sigma }_{\theta }^2}\hfill \\ \multicolumn{1}{c}{}& -\frac{1}{2}(r_{k+1|k}^2+{\sigma }_{r,k+1|k}^2)[1+\cos \left(2{\theta }_{k+1|k}\right)e^{-2{\sigma }_{\theta ,k+1|k}^2}]\hfill \\ \multicolumn{1}{c}{R_{\mathrm{du}12}^P=}& R_{\mathrm{du}21}^P=\frac{1}{2}(r_{k+1|k}^2+{\sigma }_r^2+{\sigma }_{k+1|k}^2)\hfill \\ \multicolumn{1}{c}{}& \left[\sin \left(2{\theta }_{k+1|k}\right)e^{-2{\sigma }_{\theta }^2}{e}^{-2{\sigma }_{\theta ,k+1|k}^2}\right]e^{{\sigma }_{\theta }^2}\hfill \\ \multicolumn{1}{c}{}& -\frac{1}{2}(r_{k+1|k}^2+{\sigma }_{r,k+1|k}^2)\left[\sin \left(2{\theta }_{k+1|k}\right)e^{-2{\sigma }_{\theta ,k+1|k}^2}\right]\hfill \\ \multicolumn{1}{c}{R_{\mathrm{du}22}^P=}& \frac{1}{2}(r_{k+1|k}^2+{\sigma }_r^2+{\sigma }_{k+1|k}^2)\hfill \\ \multicolumn{1}{c}{}& [1-\cos \left(2{\theta }_{k+1|k}\right)e^{-2{\sigma }_{\theta }^2}{e}^{-2{\sigma }_{\theta ,k+1|k}^2}]e^{{\sigma }_{\theta }^2}\hfill \\ \multicolumn{1}{c}{}& -\frac{1}{2}(r_{k+1|k}^2+{\sigma }_{r,k+1|k}^2)[1-\cos \left(2{\theta }_{k+1|k}\right)e^{-2{\sigma }_{\theta ,k+1|k}^2}]\hfill \end{array}$$

(21a)

The DUCMKF can be obtained through combining the converted measurement ${\mathit{Z}}_k^{\mathrm{uP}}$ and the covariance matrix ${\mathit{R}}_{\mathrm{du}}^P$ with the KF. Although the DUCMKF avoids the correlation between the measurement error covariance matrix and the measurement noise, the state prediction error is also introduced. When the state predicted error is larger than the measurement error, the performance of the DUCMKF is not better than that of the UCMKF algorithm. This may occur when the measurement error is small or the motion model does not match the true movement of target.

Nonlinear tracking algorithms with the position and range rate measurement

When the position and range rate measurements are used to tracking a target, the target state equation and measurement equation are formulas (1) and (5), respectively. For the target tracking problem, the current popular methods are the SEKF, the SUKF, and the CMKFRR.

The SEKF algorithm

The SEKF replaces the raw range rate ${\stackrel{·}{r}}_k$ with the pseudo-measurement $r_k{\stackrel{·}{r}}_k$ . The KF is applied to the converted position measurement, and the second-order EKF is used for the pseudo-measurement sequentially. The steps of the SEKF are as follows.

Conversion measurement

Apply formula (17) to convert the position measurement, and the conversion formula of pseudo-measurement is

$${\xi }_k=r_k{\stackrel{·}{r}}_k=x\stackrel{·}{x}+y\stackrel{·}{y}-\rho {\sigma }_r{\sigma }_{\stackrel{·}{r}}$$

(22)

The corresponding covariance matrix of conversion measurement is

$${\mathit{R}}_{\mathrm{pu}}^D=\left[\begin{array}{ccc}{R}_{u11}^P\hfill & R_{u12}^P\hfill & R_{\mathrm{pu}13}^D\hfill \\ \multicolumn{1}{c}{R_{u21}^P}& R_{u22}^P\hfill & R_{\mathrm{pu}23}^D\hfill \\ \multicolumn{1}{c}{R_{\mathrm{pu}31}^D}& R_{\mathrm{pu}32}^D\hfill & R_{\mathrm{pu}33}^D\hfill \end{array}\right]$$

(23)

$$\begin{array}{c}{R}_{\mathrm{pu}13}^D=R_{\mathrm{pu}31}^D=({\sigma }_r^2{\stackrel{·}{r}}_k+r_k\rho {\sigma }_r{\sigma }_{\stackrel{·}{r}})\cos {\theta }_k{e}^{-{\sigma }_{\theta }^2}\hfill \\ R_{\mathrm{pu}32}^D=R_{\mathrm{pu}32}^D=({\sigma }_r^2{\stackrel{·}{r}}_k+r_k\rho {\sigma }_r{\sigma }_{\stackrel{·}{r}})\sin {\theta }_k{e}^{-{\sigma }_{\theta }^2}\hfill \\ R_{\mathrm{pu}33}^D=r_k^2{\sigma }_{\stackrel{·}{r}}^2+{\sigma }_r^2{\stackrel{·}{r}}_k^2+3(1+{\rho }^2){\sigma }_r^2{\sigma }_{\stackrel{·}{r}}^2+r_k{\stackrel{·}{r}}_k\rho {\sigma }_r{\sigma }_{\stackrel{·}{r}}\hfill \end{array}$$

(23a)

The decoupled method in Jiao et al. ¹⁵ is used to decouple the position and the pseudo-measurements.

Sequential filtering

1. Time update filtering estimation

   $$\begin{array}{c}{\mathit{m}}_k^{-P}={\mathbf{\Phi }}_{k-1}{\mathit{m}}_{k-1}\hfill \\ {\mathit{P}}_k^{-P}={\mathbf{\Phi }}_{k-1}{\mathit{P}}_{k-1}{\mathbf{\Phi }\prime }_{k-1}\hfill \end{array}$$

   (24)

2. Position measurement updating filtering estimation

   $$\begin{array}{c}{\mathit{H}}^p=\left[\begin{array}{cccc}1\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 1\hfill & 0\hfill & 0\hfill \end{array}\right]\hfill \\ {\mathit{K}}_k^p={\mathit{P}}_k^{-p}{\mathit{H}}^p\prime {[{\mathit{H}}^p{\mathit{P}}_k^{-p}{\mathit{H}}^p\prime +{\mathit{R}}_u^P]}^{-1}\hfill \\ {\mathit{m}}_k^P={\mathit{m}}_k^{-P}+{\mathit{K}}_k^p[{\mathit{Z}}_k^{\mathrm{uP}}-{\mathit{H}}^p{\mathit{m}}_k^{-P}]\hfill \\ {\mathit{P}}_k^p=({\mathit{I}}_4-{\mathit{K}}_k^p{\mathit{H}}^p){\mathit{P}}_k^{-p}\hfill \end{array}$$

   (25)

3. Pseudo-measurement updating filtering estimation

   $$\begin{array}{c}{\mathit{H}}^{\epsilon }=[L_1+{\stackrel{·}{x}}_k^p,L_2+{\stackrel{·}{y}}_k^p,x_k^p,y_k^p]\prime \hfill \\ {\delta }_k^2=2{\mathit{P}}_k^p(1,3)+2{\mathit{P}}_k^p(2,4)\hfill \\ A_k={\mathit{P}}_k^p(1,1){\mathit{P}}_k^p(3,3)+{\mathit{P}}_k^p(2,2){\mathit{P}}_k^p(4,4)+2{\mathit{P}}_k^p(1,2){\mathit{P}}_k^p(3,4)\hfill \\ +2{\mathit{P}}_k^p(1,4){\mathit{P}}_k^p(2,3)+{\mathit{P}}_k^p{(1,3)}^2+{\mathit{P}}_k^p{(2,4)}^2\hfill \\ {\mathit{R}}_{\mathrm{pu}}^{\xi }=[R_{\mathrm{pu}31}^D,R_{\mathrm{pu}32}^D]\hfill \\ {\mathit{K}}_k^{\epsilon }={\mathit{P}}_k^p{\mathit{H}}^{\epsilon }\prime {({\mathit{H}}^{\epsilon }{\mathit{P}}_k^p{\mathit{H}}^{\epsilon }\prime +R_{\mathrm{pu}33}^D-{\mathit{R}}_{\mathrm{pu}}^{\xi }{\left({\mathit{R}}_u^P\right)}^{-1}{\mathit{R}}_{\mathrm{pu}}^{\xi }\prime +A_k)}^{-1}\hfill \\ {\mathit{m}}_k={\mathit{m}}_k^P+{\mathit{K}}_k^{\epsilon }({\epsilon }_k-L_1{x}_k^p-L_2{y}_k^p-x_k^p{\stackrel{·}{x}}_k^p-y_k^p{\stackrel{·}{y}}_k^p-\frac{1}{2}{\delta }_k^2)\hfill \\ {\mathit{P}}_k=({\mathit{I}}_4-{\mathit{K}}_k^{\epsilon }{\mathit{H}}^{\epsilon }){\mathit{P}}_k^p\hfill \end{array}$$

   (26)

   where $[L_1,L_2]=-{\mathit{R}}_{\mathrm{pu}}^{\xi }({\mathit{R}}_u^P{)}^{-1}$ and ${\epsilon }_k=L_1{x}_k^u+L_2{y}_k^u+{\xi }_k$ .

The SEKF adopts the pseudo-measurement and indeed reduces the nonlinearity of the original measurement function. However, if the range is far and the range rate is large, the multiplication of them will give a high gain to the pseudo-measurement noise, which will lead to the decrease in the tracking performance and even the divergence.

The SUKF algorithm

The SUKF directly adopts the raw range rate measurement to perform the sequential filter. First, the KF is used for the converted position measurement and then the UKF is used for the raw range rate measurement. The steps of SUKF are as follows.

Conversion measurement

Apply formula (17) to convert the position measurement, and remain the raw range rate measurement, that is, ${\stackrel{·}{r}}_k={\stackrel{·}{r}}_k$ . The corresponding covariance matrix is

$${\mathit{R}}_{\mathrm{ru}}^D=\left[\begin{array}{ccc}{R}_{u11}^P\hfill & R_{u12}^P\hfill & R_{\mathrm{ru}13}^D\hfill \\ \multicolumn{1}{c}{R_{u21}^P}& R_{u22}^P\hfill & R_{\mathrm{ru}23}^D\hfill \\ \multicolumn{1}{c}{R_{\mathrm{ru}31}^D}& R_{\mathrm{ru}32}^D\hfill & R_{\mathrm{ru}33}^D\hfill \end{array}\right]$$

(27)

$$\begin{array}{c}{R}_{\mathrm{ru}13}^D=R_{\mathrm{ru}31}^D={\lambda }_{\theta }\rho {\sigma }_r{\sigma }_{\stackrel{·}{r}}\cos {\theta }_k\hfill \\ R_{\mathrm{ru}32}^D=R_{\mathrm{ru}32}^D={\lambda }_{\theta }\rho {\sigma }_r{\sigma }_{\stackrel{·}{r}}\sin {\theta }_k\hfill \\ R_{\mathrm{ru}33}^D={\sigma }_{\stackrel{·}{r}}^2\hfill \end{array}$$

(27a)

In order to do sequential processing, it is necessary to decouple the position measurement and the range rate measurement, and the decoupling method is the same as the SEKF.

Sequential filtering

1. Time update filtering estimation with formula (24).

2. Position measurement updating filtering estimation with formula (25).

3. Range rate measurement updating filtering estimation.

Form the sigma points

$$\begin{array}{c}{\mathbf{\gamma }}_k^{(0)}={\mathit{m}}_k^P\hfill \\ {\mathbf{\gamma }}_k^{(i)}={\mathit{m}}_k^P+\sqrt{n+\lambda }{\left[\sqrt{{\mathit{P}}_k^p}\right]}_i\hfill \\ {\mathbf{\gamma }}_k^{(i+n)}={\mathit{m}}_k^P-\sqrt{n+\lambda }{\left[\sqrt{{\mathit{P}}_k^p}\right]}_i,i=1,\dots ,n\hfill \end{array}$$

(28)

Calculate the mean ${\overline{\mathit{m}}}_k$ and the covariance ${\overline{\mathit{P}}}_k$

$$\begin{array}{c}{\overline{\mathit{m}}}_k=\sum _{i=0}^{2n}{W}_i^{(\mathrm{m})}{\mathbf{\gamma }}_k^{(i)}\hfill \\ {\overline{\mathit{P}}}_k=\sum _{i=0}^{2n}{W}_i^{(\mathrm{c})}({\mathbf{\gamma }}_k^{(i)}-{\overline{\mathit{m}}}_k)({\mathbf{\gamma }}_k^{(i)}-{\overline{\mathit{m}}}_k)\prime \hfill \end{array}$$

(29)

where the weights $W_i^{(\mathrm{m})}$ and $W_i^{(\mathrm{c})}$ are calculated by formula (12). Substitute the sigma points into the measurement function

$$\begin{array}{c}{h}^{*}\left(\mathit{X}\right)=L_1{x}_k+L_2{y}_k+(x_k{\stackrel{·}{x}}_k+y_k{\stackrel{·}{y}}_k)/\sqrt{x_k^2+y_k^2}\hfill \\ [L_1,L_2]=-{\mathit{R}}_{\mathrm{ru}}{\left({\mathit{R}}_u^P\right)}^{-1},{\mathit{R}}_{\mathrm{ru}}=[R_{\mathrm{ru}31}^D,R_{\mathrm{ru}32}^D]\hfill \end{array}$$

(30)

to predict the measurement

$${\epsilon }_k^{\left(i\right)}=h^{*}\left({\mathbf{\gamma }}_k^{(i)}\right),i=0~2n$$

(31)

Compute the predicted measurement mean ${\overline{\epsilon }}_k$ , the measurement predicted covariance ${\mathit{P}}_k^{\epsilon \epsilon }$ , and the cross-covariance of the state and the measurement ${\mathit{P}}_k^{\mathit{x}\epsilon }$

$$\begin{array}{c}{\overline{\epsilon }}_k=\sum _{i=0}^{2n}{W}_i^{(\mathrm{m})}{\epsilon }_k^{\left(i\right)}\hfill \\ {\mathit{P}}_k^{\epsilon \epsilon }=\sum _{i=0}^{2n}{W}_i^{(\mathrm{c})}({\epsilon }_k^{\left(i\right)}-{\overline{\epsilon }}_k)({\epsilon }_k^{\left(i\right)}-{\overline{\epsilon }}_k)\prime \hfill \\ {\mathit{P}}_k^{\mathit{x}\epsilon }=\sum _{i=0}^{2n}{W}_i^{(\mathrm{c})}({\mathbf{\gamma }}_k^{(i)}-{\overline{\mathit{m}}}_k)({\epsilon }_k^{\left(i\right)}-{\overline{\epsilon }}_k)\prime \hfill \end{array}$$

(32)

Output the state estimation ${\mathit{m}}_k$ and covariance estimation ${\mathit{P}}_k$

$$\begin{array}{c}{\mathit{K}}_k^{\epsilon }={\mathit{P}}_k^{\mathit{x}\epsilon }{\left({\mathit{P}}_k^{\epsilon \epsilon }\right)}^{-1}\hfill \\ {\mathit{m}}_k={\overline{\mathit{m}}}_k+{\mathit{K}}_k^{\epsilon }({\stackrel{·}{r}}_k-{\overline{\epsilon }}_k)\hfill \\ {\mathit{P}}_k={\overline{\mathit{P}}}_k+{\mathit{K}}_k^{\epsilon }{\mathit{P}}_k^{\epsilon \epsilon }{\mathit{K}}_k^{\epsilon }\prime \hfill \end{array}$$

(33)

Although the mean estimate of the SUKF is the same as that of the SEKF, the covariance computation of the SEKF is exact for polynomials up to the third order. Besides, the SUKF needs more computation steps to deal with the nonlinear measurement function of the range rate.

The CMKFRR algorithm

The SEKF and the SUKF are the sequential filtering methods, and their measurement equations still contain nonlinear components. The CMKFRR directly uses the KF for the position and range rate measurement. The algorithm converts the range, bearing, and range rate measurements into Cartesian coordinate system and takes the KF of information form. The steps of CMKFRR are as follows.

Conversion measurement

$${\mathit{Z}}_k^u=\left[\begin{array}{c}{x}_k^u\hfill \\ \multicolumn{1}{c}{y_k^u}\\ \multicolumn{1}{c}{{\stackrel{·}{x}}_k^u}\\ \multicolumn{1}{c}{{\stackrel{·}{y}}_k^u}\end{array}\right]=e^{{\sigma }_{\theta }^2/2}\mathit{D}\left({\theta }_k\right)\left[\begin{array}{c}{r}_k\hfill \\ \multicolumn{1}{c}{0}\\ \multicolumn{1}{c}{{\stackrel{·}{r}}_k}\\ \multicolumn{1}{c}{{\stackrel{·}{c}}_k}\end{array}\right]$$

(34)

$$\mathit{D}\left({\theta }_k\right)=\left[\begin{array}{cc}\mathit{d}\left({\theta }_k\right)\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& \mathit{d}\left({\theta }_k\right)\hfill \end{array}\right],\mathit{d}\left({\theta }_k\right)=\left[\begin{array}{cc}\cos {\theta }_k\hfill & -\sin {\theta }_k\hfill \\ \multicolumn{1}{c}{\sin {\theta }_k}& \cos {\theta }_k\hfill \end{array}\right]$$

(35)

where ${\stackrel{·}{c}}_k$ is the cross-range rate and $c_k=0$

$${\mathit{Z}}_k^u=e^{{\sigma }_{\theta }^2/2}\left[\begin{array}{c}{r}_k\cos {\theta }_k\hfill \\ \multicolumn{1}{c}{r_k\sin {\theta }_k}\\ \multicolumn{1}{c}{{\stackrel{·}{r}}_k\cos {\theta }_k}\\ \multicolumn{1}{c}{{\stackrel{·}{r}}_k\sin {\theta }_k}\end{array}\right]$$

(36)

Then, the measurement matrix is $\mathit{H}={\mathit{I}}_4$ .

The KF of information form

$$\begin{array}{c}{\mathit{m}}_k^{-}={\mathbf{\Phi }}_{k-1}{\mathit{m}}_{k-1}\hfill \\ {\mathit{P}}_k^{-}={\mathbf{\Phi }}_{k-1}{\mathit{P}}_{k-1}{\mathbf{\Phi }\prime }_{k-1}+{\mathit{Q}}_{k-1}\hfill \\ {\mathit{W}}_k={[{\left({\mathit{P}}_k^{-}\right)}^{-1}+{\mathit{H}\prime R}_k^{-1}\mathit{H}]}^{-1}{\mathit{H}\prime R}_k^{-1}\hfill \\ {\mathit{m}}_k={\mathit{m}}_k^{-}+{\mathit{W}}_k[{\mathit{Z}}_k^u-{\mathit{m}}_k^{-}]\hfill \\ {\mathit{P}}_k={[{\left({\mathit{P}}_k^{-}\right)}^{-1}+{\mathit{H}\prime R}_k^{-1}\mathit{H}]}^{-1}\hfill \end{array}$$

(37)

where

$${\mathit{R}}_k^{-1}=\mathit{D}\left(\alpha \right){\mathit{R}}_R^{-1}\mathit{D}\left(\alpha \right)\prime$$

(38)

$${\mathit{R}}_R^{-1}=\left[\begin{array}{cc}{\left({\mathit{R}}_R^{1:3,1:3}\right)}^{-1}\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill \end{array}\right]$$

(39)

The matrix ${\mathit{R}}_R^{1:3,1:3}$ can be calculated by

$$\begin{array}{l}{R}_R^{11}=\frac{1}{2}\left[{\left({\widehat{x}}_R^1\right)}^2+P_R^{11}+{\sigma }_r^2\right]\left(1+e^{-2{\sigma }_{\theta }^2}{e}^{-2{\sigma }_{\alpha }^2}\right)e^{{\sigma }_{\alpha }^2}\\ -\frac{1}{2}\left[{\left({\widehat{x}}_R^1\right)}^2+P_R^{11}\right]\left(1+e^{-2{\sigma }_{\alpha }^2}\right)\\ R_R^{12}=0\\ R_R^{13}=\frac{1}{2}\left({\widehat{x}}_R^1{\widehat{x}}_R^3+P_R^{13}+\rho {\sigma }_r{\sigma }_{\stackrel{.}{r}}\right)\left(1+e^{-2{\sigma }_{\theta }^2}{e}^{-2{\sigma }_{\alpha }^2}\right)e^{{\sigma }_{\alpha }^2}\\ -\frac{1}{2}\left({\widehat{x}}_R^1{\widehat{x}}_R^3+P_R^{13}\right)\left(1+e^{-2{\sigma }_{\alpha }^2}\right)\\ R_R^{22}=\frac{1}{2}\left[{\left({\widehat{x}}_R^1\right)}^2+P_R^{11}+{\sigma }_r^2\right]\left(1-e^{-2{\sigma }_{\theta }^2}{e}^{-2{\sigma }_{\alpha }^2}\right)e^{{\sigma }_{\theta }^2}\\ -\frac{1}{2}\left({\left({\widehat{x}}_R^1\right)}^2+P_R^{11}\right)\left(1-e^{-2{\sigma }_{\alpha }^2}\right)\end{array}$$

$$\begin{array}{cc}\multicolumn{1}{c}{{\mathit{R}}_R^{23}=}& \frac{1}{2}({\hat{\mathit{x}}}_R^1{\hat{\mathit{x}}}_R^4+{\mathit{P}}_R^{14})(1-e^{-2{\sigma }_{\theta }^2}{e}^{-2{\sigma }_{\alpha }^2})e^{{\sigma }_{\theta }^2}\hfill \\ \multicolumn{1}{c}{}& -\frac{1}{2}({\hat{\mathit{x}}}_R^1{\hat{\mathit{x}}}_R^4+{\mathit{P}}_R^{14})(1-e^{-2{\sigma }_{\alpha }^2})\hfill \\ \multicolumn{1}{c}{{\mathit{R}}_R^{33}=}& \frac{1}{2}[{\left({\hat{\mathit{x}}}_R^3\right)}^2+{\mathit{P}}_R^{33}+{\mathbf{\sigma }}_{\stackrel{·}{r}}^2](1+e^{-2{\sigma }_{\theta }^2}{e}^{-2{\sigma }_{\alpha }^2})e^{{\sigma }_{\theta }^2}\hfill \\ \multicolumn{1}{c}{}& -\frac{1}{2}({\left({\hat{\mathit{x}}}_R^3\right)}^2+{\mathit{P}}_R^{33})(1+e^{-2{\sigma }_{\alpha }^2})\hfill \\ \multicolumn{1}{c}{}& +\frac{1}{2}[{\left({\hat{\mathit{x}}}_R^4\right)}^2+{\mathit{P}}_R^{44}+{\mathbf{\sigma }}_{\stackrel{·}{c}}^2](1-e^{-2{\sigma }_{\theta }^2}{e}^{-2{\sigma }_{\alpha }^2})e^{{\sigma }_{\theta }^2}\hfill \\ \multicolumn{1}{c}{}& -\frac{1}{2}({\left({\hat{\mathit{x}}}_R^4\right)}^2+{\mathit{P}}_R^{44})(1-e^{-2{\sigma }_{\alpha }^2})\hfill \end{array}$$

(39a)

where ${\hat{\mathit{x}}}_R=\mathit{D}\left(\alpha \right)\prime {\mathit{m}}_k^{-}$ , ${\mathit{P}}_R=\mathit{D}\left(\alpha \right)\prime {\mathit{P}}_k^{-}\mathit{D}\left(\alpha \right)$ , $\alpha ={\tan }^{-1}({\mathit{m}}_k^{-2}/{\mathit{m}}_k^{-1})$ , ${\sigma }_{\alpha }^2={\mathit{P}}_R^{22}/{\left({\hat{\mathit{x}}}_R^1\right)}^2$ . The parameter ${\sigma }_{\stackrel{·}{c}}$ is the standard deviation of cross range rate.

The CMKFRR introduces the unknown cross-range rate ${\stackrel{·}{c}}_k$ whose standard deviation ${\sigma }_{\stackrel{·}{c}}$ should be set with the prior knowledge. The parameter ${\sigma }_{\stackrel{·}{c}}$ influences the performance of CMKFRR algorithm. Bordonaro et al. ¹⁶ claim that the tracking performance of CMKFRR with an appropriate ${\sigma }_{\stackrel{·}{c}}$ is better than those of SEKF and SUKF.

Performance of tracking algorithms

The simulation experiment is to demonstrate that the performance of the UKF, the UCMKF, the DUCMKF, the SEKF, the SUKF, and the CMKFRR algorithms. Besides, the posterior Cramér–Rao lower bound (PCRLB) ²¹ for the position measurement called PCRLBR and the PCRLB for the range, bearing, and range rate measurements called PCRLBRR are calculated. Three target tracking scenarios are set up artificially in the simulation experiment. The mean square error (MSE) is used to measure the accuracy of state estimation. The MSE for target position and the MSE for target velocity are defined as, respectively

$$\begin{array}{c}\mathrm{MSE}(\hat{x},\hat{y})=\frac{1}{N}\sum _{i=1}^N[{(x-\hat{x})}^2+{(y-\hat{y})}^2]\hfill \\ \mathrm{MSE}(\widehat{\stackrel{·}{x}},\widehat{\stackrel{·}{y}})=\frac{1}{N}\sum _{i=1}^N[{(\stackrel{·}{x}-\widehat{\stackrel{·}{x}})}^2+{(\stackrel{·}{y}-\widehat{\stackrel{·}{y}})}^2]\hfill \end{array}$$

(40)

where $N$ is the Monte Carlo experiment time; $x$ and $y$ are the true position components; $\widehat{\stackrel{·}{x}}$ and $\widehat{\stackrel{·}{y}}$ are the true velocity components; $\hat{x}$ and $\hat{y}$ are the estimated position components; and $\widehat{\stackrel{·}{x}}$ and $\widehat{\stackrel{·}{y}}$ are the estimated position components:

• Scenario 1: the initial position of target is $(6\mathrm{km},10\mathrm{km})$ ; the initial magnitude and direction angle of velocity is $(50\mathrm{m}/\mathrm{s},\pi /6\mathrm{rad})$ ; the standard deviation of range measurement is ${\sigma }_r=20m$ ; the standard deviation of bearing measurement is ${\sigma }_{\theta }=0.1\circ$ .

• Scenario 2: the initial position of target is $(20\mathrm{km},40\mathrm{km})$ ; the initial magnitude and direction angle of velocity is $(50\mathrm{m}/\mathrm{s},\pi /6\mathrm{rad})$ ; the standard deviation of range measurement is ${\sigma }_r=50\mathrm{m}$ ; the standard deviation of bearing measurement is ${\sigma }_{\theta }=2\circ$ .

• Scenario 3: the initial position of target is $(6\mathrm{km},10\mathrm{km})$ ; the initial magnitude and direction angle of velocity is $(200\mathrm{m}/\mathrm{s},\pi /6\mathrm{rad})$ ; the standard deviation of range measurement is ${\sigma }_r=200\mathrm{m}$ ; the standard deviation of bearing measurement is ${\sigma }_{\theta }=2\circ$ .

The standard deviation of range rate measurement is ${\sigma }_{\stackrel{·}{r}}=0.1\mathrm{m}/\mathrm{s}$ and the correlation coefficient is $\rho =-0.9$ in all scenarios. The target follows a nearly constant velocity (CV) track in three scenarios. In order to eliminate the influence of motion model mismatch on tracking performance, all the algorithms adopt the CV motion model in the simulation and the power spectral density of the noise covariance matrix is set to $q=1\mathrm{W}/\mathrm{Hz}$ . The initial position components in tracking algorithm are set as the first converted measurement and the initial velocity components are set to $0\mathrm{m}/\mathrm{s}$ . The initial state estimation covariance matrix is set to ${\mathit{P}}_0=\mathrm{diag}(10^4,10^4,50,50)$ . The parameters of UKF and SUKF are set to $\lambda =-1$ , $\alpha =1$ , $\beta =2$ , and $n=4$ . For the CMKFRR, the medium standard deviation of cross range rate is set as ${\sigma }_{\stackrel{·}{c}}=10m/s$ .

Figures 1–3 show the MSE for position and velocity with 1000 Monte Carlo simulations in the scenarios 1–3, respectively. The PCRLBRR is lower than the PCRLBR in each tracking scenario. Thus, in theory, introducing the range rate measurement has the potential to improve the accuracy of target state estimation. In Figure 1, the MSEs of the SEKF, the SUKF, and the CMKFRR for position and velocity are obviously better than those of the UKF, the UCMKF, and the DUCMKF. The results demonstrate that the use of range rate measurement indeed improves the accuracy of state estimation in the tracking scenario 1. In Figure 2, after about 70s, the MSEs of all algorithms for position and velocity are interwoven together, but the CMKFRR, which demonstrates that the use of range rate measurement, has little influence on the accuracy of state estimation in the tracking scenario 2. In Figure 3, the MSEs of the SEKF, the SUKF, and the CMKFRR for position and velocity are lower than those of the UKF, the UCMKF, and the DUCMKF during the initial tracking period. However, the MSEs of the SEKF, the SUKF, and the CMKFRR for position are higher than those of the UKF, the UCMKF, and the DUCMKF during the middle and late tracking periods. It demonstrates that the use of range rate measurement is not always beneficial for tracking task.

Figure 1.

MSE for position and velocity with 1000 Monte Carlo simulations in the scenario 1: (a) MSE for position and (b) MSE for velocity.

Figure 2.

MSE for position and velocity with 1000 Monte Carlo simulations in the scenario 2: (a) MSE for position and (b) MSE for velocity.

Figure 3.

MSE for position and velocity with 1000 Monte Carlo simulations in the scenario 3: (a) MSE for position and (b) MSE for velocity.

A practical adaptive nonlinear tracking algorithm with the range rate measurement

The CUSUM detector based tracking algorithm with range rate

It can be seen from the results of simulation experiment in section “The CMKFRR algorithm” that the range rate measurement cannot improve the accuracy of the tracking algorithm in the all tracking scenarios. In some tracking scenarios, for example, the tracking scenarios of Lei and Han, ¹⁴ Jiao et al., ¹⁵ and Bordonaro et al. ¹⁶ and the scenario 1 in section “The CMKFRR algorithm,” the use of the range rate measurement indeed improves the state estimation accuracy. However, before performing the tracking algorithm, as the target state is stochastic and the tracking performance is influenced by the true movement of target, measurement noise, filtering algorithm and target motion model, and so on, it is difficult to judge whether the range rate measurement is beneficial to the target tracking task. In essence, this problem can boil down to the problem of whether the measurement model in the target tracking algorithm matches the current tracking scenario. When the measurement model with range rate is matched, the state estimation accuracy of the target can be improved; otherwise, it cannot. The famous IMM algorithm ²² is used in the field of tracking to solve the problem of model mismatch. The IMM algorithm models the switching between the models as Markov random process, and realizes the soft switching between the models. The IMM aims at the mismatch between the target motion state model and the real target motion, and can solve the maneuvering target tracking well. ²³ The IMM may be applied directly to the mismatch problem of the measurement models in this article. However, unlike the target maneuver, the characteristics of the nonmaneuvering tracking scenario have the spatio-temporal stability which the IMM is not suitable. ²⁴ Besides, the performance of IMM is affected by the initial model probability and the model transfer probability that need to be set manually based on prior information, while we does not have such a priori information for the measurement model.

Except the IMM, the other to solve the model mismatch problem is the decision-based method, ²⁵ which mainly executes a single model in target tracking while a detector is performed to detect whether the current model matches the true motion. Once the model mismatch is detected, it will immediately switch the model. This hard switching method is more suitable for the mismatch between the measurement model and the tracking scenario, because the fusion between two types of measurement models with inclusion relationship in this article is not needed during the tracking process. In the decision-based method, a detector should be designed to detect the mismatch of the measurement model. The current-decision-based method includes the measurement-residual (MR)-based chi-square detector algorithm, the generalized likelihood ratio (GLR) test detector algorithm, the marginalized likelihood ratio (MLR) detector algorithm, the cumulative sum–based detector (CUSUM) algorithm. ²⁶ The MR, the GLR, and the MLR are a class of batch detection algorithms, and CUSUM is a kind of sequential detection algorithm. Although the CUSUM has been put forward for decades for the change-point detection, it has recently been emphasized in theory and application. Cho and Fryzlewicz ²⁷ propose the sparsified binary segmentation (SBS) algorithm which aggregates the CUSUM statistics by adding only those that pass a certain threshold. An adaptive CUSUM is applied to a real-time detection framework for false data injection in smart grid networks. ²⁸ The CUSUM change-point detection technology is applied to the stationary evaluation method of univariate time series. ²⁹ A nonparameter CUSUM is designed for quickest detection of parameter changes in stochastic regression. ³⁰ The CUSUM is more appropriate in the target tracking problem discussed in the article, because the measurement vector of radar comes according to the time sequence. Therefore, we use the CUSUM to detect the measurement mismatch in this article.

The standard CUSUM detector is used for maneuvering target tracking, which is specifically expressed as

$$L_k=max\{L_{k-1}+\log \frac{p({\tilde{\mathit{Z}}}_k|H_1,{\mathit{Z}}^{k-1})}{p({\tilde{\mathit{Z}}}_k|H_0,{\mathit{Z}}^{k-1})},0\},L_0=0$$

(41)

Decision rules are as follows:

1. Accept $H_1$ , if $L_k\ge \kappa$ , and the maneuver time $\hat{m}=min\{k:L_k\ge \kappa \}$ ;

2. Continue detecting $k=k+1$ , if $L_k<\kappa$ .

where ${\mathit{Z}}^k\stackrel{\Delta }{=}({\mathit{Z}}_1,\dots ,{\mathit{Z}}_k)$ ; $\kappa$ is the detection threshold; ${\tilde{\mathit{Z}}}_k$ is the measurement prediction residual at time $k$ ; $p(·|·)$ is the conditional probability; $H_0$ is the assumption that the target does not maneuver; and $H_1$ is the assumption that the target maneuvers.

According to the standard CUSUM detector, the modified CUSUM detector for the measurement mismatch detection in this article is designed as follows

$$L_k=max\{L_{k-1}+\log \frac{p({\tilde{\mathit{Z}}}_k^p|{\overline{\zeta }}_{k-1},{\mathit{Z}}^{k-1})}{p({\tilde{\mathit{Z}}}_k^p|{\zeta }_{k-1},{\mathit{Z}}^{k-1})},0\},L_0=0$$

(42)

Decision rules are as follows:

1. If $L_k\ge \kappa$ , then ${\zeta }_k={\overline{\zeta }}_{k-1}$ , ${\overline{\zeta }}_k={\zeta }_{k-1}$ , $L_k=L_0$ , and $k=k+1$ ;

2. If $L_k<\kappa$ , then ${\zeta }_k={\zeta }_{k-1}$ , ${\overline{\zeta }}_k={\overline{\zeta }}_{k-1}$ , and $k=k+1$ .

where ${\tilde{\mathit{Z}}}_k^p$ is the measurement prediction residual at time $k$ ; ${\zeta }_{k-1}$ is a Boolean variable that ${\zeta }_{k-1}=1$ means the measurement model with position and range rate measurement and ${\zeta }_{k-1}=0$ means the measurement model only with position measurement; and “–” is the NOT operator.

Based on the modified CUSUM detector, a practical tracking algorithm with range rate measurement is proposed in this article, which is the CUSUM detector based tracking algorithm with range rate measurement, called CUSUM-RR. The algorithm is described as follows:

• Step 1: initialization.

Set up ${\mathit{m}}_0^0$ , ${\mathit{P}}_0^0$ , ${\mathit{Q}}^0$ , ${\mathit{R}}^0$ , ${\mathit{m}}_0^1$ , ${\mathit{P}}_0^1$ , ${\mathit{Q}}^1$ , ${\mathit{R}}^1$ , ${\zeta }_0$ , and $\kappa$ , where the superscript “1” corresponds to the measurement model with position and range rate measurement and the superscript “0” corresponds to the measurement model only with position measurement.

• Step 2: parallel filtering.

Perform one of the filter algorithms described in section “Nonlinear tracking algorithms with the position measurement” and one of the filter algorithms described in section “Nonlinear tracking algorithms with the position and range rate measurement” to obtain the variables $\{{\tilde{\mathit{Z}}}_k^j,{\mathit{S}}_k^j,{\mathit{m}}_k^j,{\mathit{P}}_k^j{\}}_{j=0}^1$ at time $k$ , where ${\tilde{\mathit{Z}}}_k^j$ is the measurement prediction residual, ${\mathit{S}}_k^j$ is the residual covariance, ${\mathit{m}}_k^j$ is the state estimation, and ${\mathit{P}}_k^j$ is the state estimation error covariance.

• Step 3: calculation likelihood function.

Assume that ${\tilde{\mathit{Z}}}_k^p$ obeys the zero mean Gauss distribution

$$p({\tilde{\mathit{Z}}}_k^p|j,{\mathit{Z}}^{k-1})={\left|2\pi {\mathit{S}}_k^p\right|}^{-1/2}\exp \{-\frac{1}{2}{\tilde{\mathit{Z}}}_k^p\prime {\left({\mathit{S}}_k^p\right)}^{-1}{\tilde{\mathit{Z}}}_k^p\}$$

(43)

where ${\mathit{S}}_k^p$ is the position measurement prediction residual covariance. If $j=0$ , then ${\tilde{\mathit{Z}}}_k^p={\tilde{\mathit{Z}}}_k^0$ and ${\mathit{S}}_k^p={\mathit{S}}_k^0$ . If $j=1$

$${\tilde{\mathit{Z}}}_k^p=({\tilde{\mathit{Z}}}_k^1\left(1\right),{\tilde{\mathit{Z}}}_k^1\left(2\right))\prime$$

(44)

$${\mathit{S}}_k^p=\left[\begin{array}{cc}{\mathit{S}}_k^1(1,1)\hfill & {\mathit{S}}_k^1(1,2)\hfill \\ \multicolumn{1}{c}{{\mathit{S}}_k^1(2,1)}& {\mathit{S}}_k^1(2,2)\hfill \end{array}\right]$$

(45)

• Step 4: detection the measurement mismatch.

Let

$$p({\tilde{\mathit{Z}}}_k^p|{\zeta }_{k-1},{\mathit{Z}}^{k-1})=p({\tilde{\mathit{Z}}}_k^p|j={\zeta }_{k-1},{\mathit{Z}}^{k-1})$$

(46)

$$p({\tilde{\mathit{Z}}}_k^p|{\overline{\zeta }}_{k-1},{\mathit{Z}}^{k-1})=p({\tilde{\mathit{Z}}}_k^p|j={\overline{\zeta }}_{k-1},{\mathit{Z}}^{k-1})$$

(47)

Then

$$L_k=max\{L_{k-1}+\log \frac{p({\tilde{Z}}_k^p|{\overline{\zeta }}_{k-1},Z^{k-1})}{p({\tilde{Z}}_k^p|{\zeta }_{k-1},Z^{k-1})},0\},L_0=0$$

(48)

If $L_k\ge \kappa$ , then ${\zeta }_k={\overline{\zeta }}_{k-1}$ , ${\overline{\zeta }}_k={\zeta }_{k-1}$ , $L_k=L_0$ ; if $L_k<\kappa$ , then ${\zeta }_k={\zeta }_{k-1}$ , ${\overline{\zeta }}_k={\overline{\zeta }}_{k-1}$ .

Output the tracking result ${\mathit{m}}_k^j$ and ${\mathit{P}}_k^j$ according to $j={\zeta }_k$ .

• Step 5: time updating.

Let $k=k+1$ , return to the parallel filtering step.

For the CUSUM-RR algorithm, two tracking algorithms with two measurement models are performed at the same time, respectively, and the state estimation which has the higher accuracy is outputted. The CUSUM-RR algorithm could adaptively choose the measurement model in the different tracking scenarios, which does not depend on the prior information to choose the measurement model artificially, to avoid the measurement mismatch. The threshold value $\kappa$ is calculated by

$$\kappa =\log ((1-p_f)/p_m)$$

(49)

where $p_f$ is the false alarm probability and $p_m$ is the false dismissal probability.

Performance of CUSUM-RR algorithm

The purpose of this section is to demonstrate the performance of the proposed CUSUM-RR algorithm. The SEKF and UCMKF are selected as the filters of CUSUM-RR according to two measurement equations. In the simulation experiment, the CUSUM-RR is compared with the SEKF, UCMKF, and IMM. The filters of IMM also consist of the SEKF and UCMKF with two measurement models, respectively. The target motion parameters are the same as the tracking scenario 3 in section “The CMKFRR algorithm.” We set three position measurement standard deviations $(50\mathrm{m},0.5\circ )$ , $(100\mathrm{m},1\circ )$ , and $(200\mathrm{m},2\circ )$ . For the CUSUM-RR, $p_f$ is set as 0.1%, $p_m$ is set as 1% and thus $\kappa \approx 2$ . The outputted initial state estimation is from the filter only with the position measurement, ${\zeta }_0=0$ . For the IMM, because of the unknown tracking scenario, the initial model probability distribution is set as ${\mu }_0=[0.5,0.5]\prime$ , and the transition probability matrix is set as $p=[0.9,0.1;0.1,0.9]$ . The other parameters are the same as in section “The CMKFRR algorithm.”

The MSEs of CUSUM-RR, IMM, SEKF, and UCMKF for position with 1000 Monte Carlo simulations are shown in Figures 4(a)–6(a). During the initial tracking period, as the MSE of SEKF is the lower than that of UCMKF, the Boolean variable changes from ${\zeta }_0=0$ to ${\zeta }_1=1$ immediately and keeps ${\zeta }_k=1$ , that means to output the state estimation from the SEKF. Then, the MSE of CUSUM-RR is the same as the SEKF almost. The MSE of the IMM is located between the MSEs of the SEKF and the UCMKF. During the middle tracking period, as the MSE of UCMKF gradually lower than that of SEKF, the MSEs of CUSUM-RR and IMM are adaptively toward the MSE of UCMKF. During the late tracking period, the MSE of CUSUM-RR converges to the MSE of UCMKF that is lower than the MSE of SEKF steadily. The IMM with the state fusion mechanism is inevitably influenced by the random noise and the high MSE of state estimation, and the convergence rate is slower than that of the CUSUM-RR algorithm. The CUSUM-RR with the CUSUM detector can effectively reduce the influence of random noise by setting up the appropriate threshold. The log-likelihood ratio achieved at 500th Monte Carlo simulation experiment is shown in Figures 4(b)–6(b). The log-likelihood ratio in Figures 4(b)–6(b) is calculated by

$$\log \frac{p({\tilde{\mathit{Z}}}_k^p|j=1,{\mathit{Z}}^{k-1})}{p({\tilde{\mathit{Z}}}_k^p|j=0,{\mathit{Z}}^{k-1})}$$

(50)

Figure 4.

The results of position measurement standard deviation $(50\mathrm{m},0.5\circ )$ : (a) MSE for position with 1000 Monte Carlo simulations, (b) log-likelihood ratio of CUSUM-RR at 500th Monte Carlo simulation, and (c) L_k of CUSUM-RR at 500th Monte Carlo simulation.

Figure 5.

The results of position measurement standard deviation $(100\mathrm{m},1\circ )$ : (a) MSE for position with 1000 Monte Carlo simulations, (b) log-likelihood ratio of CUSUM-RR at 500th Monte Carlo simulation, and (c) L_k of CUSUM-RR at 500th Monte Carlo simulation.

Figure 6.

The results of position measurement standard deviation $(200\mathrm{m},2\circ )$ : (a) MSE for position with 1000 Monte Carlo simulations, (b) log-likelihood ratio of CUSUM-RR at 500th Monte Carlo simulation, and (c) L_k of CUSUM-RR at 500th Monte Carlo simulation.

The parameter L_k in the CUSUM detector at 500th Monte Carlo simulation experiment is shown in Figure 4(c)–6(c). From Figures 4(b)–6(b), during the initial tracking period, the log-likelihood ratio far exceeds the threshold value 2. Thus, in Figures 4(c)–6(c), the value of L_k is almost 0 in the corresponding time period. In the middle tracking period, the outputted state estimation of CUSUM-RR changes between the SEKF and UCMKF, because the log-likelihood ratio fluctuates on the zero line. This is why the MSE of CUSUM-RR does not immediately follow the MSE of the UCMKF when the MSE of the UCMKF is lower than the MSE of the SEKF in the middle tracking period with 1000 Monte Carlo simulation experiment. In general, the proposed CUSUM-RR can effectively solve the problem of measurement model mismatch and provides an application path for the tracking target with range rate measurement in practice. However, the disadvantage of CUSUM-RR is that CUSUM-RR needs about two times the amount of calculation than that of SEKF or UCMKF. The CUSUM-RR also cannot obtain the higher accuracy of state estimation given SEKF and UCMKF because of the hard decision. Although the ideal MSE curves of CUSUM-RR should always follow the lower between the MSE curves of SEKF and UCMKF, due to the interference of random noise, the time of CUSUM mismatch detection is not the same in each Monte Carlo simulation. In other words, the detection delay could not been avoided. The value of threshold $\kappa$ is the key factor for controlling the detection delay. However, this parameter is difficult to give a reasonable priori value.

Conclusion and future works

The use of range rate measurement may not improve the target tracking performance, as the special tracking algorithm performance is related to the special tracking scenario. However, since the accuracy of state estimation is affected by many other factors besides the nonlinear measurement equation, it is difficult to establish the application relationship between the tracking scenario and measurement equation. In this article, the nonlinear filtering algorithms which only use the position measurement and the nonlinear filtering algorithms which use position and range rate measurement are surveyed. The performances of these algorithms are compared by simulation experiments. The experimental results demonstrate that the range rate measurement cannot improve target state estimation accuracy in any tracking scenario. In order to solve this problem, this article proposes a practical tracking algorithm with adaptive ability based on the CUSUM detector, that is, the CUSUM-RR algorithm. The algorithm can achieve good accuracy of state estimation in different tracking scenarios, which avoids the performance degradation due to the misuse of measurement equations. The decision-based algorithm prevents the bad fusion between multiple models and the interference of random noise through setting the appropriate threshold value. The simulation results demonstrate that the CUSUM-RR can better follow the state estimation corresponding to the measurement equation matching the tracking scenario and then has a lower MSE than the IMM.

In the future works, the nonlinear tracking algorithm with range rate measurement should be studied more deeply to improve the accuracy of state estimation. The particle filter algorithm may be more suitable for the strong nonlinear measurement equation. If the particle filter is used as a filter of CUSUM-RR, it is necessary to redefine the likelihood ratio to design an effective CUSUM detector. The threshold setting of the CUSUM detector should be further studied to ensure a more rapid detection for the measurement model mismatch. For the maneuvering target tracking problem, the CUSUM-RR proposed in this article cannot be applied directly. The strategy is to add an additional CUSUM detector to detect the target maneuver, or to use the CUSUM-RR as the sub-filter of the IMM. One of the future research directions is to design a composite CUSUM detector by exploring the relationship between range rate measurement and factors involved in the target tracking. The accuracy of angular velocity and normal acceleration also could be improved by the range rate measurement and the state estimation with the CUSUM-RR. In the following study, it is of importance to develop the method of fusing the range rate measurements from multiple Doppler radars in sensor networks to solve the problems of multiple target tracking and recognition.