kalman.m

来自「一个matlab实现卡尔曼滤波的程序」· M 代码 · 共 47 行

function kalman(L,Rw,Rv,d)

Ak=0.95;
Ck=1;
Bk=0;
Wk=1;
Vk=1;
w=sqrt(Rw)*randn(1,L);
v=sqrt(Rv)*randn(1,L);
x0=sqrt(d)*randn(1,L);
x(1)=w(1);                               
for i=2:L                                
    x(i)=Ak*x(i-1)+w(i-1);
end
yk=Ck*x+Vk*v;                            
yik=Ck*x;
Qk=Wk*Wk'*Rw;
Rk=Vk*Vk'*Rv;
p(1)=var(x0);
p1(1)=Ak*p(1)*Ak'+Qk;
xg(1)=0;
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);
    xg(k)=Ak*xg(k-1)+H(k)*(yk(k)-Ck*Ak*xg(k-1));
    yg(k)=Ck*xg(k);
end
n=1:L;
subplot(2,2,1);
plot(n,x(n),'b',n,xg(n),'r');
legend('x(n)','xg(n)');
subplot(2,2,2);
plot(n,p(n),'b',n,H(n),'r');
legend('p(n)','H(n)');
subplot(2,2,3);
plot(n,yk,'r',n,yik,'b');
legend('yk','yik');
subplot(2,2,4);
plot(n,yik(n),'b',n,yg(n),'r');
legend('yik','yg(n)');