SLAM学习笔记（五）——后端优化（一）

SLAM学习笔记（五）——后端优化（一）

先说明一下，书里面第八讲视觉里程计二，我写的文档丢失了，暂时先不去管他了，先继续往后看吧，之前是准备先做一个VO框架的，但是仔细思考了一下，准备先来实现一下这边，然后再去做那个VO框架。

我个人认为这里在我未来的项目中，不会是我做的重点，所以我在这块不做太过详细的学习。

EKF后端

我们知道，实际上，仅有前端的SLAM就可以支持简单的定位、建图的工作了，但是前端的结果往往是粗糙的，后端的存在就是为了继续优化前端的结果。它的本质是一种状态估计问题，主流的后端可以分为以下两类：

• 渐进式： 保持当前状态估计， 加入新信息时，更新已有的估计
• 批量式： 给定一定规模的数据， 计算该数据下的最优估计

接下来回到我们看了无数遍的方程：

这里的x是机器人的位姿，z是观测数据，w与v是噪声，我们如果做一个不太严谨的定义：

这里的$x_k$代表k时刻所有的未知量，那么我们就可以重写之前的方程：

那么这个时候，我们就可以用数学语言来表示我们的问题了：

我们要做的事情实质上是：已知初始时刻的状态，以及中间过程的观测数据与误差（这里的误差或者说是噪声是可以人为假设出来的，比如高斯）：

这里就是把原来的拆成了一个似然和一个先验的乘积，这个先验非常眼熟，上面的方程中$z_k$的计算方法蕴含了其计算形式，后面的先验可以继续展开：

这个可以拆成两部分：

• k时刻是如何受到之前状态的影响的
• k-1时刻的状态估计

在这里时产生了一定的分歧：

• 可以假设k时刻的状态只和k-1时刻有关
• 假设k时刻与之前的所有状态有关

如果按照第一种方法来，那么第一项可以被写为：

第二项可以被写为：

这里的第一项表示：已知上一时刻的状态时与当前误差时，产生当前情况的概率，也就是我们所说的当前状态只与上一时刻状态相关。

这里的第二项与之前的差别在于将k时刻的误差去除了，因为想要预估k-1时刻的状态，无需知道所有时刻的误差，只需要知道到k-1时刻即可。

在开始下一步之前，我们先来明确一个问题，啥是卡尔曼滤波？

卡尔曼滤波（KF）

假设我们的运动方程和观测方程可以用线性方程来描述：

这里的$w_k,v_k$代表的是噪声，同时我们也假设这两个噪声服从高斯分布：

首先我们通过运动方程来确定$x_k$的先验分布，后面的高斯是通过上面的线性方程得到的：

这个式子显示了如何通过上一时刻的变换来计算下一时刻的状态分布。这个分布也就是先验。

先验和后验（补充一下）

• 可以说先验就是不基于经验的
• 与之相对的是后验是基于经验的，后验需考虑当前的情况来对估计进行调整（贝叶斯）

那么我们这里的先验就记作：

这里我们找到了每个时刻的先验的状态量与协方差与上一时刻的状态与协方差的关系，这个过程实质上就是直接将上面高斯的均值与方差拿出来了。

接下来，我们可以计算在某个状态下会产生怎么样的观测数据（这里是通过上面线性方程的第二个式子得来的）

这个时候我们回看一下上一小节中我们想要求解的公式：

我们发现，我们已经将两项的分布都求出来了，接下来我们对他们两个相乘就可以求出我们目标的概率，但是在此之前，我们需要假设我们乘出来的结果也是一个高斯分布，那么我们就得到了下面的结果：

