kalman2(没调通).m

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

function kalman2
L=100;
Ak=[0.98 0.009;-0.36 0.801];
Bk=[-0.019;-0.36];
Wk=[-0.019;-0.36];
Ck=[1 0];
Vk=1;
Rw=0.1;
Rv=0.01;
u=genu(L);
w=sqrt(Rw)*randn(1,L);
v=sqrt(Rv)*randn(1,L);
x0=0;
x1=zeros(2,L);
x1[1;1]=[0;0];                               %给x(1)赋初值.
for i=2:L                                 %递推求出x(k).x(k)维数是2*1。
    x[i;i]=Ak*x1[i-1;i-1]+Bk*u(i-1)+Wk*w(i-1);
end
yk=Ck*x+Vk*v;                             %Y(k)是一维。                          
yik=Ck*x;                                 %yik是一维。
n=1:L;
subplot(2,2,1);
plot(n,yk,'r',n,yik,'b');
legend('量测值yk','状态值无激励和噪声yik',4)
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
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)',4)
subplot(2,2,4);
plot(n,yik(n),'b',n,yg(n),'r')
legend('状态值无激励和噪声yik','量测值估计yg(n)',4)