高斯牛顿&LM

好久没再写博客。重新拾起来，先从基础的开始，把还给时间的都放在这里。
高斯牛顿法及其改进，在基于优化的机器人状态估计中是非常基础和重要的方法。
资料来源于《STATE ESTIMATION FOR ROBOTICS》第4.3.1节，和VIO课程第三章Section2。
本文按照个人公式习惯总结推导过程，以备后用。

问题说明

当把问题要用优化的方式去建模和解决时，最终是会把问题建立成一个求函数最值以及最值点的问题。
比如对于以下函数

\[\boldsymbol{y} = \boldsymbol{f}(\boldsymbol{x}, \boldsymbol{\beta}) \]

这个模型的意义是，接受一个输入\(\boldsymbol{x}\)，其输出\(\boldsymbol{f}\)决定于模型参数\(\boldsymbol{\beta}\)。\(\boldsymbol{x}, \boldsymbol{\beta}\)都为高维变量。

当有一系列输入和输出对

\[(\boldsymbol{x_1}, \boldsymbol{y_1}), (\boldsymbol{x_2}, \boldsymbol{y_2}), ..., (\boldsymbol{x_n}, \boldsymbol{y_n}) \]

就可以通过计算使误差最小的\(\boldsymbol{\beta}\)使得模型符合实际。
记\(\boldsymbol{r_i=y_i-f(x_i, \beta)}\)，\(\boldsymbol{R=[r_1, r_2, ... r_n]^T}\)，\(\boldsymbol{F(\beta)=[f(x_1,\beta), f(x_2,\beta),..., f(x_n,\beta)]^T}\)
即

\[\min{\boldsymbol{E(\beta)}} = \min{\frac{1}{2}\boldsymbol{R^TR}} = \sum_{i=1}^{n} \frac{1}{2}(\boldsymbol{y_i} - \boldsymbol{f}(\boldsymbol{x_i}, \boldsymbol{\beta}) )^T (\boldsymbol{y_i} - \boldsymbol{f}(\boldsymbol{x_i}, \boldsymbol{\beta}) ) \]

求解方法

问题就是求一个\(\boldsymbol{\beta}\)，那么方法也是多种多样的。

梯度下降

