kalman.m

卡尔曼滤波的MATLAB程序

clc;
clear;
W=0.1*randn(1,1500);
for n=1:1500
s(n)=0.01*n;
end;
z=W+s;% signal 
xx(1)=-0.31;%internatial value 
Q=0.0001;
R=0.0011;
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')