移动机器人学（三）卡尔曼滤波

卡尔曼滤波器有一系列递推公式描述，提供一种高效可计算的方法来估计过程的状态，并使估计均方误差最小。它的应用广泛而强大，可以各级信号的过去和当前状态，甚至能估计将来的状态，即使是在无法得知模型的确切性质。

算法流程

直观理解

考虑一辆匀速运动的小车，它在时刻 t 的状态向量 $x(t)$ 与 $x(t-1)$ 有关，我们可以预测它的状态 $x(t)=Ax(t-1)$ 。
然而，实际情况并不是那么理想。这个递推式会受到各种因素的影响（小车结构不稳，刮风下雨），变成 $x(t)=Ax(t-1)+w_{t-1}$ ，$w_{t-1}$ 为来自各种因素的噪声，我们假设它符合正态分布 $N(0,Q)$ ，为过程噪声。
“”我们尝试估计小车的位置：
$t=0$ 时，小车位置 Pos 如下红色分布；

$t=1$ 时，我们预测 Pos 值，递推过程中增加了一层噪声，不确定度变大了，方差增大；

为了避免单纯估计带来的偏差，我们对 Pos 进行雷达观测，当然，观测也会收到各种因素的影响，我们也假设它是一个符合正态分布 $N(0,R)$ 的量，称为观测噪声。测量结果显示，Pos 符合蓝色分布；

Kalman 通过使估计均方误差最小加权方式将红蓝分布合为绿色的正态分布（这使得 Kalman 滤波可以迭代计算）：

该分布的具体推导有最大似然法、Ricatti 方程法以及对高斯密度函数变形的方法。

参考

https://zhuanlan.zhihu.com/p/495348963 卡尔曼滤波算法流程以及两个例子
https://www.zhihu.com/question/23971601/answer/46480923 EKF直观理解