标签：plot 轨迹 卡尔曼滤波 滤波 源码 zeros matlab vx vy
1 模型
【摘要】目标跟踪问题的应用背景是雷达数据处理，即雷达在搜索到目标并记录目标的位置数据，对测量到的目标位置数据（称为点迹）进行处理，自动形成航迹，并对目标在下一时刻的位置进行预测。本文简要讨论了用Kalman滤波方法对单个目标航迹进行预测，并借助于Matlab仿真工具，对实验的效果进行评估。
关键词：Kalman**滤波、目标跟踪、Matlab仿真**
2 部分代码
clear all;
clc;
%================================仿真场景===================================
Ts=2;%雷达扫描周期
d=100;%噪声的标准差值
Pv=10000;%噪声的功率
sigma=10;% 加速度方向的的扰动
offtime=900;%仿真的截止时间
Num=50;%蒙特卡洛仿真次数
N=ceil(offtime/Ts); % 采样点数
%描述系统运动轨迹
x=zeros(offtime,1);%x轴方向的运动轨迹
y=zeros(offtime,1);%y轴方向的运动轨迹
X=zeros(N,1);%x轴方向的雷达观测数据
Y=zeros(N,1);%y轴方向的雷达观测数据
%t=0:400s,速度vx,vy为沿x和y轴的速度分量(m/s),目标在这段时间做匀速运动
x0=2000;%起始点坐标
y0=10000;
vx=0;
vy=-15; % 沿-y方向
for t=1:400
x(t)=x0+vx*t;
y(t)=y0+vy*t;
end
% t=400:600s，ax,ay为沿x和y轴的加速度分量(m/s/s)
ax=0.075;
ay=0.075;
for t=0:200
x(t+401)=x(400)+vx*t+ax*t*t/2;
y(t+401)=y(400)+vy*t+ay*t*t/2;
end
vx=vx+ax*200; % 第一次机动转弯结束时的速度
vy=vy+ay*200;
%t=600:610s匀速运动
for t=0:10;
x(t+601)=x(601)+vx*t;
y(t+601)=y(601)+vy*t;
end
% t=610:660s，第二次转弯
ax=-0.3;%第二次转弯的加速度
ay=0.3;
for t=0:50
x(t+611)=x(611)+vx*t+ax*t*t/2;
y(t+611)=y(611)+vy*t+ay*t*t/2;
end
vx=vx+ax*(660-610);% 第二次机动转弯结束时的速度
vy=vy+ay*(660-610);
% 660s以后匀速运动，一直到截止时间
for t=0:(offtime-660)
x(t+661)=x(661)+vx*t;
y(t+661)=y(661)+vy*t;
end
% 得到雷达的观测数据
for n=0:Ts:offtime-1
X(n/Ts+1)=x(n+1);
Y(n/Ts+1)=y(n+1);
end
%定义系统的状态方程
Phi=[1,Ts;0,1];
Gamma=[Ts*Ts/2;Ts];
C=[1 0];
R=Pv;
Q=sigma^2;
WX=[];
WY=[];
ax=[zeros(1,400) 0.075*ones(1,200) zeros(1,10) -0.3*ones(1,50) zeros(1,offtime-660)]; % 对x方向不同时段的加速度进行描述
ay=[zeros(1,400) 0.075*ones(1,200) zeros(1,10) 0.3*ones(1,50) zeros(1,offtime-660)]; % 对y方向不同时段的加速度进行描述
%x轴方向的各类矩阵定义
Xe_x=zeros(2,1); % 用前k-1时刻的输出值估计k时刻的预测值
Xf_x=zeros(2,1); % k时刻Kalman滤波器的输出值
Xes_x=zeros(2,1); % 预测输出误差
Xef_x=zeros(2,1); % 滤波后输出的误差
Pxe_x=zeros(2,1); % 预测输出误差均方差矩阵
Px_x=zeros(2,1); % 滤波输出误差均方差矩阵
XE=zeros(1,N); % 得到最终的滤波输出值，仅仅考虑距离分量
XER(1:N,i)=X(1:N)-(XE(1:N))';
YER(1:N,i)=Y(1:N)-(YE(1:N))';
end
%滤波误差的均值
XERB=mean(XER,2);
YERB=mean(YER,2);
%滤波误差的标准差
XSTD=std(XER,1,2); % 计算有偏的估计值
YSTD=std(YER,1,2);
%===========================绘图====================================
%真实轨迹
figure
plot(X,Y,'r'),hold on
axis([1500 5000 1000 10000]),grid on;
title('目标真实轨迹')
%x轴方向的真实轨迹和滤波估计的轨迹
figure
plot(X(k),'r'),hold on
plot(XE,'b'),hold on
grid on
legend('真实轨迹','滤波估计航迹');
%y轴方向的真实轨迹和滤波估计的轨迹
figure
plot(Y,'r'),hold on
plot(YE,'b'),hold on
grid on;
legend('真实航迹','滤波估计航迹');
%目标真实轨迹和观测轨迹
figure
plot(X,Y,'r'),hold on;
plot(zx,zy,'g'),hold on;
axis([1500 5000 1000 10000]),grid on;
legend('理论航迹','观测数据');
%目标真实轨迹和滤波估计的轨迹
figure
plot(X,Y,'r'),hold on;
plot(XE,YE,'b'),hold off;
axis([1500 5000 1000 10000]),grid on;
legend('真实轨迹','滤波估计');
%估计误差曲线
figure
subplot(2,1,1)
plot(XERB)
axis([0 500 -50 50])
xlabel('观测次数')
ylabel('X方向滤波误差均值'),grid on;
subplot(2,1,2)
plot(YERB)
axis([0 500 -50 50])
xlabel('观测次数')
ylabel('Y方向滤波误差均值'),grid on;
%估计误差标准曲线
figure
subplot(2,1,1)
plot(XSTD)
axis([0 500 0 150])
xlabel('观测次数')
ylabel('X方向滤波误差标准值'),grid on;
subplot(2,1,2)
plot(YSTD)
axis([0 500 0 150])
xlabel('观测次数')
ylabel('Y方向滤波误差标准值'),grid on;
3 仿真结果
4 参考文献
[1]宋文尧, and 张牙. 卡尔曼滤波. 科学出版社, 1991.
[2]潘迪夫, 刘辉, 李燕飞. 基于时间序列分析和卡尔曼滤波算法的风电场风速预测优化模型[J]. 电网技术, 2008, 32(7):82-86.
标签：plot,轨迹,卡尔曼滤波,滤波,源码,zeros,matlab,vx,vy 来源： https://blog.csdn.net/qq_59747472/article/details/120792370
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。