SLAM中卡尔曼滤波（EKF）的详细推导

先引入状态估计的概率解释

SLAM过程可以由运动方程和观测方程来描述。假设在$t=0$到$t=N$的时间内，有位姿$x_0,x_1,...,x_N$，并且有路标$y_1,y_2,...,y_M$。运动和观测方程为：

$$\{\begin{array}{ll}{x}_k=f\left(x_{k-1},u_k\right)+w_k& k=1,\dots ,N\\ z_{k,j}=h\left(y_j,x_k\right)+v_{k,j}& j=1,\dots ,M\end{array}$$
注意:

• 观测方程中，只有当$x_k$看到了$y_j$时才会产生观测数据，否则没有。通常一个位置通常只能看到一小部分路标，且由于视觉SLAM特征点数量众多，所以实际中的观测方程数量会远大于运动方程。
• 在没有运动方程的情况下，整个优化问题就只由许多观测方程组成，相当于通过一组图像来恢复运动和结构。

每个方程都受噪声影响，所以要把这里的位姿$x$和路标$y$看成服从某种概率分布的随机变量，而不是一个数。由此问题变为：当拥有某些运动数据$u$和观测数据$z$时，如何确定状态变量$x,y$的分布。进而如果得到了新时刻的数据，其分布又将怎样变化。

常见且合理情况下，假设状态量和噪声服从高斯分布，即程序中只需要储存它们的均值和协方差矩阵。问题就转变为：当存在一些运动数据和观测数据时，如何估计状态量的高斯分布。

由于位姿和路标点都是待估计的变量，改变记法，令$x_k$为k时刻的所有未知量，包括当前时刻的相机位姿和m个路标点。写为：
x k = d e f { x k , y 1 , . . . , y m } {x_k}\mathop = \limits^{def} \{ {x_k},{y_1},...,{y_m}\} xk​=def{xk​,y1​,...,ym​}
同时把k时刻的所有观测记作$z_k$。于是运动方程和观测方程更简洁，不会出现$y$（包含在$x$中）：
$$\{\begin{array}{l}{x}_k=f\left(x_{k-1},u_k\right)+w_k\\ z_k=h\left(x_k\right)+v_k\end{array}k=1,\dots ,N$$
现在考虑k时刻情况，希望用过去0到k中的数据来估计现在的状态分布：
$$P(x_k|x_0,u_{1:k},z_{1:k})$$
下标0：k表示0时刻到k时刻的所有数据。$z_k$表示所有在k时刻的观测数据，可能不止一个。同时$x_k$实际上和$x_{k-1},x_{k-2}$这些量都有关，只是没有显示地写出来。

按照贝叶斯法则，把$z_k$和$x_k$交换位置，有：
$$\begin{array}{rl}\underset{\text{后验 }}{\underbrace{P\left(x_k\mid x_0,u_{1:k},z_{1:k}\right)}}& =\frac{P\left(x_k,x_0,u_{1:k},z_{1:k}\right)}{P\left(x_0,u_{1:k},z_{1:k}\right)}=\frac{P\left(z_k\mid x_k\right)P\left(x_k\mid x_0,u_{1:k},z_{1:k-1}\right)}{P\left(x_0,u_{1:k},z_{1:k}\right)}\\ & \propto \underset{\text{似然 }}{\underbrace{P\left(z_k\mid x_k\right)}}\underset{\text{先验}}{\underbrace{P\left(x_k\mid x_0,u_{1:k},z_{1:k-1}\right)}}\end{array}$$
似然由观测方程给定。至于先验部分，当前状态$x_k$是基于过去所有的状态估计得来的。至少是会受$x_{k-1}$的影响，以$x_{k-1}$时刻为条件概率展开：
$$P(x_k|x_0,u_{1:k},z_{1:k-1})=\int P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})d{x}_{k-1}$$
考虑更久的状态可以继续对此式展开，现在只关心k时刻可k-1时刻的情况。至此，已经给出了贝叶斯估计，因为上式没有具体的概率分布形式，无法实际操作它。

对这一步的后续处理，方法上有一些分歧。大体上存在若干种选择：

• 一种是假设马尔可夫性，简单的一阶马氏性认为，k时刻状态只与k-1时刻状态有关，与之前的无关，这样就得到以扩展卡尔曼滤波（EKF）为代表的滤波器方法。在滤波方法中，会从某时刻的状态估计推导到下一个时刻。
• 另一种方法是考虑k时刻状态与之前所有状态的关系，得到非线性优化为主体的优化框架，目前视觉SLAM的主流为非线性优化方法。

线性系统和卡尔曼滤波（KF）

