【视觉SLAM十四讲学习笔记】第六讲——状态估计问题

专栏系列文章如下：
【视觉SLAM十四讲学习笔记】第一讲——SLAM介绍
【视觉SLAM十四讲学习笔记】第二讲——初识SLAM
【视觉SLAM十四讲学习笔记】第三讲——旋转矩阵
【视觉SLAM十四讲学习笔记】第三讲——旋转向量和欧拉角
【视觉SLAM十四讲学习笔记】第三讲——四元数
【视觉SLAM十四讲学习笔记】第三讲——Eigen库
【视觉SLAM十四讲学习笔记】第四讲——李群与李代数基础
【视觉SLAM十四讲学习笔记】第四讲——指数映射
【视觉SLAM十四讲学习笔记】第四讲——李代数求导与扰动模型
【视觉SLAM十四讲学习笔记】第五讲——相机模型

我们已经知道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出，其中内参随相机固定，外参则为相机位姿。于是，我们已经弄清了经典SLAM模型在视觉情况下的表达。
然而由于噪声的存在，运动方程和观测方程的等式必定是不是精确成立的。尽管相机可以非常好地符合针孔模型，但我们得到的数据通常是受各种未知噪声影响的。
所以，预期假设数据必须符合方程，不如讨论如何在有噪声的数据中进行准确的状态估计。解决状态估计问题需要一定程度的最优化背景知识。

状态估计问题

批量状态估计与最大后验估计

经典的SLAM模型由一个运动方程和一个观测方程构成：

x_k是相机的位姿，可以用T_k∈SE(3)表达。运动方程与输入的具体形式有关。观测方程由真空相机模型给定。假设在x_k处对路标y_j进行了一次观测，对应到图像上的像素位置z_k,j，那么观测方程可以表示成：

其中K为相机内参，s为像素点的距离，也是(R_k y_j+t_k)的第三个分量。如果使用变换矩阵T_k描述位姿，那么路标点y_j必须以齐次坐标来描述，计算完成后要转换为非齐次坐标。

现在，考虑数据受噪声影响后会发生什么改变。在运动和观测方程中，我们通常假设两个噪声项w_k，v_k,j满足零均值的高斯分布：

其中N表示高斯分布，0表示零均值，R_k，Q_k,j为协方差矩阵。在这些噪声的影响下，我们希望通过带噪声的数据z和u推断位姿x和地图y（以及它们的概率分布），这构成了一个状态估计问题。

处理这个状态估计问题的方法大致分为两种。

1. 增量/渐进（incremental），又称滤波器。由于在SLAM过程中，数据随时间逐渐到来，所以我们应该持有一个当前时刻的估计状态，然后用新的数据来更新它。增量方法仅关心当前时刻的状态估计x_k，而对之前的状态则不多考虑。
2. 批量（batch），把数据攒起来一并处理。相对增量方法，批量方法可以在更大范围达到最优化，被认为优于传统的滤波器，而成为当前视觉SLAM的主流方法。

由于讨论的是批量方法，考虑从1到N的所有时刻，并假设有M个路标点。定义所有时刻的机器人位姿和路标点坐标为

同样地，用不带下标的u表示所有时刻的输入，z表示所有时刻的观测数据。那么，对机器人状态的估计，从概率学的观点来看，就是已知输入数据u和观测数据z的条件下，求状态x，y的条件概率分布：

特别地，当我们不知道控制输入，只有一张张图像时，即只考虑观测方程带来的数据时，相当于估计P(x,y|z)的条件概率分布，此问题也称为SfM，即如何从许多图像中重建三维空间结构。

为了估计状态变量的条件分布，利用贝叶斯法则，有

贝叶斯法则左侧称为后验概率，右侧的P(z|x)称为似然（Likehood），另一部分P(x)称为先验（Prior）。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化，则是可行的：

贝叶斯法则的分母部分与待估计的状态x，y无关，因而可以忽略。贝叶斯法则告诉我们，求解最大后验概率等价于最大化似然和先验的乘积。

但是，我们不知道机器人位姿或路标大概在什么地方，此时就没有了先验。那么，可以求解最大似然估计（Maximize Likelihood Estimation, MLE）：

