匀速圆周运动-目标运动模型/机动目标跟踪

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

WX: ZB823618313

匀速圆周运动

• 匀速圆周运动-目标运动模型/机动目标跟踪
• • 1. 对机动目标跟踪的理解
  • 2. 目标模型概述
  • 3. 匀速圆周运动CT
  • • 3.1 匀速圆周运动CT模型(连续)
    • 3.2 匀速圆周运动CT模型(离散)
    • 3.3 匀速圆周运动CT模型(转弯角速度未知)
  • 4 匀速圆周运动CT模型目标跟踪
  • • 4.1 卡尔曼滤波跟踪CT目标
    • 4.2 扩展卡尔曼滤波跟踪
    • 4.3 无迹卡尔曼滤波跟踪
  • 5 匀加速运动CA模型
  • 6 匀速转弯CT模型
  • 7 Singer模型
  • 8 “当前”统计模型
  • 9 Jerk统计模型

1. 对机动目标跟踪的理解

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

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

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

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

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

2. 目标模型概述

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

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

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

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

本博客主要讲下面的模型：

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

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

3. 匀速圆周运动CT

CT运动模型是用来模拟目标进行角速度$\Omega (t)$恒定的转弯运动时的运动模型。与CT模型相对应的状态向量为
X=[x, x ˙ ,y, y ˙ ,,Ω(t) ] T {X}=[x, \dot{x}, y, \dot{y},,\Omega(t)]^TX=[x,x˙,y,y˙​,,Ω(t)]T
式中，$y(t)$和 y˙ ( t ) \dot{y}(t) y˙​(t)是与$x$垂直方向上的速度和加速度。假设其角速度$\Omega (t)$在一定时间段内是恒定的，则该转弯模型为匀速转弯模型，匀速转弯模型有个优良的性质：
在$\Omega (t)$已知是，CT模型是线性模型，反之如果$\Omega (t)$未知，则为非线性模型

下面主要普遍应用的角速度已知的匀速转弯模型

3.1 匀速圆周运动CT模型(连续)

目标状态为：x维位置和速度加速度、y维位置速度加速度、即X = [ x ,x˙ ， y ,y˙ ]T {X}=[x, \dot{x}，y, \dot{y}]^T X=[x,x˙，y,y˙​]T

CT模型为：
X˙ ( t ) =[ 0 1 0 0 0 0 0 − ω 0 0 0 1 0 ω 0 0 ] X ( t ) +[ 0 0 1 0 0 0 0 1 ] W ( t ) \dot{X}(t)=\begin{bmatrix}0&1&0&0\\0&0&0&-\omega\\0&0&0&1\\0&\omega&0&0 \end{bmatrix}X(t) + \begin{bmatrix}0&0\\1&0\\0&0\\0&1\end{bmatrix}W(t) X˙(t)=⎣⎢⎢⎡​0000​100ω​0000​0−ω10​⎦⎥⎥⎤​X(t)+⎣⎢⎢⎡​0100​0001​⎦⎥⎥⎤​W(t)

3.2 匀速圆周运动CT模型(离散)

离散化上述模型：

X k + 1 =[ 1 sin ⁡ ( ω T ) ω 0 − 1−cos⁡(ωT) ω 0 cos ⁡ ( ω T ) 0 − sin ⁡ ( ω T ) 0 1 − cos ⁡ ( ω T ) ω 1 sin ⁡ ( ω T ) ω 0 sin ⁡ ( ω T ) 0 cos ⁡ ( ω T ) ] Xk +[ T 2 / 2 0 T 0 0 T 2 / 2 0 T ] Wk X_{k+1}=\begin{bmatrix}1&\frac{\sin(\omega T)}{\omega}&0&-\frac{1-\cos(\omega T)}{\omega}\\0&\cos(\omega T)&0&-\sin(\omega T)\\0&\frac{1-\cos(\omega T)}{\omega}&1&\frac{\sin(\omega T)}{\omega}\\0&\sin(\omega T)&0&\cos(\omega T)\end{bmatrix}X_{k} + \begin{bmatrix}T^2/2&0\\T&0\\0&T^2/2\\0&T\end{bmatrix}W_k Xk+1​=⎣⎢⎢⎡​1000​ωsin(ωT)​cos(ωT)ω1−cos(ωT)​sin(ωT)​0010​−ω1−cos(ωT)​−sin(ωT)ωsin(ωT)​cos(ωT)​⎦⎥⎥⎤​Xk​+⎣⎢⎢⎡​T2/2T00​00T2/2T​⎦⎥⎥⎤​Wk​

