前言
本博客主要进行不同卡尔曼滤波器的数学推导，不涉及某些传感器。在了解了卡尔曼滤波器的数学模型后，不同的传感器只需将其测量模型纳入运动/观测模型。对于基于不同传感器的滤波集成方案，在阅读论文时分别准备。
1. SLAM 定位概率模型
在 SLAM 在这个问题中，我们想通过滤波来解决的问题是：解决后检测概率，即给出一系列观测（和输入）和初始时间的先检测位置，并估计每个时间的位置，如下所示。
p ( x k ∣ x 0 , u 1 : k , z 0 : k ) p(\boldsymbol{x}_k | \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k}) p(xk∣x0,u1:k,z0:k)
其中， x 0 \boldsymbol{x}_0 x0 先验位置， u 1 : k \boldsymbol{u}_{1:k} u1:k 为 1 : k 1:k 1:k 时刻的输入， z 0 : k \boldsymbol{z}_{0:k} z0:k 为 0 : k 0:k 0:k 时刻对环境的观测。我们可以将这些变量通过一个图来表示：
可以看出，在这种图模式下，每个状态仅仅依赖于前一时刻的位姿和输入，和历史位姿无关，这体现了一阶马尔科夫性。同理，观测也只和对应时刻的位姿（以及环境）有关。利用贝叶斯公式对后验概率进行展开有：
p ( x k ∣ x 0 , u 1 : k , z 0 : k ) = p ( z k ∣ x k , x 0 , u 1 : k , z 0 : k − 1 ) p ( x k ∣ x 0 , u 1 : k , z 0 : k − 1 ) p ( z k ∣ x 0 , u 1 : k , z 0 : k − 1 ) \begin{aligned} p(\boldsymbol{x}_k|\boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k}) &= \frac{p(\boldsymbol{z}_k | \boldsymbol{x}_k, \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})p(\boldsymbol{x}_k | \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})}{p(\boldsymbol{z}_k|\boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})} \end{aligned} p(xk∣x0,u1:k,z0:k)=p(zk∣x0,u1:k,z0:k−1)p(zk∣xk,x0,u1:k,z0:k−1)p(xk∣x0,u1:k,z0:k−1)
上式即为最基本的贝叶斯滤波公式求解当前状态的后验概率，公式可以分为三部分：
- p ( z k ∣ x k ) p(\boldsymbol{z}_k | \boldsymbol{x}_k) p(zk∣xk)：当前时刻，当前状态下对环境的观测
- p ( x k ∣ x k − 1 , u k ) p(\boldsymbol{x}_k| \boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}) p(xk∣xk−1,uk)：从前一时刻到当前时刻的状态变化预测
- p ( x k − 1 ∣ x 0 , u 1 : k , z 0 : k − 1 ) p(\boldsymbol{x}_{k-1} | \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1}) p(xk−1∣x0,u1:k,z0:k−1)，上一时刻的状态后验概率
因此，用比较通俗的话来描述一下贝叶斯滤波的过程：
- 首先获得上一时刻状态的后验概率分布
- 对上一时刻所有可能的状态根据运动模型对当前状态进行预测，获得当前状态的先验分布
- 结合当前状态的先验分布和当前观测结果的概率分布计算得到当前状态的后验分布
这个流程中涉及两个部分：怎么从上一时刻的状态量结合输入预测当前时刻状态量；怎么在当前状态下获得对环境的观测分布。这分别对应系统中的两个模型：
运动模型：
x k = f ( x k − 1 , u k , w k ) \boldsymbol{x}_k = f(\boldsymbol{x}_{k-1}, \boldsymbol{u}_k, \boldsymbol{w}_k) xk=f(xk−1,uk,wk)
观测模型：
z k = f ( x k , m , v k ) \boldsymbol{z}_k = f(\boldsymbol{x}_k, m, \boldsymbol{v}_k) zk=f(xk,m,vk)
其中， w k , v k \boldsymbol{w}_k, \boldsymbol{v}_k wk,vk 分别是运动模型和观测模型的噪声， m m m 为环境信息。如果问题中不需要对环境信息估计的话，可以不考虑，在大部分视觉 SLAM 中会对环境中的特征点进行优化，此时，特征点会作为环境信息纳入优化中。
2. 卡尔曼滤波 Kalman Filter
原始卡尔曼滤波推导
卡尔曼滤波假设位姿和观测符合高斯分布，且运动模型和观测模型都是线性的，因此运动模型和观测模型可以写成以下形式：
x k = F k x k − 1 + G k u k + w k , w ∼ N ( 0 , W ) z k = H k x k + v k , v ∼ N ( 0 , V ) \begin{aligned} \boldsymbol{x}_k &= \boldsymbol{F}_k\boldsymbol{x}_{k-1} + \boldsymbol{G}_k\boldsymbol{u}_k + \boldsymbol{w}_k, \qquad &\boldsymbol{w} \sim \mathcal{N}(0, \boldsymbol{W})\\ \boldsymbol{z}_k &= \boldsymbol{H}_k\boldsymbol{x}_k + \boldsymbol{v}_k, \qquad &\boldsymbol{v} \sim \mathcal{N}(0, \boldsymbol{V}) \end{aligned} xkzk=Fkxk−1+Gkuk+wk,=Hkxk+vk,w∼N(0,W)v∼N(0,V)
为了把状态和先验和后验估计区分开，用 x ˇ \check{\boldsymbol{x}} xˇ 表示先验估计，用 x ^ \hat{\boldsymbol{x}} x^ 表示后验估计。其中运动模型中，上一时刻的状态后验写为：
p ( x k − 1 ∣ x 0 , u 1 : k − 1 , z 0 : k − 1 ) = N ( x ^ k − 1 , P ^ k − 1 ) p(\boldsymbol{x}_{k-1}|\boldsymbol{x}_0, \boldsymbol{u}_{1:k-1}, \boldsymbol{z}_{0:k-1}) = \mathcal{N}(\hat{\boldsymbol{x}}_{k-1}, \hat{\boldsymbol{P}}_{k-1}) p(xk−1∣x0,u1:k−1,z0:k−1)=N(x^k−1,P^k−1)
基于运动模型得到当前时刻的状态位姿的预测值同样服从高斯分布：
x ˇ k ∼ N ( μ ˇ k , P ˇ k ) \check{\boldsymbol{x}}_k \sim \mathcal{N}(\check{\boldsymbol{\mu}}_k, \check{\boldsymbol{P}}_k) xˇk∼<