批量（batch）状态估计问题

我们已经探讨了观测模型

\begin{cases} x_k = f(x_{k-1},u_k,w_k) \\ z_{k,j} = h(y_j,x_k,v_{k,j}) \end{cases}

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

P(X|Z)条件分布很难求解

，但可以求解

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

• 直观的解释
  • 由于噪声的存在，当我们把估计的轨迹与地图代入SLAM的运动、观测方程时，他们并不会完美的成立
  • 此时就调整状态的估计，使得误差最小化
• 该问题有何结构
  • 由许多个误差的平方和（Sigma范数和组成）
  • 虽然总体维度高，但每个项很简单，只关联2个变量
  • 如果用李代数表达位姿，那么是无约束优化问题
• 如何求解
  • 介绍通用的非线性最小二乘问题

非线性最小二乘

先考虑简单的问题：

minJ(x)=min \frac{1}{2} \begin{Vmatrix}f(x)\end{Vmatrix}_2^2

这里

x \in R^n

,f为任意函数

• 当f很简单时：
• 解：

\frac{dJ}{dx} =0

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

• 当f复杂时：

\frac{dJ}{dx}

难求，

\frac{dJ}{dx}=0

很难解

• 使用迭代方式求解

如何使用迭代的方式：

1. 给定某个初始值

x_0

1. 对于第k次迭代，寻找一个增量

\Delta x_k

，使得

\begin{Vmatrix} f(x_k+\Delta x_k)\end{Vmatrix}_2^2

达到最小值

\Delta x_k

足够小，则停止

1. 否则，令

x_{k+1} = x_k + \Delta x_k

,返回2

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

1.png

1.png

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

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

能否回避Hessian的计算？

• Gauss-Newton
• Levenberg-Marquadt

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

步骤：

1. 给定初始值

x_0

1. 对于第k次迭代，求出当前的雅克比矩阵

J(x_k)

和误差

f(x_k)

1. 求解增量方程：

H\Delta x_k = g

1. 若

\Delta x_k

足够小，则停止。否则，令

x_{k+1}=x_k + \Delta x_k

返回2

改进版的G-N

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

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

\lambda

控制着两边的权重

小结

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