（十二）机动目标跟踪-CS模型+EKF滤波算法

追风者...

已于 2025-01-02 08:31:02 修改

阅读量1.4k

点赞数 27

分类专栏：滤波算法仿真与实践文章标签：算法目标跟踪机动目标跟踪当前统计模型

于 2024-12-14 10:36:18 首次发布

本文链接：https://blog.youkuaiyun.com/h320130/article/details/140291187

版权

滤波算法仿真与实践专栏收录该内容

13 篇文章

订阅专栏

前言

机动目标的跟踪是目标跟踪中的难点问题，重点在于目标运动模型建立以及滤波器设计。本文利用当前统计模型（CS）对目标运动进行建模，利用扩展卡尔曼滤波（EKF）对目标运动状态进行滤波更新。对应的原理及仿真结果如下所示。

一、机动目标跟踪

常用的目标模型有以下几种：

1.常速模型（CV模型）

2.常加速模型（CA模型）

3.Jerk模型

4.Singer模型

5.当前统计模型（CS模型）

6.交互多模型（IMM）

……

卡尔曼滤波是雷达目标跟踪中常用的算法，但是只适用于线性系统。非线性量测系统的滤波算法有以下几种：

1.无迹卡尔曼滤波（UKF）

2.拓展卡尔曼滤波（EKF）

3.容积卡尔曼滤波（CKF）-平方跟容积卡尔曼滤波（SRCKF）

4.粒子滤波（PF）

5.强跟踪滤波（STF）

……

机动目标跟踪的重点：

1.目标运动模型的选择（如何描述机动目标运动）

2.滤波算法的设计及选择（如何滤波进行状态更新）

二、使用步骤

1.CS模型

当前统计模型从本质上说使用具有非零均值加速的Singer模型。该算法的核心思想是下一时刻加速的取值是有限的，总是在当前时刻的加速度附近，并服从瑞利分布。优点是分布随加速度均值变化而变化，可以实时修正加速度分布，并且通过方差反馈到下一时刻的滤波增益当中，实现了闭环自适应跟踪。缺点是CS模型依赖于机动频率和加速度极值，当目标实际加速度超过加速度覆盖范围时，算法的性能将急剧恶化。

CS模型可以表示为：

$X(k + 1) = F(k)X(k) + G(k)\bar a + V(k)$

其中， $X = {[x,\dot x,\ddot x]^{\rm T}}$ ， $x,\dot x,\ddot x$ 分别为目标在x轴上的距离、速度和机动加速度。 $\alpha$ 为机动频率。 $F(k)$ 为过程转移矩阵，即：

$F(k) =\begin{bmatrix}1 & T & {(\alpha T - 1 + {e^{ - \alpha T}})/{\alpha ^2}} \\ 0 & 1 & {(1 - {e^{ - \alpha T}})/\alpha } \\ 0 & 0 & {{e^{ - \alpha T}}} \\ \end{bmatrix}$

$G(k)$ 称为输入控制矩阵，即：

$G(k) = \begin{bmatrix} {{1 \over \alpha }( - T + {{\alpha {T^2}} \over 2} + {{1 - {e^{ - \alpha T}}} \over \alpha })} \\ {T - {{1 - {e^{ - \alpha T}}} \over \alpha }} \\ {1 - {e^{ - \alpha T}}} \end{bmatrix}$

过程噪声矩阵：

