Tracking maneuver target using interacting multiple model-square root cubature Kalman filter based on range rate measurement

Abstract

The problem of maneuvering target tracking is a hot issue in the field of target tracking. Due to the range rate measurement containing the maneuvering information of target, it has the important practical significance to study how to use the range rate measurement to improve the effect of maneuvering target tracking. In the framework of interacting multiple model algorithm, the range rate measurement is used to update target state estimate and the probability of motion model to improve the tracking performance. As the measurement equation including the range rate measurement is strongly nonlinear, square root cubature Kalman filter algorithm is selected as the filter in interacting multiple model algorithm. The normal acceleration is deduced from the range rate with the reality constraint. And through Monte Carlo simulation, the empirical distribution functions of the normal acceleration statistics corresponding to different motion models are obtained. Their approximate distribution functions are obtained by the use of the expectation maximization algorithm with Gaussian mixture model. Then the probability distribution and probability distribution of measurement prediction residual are combined into a new likelihood function to improve the efficiency of updating the model probability. The experimental results show that the interacting multiple model algorithm proposed in this article has the smaller root mean square error of position and velocity and has the smaller average Kullback-Leibler divergence of model probability during the motion model stable phase.

Introduction

The interacting multiple model (IMM) algorithm is considered as the most effective hybrid estimation scheme in the maneuvering target tracking problem. ¹ It is widely used in the actual engineering problems that the radar tracks the maneuvering aircraft.^2,3 Under a reasonable set of movement models and the initial probability, the performance of IMM algorithm with the radar measurements can be improved by use of a good nonlinear filter and an updating method of the model probability. The Doppler radar not only can obtain the position information of the target, but also the range rate or Doppler velocity. The literature^4–6 shows that the range rate measurement can improve the tracking performance. Yuan et al. ⁷ and Frencl and colleagues^8,9 try to get the angle velocity estimation from the Doppler velocity. Bizup and Brown ¹⁰ show that the use of range rate measurement can improve the ability of maneuvering detection. The geometric relationship and curve motion principle are adopted to obtain the normal acceleration statistic from the range rate and then achieve the goal of rapid maneuvering detection in Bizup and Brown. ¹⁰ Ru et al. ¹¹ combine the statistic with the tangential acceleration to obtain the statistic of the total acceleration. Lu et al. ¹² use the Mahalanobis distance and Euclidean distance optimization method to give two statistics of the total acceleration. Then the normal acceleration information of maneuvering targets can be acquired through the range rate measurement and then the target motion model information can also be learned. However, the literatures^10–12 only use range rate for the multi-model algorithm based on the hard decision, whose motion models are switched by the maneuvering detector. Because the range rate contains the motion model of information, it can be used for the soft decision of IMM algorithm to improve the efficiency of updating the model probability. The purpose of this article is to improve the performance of IMM algorithm by the use of the range rate measurement. However, as the measurement equation including the range rate measurement is strongly nonlinear, it has not been widely used in target tracking problem. With the development of nonlinear filtering theory, the performance of approximate filtering algorithm compared to the traditional extended Kalman Filter (EKF) algorithm ¹³ is greatly improved. Recently, Arasaratnam and Haykin ¹⁴ propose square root cubature Kalman filter (SCKF) algorithm that has the comparable nonlinear approximation ability, the numerical accuracy and stability with conversion measurement Kalman Filter (CMKF) ¹⁵ and unscented Kalman Filter (UKF). ¹⁶ Then SCKF is adopted as the nonlinear filter of IMM algorithm in this article.

In the soft decision of IMM algorithm, the introduction of the maneuvering information extracted from the range rate measurement can improve the efficiency of updating the motion model probability. Given the initial and transition model probabilities, the calculation of model probability is decided on the corresponding likelihood function. The traditional likelihood function is expressed as the Gauss distribution of measurement prediction residuals that contains the position information. But the position measurement cannot directly reflect the maneuver information of the target. The statistic of acceleration from the range rate can be used to modify the likelihood function. In this article, the normal acceleration is deduced from the range rate with the reality constraint. And through Monte Carlo simulation, the empirical distribution functions of normal acceleration statistics corresponding to different motion models are obtained. The approximate distribution function is obtained by the use of the expectation maximization (EM) algorithm ¹⁷ with Gaussian mixture model (GMM). ¹⁸ Then both GMMs and probability distribution of measurement prediction residual are combined into a new likelihood function to improve the efficiency of updating the model probability.

State space equation and measurement equation

The application background of this article is to use the Doppler radar to track the manned aircraft. In order to simplify the description, we discuss the problem in two-dimensional (2D) plane. The set of motion models in the IMM algorithm contains $M$ models. The target state space equation and measurement equation are described as

$${\mathit{X}}_k^i={\mathit{F}}^i{\mathit{X}}_{k-1}^i+{\mathit{w}}_{k-1}^i$$

(1)

$${\mathit{Z}}_k={\mathit{H}}^i\left({\mathit{X}}_k^i\right)+{\mathbf{\upsilon }}_k^i,i=1,2,...,M$$

(2)

where ${\mathit{F}}^i$ is the state transition matrix of $i\mathrm{th}$ motion model; ${\mathit{X}}_k^i$ is the state vector of $i\mathrm{th}$ motion model at time $k$ that is described in the Cartesian coordinate system; there are two common forms in 2D plane, $\mathit{X}=[x,v_x,y,v_y{]}^{\prime }$ and $\mathit{X}=[x,v_x,a_x,y,v_y,a_y{]}^{\prime }$ ; $x$ and $y$ are the position components; $v_x$ and $v_y$ are the velocity components; $a_x$ and $a_y$ are the acceleration components; ${\mathit{Z}}_k$ is the Doppler radar measurement as $\mathit{Z}=[r,\theta ,v_r{]}^{\prime }$ at time $k$ ; $r$ is the range between radar and target; $\theta$ is the angle of target relative to the radar; $v_r$ is the range rate of target relative to the radar. ${\mathit{H}}^i({\mathit{X}}_k^i)$ is the nonlinear function vector as

