kalman滤波学习笔记

一、问题建模

系统状态方程：X(k)=AX(k-1)+B U(k)+W(k) 
测量值：Z(k)=H X(k)+V(k) 

其中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R。

问题：求解满足上述条件的X(k)的最优估计值x(k)。

二、Kalman滤波算法的推导：

依据LMS准则，求解使估计误差P(k)=E[(x(k)-X(k))(x(k)-X(k))’]最小的x(k)的值。显然，P(k)是x(k)的covariance.

经推导，得到

x(k)=AX(k-1)+B U(k)+（A P(k-1) A’+Q） H’ / （H （A P(k-1) A’+Q） H’ + R)(Z(k)-H （A X(k-1)+B U(k)）)

上式即为Kalman滤波器。

三、工程实现

在实际工程应用中，Kalman滤波可以通过系统预测和估计两步来实现。

（一）、系统预测

1、状态预测

X(k|k-1)=Ax(k-1|k-1)+B U(k)……(1)

2、误差预测

P(k|k-1)=A P(k-1|k-1) A’+Q……（2）

（二）、系统估计

1、状态估计

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg为卡尔曼增益(Kalman Gain)：
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

2、误差估计

P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)

四、直观理解

以上（1）到（5）式在工程中可做如下理解。

（1）、（2）式是根据系统状态方程由k-1时刻的状态和误差预测k时刻的状态和误差。

（3）式是采用测量值对（1)式得到的状态预测值进行修正，Kg(k) (Z(k)-H X(k|k-1))是其中的修正项。这里的修正不是固定权值的加权修正，而是一种自适应的加权修正。这种“自适应”是通过卡尔曼增益来实现的。从(4)式看，卡尔曼增益可以看作是预测误差占整个误差（预测误差和测量误差）的比重。结合(3)式可以看到，卡尔曼增益决定了测量信息和预测信息在最终估计结果中的比重：与测量信息正相关，而与预测信息负相关。即：预测误差越大，卡尔曼增益越大，最优估计中预测信息比重越小，测量信息比重越大。显然，这是合乎情理的。

（5）式是对预测误差做一修正以得到k时刻的估计误差。对于单模型系统，（5）带入（4）中化简可得

P(k|k)=P(k|k-1)R/ (H P(k|k-1) H’ + R)

显然，预测误差越大，测量误差越大，则估计误差也越大。