SLAM中的EKF，UKF，PF原理简介

时间 2019-12-08

标签 slam ekf ukf 原理简介繁體版

原文原文链接

这是我在知乎上问题写的答案，修改了一下排版，转到博客里。

原问题：

可否简单而且易懂地介绍一下多个基于滤波方法的SLAM算法原理？

目前SLAM后端都开始用优化的方法来作，题主想要了解一下以前基于滤波的方法，但愿有大神可以总结一下各个原理（EKF，UKF，PF，FastSLAM），感激涕零。

做者：半闲居士
连接：https://www.zhihu.com/question/46916554/answer/103411007
来源：知乎
著做权归做者全部。商业转载请联系做者得到受权，非商业转载请注明出处。

　　我怎么会写得那么长……若是您有兴趣能够和我一块把公式过一遍。
　　要讲清这个问题，得从状态估计理论来讲。先摆上一句名言：

状态估计乃传感器之本质。（To understand the need for state estimation is to understand the nature of sensors.）

　　任何传感器，激光也好，视觉也好，整个SLAM系统也好，要解决的问题只有一个： 如何经过数据来估计自身状态。每种传感器的测量模型不同，它们的精度也不同。换句话说，状态估计问题，也就是“ 如何最好地使用传感器数据”。能够说，SLAM是状态估计的一个特例。

1. 离散时间系统的状态估计

　　记机器人在各时刻的状态为 $x_1,\ldots,x_k$ ，其中

是离散时间下标。在SLAM中，咱们一般要估计机器人的位置，那么系统的状态就指的是机器人的位姿。用两个方程来描述状态估计问题：

