因子图学习笔记其二

Reference:

1. Frank Dellaert 《机器人感知-因子图在SLAM中的应用》

2. 平滑与地图构建

下面我们将讨论平滑与地图构建(smoothing and mapping, SAM)算法，这个算法代表了离线批处理 SLAM 方法中最新的技术发展水平。我们将介绍用于求解任意非线性因子图的非线性迭代优化技术。我们在本章中不会过多地涉及稀疏线性代数的细节，而是将其留到第 3 章。

2.1 SLAM 中的因子图

对于一个更加真实的 SLAM 问题，而非第 1 章中的小型 SLAM 例子来说，因子图会像下图所示。这个因子图是模拟如下过程得到的：一个机器人在二维平面上运动大概 100 个时间步长，同时观测路标点。为了方便进行可视化，图中每个机器人位姿及路标点都由其在二维空间中的位置真值所提供。从图上可以看到，里程计因子形成了一个突出的、链状的骨架，而两边的二元似然因子连接到了 20 个左右的路标点上。通常，在这类 SLAM 问题中，除了先验因子，所有其他的因子都是非线性的。

通过观察这个因子图，我们可以发现它包含大量的结构，可以通过它们来了解这个 SLAM 问题实例。首先，如我们所预期的那样，有一些路标点被大量地观测，从而可以相对准确地定位。其他路标点只与图有微弱的联系，因此可以预期它们并没有被准确地定位。例如，对于靠近右下方、单独的那个路标点，只有一个观测量与之关联：如果这是一个方位 (角) 测量，许多可能的路标点二维位置都将同样 “正确”。以上论述等同于，部分未知状态量在部分维度上将具有无穷大的不确定性，此时我们应该利用先验知识进行修正。

SLAM 领域中的最大后验概率推断，正是一个利用已知但不确定的信息决定未知量最可能值的过程。尽管在许多实际的场合中，我们可能都有一个比较好的初始化估计值，但在真实生活中，我们并不能得到路标点，以及随时间变化的机器人位姿的真值。下面我们会展示如何通过对因子图中未知变量进行非线性优化，寻找到一个最优的赋值一一最大后验概率估计值。

2.2 非线性因子图的最大后验概率推断

下面我们将会看到，对一个符合高斯噪声模型的 SLAM 问题进行最大后验概率推断，等价于求解一个非线性最小二乘问题。实际上，对于任意一个因子图，最大后验概率推断都可以归结为最大化所有因子势能的乘积($\varphi (\mathbf{X})={\prod }_i{\varphi }_i\left({\mathbf{X}}_i\right)$)：
$$\begin{array}{rl}{\mathbf{X}}^{\mathrm{MAP}}& =\underset{\mathbf{X}}{\mathrm{argmax}}\varphi (\mathbf{X})\\ & =\underset{\mathbf{X}}{\mathrm{argmax}}\prod _i{\varphi }_i\left({\mathbf{X}}_i\right)\end{array}$$

让我们暂时假设所有的因子都具备如下形式(这里只考虑1.4节说的概率观测模型，将概率运动模型放一边)：
$${\varphi }_i\left({\mathbf{X}}_i\right)\propto \exp \left\{-\frac{1}{2}{\Vert h_i\left({\mathbf{X}}_i\right)-{\mathbf{z}}_i\Vert }_{{\Sigma }_i}^2\right\}$$

其中既包括简单的高斯先验，也包括由受到零均值正态分布噪声干扰的观测量推导出来的似然因子。对式取负对数，并且扔掉 $1/2$ 这一项，问题就可以转化为最小化非线性最小二乘(nonlinear least-square)的和(推导方式具体可见非线性优化2.2节)：
$${\mathbf{X}}^{\mathrm{MAP}}=\underset{\mathbf{X}}{\mathrm{argmin}}\sum _i{\Vert h_i\left({\mathbf{X}}_i\right)-{\mathbf{z}}_i\Vert }_{{\Sigma }_i}^2$$

最小化上述目标函数，可以通过结合一些由观测量推导出来的似然因子和一些可能的先验信息，唯一地决定未知量的最大后验概率解，从而实现传感器数据的融合。

