kalman1.m

使用matlab实现的卡尔曼滤波器

function kalman(L,Ak,Ck,Bk,Wk,Vk,Rw,Rv)
%kalman1(100,0.95,1,0,1,1,0.0975,1)
w=sqrt(Rw)*randn(1,L);     % w为均值零方差为Rw斯白噪声   
v=sqrt(Rv)*randn(1,L);     % v为均值零方差为Rv高斯白噪声
x0=sqrt(10^(-12))*randn(1,L);
x(1)=w(1);                                %给x(1)赋初值.
for i=2:L                                 %递推求出x(k).
    x(i)=Ak*x(i-1)+w(i-1);
end
yk=Ck*x+Vk*v;                            
yik=Ck*x;
n=1:L;
subplot(2,2,1);
plot(n,yk,'r',n,yik,'b');
legend('量测值yk','状态值无激励和噪声yik',1)
Qk=Wk*Wk'*Rw;
Rk=Vk*Vk'*Rv;
p(1)=var(x0);
%p(1)=10^(-12);
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
subplot(2,2,2);
plot(n,p(n),'b',n,H(n),'r')
legend('均方误差p(n)','修正参数H(n)',4)
subplot(2,2,3);
plot(n,x(n),'b',n,xg(n),'r')
legend('状态值x(n)','状态估计值xg(n)',1)
subplot(2,2,4);
plot(n,yik(n),'b',n,yg(n),'r')
legend('状态值无激励和噪声yik','量测值估计yg(n)',1)