## 3 Target Tracking Method Based on EKF
\begin{bmatrix}-\left(\mathbf{x}_{k}\left(1,1 \right)-\mathbf{x}_{k}\left(3,1\right)\right)\cdot sin\left(\mathbf{v}_{v,k}\right)+ \left(\mathbf{x}_{k}\left(2,1\right)-\mathbf{x}_{k}\left(5,1\right)\right)\cdot cos \left(\mathbf{v}_{v,k}\right)\\ \left(\mathbf{x}_{k}\left(1,1\right)-\mathbf{x}_{k}\left(3,1\right)\right)\cdot cos \left(\mathbf{v}_{v,k}\right)+\left(\mathbf{x}_{k}\left(2,1\right)-\mathbf{x}_{k}\left(5,1 \right)\right)\cdot sin\left(\mathbf{v}_{v,k}\right)\end{bmatrix}  \tag{4}\]  
where \(\mathbf{v}_{k}\) is uncorrelated, zero-mean white Gaussian noise with known second order covariance matrix. The measurement noise covariance matrix is denoted as \(\mathbf{R}\). The target tracking algorithm based on EKF is described as follows:  
\[\mathbf{x}_{k}^{-}=\mathbf{F}\cdot\mathbf{x}_{k-1}+\mathbf{B}_{k}+\mathbf{\Gamma}_{k} \cdot\mathbf{w}_{k} \tag{5}\] \[\mathbf{P}_{k}^{-}=\mathbf{F}_{k}\cdot\mathbf{P}_{k-1}\cdot\mathbf{\Gamma}_{k}^{ T}+\mathbf{\Gamma}_{k}\cdot\mathbf{Q}\cdot\mathbf{\Gamma}_{k}^{T}\] (6) \[\mathbf{\hat{z}}_{k}=h\left(\mathbf{x}_{k}^{-}\right)\] (7) \[\mathbf{S}_{k}=\mathbf{H}_{k}\cdot\mathbf{P}_{k}^{-}\cdot\mathbf{H}_{k}^{T}+\mathbf{R}\] (8) \[\mathbf{K}_{k}=\mathbf{P}_{k}^{-}\cdot\mathbf{H}_{k}^{T}\cdot\mathbf{S}_{k}^{-1}\] (9) \[\mathbf{x}_{k}=\mathbf{x}_{k}^{-}+\mathbf{K}_{k}\cdot\left(\mathbf{z}_{k}-\mathbf{ \hat{z}}_{k}\right)\] (10) \[\mathbf{P}_{k}=\left(\mathbf{I}-\mathbf{K}_{k}\cdot\mathbf{H}\right)\cdot\mathbf{P}_ {k}^{-} \tag{11}\]  
where \(\mathbf{F},\mathbf{B}_{k},\mathbf{\Gamma}_{k},\mathbf{w}_{k},\mathbf{Q},\mathbf{R}\) have mentioned above. \(\mathbf{x}_{k-1}\) and \(\mathbf{P}_{k-1}\) are the state estimate and state covariance matrix at time index \(k-1\), respectively. \(\mathbf{x}_{k}^{-}\) and \(\mathbf{P}_{k}^{-}\) are the prediction state estimate and prediction state covariance matrix at time index \(k\), respectively. \(\mathbf{x}_{k}\) and \(\mathbf{P}_{k}\) are the updated state estimate and updated state covariance matrix at time index \(k\), respectively. \(\mathbf{\hat{z}}_{k}\) and \(\mathbf{z}_{k}\) are the predicted measurement and actual measurement at time index \(k\), respectively. \(\mathbf{K}_{k}\) is the filter gain at time index \(k\). \(\mathbf{S}_{k}\) is an auxiliary variable. \(h\left(\cdot\right)\) is measurement model and \(\mathbf{H}\) is measurement matrix, which is given by:  
\[\mathbf{H}_{k}= \begin{bmatrix}-cos\left(\mathbf{\psi}_{v,k}\right)&-sin\left(\mathbf{ \psi}_{v,k}\right)&cos\left(\mathbf{\psi}_{v,k}\right)&0&sin\left(\mathbf{\psi}_{v,k} \right)&0\\ -sin\left(\mathbf{\psi}_{v,k}\right)&cos\left(\mathbf{\psi}_{v,k}\right)&sin\left(\bm {\psi}_{v,k}\right)&0&-cos\left(\mathbf{\psi}_{v,k}\right)&0\end{bmatrix}  \tag{12}\]  
According to the state vector \(\mathbf{x}_{k}\), we can calculate the forward speed and heading of MRS at time index \(k\). \[u_{f,k}=\sqrt{\mathbf{x}_{k}\left(4,1\right)^{2}+\mathbf{x}_{k}\left(6,1 \right)^{2}} \tag{13}\] \[\mathbf{\psi}_{f,k}= \begin{cases}acos\left(\frac{\mathbf{x}_{k}\left(4,1 \right)}{\sqrt{\mathbf{x}_{k}\left(4,1\right)^{2}+\mathbf{x}_{k}\left(6,1\right)^{2} }}\right)&if\ \frac{\mathbf{x}_{k}\left(6,1\right)}{\sqrt{\mathbf{x}_{k}\left(6,1\right)^{2}+\mathbf{x} _{k}\left(6,1\right)^{2}}}>0\\ 2\pi-acos\left(\frac{\mathbf{x}_{k}\left(4,1\right)}{\sqrt{\mathbf{x}_{k}\left(4,1 \right)^{2}+\mathbf{x}_{k}\left(6,1\right)^{2}}}\right)&else\end{cases}  \tag{14}\]