一个很重要但却并不显著的信息是，式 ${\mathbf{X}}^{\mathrm{MAP}}=\underset{\mathbf{X}}{\mathrm{argmin}}{\sum }_i{\Vert h_i\left({\mathbf{X}}_i\right)-{\mathbf{z}}_i\Vert }_{{\Sigma }_i}^2$中的因子通常表示了相关未知变量 ${\mathbf{X}}_i$ 上信息不充足的(uninformed)概率密度。实际上，除了简单的先验因子，观测量 ${\mathbf{z}}_i$ 通常都比未知量 ${\mathbf{X}}_i$ 的维度要低。在这些情况中，某一个观测量将与域 $X_i$ 中的一个无穷子集符合相同的似然值。例如，一部相机采集到的图像中的一个二维观测，与投射到同一个图像位置的全部三维点束一致(相机观测量$u,v$，世界坐标为$x,y,z$，观测量维度更低)。只有将多个观测量结合起来，我们才有希望得到变量的唯一解。

尽管函数 $h_i$ 是非线性的，但是如果我们有一个好的初始估计值，那么像高斯-牛顿迭代或者列文伯格-马夸尔特这些非线性优化方法将可以使解收敛到式 ${\mathbf{X}}^{\mathrm{MAP}}=\underset{\mathbf{X}}{\mathrm{argmin}}{\sum }_i{\Vert h_i\left({\mathbf{X}}_i\right)-{\mathbf{z}}_i\Vert }_{{\Sigma }_i}^2$的全局最小值。它们通过对该式进行一系列的线性近似不断地逼近最小值。下面，我们首先考虑如何构建一个非线性问题的线性化版本。

2.3 线性化

我们可以使用简单的泰勒展开公式将非线性最小二乘目标函数式${\mathbf{X}}^{\mathrm{MAP}}=\underset{\mathbf{X}}{\mathrm{argmin}}{\sum }_i{\Vert h_i\left({\mathbf{X}}_i\right)-{\mathbf{z}}_i\Vert }_{{\Sigma }_i}^2$中所有的观测函数 $h_i(\cdot )$ 线性化，
$$h_i\left({\mathbf{X}}_i\right)=h_i\left({\mathbf{X}}_i^0+{\mathbf{\Delta }}_i\right)\approx h_i\left({\mathbf{X}}_i^0\right)+{\mathbf{H}}_i{\mathbf{\Delta }}_i$$

式中，观测雅可比矩阵(measurement Jacobian) $H_i$(你这里写个$H$干哈子，写$J$多好) 是定义在一个给定线性化点 ${\mathbf{X}}_i^0$ 处关于 $h_i(\cdot )$ 的 (多变量) 偏微分。
$${{\mathbf{H}}_i\stackrel{\text{ def }}{=}\frac{\partial h_i\left({\mathbf{X}}_i\right)}{\partial {\mathbf{X}}_i}|}_{{\mathbf{X}}_i^0}$$

${\Delta }_i\stackrel{\text{ def }}{=}{X}_i-X_i^0$ 是状态更新向量(state update vector)(即想要求的增量)。注意，我们假设 $X_i$ 是在向量空间中，或者等价地，它们由一个向量表示，但实际上并非总是如此。例如，一些 $\mathbf{X}$ 中的未知状态量可以表示三维旋转，或者其他更复杂的流形。我们将会在第 6 章重新回顾这个问题。

将泰勒展开式 $h_i\left({\mathbf{X}}_i\right)=h_i\left({\mathbf{X}}_i^0+{\mathbf{\Delta }}_i\right)\approx h_i\left({\mathbf{X}}_i^0\right)+{\mathbf{H}}_i{\mathbf{\Delta }}_i$ 带入非线性最小二乘表达式 ${\mathbf{X}}^{\mathrm{MAP}}=\underset{\mathbf{X}}{\mathrm{argmin}}{\sum }_i{\Vert h_i\left({\mathbf{X}}_i\right)-{\mathbf{z}}_i\Vert }_{{\Sigma }_i}^2$ 中，可以得到一个线性最小二乘问题，其状态更新向量 $\mathbf{\Delta }$ 如下(求得最佳$\mathbf{\Delta }$使下面公式值最小):
$$\begin{array}{rl}{\Delta }^{\ast }& =\underset{\Delta }{\mathrm{argmin}}\sum _i{\Vert h_i\left({\mathbf{X}}_i^0\right)+{\mathbf{H}}_i{\mathbf{\Delta }}_i-z_i\Vert }_{{\Sigma }_i}^2\\ & =\underset{\Delta }{\mathrm{argmin}}\sum _i{\Vert {\mathbf{H}}_i{\mathbf{\Delta }}_i-\left\{z_i-h_i\left({\mathbf{X}}_i^0\right)\right\}\Vert }_{{\mathbf{\Sigma }}_i}^2\end{array}$$