令
Fk =[ 1 sin ⁡ ( ω T ) ω 0 − 1−cos⁡(ωT) ω 0 cos ⁡ ( ω T ) 0 − sin ⁡ ( ω T ) 0 1 − cos ⁡ ( ω T ) ω 1 sin ⁡ ( ω T ) ω 0 sin ⁡ ( ω T ) 0 cos ⁡ ( ω T ) ] ,Gk =[ T 2 / 2 0 T 0 0 T 2 / 2 0 T ] F_k=\begin{bmatrix}1&\frac{\sin(\omega T)}{\omega}&0&-\frac{1-\cos(\omega T)}{\omega}\\0&\cos(\omega T)&0&-\sin(\omega T)\\0&\frac{1-\cos(\omega T)}{\omega}&1&\frac{\sin(\omega T)}{\omega}\\0&\sin(\omega T)&0&\cos(\omega T)\end{bmatrix}, G_k= \begin{bmatrix}T^2/2&0\\T&0\\0&T^2/2\\0&T\end{bmatrix} Fk​=⎣⎢⎢⎡​1000​ωsin(ωT)​cos(ωT)ω1−cos(ωT)​sin(ωT)​0010​−ω1−cos(ωT)​−sin(ωT)ωsin(ωT)​cos(ωT)​⎦⎥⎥⎤​,Gk​=⎣⎢⎢⎡​T2/2T00​00T2/2T​⎦⎥⎥⎤​
则CT 模型可以写为：
X k + 1 =Fk Xk +Gk Wk X_{k+1}=F_kX_{k} +G_kW_k Xk+1​=Fk​Xk​+Gk​Wk​
其中噪声方差有两种计算方式 q k 2 为 W k 的 方 差 q_k^2为W_k的方差 qk2​为Wk​的方差，第一种：
Qk =Gk ∗qk2 ∗GkT =qk2 [ T 4 / 4 T 3 / 2 0 0 T 3 / 2 T 2 0 0 0 0 T 4 / 4 T 3 / 2 0 0 T 3 / 2 T 2 ] Q_k=G_k*q_k^2*G_k^T=q_k^2\begin{bmatrix}T^4/4&T^3/2&0&0 \\T^3/2&T^2&0&0 \\0&0&T^4/4&T^3/2 \\0&0& T^3/2&T^2\end{bmatrix} Qk​=Gk​∗qk2​∗GkT​=qk2​⎣⎢⎢⎡​T4/4T3/200​T3/2T200​00T4/4T3/2​00T3/2T2​⎦⎥⎥⎤​

第二种计算方式，直接对连续CT系统离散化积分得到：
Qk =qk2 [ T 3 / 3 T 2 / 2 0 0 T 2 / 2 T 0 0 0 0 T 3 / 3 T 2 / 2 0 0 T 2 / 2 T ] Q_k=q_k^2\begin{bmatrix}T^3/3&T^2/2&0&0 \\T^2/2&T&0&0 \\0&0&T^3/3&T^2/2 \\0&0& T^2/2&T\end{bmatrix} Qk​=qk2​⎣⎢⎢⎡​T3/3T2/200​T2/2T00​00T3/3T2/2​00T2/2T​⎦⎥⎥⎤​

两种都可以使用，我更喜欢第一种

二维匀速运动CV目标航迹如下：

代码：

