karman.m

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

function karman(L,Rw,Rv)
Ak=0.95;
Bk=0;
Wk=1;
Ck=1;
Vk=1;
w=sqrt(Rw)*randn(1,L);     
v=sqrt(Rv)*randn(1,L);
x0=sqrt(10^(-12))*randn(1,L);
x(1)=w(1);
for i=2:L
    x(i)=Ak*x(i-1)+w(i-1);
end
y=Ck*x+Vk*v;
Rk=Vk*Vk'*Rv;
Qk=Wk*Wk'*Rw;
yik=Ck*x;
xgu(1)=0;
p(1)=var(x0);
p1(1)=Ak*p(1)*Ak'+Qk;
for k=2:L
    p1(k)=Ak*p(k-1)*Ak'+Qk;
    H(k)=p1(k)*Ck'*inv((Ck*p1(k)*Ck'+Rk));
    I=eye(size(H(k)));
    p(k)=(I-H(k)*Ck)*p1(k);
    xgu(k)=Ak*xgu(k-1)+H(k)*(y(k)-Ck*Ak*xgu(k-1))+Bk;
    ygu(k)=Ck*xgu(k);
end
n=1:L;
subplot(2,2,1)
plot(n,x(n),'r',n,xgu(n),'b');
legend('x(n)','xg(n)');
subplot(2,2,2)
plot(n,y(n),'r',n,yik(n),'b');
legend('y(n)','yik(n)');
subplot(2,2,3)
plot(n,p(n),'r',n,H(n),'b');
legend('p(n)','H(n)');
subplot(2,2,4)
plot(n,yik(n),'r',n,ygu(n),'b');
legend('yik(n)','yg(n)');