$Q(k) = {\rm{E}}(V(k)V'(k)) = 2\alpha \sigma _a^2\begin{bmatrix} {{q_{11}}} & {{q_{12}}} & {{q_{13}}} \\ {{q_{21}}} & {{q_{22}}} & {{q_{23}}} \\ {{q_{13}}} & {{q_{23}}} & {{q_{33}}} \end{bmatrix}$

其中，为 ${q_{ij}}(i = 1,2,3;j = 1,2,3)$ $\alpha$ 和 $T$ 的函数，具体为：

$\begin{cases} & {q_{11}} = (1 - {e^{ - 2\alpha T}} + 2\alpha T + {{2{\alpha ^3}} \over 3} - 2{\alpha ^2}{T^2} - 4\alpha T{e^{ - \alpha T}}) \\ & {q_{12}} = {1 \over {2{\alpha ^4}}}({e^{ - 2\alpha T}} + 1 - 2{e^{ - \alpha T}} + 2\alpha T{e^{ - \alpha T}} - 2\alpha T + {\alpha ^2}{T^2}) \\ & {q_{13}} = {1 \over {2{\alpha ^3}}}(1 - {e^{ - 2\alpha T}} - 2\alpha T{e^{ - \alpha T}}) \\ & {q_{22}} = {1 \over {2{\alpha ^3}}}(4{e^{ - \alpha T}} - 3 - {e^{ - 2\alpha T}} + 2\alpha T) \\ & {q_{23}} = {1 \over {2{\alpha ^2}}}({e^{ - 2\alpha T}} + 1 - 2{e^{ - \alpha T}}) \\ & {q_{33}} = {1 \over {2\alpha }}(1 - {e^{ - 2\alpha T}}) \end{cases}$

$\sigma _a^2$ 为机动加速度方差， $\bar a(k)$ 为机动加速度均值，即：

$\sigma _a^2 = {{4 - \pi } \over \pi }\left[ {{a_{\max }} - \bar a(k)} \right]$

$\bar a(k) = \hat {\ddot x}(k/k - 1)$

进一步预测方程为：

$\hat X(k/k - 1) = F(k)\hat X(k - 1/k - 1) + G(k)\bar a(k)$

当前统计模型算法已经具备了根据前一时刻的加速度估值来自自适应地调整过程噪声的能力，其更关心的是目标机动的当前统计特征，即当目标以某一加速度机动时，起下一时刻的加速度变化范围是有限的，只能在当前加速度的某一邻域内，因而与Singer模型算法相比，当前统计模型算法更能真实地反映目标机动范围和强度的变化。

对应代码为：

a = alpha;
T = Tar_dt;


F1 = [1 T (a*T-1+exp(-a*T))/(a^2);
      0 1 (1-exp(-a*T))/(a);
      0 0 -exp(-a*T)];

Tar_F=blkdiag(F1,F1);

q11 = (2*a^3*T^3-6*a^2*T^2+6*a*T+3-12*a*T*exp(-a*T)-3*exp(-2*a*T))/(6*a^5);
q12 = (a^2*T^2-2*a*T+1-2*(1-a*T)*exp(-a*T)+exp(-2*a*T))/(2*a^4);
q13 = (1- 2*a*T*exp(-a*T)-exp(-2*a*T))/(2*a^3);
q22 = (2*a*T-3+4*exp(-a*T)-exp(-2*a*T))/(2*a^3);
q23 = (1-2*exp(-a*T)+exp(-2*a*T))/(2*a^2);
q33 = (1-exp(-2*a*T))/(2*a);

Q1 = [q11  q12 q13; 
      q12 q22 q23;
      q13 q23 q33];
if x(3)>= 0
    theatx = (4-pi)*(ac(1)-x(3))^2/pi;
else
    theatx = (4-pi)*(ac(2)+x(3))^2/pi;
end

if x(6)>= 0
    theaty = (4-pi)*(ac(3)-x(6))^2/pi;
else
    theaty = (4-pi)*(ac(4)+x(6))^2/pi;
end

Qx = 2*a*theatx*Q1;
Qy = 2*a*theaty*Q1;

Tar_Q = blkdiag(Qx,Qy);

U1 = [(-T+a*T^2/2+(1-exp(-a*T))/a)/a;
         T-(1-exp(-a*T))/a ; 
         1-exp(-a*T)];

Tar_U = [[U1;zeros(3,1)] ,[zeros(3,1); U1]];

注意CS模型的两个特点：

1.当目标的当前加速度在加速度极值附近时，对应的加速度变化范围小。这与实际情况不符，进而导致较大的跟踪估计误差。

2.当目标无机动时，过程噪声最大，加速度的变化范围最大，导致该模型在无机动时具有较大的跟踪误差。

2.EKF滤波算法

算法流程详见对应博客：

多传感器拓展卡尔曼滤波（EKF）算法https://blog.youkuaiyun.com/h320130/article/details/134950758

对应代码为：

X_pre = X;
P_pre = P ;
H = calculateHjcobi(X_pre,Sta_address);
Z_pre = nolinerMeasurementFunction(X_pre,Sta_address);
%计算卡尔曼增益
K = (P_pre*H')/(H*P_pre*H'+ R);
X_output = X_pre+K*(Z-Z_pre);%状态更新
P_output = (eye(6)-K*H) * P_pre ;%方差更新