在前面几讲，我们介绍了经典SLAM模型的运动方程和观测方程。现在我们已经知道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化 。观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿。然而，由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的，那么就需要在有噪声的数据中进行准确的状态估计。
状态估计问题
批量状态估计与最大后验估计
经典SLAM模型由一个运动方程和一个观测方程构成。
{ x k = f ( x k − 1 , u k ) + w k z k , j = h ( x k , y j ) + v k , j \left\{\begin{array}{l}\boldsymbol{x}{k}=f\left(\boldsymbol{x}{k-1}, \boldsymbol{u}{k}\right)+\boldsymbol{w}{k} \\\boldsymbol{z}{k, j}=h\left(\boldsymbol{x}{k},\boldsymbol{y}{j}\right)+\boldsymbol{v}{k, j}\end{array}\right. {xk=f(xk−1,uk)+wkzk,j=h(xk,yj)+vk,j
相机的位姿变量 x k \boldsymbol{x}{k} xk可以由 T k ∈ S E ( 3 ) \boldsymbol T{k} \in \mathrm{SE}(3) Tk∈SE(3)表达，观测方程即针孔相机模型，假设在 x k \boldsymbol{x}{k} xk处对路标 y j \boldsymbol{y}{j} yj进行了一次观测，对应到图像上的像素位置 z k , j \boldsymbol{z}_{k,j} zk,j，那么，观测方程可以表示成
s z k , j = K ( R k y j + t k ) s\boldsymbol{z}{k,j}=\boldsymbol K(\boldsymbol R{k} \boldsymbol y_{j}+\boldsymbol t_{k} ) szk,j=K(Rkyj+tk)
其中 K \boldsymbol K K为相机内参， s s s为像素点的距离。
在运动和观测方程中，我们通常假设两个噪声项 w k , v k , j \boldsymbol{w}{k},\boldsymbol{v}{k, j} wk,vk,j满足零均值的高斯分布 ，在这些噪声的影响下，我们希望通过带噪声的数据 z \boldsymbol z z和 u \boldsymbol u u推断位姿 x \boldsymbol x x和地图 y \boldsymbol y y（以及它们的概率分布），这构成了一个状态估计问题。
处理这个状态估计问题的方法大致分成两种。
增量方法与批量方法：
- 增量方法 ：是一种实时处理数据的技术，在SLAM中通过持续更新当前状态估计，逐步融合新数据。这种方法通常使用滤波器（如扩展卡尔曼滤波器EKF）来递归地估计系统状态。
- 批量方法：是一种非实时的处理技术，将数据积累到一定程度后一次性进行处理。这种方法通过对所有观测数据进行全局优化来求解SLAM问题。
大体来说，增量方法仅关心当前时刻 的状态估计 x k x_k xk，而对之前的状态则不多考虑；相对地，批量方法可以在更大的范围达到最优化，被认为优于传统的滤波器，而成为当前视觉SLAM的主流方法。
在SLAM中，为实现实时性，通常不采用SfM那样将所有数据集中处理的非实时方法。相反，SLAM中采用折中策略，如滑动窗口估计法，通过固定部分历史轨迹，仅对当前时刻附近的轨迹进行优化，从而在保证计算效率的同时维持较高的估计精度。
本讲我们重点介绍以非线性优化为主 的批量优化方法，考虑从1到 N N N的所有时刻，并假设有 M M M个路标点。定义所有时刻的机器人位姿和路标点坐标为：
x = { x 1 , ... , x N } ， y = { y 1 , ... , y M } x=\left \{ x_{1},\dots ,x_{N} \right \} ，y=\left \{ y_{1},\dots ,y_{M} \right \} x={x1,...,xN}，y={y1,...,yM}
对机器人的状态估计，从概率学的观点来看，就是已知输入数据 u \boldsymbol u u和观测数据 z \boldsymbol z z的条件下，求状态 x , y \boldsymbol x,\boldsymbol y x,y的条件概率分布：
P ( x , y ∣ z , u ) P\left ( \boldsymbol x,\boldsymbol y \mid \boldsymbol z,\boldsymbol u \right ) P(x,y∣z,u)
特别地，当我们不知道控制输入 ，只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计 P ( x , y ∣ z ) P\left ( \boldsymbol x,\boldsymbol y \mid \boldsymbol z \right ) P(x,y∣z)的条件概率分布，此问题也称为Structure from Motion（SfM），即如何从许多图像中重建三维空间结构。
为了估计状态变量的条件分布，利用贝叶斯法则，有：
P ( x , y ∣ z , u ) = P ( z , u ∣ x , y ) P ( x , y ) P ( z , u ) ∝ P ( z , u ∣ x , y ) ⏟ 似然 P ( x , y ) ⏟ 先验 P(\boldsymbol{x}, \boldsymbol{y} \mid \boldsymbol{z}, \boldsymbol{u})=\frac{P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y}) P(\boldsymbol{x}, \boldsymbol{y})}{P(\boldsymbol{z}, \boldsymbol{u})} \propto \underbrace{P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y})}{\text {似然 }} \underbrace{P(\boldsymbol{x}, \boldsymbol{y})}{\text {先验 }} P(x,y∣z,u)=P(z,u)P(z,u∣x,y)P(x,y)∝似然 P(z,u∣x,y)先验 P(x,y)
贝叶斯法则左侧称为后验概率 ，右侧的 P ( z ∣ x ) P\left ( \boldsymbol z\mid \boldsymbol x \right ) P(z∣x)称为似然 （Likehood），另一部分 P ( x ) P\left ( \boldsymbol x \right ) P(x)称为先验 （Prior）。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化（MaximizeaPosterior，MAP），则是可行的：
( x , y ) M A P ∗ = arg max P ( x , y ∣ z , u ) = arg max P ( z , u ∣ x , y ) P ( x , y ) (\boldsymbol{x}, \boldsymbol{y})_{\mathrm{MAP}}^{*}=\arg \max P(\boldsymbol{x}, \boldsymbol{y} \mid \boldsymbol{z}, \boldsymbol{u})=\arg \max P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y}) P(\boldsymbol{x}, \boldsymbol{y}) (x,y)MAP∗=argmaxP(x,y∣z,u)=argmaxP(z,u∣x,y)P(x,y)
注意贝叶斯法则的分母与状态 x , y \boldsymbol{x}, \boldsymbol{y} x,y无关，因此在最大后验概率估计（MAP）中可以忽略，求解MAP等价于最大化似然和先验的乘积。当先验信息缺失时，MAP估计简化为最大似然估计（MLE）。
( x , y ) M L E ∗ = arg max P ( z , u ∣ x , y ) (\boldsymbol{x}, \boldsymbol{y})_{\mathrm{MLE}}^{*}=\arg \max P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y}) (x,y)MLE∗=argmaxP(z,u∣x,y)
最小二乘的引出
那么如何求最大似然估计呢？我们说，在高斯分布的假设下，最大似然能够有较简单的形式。回顾观测模型，对于某一次观测：
z k , j = h ( x k , y j ) + v k , j \boldsymbol{z}{k, j}=h\left(\boldsymbol{x}{k},\boldsymbol{y}{j}\right)+\boldsymbol{v}{k, j} zk,j=h(xk,yj)+vk,j
由于我们假设了噪声项 v k ∼ N ( 0 , Q k , j ) \boldsymbol{v}{k} \sim \mathcal{N}\left(\mathbf{0}, \boldsymbol{Q}{k, j}\right) vk∼N(0,Qk,j)，所以观测数据的条件概率为：
P ( z k , j ∣ x k , y j ) = N ( h ( x k , y j ) , Q k , j ) P\left( \boldsymbol{z}{k, j} \mid \boldsymbol{x}{k},\boldsymbol{y}{j}\right)=\mathcal{N}\left(h\left(\boldsymbol{x}{k}, \boldsymbol{y}{j}\right),\boldsymbol{Q}{k, j} \right) P(zk,j∣xk,yj)=N(h(xk,yj),Qk,j)
考虑单次观测 的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然。
考虑任意高维高斯分布 x ∼ N ( μ , Σ ) \boldsymbol{x} \sim \mathcal{N}\left(\mathbf{\mu }, \boldsymbol{\Sigma }\right) x∼N(μ,Σ)，它的概率密度函数展开形式为：
P ( x ) = 1 ( 2 π ) N det ( Σ ) exp ( − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) P(\boldsymbol{x})=\frac{1}{\sqrt{(2 \pi)^{N} \operatorname{det}(\boldsymbol{\Sigma})}} \exp \left(-\frac{1}{2}(\boldsymbol{x}-\boldsymbol{\mu})^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}(\boldsymbol{x}-\boldsymbol{\mu})\right) P(x)=(2π)Ndet(Σ) 1exp(−21(x−μ)TΣ−1(x−μ))
对其取负对数，则变为：
− ln ( P ( x ) ) = 1 2 ln ( ( 2 π ) N det ( Σ ) ) + 1 2 ( x − μ ) T Σ − 1 ( x − μ ) -\ln (P(\boldsymbol{x}))=\frac{1}{2} \ln \left((2 \pi)^{N} \operatorname{det}(\boldsymbol{\Sigma})\right)+\frac{1}{2}(\boldsymbol{x}-\boldsymbol{\mu})^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}(\boldsymbol{x}-\boldsymbol{\mu}) −ln(P(x))=21ln((2π)Ndet(Σ))+21(x−μ)TΣ−1(x−μ)
因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化 。在最小化上式的 x \boldsymbol{x} x时，第一项与 x \boldsymbol{x} x无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。代入SLAM的观测模型，相当于在求：
( x k , y j ) ∗ = arg max N ( h ( x k , y j ) , Q k , j ) = arg min ( ( z k , j − h ( x k , y j ) ) T Q k , j − 1 ( z k , j − h ( x k , y j ) ) ) \begin{aligned}\left(\boldsymbol{x}{k}, \boldsymbol{y}{j}\right)^{*} & =\arg \max \mathcal{N}\left(h\left( \boldsymbol{x}{k},\boldsymbol{y}{j}\right ), \boldsymbol{Q}{k, j}\right) \\& =\arg \min \left(\left(\boldsymbol{z}{k, j}-h\left(\boldsymbol{x}{k}, \boldsymbol{y}{j}\right)\right)^{\mathrm{T}} \boldsymbol{Q}{k, j}^{-1}\left(\boldsymbol{z}{k, j}-h\left(\boldsymbol{x}{k}, \boldsymbol{y}{j}\right)\right)\right)\end{aligned} (xk,yj)∗=argmaxN(h(xk,yj),Qk,j)=argmin((zk,j−h(xk,yj))TQk,j−1(zk,j−h(xk,yj)))
该式等价于最小化误差的二次型，称为马哈拉诺比斯距离 ，即加权的欧氏距离，其中权重由信息矩阵 Q k , j − 1 \boldsymbol{Q}_{k, j}^{-1} Qk,j−1（高斯分布协方差矩阵的逆）确定。
在批量处理数据时，假设各时刻的输入和观测相互独立，这使得联合分布可以因式分解为独立分布的乘积：
P ( z , u ∣ x , y ) = ∏ k P ( u k ∣ x k − 1 , x k ) ∏ k , j P ( z k , j ∣ x k , y j ) P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y})=\prod_{k} P\left(\boldsymbol{u}{k} \mid \boldsymbol{x}{k-1}, \boldsymbol{x}{k}\right) \prod{k, j} P\left(\boldsymbol{z}{k, j} \mid \boldsymbol{x}{k}, \boldsymbol{y}_{j}\right) P(z,u∣x,y)=∏kP(uk∣xk−1,xk)∏k,jP(zk,j∣xk,yj)
这说明我们可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差：
e u k = x k − f ( x k − 1 , u k ) e z k , j = z k , j − h ( x k , y j ) \begin{array}{l}\boldsymbol{e}{u_k}=\boldsymbol{x}{k}-f\left(\boldsymbol{x}{k-1}, \boldsymbol{u}{k}\right) \\\boldsymbol{e}{z {k,j}}=\boldsymbol{z}{k, j}-h\left(\boldsymbol{x}{k}, \boldsymbol{y}_{j}\right)\end{array} euk=xk−f(xk−1,uk)ezk,j=zk,j−h(xk,yj)
那么，最小化所有时刻估计值与真实读数之间马氏距离，等价于求最大似然估计。负对数允许我们把乘积变成求和：
min J ( x , y ) = min ( ∑ k e u k T R k − 1 e u k + ∑ k ∑ j e z k , j T Q k , j − 1 e z k , j ) \min J(\boldsymbol{x}, \boldsymbol{y})=\min (\sum_{k} \boldsymbol{e}{u k}^{\mathrm{T}} \boldsymbol{R}{k}^{-1} \boldsymbol{e}{u_k}+\sum_{k} \sum_{j} \boldsymbol{e}{z{k, j}}^{\mathrm{T}} \boldsymbol{Q}{k, j}^{-1} \boldsymbol{e}{z_ {k, j}}) minJ(x,y)=min(∑keukTRk−1euk+∑k∑jezk,jTQk,j−1ezk,j)
这样就得到了一个最小二乘问题，它的解等价于状态的最大似然估计。
如果使用李代数表示增量，则该问题是无约束的最小二乘问题，接下来需要讨论无约束非线性最小二乘问题的求解方法。
非线性最小二乘
先来考虑一个简单的最小二乘问题：
min x F ( x ) = 1 2 ∥ f ( x ) ∥ 2 2 \min {\boldsymbol{x}} F(\boldsymbol{x})=\frac{1}{2}\|f(\boldsymbol{x})\|{2}^{2} minxF(x)=21∥f(x)∥22
其中，自变量 x ∈ R n \boldsymbol{x}\in \mathbf{R} ^{n} x∈Rn， f f f是任意标量非线性函数 f ( x ) : R n ⟼ R f(\boldsymbol{x}):\mathbf{R} ^{n}\longmapsto \mathbf{R} f(x):Rn⟼R。
下面讨论如何求解这样一个优化问题。显然，如果 f f f是个数学形式上很简单的函数，那么该问题可以用解析形式来求。令目标函数的导数为零，然后求解 x \boldsymbol{x} x的最优值，就和求二元函数的极值一样：
d F d x = 0 \frac{\mathrm{d} F}{\mathrm{d} x} =0 dxdF=0
如果 f f f为简单的线性函数，那么这个问题就是简单的线性最小二乘问题，但是有些导函数可能形式复杂，使得该方程可能不容易求解。求解这个方程需要我们知道关于目标函数的全局性质 ，而通常这是不大可能的。对于不方便直接求解的最小二乘问题，我们可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。具体步骤可列写如下：
- 给定某个初始值 x 0 \boldsymbol{x}_{0} x0。
- 对于第 k k k次迭代，寻找一个增量 Δ x k \Delta \boldsymbol{x}{k} Δxk，使得 ∥ f ( x k + Δ x k ) ∥ 2 2 \|f(\boldsymbol{x}{k}+\Delta \boldsymbol{x}{k})\|{2}^{2} ∥f(xk+Δxk)∥22达到极小值。
- 若 Δ x k \Delta \boldsymbol{x}_{k} Δxk足够小，则停止。
- 否则，令 x k + 1 = x k + Δ x k \boldsymbol{x}{k+1}=\boldsymbol{x}{k}+\Delta \boldsymbol{x}_{k} xk+1=xk+Δxk，返回第2步。
这让求解导函数为零 的问题变成了一个不断寻找下降增量 Δ x k \Delta \boldsymbol{x}_{k} Δxk的问题。
接下来我们考察如何寻找这个增量 Δ x k \Delta \boldsymbol{x}_{k} Δxk。
一阶和二阶梯度法
现在考虑第 k k k次迭代，假设我们在 x k \boldsymbol{x}{k} xk处，想要寻到增量 Δ x k \Delta \boldsymbol{x}{k} Δxk，那么最直观的方式是将目标函数在 x k \boldsymbol{x}_{k} xk附近进行泰勒一阶展开：
F ( x k + Δ x k ) ≈ F ( x k ) + J ( x k ) T Δ x k F\left(\boldsymbol{x}{k}+\Delta \boldsymbol{x}{k}\right) \approx F\left(\boldsymbol{x}{k}\right)+\boldsymbol{J}\left(\boldsymbol{x}{k}\right)^{\mathrm{T}} \Delta \boldsymbol{x}_{k} F(xk+Δxk)≈F(xk)+J(xk)TΔxk
( Δ x k ) ∗ = arg min ( F ( x k ) + J ( x k ) T Δ x k ) {(\Delta \boldsymbol{x}{k})}^{*}=\arg \min ( F\left(\boldsymbol{x}{k}\right)+\boldsymbol{J}\left(\boldsymbol{x}{k}\right)^{\mathrm{T}} \Delta \boldsymbol{x}{k}) (Δxk)∗=argmin(F(xk)+J(xk)TΔxk)
( Δ x k ) ∗ = − J ( x k ) {(\Delta \boldsymbol{x}{k})}^{*}=-\boldsymbol{J}\left(\boldsymbol{x}{k}\right) (Δxk)∗=−J(xk)
最速下降法 ，它的直观意义非常简单，只要我们沿着反向梯度方向前进，步长为 λ \lambda λ，在一阶（线性）的近似下，目标函数必定会下降。
将目标函数在 x k \boldsymbol{x}_{k} xk附近进行泰勒二阶展开：
F ( x k + Δ x k ) ≈ F ( x k ) + J ( x k ) T Δ x k + 1 2 Δ x k T H ( x k ) Δ x k F\left(\boldsymbol{x}{k}+\Delta \boldsymbol{x}{k}\right) \approx F\left(\boldsymbol{x}{k}\right)+\boldsymbol{J}\left(\boldsymbol{x}{k}\right)^{\mathrm{T}} \Delta \boldsymbol{x}{k}+\frac{1}{2} \Delta \boldsymbol{x}{k}^{\mathrm{T}} \boldsymbol{H}\left(\boldsymbol{x}{k}\right) \Delta \boldsymbol{x}{k} F(xk+Δxk)≈F(xk)+J(xk)TΔxk+21ΔxkTH(xk)Δxk
记， L ( Δ x k ) = F ( x k ) + J ( x k ) T Δ x k + 1 2 Δ x k T H ( x k ) Δ x k L{(\Delta \boldsymbol{x}{k})}=F\left(\boldsymbol{x}{k}\right)+\boldsymbol{J}\left(\boldsymbol{x}{k}\right)^{\mathrm{T}} \Delta \boldsymbol{x}{k}+\frac{1}{2} \Delta \boldsymbol{x}{k}^{\mathrm{T}} \boldsymbol{H}\left(\boldsymbol{x}{k}\right) \Delta \boldsymbol{x}_{k} L(Δxk)=F(xk)+J(xk)TΔxk+21ΔxkTH(xk)Δxk
( Δ x k ) ∗ = arg min ( L ( Δ x k ) ) {(\Delta \boldsymbol{x}{k})}^{*}=\arg \min(L{(\Delta \boldsymbol{x}{k})}) (Δxk)∗=argmin(L(Δxk))
d L ( Δ x k ) d Δ x k = J ( x k ) + H ( x k ) Δ x k = 0 \frac{\mathrm{d} L{(\Delta \boldsymbol{x}{k})}}{\mathrm{d} \Delta \boldsymbol{x}{k}} =\boldsymbol{J}\left(\boldsymbol{x}{k}\right)+ \boldsymbol{H}\left(\boldsymbol{x}{k}\right) \Delta \boldsymbol{x}_{k}=0 dΔxkdL(Δxk)=J(xk)+H(xk)Δxk=0
求解这个线性方程，就得到了增量。该方法又称为牛顿法。
最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。而牛顿法则需要计算目标函数的 H \boldsymbol{H} H矩阵，这在问题规模较大时非常困难，我们通常倾向于避免 H \boldsymbol{H} H的计算。对于一般的问题，一些拟牛顿法可以得到较好的结果，而对于最小二乘问题，还有几类更加实用的方法：高斯牛顿法 和列文伯格---马夸尔特方法。
高斯牛顿法
高斯牛顿法的思想是将 f ( x ) f(\boldsymbol{x}) f(x)进行一阶的泰勒展开，注意这里不是目标函数 F ( x ) F(\boldsymbol{x}) F(x)而是 f ( x ) f(\boldsymbol{x}) f(x)。
f ( x + Δ x ) ≈ f ( x ) + J ( x ) T Δ x f\left(\boldsymbol{x}+\Delta \boldsymbol{x}\right) \approx f\left(\boldsymbol{x}\right)+\boldsymbol{J}\left(\boldsymbol{x}\right)^{\mathrm{T}} \Delta \boldsymbol{x} f(x+Δx)≈f(x)+J(x)TΔx
Δ x ∗ = arg min Δ x 1 2 ∥ f ( x ) + J ( x ) T Δ x ∥ 2 \Delta \boldsymbol{x}^{*}=\arg \min _{\Delta \boldsymbol{x}} \frac{1}{2}\left\|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right\|^{2} Δx∗=argminΔx21 f(x)+J(x)TΔx 2
1 2 ∥ f ( x ) + J ( x ) T Δ x ∥ 2 = 1 2 ( f ( x ) + J ( x ) T Δ x ) T ( f ( x ) + J ( x ) T Δ x ) = 1 2 ( ∥ f ( x ) ∥ 2 2 + 2 f ( x ) J ( x ) T Δ x + Δ x T J ( x ) J ( x ) T Δ x ) \begin{aligned} \frac{1}{2}\left\|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right\|^2 & =\frac{1}{2}\left(f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right)^{\mathrm{T}}\left(f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right) \\ & =\frac{1}{2}\left(\|f(\boldsymbol{x})\|_2^2+2 f(\boldsymbol{x}) \boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}+\Delta \boldsymbol{x}^{\mathrm{T}} \boldsymbol{J}(\boldsymbol{x}) \boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right)\end{aligned} 21 f(x)+J(x)TΔx 2=21(f(x)+J(x)TΔx)T(f(x)+J(x)TΔx)=21(∥f(x)∥22+2f(x)J(x)TΔx+ΔxTJ(x)J(x)TΔx)
求上式关于 Δ x \Delta \boldsymbol{x} Δx的导数，并令其为零：
J ( x ) f ( x ) + J ( x ) J T ( x ) Δ x = 0 \boldsymbol{J}(\boldsymbol{x}) f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \boldsymbol{J}^{\mathrm{T}}(\boldsymbol{x}) \Delta \boldsymbol{x}=\mathbf{0} J(x)f(x)+J(x)JT(x)Δx=0
可以得到如下方程组：
J ( x ) J T ⏟ H ( x ) ( x ) Δ x = − J ( x ) f ( x ) ⏟ g ( x ) \underbrace{\boldsymbol{J}(\boldsymbol{x}) \boldsymbol{J}^{\mathrm{T}}}{\boldsymbol{H}(\boldsymbol{x})}(\boldsymbol{x}) \Delta \boldsymbol{x}=\underbrace{-\boldsymbol{J}(\boldsymbol{x}) f(\boldsymbol{x})}{\boldsymbol{g}(\boldsymbol{x})} H(x) J(x)JT(x)Δx=g(x) −J(x)f(x)
这个方程是关于变量 Δ x \Delta \boldsymbol{x} Δx的线性方程组，我们称它为增量方程 ，也可以称为高斯牛顿方程或者正规方程 。我们把左边的系数定义为 H \boldsymbol{H} H，右边定义为 g \boldsymbol{g} g，那么上式变为：
H Δ x = g \boldsymbol{H} \Delta \boldsymbol{x}=\boldsymbol{g} HΔx=g
这里把左侧记作 H \boldsymbol{H} H是有意义的。对比牛顿法可见，高斯牛顿法用 J J T \boldsymbol{J}\boldsymbol{J}^T JJT作为牛顿法中二阶Hessian矩阵的近似 ，从而省略了计算 H \boldsymbol{H} H的过程。求解增量方程是整个优化问题的核心所在。如果我们能够顺利解出该方程，那么高斯牛顿法的算法步骤可以写成：
-
给定初始值 x 0 \boldsymbol{x}_0 x0。
-
对于第 k k k次迭代，求出当前的雅可比矩阵 J ( x k ) \boldsymbol{J}(\boldsymbol{x}_k) J(xk)和误差 f ( x k ) f(\boldsymbol{x}_k) f(xk)。
-
求解增量方程： H Δ x k = g \boldsymbol{H} \Delta \boldsymbol{x}_k=\boldsymbol{g} HΔxk=g。
-
若 Δ x k \Delta \boldsymbol{x}k Δxk足够小，则停止。否则，令 x k + 1 = x k + Δ x k \boldsymbol{x}{k+1}=\boldsymbol{x}_{k}+\Delta \boldsymbol{x}_k xk+1=xk+Δxk，返回第2步。
增量方程的求解在算法中至关重要，但在高斯牛顿法中，若矩阵 H \boldsymbol{H} H仅为半正定 或出现奇异、病态 情况，增量的稳定性可能较差，导致算法不收敛。此外，过大的步长 可能使局部近似失效 ，甚至无法保证迭代收敛。尽管存在这些问题，高斯牛顿法仍然是非线性优化中一种简单且有效的方法，许多算法可以视为其变种。这些算法基于高斯牛顿法的思想，并通过改进，例如一些线搜索方法 加入了一个步长 α \alpha α，在确定了 Δ x \Delta\boldsymbol{x} Δx后进一步找到 α \alpha α使得 ∥ f ( x ) + α Δ x ∥ 2 \parallel f(\boldsymbol{x})+\alpha \Delta \boldsymbol{x} \parallel ^{2} ∥f(x)+αΔx∥2达到最小，而不是简单地令 α = 1 \alpha=1 α=1。
列文伯格---马夸尔特方法
高斯牛顿法只能在展开点附近具有良好的近似效果，因此引入了信赖区域 来限制优化的范围，确保近似的有效性。这类方法称为信赖区域方法 。信赖区域的范围根据近似模型与实际函数之间的差异来动态调整：若差异小则扩大信赖区域，若差异大则缩小区域。通过定义一个指标 ρ \rho ρ 来量化近似的好坏程度：
ρ = f ( x + Δ x ) − f ( x ) J ( x ) T Δ x \rho=\frac{f(\boldsymbol{x}+\Delta \boldsymbol{x})-f(\boldsymbol{x})}{\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}} ρ=J(x)TΔxf(x+Δx)−f(x)
ρ \rho ρ的分子是实际函数下降的值，分母是近似模型下降的值。如果 ρ \rho ρ接近于1，则近似是好的。如果 ρ \rho ρ太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果 ρ \rho ρ比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。
于是，我们构建一个改良版的非线性优化框架，该框架会比高斯牛顿法有更好的效果：
-
给定初始值 x 0 \boldsymbol{x}_0 x0，以及初始优化半径 μ \mu μ。
-
对于第 k k k次迭代，在高斯牛顿法的基础上加上信赖区域，求解：
min Δ x k 1 2 ∥ f ( x k ) + J ( x k ) T Δ x k ∥ 2 , \min _{\Delta \boldsymbol{x}_k} \frac{1}{2}\left\|f\left(\boldsymbol{x}_k\right)+\boldsymbol{J}\left(\boldsymbol{x}_k\right)^{\mathrm{T}} \Delta \boldsymbol{x}_k\right\|^2, \quad minΔxk21 f(xk)+J(xk)TΔxk 2, s.t. ∥ D Δ x k ∥ 2 ⩽ μ \quad\left\|\boldsymbol{D} \Delta \boldsymbol{x}_k\right\|^2 \leqslant \mu ∥DΔxk∥2⩽μ其中 μ \mu μ是信赖区域的半径， D \boldsymbol{D} D为系数矩阵，将在后文说明。
-
计算 ρ \rho ρ。
-
若 ρ > 3 4 \rho>\frac{3}{4} ρ>43，则设置 μ = 2 μ \mu=2\mu μ=2μ
-
若 ρ < 1 4 \rho<\frac{1}{4} ρ<41，则设置 μ = 0.5 μ \mu=0.5\mu μ=0.5μ
-
如果 ρ \rho ρ大于某阈值，则认为近似可行。令 x k + 1 = x k + Δ x k \boldsymbol{x}{k+1}=\boldsymbol{x}{k}+\Delta \boldsymbol{x}_k xk+1=xk+Δxk。
-
判断算法是否收敛。如不收敛则返回第2步，否则结束。
带不等式约束的优化问题，我们用拉格朗日乘子把约束项放到目标函数中，构成拉格朗日函数：
L ( Δ x k , λ ) = 1 2 ∥ f ( x k ) + J ( x k ) T Δ x k ∥ 2 + λ 2 ( ∥ D Δ x k ∥ 2 − μ ) \mathcal{L}\left(\Delta \boldsymbol{x}_k, \lambda\right)=\frac{1}{2}\left\|f\left(\boldsymbol{x}_k\right)+\boldsymbol{J}\left(\boldsymbol{x}_k\right)^{\mathrm{T}} \Delta \boldsymbol{x}_k\right\|^2+\frac{\lambda}{2}\left(\left\|\boldsymbol{D} \Delta \boldsymbol{x}_k\right\|^2-\mu\right) L(Δxk,λ)=21 f(xk)+J(xk)TΔxk 2+2λ(∥DΔxk∥2−μ)
这里 λ \lambda λ为拉格朗日乘子。类似于高斯牛顿法中的做法，令该拉格朗日函数关于 Δ x \Delta\boldsymbol{x} Δx的导数为零，它的核心仍是计算增量的线性方程：
( H + λ D T D ) Δ x k = g \left(\boldsymbol{H}+\lambda \boldsymbol{D}^{\mathrm{T}} \boldsymbol{D}\right) \Delta \boldsymbol{x}_k=\boldsymbol{g} (H+λDTD)Δxk=g
列文伯格---马夸尔特方法的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定、更准确的增量 Δ x \Delta\boldsymbol{x} Δx。
实际问题中，我们通常选择高斯牛顿法或列文伯格---马夸尔特方法其中之一作为梯度下降策略。当问题性质较好时，用高斯牛顿。如果问题接近病态，则用列文伯格---马夸尔特方法。
小结
非线性优化算法，如高斯牛顿法和列文伯格---马夸尔特方法，需要合理的初始值，因复杂的目标函数易导致迭代陷入局部极小值。一个科学的初始值对优化至关重要，如在视觉SLAM中，常用ICP或PnP算法提供初始估计。此外，线性增量方程组的求解通常采用数值方法，如QR分解、Cholesky分解，而不是直接求逆，尤其在处理大规模问题时，这种方法更为高效。