深入理解卡尔曼滤波

卡尔曼滤波

卡尔曼滤波的作用不用多说，就是根据前面已知的信息，预测将来可能发生的情况。对于一个系统来说，以跟踪系统为例，如果视频中待跟踪物体按静止不动，或者按照一定的规律运动时，那在当前帧便可以预测下一帧物体所在位置。但是实际中，物体的运动往往不是一成不变的，比如在视频中物体一直往右下角移动，但是移动的方向却或多或少有一定的变化，这种变化可以理解成一种随机噪声。因此需要找到一种随机估计方式，来估计系统下一次的状态。卡尔曼滤波便是一种随机估计算法，而且是一种线性随机估计算法。

状态—空间模型（state—space model）

在了解卡尔曼滤波之前，先了解一下状态—空间模型（state—space model）

x⃗ i+1=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢yi+1yiyi−1...yi−(n−2)⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢a0100a1010...............an−2001an−1000⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥A⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢yiyi−1yi−2...yi−(n−1)⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥x⃗ i+⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢100...0⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥Bui

上述公式中， x⃗ i 表示系统在 i 时刻的状态， yi 表示系统在 i 时刻的观测值， ui 表示系统在 i 时刻的输入量。
观察上述公式可发现，状态-空间模型将系统的状态用 n 个系统的观测值组成的向量表示，在求 x⃗ i+1 时，其表示向量中第 i+1 个观测值未知，前面 n−1 个观测值已知。而在预测状态时，将 yi+1 用前 n 个观测值线性表出。
上述公式可以写成：

x⃗ i+1=Ax⃗ i+Buiyi=[100...0]x⃗ i

写成更一般的形式：

x⃗ i+1=Ax⃗ i+Buiyi=Hix⃗ i

卡尔曼滤波

先介绍几个相关概念：
1.先验状态估计 x^− ：先验状态估计指的是在得到观察值之前的估计值
2.后验状态估计 x^ ：后验状态估计指的是在得到观察值之后的估计值
3.观测值 zk
4.真实状态 xk
5.先验估计误差：

真实状态-先验状态估计值e−k=xk−x^−k(1)

6.后验估计误差：

真实状态−后验状态估计值ek=xk−x^k(2)

7.先验估计误差的协方差:

P−k=E[e−ke−kT](3)

8.后验估计误差的协方差：

Pk=E[ekeTk](4)

卡尔曼滤波是基于状态-空间模型的一种线性估计算法，因此其模型可以参照上述状态-空间模型的表达：

xk=Axk−1+Buk+wk−1(5)zk=Hxk+vk(6)

wk 和 vk 分别是估计噪声和观测噪声，并假定其符合协方差为 Q,R 的正态分布：

p(w)∼N(0,Q)p(v)∼N(0,R)

对于后验估计误差而言，其可以用先验估计误差和观测值线性表出：

x^k=x^−k+Kvk=x^−k+K(zk−Hx^−k)(7)

上式中 zk−Hx^−k 代表残差，即观测值与先验预测值之间的差值。式中 K 值得选取取决于最小化后验误差的协方差,即公式（4），最后可以解得（具体推导过程不做解释）：

Kk=P−kHT(HP−kHT+R)−1=P−kHTHP−kHT+R

观察上式可发现，当观测值的可信度很高时，即观测误差的协方差为趋于0，

limRk→0Kk=H−1

当预测值的可信度更高时，即先验误差的协方差 P−k 趋于0时，

limP−k→0Kk=0

基于上述知识，这里给出卡尔曼滤波的著名的五个等式：

x^−k=Ax^k−1+Buk(a)P−k=APk−1AT+Q(b)Kk=P−kHT(HP−kHT+R)−1(c)x^k=x^−k+Kk(zk−Hx^−k)(d)Pk=(I−KkH)P−k(e)

下面是卡尔曼滤波的具体流程示意图：

参考文献：
An Introduction to the Kalman Filter