8.asv

卡尔曼滤波问题

clc;
clear;
z=0.01*randn(1,150)-0.37727;
xx(1)=-0.31;
Q=[0.09 0;0 0.09];
R=1;
p(1)=0.02;
 s(1)= -0.37727;
for k=2:1:150
 s(k)= -0.37727;
 xs(k)=xx(k-1);
ps(k)=p(k-1)+Q;
K(k)=ps(k)/(ps(k)+R);
xx(k)=xs(k)+K(k)*(z(k)-xs(k));
p(k)=(1-K(k))*ps(k);
end
plot(xx);
hold on 
plot(z,'r')
plot(s,'k')