clear all; close all; clc;%% initial parametern=4; %dimension of the target ;M=3; %number of raderN=100; %the runs atimechan=6; %channel, for the class of fiterw_mu=[0,0]'; % mean of process noise v_mu=[0,0]'; % mean of measurement noise%covariance of process noiseq_x=0.1; %m/s^2q_y=q_x;Qk=diag([q_x^2,q_y^2]); % state matrixT_f=1; %sample time of fusion centerw=-pi/180*3;% 转完角速度Fk= [1 sin(w*T_f)/w 0 -(1-cos(w*T_f))/w0 cos(w*T_f)   0 -sin(w*T_f)0 (1-cos(w*T_f))/w 1 sin(w*T_f)/w0  sin(w*T_f) 0 cos(w*T_f) ]; %Gk= [ T_f^2/2  0T_f     00      T_f^2/20      T_f ]; %   % intial state x_bar=[3500,-130,200,00]';P_0=diag([5e6,10^4,5e6,10^4]); %initial covariancex_bar=[-1000,50,-800,100]';P_0=diag([1e5,1e3,1e5,1e3]); %initial covariance   x0=mvnrnd(x_bar,P_0); % 初始状态%x0=(x+normrnd(0,0.001)')';x=x0';for k=1:N   %% %%%%%%% target model %%%%%%%%%%%%%%%%%%%%   %% 目标运动学模型(被跟踪目标建模)，匀速运动CV模型    w=mvnrnd(w_mu',Qk)';%过程噪声方差    x=Fk*x+Gk*w;    sV(:,k,1,1)=x;end% 二维匀速圆周CT运动目标轨迹figureplot(sV(1,:,1,1),sV(3,:,1,1),'-*r','LineWidth',1)grid onxlabel('m');ylabel('m');legend('真实轨迹')title('匀速圆周CT运动目标轨迹')  

3.3 匀速圆周运动CT模型(转弯角速度未知)

转弯角速率$\omega$未知时,该模型为非线性模型，状态为
X = [ x ,x˙ , y ,y˙ , Ω ( t )]T {X}=[x, \dot{x},y, \dot{y},\Omega(t)]^T X=[x,x˙,y,y˙​,Ω(t)]T

离散模型为

X k + 1 =[ 1 sin ⁡ ( ω T ) ω 0 − 1−cos⁡(ωT) ω 0 0 cos ⁡ ( ω T ) 0 − sin ⁡ ( ω T ) 0 0 1 − cos ⁡ ( ω T ) ω 1 sin ⁡ ( ω T ) ω 0 0 sin ⁡ ( ω T ) 0 cos ⁡ ( ω T ) 0 0 0 0 0 1 ] Xk +[ T 2 / 2 0 0 T 0 0 0 T 2 / 2 0 0 T 0 0 0 T ] Wk X_{k+1}=\begin{bmatrix}1&\frac{\sin(\omega T)}{\omega}&0&-\frac{1-\cos(\omega T)}{\omega}&0\\0&\cos(\omega T)&0&-\sin(\omega T) & 0\\0&\frac{1-\cos(\omega T)}{\omega}&1&\frac{\sin(\omega T)}{\omega}&0 \\0&\sin(\omega T)&0&\cos(\omega T) &0\\ 0&0&0&0 &1\end{bmatrix}X_{k} + \begin{bmatrix}T^2/2&0&0\\T&0&0\\0&T^2/2&0\\0&T&0\\0&0&T\end{bmatrix}W_k Xk+1​=⎣⎢⎢⎢⎢⎡​10000​ωsin(ωT)​cos(ωT)ω1−cos(ωT)​sin(ωT)0​00100​−ω1−cos(ωT)​−sin(ωT)ωsin(ωT)​cos(ωT)0​00001​⎦⎥⎥⎥⎥⎤​Xk​+⎣⎢⎢⎢⎢⎡​T2/2T000​00T2/2T0​0000T​⎦⎥⎥⎥⎥⎤​Wk​