式中，$z_i-h_i\left({\mathbf{X}}_i^0\right)$ 是在线性点处的预测误差(prediction error)，即实际观测量与预测观测量之问的差值。上面的 ${\Delta }^{\ast }$ 即局部线性问题的解。

通过变量的简单变换，我们可以将马氏范数转换为 2 -范数：有了矩阵 $\mathbf{\Sigma }$ 的平方根 ${\mathbf{\Sigma }}^{1/2}$，我们可以将一些项 $\mathbf{e}$ 的马氏范数重新写为如下形式:
$$\Vert e{\Vert }_{\Sigma }^2\stackrel{\text{ def }}{=}{e}^{\top }{\Sigma }^{-1}e={\left({\Sigma }^{-1/2}e\right)}^{\top }\left({\Sigma }^{-1/2}e\right)={\Vert {\Sigma }^{-1/2}e\Vert }_2^2$$

因此，可以通过提前将雅可比矩阵 ${\mathbf{H}}_i$ 和式$\begin{array}{r}{\Delta }^{\ast }=\underset{\Delta }{\mathrm{argmin}}\sum _i{\Vert {\mathbf{H}}_i{\mathbf{\Delta }}_i-\left\{z_i-h_i\left({\mathbf{X}}_i^0\right)\right\}\Vert }_{{\mathbf{\Sigma }}_i}^2\end{array}$
中的每一项的预测误差与 ${\Sigma }_i^{-1/2}$ 相乘来消去协方差矩阵 ${\mathbf{\Sigma }}_i$(消去${\Vert {\Sigma }^{-1/2}e\Vert }_2^2$中的${\Sigma }_i^{-1/2}$)：
$$\begin{array}{rl}{A}_i& ={\Sigma }_i^{-1/2}{H}_i\\ b_i& ={\Sigma }_i^{-1/2}\left(z_i-h_i\left({\mathbf{X}}_i^0\right)\right)\end{array}$$

这个过程叫作白化(whitening)。例如，在观测量是标量的情况下，只需要对每一项简单地除以观测标准差 ${\sigma }_i$。注意，这个过程消掉了观测量的单位（例如 长度、角度)，所以上式中不同的行可以被合并到单独一个代价函数中。最后可以得到如下标准最小二乘问题：
$$\begin{array}{rl}{\Delta }^{\ast }& =\underset{\Delta }{\mathrm{argmin}}\sum _i{\Vert {\mathbf{A}}_i{\mathbf{\Delta }}_i-b_i\Vert }_2^2\\ & =\underset{\mathbf{\Delta }}{\mathrm{argmin}}\Vert \mathbf{A}\mathbf{\Delta }-\mathbf{b}{\Vert }_2^2\end{array}$$

式中，$\mathbf{A}$ 和 $\mathbf{b}$ 通过相应地合并所有白化后的雅可比矩阵 ${\mathbf{A}}_i$ 和预测误差 $b_i$，将其合并到一个大的矩阵 $\mathbf{A}$ 和右侧(right hand-side，缩写为 RHS)向量 $\mathbf{b}$ 中得到。

雅可比矩阵 $\mathbf{A}$ 是一个大而稀疏的矩阵，它有一个块结构，反映出相对应的因子图的结构，我们将在第3章详细探索这种稀疏性结构的细节。在这之前，让我们首先回顾更经典的线性代数方法。

2.4 最小二乘问题的直接求解方法

