kf.m

基本卡尔曼滤波算法

function [zzkf]=kf()
%  clear all;
%  close all;
randn('state',0)
steps=80;
T=1;
T0=1;
x=zeros(4,steps);
x(:,1)=[0,10,0,10]';
for t=1:29
    x(1,t+1)=x(1,t)+10;
    x(2,t+1)=10;
    x(3,t+1)=x(3,t)+10;
    x(4,t+1)=10;
end
ax=20;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ay=20;
  for t=1:30
    x(1,t+30)=x(1,30)+x(2,31)*t+ax*t*t/2;
    x(2,t+30)=x(2,30)+ax*t;
    x(3,t+30)=x(3,30)+x(4,31)*t+ay*t*t/2;
    x(4,t+30)=x(4,30)+ay*t;
  end
for t=1:20
    x(1,t+60)=x(1,60)+x(2,60)*t;
    x(2,t+60)=x(2,60);
    x(3,t+60)=x(3,60)+x(4,60)*t;
    x(4,t+60)=x(4,60);
end
%  plot(x(1,1:30),x(3,1:30),'ko',x(1,31:60),x(3,31:60),'r*',x(1,61:80),x(3,61:80),'ko')
% h=legend('匀速阶段','加速阶段',2);
% xlabel('目标轨迹的X轴位移（单位：m）');
% ylabel('目标轨迹的Y轴位移（单位：m）');
% title('目标运动的真实轨迹')
H=[1,0,0,0
    0,0,1,0];
F=[1,T,0,0
    0,1,0,0
    0,0,1,T
    0,0,0,1];
G=[0.5*T^2,0
    T,0
    0,0.5*T^2
    0,T];
d=1;%%%%%%%%%%%%%%%%%%%%%%%%%%%%
vt=randn(2,steps);
z=H*x+d*vt;
Xn=zeros(4,steps);
        Xp=zeros(4,steps);
        Xn(:,1)=x(:,1);
        P=zeros(4,4,steps);
        Pp=zeros(4,4,steps);
        P(:,:,1)=[10,0,0,0
                0,1,0,0
                0,0,10,0
                0,0,0,1];
        Q=zeros(2,2,steps);
        R=zeros(2,2,steps);
        K=zeros(4,2,steps);
        I=eye(4);
        for k=1:(steps-1)
            Q(:,:,k+1)=5*eye(2);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            R(:,:,k+1)=1*eye(2);
            Xp(:,k+1)=F*Xn(:,k);
           Pp(:,:,k+1)=F*P(:,:,k)*F'+G*Q(:,:,k+1)*G';
            K(:,:,k+1)=Pp(:,:,k+1)*H'*inv(H*Pp(:,:,k+1)*H'+R(:,:,k+1));
            Xn(:,k+1)=Xp(:,k+1)+K(:,:,k+1)*(z(:,k+1)-H*Xp(:,k+1));   %z(k+1) 改为 z(:,k+1)
            P(:,:,k+1)=(I-K(:,:,k+1)*H)*Pp(:,:,k+1);
        end
zzkf=abs(Xn(3,:)-x(3,:));
% figure(1)
% plot(z(1,1:30),z(2,1:30),'g*',Xn(1,:),Xn(3,:),'r',z(1,31:60),z(2,31:60),'b*',z(1,61:80),z(2,61:80),'g*');
% title('量测/估计值')
% h=legend('量测值','估计值',2);
% figure(2)
% plot(zzkf,'g');
% h=legend('kf估计误差',2); 
% legend('kf估计误差',4)
% title('误差分布')