关于卡尔曼滤波的数学原理

介绍

首先把这几个看着发麻的公式放在这里坐镇！

$$1):\hat x_i^-=F\hat x_{i-1}+Bu_{i-1}$$ $$2):P_i^-=FP_{i-1}F^T+Q$$ $$3):K_i=P_i^-H^T(HP_i^-H^T+R)^{-1}$$ $$4):\hat x_i=\hat x_i^-+K_i(y_i-H\hat x_i^-)$$ $$5):P_i=(E-K_iH)P_i^-$$

这五个公式看起来非常难受，我们先解释一下符号的含义：$\bar x_i^-$表示$P(x_i|y_0,…,y_{i-1})$的概率值，而$\bar x_i$表示为$P(x_i|y_0,…,y_{i})$的概率值。前后两者对应的协方差矩阵为：$P_i^-$和$P_i$，$y_i$表示i状态的观测值。

在每种情况下$x_{i-1}^-$与$x_{i-1}$都是已知的，因为是已经确定的概率，我们要做的就是计算出下一个状态的$X_i^-$。这里要补充一点，如果你是在计算机视觉中使用的话，公式1可以简化为：

$$1^*):\hat x_i^-=F\hat x_{i-1}$$

这样前面的所有公式都是已知量了，我们只需更新K的值即可。而1和2就是我们需要预测的内容，345就是我们需要进行的更新。

推导

首先我们假设一种线性的环境方程。x包含了所有的变量值的向量，比如（x坐标，y坐标）我们提供了环境状态方程：

$$6):\hat x_i^-=F\hat x_{i-1}+Bu_{i-1}+\epsilon$$

其中F是负责状态转移，B是控制输入的矩阵，u为外界提供的输入，ε为过程噪声。这样我们就有(1)式子。为了观测状态，我们有状态更新：

$$7):y_i=H\hat x_i+v_i$$

H为状态观测矩阵，负责使用预测值去得到观测值，需要进行测量。v为测量噪声。

由K的定义为K=预测误差/(预测误差+测量误差)可有：

$$8):K=\frac{\hat x_i-\hat x_i^-}{y_i-H\hat x_i^-}$$

得到(4)公式：

$$4*)\hat x_i=\hat x_i^-+K_i(y_i-H\hat x_i^-)$$

从4式和7式我们写出：

$$9):\hat x_i=\hat x_i^-+K_i(Hx_i+v_i-Hx_i^-)$$ $$10):\hat x_i=\hat x_i^-+K_iHx_i-K_iHx_i^-+K_iv_i$$

简单变换一下：

$$11):\hat x_i-x_k=\hat x_i^--x_k+K_iH(x_i-x_i^-)+K_iv_i$$ $$12):x_i-\hat x_i=(E-K_iH)(x_i-\hat x_i^-)-K_iv_i$$

对于实对称矩阵，有：$AA^T=A^2$，后面的协方差矩阵包含平方项，所以只需展开矩阵转置积就可以了

这样协方差矩阵为：

$$P_i=E[(x_i-\hat x_i)(x_i-\hat x_i)^T]$$ $$13):P_i=(E-K_iH)P_i^-(E-K_iH)^T+K_iRK_i^T$$

R为噪声，由此展开为：

$$14):P_i=P_i^--K_iHP_i^--P_i^-H^TK_i^T+K_i(HP_i^-H^T+R)K^T$$

为了最小化协方差，我们让它对K的偏导数为0：

$$\frac{\partial P_i}{\partial_K}=-2(P_i^-H^T)+2K_i(HP_i^-H^T+R)$$

所以我们得到3式：

$$3^*):K_i=P_i^-H^T(HP_i^-H^T+R)^{-1}$$

我们联立11与14式得到第5条式子：

$$5^*):P_i=(E-K_iH)P_i^-$$

最后我们使用状态更新设e为先验状态的误差：

$$e_{i+1}^-=x_{i+1}-\hat x_{i+1}^-=(F\hat x_i+Bu_{i}+w_i)-(F\hat x_i+Bu_i)$$

可以化简为递推式子：

$$e_{i+1}^-=Fe_i+w_i$$

我们使用协方差的计算方式计算：

$$P(e_{i+1}^-e_{i+1}^{-T})$$

我们使用对称矩阵的平方计算一下：

$$2^*):P_i^-=FP_{i-1}F^T+Q$$

应用

先占个位置，想起来就补上。

总结

本文着重推导了卡尔曼滤波公式，知其然亦要知其所以然，同时读者可以更好的理解相关变量的含义，在计算机视觉上可以用方程预测位置。至于前向和后向平滑，这里不做过多讨论。

参考文献：https://zhuanlan.zhihu.com/p/48876718