kalman31.m

卡尔曼滤波仿真程序

clear all; %清空变量
hold off
T=0.1; %观测周期 秒
N=50; %门特卡罗数
SK=300; %观测数
x0=0;y0=0;z0=0; %初始位置
vx=300;vy=300;vz=300;ax=0;ay=0;az=0;%x,y,z三个方向都是速度为10的匀速直线运动 米/秒
dR=200;%观测噪声模型的距离方差  米/秒
da=0.001;%观测噪声模型的轨迹于Z正半轴的夹角方差  弧度/秒
db=0.001; %观测噪声模型方位角的方差    弧度/秒

fai=[1 T T^2/2 0 0 0 0 0 0;0 1 T 0 0 0 0 0 0;0 0 1 0 0 0 0 0 0;
0 0 0 1 T T^2/2 0 0 0;0 0 0 0 1 T 0 0 0;0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 1 T T^2/2;0 0 0 0 0 0 0 1 T;0 0 0 0 0 0 0 0 1];  %状态转移矩阵

Gk=eye(9);%为了后头计算方便，定义一个9*9的对角单位矩阵

Hk=[1 0 0 0 0 0 0 0 0;0 0 0 1 0 0 0 0 0;0 0 0 0 0 0 1 0 0 ];%观测矩阵
Po=eye(9);   %为滤波状态相关矩付初值
Qk=0.1*eye(9);  %设定过程噪声相关矩
Rk=1*eye(3); 
xk=[x0 vx ax y0 vy ay z0 vz az]';
Roav=zeros(1,SK+1);%存放理论值
Rfoav=zeros(1,SK+1);%存放估计值

for i=1:N    %大循环
Reoo=0;Roo=0;Rfo=0;Pab=Po;
for k=1:SK %观测数
%直角坐标系航迹 
xt=x0+vx*k*T+ax*(k*T)^2/2; 
yt=y0+vy*k*T+ay*(k*T)^2/2; 
zt=z0+vz*k*T+az*(k*T)^2/2; 
%极坐标系航迹
Rt=sqrt(xt.^2+yt.^2+zt.^2); 
at=atan(xt/yt); 
bt=atan(zt/sqrt(xt.^2+yt.^2)); 
Reo=Rt;
Reoo=[Reoo,Reo];
%观测值
Ro=Rt+normrnd(0,dR);                        
Roo=[Roo,Ro];%
ao=at+normrnd(0,da);                        
bo=bt+normrnd(0,db);                         
xo=Ro*cos(bo)*sin(ao);
yo=Ro*cos(bo)*cos(ao);
zo=Ro*sin(bo);
zz=[xo yo zo]';
%卡尔曼滤波
xab=fai*xk;
Kk=fai*Pab*Hk'*inv(Hk*Pab*Hk'+Rk); %增益
xk=xab+Kk*(zz-Hk*xab); %状态预测
Po=Pab-inv(fai)*Kk*Hk*Pab;
%Po=(eye(9)-Kk*Hk)*Pab;%更新滤波状态误差相关矩
Pab=fai*Po*fai'+Gk*Qk*Gk';%一步预测误差相关矩
Rf=sqrt(xk(1).^2+xk(4).^2+xk(7).^2);
Rfo=[Rfo,Rf];
end
Roav=Reoo+Roav;%真实值
Rfoav=Rfo+Rfoav;%预测值
end
n=1:SK;
figure(1)

plot(Roav/N,'r');%红色代表真实值
hold on
plot(Rfoav/N,'g');%绿色代表跟踪
plot(Roav/N-Rfoav/N,'b');%蓝色代表误差
legend('Ro(k)红色表示真实值 ','Rf(k)绿色表示跟踪 ','Rp(k)蓝色表示误差'); 
xlabel('k');
ylabel('Observation/Filtering/Prediction error(m)');
title('匀速直线飞行目标跟踪');
grid on