karman1.m

一个matlab实现卡尔曼滤波的程序

function karman1(L,Rw,Rv)

w=sqrt(Rw)*randn(1,L);
v=sqrt(Rv)*randn(1,L);
p=[0.01 0.04];
p0=diag(p,0);
Ak=[0.98 0.009;-0.36 0.801];
Bk=[-0.019 -0.36]';
Wk=[-0.019 -0.36]';
Ck=[1 0];
Vk=1;
I=eye(2);

u=zeros(30);
for i=1:30
    if i<10
        u(i)=1;
    else
        u(i)=0;
    end
end
N=ceil(L/30);
U=repmat(u,1,N);  %产生周期序列
u1=zeros(L);
for j=1:L
    u1(j)=U(j);
end

X=zeros(2,L);
x=[0;0];
for i=2:L
    x=Ak*x+Bk*u1(i-1)+Wk*w(i-1);
    X(1,i)=x(1);
    X(2,i)=x(2);
end
yik=Ck*X;
y(1)=v(1);
y=Ck*X+v;
k=1:L;
subplot(3,2,1)
plot(k,yik,'r',k,y,'b');
legend('yik','y');

Rk=Vk*Vk'*Rv;
Qk=Wk*Wk'*Rw;

xgu=[0;0];
Xgu(:,1)=xgu;
p=zeros(2,2*L);
p(:,1)=p0(:,1);
p(:,2)=p0(:,2);
for i=2:L
    Pp=Ak*p0*Ak'+Qk;
    Hk=Pp*Ck'*inv(Ck*Pp*Ck'+Rk);
    xgu(:,i)=Ak*xgu(:,i-1)+Hk*(y(i)-Ck*Ak*xgu(:,i-1))+Bk*u1(i);
    Pk=(I-Hk*Ck)*Pp;
    hk(:,i-1)=Hk;
    Xgu(:,i)=xgu(:,i);
    p(:,2*i-1)=Pk(:,1);
    p(:,2*i)=Pk(:,2);
end
Yk=Ck*Xgu;
subplot(3,2,2);
plot(k,yik,'r',k,Yk,'b');
axis([1,L,-1,0]);
legend('yik','Yk');         
grid ON

subplot(3,2,3);
plot(k,Xgu(1,:),'r',k,X(1,:),'b');
legend('Xgu(1,:)','X(1,:)');                
grid ON
subplot(3,2,4);
plot(k,Xgu(2,:),'r',k,X(2,:),'b');
legend('Xgu(2,:)','X(2,:)');                 
grid ON
n=1:99;
subplot(3,2,5);
plot(n,hk(1,:),'r',n,p(1,1:2:2*L-2),'b',n,p(1,2:2:2*L-2),'b');
legend('hk(1)','p(1,1)','p(1,2)');                 
grid ON
subplot(3,2,6)
plot(n,hk(2,:),'r',n,p(2,1:2:2*L-2),'b',n,p(2,2:2:2*L-2),'b');
legend('hk(2)','p(2,1)','p(2,2)');                  
grid ON