对于一个满秩的 $m\times n$ 的矩阵 $\mathbf{A}$(这里是举个栗子，后面应用的是雅克比矩阵$J$，但感觉还是看之前写的文章: 非线性优化 更直观)，其中 $m⩾n$，式 $\begin{array}{r}{\Delta }^{\ast }=\underset{\mathbf{\Delta }}{\mathrm{argmin}}\Vert \mathbf{A}\mathbf{\Delta }-\mathbf{b}{\Vert }_2^2\end{array}$ 中唯一的最小二乘解可以通过求解正规方程(normal equation) 得到：
$$\left({\mathbf{A}}^{\top }\mathbf{A}\right){\Delta }^{\ast }={\mathbf{A}}^{\top }\mathbf{b}$$

上述方程的求解通常通过分解信息矩阵(information matrix) $\mathbf{\Lambda }$ 来实现，信息矩阵按照如下方式定义和分解：
$$\mathbf{\Lambda }\stackrel{\text{ def }}{=}{\mathbf{A}}^{\top }\mathbf{A}={\mathbf{R}}^{\top }\mathbf{R}$$

式中，乔里斯基三角形矩阵(cholesky triangle) $\mathbf{R}$ 是通过乔里斯基分解(cholesky factorization，一种对称正定矩阵的 LU 分解的变种) 得到的一个 $n\times n$ 的上三角形矩阵。之后，${\Delta }^{\ast }$ 可以通过下面的正向和反向替代过程得到。首先，
$${\mathbf{R}}^{\top }\mathbf{y}={\mathbf{A}}^{\top }\mathbf{b}$$

然后，
$$\mathbf{R}{\mathbf{\Delta }}^{\ast }=\mathbf{y}$$

(为了避免求比较大的矩阵的逆，所以先通过${\mathbf{R}}^{\top }\mathbf{y}={\mathbf{A}}^{\top }\mathbf{b}$求出$\mathbf{y}$，然后再通过$\mathbf{R}{\mathbf{\Delta }}^{\ast }=\mathbf{y}$求出${\mathbf{\Delta }}^{\ast }$么？？？)
对于稠密矩阵，乔里斯基分解需要 $n^3/3$ 数量级的浮点运算，整体算法，包括计算一半的对称阵 ${\mathbf{A}}^{\top }\mathbf{A}$，需要 $(m+n/3)n^2$ 数量级的浮点运算。也可以利用 LDL 分解------一种可以避免计算平方根的乔里斯基分解的变种。

一种比乔里斯基分解更加精确和数值稳定的方法是 $\mathbf{QR}$ 分解。相比于乔里斯基分解，$QR$ 分解不需要计算信息矩阵 $\mathbf{\Lambda }$。不同的是，我们同时计算 $\mathbf{A}$ 本身及其相应的右侧向量 $\mathbf{b}$ 的 $\mathrm{QR}$ 分解：
$$A=Q\left[\begin{array}{c}R\\ 0\end{array}\right]\left[\begin{array}{l}d\\ e\end{array}\right]=Q^{\top }b$$

这里，$\mathbf{Q}$ 是一个 $m\times m$ 的正交矩阵(注意是正交矩阵)，$\mathbf{d}\in {\mathbb{R}}^n$，$\mathbf{e}\in {\mathbb{R}}^{m-n}$，$\mathbf{R}$ 是与乔里斯基三角形矩阵相同的上三角形矩阵。一个比较好的分解稠密矩阵 $\mathbf{A}$ 的方法是从左至右按列计算 $\mathbf{R}$。对于列 $j$，通过将矩阵 $\mathbf{A}$ 左乘一个 Householder 反射矩阵(householder reflection matrix) ${\mathbf{H}}_j$，可以将列 $j$ 在 $\mathbf{A}$ 矩阵对角线下方的非零元素全部置零。经过 $n$ 次迭代后，矩阵 $\mathbf{A}$ 就被全部分解(论计算$\mathbf{R}$的过程)：
$$H_n\cdots H_2{H}_{1}A=Q^{\top }A=\left[\begin{array}{c}R\\ 0\end{array}\right]$$

