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

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})$
左边即k时刻的状态分布（后验分布，它根据了$z_k$观测数据来求状态，是知果求因）。右边第一项是似然（知因求果），第二项是先验（历史求因）：
$后验\propto 似然\times 先验$
我们卡尔曼滤波器，求的就是这个式子，我们希望估计k时刻的后验分布，那么只要求得k时刻的似然和先验即可，整个推导过程牢记这个式子，以免在推导的过程中迷失了自己。（甚至可以粗暴的认为，卡尔曼滤波器就是一个矩阵，k-1时刻的后验为x，通过Ax=b我们就求得的k时刻的后验分布。）

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

$x时刻的先验P(A|B)=\int P(A|B,x_{k-1})P(x_{k-1}|B)d{x}_{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{array}{l}{x}_k=A_k{x}_{k-1}+u_k+w_k\\ z_k=C_k{x}_k+v_k\end{array}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}_k^{\top }+R)$
我们记预测的两个式子为预测：（k时刻的先验=线性k-1时刻的后验）
${\check{x}}_k=A_k{\hat{x}}_{k-1}+uk,{\check{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^{\top }+R$

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

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

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

• 预测：
  - ${\check{x}}_k=A_k{\hat{x}}_{k-1},{\check{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^{\top }+R$
• 更新
  • 卡尔曼增益：
    • $K={\check{P}}_k{C}_k^{\top }(C_k{\check{P}}_k{C}_k^{\top }+Q_k{)}^{-1}$
  • 后验：
    • ${\hat{x}}_k={\check{x}}_k+K(z_k-C_k{\check{x}}_k)$
    • ${\hat{P}}_k=(I-K{C}_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. 更新：得到后验关于先验的映射关系