kalman.m

来自「基于卡尔曼滤波器的交通流量分析系统」· M 代码 · 共 43 行

clear all;
close all;
N=60;                               %采样周期
a=load('speed.txt');%读取速度数据
X(1)=60;
v=randn(1,N);                       %模型噪声
w=randn(1,N)*5;                     %观测噪声
X_est(1)=60;
X_est_1(1)=60;
P(1)=1;
for k=1:N
    Y(k)=a(k);
    if k>1
        X(k)=X(k-1)+v(k-1);          %状态方程
        Y(k)=X(k)+w(k);             %测量方程
        P_1(k)=P(k-1)+cov(v);
        K(k)=P_1(k)/(P_1(k)+cov(w));%卡尔曼增益
        X_est_1(k)=X_est(k-1);
        X_est(k)=X_est_1(k)+K(k)*(Y(k)-X_est_1(k));
        P(k)=(1-K(k))*P_1(k);
    end
    rerr(k)=(X_est_1(k)-X(k))/X(k);
    rerr_1(k)=(X_est(k)-X(k))/X(k);
end
figure(1)
plot(X_est_1,'r');hold on
plot(X,'b');hold on
plot(Y,'g');
axis([0 N 10 100]);
legend('预测值','真实值','测量值');
figure(2)
plot(rerr);
legend('预测相对误差');
figure(3)
plot(X_est,'r');hold on
plot(X,'b');hold on
plot(Y,'g');
axis([0 N 10 100]);
legend('估计值','真实值','测量值');
figure(4)
plot(rerr_1);
legend('估计相对误差');