非线性优化

状态估计问题

最大后验与最大似然

经典slam模型：

$\begin{cases} \boldsymbol{x}_k=f(\boldsymbol{x}_{k-1},\boldsymbol{u}_k)+\boldsymbol{w}_k\\ \boldsymbol{z}_{k,j}=h(\boldsymbol{y}_j,\boldsymbol{x}_k)+\boldsymbol{v}_{k,j}\\ \end{cases}$

对机器人状态的估计，就是求已知输入数据$ u $和观测数据$ z $的条件下，计 算状态$ x $的条件概率分布。
最小二乘法：

$J ( x ) = \sum _ { k } e _ { v,k } ^ { T } R _ { k } ^ { - 1 } e _ { v , k } + \sum _ { k } \sum _ { k } e _ { y , k , j }^T Q _ { k , j }^{-1} e _ { y , k , j }$

它的最优解等价于状态的最大似然估计。
对于一个简单的最小二乘问题：

$\min_x \frac { 1 } { 2 } || f ( x ) | | _ { 2 } ^ { 2 }$

对于$\Delta x _ { k }$d的确定可以采用下面方法：

$| | f ( x + \Delta x ) | | _ { 2 } ^ { 2 } \approx | | f(x) ||_ { 2 } ^ { 2 } + J ( x ) \Delta x + \frac { 1 } { 2 } \Delta x ^ { T } H \Delta x$

选择$\Delta x ^ { * } = - J ^ { T } ( x )$为最速下降法。

Gauss-Newton 的算法步骤可以写成：

Levenberg-Marquadt 的算法步骤可以写成：