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

介绍

首先把这几个看着发麻的公式放在这里坐镇！
$$
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
$$