$${\mathit{H}}^i\left({\mathit{X}}_k^i\right)=\left[\begin{array}{c}\sqrt{x_k^2+y_k^2}\hfill \\ \multicolumn{1}{c}{\arctan \frac{y_k}{x_k}}\\ \multicolumn{1}{c}{\frac{x_k{v}_{x,k}+y_k{v}_{y,k}}{\sqrt{x_k^2+y_k^2}}}\end{array}\right]$$

(3)

where ${\mathit{w}}_k$ and ${\mathbf{\upsilon }}_k$ are the independent white Gaussian noises and their corresponding covariance matrixes are ${\mathit{Q}}_k$ and ${\mathit{R}}_k$ respectively. The target state space equations with the same state transition matrix $\mathit{F}$ and different ${\mathit{Q}}_k$ are considered as the different motion models. ${\mathit{R}}_k$ can be described as

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

(4)

where ${\sigma }_r$ , ${\sigma }_{\theta }$ , ${\sigma }_{v_r}$ are the standard deviations; $\rho \in [-1,1]$ is the correlation coefficient between $r$ and $v_r$ . ¹⁹ In this article, we only discuss the multimodality of target state equation, regardless of the measurement equation. Therefore, the $i$ in ${\mathit{H}}^i(·)$ can be removed without the introduction of ambiguity.

SCKF algorithm

Because of the strongly nonlinear relationship between the range rate and the target state, EKF algorithm cannot make full use of the range rate measurement to improve the estimate accuracy. Arasaratnam and Haykin ¹⁴ proposes SCKF that use a group of equal weight cubature points and is based on the spherical radial integral criterion to solve Bayesian filtering. SCKF algorithm is to first calculate the cubature points and the corresponding weights. The cubature points under the third-order cubature criterion and the corresponding weights are

$${\mathbf{\xi }}_j=\sqrt{\frac{m}{2}}{\left[1\right]}_j,{\omega }_j=\frac{1}{m},j=1,2,...,m$$

(5)

where $m$ is the total number of cubature points and $m=2n_{\mathit{X}}$ ; $n_{\mathit{X}}$ is the state vector dimension; the sign $[1]$ is the point set whose elements are from the whole arrangement and invert for the vector $e=[1,0,...,0{]}^{\prime }$ ; $[1{]}_j$ is the $j\mathrm{th}$ element in the set $[1]$ . Assume that the posterior probability distribution of the target states of $i\mathrm{th}$ motion model at time $k-1$ is

$$p\left(X_{k-1}^i|Z^{k-1}\right)={\rm N}\left(X_{k-1}^i;{\widehat{X}}_{k-1|k-1}^i,P_{k-1|k-1}^i\right)$$

(6)

The specific steps of SCKF are as follows.

Time updating

1. Decomposing the covariance ${\mathit{P}}_{k-1|k-1}^i$

   $$P_{k-1|k-1}^i=S_{k-1|k-1}^i{\left(S_{k-1|k-1}^i\right)}^{\prime }$$

   (7)

2. Computing the state cubature points and spreading them through the state space equation

   $${\overline{\mathit{X}}}_{j,k-1|k-1}^i={\mathit{S}}_{k-1|k-1}^i{\mathbf{\xi }}_j+{\hat{\mathit{X}}}_{k-1|k-1}^i$$

   (8)
   $${\mathit{X}}_{j,k|k-1}^i={\mathit{F}}^i{\overline{\mathit{X}}}_{j,k-1|k-1}^i$$

   (9)

3. Computing the one-step prediction state and the square root of its covariance

   $${\hat{\mathit{X}}}_{k|k-1}^i=\sum _{j=1}^m{\omega }_j{\mathit{X}}_{j,k|k-1}^i$$

   (10)
   $$S_{k|k-1}^i=Tria\left(\left[{\chi }_{k|k-1}^i,S_{Q,k-1}^i\right]\right)$$

   (11)

where $\mathrm{Tria}(·)$ is the sign for the triangle decomposition; ${\mathit{S}}_{Q,k-1}^i$ is the square root of ${\mathit{Q}}_{k-1}^i$ and ${\mathit{Q}}_{k-1}^i={\mathit{S}}_{Q,k-1}^i·({\mathit{S}}_{Q,k-1}^i{)}^{\prime }$ ; $S_{k|k-1}^i$ is the lower triangular matrix; ${\mathbf{\chi }}_{k|k-1}^i$ is

$${\chi }_{k|k-1}^i=\frac{1}{\sqrt{m}}\left(X_{1,k|k-1}^i-{\widehat{X}}_{k|k-1}^i,\mathrm{\dots },X_{m,k|k-1}^i-{\widehat{X}}_{k|k-1}^i\right)$$

(12)

Measurement updating

1. Computing the cubature points and spreading them through the measurement equation

   $${\overline{\mathit{X}}}_{j,k|k-1}^i={\mathit{S}}_{k|k-1}^i{\mathbf{\xi }}_j+{\hat{\mathit{X}}}_{k|k-1}^i$$

   (13)
   $$Z_{j,k|k-1}^i=H\left({\overline{X}}_{j,k|k-1}^i\right)$$

   (14)

2. Calculating the prediction measurements

   $${\mathit{Z}}_{k|k-1}^i=\sum _{j=1}^m{\omega }_j{\mathit{Z}}_{j,k|k-1}^i$$

   (15)

3. Calculating the innovation covariance square root and covariance

   $$S_{Z,k}^i=Tria\left(\left[{\gamma }_k^i,S_{R,k}\right]\right)$$

   (16)
   $${\mathit{P}}_{\mathrm{XZ},k|k-1}^i={\overline{\mathbf{\chi }}}_k^i{\left({\mathbf{\gamma }}_k^i\right)}^{\prime }$$

   (17)

   where

   $${\gamma }_k^i=\frac{1}{\sqrt{m}}\left[Z_{1,k|k-1}^i,-Z_{k|k-1}^i,\mathrm{\dots },Z_{m,k|k-1}^i,-Z_{k|k-1}^i\right],{\overline{\chi }}_k^i=\frac{1}{\sqrt{m}}\left[{\overline{X}}_{1,k|k-1}^i,-{\widehat{X}}_{k|k-1}^i,\mathrm{\dots },{\overline{X}}_{m,k|k-1}^i,-{\widehat{X}}_{k|k-1}^i\right]$$

