SLAM中的非线性优化方法

SLAM中的非线性优化方法

理论基础

矩阵求导

若a和A不是关于x的函数矩阵，则有：

$$\begin{gathered} \frac{\partial a\cdot x}{\mathrm{d}x}=a \\ \frac{\partial x^{T}A\cdot x}{\mathrm{d}x}=2x^{T}A \\ \frac{\partial u^T\cdot v}{\mathrm{d}x}=u^T\cdot \frac{\partial v\cdot x}{\mathrm{d}x}+v^T\cdot \frac{\partial u\cdot x}{\mathrm{d}x} \end{gathered}$$

病态矩阵：Ax=b求解时，x+一个小的增量，结果有很大波动。

奇异矩阵：Ax=0有无穷多解，AX=b有无穷多解或无解，系数矩阵A不是行满秩 A的行列式为0

为了求解$H\Delta =g,H=J^{T}J$需要对H求逆，而H为半正定矩阵，可能为病态或者奇异，此时稳定性较差，可能不收敛，即使H是非病态和非奇异，也有可能因为$\Delta x$过大而导致不收敛。


优化问题具体步骤
1.给定初值$x_0$。
2.对于第k次迭代，寻找一个迭代量$\Delta x$，使得$||f(x+\Delta x_k)|{|}_2^2$达到最小值。
3.若$\Delta x$足够小，则停止。
4.否则令$x_k+1=x_k+\Delta x_k$返回第二步。

高斯牛顿法的具体步骤：
1.给定初值$x_0$。
2.对于第k次迭代，计算当前迭代的雅可比矩阵$J(x_k)$和误差$f(x_k)$。
3.求解增量方程$H\Delta x=g，H=J{J}^T，g=-J^{T}f(x)$

---

待更新：

编程实现

SLAM十四讲第六讲课后习题，曲线拟合问题。

拟合一条满足一下方程的曲线：

$$y=exp(a{x}^2+bx+c)+w$$

a，b，c为曲线参数，w为高斯噪声，假设有N个关于x,y的观测数据点，根据这些点来求出曲线的参数，那么可以求解以下的最小二乘问题以估计曲线参数：

$$\underset{a,b,c}{\min }\frac{1}{2}\sum _{i=1}^N||y_i-exp(a{x}_i^2+b{x}_i+c)|{|}^2$$

定义误差为：

$$e_i=y_i-(a{x}_i^2+b{x}_i+c)$$

那么，每个误差项对于状态向量的导数为：

$$\begin{gathered} \frac{\partial e}{\partial a}=-x_i^{2}exp(a{x}_i^2+b{x}_i+c) \\ \frac{\partial e}{\partial b}=-x_{i}exp(a{x}_i^2+b{x}_i+c) \\ \frac{\partial e}{\partial a}=-exp(a{x}_i^2+b{x}_i+c) \end{gathered}$$

于是$J=[\frac{\partial e}{\partial a},\frac{\partial e}{\partial b},\frac{\partial e}{\partial c}{]}^T$

ceres库求解曲线拟合问题

G2O库求解曲线拟合问题