正交矩阵 $\mathbf{Q}$ 通常并不需要被显式地计算出来：右侧向量 ${\mathbf{Q}}^{\top }\mathbf{b}$ 可以通过将 $\mathbf{b}$ 作 为一个附加列增广在 $\mathbf{A}$ 矩阵右边，然后进行 $\mathrm{QR}$ 分解获得。因为矩阵 $\mathbf{Q}$ 是正交的，所以可以得到(乘一个$Q$是因为是最小二乘，等于是乘了俩，就变成单位矩阵了)：
$$\Vert \mathbf{A}\mathbf{\Delta }-\mathbf{b}{\Vert }_2^2={\Vert {\mathbf{Q}}^{\top }\mathbf{A}\mathbf{\Delta }-{\mathbf{Q}}^{\top }\mathbf{b}\Vert }_2^2=\Vert \mathbf{R}\mathbf{\Delta }-\mathbf{d}{\Vert }_2^2+\Vert \mathbf{e}{\Vert }_2^2$$

这里，我们利用了$A=Q\left[\begin{array}{c}R\\ 0\end{array}\right]$和$\left[\begin{array}{l}d\\ e\end{array}\right]=Q^{\top }b$。显然，$\Vert \mathbf{e}{\Vert }_2^2$ 为最小二乘的残差平方和，最小二乘的解 ${\Delta }^{\ast }$ 可以通过反向替换求解三角形矩阵方程组得到：
$$R{\Delta }^{\ast }=d$$

$\mathrm{QR}$ 分解的时间花销主要由 Householder 反射部分所决定，为 $O\left(2(m-n/3)n^2\right)$ 。将它与乔里斯基分解方法进行对比，可以看到，当 $m\gg n$ 时，两种算法都需要 $O\left(m{n}^2\right)$ 次操作。但是 $\mathrm{QR}$ 分解由于有一个 2 倍的常数因子，所以更慢一些。注意，通过 $\mathrm{QR}$ 分解得到的上三角因子 $\mathbf{R}$ 与乔里斯基分解得到的因子 $\mathbf{R}$ 相同(对角线上元素的正负号可能不同)，如下：

$${\mathbf{A}}^{\top }\mathbf{A}={\left[\begin{array}{c}\mathbf{R}\\ 0\end{array}\right]}^{\top }{\mathbf{Q}}^{\top }\mathbf{Q}\left[\begin{array}{c}\mathbf{R}\\ 0\end{array}\right]={\mathbf{R}}^{\top }\mathbf{R}$$
这里，我们再次利用了矩阵 $\mathbf{Q}$ 是正交矩阵这个性质。

对于 $\mathrm{QR}$ 分解和乔里斯基分解，存在一些用来分解大型稀疏矩阵的高效变种算法。稀疏矩阵分解的时间开销可能远远小于其稠密等价形式，这取决于非零元素的个数及其稀疏结构。目前已经有高效的软件实现，例如，CHOLMOD 和 SuiteSparseQR，它们同时也支持 MATLAB 调用。在实际稀疏问题的求解中，稀疏乔里斯基分解和 LDL 分解的表现远远优于 $\mathrm{QR}$ 分解。

总结一下，与 SLAM 问题相关的优化问题，可以很简洁地用稀疏线性代数的方式表述。这个过程可以归结为：要么将信息矩阵 $\mathbf{\Lambda }$ 分解为平方根的形式，要么将观测雅可比矩阵 $\mathbf{A}$ 分解为平方根的形式。正是因为这些方法是基于平滑与地图构建问题推导而来的矩阵平方根，所以我们将这一类方法称作平方根SAM (square root SAM)，或者简记为 $\sqrt{SAM}$。

2.5 最大后验概率推断的非线性优化

对于非线性最小二乘问题，无法直接对其进行求解，需要从一个合适的初始估计值开始不断地迭代求解。目前存在的多种迭代求解算法之间的区别在于如何在局部对代价函数进行近似，以及如何通过局部近似找到一个更优的估计。在我们的例子中，代价函数为：
$$g(\mathbf{X})\stackrel{\text{ def }}{=}\sum _i{\Vert h_i\left({\mathbf{X}}_i\right)-{\mathbf{z}}_i\Vert }_{{\Sigma }_i}^2$$

这个代价函数有一个对应的因子图，并且这个因子图是由观测量及部分或所有未知量上的先验概率密度推导而来的。

