世事短如春梦，人情薄似秋云。
不须计较苦劳心，万事原来有命。
幸遇三杯酒好，况逢一朵花新。
片时欢笑且相亲，明日阴晴未定。
——《西江月》 朱敦儒
引子
有一辆汽车在马路上匀加速前进，随着时间的推移，汽车的位置和速度都会发生变化，而在真实世界中，汽车的位置和速度跟理想状态下是不一样的，比如会受到风力影响，导致汽车的运动方式不是严格的匀加速运动。那么在这种情况下如何对汽车的运动状态进行预测呢？没错，这个问题可以用今天介绍的卡尔曼滤波器（Kalman Filter, KF）来解决。
图1. 汽车运动图示
卡尔曼录波器原理
还是以上述汽车匀加速行驶作为例子来介绍卡尔曼滤波器的原理。假设现在有两种方式可以对汽车的运动状态进行估计，即理论预测和实际测量。
图2. 汽车运动状态变换示意图
### 理论预测
第一种是通过理论估计来预测汽车在 \(t\) 时刻的状态，假如我们已经知道汽车在 \(t-1\) 时刻的位置和速度，以及匀加速运动的加速度 \(a\)，那么，我们可以根据匀加速运动定律对汽车在 \(t\) 时刻的位置和速度进行一个理想（没有误差）的估计：
\[{d_{t|t - 1}} = {d_{t - 1|t - 1}} + {v_{t - 1|t - 1}}\Delta t + \frac{1}{2}a\Delta {t^2}\quad\quad(1) \]
同样的，我们也可以对 \(t\) 时刻的汽车速度做一个理想的（没有误差）估计：
\[{v_{t|t - 1}} = {v_{t - 1|t - 1}} + a\Delta t\quad\quad(2) \]
上面所述的位置 \(d\) 和速度 \(v\) 称为系统的状态变量，如果还需要估计其他的状态变量，如汽车的耗油量、发动机的温度等，都可以类似位置和速度一样加进来，这里仅以位置和速度为例介绍。上面两个状态变量的理想估计可以写成紧凑的矩阵形式：
\[{{\pmb{x}}_{t|t - 1}} = \left[ {\begin{array}{*{20}{c}} 1&{\Delta t}\\ 0&1 \end{array}} \right] \times \left[ {\begin{array}{*{20}{c}} {{d_{t - 1}}}\\ {{v_{t - 1}}} \end{array}} \right] + {\pmb{B}}\left[ {\begin{array}{*{20}{c}} {\frac{1}{2}\Delta {t^2}}\\ {\Delta t} \end{array}} \right] \times {{\pmb{u}}_t} = {\pmb{F}}{{\pmb{x}}_{t - 1|t - 1}} + {\pmb{B}}{{\pmb{u}}_t}\quad\quad(3)\]
这里 \({\pmb{F}} = \left[ {\begin{array}{*{20}{c}} 1&{\Delta t}\\ 0&1 \end{array}} \right]\) 称为状态变换矩阵，\({\pmb{B}} = \left[ {\begin{array}{*{20}{c}} {\frac{1}{2}\Delta {t^2}}\\ {\Delta t} \end{array}} \right]\) 称为控制矩阵，\({{\pmb{u}}_t} = a\) 称为控制向量。
这里 \({{\pmb{x}}_{t|t - 1}}\) 表示基于 \(t-1\) 时刻估计状态下理论预测的 \(t\) 时刻的状态，\({{\pmb{x}}_{t - 1|t - 1}}\) 表示在 \(t-1\) 时刻综合理论预测和实际测量得到的估计值。其他的下标具有类似意思。
然而实际系统中总会存在噪声，比如汽车的运动并不严格遵循匀加速运动。那么汽车在 \(t\) 时刻的真实状态可以表示为基于 \(t-1\) 时刻的真实状态值经过 \(\Delta t\) 时间后加上噪声项：
\[{{\pmb{x}}_t} = {\pmb{F}}{{\pmb{x}}_{t - 1}} + {\pmb{B}}{{\pmb{u}}_t} + {{\pmb{w}}_t}\quad\quad(4) \]
这里 \({\pmb{x}_{t}}\) 表示 \(t\) 时刻的状态真实值，\({{\pmb{x}}_{t - 1}}\) 表示汽车在 \(t-1\) 时刻的状态真实值。噪声 \({{\pmb{w}}_t}\) 为服从均值为0，协方差为 \({{\pmb{Q}}_t}\) 的二元独立高斯分布，\({{\pmb{Q}}_t} = {\mathop{\rm cov}} ({{\pmb{w}}_t})\)。
那么如何度量理论估计值 \(\pmb{x}_{t|t - 1}\) 与真实状态 \({\pmb{x}_{t}}\) 之间的差距呢？也就是说理论估计值的估计误差有多少？这可以用协方差矩阵来表示：
\[{{\pmb{P}}_{t|t - 1}} = {\mathop{\rm cov}} ({{\pmb{x}}_t} - {{\pmb{x}}_{t|t - 1}})\quad\quad(5) \]
(5)式经过推导得：
\[\begin{array}{l} {{\pmb{P}}_{t|t - 1}} = {\mathop{\rm cov}} ({{\pmb{x}}_t} - {{\pmb{x}}_{t|t - 1}})\\ {\rm{ \quad\quad\:\:= cov(}}{\pmb{F}}{{\bf{x}}_{t - 1}} + {\pmb{B}}{{\pmb{u}}_t}{\rm{ + }}{{\pmb{w}}_t} - {\pmb{F}}{{\pmb{x}}_{t - 1|t - 1}} - {\pmb{B}}{{\pmb{u}}_t}{\rm{)}}\\ {\rm{ \quad\quad\:\:= cov}}\left( {{\pmb{F}}({{\pmb{x}}_{t - 1}} - {{\pmb{x}}_{t - 1|t - 1}}){\rm{ + }}{{\pmb{w}}_t}} \right)\\ \quad\quad\:\:= {\pmb{F}}{\mathop{\rm cov}} ({{\pmb{x}}_{t - 1}} - {{\pmb{x}}_{t - 1|t - 1}}){{\pmb{F}}^{\rm T}} + {\mathop{\rm cov}} ({{\pmb{w}}_t})\\ \quad\quad\:\:= {\pmb{F}}{{\pmb{P}}_{t - 1|t - 1}}{{\pmb{F}}^{\rm T}} + {{\pmb{Q}}_t} \end{array}\quad\quad(6)\]
实际测量
第二种是通过各种传感器或其他手段来测量状态变量，比如力传感器，加速度传感器等。同样的，通过传感器测量得到的准确值可以表示为：
\[{{\pmb{z}}_t} = {\pmb{H}}{{\pmb{x}}_t} + {{\pmb{v}}_t}\quad\quad(7) \]
这里 \(\pmb{H}\) 也是一个变换矩阵，作用是将真实状态空间映射到测量空间，比如说前面理论估计的是位置和速度，如果传感器测量的是力或者加速度，那么需要通过变换矩阵 \(\pmb{H}\) 将位置和加速度映射到力或加速度相同的空间中来。噪声 \({{\pmb{v}}_t}\) 为服从均值为0，协方差为 \({{\pmb{R}}_t}\) 的二元独立高斯分布，\({{\pmb{R}}_t} = {\mathop{\rm cov}} ({{\pmb{v}}_t})\)。
类似于理论预测中用协方差来度量理论估计值与真实值之间的估计误差，同样的，理论估计值与测量值之间的误差也可以用协方差矩阵来表示：
\[{{\pmb{S}}_t} = {\mathop{\rm cov}} ({{\pmb{z}}_t} - {\pmb{H}}{{\pmb{x}}_{t|t - 1}})\quad\quad(8) \]
(8)式经过推导得：
\[\begin{array}{l} {{\pmb{S}}_t} = {\mathop{\rm cov}} ({{\pmb{z}}_t} - {\pmb{H}}{{\pmb{x}}_{t|t - 1}})\\ \quad= {\mathop{\rm cov}} ({\pmb{H}}{{\pmb{x}}_t} + {{\pmb{v}}_t} - {\pmb{H}}{{\bf{x}}_{t|t - 1}})\\ \quad= {\mathop{\rm cov}} \left( {{\pmb{H}}({{\pmb{x}}_t} - {{\pmb{x}}_{t|t - 1}}){{\pmb{H}}^{\rm T}}} \right) + {\mathop{\rm cov}} ({{\pmb{v}}_t})\\ \quad= {\pmb{H}}{{\pmb{P}}_{t|t - 1}}{{\pmb{H}}^{\rm T}} + {{\pmb{R}}_t} \end{array}\quad(9)\]
理论预测与实际测量融合
现在已经知道了 \(t\) 时刻的理论预测值和实际测量值，并知道了理论预测值与 \(t\) 时刻真实值及测量值之间的估计误差，那么根据理论预测值和实际测量值得到 \(t\) 时刻的估计值呢？卡尔曼滤波的思想是分别给理论预测值和实际测量值一个权重，通过理论预测值与实际测量值的加权线性组合来得到估计值，即：
\[{{\pmb{x}}_{t|t}} = {{\pmb{K}}_t}{{\pmb{z}}_t} + ({\pmb{I}} - {{\pmb{K}}_t}{\pmb{H}}){{\pmb{x}}_{t|t - 1}}\quad(10) \]
这里 \({{\pmb{K}}_t}\) 称为卡尔曼增益，
那么这个权重怎么确定呢？
我们的目标是使得 \(t\) 时刻加权后的估计值与系统的真实值之间的误差最小，也就是 \({{\pmb{x}}_{t|t}}\) 与 \({{\pmb{x}}_t}\) 之间的距离最小化。在这个前提下求得的权重因子（卡尔曼增益）就是最佳的。那么\({{\pmb{x}}_{t|t}}\) 与 \({{\pmb{x}}_t}\) 之间的距离最小化可以描述为：
\[\mathop {\min }\limits_{{{\pmb{K}}_t}} {\left\| {{{\pmb{x}}_t} - {{\pmb{x}}_{t|t}}} \right\|^2} \Leftrightarrow \mathop {\min }\limits_{{{\pmb{K}}_t}} {\rm{Tr}}\left( {{\mathop{\rm cov}} ({{\pmb{x}}_t} - {{\pmb{x}}_{t|t}})} \right) = \mathop {\min }\limits_{{{\pmb{K}}_t}} {\mathop{\rm Tr}\nolimits} ({{\pmb{P}}_{t|t}})\quad(11) \]
这一位置只需到展开 \({{\pmb{P}}_{t|t}}\)，然后对其求一阶导，令导数等于0，即可得到卡尔曼增益：
\[{{\pmb{K}}_t} = {{\pmb{P}}_{t|t - 1}}{{\pmb{H}}^{\rm T}}{\pmb{S}}_t^{ - 1}\quad\quad\quad(12) \]
得到卡尔曼增益后，将 \({{\pmb{K}}_t}\) 代回 \({{\pmb{P}}_{t|t}}\) 中即可得到关于 \({{\pmb{K}}_t}\) 的加权估计下的协方差矩阵:
\[{{\pmb{P}}_{t|t}} = ({\pmb{I}} - {{\pmb{K}}_t}{\pmb{H}}){{\pmb{P}}_{t|t - 1}}\quad\quad\quad(13) \]
卡尔曼滤波器迭代过程
综上所述，卡尔曼滤波器的迭代过程可以总结为：
\[\begin{array}{l} {{\pmb{x}}_{t|t - 1}} = {\pmb{F}}{{\bf{x}}_{t - 1|t - 1}} + {\pmb{B}}{{\pmb{u}}_t}\quad(1)\\ {{\pmb{P}}_{t|t - 1}} = {\pmb{F}}{{\pmb{P}}_{t - 1|t - 1}}{{\pmb{F}}^{\rm T}} + {{\bf{Q}}_t}\quad(2)\\ {{\pmb{z}}_t} = {\pmb{H}}{{\pmb{x}}_t} + {{\pmb{v}}_t}\quad(3)\\ {{\pmb{S}}_t} = {\pmb{H}}{{\pmb{P}}_{t|t - 1}}{{\pmb{H}}^{\rm T}} + {{\pmb{R}}_t}\quad(4)\\ {{\pmb{x}}_{t|t}} = {{\pmb{K}}_t}{{\pmb{z}}_t} + ({\pmb{I}} - {{\pmb{K}}_t}{\pmb{H}}){{\pmb{x}}_{t|t - 1}}\quad(5)\\ {{\pmb{K}}_t} = {{\pmb{P}}_{t|t - 1}}{{\pmb{H}}^{\rm T}}{\pmb{S}}_t^{ - 1}\quad(6)\\ {{\pmb{P}}_{t|t}} = ({\pmb{I}} - {{\pmb{K}}_t}{\pmb{H}}){{\pmb{P}}_{t|t - 1}}\quad(7) \end{array}\]
讨论
最小二乘（Least Squares）是优化方法中的一种特殊情况，而卡尔曼滤波又是最小二乘法的一种特殊情况。 古典最小二乘中，假设了每一次测量的权重相同，但事实上这样并不合理，后来演化为加权最小二乘法，至此最小二乘估计所做的都是批处理（Batch），这样比较占内存，不符合动态系统状态估计的需要，即每一次更新输入时，都要从新计算之前所有的记录值。而后，提出递推最小二乘法，模型就不用每次都重新计算了。与递归最小二乘相似，卡尔曼滤波加入了系统内部变化的考虑。即利用process model对系统在下一时刻的状态进行预测。
当对于系统不够了解时，使用最小二乘法比较合适，而对于系统了解比较多时，可以采用Kalman滤波。改变量测噪声、系统噪声都会对Kalman滤波的效果产生影响，而不会对最小二乘滤波产生影响，而改变最小二乘的阶数会对其产生影响。
往期精选：
更多精彩内容请关注订阅号优化与算法和加入QQ讨论群1032493483获取更多资料
