卡尔曼滤波r语言 卡尔曼滤波slam

视觉SLAM十四讲 第9讲 卡尔曼滤波

• 1. 什么是卡尔曼滤波
• 2. SLAM的状态估计
• 3. 卡尔曼滤波器的推导
• 4. 扩展卡尔曼滤波

1. 什么是卡尔曼滤波

百度百科定义：卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可以看作是滤波过程。

有简单一点的话术来总结：

卡尔曼滤波是一种系统状态最优估计的方法。

2. SLAM的状态估计

在后端优化中，我们可以根据所考虑时刻（帧数）信息的多少来进行分类：

• k时刻的状态和前面所有时刻的信息有关，甚至用未来的信息来更新，这样的处理方式称为“批量的（Batch）”。这种处理方式常用非线性优化方法。
• k时刻的状态只和k-1时刻的状态有关，即马尔科夫性，并且是一阶马尔科夫性。这种处理方式称为滤波器方法（卡尔曼滤波器）。

3. 卡尔曼滤波器的推导

根本思路：记住卡尔曼滤波器是对系统状态进行最优估计的方法，且利用的是线性系统状态方程。

第一步：状态估计：

根据运动方程的状态方程（式9.3），我们用贝叶斯法则，利用0到k时刻的数据来估计k时刻状态分布：
$P(x_k|x_0,u_{1:k},z_{1:k})\propto P(z_k|x_k)P(x_k|x_0,u_{1:k},z_{1:k-1})$

• 正比于符号$\propto$的左边即k时刻的状态分布（后验分布，它根据了$z_k$观测数据来求状态，是知果求因）
• 符号右边第一项是似然（知因求果）
• 符号右边第二项是先验（历史求因）

实际上，整个式子就是如下公式：$后验 \propto 似然 \times 先验$
我们卡尔曼滤波器，求的就是这个式子，我们希望估计k时刻的后验分布，那么只要求得k时刻的似然和先验即可，整个推导过程牢记这个式子，以免在推导的过程中迷失了自己。（甚至可以粗暴的认为，卡尔曼滤波器就是一个矩阵A，假设k-1时刻的后验为x，通过Ax=b我们就求得的k时刻的后验分布b。）

然后，我们将先验以$x_{k-1}$时刻为条件概率展开：

x时刻的先验：（下面为了简便用A、B表示）
$P(x_k|x_0,u_{1:k},z_{1:k-1}) = P(A|B)=\int P(A|B,x_{k-1})P(x_{k-1}|B)dx_{k-1}$
然后我们再假设马尔科夫性，简化这个式子，会得到一个抽象关系式：
$x时刻的先验 = k时刻的运动方程 \times (k-1)时刻的状态分布（后验）$
这两步想说明的是，我们实际在做的是“如何把k-1时刻的状态分布推导至k时刻”，即我们用k-1时刻的后验分布，计算k时刻的先验，最后求得k时刻的后验（状态分布）。我们只要维护一个状态量（后验），对它不断迭代和更新即可。
后面的证明不会用到这两步。只需要知道，我们假设k-1时刻的后验已知，后验分布均值为$\hat{x}_{k-1}$，后验分布协方差为$\hat{P}_{k-1}$

第二步：线性高斯系统

我们知道卡尔曼滤波器利用的是线性系统状态方程，所以我们要假设运动方程和观测方程可以用线性方程描述：
$\begin{cases}x_k=A_kx_{k-1}+u_k+w_k \\ z_k = C_kx_k+v_k\end{cases} k=1,...,N$
这里的噪声服从零矩阵高斯分布$w_k \sim N(0,R). \ \ v_k \sim N(0,Q)$

实际上，运动方程这里隐藏的信息是：k时刻的先验=k-1时刻的后验的线性映射

第三步：预测

预测，意思是如何从上一个时刻的状态（即后验，假设已知），根据输入信息（有噪声，服从高斯分布）推断当前时刻的状态分布（先验）。然后根据线性方程即运动方程和附录A.3（高斯系统复合即加、乘之后的均值和协方差的变化情况），可以将k时刻的先验分布求出来：
$P(x_k|x_0,u_{1:k},z_{1:k-1})=N(A_k\hat{x}_{k-1}+u_k,A_k\hat{P}_{k-1}A^\top_k + R)$
我们记预测的两个式子为预测：（k时刻的先验=线性k-1时刻的后验）
$\check{x}_k = A_k\hat{x}_{k-1}+uk, \ \ \check{P}_k=A_k\hat{P}_{k-1}A^\top_k+R$

