高翔老师SLAM14讲学习笔记
后端
- 后端（Backend）(状态估计问题)
- 从带噪声的数据估计内在状态——状态估计问题
- Estimated the inner state from noisy data
- 渐进式（Incremental）
- 保持当前状态的估计，在加入新信息时，更新已有的估计（滤波）
- 批量式（Batch）
- 给定一定规模的数据，计算该数据下的最优估计（优化）
1 状态估计问题
- 给定一定规模的数据，计算该数据下的最优估计（优化）
- 数学描述
- 符号定义：
- t = 0 … N 时间内，机器人的位姿为：
（相当于一个R、t），同时有路标
（三D点） - 运动和观测方程记为：
f：非线性函数，Uk：运动输入量，wk：噪声量；
第二个公式呢：在k时刻对yj这个路标做一次观测，拿到了zk,j(2d点
u、v)，（对3D观测得到一个2D像素，就是这样一件事情），h也是一个非线性函数。
如果是纯视觉SLAM，没有对相机运行估计，没有相机运动的输入量、观测量，那只有下面这个式子（观测方程）。
整个过程可以理解为，当你拿有噪声的数据时，任意时候只拿到了z 和
u，通过z和u去估计x0-xn和y0-yn服从怎样的一种分布。一般是拿到x、y的联合分布。
- 用随机变量表示状态，估计其概率分布
- k 时刻所有待估计的量组成k时刻状态：
符号稍微有点冲突 - k 时刻观测统一记成：
方程简化为：
- 根据过去0时刻到k时刻的数据
- 估计当前的状态：
- Bayes展开：
（提取zk） - 先验部分按条件概率展开：
k时刻受先前状态的影响 k-1时刻的状态估计
- 分歧：
- 可以假设k时刻状态只和k-1时刻有关（假设了一阶马尔可夫性）
- 或假设k时刻状态与先前所有时刻均有关（不假设马尔可夫性）
假设一阶马尔可夫性的情况：
（运行方程）
第二项：
状态观测方程。
- 于是，该公式指出了如何从k-1时刻的状态分布递推至k时刻的分布
- 只是现在我们还没有代入具体的分布形式
在线性模型、高斯状态分布下，我们将得到卡尔曼滤波器
在非线性模型、高斯状态分布下，可以在工作点附近线性展开，得到扩展卡尔曼滤波器
2 卡尔曼滤波器（KF）
目的：求
- 只是现在我们还没有代入具体的分布形式
在线性模型、高斯状态分布下，我们将得到卡尔曼滤波器
在非线性模型、高斯状态分布下，可以在工作点附近线性展开，得到扩展卡尔曼滤波器
- 卡尔曼滤波器的推导
- 线性模型和高斯噪声：
（高斯白噪声）
状态的高斯分布（区别先后验）
后验（仅通过运行方程推的话称为后验，上尖尖表示，假设k-1时刻是知道了，它满足一个高斯分布，均值是
，协方差是
）
先验（上杠杠）。 忽略计算，得
卡尔曼滤波的更新式：
总结： 经典卡尔曼滤波器的五个公式，给出了线性高斯系统的最优无偏估计
10.24：使用运行方程，k-1的后验推k时刻的先验。
10.25-10.26：k时刻的先验推k时刻的后验。
3 扩展卡尔曼滤波器（EKF）
-假设在工作点附近是一个线性系统，同样使用卡尔曼推导有：
-
- 线性模型和高斯噪声：
- EKF优点
- 推导简单清楚，适用各种传感器形式
- 易于做多传感器融合
- EKF缺点
- 一阶马尔可夫性过于简单
- 可能会发散（要求数据不能有outlier，如 特征匹配错误）
- 线性化误差（仅有一次迭代的线性优化问题）
- 需要存储所有状态量的均值和方差，平方增长
4 BA与图优化
目的：BA与图优化结合，更好更快的优化BA。
- Bundle Adjustment已经在之前介绍过了
- 事实上BA属于批量式的优化方法
- 给定很多个相机位姿与观测数据，计算最优的状态估计
- 定义每个运动/观测方程的误差，并从初始估计开始寻找梯度下降
- BA问题与图结构的关系
- BA虽然是个纯优化问题，但亦可以用图模型清晰地表述出来
- 顶点为优化变量，边为运动/观测约束
- 本身还有一些特殊的结构
- 考虑在位姿 i 处对路标 j 的一次观测 zij：
特点：
- 每个观测只关系两个变量，其中一个是相机，一个是路标
- 纯视觉Ba中，不存在相机与相机/路标与路标之间的关联
- 整个误差函数由许多个这样小的项组成
如果对变量做好排序，例如所有相机位姿在前，路标在后，那么 H 有一定的特殊结构：
图模型与H矩阵存在对应关系：图模型中存在边=>H相应地方出现非零块。 - 实际当中的H，路标数量远大于位姿数量（箭头形矩阵或镐子形矩阵）
- 利用H的特殊结构，可以大幅加速Hx=-b线性方程的求解
- 加速手段又称边缘化（Marginalization）[意义有很多种]
- Hx=-b的结构（以下目的为了消元）：
B：小，对角块；C，大，对角块
E和E转置：与图模型对应，可稠密 - 因为 C 是对角块，所以可以用C对E进行消元（高斯消元），得到：
从而：
- 该方程组分为两步来求：
- 求解上半部分，规模较小，得到
- 将结果代入下半部分，得到
- 求解上半部分，规模较小，得到
- 这个做法称为Marginalization或Schur消元
- 从消元角度来讲，亦可使用Cholesky等其他消元方式解此稀疏方程
- 从Marginalization角度来讲，是我们把所有的路标信息边缘化到了相机的信息中（这样说
边缘化就是“消元”）