4. Calculating the filtering gain matrix

   $${\mathbf{W}}_k^i=\left({\mathbf{P}}_{XZ,k|k-1}^i/{\left({\mathbf{S}}_{Z,k}^i\right)}^{\prime }\right)/{\mathbf{S}}_{Z,k}^i$$

   (18)

5. Calculating the optimal state estimate and covariance square root at time $k$

   $${\widehat{X}}_{k|k}^i={\widehat{X}}_{k|k-1}^i+{\mathbf{W}}_k^i\left({\mathbf{Z}}_k-{\mathbf{Z}}_{k|k-1}^i\right)$$

   (19)
   $${\mathbf{S}}_{k|k}^i=Tria\left(\left[{\overline{\chi }}_k^i-{\mathbf{W}}_k^i{\mathbf{\gamma }}_k^i,{\mathbf{W}}_k^i{\mathbf{S}}_{\mathbf{R},k}\right]\right)$$

   (20)

The statistic of normal acceleration

The normal acceleration

The target moves in 2D plane of coordinate system $\mathrm{XOY}$ and the Doppler radar is located at the origin of coordinate. The basic relationship can be obtained ¹⁰

$$v_r=v\cos (\alpha -\theta )$$

(21)

where $v$ is the velocity size of the target; $\alpha$ is the velocity direction. Then

$$\alpha =\pm \arccos \left(\frac{v_r}{v}\right)+\theta$$

(22)

Assume the velocity direction ${\alpha }_{k-1}$ is known at time $t=k-1$ . After the measurements from radar $(r_k,{\theta }_k,v_{r,k})$ coming at time $k$ , by formula (22) ${\alpha }_k$ may be

