第六讲学习笔记如下：
视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合_goldqiu的博客-CSDN博客
前五讲学习笔记总结如下：
视觉SLAM十四讲学习笔记---前三讲学习笔记总结之SLAM的作用、变换和位姿表示_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第四讲---第五讲学习笔记总结---李群和李代数、相机_goldqiu的博客-CSDN博客
第六讲
总结：
经典SLAM模型的位姿可以由变换矩阵来描述，然后用李代数进行优化。由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的。得到的数据通常是受各种未知噪声影响的。如何在有噪声的数据中进行准确的状态估计，这需要非线性优化的相关知识。
第一步：建模
经典SLAM模型由一个运动方程和一个观测方程构成：
Xk是机器人的位姿变量，可以由
表达，yj为路标点，uk为系统输入。运动方程与输入的具体形式有关，这里假设没有特殊性，和普通的机器人、无人车的情况一样；观测方程由传感器模型给定。在运动和观测方程中，通常假设两个噪声项满足零均值的高斯分布，Rk、Qk,j为协方差矩阵。在这些噪声的影响下，希望通过带噪声的数据z和u推断位姿x和地图y以及它们的概率分布，这构成了一个状态估计问题。
定义所有时刻的机器人位姿和路标点坐标：用不带下标的u表示所有时刻的输入，z表示所有时刻的观测数据。从概率学的观点来看，对机器人状态的估计就是已知输入数据u和观测数据z的条件下，求状态变量x、y的条件概率分布：
为了估计状态变量的条件分布，利用贝叶斯法则，有：
贝叶斯法则左侧P(x ,y | z ,u)称为后验概率，右侧的P(z ,u | x ,y)称为似然概率，另一部分P(x ,y)称为先验概率。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化（Maximize a Posterior，MAP），则是可行的：
贝叶斯法则说明，求解最大后验概率等价于最大化似然概率和先验概率的乘积。下面用贝叶斯网络来描述SLAM的概率建模问题，这里不考虑输入u，公式则为：
在贝叶斯网络中定义了两类四种变量，这些变量通过有向的边来连接。如图1中，蓝色是路标节点变量，也就是y；黄色是机器人的状态变量，也就是x；它们都属于状态变量。而红色是机器人对路标点的观测变量，绿色是机器人对自身运动的观测变量，它们都属于观测变量，也就是z。这个贝叶斯网络实际上描述了状态变量和观测变量的联合概率模型。
先验概率P(x ,y)是与似然概率P(z ,u | x ,y)相互独立的，所以MAP公式如下：
所有的似然概率是以乘积的形式乘积在一起，且可分解；表示在因子图里面，似然概率分解的每一个项就是一个因子，乘积在一起用图的形式来描述就是因子图：
因子图里面包括两类节点：状态变量节点和观测量节点（因子）。此外因子图里还包括一个先验因子，代表了公式中的先验概率，用来固定整个系统的解。整个因子图实际上就是每个因子的乘积。如图2红色方框对应观测量因子；绿色方框对应状态量之间测量值的因子，比如里程计；紫色方框对应先验因子。求解因子图就是将这些因子乘积起来后求一个最大值，得到的系统状态就是在概率上最大可能性的系统状态，公式如下：
对于因子图模型的相关原理见之前写的因子图专栏：
因子图优化专栏汇总_goldqiu的博客-CSDN博客
第二步：概率最大化问题转化为非线性最小二乘问题：
因为噪声项满足：
所以观测数据的条件概率为：
它依然是一个高斯分布。考虑单次观测的最大似然估计，可以使用最小化的负对数来求一个高斯分布的最大似然概率。高斯分布在负对数下有较好的数学形式。考虑任意高维高斯分布
它的概率密度函数展开形式为：
对其取负对数，则变为：
因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。在最小化上式的x时，第一项与x无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。代入SLAM的观测模型，相当于在求：
该式等价于最小化噪声项（即误差）的一个二次型。这个二次型称为马哈拉诺比斯距离,又叫马氏距离。
现在考虑批量时刻的数据。通常假设各个时刻的输入和观测是相互独立的，这意味着各个输入之间是独立的，各个观测之间是独立的，并且输入和观测也是独立的。于是可以对联合概率分布进行因式分解：
这说明可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差：
那么，最小化所有时刻估计值与真实值之间的马氏距离，等价于求最大似然估计。负对数允许把乘积变成求和：
这样就得到了一个非线性最小二乘问题，它的解等价于状态的最大似然估计，在因子图中就是所有因子的乘积最大化。直观上看，由于噪声的存在，当我们把估计出的轨迹与地图代入SLAM的运动、观测方程中时，它们并不会完美地成立。这时对状态的估计值进行微调，使得整体的误差下降一些，一般会到达一个极小值。这就是一个典型非线性优化的过程。
第三步：求解非线性最小二乘问题
求解非线性最小二乘优化问题的方法很多，而高斯牛顿法是其中最简单和最常用的方法。由前面可以假设因子的函数形式是：
用不带下标的f(x)表示所有时刻因子的误差函数，通常形式是：f(x) = x - z。而对于负指数函数，每一个因子乘积最大化代表函数里面的f(x)最小化。这正好符合希望系统误差函数的误差值最小，即系统状态值与观测值越接近，则误差函数值越小，那么就越大，系统的状态变量和观测量接近的概率就越大。首先是对f(x)进行一阶泰勒展开：
这里 为f(x)关于x的导数，为n×1的列向量。目标是寻找增量，使得
达到最小。为了求，需要解一个线性的最小二乘问题：
根据极值条件，将上述目标函数对求导，并令导数为零。可以得到如下方程组：
这个方程是关于变量的线性方程组，称它为增量方程，也可以称为高斯牛顿方程。把左边的系数定义为H(x)，右边定义为g(x)，那么上式变为：
高斯牛顿法用
实现了对二阶Hessian矩阵的近似，从而省略了计算Hessian矩阵的过程。求解增量方程是整个非线性优化问题的核心所在。高斯牛顿法的算法步骤可以写成：
1、给定初始值x0。
2、对于第k次迭代，求出当前的雅可比矩阵
和误差函数
3、求解增量方程：
4、若
足够小，则停止。否则，令
返回第2步。