非线性优化

对于一个slam系统，我们可以得到motion function 和 estimation function，如下所示：

$$\left\{\begin{matrix}x_{k}=f\left ( x_{k-1,\mu _{k}} \right )+w_{k} \\ z_{k,j}=h\left ( y_{j},x_{k} \right )+v_{k,j} \end{matrix}\right.$$
对于位姿变量$x_{k}$，我们可以用$T_{k}$或$e^{\hat{\xi }}$表示. $y_{j}$表示第j个landmark。由相机的针孔成像模型可知，观测模型可以表示为：
$$sz_{k,j}=Ke^{\hat{\xi }}y_{j}$$
其中$K$为相机内参，$s$为像素点的距离，$z_{k,j}$ 和$y_{j}$都需要用齐次坐标表示。在运动和观测方程中，假设噪声为0均值的高斯分布。
我们的目的是通过带噪声的数据来求求得位姿和地图的最优估计值，可以用非线性优化来解决这一问题。在非线性优化中，把所有估计的变量放在一个“状态变量“中：
$$x=\left \{ x_{1},...,x_{N},y_{1},...,y_{M} \right \}$$
对机器人的状态估计，就是已知输入数据$\mu$和观测数据$z$的条件下，得到状态$x$的条件概率分布：
$$P(x|z,\mu)$$
可以通过贝叶斯法则，可以转换为maximize a posterior,即：
$$x_{MAP}^{*}=argmaxP\left ( z|x \right )P\left ( x \right )$$
如果不知道机器人的位姿大概在什么地方，此时就没了先验信息，问题转变为求解$x$ 的最大似然问题：
$$x_{MLE}^{*}=argmaxP\left ( z|x \right )$$
对于某一次观测模型，设观测数据的条件概率为：
$$x=\left ( z_{j,k} \right|x_{k},y_{j})=N\left (h\left(y_{j},x_{k}\right),Q_{k,j}\right)$$
对上式取负对数并得到最小值可估计:
$$x^{*}=argmin((z_{k,j}-h(x_{k},y_{j})^{T}Q_{k,j}^{-1}(z_{k,j}-h(x_{k},y_{j}))$$
因此，对于所有的运动和任意的观测，定义数据与估计值之间的误差：
$$e_{v,k}=x_{k}-f({x_{k-1},\mu_{k}})$$ $$e_{y,j,k}=z_{k,j}-h(x_{k},y_{j})$$
因此可以得到误差项：
$$J(x)=\sum_{k}e_{v,k}^{T}R_{k}^{-1}e_{v,k}+\sum_{k}\sum_{j}e_{y,k,j}^{T}Q_{k,j}^{-1}e_{y,k,j}$$
可以看出SLAM中的最小最小二乘问题具有以下结构：1.虽然总体的状态变量的维数很高，但是每个误差项都是一个小规模的约束，可以把误差项的小雅可比矩阵块放到整体的雅可比矩阵中。2.增量方程的求解会具有一定的稀疏性。3.如果是用李代数表示，则该问题是无约束的最小二乘问题，如果用旋转矩阵表示位姿，则会引入旋转矩阵的约束。4.可以采用非二范数的形式度量误差。
非线性最小二乘求解：
直接求解（求鞍点，比较函数极值大小）不太方便，因此可以采用迭代的方法，步骤如下：
1.给定某个初始值$x_{0}$
2.对于第$k$次迭代，寻找一个增量$\Delta x_{k}$，使得$||f(x_{k}+\Delta x_{k})||_{2}^{2}$达到极小值
3.若$\Delta x_{k}$足够小，则停止
4.否则更新$x$，同时返回第二步
求解非线性最小二乘的方法有：
(1) 一阶和二阶梯度法：
将目标函数在$x$附近进行泰勒展开： $$||f(x_{k}+\Delta x_{k})||_{2}^{2}\approx ||f(x)||_{2}^{2}+J(x)\Delta x + \frac{1}{2}\Delta x^{T}H\Delta x$$
需要注意，这里的$J$是$||f(x)||^{2}$的Jacobian Matrix, H 为 Hessian Matrix. 如果保留一阶梯度，则为steepest descent method，得到$\Delta x=-J^{T}(x)$，若要保留二阶梯度信息，则$H\Delta x=-J^{T}$，这种方法也叫做newton method，但是需要计算hessian matrix。梯度下降法则过于贪心，容易走出锯齿路线。
(2)Gaussian-Newton Method:
GN法是将$f(x)$进行一阶泰勒展开： $$f(x+\Delta x ) \approx f(x) + J(x)\Delta x$$ ，因此可以转换为一个线性的最小二乘问题： $$\Delta x^{*}=argmin_{\Delta x}\frac{1}{2}|||f(x)+J(x)\Delta x||^{2}$$ 对此方程展开，并求上式关于$\Delta x$的导数，并将其为零，可得： $$J(x)^{T}J(x)\Delta x=-J(x)^{T}f(x)$$ ，将左边的系数定义为$H$，右边定义为$g$，那么上式为$H\Delta x=g$因此对于GN法，每次迭代算出jacobian和residue。对于GN，H应该可逆，但是并不能保证，这会导致算法不收敛，同时若$\Delta x$较大，会使得局部近似不准确。
(3) LM法：
需要将$\Delta x$添加一个Trust Region, 使用$\rho=\frac{f(x+\Delta x) - f(x)}{J(x)\Delta x}$. 若$\rho$接近1，则近似是好的。如果$\rho$比较大，则应该放大近似范围，反之则反之。用拉格朗日乘子将问题转换为一个无约束优化问题：
$$min_{\Delta{x_{k}}}\frac{1}{2}||f(x_{k}+J(x_{k})\Delta x_{k})||^{2}+\lambda ||D\Delta x||^{2}$$ 可得表达式： $$(H+\lambda D^{T}D)\Delta x = g$$ ，通常取$D=I$