【Kalman】卡尔曼滤波Matlab简单实现

  本节卡尔曼滤波Matlab实现是针对线性系统估计的，仅为简单仿真。

1.离散时间线性动态系统的状态方程

  线性系统采用状态方程、观测方程及其初始条件来描述。线性离散时间系统的一般状态方程可描述为

  其中，X(k) 是 k 时刻目标的状态向量，V(k)是过程噪声，它是具有均值为零、方差矩阵为 Q(k) 的高斯噪声向量，即
　

  Q(k)是状态转移矩阵， G(k)是过程噪声增益矩阵。

2.传感器的测量（观测）方程

  传感器的通用观测方程为

　
  这里， Z(k+1)是传感器在 k+1 时刻的观测向量，观测噪声 W(k+1) 是具有零均值和正定协方差矩阵 R(k+1) 的高斯分布测量噪声向量，即

3.初始状态的描述

  初始状态 X(0) 是高斯的，具有均值 X(0|0) 和协方差 ，即

4.Kalman滤波算法

  状态估计的一步预测方程为

  一步预测的协方差为

  预测的观测向量为

  观测向量的预测误差协方差为

  新息或量测残差为

  滤波器增益为

  Kalman滤波算法的状态更新方程为

  滤波误差协方差的更新方程为

% Kalman滤波技术

A=1;                                        % 状态转移矩阵 Φ(k)
H=0.2;                                      % 观测矩阵 H(k)
X(1)=0;                                     % 目标的状态向量 X(k)
% V(1)=0;                                   % 过程噪声 V(k)
Y(1)=1;                                     % 一步预测x(k)的更新 X(k+1|k+1)
P(1)=10;                                    % 一步预测的协方差 P(k)
N=200;
V=randn(1,N);                               % 模拟产生过程噪声(高斯分布的随机噪声)
w=randn(1,N);                               % 模拟产生测量噪声

for k=2:N

    X(k) = A * X(k-1)+V(k-1);               % 状态方程:X(k+1)=Φ(k)X(k)+G(k)V(k),其中G(k)=1

end

Q=std(V)^2;                                 % W(k)的协方差,std()函数用于计算标准偏差  
R=std(w)^2;                                 % V(k)的协方差 covariance

Z=H*X+w;                                    % 观测方程:Z(k+1)=H(k+1)X(k+1)+W(k+1),Z(k+1)是k+1时刻的观测值

for t=2:N

    P(t) = A * P(t-1)+Q;                    % 一步预测的协方差 P(k+1|k)   

    S(t) = H.^2 * P(t)+R;                   % 观测向量的预测误差协方差 S(k+1)

    K(t) = H * P(t)/S(t);                   % 卡尔曼滤波器增益 K(k+1) 

    v(t) = Z(t) - ( A * H * Y(t-1) );       % 新息/量测残差 v(k+1)

    Y(t)=A * Y(t-1) + K(t) * v(t);          % 状态更新方程 X(k+1|k+1)=X(k+1|k)+K(k+1)*v(k+1)

    P(t)=(1-H * K(t)) * P(t);               % 误差协方差的更新方程: P(k+1|k+1)=(I-K(k+1)*H(k+1))*P(k+1|k)
end


t=1:N;
plot(t,Y,'r',t,Z,'g',t,X,'b');              % 红色线最优化估算结果滤波后的值，%绿色线观测值，蓝色线预测值
legend('Kalman滤波结果','观测值','预测值');

运行结果