【滤波跟踪】基于交互式多模型的UKF滤波机动目标跟踪问题研究附matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

更多Matlab仿真内容点击👇

智能优化算法  神经网络预测 雷达通信  无线传感器

信号处理 图像处理 路径规划 元胞自动机 无人机  电力系统

⛄ 内容介绍

本文研究的实例是一个二维平面的雷达，雷达可以探测到目标的径向距离和

方位角（如下图所示）。这个问题属于单目标跟踪问题，一般来说，如果目标做

匀速直线运动时，跟踪问题十分容易；但当目标做机动时，由于无法准确预知目

标下一时刻的运动状态，使得跟踪变得很困难。这就需要发展合适的目标运动模

型，现在的各种模型大致分为单模型和多模型方法，由于多模型较为复杂，这里

我们仅对单模型方法进行讨论。常用的单模型有匀速模型（CV）、匀加速模型

（CA）、Signer模型和均值自适应的“当前”统计模型（CS）；多模型有交互式

多模型（IMM）。现实世界中的大部分运动目标都存在各种机动，目标做匀速直

线飞行的概率很小，采用CV模型一般是不可取的，只有当目标做匀速直线飞行

或者近似匀速直线飞行时才能取得很好的效果。机动强度不大时，可以采用CA

模型或者Singer模型；机动强度较大时，采用CS模型后IMM可以取得较好的效果。

这里我们采用CA模型和交互式多模型。

雷达对目标的量测并不真实准确，而是存在一定的随机噪声干扰，一般假设

噪声符合高斯分布。目标跟踪一般在混合坐标系下进行，此时雷达的量测值是距

离和角度信息，而状态量一般为位置、速度和加速度。那么量测方程就是非线性

的，这就需要采用非线性滤波方法，常用的非线性滤波方法有扩展卡尔曼滤波

（EKF）、不敏卡尔曼滤波（UKF）和粒子滤波（PF）。EKF算法是较早发展的非

线性滤波算法，该算法是利用非线性方程在预测值附近泰勒展开，忽略高阶项得

到线性化的方程。UKF滤波的基本思想是认为近似非线性函数的分布要比近似非

线性函数本身要容易，该算法基于UT变换来近似非线性函数的分布然后采用标

准卡尔曼滤波算法的框架，相对于EKF算法，UKF具有以下优点：。PF算法采用

随机采样策略，需要的计算量较大，本文没有采用该算法

⛄ 部分代码

clear all;

clc

close all

load Measure.mat%加载量测数据

load Real.mat%加载真实值数据

Z=TimeRPhi(2:3,:);%量测值

Xr=TXYdXdYAxAyTPhi;%真值

T=0.1;

pai=[0.8 0.2;0.1 0.9];  %定义一步转移概率矩阵

miu_CV=0;            %匀速运动模型在初始时刻正确的概率

miu_CA=1;            %匀加速运动模型在初始时刻正确概率

%UKF滤波器初始化

alf=0.001;

beta=2;

i=6;

a=((alf)^2-1)*i;

Wm0=a/(i+a);

Wc0=a/(i+a)+(1-(alf)^2+beta);

Wm=1/(2*(i+a));  %求一阶统计特性时的权系数

Wc=1/(2*(i+a));  %求二阶统计特性时的权系数

X1=[10000;0;0;0;0;0];%状态变量初值

P1=diag([0.1,0.1,0.1,0.1,0.1,0.1]);%误差协方差初值

Rk1=diag([100,(0.1/57.3)^2]);%量测噪声方差

% Ta1=[T^2/2 0 T 0 1 0;

%     0 T^2/2 0 T 0 1];

% Ta1=Ta1';%噪声阵

% sigam1=10;

% Qk1=Ta1*Ta1'*sigam1^2;%系统噪声方差初值

​