所有的算法都具有以下所述基本结构：它们从一个初始估计值 ${\mathbf{X}}^0$ 开始迭代。在每一次迭代中，计算出更新步长 $\mathbf{\Delta }$，并应用它获得下一个估计值 ${\mathbf{X}}^{t+1}=$ $X^t+\Delta$。当满足一定的收敛条件，比如增量 $\Delta$ 小于一个小的阈值时，迭代过程就会结束。

2.5.1 梯度下降法

梯度下降法(steepest descent, SD) 使用当前位置处梯度下降的方向来计算接下来的更新步长:
$${\mathbf{\Delta }}_{\text{sd }}=-{\alpha \nabla g(\mathbf{X})|}_{\mathbf{X}={\mathbf{X}}^t}$$

这里，我们使用负梯度来指定梯度下降的方向。对于非线性最小二乘目标函数式$g(\mathbf{X})\stackrel{\text{ def }}{=}{\sum }_i{\Vert h_i\left({\mathbf{X}}_i\right)-{\mathbf{z}}_i\Vert }_{{\Sigma }_i}^2$，我们利用 $2.3$ 节中的方法计算雅可比矩阵 $\mathbf{A}$, 局部近似 $g(\mathbf{X})\approx$ ${\Vert \mathbf{A}\left(\mathbf{X}-{\mathbf{X}}^t\right)-\mathbf{b}\Vert }_2^2$，并且在线性化点 ${\mathbf{X}}^t$ 处得到直接梯度 ${\nabla g(\mathbf{X})|}_{\mathbf{X}={\mathbf{X}}^t}=-2{\mathbf{A}}^{\top }\mathbf{b}$ 。

为了在安全地进行更新和合理的收玫速度之间进行平衡，需要仔细权衡步长 $\alpha$ 的选择。线搜索方法可以找到给定方向上的极小值。梯度下降法的实现很简单，但是在极小值附近收敛速度却很慢。

2.5.2 高斯-牛顿法

高斯-牛顿法(Gauss-Newton, GN) 利用二阶更新量实现了更快的收敛，它利用了非线性最小二乘问题的特殊结构，将海森矩阵近似为雅可比矩阵的平方 ${\mathbf{A}}^{\top }\mathbf{A}$。通过求解正规方程式$\left({\mathbf{A}}^{\top }\mathbf{A}\right){\Delta }^{\ast }={\mathbf{A}}^{\top }\mathbf{b}$，我们可以得到高斯-牛顿法的更新步长：
$${\mathbf{A}}^{\top }\mathbf{A}{\mathbf{\Delta }}_{\mathrm{gn}}={\mathbf{A}}^{\top }\mathbf{b}$$

利用 $2.4$ 节中的任何一种方法都可以求解上式。对于一个性质较好（即近似于 二阶多项式 ) 的目标函数，以及一个好的初始估计值，高斯-牛顿法展示了二 阶的收敛速度。如果二阶多项式的拟合结果非常不好，高斯-牛顿步长就会导致一个新的估计值远离极小值，并且发散而不收敛。

2.5.3 列文伯格-马夸尔特算法

列文伯格-马夸尔特(Levenberg-Marquardt, LM) 算法允许多次迭代至收敛，但是步长被控制在一个高斯-牛顿法所生成的二次多项式近似被信任的区域内。因此，这样的方法又称为置信域方法(trust region method)。

为了结合梯度下降法和高斯-牛顿法的优点, 列文伯格 ${}^{133]}$ 提出，通过在 对角线上增加非负常数 λ ∈ R + ∪ { 0 } \lambda \in \mathbb{R}^{+} \cup\{0\} λ∈R+∪{0}，来修改正规方程式 (2.14)：
$$\left(A^{\top }A+\lambda I\right){\Delta }_{\mathrm{lb}}=A^{\top }b$$

注意，当 $\lambda =0$ 时，就得到了高斯-牛顿法；当 $\lambda$ 很大时，近似得到 ${\Delta }^{\ast }\approx \frac{1}{\lambda }{\mathbf{A}}^{\top }\mathbf{b}$，即代价函数 $g[$ 式 (2.23)]的负梯度方向的更新量。因此，列文伯格-马夸尔特算法可以看作高斯-牛顿法和梯度下降法的一种结合。

