机器人学中的状态估计学习笔记（三）第四章 非线性非高斯系统的状态估计

4.1 引言

  本小节主要是从一个简化的、一维度的非线性状态估计问题——估计立体相机中路标点的位置，来对比全贝叶斯估计和最大后验估计。
  在全贝叶斯估计中尽管先验和测量均服从高斯分布，但最终得到的后验却是非对称的；经过非线性的观测模型后，后验明显偏向一边。但由于后验仍然是单峰的，仍可用高斯近似它。在引入测量后，得到后验概率密度比先验更加集中（即更加确定）；这正是隐含在贝叶斯估计背后的主要思想：我们希望将测量结果与先验结合，得到更确定的后验状态，如下图所示。

  计算贝叶斯后验通常是不切实际的，一般的做法是估计单个点，在这个点上状态后验概率得以最大化。这称为最大后验估计，如下图所示：

MAP解就是后验分布的最高值，同时也是$-ln(p(x|y))$的最低值。
  需要计算$${\hat{x}}_{map}=arg\underset{x}{\max }p(x|y)$$
  如下图所示为立体相机例子中MAP估计的偏差，可以看到MAP估计是有偏的，因为MAP值和先验均值并不在一条线上。

4.2 离散时间的递归估计问题

  首先定义了一系列的运动和观测模型：

其描述的系统随时间演变的图模型如下图所示：

  从中可以观测到该系统具有马尔可夫性：当一个随机过程在给定现在状态及所有过去状态的情况下，其未来状态的条件概率分布仅依赖于当前状态；换句话说，在给定现在状态时，未来状态与过去状态（即该过程的历史路径）是条件独立的，那么此随机过程称为马尔可夫过程，或者说它具有马尔可夫性。
  首先从贝叶斯滤波将起，贝叶斯滤波仅使用过去以及当前的测量，构造一个完整的PDF来刻画当前状态。其表达式如下：$$\underset{后验置信度}{\underbrace{p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})}}=\eta \underset{利用g(\cdot )进行更新}{\underbrace{p(y_k|x_k)}}\int \underset{利用f(\cdot )进行预测}{\underbrace{p(y_k|x_{k-1},v_k)}}p(x_{k-1}|{\check{x}}_0,v_{1:k-1},y_{0:k-1}d{x}_{k-1}$$可以看到该式具有预测-校正的形式。在预测阶段，先验置信度$p(x_{k-1}|{\check{x}}_0,v_{1:k-1},y_{0:k-1})$通过输入$v_k$和运动模型$f(\cdot )$在时间上进行前向传播。在校正阶段，则通过观测$y_k$和观测模型$g(\cdot )$来更新预测估计状态，并得到后验置信度$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})$，下图展示了贝叶斯滤波器中的信息流向。

  贝叶斯滤波虽然精确，但也仅仅是一个精美的数学产物；除了线性高斯的情况外，在实际中它基本不可能实现。主要原因有两个，为此需要适当地做一些近似：

1. 概率密度函数存在于在无限维的空间中（与所有的连续函数一样），因此需要无限的存储空间来完全表示置信度$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k})$。为了克服存储空间的问题，需要将这个置信度大致地表现出来。一种方法是将该函数近似为高斯（即只关心一、二阶矩：均值和协方差），另一种方法是使用有限数量的随机样本来近似。
2. 贝叶斯滤波器的积分在计算上十分耗时，它需要无限的计算资源去计算精确的结果。为了克服计算资源的问题，必须对积分进行近似。一种方法是对运动和观测模型进行线性化，然后对积分进行闭式求解。另一种方法是使用蒙特卡罗积分。

  将本节中的递归式非线性系统状态估计方法总结如下，其中贝叶斯滤波作为最精确的滤波器位于顶层，根据所做近似的不同得到不同的滤波器。

  如果将置信度和噪声限制为高斯分布，并且对运动模型和观测模型进行线性化，计算贝叶斯滤波中的积分（以及归一化积），将得到著名的拓展卡尔曼滤波（EKF）。