$${\alpha }_k=\{\begin{array}{c}{\alpha }_k^{(1)}=\arccos \left(\frac{v_{r,k}}{v_k}\right)+{\theta }_k\hfill \\ \multicolumn{1}{c}{{\alpha }_k^{(2)}=-\arccos \left(\frac{v_{r,k}}{v_k}\right)+{\theta }_k}\end{array}$$

(23)

In the radar scanning period $T$ , there are an infinite number of ways from ${\alpha }_{k-1}$ to ${\alpha }_k$ . It is assumed that the change of target velocity direction is less than $2\pi$ during $T$ in Bizup and Brown. ¹⁰ Under this assumption, there are four turning ways from ${\alpha }_{k-1}$ to ${\alpha }_k$ . The maximum instantaneous turning angle rate of the manned maneuver aircraft is less than $\pi /6$ and the scanning period of the common Doppler radar is less than 1 s at present. Then, we assume that the change of target velocity direction is less than $\pi$ during $T$ . There are only two turning ways from ${\alpha }_{k-1}$ to ${\alpha }_k$ . $\Delta {\alpha }_k\in [-\pi ,\pi ]$ is denoted as the change of target velocity direction during $T$ , as formula (24). If the direction from ${\alpha }_{k-1}$ to ${\alpha }_k$ is the clockwise direction, denote $\Delta {\alpha }_k<0$ ; otherwise, $\Delta {\alpha }_k>0$

$$\Delta {\alpha }_k=\{\begin{array}{cc}{\alpha }_k-{\alpha }_{k-1}\hfill & ({\alpha }_k,{\alpha }_{k-1}\in [0,\pi ])\cup ({\alpha }_k,{\alpha }_{k-1}\in [-\pi ,0])\hfill \\ \multicolumn{1}{c}{{\alpha }_k-{\alpha }_{k-1}}& ({\alpha }_k\in [0,\pi ],{\alpha }_{k-1}\in [-\pi ,0])\cup ({\alpha }_k-{\alpha }_{k-1}\le \pi )\hfill \\ \multicolumn{1}{c}{{\alpha }_k-{\alpha }_{k-1}}& ({\alpha }_k\in [-\pi ,0],{\alpha }_{k-1}\in [0,\pi ])\cup ({\alpha }_k-{\alpha }_{k-1}\ge -\pi )\hfill \\ \multicolumn{1}{c}{{\alpha }_k-{\alpha }_{k-1}-2\pi }& ({\alpha }_k\in [0,\pi ],{\alpha }_{k-1}\in [-\pi ,0])\cup ({\alpha }_k-{\alpha }_{k-1}\ge \pi )\hfill \\ \multicolumn{1}{c}{{\alpha }_k-{\alpha }_{k-1}+2\pi }& ({\alpha }_k\in [-\pi ,0],{\alpha }_{k-1}\in [0,\pi ])\cup ({\alpha }_k-{\alpha }_{k-1}\le -\pi )\hfill \end{array}$$

(24)

The normal acceleration can be calculated by $a_{n,k}={\omega }_k{v}_k$ . Assume that the change of velocity is small, that is, $v_k\approx v_{k-1}$ . Then $a_{n,k}$ can be expressed as

$$a_{n,k}=\{\begin{array}{c}{a}_{n,k}^{(1)}=\frac{\Delta {\alpha }_k(\arccos \left(\frac{v_{r,k}}{v_{k-1}}\right)+{\theta }_k,{\alpha }_{k-1})}{T}{v}_{k-1}\hfill \\ \multicolumn{1}{c}{a_{n,k}^{(2)}=\frac{\Delta {\alpha }_k(-\arccos \left(\frac{v_{r,k}}{v_{k-1}}\right)+{\theta }_k,{\alpha }_{k-1})}{T}{v}_{k-1}}\end{array}$$

(25)

The measurements of $v_{r,k}$ and ${\theta }_k$ can be obtained from the Doppler radar. The estimation values of ${\alpha }_{k-1}$ and $v_{k-1}$ at time $k-1$ can be obtained with the state estimate ${\hat{\mathit{X}}}_{k-1|k-1}$

$${\hat{\alpha }}_{k-1}=\{\begin{array}{cc}\arccos (\frac{{\hat{v}}_{x,k-1|k-1}}{\sqrt{{\hat{v}}_{x,k-1|k-1}^2+{\hat{v}}_{\mathrm{y},k-1|k-1}^2}})\hfill & {\hat{v}}_{\mathrm{y},k-1|k-1}\ge 0\hfill \\ \multicolumn{1}{c}{-\arccos (\frac{{\hat{v}}_{x,k-1|k-1}}{\sqrt{{\hat{v}}_{x,k-1|k-1}^2+{\hat{v}}_{\mathrm{y},k-1|k-1}^2}})}& {\hat{v}}_{\mathrm{y},k-1|k-1}<0\hfill \end{array}$$

(26)

$${\hat{v}}_{k-1|k-1}=\sqrt{{\hat{v}}_{x,k-1|k-1}^2+{\hat{v}}_{y,k-1|k-1}^2}$$

(27)

We can obtain the estimation values ${\hat{a}}_{n,k}^{(1)}$ and ${\hat{a}}_{n,k}^{(2)}$ for $a_{n,k}$ by formulas (25). In Bizup and Brown, ¹⁰ the statistic $c_{min}=min\{{\hat{a}}_{n,k}^{(1)},{\hat{a}}_{n,k}^{(2)}\}$ is employed. However, if $|\alpha -\theta |$ is small, ${\hat{a}}_n^{(1)}$ and ${\hat{a}}_n^{(2)}$ will be similar. They may be taken with the same probability as an estimate of $a_n$ . Then $c_{min}$ will introduce a statistical error as the smaller between ${\hat{a}}_n^{(1)}$ and ${\hat{a}}_n^{(2)}$ is selected. A new statistic $c_R$ is proposed in this article based on the statistic $c_{min}$ , that is

$$c_R=\{\begin{array}{ll}\min \left\{{\widehat{a}}_{n,k}^{(1)},{\widehat{a}}_{n,k}^{(2)}\right\}\hfill & \arccos \left(v_{r,k}/{\widehat{v}}_{k-1|k-1}\right)\ge \eta \hfill \\ \mathrm{rand}\left\{{\widehat{a}}_{n,k}^{(1)},{\widehat{a}}_{n,k}^{(2)}\right\}& \arccos \left(v_{r,k}/{\widehat{v}}_{k-1|k-1}\right)<\eta \hfill \end{array}$$

(28)

where $\mathrm{rand}\{{\hat{a}}_{n,k}^{(1)},{\hat{a}}_{n,k}^{(2)}\}$ means to select a value between ${\hat{a}}_{n,k}^{(1)}$ and ${\hat{a}}_{n,k}^{(2)}$ randomly; $\eta \approx \pi /18$ is the threshold value.

Empirical distribution and GMM

The $i\mathrm{th}$ motion model is determined given ${\mathit{F}}^i$ and ${\mathit{Q}}_{k-1}^i$ . If the initial state ${\mathit{X}}_0^i$ and the $i\mathrm{th}$ motion model are set, we can generate the target real trajectory and radar measurement through the state space equation (1) and measurement equation (2). Use SCKF algorithm to obtain the state estimate. The statistic $c_R^i$ can be calculated by formulas (24)–(28). Use the Monte Carlo method for simulation experiment and then record $c_R^i$ to build the empirical distribution $g(c_R^i)$ . In this article, we use the mixture Gauss model function $G(c_R^i)$ to approximate $g(c_R^i)$ . The unknown parameters in $G(c_R^i)$ can be learned by EM algorithm. A mixture model for one-dimensional Gauss distribution function is

$$p\left(x|\mathbf{\psi }\right)={\displaystyle \sum _{j=1}^K{\lambda }_{j}p\left(x|{\mu }_j,{\sigma }_j\right)}$$

(29)

$$p\left(x|{\mu }_j,{\sigma }_j\right)=\frac{1}{\sqrt{2\pi }{\sigma }_j}{e}^{-\frac{{\left(x-{\mu }_j\right)}^2}{2_{{\sigma }_j}^2}}$$

(30)

where $x=c_R$ ; $\mathbf{\psi }=\{{\lambda }_j,{\mu }_j,{\sigma }_j{\}}_{j=1}^K$ are the parameters to be learned and ${\mathbf{\gamma }}_j=\{{\mu }_j,{\mu }_j,{\sigma }_j\}$ ; $\mathit{x}=\{x_i{\}}_{i=1}^N$ is the sample set. The log likelihood²⁰ of incomplete data is

$$\ln p(\mathit{x}|\mathbf{\psi })=\sum _{i=1}^N\ln \sum _{j=1}^K{\lambda }_{j}p(x_i|{\mu }_j,{\sigma }_j)$$

(31)

where $\mathit{y}=\{y_i{\}}_{i=1}^N$ is the non-observed data set and $\forall y_i\in U=\{1,2,...,M\}$ represents that the sample $(x_i)$ from the $y_i\mathrm{th}$ Gauss distribution function. The likelihood function of the complete data set $\{\mathit{x},\mathit{y}\}$ is

$$\begin{array}{c}\multicolumn{1}{c}{\ln p(\mathit{x},\mathit{y}|\mathbf{\psi })=\ln \underset{i=1}{\overset{N}{\Pi }}p(x_i,y_i|\mathbf{\psi })=\sum _{i=1}^N\ln p(x_i,y_i|\mathbf{\psi })}\\ \multicolumn{1}{c}{=\sum _{i=1}^N\ln {\lambda }_{y_i}p(x_i|{\mathbf{\gamma }}_{y_i})}\end{array}$$

(32)

The expression for the expectation

$$\begin{array}{c}Q(\mathbf{\psi }|{\hat{\mathbf{\psi }}}_k)=E_{\mathit{y}|\mathit{x},{\hat{\mathbf{\psi }}}_k}\ln p(\mathit{x},\mathit{y}|\mathbf{\psi })=E_{\mathit{y}|\mathit{x},{\hat{\mathbf{\psi }}}_k}\sum _{i=1}^N\ln {\lambda }_{y_i}p(x_i|{\mathbf{\gamma }}_{y_i})\hfill \\ =\sum _{y_i=1}^K\sum _{i=1}^N\left[\ln {\lambda }_{y_i}p(x_i|{\mathbf{\gamma }}_{y_i})\right]p(y_i|x_i,{\hat{\mathbf{\psi }}}_k)\hfill \\ =\sum _{j=1}^K\sum _{i=1}^N\left[\ln {\lambda }_{y_i}p(x_i|{\mathbf{\gamma }}_{y_i})\right]p(j|x_i,{\hat{\mathbf{\psi }}}_k)\hfill \end{array}$$

(33)

where $p(x_i|{\mathbf{\gamma }}_{y_i})$ is the one-dimensional Gauss distribution function as formula (30). And $p(j|x_i,{\hat{\mathbf{\psi }}}_k)$ is calculated by

$$p(j|x_i,{\hat{\mathbf{\psi }}}_k)=\frac{{\hat{\lambda }}_{j,k}{p}_j(x_i|{\hat{\mathbf{\gamma }}}_j)}{\sum _{s=1}^M{\hat{\lambda }}_{s,k}{p}_s(x_i|{\hat{\mathbf{\gamma }}}_s)}$$

(34)

The expression for maximum

$${\hat{\mathbf{\psi }}}_{k+1}=\arg \underset{\mathbf{\psi }}{max}Q(\mathbf{\psi }|{\hat{\mathbf{\psi }}}_k)$$

(35)

$$\sum _{j=1}^M{\lambda }_j=1$$

(36)

Use the Lagrange multiplier method to estimate parameter ${\hat{\mathbf{\psi }}}_{k+1}$ . ${\hat{\mathbf{\psi }}}_{k+1}$ is taken as the initial value for the next step. Repeat the above steps until convergence. Finally, we can obtain the parameters of the approximation distribution function $G(c_R^i)$ by EM algorithm.

IMM-SCKF-USP

The range rate measurement is applied into IMM algorithm on the double sides. First, it is used to update the state estimate; second, it is taken as the likelihood parameter to update the model probability. We mark this IMM algorithm as IMM-SCKF-updating the state estimation and the model probability (USP):

1. Reinitializing the model conditionally

   For the $j\mathrm{th}$ SCKF filter, the state and covariance matrix are reinitialized as

   $${\hat{\mathit{X}}}_{k-1|k-1}^{\mathrm{oj}}=\sum _{i=1}^M{\hat{\mathit{X}}}_{k-1|k-1}^i{\mu }_{k-1|k-1}^{(i,j)}$$

   (37)
   $$\begin{array}{c}\multicolumn{1}{c}{{\mathit{P}}_{k-1|k-1}^{\mathrm{oj}}=\sum _{i=1}^M[{\mathit{S}}_{k-1|k-1}^i({\mathit{S}}_{k-1|k-1}^i{)}^{\prime }+({\hat{\mathit{X}}}_{k-1|k-1}^i-{\hat{\mathit{X}}}_{k-1|k-1}^{\mathrm{oj}})}\\ \multicolumn{1}{c}{({\hat{\mathit{X}}}_{k-1|k-1}^i-{\hat{\mathit{X}}}_{k-1|k-1}^{\mathrm{oj}}{)}^{\prime }]{\mu }_{k-1|k-1}^{(i,j)}}\end{array}$$

   (38)

   where ${\hat{\mathit{X}}}_{k-1|k-1}^{\mathrm{oj}}$ is the mixture state estimate reinitialized of the $j\mathrm{th}$ SCKF filter; ${\mathit{P}}_{k-1|k-1}^{\mathrm{oj}}$ is the covariance matrix of ${\hat{\mathit{X}}}_{k-1|k-1}^{\mathrm{oj}}$ ; ${\hat{\mathit{X}}}_{k-1|k-1}^i$ is the state estimate of the $i\mathrm{th}$ SCKF filter; ${\mathit{S}}_{k-1|k-1}^i$ is the square root of the covariance matrix of ${\hat{\mathit{X}}}_{k-1|k-1}^i$ ; ${\mu }_{k-1|k-1}^{(i,j)}$ is the mixture probability

   $${\mu }_{k-1|k-1}^{(i,j)}=\frac{1}{{\overline{C}}_j}{p}_{\mathrm{ij}}{\mu }_{k-1}^i,{\overline{C}}_j=\sum _{i=1}^M{p}_{\mathrm{ij}}{\mu }_{k-1}^i$$

   (39)

   where $p_{\mathrm{ij}}$ is the transition probability; ${\mu }_{k-1}^i$ is the $i\mathrm{th}$ motion model probability at time $k-1$ .

2. Updating the state estimates

   Given the initial condition $\{{\hat{\mathit{X}}}_{k-1|k-1}^{\mathrm{oi}},{\mathit{P}}_{k-1|k-1}^{\mathrm{oi}}{\}}_{i=1}^M$ of the each model, the estimates $\{{\hat{\mathit{X}}}_{k|k}^i,{\mathit{S}}_{k|k}^i{\}}_{i=1}^M$ can be obtained by SCKF.

3. Updating the model probability

   Calculate the estimate value $c_{R,k}^i$ of statistic $c_R^i$ at time $k$ by formulas (24)–(28)

   $${\Phi }_k^i=p({\overline{\mathit{z}}}_k^i|{\mathit{Z}}^{k-1})G(c_{R,k}^i|c_R^i)$$

   (40)
   $${\mu }_k^i=\frac{1}{C}{\overline{C}}_i\log {\Phi }_k^i,C=\sum _{i=1}^M{\overline{C}}_i\log {\Phi }_k^i,{\overline{C}}_i=\sum _{j=1}^M{p}_{\mathrm{ji}}{\mu }_{k-1}^j$$

   where $p({\overline{\mathit{z}}}_k^i|{\mathit{Z}}^{k-1})$ is the likelihood function of the measurement prediction residual ${\overline{\mathit{z}}}_k^i$ that is assumed to be the Gauss distribution with zero mean, such as formula (41). $G(c_{R,k}^i|c_R^i)$ is the likelihood function of $c_R^i$ that is a GMM

   $$\begin{array}{c}\multicolumn{1}{c}{p({\overline{\mathit{z}}}_k^i|{\mathit{Z}}^{k-1})={\left|2\pi {\mathit{S}}_{Z,k}^i{\left({\mathit{S}}_{Z,k}^i\right)}^{\prime }\right|}^{-1/2}}\\ \multicolumn{1}{c}{\exp \{-\frac{1}{2}{\left({\overline{\mathit{z}}}_k^i\right)}^{\prime }{\left[{\mathit{S}}_{Z,k}^i{\left({\mathit{S}}_{Z,k}^i\right)}^{\prime }\right]}^{-1}{\overline{\mathit{z}}}_k^i\}}\end{array}$$

   (41)

   where ${\mathit{S}}_{Z,k}^i$ is the square root of the covariance matrix of the measurement prediction residual by SCKF.

4. Estimation fusion

The overall estimate and overall estimation error covariance matrix at time $k$ are

$${\hat{\mathit{X}}}_{k|k}=\sum _{i=1}^M{\hat{\mathit{X}}}_{k|k}^i{\mu }_k^i$$

(42)

$$P_{k|k}={\displaystyle \sum _{i=1}^M\left[S_{k|k}^i{\left(S_{k|k}^i\right)}^{\prime }+\left({\widehat{X}}_{k|k}-{\widehat{X}}_{k|k}^i\right){\left({\widehat{X}}_{k|k}-{\widehat{X}}_{k|k}^i\right)}^{\prime }\right]}{\mu }_k^i$$

(43)

The main difference between IMM-SCKF-USP algorithm and standard IMM algorithm is the likelihood function designed at step (3). The standard IMM algorithm only takes the likelihood function of the measurement prediction residual ${\overline{\mathit{z}}}_k^i$ , that is $p({\overline{\mathit{z}}}_k^i|{\mathit{Z}}^{k-1})$ , to update the model probability. In general nonlinear filters, the assumption that measurement prediction residual obeys the Gauss distribution of zero mean is not fully established. In this article, the likelihood function of acceleration statistic $c_R$ is taken as a correction factor of standard likelihood function to improve the efficiency of updating the model probability.

Simulation experiment

Simulation settings

The initial state of target $(x_0,v_{x,0},y_0,v_{y,0})$ is set as $(50\mathrm{km},300\mathrm{m}/\mathrm{s},40\mathrm{km},50\mathrm{m}/\mathrm{s})$ . The Doppler radar is located at the coordinate system origin and its scanning period is set as $T=1\mathrm{s}$ . The target movement process is divided into three phases: in 0–50 s, the target carries on a uniform speed motion in a straight line; in 51–100 s, the target carries on the uniform turning motion with the turning rate ${\omega }_1=-0.05\mathrm{rad}/\mathrm{s}$ ; in 101–150 s, the target carries on the uniform turning motion with the turning rate ${\omega }_2=0.05\mathrm{rad}/\mathrm{s}$ . And the real trajectory is shown in Figure 1.

Figure 1.

The real trajectory of target.

The standard deviations of radar measurement noise are set as ${\sigma }_r=50\mathrm{m}$ , ${\sigma }_{\theta }=0.003\mathrm{rad}$ and ${\sigma }_{v_r}=0.5\mathrm{m}/\mathrm{s}$ . Then the covariance ${\mathit{R}}_k$ can be determined with $\rho =0.4$ .

Corresponding to the true motion of target, the motion model set of IMM algorithm includes one constant velocity (CV) motion model ${\mathit{F}}^1$ and two constant turning (CT) motion models ${\mathit{F}}^2$ with ${\omega }_1=-0.05\mathrm{rad}/\mathrm{s}$ and ${\mathit{F}}^3$ ${\omega }_2=0.05\mathrm{rad}/\mathrm{s}$ , respectively, as formulas (44)

$$\begin{array}{c}\multicolumn{1}{c}{{\mathit{F}}^1=\left[\begin{array}{cccc}1\hfill & T\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 1\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill & T\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 1\hfill \end{array}\right],}\\ \multicolumn{1}{c}{{\mathit{F}}^2\mathrm{or}{\mathit{F}}^3=\left[\begin{array}{cccc}1\hfill & \frac{\sin ({\omega }_{1\mathrm{or}2}T)}{{\omega }_{1\mathrm{or}2}}\hfill & 0\hfill & \frac{\cos ({\omega }_{1\mathrm{or}2}T)-1}{{\omega }_{1\mathrm{or}2}}\hfill \\ \multicolumn{1}{c}{0}& \cos ({\omega }_{1\mathrm{or}2}T)\hfill & 0\hfill & -\sin ({\omega }_{1\mathrm{or}2}T)\hfill \\ \multicolumn{1}{c}{0}& -\frac{\cos ({\omega }_{1\mathrm{or}2}T)-1}{{\omega }_{1\mathrm{or}2}}\hfill & 1\hfill & \frac{\sin ({\omega }_{1\mathrm{or}2}T)}{{\omega }_{1\mathrm{or}2}}\hfill \\ \multicolumn{1}{c}{0}& \sin ({\omega }_{1\mathrm{or}2}T)\hfill & 0\hfill & \cos ({\omega }_{1\mathrm{or}2}T)\hfill \end{array}\right]}\end{array}$$

(44)

The corresponding noise covariance matrixes ${\mathit{Q}}^1$ , ${\mathit{Q}}^2$ , and ${\mathit{Q}}^3$ respectively are

$${\mathit{Q}}^1=\mathit{Q}(q=10),{\mathit{Q}}^2=\mathit{Q}(q=500),{\mathit{Q}}^2=\mathit{Q}(q=500)$$

$$\mathit{Q}\left(q\right)=q·\left[\begin{array}{cccc}{T}^3/3\hfill & T^2/2\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{T^2/2}& T\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & T^3/3\hfill & T^2/2\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & T^2/2\hfill & T\hfill \end{array}\right]$$

(45)

The initial model probability is set as $p_0=(0.7,0.15,0.15)$ . The model transition probability $p_M$ and initial state estimation covariance matrix ${\mathit{P}}_0$ are

$$p_M=\left[\begin{array}{ccc}0.8\hfill & 0.1\hfill & 0.1\hfill \\ \multicolumn{1}{c}{0.1}& 0.8\hfill & 0.1\hfill \\ \multicolumn{1}{c}{0.1}& 0.1\hfill & 0.8\hfill \end{array}\right],{\mathit{P}}_0=\left[\begin{array}{cccc}1000\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 5\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1000\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 5\hfill \end{array}\right]$$

The results of empirical distributions and GMMs

The 100 time Monte Carlo simulations are carried out for each motion model ${\mathit{F}}^1$ , ${\mathit{F}}^2$ , and ${\mathit{F}}^3$ . SCKF algorithm is used to track the target. $c_R^1$ , $c_R^2$ , and $c_R^3$ are calculated and recorded, respectively.

In order to illustrate the specific steps of the simulation experiment, we take an example of obtaining the empirical distribution $g(c_R^1)$ :

1. According to the parameters set in section “Simulation settings,” let the target moves 100 times with the motion model ${\mathit{F}}^1$ and the duration of each movement is 40 s to generate 100 radar measurement sequences.

2. Use SCKF algorithm with the motion model ${\mathit{F}}^1$ to deal with 100 radar measurement sequences and obtain 100 target state estimation sequences.

3. Use each radar measurement sequence in 10–30 s and corresponding state estimation sequences to calculate $c_R^1$ and totally record 2000 $c_R^1$ for building the empirical distribution function $g(c_R^1)$ .

After having the empirical distribution functions $g(c_R^1)$ , $g(c_R^2)$ , and $g(c_R^3)$ , GMMs $G(c_R^1)$ , $G(c_R^2)$ , and $G(c_R^3)$ are applied to fit $g(c_R^1)$ , $g(c_R^2)$ , and $g(c_R^3)$ , respectively, as shown in Figure 2. Their unknown parameters are learned by EM algorithm, as shown in Table 1.

Figure 2.

The empirical distribution functions and GMMs: (a) $g(c_R^1)$ and $G(c_R^1)$ , (b) $g(c_R^2)$ and $G(c_R^2)$ , and (c) $g(c_R^3)$ and $G(c_R^3)$ .

Table 1.

The parameters of GMMs.

Motion model	Mean	Variance	Weight
${\mathit{F}}^1$	−0.125	80.4277	0.8415
	−0.004	1.0904	0.1585
${\mathit{F}}^2$	−5.8611	18.2723	0.6245
	−16.0182	2.0813	0.3755
${\mathit{F}}^3$	5.2592	20.3804	0.6473
	16.3574	2.3400	0.3527

Tracking performance comparison

We compare IMM-SCKF-USP algorithm proposed in this article with the popular IMM-SCKF, IMM-UKF, IMM-EKF, and IMM-CMKF that all use the range rate measurement to update the state estimate.

The Sigma points and corresponding weights of UKF algorithm can be calculated by

$$\{\begin{array}{c}{\mathit{X}}^0=\hat{\mathit{X}}\hfill \\ \multicolumn{1}{c}{{\mathit{X}}^i=\hat{\mathit{X}}+{\left(\sqrt{(n+\lambda )\mathit{P}}\right)}_i,i=1~n}\\ \multicolumn{1}{c}{{\mathit{X}}^i=\hat{\mathit{X}}-{\left(\sqrt{(n+\lambda )\mathit{P}}\right)}_i,i=n+1~2n}\end{array}$$

(46)

$$\{\begin{array}{c}{\omega }_m^0=\frac{\lambda }{n+\lambda }\hfill \\ \multicolumn{1}{c}{{\omega }_c^0=\frac{\lambda }{n+\lambda }+(1-{\alpha }^2+\beta )}\\ \multicolumn{1}{c}{{\omega }_m^i={\omega }_c^i=\frac{\lambda }{2(n+\lambda )},i=1~2n}\end{array}$$

(47)

where $\hat{\mathit{X}}$ is the state estimate; $\mathit{P}$ is the covariance; ${\mathit{X}}^i$ is the $2n+1$ Sigma points; $n$ is the state dimension; ${\omega }_m^i$ is the weight. The parameters in formulas (46) and (47) are set as $\lambda =-1$ , $\alpha =1$ , $\beta =2$ , and $n=4$ . The specific steps of UKF algorithm can refer to Wan and Van Der Merwe. ¹⁶

The Jacobian matrix of nonlinear measurement function of EKF algorithm ¹³ is

$$\overline{\mathit{H}}=\left[\begin{array}{cccc}\frac{x_k}{\sqrt{x_k^2+y_k^2}}\hfill & \frac{y_k}{\sqrt{x_k^2+y_k^2}}\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{\frac{-y_k}{x_k^2+y_k^2}}& \frac{x_k}{x_k^2+y_k^2}\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{\frac{y_k^2{v}_{x,k}-x_k{y}_k{v}_{y,k}}{\sqrt[3/2]{x_k^2+y_k^2}}}& \frac{x_k^2{v}_{y,k}-x_k{y}_k{v}_{x,k}}{\sqrt[3/2]{x_k^2+y_k^2}}\hfill & \frac{x_k}{\sqrt{x_k^2+y_k^2}}\hfill & \frac{y_k}{\sqrt{x_k^2+y_k^2}}\hfill \end{array}\right]$$

(48)

CMKF algorithm proposed by Bordonaro et al. ¹⁵ is used. The root mean square error (RMSE) of state estimate is used to measure the performance of the tracking algorithm. The RMSE of position and RMSE of velocity are respectively

$$\mathrm{RMSE}({\hat{x}}_k,{\hat{y}}_k)=\sqrt{\frac{1}{N}\sum _{i=1}^N[{(x_k-{\hat{x}}_k)}^2+{(y_k-{\hat{y}}_k)}^2]}$$

(49)

$$RMSE\left({\widehat{v}}_{x,k},{\widehat{v}}_{y,k}\right)=\sqrt{\frac{1}{N}{\displaystyle \sum _{i=1}^N\left[{\left(v_{x,k}-{\widehat{v}}_{x,k}\right)}^2+{\left(v_{y,k}-{\widehat{v}}_{y,k}\right)}^2\right]}}$$

(50)

where $N$ is the number of Monte Carlo simulation; $x_k$ and $y_k$ are the real position components; ${\hat{x}}_k$ and ${\hat{y}}_k$ are the estimation of position components; $v_{x,k}$ and $v_{y,k}$ are the real velocity components; ${\hat{v}}_{x,k}$ and ${\hat{v}}_{y,k}$ are the estimation of velocity components.

For the each IMM algorithm, we carry out 100 Monte Carlo simulations and the RMSE of position and RMSE of velocity are calculated and shown in Figure 3. Figure 4 shows the motion model probability distributions and tracking results of each IMM algorithm at 50th Monte Carlo simulation.

Figure 3.

(a) RMSE of position and (b) RMSE of velocity for each IMM algorithm.

Figure 4.

Tracking results of each IMM algorithm at 50th Monte Carlo simulation: (a) model probability of IMM-SCKF-USP, (b) tracking result, (c) model probability of IMM-SCKF, (d) tracking result, (e) model probability of IMM-UKF, (f) tracking result, (g) model probability of IMM-CMKF, (h) tracking result, (i) model probability of IMM-EKF, and (j) tracking result.

From Figures 3 and 4, except IMM-EKF algorithm, the other four IMM algorithms can accurately track the target. It also suggests that SCKF, UKF, and CMKF filters can solve the target tracking problem with the strongly nonlinear measurement function. EKF algorithm approximates linearly formula (3) by the first-order Taylor formula and however cannot obtain the good approximation effect.

IMM-SCKF-USP algorithm at the target maneuver phase, namely during 51–150 s, has the better tracking precision than other four IMM algorithms. And at the non-maneuver phase, it has the comparable tracking precision with others. The main reason is that IMM-SCKF-USP algorithm adopts the likelihood function of normal acceleration statistics to modify the process of updating the model probability that leads to a suited probability distribution of the motion model. The model probability distribution of IMM-SCKF-USP algorithm shown in Figure 4(a) is more close to the real target motion compared with other four IMM algorithms as shown in Figure 4(c), (e), and (g). And at the model switching phase that is close to 51 or 101 s, IMM-SCKF-USP algorithm is more sensitive. The model probability distribution with EKF algorithm has the obvious anomaly as shown in Figure 4(i). In order to reasonably illustrate the function of modification for updating the model probability, we adopt the average Kullback-Leibler (KL) divergence ²¹ to measure the difference between the model probability distribution of IMM algorithm and the true target motion model probability distribution.

The true motion model probability distribution is

$$\begin{array}{c}p(F_k=F^1)=1,p(F_k=F^2)=0,\hfill \\ p(F_k=F^3)=0k=1,2,...,50\hfill \\ p(F_k=F^1)=0,p(F_k=F^2)=1,\hfill \\ p(F_k=F^3)=0k=51,1,...,100\hfill \\ p(F_k=F^1)=0,p(F_k=F^2)=0,\hfill \\ p(F_k=F^3)=1k=101,102,..,150\hfill \end{array}$$

(51)

where $p(F_k=F^i)$ is the probability that the motion model $F_k$ is $F^i$ at time $k$ , $i=1,2,3$ . By formula (40), we know that the probability distribution of motion model is ${\mu }_k^i$ , $i=1,2,3$ at time $k$ . Then the average KL divergence from ${\mu }_k^i$ to $p(F_k=F^i)$ at time $k$ is

$${\overline{D}}_k=\frac{1}{N}\sum _{j=1}^N\sum _{i=1}^{3}p(F_k=F^i){\log }_2\left(p(F_k=F^i)/{\mu }_k^i\right)$$

(52)

If $p(F_k=F^i)=0$ , then

$$p(F_k=F^i){\log }_2\left(p(F_k=F^i)/{\mu }_k^i\right)=0$$

(53)

Figure 5 shows the average KL divergence of the five IMM algorithms with 100 Monte Carlo simulations at every moment. It can be seen that the model probability distribution of IMM-SCKF-USP algorithm during the motion model stable phase has a lower KL divergence.

Figure 5.

Average KL divergence: (a) average KL divergence in 0–150 s, (b) average KL divergence in 0–50 s, (c) average KL divergence in 51–100 s, and (d) average KL divergence in 101–150 s.

Conclusion

The purpose of this article is to develop the range rate measurement potential in the IMM algorithm for the maneuvering target tracking problem. On the state estimate side, the range rate measurement is introduced into the measurement equation to improve the precision of target state estimation. In order to solve the problem that the measurement equation is strongly nonlinear, this article uses the popular SCKF algorithm as the filter in IMM algorithm. In the simulation experiment, it is verified that the IMM-SCKF has the similar nonlinear approximation performance with IMM-UKF algorithm, as shown in Figures 3 and 4(c)–(f). The focus of this article is that the maneuvering information extracted from the range rate measurement is applied into updating the motion model probability. We renew to establish the relationship between the normal accelerate and range rate. Through the Monte Claro simulation and EM algorithm, the approximate GMM of the probability distribution of the normal accelerate statistic can be obtained. And the approximate GMM is combined with the probability distribution of measurement prediction residual error to calculate the likelihood function of motion model. Then, a novel IMM-SCKF-USP algorithm is proposed in section “IMM-SCKF-USP.” Simulation experimental results show that IMM-SCKF-USP algorithm compared with other IMM algorithms has the better performance of tracking maneuvering target.