klaman.m

卡尔曼滤波相信大家都很熟悉了

clc;clear all;
close all;

%产生真实数据x(k)和观测数据y(k)
q=1;
t=[1:51];
m=randn(2,length(t));      %产生状态噪声w(k)
w=[sqrt(q),0;0,sqrt(q)]*m;
n=randn(2,length(t));       %产生状态噪声v(k)
v=[1,0;0,1]*n;
x=zeros(1,length(t));       %赋初值x,vx,y,vy分别为x(k)第一，二，三，四个数据
y=zeros(1,length(t));
vx=zeros(1,length(t));
vy=zeros(1,length(t));
zx=zeros(1,length(t));
zy=zeros(1,length(t));
x1s=zeros(1,length(t));    
y1s=zeros(1,length(t));
vx1s=zeros(1,length(t));
vy1s=zeros(1,length(t));
a=[1,1,0,0;0,1,0,0;0,0,1,1;0,0,0,1];
b=[0.5,0;1,0;0,0.5;0,1];
c=[1,0,0,0;0,0,1,0];
R=[1 0;0 1];   
g=[q,0;0,q];
p=[1 1 0 0;1 2 0 0;0 0 1 1;0 0 1 2];
for i=1:50                %根据状态方程和量测方程产生真实数据和观测数据
 x(1)=5;
 vx(1)=1;
 y(1)=7;
 vy(1)=2;
 zx(1)=5+v(1,1);
 zy(1)=7+v(2,1);
 xk=a*[x(i),vx(i),y(i),vy(i)]'+b*w(:,i);    %状态方程
 x(i+1)=xk(1,1);
 vx(i+1)=xk(2,1);
 y(i+1)=xk(3,1);
 vy(i+1)=xk(4,1);
 z=c*xk+v(:,i+1);                 %量测方程
 zx(i+1)=z(1,1);
 zy(i+1)=z(2,1);
end    
%绘图
%figure(1);
%plot(t,x,'b-',t,zx,'r:');
%legend('x真实轨迹','x观测样本');
%figure(2);
%plot(t,y,'b-',t,zy,'r:');
%legend('y真实轨迹','y观测样本');
%figure(3);
%plot(x,y,'b-',zx,zy,'r:');
%legend('运动真实轨迹','观测轨迹');
%-------------------------------------------------------------------------
%Kalman滤波
x1s(1)=x(1);
vx1s(1)=vx(1);
y1s(1)=y(1);
vy1s(1)=vy(1);
for r=1:50
    z=[zx(r+1);zy(r+1)];
    xks1=a*[x1s(r),vx1s(r),y1s(r),vy1s(r)]';
    p1=a*p*a'+b*g*b';
    h=p1*c'*inv(c*p1*c'+R)
    p=(eye(4)-h*c)*p1;
    xks=xks1+h*(z-c*xks1);
    x1s(r+1)=xks(1,1);
    vx1s(r+1)=xks(2,1);
    y1s(r+1)=xks(3,1);
    vy1s(r+1)=xks(4,1);
end    
figure(4);
plot(x,y,'b-',zx,zy,'r:',x1s,y1s,'k-.');
legend('运动真实轨迹','观测轨迹','估计轨迹');