$\left\{ \begin{array}{l} {x_k} = f\left( {{x_{k - 1}},{u_k},{w_k}} \right)\\ {y_k} = g\left( {{x_k},{n_k}} \right) \end{array} \right.$
　　解释一下变量：

-运动方程
　　

- 输入
　　

- 输入噪声
　　

- 观测方程
　　

- 观测数据
　　

- 观测噪声

　　运动方程描述了状态 $x_{k-1}$ 是怎么变到

的，而观测方程描述的是从

是怎么获得观察数据

的。
　　请注意这是一种抽象的写法。当你有实际的机器人，实际的传感器时，方程的形式就会变得具体，也就是所谓的 参数化。例如，当咱们关心机器人空间位置时，能够取

。进而，机器人携带了里程计，可以获得两个时间间隔中的相对运动，像这样 $\Delta x_k=[\Delta x, \Delta y, \Delta z]_k$ ，那么运动方程就变为：
$x_{k+1}=x_k+\Delta x_k+w_k$
　　同理，观测方程也随传感器的具体信息而变。例如激光传感器能够获得空间点离机器人的距离和角度，记为 $y_k=[r,\theta]_k$ ，那么观测方程为：
${\left[ \begin{array}{l} r\\ \theta \end{array} \right]_k} = \left[ \begin{array}{l} \sqrt {{{\left\| {{x_k} - {L_k}} \right\|}_2}} \\ {\tan ^{ - 1}}\frac{{{L_{k,y}} - {x_{k,y}}}}{{{L_{k,x}} - {x_{k,x}}}} \end{array} \right] + {n_k}$ 　　其中 $L_k=[L_{k,x},L_{k,y}]$ 是一个2D路标点。

　　举这几个例子是为了说明， 运动方程和观测方程具体形式是会变化的。可是，咱们想讨论更通常的问题：当我不限制传感器的具体形式时，可否设计一种方式，从已知的

（输入和观测数据）从，估计出

呢？

　　这就是最通常的状态估计问题。咱们会根据

是否线性，把它们分为 线性/非线性系统。同时，对于噪声

，根据它们是否为高斯分布，分为 高斯/非高斯噪声系统。最通常的，也是最困难的问题，是非线性-非高斯(NLNG, Nonlinear-Non Gaussian)的状态估计。下面先说最简单的状况：线性高斯系统。

2. 线性高斯系统(LG，Linear Gaussian)

　　在线性高斯系统中，运动方程、观测方程是线性的，且两个噪声项服从零均值的高斯分布。这是最简单的状况。简单在哪里呢？主要是由于 高斯分布通过线性变换以后仍为高斯分布。而对于一个高斯分布，只要计算出它的一阶和二阶矩，就能够描述它（高斯分布只有两个参数 $\mu, \Sigma$ ）。
　　线性系统形式以下：
$\left\{ \begin{array}{l} {x_k} = {A_{k - 1}}{x_{k - 1}} + {u_k} + {w_k}\\ {y_k} = {C_k}{x_k} + {n_k}\\ {w_k}\sim N\left( {0,{Q_k}} \right)\\ {n_k}\sim N(0,{R_k}) \end{array} \right.$ 　　其中

是两个噪声项的协方差矩阵。

为转移矩阵和观测矩阵。
　　对LG系统，能够用贝叶斯法则，计算

的后验几率分布——这条路直接通向 卡尔曼滤波器。卡尔曼是线性系统的递推形式（recursive，也就是从 $x_{k-1}$ 估计

）的无偏最优估计。因为解释EKF和UKF都得用它，因此咱们来推一推。若是读者不感兴趣，能够跳过公式推导环节。
　　符号：用 $\hat{x}$ 表示

的后验几率，用 $\tilde x$ 表示它的先验几率。由于系统是线性的，噪声是高斯的，因此状态也服从高斯分布，须要计算它的均值和协方差矩阵。记第

时刻的状态服从： $x_k\sim N({{\bar x}_k},{P_k})$

　　咱们但愿获得状态变量

的最大后验估计（MAP，Maximize a Posterior），因而计算：
$\begin{array}{ccl} \hat x &=& \arg \mathop {\max }\limits_x p\left( {x|y,v} \right)\\ &=& \arg \max \frac{{p\left( {y|x,v} \right)p\left( {x|v} \right)}}{{p\left( {y|v} \right)}} \\ &=& \arg \max p(y|x)p\left( {x|v} \right) \end{array}$
　　第二行是贝叶斯法则，第三行分母和

无关因此去掉。
　　第一项即观测方程，有：

$p\left( {y|x} \right) = \prod\limits_{k = 0}^K {p\left( {{y_k}|{x_k}} \right)}$ 　　很简单。
　　第二项即运动方程

$p\left( {x|v} \right) = \prod\limits_{k = 0}^K {p\left( {{x_k}|{x_{k - 1}},v_k} \right)}$ 　　也很简单。
　　如今的问题是如何求解这个最大化问题。对于高斯分布，最大化问题能够变成最小化它的负对数。当我对一个高斯分布取负对数时，它的指数项变成了一个二次项，而前面的因子则变为一个无关的常数项，能够略掉（这部分我不敲了，有疑问的同窗能够问）。因而，定义如下形式的最小化函数：

$\begin{array}{l} {J_{y,k}}\left( x \right) = \frac{1}{2}{\left( {{y_k} - {C_k}{x_k}} \right)^T}R_k^{ - 1}\left( {{y_k} - {C_k}{x_k}} \right)\\ {J_{v,k}}\left( x \right) = \frac{1}{2}{\left( {{x_k} - {A_{k - 1}}{x_{k - 1}} - {v_k}} \right)^T}Q_k^{ - 1}\left( {{x_k} - {A_{k - 1}}{x_{k - 1}} - {v_k}} \right) \end{array}$ 　　那么最大后验估计就等价于：
$\hat x = \arg \min \sum\limits_{k = 0}^K {{J_{y,k}} + {J_{v,k}}}$
　　这个问题如今是二次项和的形式，写成矩阵形式会更加清晰。定义：
$\begin{array}{l} z = \left[ \begin{array}{l} {x_0}\\ {v_1}\\ \vdots \\ {v_K}\\ {y_0}\\ \vdots \\ {y_K} \end{array} \right],x = \left[ \begin{array}{l} {x_0}\\ \vdots \\ {x_K} \end{array} \right]\\ H = \left[ {\begin{array}{*{20}{c}} 1&{}&{}&{}\\ { - {A_0}}&1&{}&{}\\ {}& \ddots & \ddots &{}\\ {}&{}&{ - {A_{K - 1}}}&1\\ {{C_0}}&{}&{}&{}\\ {}& \ddots &{}&{}\\ {}&{}& \ddots &{}\\ {}&{}&{}&{{C_K}} \end{array}} \right],W = \left[ {\begin{array}{*{20}{c}} {{P_0}}&{}&{}&{}&{}&{}&{}\\ {}&{{Q_1}}&{}&{}&{}&{}&{}\\ {}&{}& \ddots &{}&{}&{}&{}\\ {}&{}&{}&{{Q_K}}&{}&{}&{}\\ {}&{}&{}&{}&{{R_1}}&{}&{}\\ {}&{}&{}&{}&{}& \ddots &{}\\ {}&{}&{}&{}&{}&{}&{{R_K}} \end{array}} \right] \end{array}$

　　就获得矩阵形式的，相似最小二乘的问题：
$J\left( x \right) = \frac{1}{2}{\left( {z - Hx} \right)^T}{W^{ - 1}}\left( {z - Hx} \right)$
　　因而令它的导数为零，获得：
$\frac{{\partial J}}{{\partial {x^T}}} = - {H^T}{W^{ - 1}}\left( {z - Hx} \right) = 0 \Rightarrow \left( {{H^T}{W^{ - 1}}H} \right)x = {H^T}{W^{ - 1}}z$ (*)
　　读者会问，这个问题和卡尔曼滤波有什么问题呢？事实上， 卡尔曼滤波就是递推地求解(*)式的过程。所谓递推，就是只用 $x_{k-1}$ 来计算

。对(*)进行Cholesky分解，就能够推出卡尔曼滤波器。详细过程限于篇幅就不推了，把卡尔曼的结论写一下：
$\begin{array}{l} {{\tilde P}_k} = {A_{k - 1}}{{\hat P}_{k - 1}}A_{k - 1}^T + {Q_k}\\ {{\tilde x}_k} = {A_{k - 1}}{{\hat x}_{k - 1}} + {v_k}\\ {K_k} = {{\tilde P}_k}C_k^T{\left( {{C_k}{{\tilde P}_k}C_k^T + {R_k}} \right)^{ - 1}}\\ {{\hat P}_k} = \left( {I - {K_k}{C_k}} \right){{\tilde P}_k}\\ {{\hat x}_k} = {{\tilde x}_k} + {K_k}\left( {{y_k} - {C_k}{{\tilde x}_k}} \right) \end{array}$
　　前两个是预测，第三个是卡尔曼增益，四五是校订。
　　另外一方面，可否直接求解(*)式，获得 $\hat{x}$ 呢？答案是能够的，并且这就是优化方法（batch optimization）的思路：将全部的状态放在一个向量里，进行求解。与卡尔曼滤波不一样的是，在估计前面时刻的状态（如

）时，会用到后面时刻的信息（

等）。从这点来讲，优化方法和卡尔曼处理信息的方式是至关不一样的。

3. 扩展卡尔曼滤波器

　　线性高斯系统固然性质很好啦，但许多现实世界中的系统都不是线性的，状态和噪声也不是高斯分布的。例如上面举的激光观测方程就不是线性的。当系统为非线性的时候，会发生什么呢？
　　一件悲剧的事情是： 高斯分布通过非线性变换后，再也不是高斯分布。并且，是个什么分布，基本说不上来。（摊手）
　　若是没有高斯分布，上面说的那些都再也不成立了。 因而EKF说，嘛，咱们睁一只眼闭一只眼，用高斯分布去近似它，而且，在工做点附近对系统进行线性化。固然这个近似是很成问题的，有什么问题咱们以后再说。
　　EKF的作法主要有两点。其一，在工做点附近 ${{\hat x}_{k - 1}},{{\tilde x}_k}$ ，对系统进行线性近似化：
$\begin{array}{l} f\left( {{x_{k - 1}},{v_k},{w_k}} \right) \approx f\left( {{{\hat x}_{k - 1}},{v_k},0} \right) + \frac{{\partial f}}{{\partial {x_{k - 1}}}}\left( {{x_{k - 1}} - {{\hat x}_{k - 1}}} \right) + \frac{{\partial f}}{{\partial {w_k}}}{w_k}\\ g\left( {{x_k},{n_k}} \right) \approx g\left( {{{\tilde x}_k},0} \right) + \frac{{\partial g}}{{\partial {x_k}}}{n_k} \end{array}$
　　这里的几个偏导数，都在工做点处取值。因而呢，它就被 活生生地当成了一个线性系统。
　　第二，在线性系统近似下，把噪声项和状态都 当成了高斯分布。这样，只要估计它们的均值和协方差矩阵，就能够描述状态了。通过这样的近似以后呢，后续工做都和卡尔曼滤波是同样的了。因此EKF是卡尔曼滤波在NLNG系统下的直接扩展（因此叫扩展卡尔曼嘛）。EKF给出的公式和卡尔曼是一致的，用线性化以后的矩阵去代替卡尔曼滤波器里的转移矩阵和观测矩阵便可。
$\begin{array}{l} {{\tilde P}_k} = {F_{k - 1}}{{\hat P}_{k - 1}}F_{k - 1}^T + Q_k'\\ {{\tilde x}_k} = f\left( {{{\hat x}_{k - 1}},{v_k},0} \right)\\ {K_k} = {{\tilde P}_k}G_k^T{\left( {{G_k}{{\tilde P}_k}G_k^T + R_k'} \right)^{ - 1}}\\ {{\hat P}_k} = \left( {I - {K_k}{G_k}} \right){{\tilde P}_k}\\ {{\hat x}_k} = {{\tilde x}_k} + {K_k}\left( {{y_k} - g({{\tilde x}_k},0)} \right) \end{array}$ 　　其中

