✅作者简介：热爱科研的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.
