Kalman滤波详解（一）——基本理论

Kalman滤波是什么？

Kalman滤波主要是用来跟踪某一个变量的值，跟踪的依据是首先根据系统的运动方程来对该值进行预测，比如我们知道一个物体的运动速度，那么下面时刻它的位置按照道理是可以预测出来的，不过该预测肯定有误差，只能作为跟踪的依据。另一个依据是可以用测量手段来测量那个变量的值，当然该测量也是有误差的，也只能作为依据，不过这两个依据的权重比例不同。Kalman滤波就是利用这两个依据进行一系列迭代进行目标跟踪的。
Kalman滤波：预测未来、修正当下
卡尔曼滤波器本质是一个“optimal recursive data processing algorithm“（最优自回归数据处理算法）。
Kalman滤波能用来做什么？
Kalman滤波是用来对状态进行估计的算法，本质上是利用状态过程噪声和测量噪声对状态进行估计。

Kalman滤波需要满足的条件：

注意：此处Kalman滤波指的都是线性Kalman滤波
线性随机微分系统、过程和测量噪声都是高斯白噪声

Kalman滤波过程

Kalman滤波算法分为两个阶段：
预测阶段：卡尔曼滤波器使用由当前点计算的协方差来估计目标的新位置；
更新阶段：卡尔曼滤波器记录目标的位置，并为下一次循环计算修正协方差。
卡尔曼滤波算法中包含系统预测过程（根据系统状态方程对系统状态参数进行预测）和系统测量（即系统观测量，观测的状态参量不一定和系统状态参量一样）两个重要部分。
首先引入一个离散控制过程的系统，该系统能够用一个线性随机微分方程描述（即系统状态方程）：
$X(k)=A X(k-1)+B U(k)+W(k)$
A——状态转移矩阵；
B——控制输入矩阵；
U(k)——k时刻对系统的控制量；
W(k)——系统过程噪声，为高斯白噪声，协方差为Q。
再加上系统的测量值（即观测量）：
$Z(k)=H X(k)+V(k)$
H——系统测量矩阵；
V(k)——测量噪声，为高斯白噪声，协方差为R；
Z(k)——k时刻的测量值，也即对系统能够观测的量。对于系统状态变量而言，并不一定能够观测所有的量。

整个系统更新思路包括：状态更新和协方差更新
Kalman滤波的五个关键公式：
$X(k|k-1)=A X(k-1|k-1)+B U(k)$ ……….. (1)
$P(k|k-1)=A P(k-1|k-1) A’+Q$……… (2) 预测的协方差更新
公式(1)和(2)表示对系统的预测值的计算；
$Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)$……… (3) Kg为卡尔曼增益
$X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))$ ……… (4)
公式(3)和(4)表示对系统的测量；
由以上四个公式，已经得到了k状态下的最优估计值$X(k|k)$。
但是为了使卡尔曼滤波器不断的运行下去直到系统过程结束，需要更新k状态下X(k|k)的协方差：
$P(k|k)=（I-Kg(k) H）P(k|k-1)$……… (5)
其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。
运行过程：(1)(2)-(3)-(4)(5)-(1)(2)……
输出量：X(k|k)——k时刻系统状态最优估计值。
必要解释：
P是系统的误差协方差矩阵（也叫后验错误协方差矩阵），整个公式的思想和目的就是找到一个合适的Kalman增益，使得误差协方差矩阵最小。

从以上的迭代过程来看，为了能够进行迭代求解，需要初始化的变量包括：
状态转移矩阵、测量矩阵（H）、控制向量(没有的话，为0)、过程噪声协方差矩阵、测量噪声协方差矩阵、后验错误协方差矩阵、前一状态校正后的值（即状态变量的初始值）、当前观测值。