$F_{k-1} = {\left. {\frac{{\partial f}}{{\partial {x_{k - 1}}}}} \right|_{{{\hat x}_{k - 1}}}},{G_k} = {\left. {\frac{{\partial f}}{{\partial {x_k}}}} \right|_{{{\tilde x}_k}}}$

　　这样作听起来仍是挺有道理的，实际上也是能用的，可是问题仍是不少的。
　　考虑一个服从高斯分布的变量 $x \sim N(0,1)$ ，如今

，问

服从什么分布？
　　我几率比较差，不过这个彷佛是叫作卡尔方布。

应该是下图中k=1那条线。

　　可是按照EKF的观点，咱们要用一个高斯分布去近似

。假设咱们采样时获得了一个

，那么就会近似成一个均值为0.25的高斯分布，然而卡方分布的指望应该是1。……可是各位真以为k=1那条线像哪一个高斯分布吗？
　　因此EKF面临的一个重要问题是，当一个高斯分布通过非线性变换后，如何用另外一个高斯分布近似它？按照它如今的作法，存在如下的局限性：（注意是滤波器本身的局限性，还没谈在SLAM问题里的局限性）。

即便是高斯分布，通过一个非线性变换后也不是高斯分布。EKF只计算均值与协方差，是在用高斯近似这个非线性变换后的结果。（实际中这个近似可能不好）。
系统自己线性化过程当中，丢掉了高阶项。
线性化的工做点每每不是输入状态真实的均值，而是一个估计的均值。因而，在这个工做点下计算的，也不是最好的。
在估计非线性输出的均值时，EKF算的是 $\mu_y=f(\mu_x)$ 的形式。这个结果几乎不会是输出分布的真正指望值。协方差也是同理。