这就是EKF的经典递归更新方程。通过更新方程可以由{ ${\hat{x}}_{k-1},{\hat{P}}_{k-1}$}计算出{ ${\hat{x}}_k,{\hat{P}}_k$}，注意到这与线性高斯系统所推到出来的卡尔曼滤波器具有类似的结构，然而，它们由两个区别：
2. 通过非线性的运动和观测模型来传递估计的均值；
3. 噪声协方差$Q_k^{\prime }$和 $R_k^{\prime }$中包含了雅可比矩阵，这是因为我们允许噪声应用于非线性模型中。

需要注意的是，EKF具有以下三个问题：
4. 它并不能保证在一般的非线性系统中能够充分地发挥作用；
5. 有时EKF的估计虽然没有明显的一场，但常常是有偏差或不一致的，更经常是两者都有；
6. 线性化点是估计状态的均值，但离真值有可能较远。

  广义高斯滤波中校正步骤的方程为：

  而迭代拓展卡尔曼滤波(IEKF)预测步骤与拓展卡尔曼滤波器的推导基本相同，不同之处在于校正步骤对其中任意一个点$x_{op,k}$进行线性化，最终得到：

以上方程跟拓展卡尔曼滤波器中的卡尔曼增益和校正方程非常相似，唯一的区别在于线性化的工作点。如果将线性化的工作点设置为预测先验的均值，即$x_{op,k}={\check{x}}_k$，那么扩展卡尔曼滤波器与该迭代拓展卡尔曼滤波器完全相同。
  在迭代拓展卡尔曼滤波中可以不断对得到的结果进行迭代，每次都将上一次迭代结果作为下一次的线性化点，从而得到更好的结果：$$x_{op,k}\leftarrow {\hat{x}}_k$$在第一次迭代中，我们令$x_{op,k}={\check{x}}_k$。这使得我们能够对更好的估计进行线性化，从而改进每次迭代的近似程度。在迭代的过程中，若$x_{op,k}$的改变足够小的时候就终止迭代。
  接下来我们来看一下EKF、IEKF与全贝叶斯后验之间的关系如下：

可以看到，IEKF对应于全后验概率的（局部）极大值；换句话说，它是一个MAP估计。另一个方面，由于EKF的校正部分并没有迭代，它可能远离局部极大值。由于IEKF在最优估计出迭代地进行线性化，其估计结果等同于MAP的解。因此，IEKF高斯估计器的“均值”并不等于全贝叶斯后验的均值——它计算的是模。
  从以上可以知道，在推导EKF和IEKF时，我们使用线性化的方法将PDF传递进非线性函数中。具体来说，就是在非线性模型的工作点进行线性化，然后通过解析的线性化模型传递高斯PDF。除此之外还有蒙特卡罗方法（暴力的）以及sigmapoint或无迹（unscented）变换。
  蒙特卡罗方法本质上是一种“暴力”的方法，其过程如下图所示：

根据输入的概率密度采集大量的样本，接着通过非线性函数将每一个样本精确地进行转换，最后从转换的样本中构建输出的概率密度。笼统地说，大数定律确保了当样本数量接近无穷大时，这种做法将会使得结果收敛到正确的值。
  这种方法存在的明显问题是，它可能非常低效，特别是在高维问题上。它还有以下优点：
7. 适用于任何PDF，而不仅仅是高斯分布。
8. 可以处理任何类型的非线性函数（不要求可微，甚至连续）。
9. 不需要知道非线性函数的数学形式。在实际中，非线性函数可以是任何其他形式的。
10. 这是一个“任意时间”算法——通过调整采样点数量，我们很容易在精度和速度上进行折中。

由于该方法的精度很高，所以常用来衡量其他方法的性能。
  在线性化方法中，通过非线性函数传递高斯PDF最常见的方法就是线性化，我们已经用它来推导出了EKF和IEKF。严谨地说，均值实际上是通过非线性函数精确地传递的，而协方差则是近似地通过非线性函数的线性化版本。通常，线性化过程的工作点是PDF的均值。该过程如下图所示：

