【卡尔曼滤波】卡尔曼滤波在雷达目标跟踪中的应用仿真matlab源码

2021/10/16 1:14:37

编程Tag： 源码 matlab 轨迹滤波 plot VX zeros 卡尔曼滤波 vy

本文主要是介绍【卡尔曼滤波】卡尔曼滤波在雷达目标跟踪中的应用仿真matlab源码，对大家解决编程问题具有一定的参考价值，需要的程序猿们随着小编来一起学习吧！

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;