令
Fk =[ 1 sin ⁡ ( ω T ) ω 0 − 1−cos⁡(ωT) ω 0 0 cos ⁡ ( ω T ) 0 − sin ⁡ ( ω T ) 0 0 1 − cos ⁡ ( ω T ) ω 1 sin ⁡ ( ω T ) ω 0 0 sin ⁡ ( ω T ) 0 cos ⁡ ( ω T ) 0 0 0 0 0 1 ] ,Gk =[ T 2 / 2 0 0 T 0 0 0 T 2 / 2 0 0 T 0 0 0 T ] F_k=\begin{bmatrix}1&\frac{\sin(\omega T)}{\omega}&0&-\frac{1-\cos(\omega T)}{\omega}&0\\0&\cos(\omega T)&0&-\sin(\omega T) & 0\\0&\frac{1-\cos(\omega T)}{\omega}&1&\frac{\sin(\omega T)}{\omega}&0 \\0&\sin(\omega T)&0&\cos(\omega T) &0\\ 0&0&0&0 &1\end{bmatrix}, G_k=\begin{bmatrix}T^2/2&0&0\\T&0&0\\0&T^2/2&0\\0&T&0\\0&0&T\end{bmatrix} Fk​=⎣⎢⎢⎢⎢⎡​10000​ωsin(ωT)​cos(ωT)ω1−cos(ωT)​sin(ωT)0​00100​−ω1−cos(ωT)​−sin(ωT)ωsin(ωT)​cos(ωT)0​00001​⎦⎥⎥⎥⎥⎤​,Gk​=⎣⎢⎢⎢⎢⎡​T2/2T000​00T2/2T0​0000T​⎦⎥⎥⎥⎥⎤​
则CT 模型可以写为：
X k + 1 =Fk Xk +Gk Wk X_{k+1}=F_kX_{k} +G_kW_k Xk+1​=Fk​Xk​+Gk​Wk​
其中噪声方差有两种计算方式 q k 2 为 W k 的 方 差 q_k^2为W_k的方差 qk2​为Wk​的方差，第一种：
Qk =Gk ∗qk2 ∗GkT =qk2 [ T 4 / 4 T 3 / 2 0 0 0 T 3 / 2 T 2 0 0 0 0 0 T 4 / 4 T 3 / 2 0 0 0 T 3 / 2 T 2 0 0 0 0 0 T 2 ] Q_k=G_k*q_k^2*G_k^T=q_k^2\begin{bmatrix}T^4/4&T^3/2&0&0 &0 \\T^3/2&T^2&0&0 &0 \\0&0&T^4/4&T^3/2 &0 \\0&0& T^3/2&T^2&0 \\0&0&0&0&T^2\end{bmatrix} Qk​=Gk​∗qk2​∗GkT​=qk2​⎣⎢⎢⎢⎢⎡​T4/4T3/2000​T3/2T2000​00T4/4T3/20​00T3/2T20​0000T2​⎦⎥⎥⎥⎥⎤​

第二种计算方式，直接对连续CT系统离散化积分得到：
Qk =qk2 [ T 3 / 3 T 2 / 2 0 0 0 T 2 / 2 T 0 0 0 0 0 T 3 / 3 T 2 / 2 0 0 0 T 2 / 2 T 0 0 0 0 0 T ] Q_k=q_k^2\begin{bmatrix}T^3/3&T^2/2&0&0 &0 \\T^2/2&T&0&0 & 0 \\0&0&T^3/3&T^2/2 &0 \\0&0& T^2/2&T &0 \\0&0&0&0&T\end{bmatrix} Qk​=qk2​⎣⎢⎢⎢⎢⎡​T3/3T2/2000​T2/2T000​00T3/3T2/20​00T2/2T0​0000T​⎦⎥⎥⎥⎥⎤​

4 匀速圆周运动CT模型目标跟踪

4.1 卡尔曼滤波跟踪CT目标

代码联系WX: ZB823618313

4.2 扩展卡尔曼滤波跟踪

代码联系WX: ZB823618313

4.3 无迹卡尔曼滤波跟踪

代码联系WX: ZB823618313

5 匀加速运动CA模型

匀加速运动CA模型见另一个博客

6 匀速转弯CT模型

匀速转弯CT运动模型见另一个博客：包括二维、三维

7 Singer模型

Singer模型见另一个博客

8 “当前”统计模型

当前统计模型见另一个博客

9 Jerk统计模型

Jerk模型见另一个博客

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