第2章 非线性优化

2.1 最小二乘的引出

高维高斯分布

  对于一个任意的高维高斯分布$ \boldsymbol{x} ～ N \left( \boldsymbol{\mu},\boldsymbol{\Sigma} \right) $，它的概率密度函数展开形式：

$$
P(\boldsymbol{x}) = \frac{1}{\sqrt{(2\pi)^N det(\boldsymbol{\Sigma})}}
exp\left( -\frac{1}{2} (\boldsymbol{x}-\boldsymbol{\mu})^T \boldsymbol{\Sigma}^{-1} (\boldsymbol{x}-\boldsymbol{\mu}) \right)
$$

  取其负对数：

$$
-\ln \left( P\left(\boldsymbol{x}\right) \right) = \frac{1}{2} \ln\left( (2\pi)^N det(\boldsymbol{\Sigma}) \right) +
\frac{1}{2} (\boldsymbol{x}-\boldsymbol{\mu})^T {\boldsymbol{\Sigma}}^{-1} (\boldsymbol{x}-\boldsymbol{\mu})
$$

  只要最小化上式右侧第二项，就得到原分布最大化，即得到对状态的最大似然估计。

构建最小二乘问题

  状态估计问题中，假设运动、观测噪声服从高斯分布，根据运动、观测方程，二者数据也服从高斯分布，且条件概率分别为：

$$
\begin{cases}
P(\boldsymbol{x}_k|\boldsymbol{x}_{k-1},\boldsymbol{u}_k) = N\left( f(\boldsymbol{x}_{k-1},\boldsymbol{u}_k),\boldsymbol{R}_k \right) \
P(\boldsymbol{z}_{j,k}|\boldsymbol{x}_k,\boldsymbol{y}_j) = N\left( h(\boldsymbol{x}_k,\boldsymbol{y}_j),\boldsymbol{Q}_{k,j} \right)
\end{cases}
$$

  综合考虑二者，定义数据与估计的误差，且有误差的平方和。使该平方和最小的最优解，即为状态的最大似然估计。

$$
\begin{aligned}
f(\boldsymbol{x}) &= \sum_k {e_{v,k}}^T {\boldsymbol{R}_k}^{-1} e_{v,k} + \sum_k \sum_j {e_{y,k,j}}^T {\boldsymbol{Q}_{k,j}}^{-1} e_{y,k,j} \
e_{v,k} &= \boldsymbol{x}_k - f(\boldsymbol{x}_{k-1},\boldsymbol{u}_k) \
e_{y,k,j} &= \boldsymbol{z}_{j,k} - h(\boldsymbol{x}_k,\boldsymbol{y}_j)
\end{aligned}
$$

2.2 非线性最小二乘

  对于如下最小二乘问题，使用迭代方法求解：从一个初始值出发，不断寻找梯度并下降，直到增量非常小，无法再使函数下降，此时算法收敛，目标达到了一个极小。其中对于增量的确定，分为多种方法，如牛顿法、高斯牛顿法、列文伯格-马夸尔特。

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

• 给定某个初始值$ \boldsymbol{x}_0 $
• 对于第$ k $次迭代，寻找一个增量$ \Delta \boldsymbol{x}_k $，使得$ {{|| f(\boldsymbol{x})+\Delta\boldsymbol{x}_k ||}_2}^2 $达到极小值
• 若$ \Delta \boldsymbol{x}_k $足够小，则停止
• 否则，令$ \boldsymbol{x}_{k+1}=\boldsymbol{x}_k+\Delta\boldsymbol{x}_k $，返回$ 2 $

2.2.1 一阶和二阶梯度法

推导：

  将目标函数在$ \boldsymbol{x} $附近进行泰勒展开。其中$ \boldsymbol{J}(\boldsymbol{x}) $是$ {{|| f(\boldsymbol{x}) ||}_2}^2 $关于$ \boldsymbol{x} $的导数（Jacobian矩阵），而$ \boldsymbol{H} $是二阶导数（Hessian矩阵）。

$$
{{|| f(\boldsymbol{x})+\Delta\boldsymbol{x} ||}_2}^2 =
{{|| f(\boldsymbol{x}) ||}_2}^2 + \boldsymbol{J}(\boldsymbol{x})\Delta\boldsymbol{x} + \frac{1}{2}\Delta\boldsymbol{x}^T\boldsymbol{H}\Delta\boldsymbol{x} + \cdots
$$

