第六讲 非线性优化

状态估计问题

SLAM基本方程（运动方程和观测方程）：

概率学模型来表示SLAM问题：P(x,y|z,u)，即已知所有时刻的输入u和观测z，求解x，y的条件概率分布。

P(x,y|z,u)称为后验，使用贝叶斯法则可得

最大后验估计：

最大似然估计：

把状态最大似然估计转化为最小二乘问题

非线性最小二乘

对F(x)求导求极值点，但是这需要知道F(x)的全局性质，使用迭代的方法，问题的核心转化为如何寻找能让F(x)的值不断下降的△ xk

一阶和二阶梯度法

这两种方法并不实用，因为：最速下降法过于贪心，反而容易增加迭代次数，使收敛变慢。牛顿法需要求解海塞矩阵H，但这十分困难。

高斯牛顿法

对f(x)Taylor展开，而不是F(x)。对f(x)一阶Taylor展开，然后再取范数取平方，我们就得到了与F(x)的二阶Taylor近似的式子

求关于△ x的导数，令导数为0

H(x)不是真正的海塞矩阵，而是f(x)的雅克比矩阵J近似出来的海塞矩阵。

高斯牛顿具体步骤：

对于第k次迭代，求出当前的雅可比矩阵J（xk）和误差f（xk）。求解增量方程：H△xk=g.其他步骤与普通迭代法相同。

高斯牛顿法的缺点：需要求解 H−1 ，但是H矩阵可能不可逆，也可能是病态矩阵，导致算法稳定性差。即使可逆也不病态，如果求出来的△x太大，也会使得算法容易发散。

改进版G-N:

LM方法的具体步骤：