简单解释卡尔曼滤波

卡尔曼滤波

1、定义

卡尔曼滤波是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。

2、算法流程

• 1.预测
  在预测步骤中，卡尔曼滤波通过状态方程以上一时刻的状态估计当前的状态，其中也计算了环境等不受人为控制因素的影响；
• 2.校正和更新
  对当前时刻状态的估计和传感器得到的测量值通过卡尔曼增益结合在一起，得到校正后的后验估计。再对误差协方差进行更新，以进行下一次迭代。

3、卡尔曼滤波理解

3.1 线性系统微分表达

根据现代控制理论，一个线性系统可以被描述为以下形式：

状态方程
$$X_k=A{\hat{X}}_{k-1}+B{u}_k+w_k$$

$$其中A是状态转移矩阵，B是控制矩阵,u_k表示控制变量，w_k是过程噪声$$

观测方程：
$$Z_k=H{X}_k+v_k$$

$$p(w)—N(0,Q)$$

$$p(v)—N(0,R)$$

$$v_k是测量时的噪声，v_k和w_k都服从正态分布p(v)和p(w)$$
其中Q是过程噪声的协方差，R是观测噪声的协方差。

3.2 预测

得到状态方程之后，可以在数学上对下一时刻的状态量进行预测，得到物体在下个时刻的先验估计状态方程和误差协方差矩阵：
$${\hat{X}}_{\bar{k}}=A{\hat{X}}_{k-1}+B{u}_k$$

$${\hat{X}}_{k-1}表示k-1时刻的校正后估计。通过上述公式就可以获得校正前的k时刻估计{\hat{X}}_{\bar{k}}$$

误差协方差矩阵预测如下：
$$P_{\bar{k}}=A{P}_{k-1}{A}^T+Q$$

$$A和A^T如上是状态转移矩阵，Q是过程噪声的协方差$$

$$P_{k-1}是校正后的误差协方差矩阵，通过P_{\overline{k-1}}得到的，P_{\bar{k}}代表k时刻的预测误差协方差矩阵$$

3.3 校正和更新

在3.2中通过数学模型已经获得k时刻的状态估计值，再通过传感器获得测量值，通过卡尔曼增益把两者融合在一起，就能得到真正的后验预测值。

卡尔曼增益：
$$K_k=P_{\bar{k}}{H}^T/(H{P}_{\bar{k}}{H}^T+R)$$
​ 预测状态量和观测量可能维度不同，需要将他们同时转换到一个向量空间，就产生了线性转换矩阵H，H可以将变量从状态域转换到观测域。

后验估计值：
$${\hat{X}}_k={\hat{X}}_{\bar{k}}+K_k(Z_k-H{\hat{X}}_{\bar{k}})$$

$$P_k=(1-K_{k}H)P_{\bar{k}}$$

得到两个数据融合的协方差矩阵，用于下一个迭代计算。

4、总结

$$初始状态值{\hat{X}}_{\bar{0}}，初始协方差矩阵P_0$$

$$\downarrow$$

$$状态预测估计：{\hat{X}}_{\bar{k}}=A{\hat{X}}_{k-1}+B{u}_k$$

$$协方差矩阵预测估计：P_{\bar{k}}=A{P}_{k-1}{A}^T+Q$$

$$\downarrow$$

$$卡尔曼增益：K_k=P_{\bar{k}}{H}^T/(H{P}_{\bar{k}}{H}^T+R)$$

$$后验估计：{\hat{X}}_k={\hat{X}}_{\bar{k}}+K_k(Z_k-H{\hat{X}}_{\bar{k}})$$

$$协方差矩阵更新：P_k=(1-K_{k}H)P_{\bar{k}}$$

$$\downarrow$$

$$下一次迭代，输入{\hat{X}}_{\bar{k}}，协方差矩阵P_k$$

5、参考

链接：https://blog.youkuaiyun.com/shuoyueqishilove/article/details/81713142
https://blog.youkuaiyun.com/v_JULY_v/article/details/120344668