• 一阶（最速下降法）：取前两项，关于$ \Delta\boldsymbol{x} $求导并令其值等于$ 0 $，可得到增量的解为$ \boldsymbol{J}^T(\boldsymbol{x}) $，即沿着反向梯度方向前进即可。实际中可加上步长系数，即最快的下降速度（最速下降法）。

$$ \Delta\boldsymbol{x}^* = -\boldsymbol{J}^T(\boldsymbol{x}) $$

• 二阶（牛顿法）：取前三项，关于$ \Delta\boldsymbol{x} $求导并令为$ 0 $，得到增量的解如下。

$$ \boldsymbol{H}\Delta\boldsymbol{x} = -\boldsymbol{J}^T(\boldsymbol{x}) $$

优缺点：

• 直观，只需把函数在迭代点附近进行泰勒展开，并针对更新量做小化即可。
• 最速下降法：过于贪心，容易走出锯齿路线，反而增加迭代次数。
• 牛顿法：问题规模很大时，计算目标函数的$ \boldsymbol{H} $矩阵非常困难，因此通常需避免$ \boldsymbol{H} $的计算，该方法不常用。

2.2.2 高斯牛顿法(G-N)

推导：

• 将$ f(\boldsymbol{x}) $进行一阶的泰勒展开
  $$ f(\boldsymbol{x}+\Delta\boldsymbol{x}) \approx f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})\Delta\boldsymbol{x} $$

• 将上述展开式代入原始最小二乘问题，构建新的线性最小二乘问题：
  $$ \Delta\boldsymbol{x}^* = arg \min_{\Delta\boldsymbol{x}} \frac{1}{2} ||f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})\Delta\boldsymbol{x}||^2 $$

• 展开目标函数的平方项：

$$
\begin{aligned}
\frac{1}{2} {||f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})\Delta\boldsymbol{x}||}^2
&= \frac{1}{2} {\left( f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})\Delta\boldsymbol{x} \right)}^T \left( f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})\Delta\boldsymbol{x} \right) \
&= \frac{1}{2} \left( {{||f(\boldsymbol{x})||}_2}^2 + 2f(\boldsymbol{x})^T\boldsymbol{J}(\boldsymbol{x})\Delta\boldsymbol{x} + \Delta\boldsymbol{x}^T\boldsymbol{J}(\boldsymbol{x})^T\boldsymbol{J}(\boldsymbol{x})\Delta\boldsymbol{x} \right)
\end{aligned}
$$

• 求上式关于$ \Delta\boldsymbol{x} $的导数，并令其为零，可得到一个线性方程组:
  $$
  2\boldsymbol{J}(\boldsymbol{x})^Tf(\boldsymbol{x}) + 2\boldsymbol{J}(\boldsymbol{x})^T\boldsymbol{J}(\boldsymbol{x})\Delta\boldsymbol{x} = \boldsymbol{0}
  $$
  $$
  \boldsymbol{J}(\boldsymbol{x})^T\boldsymbol{J}(\boldsymbol{x})\Delta\boldsymbol{x} = -\boldsymbol{J}(\boldsymbol{x})^Tf(\boldsymbol{x})
  $$

• 求解正规方程(高斯牛顿方程/增量方程)：将上式左边系数定义为$ \boldsymbol{H} $，右边定义为$ \boldsymbol{g} $。即高斯牛顿法使用作为$ \boldsymbol{H} $（二阶Hessian矩阵）的近似，从而省略$ \boldsymbol{H} $的过程。
  $$ \boldsymbol{H} \Delta \boldsymbol{x} = \boldsymbol{g} $$

总结：

• 给定初始值$ \boldsymbol{x}_0 $
• 对于第$ k $次迭代，求出当前的雅可比矩阵矩阵$ \boldsymbol{J}(\boldsymbol{x}_k) $和误差$ f(\boldsymbol{x}_k) $
• 求解增量方程
  $$
  \begin{cases}
  \boldsymbol{H} \Delta \boldsymbol{x}_k = \boldsymbol{g} \
  \boldsymbol{H}=\boldsymbol{J}(\boldsymbol{x}_k)^T\boldsymbol{J}(\boldsymbol{x}_k) \
  \boldsymbol{g}=-\boldsymbol{J}(\boldsymbol{x}_k)^Tf(\boldsymbol{x}_k)
  \end{cases}
  $$