随后，马夸尔特提出了通过为对角线元素增加比例系数来提供更快的收敛速度:
$$\left({\mathbf{A}}^{\top }\mathbf{A}+\lambda \mathrm{diag}\left({\mathbf{A}}^{\top }\mathbf{A}\right)\right){\mathbf{\Delta }}_{\mathrm{lm}}={\mathbf{A}}^{\top }\mathbf{b}$$

如果梯度值很小 (目标函数几乎是平面的区域)，对角线元素的倒数将会变得很大，那么这个修改就会导致在梯度方向上产生较大的步长。反之，在目标函数的梯度方向上, 算法将变得更加保守, 并采取更小的步长。两种对正规方程的修改都可以从贝叶斯的角度理解为给系统增加一个零均值的先验信息。

算法 2.1为列文伯格-马夸尔特算法。高斯-牛顿法和列文伯格-马夸尔特算法之间最关键的区别在于，后者会拒绝可能导致更大的残差平方和的更新，而前者不会。一个更新被拒绝，说明非线性函数在局部附近表现不好，所以需要更小的更新步长。缩小更新步长可以通过启发式地增大 $\lambda$ 的值来实现，例如，可以将现有的 $\lambda$ 值乘以因子 10，并且求解修改后的正规方程。另一方面，如果一个步长导致了残差平方和的减小，这个步长就会被接受，并且状态估计值也会相应地被更新。在这种情况下，减小 $\lambda$（通过将 $\lambda$ 除以 10 ），算法在一个新的线性化点处继续迭代，直到收敛为止。

2.5.4 Dogleg 最小化法

Powell 的 Dogleg ( PDL ) 算法 $[167]$ 是一个比列文伯格-马夸尔特算法 $[140]$ 更 为高效的算法。列文伯格-马夸尔特算法的一个很大的缺点是, 当一次更新被 拒绝后, 修改后的信息矩阵需要被重新分解, 而分解的过程是整个算法中最耗 时的一个部分。因此, PDL 算法背后关键的想法是: 分别利用高斯-牛顿法和 梯度下降法计算更新量, 然后将其有效地结合起来。如果更新被拒绝, 更新的 方向仍然是有效的, 并且它们可以一种不同的方式重新结合起来, 直到目标函 数的值下降为止。因此, 每次状态估计量的更新只涉及一个而非多个矩阵的 分解。

图 2-2 展示了高斯-牛顿法和梯度下降法得到的更新量是如何被结合起来 的。为了计算结合后的更新量, 我们以梯度下降法得到的更新量作为初始值, 随后向高斯-牛顿更新方向有一个大的弯折（该弯折也是该算法被称为 Dogleg 的原因 ), 但是停止于置信域的边界处。与列文伯格-马夸尔特算法不同, PDL 算法在我们信任其线性假设的范围内维护了一个明确的置信域 ${\Delta }_{\circ }$ 线性近似的 合理性由增益比例来保证:
$$\rho =\frac{g\left({\mathbf{X}}^t\right)-g\left({\mathbf{X}}^t+\mathbf{\Delta }\right)}{L(0)-L(\mathbf{\Delta })}$$

式中 $L(\mathbf{\Delta })={\mathbf{A}}^{\top }\mathbf{A}\mathbf{\Delta }-{\mathbf{A}}^{\top }\mathbf{b}$, 是在当前估计值 ${\mathbf{X}}^t$ 处由式 (2.23) 中的非线性二次 代价函数 $g$ 得到的线性项。如果 $\rho$ 很小, 例如 $\rho <0.25$, 说明通过当前线性化 项预测的目标函数值并末按照预期减小, 那么就将信任区域缩小。另一方面, 如果代价函数值如期缩小 (甚至更好), 例如 $\rho >0.75$, 那么信任区域根据更 新向量的大小进行增长, 并且更新步长会被接受。

两种算法，高斯-牛顿法和 Powell 的 Dogleg 算法，都要求观测量雅可比矩阵是满秩的，以保证 ${\mathbf{A}}^{\top }\mathbf{A}$ 是可逆的。当遇到欠约束的系统（没有足够的观测值)，或者数值上病态的系统，就可以使用列文伯格-马夸尔特算法，尽管收敛的速度可能会受到影响。