I1(:,ii)-X1)*((zk1(:,ii)-Zk1)');

    end

   %计算UKF增益，更新状态向量和方差

    zk1=Z(:,k)-Zk1;

    K1=Pxz1*(inv(Pzz1));

    X1=X1+K1*(Z(:,k)-Zk1);

    P1=Pk1-K1*Pzz1*(K1)';

    Xe1(:,k)=X1;                                  

    

                                                                  %%%%%UKF滤波器开始滤波%%%%%

    %%%%%%计算Sigma点%%%%%%%

    si2(:,1)=X2;

    for ii=2:7

        si2(:,ii)=X2+((i+a)^(0.5))*A2(:,ii-1);

    end

    for ii=8:13

        si2(:,ii)=X2-((i+a)^(0.5))*A2(:,ii-7);

    end

    %%%%%%时间更新%%%%%%%

    %利用状态方程传递取样点

  for ii=1:13

      SI2(:,ii)=FX2(si2(:,ii));

  end

%     %利用预测取样点，权值计算预测均值和协方差

    X2=Wm0*SI2(:,1);

    for ii=2:13

        X2=X2+Wm*SI2(:,ii);

    end

    

    Xk2=X2;

    

    Pk2=Wc0*(SI2(:,1)-X2)*((SI2(:,1)-X2)');

    for ii=2:13

        Pk2=Pk2+Wc*(SI2(:,ii)-X2)*((SI2(:,ii)-X2)');

    end

    Pk2=Pk2+Qk2; 

    

    %利用预测取样点预测测量取样点

     for ii=1:13

      zk2(:,ii)=FZ(SI2(:,ii));

     end

     

    %预测测量值

    Zk2=Wm0*zk2(:,1);

    for ii=2:13

        Zk2=Zk2+Wm*zk2(:,ii);

    end

    %%%%%%量测更新%%%%%%%

    %预测协方差

    Pzz2=Wc0*(zk2(:,1)-Zk2)*((zk2(:,1)-Zk2)');

    for ii=2:13

        Pzz2=Pzz2+Wc*(zk2(:,ii)-Zk2)*((zk2(:,ii)-Zk2)');

    end

    Pzz2=Pzz2+Rk2;

   

    Pxz2=Wc0*(SI2(:,1)-X2)*((zk2(:,1)-Zk2)');

    for ii=2:13

        Pxz2=Pxz2+Wc*(SI2(:,ii)-X2)*((zk2(:,ii)-Zk2)');

    end

   %计算UKF增益，更新状态向量和方差

     zk2=Z(:,k)-Zk2;

    K2=Pxz2*(inv(Pzz2));

    X2=X2+K2*(Z(:,k)-Zk2);

    P2=Pk2-K2*Pzz2*(K2)';

    Xe2(:,k)=X2;                                  

    %----------------------------------------------------------------------

    %第三步 模型概率更新

     %计算模型匹配函数

    like1=exp(-0.5*zk1'*inv(Pzz1)*zk1)/(sqrt(det(2*3.1415926*Pzz1)));

    like2=exp(-0.5*zk2'*inv(Pzz2)*zk2)/(sqrt(det(2*3.1415926*Pzz2)));   

    

    c_1=pai(1,1)*miu_CV+pai(2,1)*miu_CA;

    c_2=pai(1,2)*miu_CV+pai(2,2)*miu_CA;

    c=like1*c_1+like2*c_2;

    miu_CV=like1*c_1/c;

    miu_CA=like2*c_2/c;

    

    %----------------------------------------------------------------------

    %第四步 估计融合  

    X=X1*miu_CV+X2*miu_CA;

    

    Xe(:,k)=X;

    X11=X1;

    X22=X2;

    P11=P1;

    P22=P2;

    

end

figure(1)

plot(Xr(2,:)-Xe(1,:),'r')

figure(2)

plot(Xr(3,:)-Xe(2,:),'r')

figure(3)

plot(Xr(4,:)-Xe(3,:),'r')

figure(4)

plot(Xr(5,:)-Xe(4,:),'r')

figure(5)

plot(Xr(6,:)-Xe(5,:),'r')

figure(6)

plot(Xr(7,:)-Xe(6,:),'r')

⛄ 运行结果

⛄ 参考文献

[1]崇阳, 张科, 吕梅柏. 基于"当前"模型的IMM-UKF机动目标跟踪融合算法研究[J]. 西北工业大学学报, 2011, 29(6):8.

❤️ 关注我领取海量matlab电子书和数学建模资料

❤️部分理论引用网络文献，若有侵权联系博主删除