滤波_1_卡尔曼滤波理解

1 📖 导文（入门）

卡尔曼滤波是无人驾驶中最基本的算法之一，在传感器融合与定位中几乎无处不在。这里借助笔记顺一顺卡尔曼滤波的过程

适用系统：线性高斯系统

宏观意义：噪声既加权（下面与理想状态的低通滤波进行对比）

	加权公式
低通滤波	低频信号$\times 1$+高频信号$\times 0$	这里的低指的是低频信号值，高指的是高频噪声，对高频噪声给定权重，才是最理想的状态
卡尔曼滤波	估计值$\times p_1$+观测值${\times }_{p_2}$	实际上就是一个给定权重的过程，对于卡尔曼滤波就是分别给定估计值和理想值权重来得到一个修正的值。

状态空间表达式

---

状态方程：
$$\begin{array}{r}{x}_k=F{x}_{k-1}+B{u}_k+{\omega }_k\end{array}$$
观测方程：
$$\begin{array}{r}{z}_k=H{x}_k+v_k\end{array}$$

${\omega }_k$：过程噪声，${\omega }_k\in N\left(0,Q_k\right)$

$v_k$：观测噪声，$v_k\in N\left(0,R_k\right)$

注意：这里的$Q、R$为方差，有时也称这些为超参数

符号约定（以$x$变量举例，其他类比）

• ${\hat{x}}_k$ ：最优估计值（也既为加权后纠正值或者后验估计值）
• ${\hat{x}}_k^{-}$ ：先验估计值（预测值，一般通过前一个时刻的后验值${\hat{x}}_{k-1}$进行预测得出）
• $x_k$ ： 没啥意义，就是k时刻的原始值（或者为测量值）

2 📖 卡尔曼直观图解

实现过程

使用上一次的最优结果预测当前的值，同时使用观测值修正答盎前置，得到最优结果。

• 预测当前的值（${\hat{x}}_k^{-}$）：指的是先验估计
• 观测值（$z_k$）：指的是传感器传回值
• 最优结果（${\hat{x}}_k$）：指的是修正后的结果（即：后验估计）

这里借用《State Estimation for Robotics》3.3节一张图说明（懒得自己画了）

注意一下这里的符号说明

${\hat{x}}_k$ ：最优估计值（也既为加权后纠正值或者后验估计值）（意义同1节符号约定）

${\check{x}}_k$ ：先验估计值（预测值，一般通过前一个时刻的后验值${\hat{x}}_{k-1}$进行预测得出）（即1节符号约定里面的${\hat{x}}_k^{-}$ ）

$y_k$ ：测量值（对应$z_k$） $y_k=z_k=x_k$

---

正如上图，卡尔曼滤波要得出最优的估计值${\hat{x}}_k$ ，而该值的得出需要对预测值（先验估计）和测量值进行加权处理估计出（即，更相信谁？）

2 📖 卡尔曼公式理解

符号约定（以$x$变量举例，其他类比）

• ${\hat{x}}_k$ ：最优估计值（也既为加权后纠正值或者后验估计值）
• ${\hat{x}}_k^{-}$ ：先验估计值（预测值，一般通过前一个时刻的后验值${\hat{x}}_{k-1}$进行预测得出）
• $x_k$ ： 没啥意义，就是k时刻的原始值（或者为测量值）

方程建模

状态方程 $x_t=F{x}_{t-1}+B{u}_{t-1}$

观测方程 $z_t=H{x}_t+v_t$

预测过程(⭐️)

预测
${\hat{x}}_t^{-}=F{\hat{x}}_{t-1}+B{u}_{t-1}$	先验估计（需要上一时刻的后验最优估计信息）
$P_t^{-}=F{P}_{t-1}{F}^T+Q$	预测估计的协方差矩阵

更新过程(⭐️)

更新
$K_t=P_t^{-}{H}^T{\left(H{P}_t^{-}{H}^T+R\right)}^{-1}$	卡尔曼增益
${\hat{x}}_t={\hat{x}}_t^{-}+K_t\left(z_t-H{\hat{x}}_t^{-}\right)$	最优状态估计（后验估计）
$P_t=\left(I-K_{t}H\right)P_t^{-}$	最终的协方差矩阵（更新协方差估计）

注意：

$z_t-H{\hat{x}}_t^{-}$ ：表示测量值与预测的差值（测量余量）

3 📖 卡尔曼公式推导

参考链接：

卡尔曼滤波算法详细推导

4 📖 其他

关于Q、R

传感器精度很高，更相信观测值，那么就可以将R调小一点。

如果觉得运动模型很理想，没有过程噪声和摩擦等情况，可以将Q调节小一点。

10 📖 参考资料

视频

【图灵鸡】什么是卡尔曼滤波？其实一点也不难！详解卡尔曼滤波——原理/推导/程序实现

从放弃到精通！卡尔曼滤波从理论到实践~（推荐）

文章

中英文翻译

How a Kalman filter works, in pictures
详解卡尔曼滤波原理 - 清风莞尔的博客 - CSDN博客

傻瓜也能懂的卡尔曼滤波器（翻译自外网博客）（推荐）

无人驾驶技术入门（十三）| 手把手教你写卡尔曼滤波器

卡尔曼滤波算法详细推导

书籍

《State Estimation for Robotics》