批量（batch）状态估计问题

我们已经探讨了观测模型

X为旋转+平移，h为相机观测模型

• ，但可以求解
  
  

eg.从最大似然到最小二乘

• 直观的解释

  • 由于噪声的存在，当我们把估计的轨迹与地图代入SLAM的运动、观测方程时，他们并不会完美的成立
  • 此时就调整状态的估计，使得误差最小化

• 该问题有何结构

  • 由许多个误差的平方和（Sigma范数和组成）
  • 虽然总体维度高，但每个项很简单，只关联2个变量
  • 如果用李代数表达位姿，那么是无约束优化问题

• 如何求解

  • 介绍通用的非线性最小二乘问题

非线性最小二乘

先考虑简单的问题：这里,f为任意函数

• 当f很简单时：

• 解： 将得到极值点或者鞍点，比较这些点即可。

• 当f复杂时：

• 难求， 很难解

• 使用迭代方式求解

如何使用迭代的方式：

1. 给定某个初始值
2. 对于第k次迭代，寻找一个增量，使得达到最小值
3. 足够小，则停止
4. 否则，令,返回2

如何确定增量？
确定增量的方法（即梯度下降策略）：一阶或者二阶的泰勒展开

1.png

1.png

最速下降法和牛顿法虽然直观，但实用当中存在一些缺点

• 最速下降法会碰到zigzag问题（过于贪婪）
• 牛顿法迭代次数少，但需要计算复杂的Hessian矩阵

能否回避Hessian的计算？

• Gauss-Newton

• Levenberg-Marquadt

  
  
  

G-N用J的表达式近似了H

步骤：

1. 给定初始值
2. 对于第k次迭代，求出当前的雅克比矩阵和误差
3. 求解增量方程：
4. 若足够小，则停止。否则，令返回2
   
   

改进版的G-N

LM相比于GN，能够保证增量方程的正定性

• 即，认为近似只在一定范围内成立，如果近似不好则缩小范围
• 从增量方程来看，可以看成一阶和二阶的混合
• 参数控制着两边的权重

小结

• 非线性优化是个很大的主体，研究者们为之奋斗多年
• 主要方法：最速下降，牛顿，G-N，L-M，DogLeg
• 与线性规划不同，非线性需要针对具体问题具体分析
• 问题非凸时，对非凸敏感，会陷入局部最优
  • 目前没有非凸问题的通用最优值的寻找方法
  • 问题凸时，二阶方法通常一两步就能收敛