卡尔曼滤波的原理和实现

卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值。它是一种纯粹的时域滤波器。卡尔曼滤波在技术领域有许多的应用，比如飞行导航控制，机器人运动规划等控制领域。

卡尔曼滤波适用于如下系统控制模型：
X(K) = AX(K-1) + BU(K-1) + W(K-1);
Z(K) = HX(K) + V(K);
其中
A是作用在X(K−1)上的状态变换模型（矩阵/矢量）。
B是作用在控制器向量U(K)上的输入－控制模型。
W(K)是过程噪声，并假定其符合均值为零，协方差矩阵为Q(K)的多元正态分布。
H是观测模型，它把真实状态空间映射成观测空间。
V(K)是观测噪声，其均值为零，协方差矩阵为R(K),且服从正态分布。
对于不满足如上模型的控制系统，可以使用其他的卡尔曼滤波器的变种，下面我们只讨论基本的卡尔曼滤波器。

卡尔曼滤波器的操作包括两个阶段：预测与校正。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在校正阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

预测阶段的方程如下：
X(k|k-1) = AX(k-1|k-1) + BU(k-1)； （预测状态）
P(k|k-1) = AP(k-1|k-1) A’ + Q； （预测估计协方差矩阵）

校正阶段的方程如下：
Kg(k)= P(k|k-1) H’ / (HP(k|k-1) H’ + R) (卡尔曼增益)
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-HX(k|k-1)) （校正后的状态估计）
P(k|k)= (I-Kg(k)H)P(k|k-1) （更新后的协方差估计）

通过上述5个方程就能实现一个卡尔曼滤波器。下面我们来看下如何具体实现一个卡尔曼滤波器，需要求解哪些量。

上面5个方程中A、B和H都是一个固定的量，在一个控制系统中，这3个都是确定已知的常量。
R是测量噪声协方差，它是一个数值，这是和测量相关的一个特性，作为已知条件输入滤波器，我们通常通过实验确定这个值。
Q 表示过程激励噪声的协方差，它是状态转移矩阵与实际过程之间的误差。这个矩阵是卡尔曼滤波中比较难确定的一个量，但是在某些稳定的过程可以假定它是固定的矩阵，通过寻找最优的Q值使滤波器获得更好的性能。