这个过程是非常不准确的，原因如下：
11. 通过非线性函数传递高斯PDF的结果不会是另一个高斯PDF。线性化方法仅仅保留了后验PDF的均值和协方差，是对后验的一种近似（丢弃了高阶矩）。
12. 通过线性化非线性函数来近似真实输出PDF的协方差。
13. 线性化的工作点通常不是先验PDF的真实均值，而是我们对输入PDF的均值的估计。这也是引入误差的地方。
14. 通过简单地将先验PDF的均值经过非线性变换来逼近真实输出PDF的均值。这并不是真实的输出均值。

线性化的另一个缺点是，我们必须解析地或者数值地计算非线性函数的雅可比矩阵（这又引入了一个误差）。但如果函数只是“轻微”的非线性，并且输入是高斯的，那么线性化方法可以说是一种简单易懂并且易于实现的方法。并且它是可逆的。
  而sigmapoint变换从某种意义上来说，当输入概率密度大致为高斯时，sigmapoint(SP)或无迹变换是蒙特卡罗方法和线性化方法的折中。它比线性化方法更准确，除了计算开销稍大一些。
  如下图展示了一维情况下最简单的版本，一般来说，任何一个SP变换的版本，都是在输入概率密度均值的基础版本上添加了一个附加样本。

  相比于线性化方法，这种方法具有许多优点：
15. 通过对输入密度进行近似，避免了线性化方法中非线性函数的雅可比矩阵（解析或数值）的计算，如下图展示了二维高斯中sigmapoint的选取。

16. 仅使用标准线性代数运算（Cholesky分解、外积、矩阵求和）
17. 计算代价和线性化方法（当使用数值方法求解雅可比矩阵时）相似。
18. 不要求非线性函数光滑和可微。

  接下来将要介绍粒子滤波，它是采用大量的样本来近似地描述PDF，它是唯一一种能够处理非高斯噪声、非线性观测模型和运动模型的实用技术。其实用之处在于它很容易实现：甚至不需要知道$f(\cdot )$和$g(\cdot )$的解析表达式，也不需要求得它们的偏导。其主要步骤如下：

  对于基础的EKF算法，可以改进的另一个思路是：在非线性的运动和观测模型中，不采用线性化的方法，而是使用sigmapoint变换来传递PDF。这就导出了sigmapoint卡尔曼滤波（SPKF），有时也称为无迹卡尔曼滤波（UKF）。它的优点是：

1. 完全不需要求导；
2. 甚至不需要运动和观测方程的解析形式，视为黑盒模型；
3. 如同IEKF之于EKF一样，SPKF也可以有迭代版本ISPKF。

  对于迭代sigmapoint卡尔曼滤波（ISPKF），在每次迭代中，我们在工作点$x_{op,k}$附近计算作为输入的sigmapoint。第一次迭代时，令$x_{op,k}={\check{x}}_k$，但在随后每次迭代中$x_{op,k}$都会被优化。使用统计的而不是解析的雅可比矩阵：

最终得到的结果如下：

最初我们讲工作点设置为先验的均值：$x_{op,k}={\check{x}}_k$。在随后的迭代中，我们将其设置为当前迭代下的最优估计：$$x_{op,k}\leftarrow {\hat{x}}_k$$当下一次迭代的增量足够小时，该过程终止。
  可以看到，IEKF的第一次迭代即对应EKF方法，SPKF和ISPKF的关系也是如此。令上式中的$x_{op,k}={\check{x}}_k$，可以得到：$${\hat{x}}_k={\check{x}}_k+K_k(y_k-u_{y,k})$$这与sigmapoint卡尔曼滤波是相同的。
  最后是对比以下在立体相机例子中，IEDKF，SPKF和ISPKF与全贝叶斯后验之间的关系，如下图所示：

  经过大量的试验，得到迭代的sigmapoint方法的解与$x_{true}$的平均误差为-3.84cm，优于MAP估计，可以合理地认为迭代的sigmapoint方法收敛于全后验概率的均值而不是模。

