kalman12.m

这是用matlab编写的一个kalman滤波器

Ts=0.02;
N=42;
A(1:N)=exp(-Ts);
C(1:N)=1;
R(1:N)=1;
Q(1:N)=1-exp(-2*Ts);
P(1)=1;
y = [-3.2 -0.8  -14  -16  -17  -18  -3.3  -2.4  -18  -.3  -.4  -.8  -19  -2.0  -1.2  -11  -14  -.9   .8  10  .2  .5  -.5  2.4  -.5  .5  -13  .5  10  -12  .5  -.6  -15  -.7  15  .5  -.7  -2.0  -19  -17  -11  -14];
x(1)=y(1);%x(0)=0;H(1)=1;
for n=2:N
    P1(n)=A(n)*A(n)*P(n-1)+Q(n);
    H(n)=C(n)*P1(n)/(C(n)*C(n)*P1(n)+R(n));
    x(n)=A(n)*x(n-1)+H(n)*(y(n)-C(n)*A(n)*x(n-1));
    P(n)=(1-C(n)*H(n))*P1(n);
end
i=[1:N];
plot(i,x);
xlabel('n'),ylabel('估计值x(n)');
grid on;
title('估计信号x的波形');