等式后面的第二项的均值与方差来自于先验，而第一项来自于我们刚才的推导，我们这里先来复习一下高斯（这里开始参考这篇博客）：
$$f(x)=\frac{1}{\sqrt{2\pi }\sigma }{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$$
上面我们看到的是一个普通一元的高斯，其中$\mu$是均值，$\sigma$是方差。显然，我们的变量（状态变量）是非常多的变量构成的，也就是说这里的高斯不再是简单的一元高斯，而是多元高斯，那么多元高斯是怎么写的呢？我们假设所有的变量互不相关，就可以有下面的关系：
$$f(x)=p(x_1,x_2....x_n)=p(x_1)p(x_2)....p(x_n)=\frac{1}{(\sqrt{2\pi }{)}^n{\sigma }_1{\sigma }_2\cdots {\sigma }_n}{e}^{-\frac{(x_1-{\mu }_1{)}^2}{2{\sigma }_1^2}-\frac{(x_2-{\mu }_2{)}^2}{2{\sigma }_2^2}\cdots -\frac{(x_n-{\mu }_n{)}^2}{2{\sigma }_n^2}}$$
假如我们有：$z^2=\frac{(x_1-{\mu }_1{)}^2}{{\sigma }_1^2}+\frac{(x_2-{\mu }_2{)}^2}{{\sigma }_2^2}\cdots +\frac{(x_n-{\mu }_n{)}^2}{{\sigma }_n^2}$，那么我们能够把原来的式子简写成：
$$f(z)=\frac{1}{(\sqrt{2\pi }{)}^n{\sigma }_z}{e}^{-\frac{z^2}{2}}$$
我们可以将$z^2$展开为矩阵的形式：
$$z^2=z^{\mathrm{T}}z=\left[\begin{array}{c}{x}_1-{\mu }_1,x_2-{\mu }_2,\cdots ,x_n-{\mu }_n\end{array}\right]\left[\begin{array}{cccc}\frac{1}{{\sigma }_1^2}& 0& \cdots & 0\\ 0& \frac{1}{{\sigma }_2^2}& \cdots & 0\\ ⋮& \cdots & \cdots & ⋮\\ 0& 0& \cdots & \frac{1}{{\sigma }_n^2}\end{array}\right]{\left[\begin{array}{c}{x}_1-{\mu }_1,x_2-{\mu }_2,\cdots ,x_n-{\mu }_n\end{array}\right]}^{\mathrm{T}}$$
我们可以把左边的矩阵转换成两个矩阵差的形式，即：$x-{\mu }_x={\left[\begin{array}{c}{x}_1-{\mu }_1,x_2-{\mu }_2,\cdots ,x_n-{\mu }_n\end{array}\right]}^{\mathrm{T}}$

再把中间的协方差矩阵做一个符号的约定：
$$\sum =\left[\begin{array}{cccc}{\sigma }_1^2& 0& \cdots & 0\\ 0& {\sigma }_2^2& \cdots & 0\\ ⋮& \cdots & \cdots & ⋮\\ 0& 0& \cdots & {\sigma }_n^2\end{array}\right]$$
因为他是一个对角矩阵，所以他的逆就是全都取倒数就可以了：
$$((\sum {)}^{-1}=\left[\begin{array}{cccc}\frac{1}{{\sigma }_1^2}& 0& \cdots & 0\\ 0& \frac{1}{{\sigma }_2^2}& \cdots & 0\\ ⋮& \cdots & \cdots & ⋮\\ 0& 0& \cdots & \frac{1}{{\sigma }_n^2}\end{array}\right]$$
当然了，我们在分母项里面需要做一个累乘，所以我们还是做下面的约定：
$${\sigma }_z={\left|\sum \right|}^{\frac{1}{2}}={\sigma }_1{\sigma }_2.....{\sigma }_n$$
最后，我们把说的这些东西综合起来，$z^2$就可以写成下面的形式：
$$z^{\mathrm{T}}z=(x-{\mu }_x{)}^{\mathrm{T}}\sum {}^{-1}(x-{\mu }_x)$$
最后，我们来看部分展开的多元高斯：
$$f(z)=\frac{1}{(\sqrt{2\pi }{)}^n{\sigma }_z}{e}^{-\frac{z^2}{2}}=\frac{1}{(\sqrt{2\pi }{)}^n{\left|{\sum }_{}^{}\right|}^{\frac{1}{2}}}{e}^{-\frac{(x-{\mu }_x{)}^{\mathrm{T}}({\sum }_{}{)}^{-1}(x-{\mu }_x)}{2}}$$
由于这里的两侧都是高斯，所以我们就只考虑指数部分就可以了，我们就有：

可以将两侧展开，分别比较x的二次和一次系数，对于二次系数，有：

这里的二次项就是展开后含有$x_k^2$的项，这里为了简化计算，我们定义一个中间变量：

接下来，我们将上面式子的左边通过乘上他的逆的形式转换为一个单位矩阵，就可以得到下面的式子：

这里转换的过程中，同时左乘了一个$P_k$帽，然后又使用刚才定义的$K$，简化了这个式子。然后通过简单变换，可以写出：

这里是二次项，接下来我们来比较一次项：

经过一波化简，（先取转置，然后再消元）就可以得到下面的形式：

最后，两侧同时乘以$P_k$帽，并且带入上面的中间变量$K$与$P_k$帽的表达就可以得到下面的式子：

于是呢，我们又找到了$x_k$帽与其他之间的关系，同时我们根据二次项也可以知道$P_k$帽与其他之间的关系，于是我们就可以根据之前的信息对当前的信息进行更新，这一步我们叫做预测：

那么预测完成后，我们要计算非常重要的K：

最终，计算后验分布的概率：

这就是卡尔曼滤波的全过程，说实话我感觉我顺一遍还是有点小问题，难顶。

回到EKF后端

我们上面说的是KF，也就是标准的卡尔曼滤波，他只能运用到线性系统中，那么我们需要用EKF来做实际应用，接下来我们看具体是怎么做的：

首先，我们用泰勒做一个一阶展开：

我们将这里的偏导数记作：

对于观测方程也是如此：

一样的道理，我们用符号来表达一下：

这里与上面的卡尔曼滤波的过程相似，最终得到的结果是：

我们目前对卡尔曼滤波仅作简单掌握吧，我们可以看到，卡尔曼滤波存在一个较大的局限：我们展开的过程是做的一阶展开，实质上是在用线性函数来近似非线性函数，这样的做法有非常大的局限性，并且不够精准，接下来我们来看下一个方法——BA与图优化。

BA与图优化

在看这个之前，我们需要复习一下我们的相机模型

• 首先从世界坐标到相机坐标：
• 从相机坐标到归一化坐标：
• 进行畸变处理：
• 根据内参模型，来计算图像坐标：

我们将上面四个步骤使用一个函数来概括：
$$z=h(x,y)$$
我们可以用流程图来概括$h(x,y)$的作用：

这里的x指代当前相机的位姿，也就是外参$R,t$，它对应的李代数是$\xi$。路标y是这里的三维点，而观测数据是：$z^{\Delta }={\left[u_s,v_s\right]}^T$，我们可以构建残差：
$$e=z-h(x,y)$$
他的实质含义与我们以前经常说的一样：实际观测到的数据，与我们预测出的数据的差异。接下来我们将总体（也就是不同时刻）的残差一并考虑进来，即可构建代价函数：

我们对这个目标方程来做一个优化就是传说中的BA了。

BA求解

我们可以判断，上面的目标函数并不是一个线性函数，所以我们需要使用一些非线性优化的方法来优化他，具体来说就是通过从某一个初值点开始，不断地寻找下降方向$\Delta x$来找到目标函数的最优解。对于整体的目标函数，我们必须把自变量定义成所有待优化的变量：
$$x={\left[{\xi }_1,...,{\xi }_m,p_1,...,p_n\right]}^T$$
相应的，当我们对方程中的x增加一定的增量时，其误差变化如下：

我们可以将原来的x拆分成两个部分：
$$x_{\xi }=\left[{\xi }_1,{\xi }_2,...,{\xi }_m\right],x_p=[p_1,...,p_n]$$
于是这个式子就可以化简成这个形式：

我们面对的是这样的一个增量线性方程：
$$H\Delta x=g$$
其中，由于我们吧变量分为了位姿和空间点两种，所以雅可比矩阵也可以分块：
$$J=\left[\begin{array}{cc}F& E\end{array}\right]$$
以高斯牛顿为例:
$$H=J^{T}J$$
这样就可以进行求解了，后面好像还讲了一些其他的，关于这个矩阵H的逆的求法的问题，但是由于篇幅太长，而且和我做的事情的关系不是很大的原因，我决定先跳过这一部分，日后再看。

小结

这节主要讲了两种后端优化方法：

• 扩展卡尔曼滤波：实质上还是线性的卡尔曼滤波，只不过是用了一阶展开来做了线性近似，有一定的局限性，但是效果不错（据说）
• BA：使用最优化方法来解的全局能量函数最小化问题，比较灵活，看作者的意思是，比较推荐用BA