• 若$ \Delta \boldsymbol{x}_k $足够小，则停止。否则，令$ \boldsymbol{x}_{k+1}=\boldsymbol{x}_k+\Delta\boldsymbol{x}_k $，返回$ 2 $

优缺点：

• 非线性优化中，相当多算法都可以归结为高斯牛顿法的变种。
• 可能出现$ \boldsymbol{J}^T\boldsymbol{J} $为奇异矩阵或者病态矩阵的情况，此时增量的稳定性较差，导致算法不收敛。
• 如果求出的步长$ \Delta \boldsymbol{x} $太大，导致局部近似不准确，甚至可能无法保证迭代收敛。

2.2.3 列文伯格-马垮尔特法(L-M)

改进点：

G-N法中采用近似二阶泰勒展开只能在展开点附近有较好的近似效果，因此想到给$ \Delta \boldsymbol{x} $添加一个信赖区域，使用实际函数和近似模型之间的差异来确定信赖区域的范围。若$ \rho $接近1，则近似是好的；若太小，则减小近似范围；反之增大近似范围。

$$ \rho = \frac{f(\boldsymbol{x}+\Delta\boldsymbol{x})-f(\boldsymbol{x})}{\boldsymbol{J}\Delta\boldsymbol{x}} $$

推导：

• 给定初值$ \boldsymbol{x}_0 $，以及初始化半径$ \mu $

• 对于第$ k $次迭代，求解如下问题：

  • $ \mu $：信赖区域的半径
  • $ \boldsymbol{D} $：取为单位矩阵$ \boldsymbol{I} $，相当于把$ \Delta\boldsymbol{x} $约束在一个球中；取为非负数对角阵（常用$ \boldsymbol{J}^T\boldsymbol{J} $的对角元素平方根），使得在梯度小的维度上约束范围更大。
    $$ \min_{\Delta \boldsymbol{x}_k} \frac{1}{2} {|| f(\boldsymbol{x}_k)+\boldsymbol{J}(\boldsymbol{x}_k)\Delta\boldsymbol{x}_k ||}^2，s.t.{|| \boldsymbol{D} \Delta \boldsymbol{x}_k ||}^2 \leq \mu $$
  • 利用拉格朗日乘子$ \lambda $将上式转换为无约束的优化问题：
    $$ \min_{\Delta \boldsymbol{x}_k} \frac{1}{2} {|| f(\boldsymbol{x}_k)+\boldsymbol{J}(\boldsymbol{x}_k)\Delta\boldsymbol{x}_k ||}^2 + \frac{\lambda}{2}{|| \boldsymbol{D} \Delta \boldsymbol{x} ||}^2 $$
  • 类似G-N的做法，展开后计算：
    $$ (\boldsymbol{H}+\lambda\boldsymbol{D}^T\boldsymbol{D})\Delta\boldsymbol{x} = \boldsymbol{g} $$
  • 相比G-N，多了一项$ \lambda\boldsymbol{D}^T\boldsymbol{D} $，若$ \boldsymbol{D}=\boldsymbol{I} $，则转为求解：
    $$ (\boldsymbol{H}+\lambda\boldsymbol{I})\Delta\boldsymbol{x} = \boldsymbol{g} $$

• 计算$ \rho $。

• 若$ \rho > \frac{3}{4} $，则$ \mu = 2\mu $。

• 若$ \rho < \frac{1}{4} $，则$ \mu = 0.5\mu $。

• 如果$ \rho $大于某阈值，认为近似可行。令$ \boldsymbol{x}_{k+1} = \boldsymbol{x}_k + \Delta \boldsymbol{x}_k $。

• 判断算法是否收敛。如不收敛则返回2，否则结束。

优缺点：

• 当参数$ \lambda $较小时，二次近似模型在该范围较好，L-M法更接近G-N法。

• 当$ \lambda $较大时，附近的二次近似不好，L-M更接近一阶梯度下降法。