关于卡尔曼滤波的数学原理
介绍
卡尔曼滤波是一套递推的估计方法：手里有一个不太准的状态先验，又来了一次带噪声的观测，如何按各自的不确定度把两者加权捏到一起，得到一个更准的估计，这就是它干的事。它在导航、目标跟踪、传感器融合里到处都是，早年阿波罗飞船的导航计算机上跑的就是这套算法。这里我们只讲最基础的线性卡尔曼，前提假设是状态和噪声都服从高斯分布，这样所有信息都可以用均值和协方差描述，公式才是清爽的矩阵乘加形式。
首先把这几个看着发麻的公式放在这里坐镇！
这五个公式看起来非常难受，我们先解释一下符号的含义：$\hat x_i^-$表示根据$y_0,…,y_{i-1}$得到的先验估计，也就是还没看到i时刻观测之前的猜测；而$\hat x_i$表示加入$y_i$之后的后验估计。前后两者对应的协方差矩阵为$P_i^-$和$P_i$，它俩对角线上的元素就是各个状态量的估计方差，衡量我们有多不确定；$y_i$表示i时刻的观测值。另外公式里的F是状态转移矩阵，描述状态怎么随时间演化；B是控制输入矩阵，u是外界输入；Q是过程噪声的协方差，代表模型本身有多不靠谱；H是观测矩阵，负责把状态映射到观测所在的尺度；R是观测噪声的协方差；K就是卡尔曼增益，相当于一个权重，决定这一轮到底听预测多一点还是听观测多一点。
这五条式子其实分成两步在用：1、2是预测步，拿上一时刻的结果往前推一步，得到先验；3、4、5是更新步，把观测$y_i$揉进去修正成后验，修正完的新结果又作为下一轮预测的起点，整个滤波就是这么一步步滚下去的。
每一步中$\hat x_{i-1}$都是已知的，我们要做的就是计算出下一个状态的$\hat x_i^-$。这里要补充一点，如果你是在计算机视觉中使用的话，公式1可以简化为：
因为跟踪这类任务一般用的是匀速或者匀加速模型，没有外力输入，Bu这一项直接省掉，公式里就只剩已知量了。调参的重点也就落在Q和R上，这两个值的相对大小，直接决定了滤波结果是灵敏还是平滑，后面应用那节再细说。
推导
首先我们假设一个线性的环境方程。x是包含所有状态变量的向量，比如（x坐标，y坐标），我们给出环境状态方程：
其中F是状态转移矩阵，B是控制输入矩阵，u为外界提供的输入，ε为过程噪声。这样我们就有(1)式。为了观测状态，我们有状态更新：
H为观测矩阵，负责把状态映射到观测空间，v为测量噪声。比如状态里既有位置又有速度，而传感器只能测到位置，那H就是一个把速度分量“扔掉”的矩阵，维度上正好对得上：状态n维、观测m维时，H就是m×n的形状。
卡尔曼增益的物理意义可以这么理解：K=预测误差/(预测误差+测量误差)，预测越不准，K就越接近1，这一轮多听观测的；观测越不准，K越接近0，就多保留预测。矩阵形式的3式不过是把这个权重推广到了多维。
由K的定义为K=预测误差/(预测误差+测量误差)可有：
得到(4)公式：
从4式和7式我们写出：
简单变换一下：
对于实对称矩阵，有：$AA^T=A^2$，后面的协方差矩阵包含平方项，所以只需展开矩阵转置积就可以了
另外这里默认估计误差和测量噪声是不相关的，所以12式里那两部分交叉相乘的期望为零，下面写协方差的时候可以直接省掉。这样协方差矩阵为：
R为观测噪声的协方差，把13式的转置积硬展开就是这个样子：
两个交叉项互为转置，最后两小块合成一个带$HP_i^-H^T+R$的项。到这里$P_i$已经是一个关于$K_i$的二次型了，前面两项跟K无关，只有最后那一坨随K变。对角元代表各方向的估计方差，方差越小估计越接近真值，所以为了最小化协方差，我们让它对K的偏导数为0：
所以我们得到3式：
把3式代入14式得到第5条式子：
最后一步，把预测步里协方差怎么变也推出来。设e为估计误差，下一时刻的先验误差为：
可以化简为递推式子：
我们使用协方差的计算方式计算：
把它按定义展开，期望里一共四项：$FP_{i-1}F^T$、两个交叉项，还有$E[w_iw_i^T]$。假设过程噪声和估计误差不相关，两个交叉项为零，再把$E[w_iw_i^T]$记作Q，就得到预测步的协方差递推：
到这里五个公式就都能自己推出来了，其实没有想象中那么玄乎，就是线性代数硬展开。顺带理解一下$FPF^T$这一项：它把上一轮的协方差按状态转移“推”到这一时刻，而Q是推的过程里模型自己注入的不确定性，所以P不会越推越小直到归零，滤波始终留着修正的余地。
应用
这里拿计算机视觉里最常见的用法举个例子：目标跟踪。我们把目标的位置和速度作为状态量塞进状态向量，二维平面里的匀速模型可以写成$x_i=(x,y,\dot x,\dot y)^T$，对应的状态转移矩阵就是
它的作用很直白：新的位置等于上一时刻位置加上速度乘时间，速度保持不变。而检测器每帧给出的只是一个检测框，也就是只有位置没有速度，观测矩阵就写成
预测的时候状态按1式往前推，协方差按2式膨胀一点，等到新一帧检测结果进来，再用3、4、5把状态往观测的方向拉一把。检测框偶尔跳一下也没关系，K会自动按Q、R的大小分配信任度，滤波输出会比原始检测框平顺很多；就算某几帧检测丢了，靠预测也能接着顶一会儿。
Q和R往哪调是有讲究的：R可以按观测噪声的实际方差给初值，Q一般给得比较小。Q越大越不相信预测、越依赖观测，结果跟得紧但抖；R越大越相信预测、输出平滑但滞后。调这两个参数没什么理论捷径，存一段实测数据跑一遍看曲线，比在纸上拍脑袋强。
如果状态转移是非线性的，比如带三角函数、要估角度，线性卡尔曼就不够用了，得扩展成EKF或者UKF，那又是另一摊事，这里先不展开。
总结
卡尔曼滤波的公式到这里就推完了，知其然也要知其所以然，变量含义也在推导里过了一遍。在计算机视觉里用它做位置预测是很常见的用法，至于前向和后向平滑，这里就不展开说了。写代码就是照着这五个式子顺下来，难的反而是Q和R怎么调，多存几段数据看看曲线就心里有数了。