机动目标跟踪

原创不易，路过的各位大佬请点个赞

WX: ZB823618313

  机动目标跟踪一直是目标跟踪领域研究的难点和重点问题。建立目标运动模型和滤波算法是目标跟踪的两个重要因素。由于目标的机动具有不可预测性，使得我们很难建立精确的目标运动模型。如何建立一种有效的模型来反映目标真实的运动轨迹是高机动目标跟踪系统急需解决的问题。经过近三十年的研究，该领域取得了许多重要成果。

个人理解：机动目标跟踪拥有三要素：

被跟踪目标建模（也是本博客重点） 传感器测量（另一个博客介绍） 滤波器设计（见目标跟踪专栏）

  从算法层面，在目标跟踪系统中，常用的滤波算法是以卡尔曼滤波器为基本框架的估计算法。卡尔曼滤波器是一种线性、无偏、以误差均方差最小为准则的最优估计算法，它有精确的数学形式和优良的使用效能。卡尔曼滤波方法实质上是一种数据处理方法，它采用递推滤波方法，根据获取的量测数据由递推方程递推给出新的状态估计。由于计算量和存储量小，比较容易满足实时计算的要求，在工程实践中得到广泛应用。   除此之外，非线性滤波也广泛应用与机动目标跟踪，比如：

扩展卡尔曼滤波EKF 无迹卡尔曼滤波UKF 容积卡尔曼滤波CKF 求积卡尔曼滤波QKF 中心差分卡尔曼滤波CDKF Divided difference filter DDF 高斯混合滤波GSF 强跟踪滤波STF 粒子滤波PF … …

  机动目标模型描述了目标状态随着时间变化的过程。一个好的模型抵得上大量的数据。当前几乎所有的目标跟踪算法都是基于模型进行状态估计的。在卡尔曼滤波器被引入目标跟踪领域后，基于状态空间的机动目标建模成为主要研究对象之一。

目标的空间运动基于不同的运动轨迹和坐标系 一维运动 二维运动 三维运动

根据不同方向的运动是否相关 坐标间不耦合模型 坐标间耦合模型