4.3 离散时间的批量估计问题

  在本节中，从线性高斯估计一章中的批量估计器的非线性版本着手，将状态估计问题转化为优化问题，分别从最大后验估计、贝叶斯推断、最大似然估计来进行批量估计。
  在最大后验估计中，首先是构建目标函数：$$J(x)=\frac{1}{2}e(x{)}^T{W}^{-1}e(x)$$我们的目标是最小化目标函数，得到最优参数$\hat{x}$:$$\hat{x}=arg\underset{x}{\underbrace{\min }}J(x)$$在这里我们将整个问题看成非线性优化问题。可以通过牛顿法、高斯-牛顿法以及列文伯格-马夸尔特法（LM）来进行求解这个二次型的表达式。
  采用高斯-牛顿法来进行批量式估计，从误差表达式的近似开始推导，最终求得以下方程：$$\underset{三对角块}{\underbrace{(H^T{W}^{-1}H)}}\delta x^{\ast }=H^T{W}^{-1}e(x_{op})$$这与批量式线性高斯的情况非常相似。关键区别在于，我们实际上对整条轨迹x进行迭代，从而得到最终的解。
  在贝叶斯推断中，我们也可以从贝叶斯推断的角度得到相同的更新方程。假设整条轨迹的初值为$x_{op}$。我们可以对运动模型在初值处进行线性化，并且使用所有的输入构造整条轨迹的先验。线性化的运动模型为：$$x_k\approx f(x_{op,k-1},v_k,0)+F_{k-1}(x_{k-1}-x_{op,k-1}+w_k^{{}^{\prime }})$$线性化的观测模型为：$$y_k\approx g(x_{op,k},0)+G_k(x_{k-1-x_{op,k-1}})+n_k^{{}^{\prime }}$$最终可以得到：$$\underset{三对角块}{\underbrace{(H^T{W}^{-1}H)}}\delta x^{\ast }=H^T{W}^{-1}e(x_{op})$$这与上一节的更新方程相同。接着只需要不断地迭代直到收敛即可。贝叶斯推断和MAP方法之间的区别，基本上可以归结于SMW等式从哪一边开始；另外，贝叶斯方法明确地得到了协方差，尽管在MAP方法中也可以获得协方差。
  值得注意的是，由于我们选择了迭代地对最优估计的均值进行重新线性化，从而导致了贝叶斯方法与MAP具有相同的“均值”。这个现象在之前的IEKF章节就出现了。但在批量式估计中，如果不采用线性化的方法（而是采用粒子滤波、sigmapoint变换等方法）来计算更新方程所需的矩，得到的结论是不一样的。
  在最大似然估计中，我们将研究批量式估计问题的简化版本，即不考虑先验，仅使用测量数据的情况。首先式采用高斯-牛顿解法，假设采用简单形式的观测模型——测量噪声为加数的形式（即非线性函数之外）：$$y_k=g_k(x)+n_k$$  不考虑先验时，目标函数为

其中C为常数。因为目标函数中没有了先验，而最小化目标函数的过程，也是最大化测量似然函数的过程，所以称之为最大似然估计（ML）问题：$$\hat{x}=arg\underset{x}{\min }J(x)=arg\underset{x}{\max }p(y|x)$$  然后是采用高斯-牛顿算法来求解该ML问题。
  MAP方法相对于均值误差是有偏差的，而实际上，ML方法也是有偏差的。
  如果我们把EKF看作是全非线性高斯-牛顿（或甚至是牛顿）方法的近似，那么它的表现是不尽人意的。主要原因是，EKF没有迭代至收敛的过程，其雅可比矩阵也只计算一次（可能远离最优估计）。从本质上看，EKF可以做得比单次高斯-牛顿迭代更好，因为它没有一次性计算所有得雅可比矩阵。EKF得主要缺陷在于缺乏迭代这个步骤。从优化的角度来看，这是显而易见的，因为优化是需要迭代至收敛的。然而，EKF是由贝叶斯滤波推导而来的，推导的过程中使用了马尔可夫假设来实现其递归形式。马尔可夫假设的问题在于，一旦估计器建立在该假设上，我们就无法摆脱它。这是一个不能克服的、根本性的制约因素。
  高斯-牛顿与IEKF的区别如下图所示：

  从中可以看到，高斯-牛顿的批量式估计也存在一些问题。它必须离线运行，不是一个恒定时间的算法。而EKF既可以是在线方法，也可以是恒定时间方法。所谓的滑动窗口滤波器则是在由多个时间步长组成的窗口内进行迭代，并且将这个窗口进行滑动，从而达到在线和恒定时间的实现。