那么，怎么克服以上的缺点呢？途径不少，主要看咱们想不想维持EKF的假设。若是咱们比较乖，但愿维持高斯分布假设，能够这样子改：

为了克服第3条工做点的问题，咱们以EKF估计的结果为工做点，从新计算一遍EKF，直到这个工做点变化够小。是为迭代EKF（Iterated EKF, IEKF）。
为了克服第4条，咱们除了计算 $\mu_y=f(\mu_x)$ ，再计算其余几个精心挑选的采样点，而后用这几个点估计输出的高斯分布。是为Sigma Point KF（SPKF，或UKF）。

　　若是不那么乖，能够说： 咱们不要高斯分布假设，凭什么要用高斯去近似一个长得根本不高斯的分布呢？因而问题变为，丢掉高斯假设后，怎么描述输出函数的分布就成了一个问题。一种比较暴力的方式是：用足够多的采样点，来表达输出的分布。这种蒙特卡洛的方式，也就是粒子滤波的思路。
　　若是再进一步，能够丢弃滤波器思路，说： 为何要用前一个时刻的值来估计下一个时刻呢？ 咱们能够把全部状态当作变量，把运动方程和观测方程当作变量间的约束，构造偏差函数，而后最小化这个偏差的二次型。这样就会获得非线性优化的方法，在SLAM里就走向图优化那条路上去了。不过，非线性优化也须要对偏差函数不断地求梯度，并根据梯度方向迭代，于是局部线性化是不可避免的。
　　能够看到，在这个过程当中，咱们逐渐放宽了假设。

4. UKF 无迹卡尔曼

　　因为题主问题里没谈IEKF，咱们就简单说说UKF和PF。
　　UKF主要解决一个高斯分布通过非线性变换后，怎么用另外一个高斯分布近似它。假设 $x \sim N(\mu_x \Sigma_{xx} ), y=g(x)$ ，咱们但愿用 $N(\mu_y, \Sigma_{yy})$ 近似

。按照EKF，须要对

作线性化。但在UKF里，没必要作这个线性化。
　　UKF的作法是找一些叫作Sigma Point的点，把这些点用

