卡尔曼滤波学习

clear;clc;
F=[1 1;0 1];B=[0.5;1];H=[0 1];
X=[10; 10];%初始状态
U=1;%加速度
P=[1 0;0 1];%初始协方差矩阵
Q=[0.001 0;0 0.001];
Z=1:1000;%测量值，匀加速运动
noise=15*randn(1,1000);%测量噪声100个
Z=Z+noise;
R=1;
state=zeros(2,1000);
for i=1:1000
    state(:,i)=X;
    %根据上步最优值计算估计值
    X_=F*X+B*U;
    P_=F*P*F'+Q;
    %更新计算最优值
    K=P_*H'/(H*P_*H'+R);
    P=(eye(2)-K*H)*P_;
    X=X_+K*(Z(i)-H*X_);
end
t=1:1000;
plot(t,state(1,t),'r.',t,state(2,t),'b')
hold on
plot(t,Z);
legend('位移','速度')
axis([1 1000 1 12000])

小车为匀加速运动，速度测量值带有噪声（蓝色），从上图可以看出滤波得到的值比较好

卡尔曼滤波的核心思想是根据上一步计算得到的最优值对下一步的值进行预测，得到一个预测值，再根据当前的观测值计算一个当前的最优值，不断地更新系数K(与过程噪声方差Q和测量噪声方差R相关，噪声服从均值高斯分布，测量噪声与传感器相关，因此K值决定了是更倾向于测量值还是预测值)