kaiman2.m

一个二维的卡尔曼滤波程序

clear;
x0=[1;0];
p0=[1 0;0 10];
fai=[1 0;0 0.5];
q=[0.01 0;0 0.01];
%gama=[1,0,0,1]
%省去gama
h=[1 0;0 1];
r=[0.01 0;0 0.01];
n=100;
%产生状态和测量
for k=1:n;
    w(:,k)=sqrtm(q)*randn(2,1);
    v(:,k)=sqrtm(r)*randn(2,1);
    if k==1;
        x(:,k)=fai*x0+w(:,k);
        z(:,k)=h*x(:,k)+v(:,k);
    else 
        x(:,k)=fai*x(:,k-1)+w(:,k-1);
        z(:,k)=h*x(:,k)+v(:,k);
    end
end
%kalman滤波算法
for k=1:n;
    if k==1;
        x_pre(:,k)=fai*x0;
        p_pre(:,:,k)=fai*p0*fai'+q;
        K(:,:,k)=p_pre(:,:,k)*h'*inv(h*p_pre(:,:,k)*h'+r);
        x_est(:,k)=x_pre(:,k)+K(:,:,k)*(z(:,k)-h*x_pre(:,k));
        p_est(:,:,k)=(eye(2)-K(:,:,k)*h)*p_pre(:,:,k);
    else
        x_pre(:,k)=fai*x_est(:,k-1);
        p_pre(:,:,k)=fai*p_est(:,:,k-1)*fai'+q;
        K(:,:,k)=p_pre(:,:,k)*h'*inv(h*p_pre(:,:,k)*h'+r);
        x_est(:,k)=x_pre(:,k)+K(:,:,k)*(z(:,k)-h*x_pre(:,k));
        p_est(:,:,k)=(eye(2)-K(:,:,k)*h)*p_pre(:,:,k);
    end
end
for k=1:100
    err(:,k)=x(:,k)-x_est(:,k);
    tr_est(k)=trace(p_est(:,:,k));
end
%绘制曲线(状态值和测量值)
figure
hold on
plot(x(1,:),'b')
plot(z(1,:),'g')
xlabel('时刻k')
ylabel('状态值和测量值')
title('状态值和测量值曲线图')
legend('状态值','测量值')
hold off
%绘制曲线(状态值和估计值)
figure
hold on
plot(x(1,:),'b')
plot(x_est(1,:),'g')
xlabel('时刻k')
ylabel('状态值和估计值')
title('状态值和估计值曲线图')
legend('状态值','估计值')
hold off
%绘制曲线(估计误差)
figure
plot(tr_est,'g')
xlabel('时刻k')
ylabel('估计误差')
title('估计误差曲线图')