坐标间不耦合模型： 这类模型假设三维空间三个正交方向上的目标机动过程不耦合。目标机动是飞行器受到外力作用而使得加速度变化所致，所以对机动建模的难点在于对目标加速度的描述。对于无机动目标，常速(Constant Velocity，CV〉模型常用于描述这类目标的运动，而常加速度(Constant Acceleration，CA）模型则常用于描述加速度趋近常数的机动目标的运动。

坐标间耦合模型： 坐标间耦合模型绝大多数情况下指的是转弯运动模型。由于此类模型与坐标密切相关，所以可以分为两类:二维转弯模型和三维转弯模型。二维转弯模型又称为平面转弯模型，即CT模型。

下面二维匀加速运动CA模型：

二维匀加速运动模型(连续)： 目标状态为：x维位置和速度加速度、y维位置速度加速度、即 X = [ x , x ˙ , x ¨ , y , y ˙ , y ¨ ] T {X}=[x, \dot{x},\ddot{x}, y, \dot{y},\ddot{y}]^T X=[x,x˙,x¨,y,y˙​,y¨​]T

CA模型为 X ˙ ( t ) = [ 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 ] X ( t ) + [ 0 0 0 0 1 0 0 0 0 0 0 1 ] W ( t ) \dot{X}(t)=\begin{bmatrix}0&1&0&0&0&0\\0&0&1&0&0&0\\0&0&0&0&0&0 \\0&0&0&0&1&0 \\0&0&0&0&0&1 \\0&0&0&0&0&0 \end{bmatrix}X(t) + \begin{bmatrix}0&0\\0&0\\1&0\\ 0&0\\0&0\\0&1\end{bmatrix}W(t) X˙(t)=⎣⎢⎢⎢⎢⎢⎢⎡​000000​100000​010000​000000​000100​000010​⎦⎥⎥⎥⎥⎥⎥⎤​X(t)+⎣⎢⎢⎢⎢⎢⎢⎡​001000​000001​⎦⎥⎥⎥⎥⎥⎥⎤​W(t) 为了方便，定义 A = [ 0 1 0 0 0 1 0 0 0 ] , B = [ 0 0 1 ] A=\begin{bmatrix}0&1&0\\0&0&1\\0&0&0\end{bmatrix}, B= \begin{bmatrix}0\\0\\1\end{bmatrix} A=⎣⎡​000​100​010​⎦⎤​,B=⎣⎡​001​⎦⎤​ 则上式变为 X ˙ ( t ) = [ A 0 0 A ] X ( t ) + [ B 0 0 B ] W ( t ) \dot{X}(t)=\begin{bmatrix}A&0\\0&A \end{bmatrix}X(t) + \begin{bmatrix}B&0\\0&B\end{bmatrix}W(t) X˙(t)=[A0​0A​]X(t)+[B0​0B​]W(t)

离散化上述模型：

二维匀加速运动模型(离散)： X k + 1 = [ 1 T T 2 / 2 0 0 0 0 1 T 0 0 0 0 0 1 0 0 0 0 0 0 1 T T 2 / 2 0 0 0 0 1 T 0 0 0 0 0 1 ] X k + [ T 2 / 2 0 T 0 1 0 0 T 2 / 2 0 T 0 1 ] W k X_{k+1}=\begin{bmatrix}1&T&T^2/2&0&0&0\\0&1&T&0&0&0\\0&0&1&0&0&0 \\0&0&0&1&T&T^2/2 \\0&0&0&0&1&T \\0&0&0&0&0&1 \end{bmatrix}X_{k} + \begin{bmatrix}T^2/2&0\\T&0\\1&0\\ 0&T^2/2\\0&T\\0&1\end{bmatrix}W_k Xk+1​=⎣⎢⎢⎢⎢⎢⎢⎡​100000​T10000​T2/2T1000​000100​000T10​000T2/2T1​⎦⎥⎥⎥⎥⎥⎥⎤​Xk​+⎣⎢⎢⎢⎢⎢⎢⎡​T2/2T1000​000T2/2T1​⎦⎥⎥⎥⎥⎥⎥⎤​Wk​

为了方便，定义如下矩阵； F = [ 1 T T 2 / 2 0 1 T 0 0 1 ] , G = [ T 2 / 2 T 1 ] F=\begin{bmatrix}1&T&T^2/2\\0&1&T\\0&0&1\end{bmatrix}, G= \begin{bmatrix}T^2/2\\T\\1\end{bmatrix} F=⎣⎡​100​T10​T2/2T1​⎦⎤​,G=⎣⎡​T2/2T1​⎦⎤​ 则上式变为

X k + 1 = [ F 0 0 F ] X k + [ G 0 0 G ] W k X_{k+1}=\begin{bmatrix}F&0\\0&F \end{bmatrix}X_{k} + \begin{bmatrix}G&0\\0&G\end{bmatrix}W_k Xk+1​=[F0​0F​]Xk​+[G0​0G​]Wk​ 白噪声 W k W_k Wk​的方差为连续白噪声 W ( t ) W(t) W(t)的功率谱强度 q q q。

很多参考文献也将二维的匀加速模型表述为如下形式 X k + 1 = [ F 0 0 F ] X k + W k X_{k+1}=\begin{bmatrix}F&0\\0&F \end{bmatrix}X_{k} +W_k Xk+1​=[F0​0F​]Xk​+Wk​ 即将form 1 中的 G G G矩阵直接合成到噪声 W W W中，此时噪声均值仍然不变，噪声方差如下 Q k = [ Q 0 0 Q ] Q_k=\begin{bmatrix}Q&0\\0&Q \end{bmatrix} Qk​=[Q0​0Q​] 其中 Q = [ T 5 / 20 T 4 / 8 T 3 / 6 T 4 / 8 T 3 / 3 T 2 / 2 T 3 / 6 T 2 / 2 T ] Q=\begin{bmatrix}T^5/20&T^4/8&T^3/6\\T^4/8&T^3/3&T^2/2 \\ T^3/6& T^2/2 &T \end{bmatrix} Q=⎣⎡​T5/20T4/8T3/6​T4/8T3/3T2/2​T3/6T2/2T​⎦⎤​

位置轨迹： 图1 速度轨迹： 图2 加速度轨迹： 图3

算法：卡尔曼滤波、扩展卡尔曼滤波 传感器：雷达 跟踪代码：加微信：ZB823618313 两个场景：低精度，高精度

在二维情况下，雷达量测为距离，方位角 r k m = r k + r ~ k b k m = b k + b ~ k {r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde{b}_k rkm​=rk​+r~k​bkm​=bk​+b~k​ 其中 r k = h r ( x k , v k ) = ( x k − x 0 ) + ( y k − y 0 ) 2 ) b k = h b ( x k , v k ) = tan ⁡ − 1 y k − y 0 x k − x 0 r_k=h_r(x_k,v_k)=\sqrt{(x_k-x_0)^+(y_k-y_0)^2)}\\ b_k=h_b(x_k,v_k)=\tan^{-1}{\frac{y_k-y_0}{x_k-x_0}}\\ rk​=hr​(xk​,vk​)=(xk​−x0​)+(yk​−y0​)2) ​bk​=hb​(xk​,vk​)=tan−1xk​−x0​yk​−y0​​ [ x 0 , y 0 ] [x_0,y_0] [x0​,y0​]为传感器（雷达）坐标，一般情况为0。雷达量测为 z k = [ r k , b k ] ′ z_k=[r_k,b_k]' zk​=[rk​,bk​]′。

跟踪轨迹图下图所示：

下面是跟踪RMSE曲线:评估跟踪精度

位置RMSE 速度RMSE

==原创不易，路过的各位大佬请点个赞=