激光SLAM后端优化总结之卡尔曼滤波

一、贝叶斯滤波

1.1 状态估计模型

1.2 公式推导

二、卡尔曼滤波（KF）

已知上一时刻系统后验概率分布：
$$P(x_{k-1})=N({\hat{x}}_{k-1},{\hat{P}}_{k-1})$$
假设系统是线性的：

运动方程可以表示为：

观测方程可以表示为：

又后验概率正比于似然概率与先验概率的乘积，所以有：

随机变量服从高斯分布的高维形式可以表示为：

所以我们可以得到${\hat{x}}_k$与${\hat{P}}_k$的求解关系：

${\hat{P}}_k$的求解：

${\hat{x}}_k$的求解：

后验概率的均值${\hat{x}}_k$，等于先验概率的均值${\bar{x}}_k$加上一个修正量即$K(z_k-C_k{\bar{x}}_k)$。
所以，卡尔曼滤波的一般流程如下：

在使用卡尔曼滤波时需要满足以下两个基本条件：

• 卡尔曼滤波假设系统为线性；
• 卡尔曼滤波要求变量符合高斯分布；

三、扩展卡尔曼滤波（EKF）