直观地讲，似然是指“在现在的位姿下，可能产生怎样的观测数据”。由于我们知道观测数据，所以最大似然估计可以理解成：“在什么样的状态下，最可能产生现在观测到的数据”。这就是最大似然估计的直观意义。

最小二乘的引出

如何求最大似然估计呢？在高斯分布的假设下，最大似然能够有较简单的形式。回顾观测模型，对于某一次观测：

由于我们假设了噪声项

所以观测数据的条件概率为：

它依然是一个高斯分布。考虑单词观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然。

我们知道高斯分布在负对数下有较好的数学形式。考虑任意高位高斯分布x~N(μ, Σ)，它的概率密度函数展开形式为

对其去负对数，则变为

因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。在最小化上式的x时，第一项与x无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。代入SLAM的观测模型，相当于在求：

我们发现，该式等价于最小化噪声项的一个二次型，称为马哈拉诺比斯距离，又叫马氏距离。它也可以看成由Q_k,j^-1 加权之后的欧氏距离（二范数），这里Q_k,j^-1 也叫做信息矩阵，即高斯分布协方差矩阵之逆。

现在我们考虑批量时刻的数据。通常假设各个时刻的输入和观测是相对独立的，这意味着各个输入之间是独立的，各个观测之间是独立的，并且输入和观测也是独立的。于是我们可以对联合分布进行因式分解：

这说明我们可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差：

那么，最小化所有时刻估计值与真实读数之间的马氏距离，等价于求最大似然估计。负对数允许我们把乘积变成求和：

这样就得到了一个最小二乘问题（Least Square Problem），它的解等价于状态的最大似然估计。直观上看，由于噪声的存在，当我们把估计的轨迹与地图代入SLAM的运动、观测方程中时，它们并不会完美地成立。对于这种情况，我们对状态的估计值进行微调，使得整体的误差下降一些。当然，这个下降也有限度，它一般会到达一个极小值。这就是一个典型的非线性优化的过程。

仔细观察上式，我们发现SLAM中的最小二乘问题具有一些特定的结构：

• 首先，整个问题的目标函数由许多个误差的（加权的）二次型组成。虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关。例如，运动误差只与x_k-1，x_k有关，观测误差只与x_k，y_j有关。这种关系会让整个问题有一种稀疏的形式。
• 其次，如果使用李代数表示增量，则该问题是无约束的最小二乘问题。如果用旋转矩阵/变换矩阵描述位姿，则会引入旋转矩阵自身的约束，即需在问题中加入s.t.R^T R=I且det(R)=1这样令人头大的条件。额外的约束会使优化变得更加困难。这体现了李代数的优势。
• 最后，我们使用了二次型度量误差。误差的分布将影响此项在整个问题中的权重。例如，某次的观测非常准确，那么协方差矩阵就会“小”，而信息矩阵就会“大”，所以这个误差会在整个问题中占有较高的权重。

批量状态估计

现在介绍如何求解这个最小二乘问题，这需要一些非线性优化的基本知识。

考虑一个非常简单的离散时间系统：

这可以表达一辆沿x轴前进或后退的汽车。第一个公式为运动方程，u_k为输入，w_k为噪声；第二个公式为观测方程，z_k为对汽车位置的测量。取时间k=1,…,3，现希望根据已有的v，y进行状态估计。

设初始状态x_0已知，下面来推导批量状态的最大似然估计。

首先，令批量状态变量为x=[x_0,x_1,x_2,x_3]^T ，令批量观测为z=[z_1,z_2,z_3]^T ，按同样方式定义u=[u_1,u_2,u_3]^T 。按照先前的推导，我们知道最大似然估计为

对于具体的每一项，比如运动方程，我们知道：

观测方程：

根据这些方法，我们就能够实际地解决上面的批量状态估计问题。根据之前的叙述，可以构建误差变量：

于是最小二乘的目标函数为

此外，这个系统是线性系统，我们可以很容易地将它写成向量形式。定义向量y=[u,z]^T ，那么可以写出矩阵H，使得

那么：

且

整个问题可以写成

之后我们将看到，这个问题有唯一解：