投影过去。而后，用投影以后的点作出一个高斯分布，以下图：

　　这里选了三个点： $\mu_x, \mu_x+\sigma_x, \mu_x-\sigma_x$ 。对于维数为N的分布，须要选2N+1个点。篇幅所限，这里就不解释这些点怎么选，以及为什么要这样选了。总之UKF的好处就是：

没必要线性化，也没必要求导，对没有光滑性要求。
计算量随维数增加是线性的。

5. PF 粒子滤波

　　UKF的一个问题是输出仍假设成高斯分布。然而，即便在很简单的状况下，高斯的非线性变换仍然不是高斯。而且，仅在不多的状况下，输出的分布有个名字（好比卡方），多数时候你都不知道他们是啥……更别提描述它们了。
　　由于描述很困难，因此粒子滤波器采用了一种暴力的，用大量采样点去描述这个分布的方法（老子就是无参的你来打我呀）。框架大概像下面这个样子，就是一个不断采样——算权重——重采样的过程：

　　越符合观测的粒子拥有越大的权重，而权重越大就越容易在重采样时被采到。固然，每次采样数量、权重的计算策略，则是粒子滤波器里几个比较麻烦的问题，这里就不细讲了。
　　这种采样思路的最大问题是： 采样所需的粒子数量，随分布是指数增加的。因此仅限于低维的问题，高维的基本就没办法了。

6. 非线性优化

　　非线性优化，计算的也是最大后验几率估计（MAP），但它的处理方式与滤波器不一样。对于上面写的状态估计问题，能够简单地构造偏差项：
$\begin{array}{l} {e_{v,k}}\left( x \right) = {x_k} - f\left( {{x_{k - 1}},{v_k},0} \right)\\ {e_{y,k}}\left( x \right) = {y_k} - g\left( {{x_k},0} \right) \end{array}$ 　　而后最小化这些偏差项的二次型：
$\min J\left( x \right) = \sum\limits_{k = 1}^K {\left( {\frac{1}{2}{e_{v,k}}{{\left( x \right)}^T}W_{v,k}^{ - 1}{e_{v,k}}\left( x \right)} \right) + \sum\limits_{k = 1}^K {\left( {\frac{1}{2}{e_{y,k}}{{\left( x \right)}^T}W_{v,k}^{ - 1}{e_{v,k}}\left( x \right)} \right)} }$
　　这里仅用到了噪声项知足高斯分布的假设，再没有更多的了。当构建一个非线性优化问题以后，就能够从一个初始值出发，计算梯度（或二阶梯度），优化这个目标函数。常见的梯度降低策略有牛顿法、高斯-牛顿法、Levenberg-Marquardt方法，能够在许多讲数值优化的书里找到。
　　非线性优化方法如今已经成为视觉SLAM里的主流，尤为是在它的稀疏性质被人发现且利用起来以后。它与滤波器最大不一样点在于，一次能够考虑整条轨迹中的约束。它的线性化，即雅可比矩阵的计算，也是相对于整条轨迹的。相比之下，滤波器仍是停留在马尔可夫的假设之下，只用上一次估计的状态计算当前的状态。能够用一个图来表达它们之间的关系：

　　固然优化方式也存在它的问题。例如优化时间会随着节点数量增加——因此有人会提double window optimization这样的方式，以及可能落入局部极小。可是就目前而言，它比EKF仍是优很多的。

7. 小结

卡尔曼滤波是递归的线性高斯系统最优估计。
EKF将NLNG系统在工做点附近近似为LG进行处理。
IEKF对工做点进行迭代。
UKF没有线性化近似，而是把sigma point进行非线性变换后再用高斯近似。
PF去掉高斯假设，以粒子做为采样点来描述分布。
优化方式同时考虑全部帧间约束，迭代线性化求解。

　　呃好像题主还问了FastSLAM，有空再写吧……

注：
* 本文大量观点来自Timothy. Barfoot, "State estimation for Robotics: A Matrix Lei Group Approach", 2016. 图片如有侵权望告知。

PS：

　　SLAM中，状态变量常常是六自由度的位姿，由旋转矩阵和平移向量构成。然而问题是，旋转矩阵并不存在加法，只有对应到李代数上才能够清楚地定义它的运算。所以，当咱们讨论这个位姿的噪声，说它服从高斯分布时， 咱们究竟在说什么，是一个很严重的问题。从此的博客将更深刻地介绍李群李代数的知识。