第四步：卡尔曼增益和后验分布的推导

1. 我们知道了先验分布怎么求，接下来求似然。根据观测方程和附录A.3（高斯系统复合的性质）求出似然的均值和协方差：
   $P(z_k|x_k)=N(C_kx_k,Q)$
   至于这里为什么协方差不是附录4.3中的形式即$C_k\hat{P}_kC^\top_k+Q$，这个问题，博客zkk9527给出了解释。而我的理解是，回顾整个推导过程，$x_k$（既不是后验也不是先验）的作用是作为一个参考量，我们利用与$x_k$相关的一次项系数和二次项系数来求得预测和卡尔曼增益。它不存在于我们最后的卡尔曼滤波器5个公式中，所以这里的$x_k$是一个定值，如果是定值则没有协方差之说，而均值就等于它本身。
2. 现在有了先验和似然，我们将两项乘起来便得到k时刻的后验：
   $N(\hat{x}_k,\hat{P}_k)=\eta N(C_kx_k,Q)\cdot N(\check{x}_k,\check{P}_k)$
3. 我们已经知道等式两侧都是高斯分布，所以只需要比较指数部分，不需要比较高斯分布前面的因子部分
   （回顾书上P236式9.5中的贝叶斯公式，分母evidence即$P(A|B)=\frac{P(B|A)P(A)}{P(B)}$中的$P(B)$被省略掉了，所以等式用了正比于符号$\propto$表示。而后推导出两边高斯分布的均值和协方差后，又把正比于符号用等号代替，增加了一个比例系数$\eta$，那么严格来说这里的$\eta$，就是前面的evidence的倒数，即使知道了这个，书上的推导还是假设了两边的因子相等，不知道为什么）
   所以接下来我们把指数部分展开：

至此，我们已经推导了卡尔曼滤波器的整个过程。再来总结一下，卡尔曼滤波器推导后有5个公式（2个预测即先验的均值和协方差，1个卡尔曼增益，还有2个即后验分布的均值和协方差）：

• 预测：

• $\check{x}_k=A_k\hat{x}_{k-1}, \ \ \check{P}_k=A_k\hat{P}_{k-1}A^\top_k + R$

• 更新

• 卡尔曼增益：

• $K=\check{P}_kC^\top_k(C_k\check{P}_kC^\top_k+Q_k)^{-1}$

• 后验：

• $\hat{x}_k=\check{x}_k+K(z_k-C_k\check{x}_k)$
• $\hat{P}_k = (I-KC_k)\check{P}_k$

如何更好的记忆呢？
k时刻的先验和k-1时刻的后验成线性映射；k时刻后验的均值等于先验均值加上一个卡尔曼增益和观测误差的乘积。

---

卡尔曼的滤波全部推导完了，现在我们总结一下简化的步骤：

1. 状态估计：由贝叶斯定理得到状态估计的后验正比于似然×先验
2. 线性系统：运动方程和观测方程的线性方程列写
3. 预测：进行贝叶斯方程的先验推导，利用运动线性方程和高斯方程加乘性质
4. 卡尔曼增益推导：

1. 利用观测方程的高斯分布性质求似然。
2. 后验=似然×先验
3. 比较等式两边指数部分，得到卡尔曼增益K的定义（最后的K用SMW进行相等）

5. 更新：利用后验的等式和卡尔曼增益，推出后验的均值和协方差式子

4. 扩展卡尔曼滤波

我们知道，卡尔曼滤波利用的是线性系统方程，而SLAM中的运动方程和观测方程通常是非线性的，特别是加入了相机内参模型和李代数表示的位姿，更不可能是一个线性系统。一个高斯分布，经过非线性变换后，往往不再是高斯分布。

所以我们需要将卡尔曼滤波器的结果拓展到非线性系统中。处理方法是：在某个点附近考虑运动方程及观测方程的一阶泰勒展开，只保留一阶项，即线性的部分。然后按照线性系统进行推导。总的来说，就是把非线性近似到线性，再利用相似的推导推导出扩展卡尔曼滤波。

流程：

1. 非线性到线性：将运动方程（xk-1）和观测方程（xk）一阶泰勒展开
2. 状态估计：即后验的贝叶斯方程，后验=n似然×先验
3. 线性系统：第一步已经给出
4. 预测：利用线性运动方程和xk-1时刻的后验和高斯复合法则，求出k时刻的先验
5. 卡尔曼增益的推导：计算出似然，得到贝叶斯方程两边的结果。比较指数部分，定义卡尔曼增益
6. 更新：得到后验关于先验的映射关系