非线性优化-SLAM十四讲笔记

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

文章目录

• 前言
• 一、pandas是什么？
• 二、使用步骤
  • 1.引入库
  • 2.读入数据
• 总结

---

非线性优化思路：设定目标函数 ------选定初值 ------寻找梯度方向------迭代更新直至收敛

---

批量状态估计

回到老生常谈的问题，我们考虑从1到N的所有时刻，并假设有M个路标点，定义机器人所有时刻的位姿和路标点坐标为

 这样一来从概率角度来看，我们对机器人的估计就是已知输入数据u，和观测数据z的条件下，求状态x,y的条件概率分布：

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

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

 有时候我们求先验也是比较困难的（可以理解为根据之前的经验，他的位姿和路边最应该在那个地方，即我们不知道机器人的位姿和路标大概在哪个地方）此时就没有了先验，那么可以求解最大似然估计：（可以理解为在什么样的状态下，最可能产生现在的观测数据）

 求解最大似然：

---

以观测模型为例：（运动模型和激光没有太大差别）

回顾观测模型，对于某一次的观测：

 由于假设噪声项，所以观测数据的条件概率为：

 这依然是一个高斯分布，考虑单次的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然，考虑任意高维高斯分布，它的概率密度函数展开形式为：

 对其取对数，则变为：

这里需要注意一下，对原函数球最大概率就相当于对对数求最小化，在最小化上式的x时，第一项与x无关，可以略去，于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计，带入SLAM的观测模型：

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

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

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

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

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

 这里我们回顾一下，首先我们通过一个

  这里举例一个例子，加深下理解：

 这里可能有人会问H矩阵是怎么来的，我认为为了方便书上应该简写了，它是将1,2,3时刻的运动和观测方程合起来写成了一个矩阵运算的形式，而H则为x的系数。

 

 梯度下降方法

(这里都是书上的介绍不懂得可以看一下，主要介绍一阶，二阶梯度法，以及高斯牛顿法求解目标函数)

我们首先考虑最简单的误差函数例子，使下式子得到最小值，我们可以求目标函数导数为0的x值。

 但是如果这个函数比较复杂不容易求导，则我们就可以使用迭代法，从一个初始值出发，不断地更新当前的优化变量，使目标函数值下降。让求解导数为零的问题变成一个不断寻找下降增量的问题。

 一阶和二阶梯度法

现在考虑第k次迭代，假设我们在 ak处，想要寻到增量△ax，那么最直观的方式是将目标函数在ax附近进行泰勒展开:（这里有关概念问题直接看书吧）

 高斯牛顿法

他的思想是将f（x）进行一阶泰勒展开，注意这里不是F（x）:

 ​​​​