最自然而然的就是梯度下降。当一个函数在一个位置\(\boldsymbol{\beta_0}\)是连续可导，那么在这个点沿着梯度的负方向附近的一个点\(\boldsymbol{\beta}'\)必然小于该点。那么可以使用

\[\boldsymbol{\beta} \gets \boldsymbol{\beta_0} - \left. \alpha \boldsymbol{\frac{\partial E}{\partial \beta} } \right| _{\beta = \beta_0} \]

的策略，走一步求一遍梯度，更新一次\(\boldsymbol{\beta}\)，直到发现\(\boldsymbol{E}\)无法再减小。至于走的多长，在这里就是一个调参问题。

牛顿法

梯度下降是一种走一步看一步的方法。通过以对函数求二阶泰勒展开的形式，去掉高阶项，那么这个函数就是一个碗型的函数（二维输入变量），我们可以通过解析方法，一次就走到碗底。有两点需要注意

• \(\boldsymbol{E}\)的形式是关于 \(\boldsymbol{f}\) 的二次型，但是里面包的 \(\boldsymbol{f}\) 是一个高维非凸的函数，所以这里有二阶泰勒近似展开的说法。
• 任何近似都是在其展开点附近成立的，所以参数的不同初始位置会导致不同的优化结果。
  接着往下推

\[\boldsymbol{E(\beta)} \approx \boldsymbol{J(\delta \beta)} = \left. \boldsymbol{E(\beta_0)} + \boldsymbol {\left. \frac{\partial E(\beta)} {\partial \beta} \right|_{\beta=\beta_0} \delta \beta } + \frac{1}{2} \boldsymbol {\delta \beta^T \frac{\partial^2E(\beta)}{\partial \beta \partial \beta^T}} \right|_{\boldsymbol{\beta=\beta_0}}\boldsymbol{\delta \beta } \]

问题近似于令\(\boldsymbol{J(\delta \beta)}\)最小化，即，使其对于\(\boldsymbol{\delta\beta}\)的导数为0。

即

\[\frac{\partial \boldsymbol{J(\beta)}}{\partial \boldsymbol{\delta\beta}}=\boldsymbol{\left( \left.\frac{\partial E(\beta)}{\partial \beta}\right|_{\beta=\beta_0}\right) }+\left(\boldsymbol {\delta \beta ^{*T}\left.\frac{\partial^2E(\beta)}{\partial \beta \partial \beta^T}\right|_{\beta=\beta_0}}\right)=0 \]

其中\(\boldsymbol{\beta ^{*T}}\)即为一次迭代中要求的近似凸函数极值点。即

\[\boldsymbol {\left.\frac{\partial^2E(\beta)}{\partial \beta \partial \beta^T}\right|_{\beta=\beta_0}\delta \beta ^*} = -\boldsymbol{\left( \left.\frac{\partial E(\beta)}{\partial \beta}\right|_{\beta=\beta_0}\right) }^T \]

可以使用\(\boldsymbol{\beta} \gets \boldsymbol{\beta_0} + \boldsymbol{\delta \beta^*}\)的策略，逐步更新待求参数。
牛顿法将目标函数在局部近似为二次型的形式求得迭代增量，一次就直接达到近似函数的最低点，同时不用再引入迭代步长。
其缺点就是需要计算二阶导矩阵（Hessian矩阵），当函数很复杂时，算法的应用存在困难。

高斯牛顿法

牛顿法的缺陷在于需要求Hessian矩阵，从牛顿法继续推导，将Hession矩阵的形式进行近似简化就可得到高斯牛顿法。
一阶导：

\[\boldsymbol{ \left.\frac{\partial E(\beta)}{\partial \beta}\right|_{\beta=\beta_0} }=\frac{}{}\boldsymbol{R(\beta_0)^T\left.\frac{\partial R}{\partial \beta}\right|_{\beta=\beta_0}} \]

继续求二阶导，

\[\boldsymbol {\left.\frac{\partial^2E(\beta)}{\partial \beta \partial \beta^T}\right|_{\beta=\beta_0}} = \boldsymbol{ \left( \left.\frac{\partial R}{\partial \beta}\right|_{\beta=\beta_0}\right)^T \left( \left.\frac{\partial R}{\partial \beta}\right|_{\beta=\beta_0}\right)+ R(\beta_0) H } \]

其中\(\boldsymbol{H}\)为\(\boldsymbol{R}\)关于\(\boldsymbol{\beta}\)的Hession矩阵。注意上式等号右边加号后的项有一个\(\boldsymbol{R}\)的项。当\(\boldsymbol{\beta}\)的初始值选得不差时，可以假设\(\boldsymbol{R}\)很小，从而可以约去加号后面的项，即

\[\boldsymbol {\left.\frac{\partial^2E(\beta)}{\partial \beta \partial \beta^T}\right|_{\beta=\beta_0}} \approx \boldsymbol{ \left( \left.\frac{\partial R}{\partial \beta}\right|_{\beta=\beta_0}\right)^T \left( \left.\frac{\partial R}{\partial \beta}\right|_{\beta=\beta_0}\right)} \]

即求

\[\boldsymbol{ \left( \left.\frac{\partial R}{\partial \beta}\right|_{\beta=\beta_0}\right)^T \left( \left.\frac{\partial R}{\partial \beta}\right|_{\beta=\beta_0}\right)} \boldsymbol {\delta \beta ^*} = -\boldsymbol{\left(\left.\frac{\partial R}{\partial \beta}\right|_{\beta=\beta_0}\right)^TR(\beta_0)} \]

自此，即避免了求Hession矩阵，又可以做到近似二阶下降。
注意在这里

\[\boldsymbol{\boldsymbol{\left.\frac{\partial E(\beta)}{\partial \beta}\right|_{\beta=\beta_0} = R(\beta_0)^T\left.\frac{\partial R}{\partial \beta}\right|_{\beta=\beta_0}}} \]

LM优化(Levenberg-Marquardt)

将\(\boldsymbol{\partial\beta^*}\)的求解公式改进为

\[\left( \boldsymbol{ \left( \left.\frac{\partial R}{\partial \beta}\right|_{\beta=\beta_0}\right)^T \left( \left.\frac{\partial R}{\partial \beta}\right|_{\beta=\beta_0}\right) + \lambda I } \right) \boldsymbol {\delta \beta ^*} = - \boldsymbol{\left( \left. \frac{\partial R}{\partial \beta} \right|_{\beta=\beta_0}\right)^TR(\beta_0)} \]

当\(\lambda\) 很大时，

\[\boldsymbol{ \left( \left.\frac{\partial R}{\partial \beta}\right|_{\beta=\beta_0}\right)^T \left( \left.\frac{\partial R}{\partial \beta}\right|_{\beta=\beta_0}\right) } \]

所占的比重就很小

\[\boldsymbol {\delta \beta ^*} \approx -\frac{1}{\lambda} \boldsymbol{\left( \left. \frac{\partial R}{\partial \beta} \right|_{\beta=\beta_0}\right)^TR(\beta_0)} \]

此时迭代方式是一个缓慢的梯度下降策略。
当\(\lambda=0\)时恢复为正常的高斯牛顿更新。这样就可以使算法自动在梯度下降和高斯牛顿之间变换。
问题是该以怎样的策略更新\(\lambda\)？

比例因子

按照定义，牛顿法是将原函数在局部近似为二次型函数，高斯牛顿进一步省略Hession矩阵中的二阶项。
上文中提到两个符号\(\boldsymbol{E,J}\)，由于\(\boldsymbol{J}\)已经是近似后的二次型，每次迭代必然能使\(\boldsymbol{J}\)在展开点\(\boldsymbol{\beta}\)优化到最小，即\(\boldsymbol{J(0)-J(\delta \beta ^*)\ge 0}\)一定成立。

• 当原函数在展开点附近更“像”凸函数，算法能够使得函数下降，此时应该减小\(\lambda\)，让收敛更像高斯牛顿同时也增大了步长。
• 否则原函数就得不到下降，即\(\boldsymbol{E(\beta_0)-E(\beta_0+\delta \beta ^*)>0}\)不一定成立。当不成立时，说明此处的函数维度更高需要减缓下降速度。

可以使用

\[\rho=\boldsymbol{\frac{E(\beta_0)-E(\beta_0+\delta \beta ^*)}{J(0)-J(\delta \beta ^*)}} \]

来判定当\(\rho<0\)或者不够大就减小\(\lambda\)，当\(\rho > 0\)并且较大就增大\(\lambda\)。
在这里，\(\boldsymbol{J(0)-J(\delta \beta ^*)}\)可以进一步简化（代码里可以通过缓存免算？）：
记\(\boldsymbol{E'=\left. \frac{\partial E(\beta)} {\partial \beta}\right|_{\beta=\beta_0}}\)，并且\(\boldsymbol{R'^TR\delta\beta+\lambda I=E^T}\)，则

\[\begin{array}{ccc} \boldsymbol{J(0)-J(\delta \beta ^*)} &= & \boldsymbol{-(E'\delta \beta^* +\frac{1}{2}\delta\beta^{*T}R'^TR'\delta \beta^*) }\\&=& -\frac{1}{2}\boldsymbol{(-2E' - \delta \beta^{*T}(R'^TR' \lambda I -\lambda I))\delta\beta^*} \\&=& \frac{1}{2}(\boldsymbol{-E' +\lambda\delta \beta^{*T} ) \delta\beta}^* \end{array} \]

比例因子给了调整\(\lambda\)的依据，至于如何调整，目前有以下方法，其中的\(\mu\)就是本文的\(\lambda\)。