kalman.m

设计一个卡尔曼滤波器

clear all
N=128;
n=1:N;
%设置状态输入x(n)
v1=0.2*randn(1,N);
v2=3*randn(1,N);
x=zeros(1,N);
x(1)=0;
x(2)=0;
for i=3:N;
    x(i)=1.74*x(i-1)-0.81*x(i-2)+v1(i);
end
C=[0 1];
for i=2:N;
   X(i,:)=[x(i-1) x(i)];
end
%观测值y(n)
for i=1:N;
    y(i)=C*X(i,:)'+v2(i);
end
%设置初始参数
XX(:,3)=[0;0];
K=[0 0;0 0.04];
F=[0 1;-0.81 1.74];
Q2=9;
Q1=[0 0;0 0.04];
%迭代运算
for i=3:N;
    G(:,i)=F*K*C'*inv(C*K*C'+Q2);
    a(i)=y(i)-C*XX(:,i);
    XX(:,i+1)=F*XX(:,i)+G(:,i)*a(i);
    P=K-inv(F)*G(:,i)*C*K;
    K=F*P*F'+Q1;
end
plot(n,XX(2,1:N),n,x,'r',n,y,'-');
title('kalman滤波');
grid on
axis([0,150,-10,10]);
legend('滤波器输出XX(n)','状态输入x(n)','观测值y(n)')
figure
plot(n,G(1,:));
legend('增益G(n)');