1 简介
Kalman滤波器在各个领域都有广泛的应用,如航天器的轨道计算、雷达目标跟踪、生产过程的自动控制等.卡尔曼滤波器在机动目标跟踪中具有良好的性能,它是一种最佳估计并能够进行递推计算.
2 部分代码
%---------------------------------------------%
%% VD算法
clear all; close all; clc;
%% 参数设置
%% 产生真实轨迹
N1=400/T;N2=600/T;N3=610/T;N4=660/T;N=1000/T;
x=zeros(N,1);y=zeros(N,1);
vx=zeros(N,1);vy=zeros(N,1);
x(1)=2000;y(1)=10000; %位置、速度、加速度真实值数组
vx(1)=0;vy(1)=-15;
ax=0;ay=0;
var=100;
for i=1:N-1
if(i>N1-1&&i<=N2-1)
ax=0.075;ay=0.075;
vx(i+1)=vx(i)+ax*T;
vy(i+1)=vy(i)+ay*T;
elseif(i>N2-1&&i<=N3-1)
ax=0;ay=0;
vx(i+1)=vx(i)+ax*T;
vy(i+1)=vy(i)+ay*T;
elseif(i>N3-1&&i<=N4-1)
ax=-0.3;ay=-0.3;
vx(i+1)=vx(i)+ax*T;
vy(i+1)=vy(i)+ay*T;
else
ax=0;ay=0;
vx(i+1)=vx(i)+ax*T;
vy(i+1)=vy(i)+ay*T;
end
x(i+1)=x(i)+vx(i)*T+0.5*ax*T^2;
y(i+1)=y(i)+vy(i)*T+0.5*ay*T^2;
end
rex=zeros(M,N);%保留x方向位置每次滤波结果
rey=zeros(M,N);%保留y方向位置每次滤波结果
revx=zeros(M,N);%保留x方向速度每次滤波结果
revy=zeros(M,N);%保留y方向速度每次滤波结果
%% 卡尔曼滤波VD算法
%滤波50次
for m=1:M
%% 观测数据生成
noise=mvnrnd([0,0],[10000,500;500,10000],N); % sigma
zx=x+noise(:,1);
zy=y+noise(:,2);
%% 滤波初始化
ki=0; %机动模型持续时间
low=1;high=0; %low表示非机动模型是否初始化、high表示机动模型中是否初始化
u=0;ua=0; %分别表示非机动模型和机动模型中的判决量
o=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1]; %状态转移矩阵
h=[1 0 0 0;0 0 1 0]; %测量矩阵
g=[T/2 0;1 0;0 T/2;0 1]; %扰动噪声矩阵
q=[0,0;0,0]; %扰动噪声协方差阵
R=[10000,500;500,10000]; %测量噪声协方差矩阵
% 按照论文要求产生初始值
zx0=x(1)-vx(1)*T+var*randn(1,1);zy0=y(1)-vy(1)*T+var*randn(1,1);
%kalman滤波开始
end
ex=0;ey=0;
eqx=0;eqy=0;
ex1=N:1;ey1=N:1;
qx=N:1;qy=N:1;
%计算滤波均值和均方差
for i=1:N
for j=1:M
ex=ex+x(i)-rex(j,i);
ey=ey+y(i)-rey(j,i);
eqx=eqx+(x(i)-rex(j,i))^2;
eqy=eqy+(y(i)-rey(j,i))^2;
end
ex1(i)=ex/M;%x方向位置的误差值
ey1(i)=ey/M;%y方向位置的误差值
qx(i)=(eqx/M-(ex1(i)^2))^0.5;%x方向位置的根方差
qy(i)=(eqy/M-(ey1(i)^2))^0.5;%y方向位置的根方差
ex=0;eqx=0;eqy=0;
ey=0;
end
for i=1:N
for j=1:M
ex=ex+vx(i)-revx(j,i);
ey=ey+vy(i)-revy(j,i);
eqx=eqx+(vx(i)-revx(j,i))^2;
eqy=eqy+(vy(i)-revy(j,i))^2;
end
ex1(i)=ex/M;%x方向位置的误差值
ey1(i)=ey/M;%y方向位置的误差值
qvx(i)=(eqx/M-(ex1(i)^2))^0.5;%x方向位置的根方差
qvy(i)=(eqy/M-(ey1(i)^2))^0.5;%y方向位置的根方差
ex=0;eqx=0;eqy=0;
ey=0;
end
%绘图
figure
plot(x,y,'k-',zx,zy,'g:',xks,yks,'r-');
legend('真实轨迹', '观测样本', '估计轨迹');
figure
subplot(221);
plot(qvx); title('x方向速度估计的标准差曲线');
subplot(222)
plot(qvy); title('y方向速度估计的标准差曲线');
subplot(223)
plot(qx); title('x方向位置估计的标准差曲线');
subplot(224)
plot(qy); title('y方向位置估计的标准差曲线');
3 仿真结果
4 参考文献
博主简介:擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真,相关matlab代码问题可私信交流。
部分理论引用网络文献,若有侵权联系博主删除。