假设马尔可夫性，当前时刻状态只和上一个时刻有关，即：
$$\begin{array}{rl}& P\left(x_k\mid x_{k-1},x_0,u_{1:k},z_{1:k-1}\right)\to P\left(x_k\mid x_{k-1},u_k\right)\\ & P\left(x_{k-1}\mid x_0,u_{1\cdot k},z_{1\cdot k-1}\right)\to P\left(x_{k-1}\mid x_0,u_{1\cdot k-1},z_{1\cdot k-1}\right)\\ & \end{array}$$

• 第一部分：由于k时刻状态与k-1之前的无关，所以简化成只与$x_{k-1}$和$u_k$有关的形式。
• 第二部分：考虑到k时刻的输入量$u_k$与k-1时刻的状态无关，所以把$u_k$拿掉。可以看到，这一项实际上就是k-1时刻的状态分布。

这一系列方程说明实际在做的事是“如何把k-1时刻的状态分布推导至k时刻”。即在程序运行期间只要维护一个状态量，对它不断地进行迭代和更新即可。如假设状态量服从高斯分布，那么只需要考虑维护状态量的均值和协方差即可。

从最简单的线性高斯系统开始，最后得到卡尔曼滤波器。明确了起点和终点之后再来考虑中间路线。线性高斯系统是指运动方程和观测方程可以由线性方程来描述：
$$\{\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,\dots ,N$$
并假设所有的状态和噪声均满足高斯分布：
$$w_k\sim N\left(0,R_k\right)v_k\sim N\left(0,Q_k\right)$$
现在利用马尔可夫性，假设知道了k-1时刻的后验状态估计${\hat{x}}_{k-1}$及协方差${\hat{P}}_{k-1}$，现在要根据k时刻的输入和观测数据，确定$x_k$的后验分布。未区分推导中的先验和后验，假设${\hat{x}}_k$表示后验，${\check{x}}_k$表示先验分布。

卡尔曼滤波第一步，通过运动方程确定$x_k$的先验分布。运动方程是线性的，而高斯分布线性变换后仍是高斯分布。所以有：
$$P\left(x_{k-1}\mid x_0,u_{1:k},z_{1:k-1}\right)\sim N\left(A_k{\hat{x}}_{k-1}+u_k,A_k{\hat{P}}_{k-1}{A}_k^T+R_k\right)$$
这一步称为预测，显示了如何从上一个时刻的状态，根据输入信息（但是有噪声）推断当前时刻的状态分布。这个分布就是先验。记：
$${\check{x}}_k=A_k{\hat{x}}_{k-1}+u_k{\check{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^T+R$$
一方面，显然这一步状态的不确定性要变大，因为系统添加了噪声。另一方面，可以计算在某个状态下应该产生怎样的观测数据：
$$P(z_k|x_k)\sim N(C_k{x}_k,Q_k)$$
为了得到后验概率，想要计算它们的乘积，即公式：
$$\underset{\text{后验 }}{\underbrace{P\left(x_k\mid x_0,u_{1:k},z_{1:k}\right)}}\propto \underset{\text{似然 }}{\underbrace{P\left(z_k\mid x_k\right)}}\underset{\text{先验}}{\underbrace{P\left(x_k\mid x_0,u_{1:k},z_{1:k-1}\right)}}$$
虽然最后会得到一个关于$x_k$的高斯分布，但是计算有一点麻烦，先把结果设为$x_k\sim N({\check{x}}_k,{\check{P}}_k)$，那么：
$$N({\hat{x}}_k,{\hat{P}}_k)=\eta \cdot N(C_k{x}_k,Q)\cdot N({\check{x}}_k,{\hat{P}}_k)$$
用稍微讨巧的方法。已经知道等式两侧都是高斯分布，就只需比较指数部分，无需理会高斯分布前面的因子部分。指数部分很像一个二次型的配方，推导一下，先把指数部分展开：
$${\left(x_k-{\hat{x}}_k\right)}^T{\hat{P}}_k^{-1}\left(x_k-{\hat{x}}_k\right)={\left(z_k-C_k{x}_k\right)}^T{Q}_k^{-1}\left(z_k-C_k{x}_k\right)+{\left(x_k-{\check{x}}_k\right)}^T{\check{P}}_k^{-1}\left(x_k-{\check{x}}_k\right)$$
这里的等号并不严格，实际允许相差与$x_k$无关的常数。为了求得左侧的${\hat{x}}_k,{\hat{P}}_k$，将两边展开。比较$x_k$的二次和一次系数。

对于二次项系数，有：
$${\hat{P}}_k^{-1}=C_k^T{Q}_k^{-1}{C}_k+{\check{P}}_k^{-1}$$
该式给出了协方差矩阵的计算过程。为了便于后面的式子，定义中间变量：
$$K={\hat{P}}_k{C}_k^T{Q}_k^{-1}$$

将${\hat{P}}_k^{-1}=C_k^T{Q}_k^{-1}{C}_k+{\check{P}}_k^{-1}$两边左乘${\hat{P}}_k$，有
$$I={\hat{P}}_k{C}_k^T{Q}_k^{-1}{C}_k+{\hat{P}}_k{\check{P}}_k^{-1}=K{C}_k+{\hat{P}}_k{\check{P}}_k^{-1}$$

于是有：

$${\hat{P}}_k=(I-K{C}_k){\check{P}}_k$$
这里有点循环定义的意思，由${\hat{P}}_k$定义了$K$，又把${\hat{P}}_k$写成了$K$的表达式。而实际中$K$可以不依靠${\hat{P}}_k$算得（需要用到SMW公式）：
$$K={\check{P}}_k{C}_k^T(C_k{\check{P}}_k{C}_k^T+Q_k{)}^{-1}$$

再比较一次项系数，有：
$$-2{\hat{x}}_k^T{\hat{P}}_k^{-1}{x}_k=-2z_k^T{Q}_k^{-1}{C}_k{x}_k-2{\check{x}}_k^T{\check{P}}_k^{-1}{x}_k$$
取系数并转置得：
$${\hat{P}}_k^{-1}{\hat{x}}_k=C_k^T{Q}_k^{-1}{z}_k+{\check{P}}_k^{-1}{\check{x}}_k$$
两侧左乘${\hat{P}}_k$，得：
$${\hat{x}}_k={\hat{P}}_k{C}_k^T{Q}_k^{-1}{z}_k+{\hat{P}}_k{\check{P}}_k^{-1}{\check{x}}_k$$
$$\begin{array}{rl}{\hat{x}}_k& ={\hat{P}}_k{C}_k^T{Q}_k^{-1}{z}_k+{\hat{P}}_k{\check{P}}_k^{-1}{\check{x}}_k\\ & =K{z}_k+(I-K{C}_k){\check{x}}_k\\ & ={\check{x}}_k+K(z_k-C_k{\check{x}}_k)\end{array}$$
由此得到了后验均值得表示，上面两个步骤可以归纳为“预测”和“更新”。流程如下所示：

预测：
$${\check{x}}_k=A_k{\hat{x}}_{k-1}+u_k{\check{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^T+R$$

更新：先计算K，它又称卡尔曼增益：
$$K={\check{P}}_k{C}_k^T(C_k{\check{P}}_k{C}_k^T+Q_k{)}^{-1}$$
然后计算后验概率分布：
$$\begin{array}{rl}{\hat{x}}_k& ={\check{x}}_k+K(z_k-C_k{\check{x}}_k)\\ {\hat{P}}_k& =(I-K{C}_k){\check{P}}_k\end{array}$$

至此从概率角度出发的最大后验概率估计方式推导了经典的卡尔曼滤波器的整个过程。在线性高斯系统中，卡尔曼滤波构成了该系统中的最大后验概率估计。由于高斯分布经过线性变换仍服从高斯分布，整个过程没有任何近似，可以说卡尔曼滤波器构成线性系统的最优无偏估计。

非线性系统和扩展卡尔曼滤波（EKF）

SLAM中的运动方程和观测方程通常是非线性函数，尤其是视觉SLAM中的相机模型。一个高斯分布，经过非线性变换后，往往不再是高斯分布，所以在非线性系统中，必须取一定的近似，将一个非高斯分布近似成高斯分布。

把卡尔曼滤波器的结果扩展到非线性系统中，称为扩展卡尔曼滤波器。通常的做法是在某个点附近考虑运动方程和观测方程一阶泰勒展开，只保留一阶项（线性部分），然后按照线性系统进行推导。

令$k-1$时刻的的均值和协方差矩阵为${\hat{x}}_{k-1}$和${\hat{P}}_{k-1}$，在k时刻，把运动方程和观测方程在${\hat{x}}_{k-1}$和${\hat{P}}_{k-1}$处线性化（一阶泰勒展开），有：
$$x_k\approx f({\hat{x}}_{k-1},u_k)+\frac{\partial f}{\partial x_{k-1}}{|}_{{\hat{x}}_{k-1}}(x_{k-1}-{\hat{x}}_{k-1})+w_k$$
记这里的偏导数为：
$$F=\frac{\partial f}{\partial x_{k-1}}{|}_{{\hat{x}}_{k-1}}$$
同样，对于观测方程，有：
$$z_k\approx h({\check{x}}_k)+\frac{\partial h}{\partial x_k}{|}_{{\check{x}}_k}(x_k-{\check{x}}_k)+n_k$$
记这里的偏导数为：
$$H=\frac{\partial h}{\partial x_k}{|}_{{\check{x}}_k}$$
那么，在预测步骤中，根据运动方程有：
$$P\left(x_k\mid x_0,u_{1:k},z_{0:k-1}\right)\sim N\left(f({\hat{x}}_{k-1},u_k),F{\hat{P}}_{k-1}{F}^T+R_k\right)$$
这些推导与卡尔曼滤波十分相似。为方便表述，记这里的先验和协方差的均值为:
$${\check{x}}_k=f({\hat{x}}_{k-1},u_k){\check{P}}_k=F{\hat{P}}_{k-1}{F}^T+R_k$$
即观测为：
$$P\left(x_k\mid x_0,u_{1:k},z_{0:k-1}\right)\sim N({\check{x}}_k,{\check{P}}_k)$$
然后，考虑在观测中有：
$$P(z_k|x_k)\sim N(h({\check{x}}_k)+H(x_k-{\check{x}}_k),Q_k)$$
最后，根据最开始的贝叶斯公式，可以推导 的后验概率形式。这里只介绍结果，简而言之，会先定义一个卡尔曼增益$K_k$：
$$K_k={\check{P}}_k{H}^T(H{\check{P}}_k{H}^T+Q_k{)}^{-1}$$
在卡尔曼增益的基础上，后验概率的形式为：
$${\hat{x}}_k={\check{x}}_k+K_k(z_k-h({\check{x}}_k){\hat{P}}_k=(I-K_{k}H){\check{P}}_k$$
卡尔曼滤波器给出了在线性化之后状态变量分布的变化过程。在线性系统和高斯噪声下，卡尔曼给出了无偏最优估计。而在SLAM这种非线性情况下，它给出了单次线性近似下的最大后验估计。

EKF证明过程
把结果设为$x_k\sim N({\hat{x}}_k,{\hat{P}}_k)$，那么：
$$N({\hat{x}}_k,{\hat{P}}_k)=\eta \cdot N(h({\check{x}}_k)+H(x_k-{\check{x}}_k),Q_k)\cdot N({\check{x}}_k,{\check{P}}_k)$$
即：
$${\left(x_k-{\hat{x}}_k\right)}^T{\hat{P}}_k^{-1}\left(x_k-{\hat{x}}_k\right)={\left(z_k-[h({\check{x}}_k)+H(x_k-{\check{x}}_k)]k\right)}^T{Q}_k^{-1}\left(z_k-[h({\check{x}}_k)+H(x_k-{\check{x}}_k)]k\right)+{\left(x_k-{\check{x}}_k\right)}^T{\check{P}}_k^{-1}\left(x_k-{\check{x}}_k\right)$$
定义：
$$K_k={\hat{P}}_k{H}^T{Q}_k^{-1}$$
对于二次项系数：
$${\hat{P}}_k^{-1}=H^T{Q}_k^{-1}H+{\check{P}}_k^{-1}$$
两边同时左乘${\hat{P}}_k$，得：
$$\begin{array}{c}I=K_{k}H+{\hat{P}}_k{\check{P}}_k^{-1}\\ \downarrow \\ {\hat{P}}_k=\left(I-K_{k}H\right)\check{P_k}\end{array}$$
对于一次项系数：
$$-2{\hat{x}}_k^T{\hat{P}}_k^{-1}{x}_k=-2[z_k-h({\check{x}}_k)+H{\check{x}}_k{]}^T{Q}_k^{-1}H{x}_k-2{\check{x}}_k^T{\check{P}}_k^{-1}{x}_k$$
取系数并转置：
$${\hat{P}}_k^{-1}{\hat{x}}_k=H^T{Q}_k^{-1}[z_k-h({\check{x}}_k)+H{\check{x}}_k]+{\check{P}}_k^{-1}{\check{x}}_k$$
两侧左乘${\hat{P}}_k$，得：
$$\begin{array}{rl}{\hat{x}}_k& ={\hat{P}}_k{H}^T{Q}_k^{-1}[z_k-h({\check{x}}_k)+H{\check{x}}_k]+{\hat{P}}_k{\check{P}}_k^{-1}{\check{x}}_k\\ & =K_k[z_k-h({\check{x}}_k)+H{\check{x}}_k]+(I-K_{k}H){\check{x}}_k\\ & ={\check{x}}_k+K_K(z_k-h({\check{x}}_k))\end{array}$$

总结

• 预测，根据运动方程估计先验概率分布${\check{x}}_k$和${\check{P}}_k$。

• 更新，根据观测方程计算$K$并更新后验概率分布${\hat{x}}_k$和${\hat{P}}_k$。