Loading [MathJax]/extensions/tex2jax.js
RMVL  
Robotic Manipulation and Vision Library
全部  命名空间 文件 函数 变量 类型定义 枚举 枚举值 宏定义  
非线性最小二乘

涉及 Gauss-Newton 迭代LM 非线性最小二乘求解算法

作者
赵曦
日期
2024/05/24
版本
1.0

上一篇教程:最小二乘法
下一篇教程:非线性方程(组)数值解与迭代法


\[\def\red#1{\color{red}{#1}} \def\teal#1{\color{teal}{#1}} \def\green#1{\color{green}{#1}} \def\transparent#1{\color{transparent}{#1}} \def\orange#1{\color{orange}{#1}} \def\Var{\mathrm{Var}} \def\Cov{\mathrm{Cov}} \def\tr{\mathrm{tr}} \def\fml#1{\text{(#1)}} \def\ptl#1#2{\frac{\partial#1}{\partial#2}} \]

1. Gauss-Newton 迭代

1.1 算法原理

数据处理中,最常见的一种函数形式是

\[f(\boldsymbol x)=\frac12\sum_{i=1}^m\varphi_i^2(\boldsymbol x)\tag{1-1}\]

如果 \(f(x)\) 的极小点 \(x^*\) 满足 \(f(x^*)< e\)( \(e\) 是预先给定的精度),那么可以认为 \(x^*\) 是 方程组

\[\varphi_i(x_1,x_2,\cdots,x_n)=0,\quad i=1,2,\cdots,m(m\ge n)\]

的解。

对式 \(\fml{1-1}\),可以用一阶导数运算来代替牛顿法中的二阶导数矩阵的求逆运算。因为

\[\begin{align}\nabla f(\boldsymbol x)&=\begin{bmatrix} \sum\limits_{i=1}^m\ptl{\varphi_i(\boldsymbol x)}{x_1}\varphi_i(\boldsymbol x)\\ \sum\limits_{i=1}^m\ptl{\varphi_i(\boldsymbol x)}{x_2}\varphi_i(\boldsymbol x)\\ \vdots\\\sum\limits_{i=1}^m\ptl{\varphi_i(\boldsymbol x)}{x_n}\varphi_i(\boldsymbol x)\end{bmatrix}=\begin{bmatrix} \ptl{\varphi_1(\boldsymbol x)}{x_1}&\ptl{\varphi_2(\boldsymbol x)}{x_1}&\cdots&\ptl{\varphi_m(\boldsymbol x)}{x_1}\\ \ptl{\varphi_1(\boldsymbol x)}{x_2}&\ptl{\varphi_2(\boldsymbol x)}{x_2}&\cdots&\ptl{\varphi_m(\boldsymbol x)}{x_2}\\ \vdots&\vdots&\ddots&\vdots\\ \ptl{\varphi_1(\boldsymbol x)}{x_n}&\ptl{\varphi_2(\boldsymbol x)}{x_n}&\cdots&\ptl{\varphi_m(\boldsymbol x)}{x_n} \end{bmatrix}\begin{bmatrix} \varphi_1(\boldsymbol x)\\\varphi_2(\boldsymbol x)\\\vdots\\\varphi_m(\boldsymbol x)\end{bmatrix}\\ &=\boldsymbol J^T(\boldsymbol x)\boldsymbol\varphi(\boldsymbol x)\tag{1-2}\end{align}\]

式中, \(\boldsymbol\varphi(\boldsymbol x)\) 为 \(m\) 维的函数向量,即 \(\boldsymbol\varphi(\boldsymbol x)=[\varphi_1(x),\varphi_2(x),\cdots,\varphi_m(x)]^T\); \(\boldsymbol J(\boldsymbol x)\) 为函数 \(\boldsymbol\varphi(\boldsymbol x)\ (i=1,2,\cdots,m)\) 一阶偏导数组成的矩阵,即

\[\boldsymbol J(\boldsymbol x)=\begin{bmatrix} \ptl{\varphi_1(\boldsymbol x)}{x_1}&\ptl{\varphi_1(\boldsymbol x)}{x_2}&\cdots&\ptl{\varphi_1(\boldsymbol x)}{x_n}\\ \ptl{\varphi_2(\boldsymbol x)}{x_1}&\ptl{\varphi_2(\boldsymbol x)}{x_2}&\cdots&\ptl{\varphi_2(\boldsymbol x)}{x_n}\\ \vdots&\vdots&\ddots&\vdots\\ \ptl{\varphi_m(\boldsymbol x)}{x_1}&\ptl{\varphi_m(\boldsymbol x)}{x_2}&\cdots&\ptl{\varphi_m(\boldsymbol x)}{x_n} \end{bmatrix}\tag{1-3}\]

特别当 \(\boldsymbol\varphi(\boldsymbol x)\) 是 \(\boldsymbol x\) 的线性函数时,式 \(\fml{1-2}\) 中的 \(\boldsymbol J(\boldsymbol x)\) 是常系数矩阵。这时式 \(\fml{1-1}\) 的海赛矩阵可以写成

\[\boldsymbol H=\boldsymbol J^T\boldsymbol J\tag{1-4}\]

这样,关于 \(\boldsymbol\varphi(\boldsymbol x)\) 是 \(\boldsymbol x\) 的线性函数时的最小二乘法的迭代公式可以写为

\[\boldsymbol x_{k+1}=\boldsymbol x_k-\boldsymbol H^{-1}(\boldsymbol x_k)\boldsymbol J^T(\boldsymbol x_k)\boldsymbol\varphi(\boldsymbol x_k)\tag{1-5}\]

当 \(\boldsymbol\varphi(\boldsymbol x)\) 不是 \(\boldsymbol x\) 的线性函数时,也可以近似将式 \(\fml{1-4}\) 视为函数 \(f(\boldsymbol x)\) 的海赛矩阵,即

\[\boldsymbol H\approx\left[\boldsymbol J(\boldsymbol x_k)\right]^T\boldsymbol J(\boldsymbol x_k)\tag{1-6}\]

所以,关于 \(\boldsymbol\varphi(\boldsymbol x)\) 不是 \(\boldsymbol x\) 的线性函数时的最小二乘法的迭代公式也可以写为 \(\fml{1-5}\) 的形式。

1.2 迭代步骤

  1. 选择一初始点 \(\boldsymbol x_0=(x_{1,0},x_{2,0},\cdots,x_{n,0})^T\);
  2. 算出

    \[\Delta\boldsymbol x_0=-\boldsymbol H_0^{-1}\boldsymbol J_0^T\boldsymbol\varphi(\boldsymbol x_k)\tag{1-6}\]

  3. 令 \(\boldsymbol x_1\) 为函数 \(f(\boldsymbol x)\) 的极小点的第 1 次近似,则有

    \[\boldsymbol x_1=\boldsymbol x_0+\Delta\boldsymbol x_0\tag{1-7}\]

  4. 以 \(\boldsymbol x_1\) 代替前面的 \(\boldsymbol x_0\), \(\Delta\boldsymbol x_1\) 代替 \(\Delta\boldsymbol x_0\),重复上述计算过程,直到

    \[\|\boldsymbol\varphi(\boldsymbol x_k)\|<\epsilon'\tag{1-8a}\]

    \[\|\nabla f(\boldsymbol x_k)\|<\epsilon''\tag{1-8b}\]

    为止。 \(\epsilon'\) 和 \(\epsilon''\) 是预先给定的精度。

1.3 改进

上述高斯-牛顿最小二乘法利用了目标函数在极小值处近似为自变量各分量的平方和的特点,用 \(\boldsymbol J^T\boldsymbol J\) 近似代替牛顿法中 \(f(\boldsymbol x)\) 的二阶导数矩阵,大大节省了计算量。但是它对初始点 \(\boldsymbol x_0\) 的要求比较严格,如果初始点 \(\boldsymbol x_0\) 与极小点 \(\boldsymbol x^*\) 相距很远,这个算法往往失败。原因是

  1. 上述算法基于线性逼近,但在 \(\boldsymbol x_0\) 远离极小点时,这种线性逼近无效;
  2. \(\boldsymbol J_0^T\boldsymbol J_0\) 的最大特征值与最小特征值的比很大,致使解 \(\Delta\boldsymbol x_0\) 变得无意义。

为此采取下述改进的办法。在求出 \(\boldsymbol x_k\) 的校正量 \(\Delta\boldsymbol x_k\) 后,不把 \(\boldsymbol x_k+\Delta\boldsymbol x_k\) 作为第 \(k+1\) 次近似点,而是将 \(\Delta\boldsymbol x_k\) 作为下一步的一维方向搜索。求 \(\alpha_k\),使

\[f(\boldsymbol x_k+\alpha_k\boldsymbol s_k)=\min_{\alpha>0}f(\boldsymbol x_k+\alpha\boldsymbol s_k)\]

然后令

\[\boldsymbol x_{k+1}=\boldsymbol x_k+\alpha_k\boldsymbol s_k\]

以 \(\boldsymbol x_{k+1}\) 代替 \(\boldsymbol x_k\) 重复上述计算过程,直到 \(\|\boldsymbol\varphi(\boldsymbol x_k)\|<\epsilon'\) 或 \(\|\nabla f(\boldsymbol x_k)\|<\epsilon"\) 为止。

1.4 如何使用

RMVL 提供了改进的 Gauss-Newton 迭代算法,可参考 rm::lsqnonlin 函数。例如,我们需要拟合一个正弦函数

\[y=A\sin(\omega t+\varphi_0)+b\]

其中, \(A,\omega,\varphi_0,b\) 是待拟合的参数,不妨统一写为 \(\boldsymbol x=(A,\omega,\varphi_0,b)\),也就是说我们需要拟合的函数是

\[\green y=x_1\sin(x_2\green t+x_3)+x_4\]

其中 \(t\) 和 \(y\) 是可以观测到的数据,我们需要通过观测的数据来拟合 \(\boldsymbol x\) 的值。比方说,下面的 obtain 函数就可以观测每一帧的数据。

double obtain();

例如经过了 20 帧的数据采集,我们得到了一个长度为 20 的队列,即

std::deque<double> datas;
/* code */
datas.push_front(obtain());
if (datas.size() == 20)
datas.pop_back();
/* code */

准备好数据后,可以使用下面的代码来拟合正弦函数。

std::vector<rm::FuncNd> lsq_sine(datas.size());
for (std::size_t i = 0; i < lsq_sine.size(); ++i)
lsq_sine[i] = [=](const std::valarray<double> &x) { return x[0] * std::sin(x[1] * i + x[2]) + x[3] - datas[i]; };
rm::FuncNds lsq_sine_f = [&](const std::valarray<double> &x) {
std::valarray<double> ret(lsq_sine.size());
for (std::size_t i = 0; i < lsq_sine.size(); ++i)
ret[i] = lsq_sine[i](x);
return ret;
};
// 拟合正弦函数,初始值为 (1, 0.02, 0, 1.09)
auto x = rm::lsqnonlin(lsq_sine_f, {1, 0.02, 0, 1.09}); // 默认采用 Gauss-Newton 算法
std::function< std::valarray< double >(const std::valarray< double > &)> FuncNds
多元函数组
定义 numcal.hpp:310
std::valarray< double > lsqnonlin(const FuncNds &func, const std::valarray< double > &x0, const OptimalOptions &options={})
非线性最小二乘求解,实现与 agarwal23 类似的算法

2. Levenberg–Marquardt 算法

2.1 算法原理

普通的 Gauss-Newton 迭代,在初始值附近做了一阶线性化处理,而当初始值与极小值相差较远时,曲线的非线性特性会导致迭代失败。为了解决这个问题,Levenberg–Marquardt 算法在 Gauss-Newton 迭代的基础上,引入了一个参数 \(\lambda\),使得迭代公式变为

\[\boldsymbol x_{k+1}=\boldsymbol x_k-\left(\boldsymbol J^T\boldsymbol J+\red{\lambda\boldsymbol I}\right)^{-1}\boldsymbol J^T\boldsymbol\varphi(\boldsymbol x_k)\tag{2-1}\]

\(I\) 是单位矩阵, \(\lambda\) 是一个非负数。如果 \(\lambda\) 取值较大时, \(\lambda I\) 占主要地位,此时的 LM 算法更接近一阶梯度下降法,说明此时距离最终解还比较远,用一阶近似更合适。反之,如果 \(\lambda\) 取值较小时, \(\boldsymbol H=\boldsymbol J^T\boldsymbol J\) 占主要地位,说明此时距离最终解距离较近,用二阶近似模型比较合适,可以避免梯度下降的震荡,容易快速收敛到极值点。因此参数 \(\lambda\) 不仅影响到迭代的方向还影响到迭代步长的大小。

令初值为 \(\boldsymbol x_0\),可以设置 \(\lambda\) 的初值 \(\lambda_0\) 为

\[\begin{align}\boldsymbol A_0&=\boldsymbol J^T(\boldsymbol x_0)\boldsymbol J(\boldsymbol x_0)\\ \lambda_0&=\tau\max_i\left\{a_{ii}^{(0)}\right\}\end{align}\tag{2-2}\]

其中, \(\tau\) 可以自己指定, \(a_{ii}\) 表示矩阵 \(\boldsymbol A\) 对角线元素。此外, \(\lambda\) 需要在迭代过程中不断调整,以保证迭代的收敛性。一般会判断近似的模型与实际函数之间的差异,可以使用下面的公式来判断

\[\rho_k=\frac{f(\boldsymbol x_k+\Delta\boldsymbol x_k)-f(\boldsymbol x_k)}{\boldsymbol J(\boldsymbol x_k)\Delta\boldsymbol x_k}\tag{2-3}\]

2.2 迭代步骤

  1. 选择一初始点 \(\boldsymbol x_0=(x_{1,0},x_{2,0},\cdots,x_{n,0})^T\),按照式 \(\fml{2-2}\) 计算 \(\lambda_0\);
  2. 对于第 \(k\) 次迭代,根据式 \(\fml{2-1}\) 计算 \(\Delta\boldsymbol x_k\),并计算 \(\rho_k\);
  3. 如果
    • \(\rho_k\le0.25\),应减小 \(\lambda_k\),即

      \[\lambda_{k+1}=\frac{\lambda_k}2\]

    • \(0.25<\rho_k\le0.75\),保持 \(\lambda_k\) 不变;
    • \(\rho_k>0.75\),增大 \(\lambda_k\),即

      \[\lambda_{k+1}=2\lambda_k\]

    • 如果 \(\rho_k\le0\),这时不应该更新 \(\boldsymbol x_k\),即

      \[\boldsymbol x_{k+1}=\boldsymbol x_k\]

      并且和上面 \(\rho_k\le0.25\) 的情况一样,减小 \(\lambda_k\),反之,在 \(\rho_k>0\) 的情况下,更新 \(\boldsymbol x_k\),即

      \[\boldsymbol x_{k+1}=\boldsymbol x_k\]

2.3 如何使用

还是上面的例子,我们可以使用下面的代码来拟合正弦函数。

// 拟合正弦函数,初始值为 (1, 0.02, 0, 1.09)
options.lsq_mode = rm::LsqMode::LM; // 使用 LM 算法
options.max_iter = 2000; // 最大迭代次数可以设置高一点,以保证收敛
auto x = rm::lsqnonlin(lsq_sine, {1, 0.02, 0, 1.09}, options);
@ LM
Levenberg-Marquardt 法 eade13 madsen04
定义 numcal.hpp:331
无约束多维函数优化选项
定义 numcal.hpp:336
int max_iter
最大迭代次数
定义 numcal.hpp:340
LsqMode lsq_mode
最小二乘求解模式,默认为改进的 Gauss-Newton 法 LsqMode::SGN
定义 numcal.hpp:339

3. Robust 核函数

3.1 加权与核函数

鲁棒核函数(Robust Kernel Function)是在优化问题中用来减少离群值(outliers)影响的一种技术。在 Bundle Adjustment (BA) 等计算机视觉问题中,鲁棒核函数特别有用,因为这些问题常常受到错误匹配、遮挡或其他因素导致的离群值影响

标准的最小二乘优化问题可以表示为:

\[\min_{\boldsymbol x}\sum_i\frac12\|e_i(\boldsymbol x)\|^2\tag{3-1}\]

其中 \(e_i(x)\) 是第 \(i\) 个观测的误差。引入鲁棒核函数后,优化问题变为:

\[\min_{\boldsymbol x}\sum_i\rho(e_i(\boldsymbol x))\tag{3-2}\]

其中 \(\rho(s)\) 是鲁棒核函数。鲁棒核函数主要有

  • 对小误差的敏感度与标准二次函数相似;
  • 对大误差(可能是离群值)的敏感度较低,减少了它们的影响。

的特点,常用的鲁棒核函数是 Huber 损失函数:

\[\rho(s)=\begin{cases}\frac12s^2&\quad|s|\leq k\\k(|s|-\frac12k)&\quad|s|>k\end{cases}\tag{3-3}\]

当 \(k=1\) 时,Huber 核函数的图像如下图所示。

图 3-1 Huber 核函数

Huber 核函数是一个连续可导的函数,它的优点是它在 \(s=0\) 附近是二次的,这使得它对小误差的敏感度与标准二次函数相似,而对大误差的敏感度较低,减少了它们的影响。对于 \(\fml{3-2}\) 式这一新的最优化目标函数,按照一般想法,求解其导数的零点,便能得到最优解。

\[\begin{align}f(\boldsymbol x)&=\sum_{i=1}^m\rho(e_i)\\\ptl{f(\boldsymbol x)}{\boldsymbol x}&=\sum_{i=1}^m\ptl{\rho(e_i)}{\boldsymbol x}=\sum_{i=1}^m\rho'(e_i)\ptl{e_i(\boldsymbol x)}{\boldsymbol x}\\&=\begin{bmatrix}\sum\limits_{i=1}^m\rho'(e_i)\ptl{e_i}{x_1}\\\sum\limits_{i=1}^m\rho'(e_i)\ptl{e_i}{x_2}\\\vdots\\\sum\limits_{i=1}^m\rho'(e_i)\ptl{e_i}{x_n}\end{bmatrix}=\boldsymbol J^T\begin{bmatrix}\rho'(e_1)\\\rho'(e_2)\\\vdots\\\rho'(e_m)\end{bmatrix}\stackrel{\triangle}{=}\boldsymbol J^T\boldsymbol\rho'=0\tag{3-4}\end{align}\]

对于加权最小二乘问题,目标函数形如

\[f(\boldsymbol x)=\frac12\sum_{i=1}^mw_ie_i^2(\boldsymbol x)\tag{3-5}\]

同样求解其导数的零点,能得到加权最小二乘问题的最优解。

\[\ptl{f(\boldsymbol x)}{\boldsymbol x}=\sum_{i=1}^mw_ie_i(\boldsymbol x)\ptl{e_i(\boldsymbol x)}{\boldsymbol x}=\boldsymbol J^T\begin{bmatrix}w_1e_1(\boldsymbol x)\\w_2e_2(\boldsymbol x)\\\vdots\\w_me_m(\boldsymbol x)\end{bmatrix}\stackrel{\triangle}{=}\boldsymbol J^T\boldsymbol{We}\tag{3-6}\]

其中 \(\boldsymbol W\) 是以 \(w_i\) 为对角元的对角矩阵。

对比 \(\fml{3-4}\) 式和 \(\fml{3-6}\) 式,我们希望 Huber 核函数的最优化问题能够转换为加权最小二乘问题,即

\[\begin{align}\boldsymbol{We}&=\boldsymbol\rho'\\w_ie_i(\boldsymbol x)&=\rho'(e_i)\quad i=1,2,\cdots,m\end{align}\tag{3-7}\]

因此 \(w_i\)可以定义为

\[w_i=\frac{\rho'(e_i)}{e_i}\tag{3-8}\]

这样,我们就可以将 Huber 核函数的最优化问题转换为加权最小二乘问题。

3.2 权值的计算

对于 Huber 损失函数,我们有

\[\rho'(e_i)=\begin{cases}e_i&|e_i|\leq k\\k\cdot\texttt{sgn}(e_i)&|e_i|>k\end{cases}\tag{3-9}\]

其中, \(\texttt{sgn}\)为符号函数,可参考 rm::sgn 。因此,权重 \(w_i\)为

\[w_i=\frac{\rho'(e_i)}{e_i}=\begin{cases}1&|e_i|\leq k\\\frac{k}{|e_i|}&|e_i|>k\end{cases}\tag{3-10}\]

这有比较明确的物理意义

  1. 减小离群点的影响
    • 离群点的残差 \(|e_i|\)较大,通过 \(w_i=\frac{k}{|e_i|}\)将权重减小,降低其对总损失的影响;
    • 正常数据点的残差 \(|e_i|\)较小,权重 \(w_i=1\),与普通最小二乘法一样。
  2. 逐步逼近真实参数
    • 迭代加权最小二乘法(IRLS):在每次迭代中,根据当前的残差更新权重,然后求解加权最小二乘问题;
    • 随着迭代进行,权重 \(w_i\)动态调整,使得模型对异常值的敏感性降低。

此时 \(\Delta\boldsymbol x_k\)搜索方向的计算可以改为

\[\Delta\boldsymbol x_k=-\left(\boldsymbol J^T\boldsymbol W\boldsymbol J\right)^{-1}\boldsymbol J^T\boldsymbol W\boldsymbol e\tag{3-11a}\]

Levenberg–Marquardt 算法的迭代公式也可以改为

\[\boldsymbol x_{k+1}=\boldsymbol x_k-\left(\boldsymbol J^T\boldsymbol W\boldsymbol J+\lambda\boldsymbol I\right)^{-1}\boldsymbol J^T\boldsymbol W\boldsymbol e\tag{3-11b}\]

3.3 常用的 Robust 核函数

在实际应用中,通常取 \(\rho(s)=\rho\left(\frac{e_1}\sigma\right)\),而并不直接使用 \(\rho(e_i)\),其中 \(\sigma\) 一般使用中位绝对偏差(MAD)来估计,以保证不过分受异常值的影响。可使用一下公式来计算 \(\sigma\):

\[\hat\sigma=1.4826\times\text{median}\left\{|e_i-\text{median}(e_i)|\right\}\tag{3-12}\]

常用的 Robust 核函数及其权重如下表所示。

表 3-1 常用的 Robust 核函数及其权重
\(\rho(s)\) \(\rho'(s)\) \(w_i=\frac{\rho'(s)}{s}\)
L2 \(\frac12s^2\) \(s\) \(1\)
Huber

\[\begin{cases}\frac12s^2&\vert s\vert\leq k\\k(\vert s\vert-\frac12k)&\vert s\vert>k\end{cases}\]

\[\begin{cases}s&\vert s\vert\leq k\\k\cdot\texttt{sgn}(s)&\vert s\vert>k\end{cases}\]

\[\begin{cases}1&\vert s\vert\leq k\\\frac{k}{\vert s\vert}&\vert s\vert>k\end{cases}\]

Tukey

\[\begin{cases}\frac{k^2}{6}\left[1-\left(1-\frac{s^2}{k^2}\right)^3\right]&\vert s\vert\leq k\\\frac{k^2}{6}&\vert s\vert>k\end{cases}\]

\[\begin{cases}s\left(1-\frac{s^2}{k^2}\right)^2&\vert s\vert\leq k\\0&\vert s\vert>k\end{cases}\]

\[\begin{cases}\left(1-\frac{s^2}{k^2}\right)^2&\vert s\vert\leq k\\0&\vert s\vert>k\end{cases}\]

GM

\[\frac{s^2}{2\left(1+s^2\right)}\]

\[\frac{s}{\left(1+s^2\right)^2}\]

\[\frac{1}{\left(1+s^2\right)^2}\]

Cauchy

\[\frac{c^2}2\log\left[1+\left(\frac sk\right)^2\right]\]

\[\frac{s}{1+\left(\frac{s}{k}\right)^2}\]

\[\frac{1}{1+\left(\frac{s}{k}\right)^2}\]

不难发现,L2 核函数就是原来的目标函数 \(\fml{3-1}\)。在正态分布的假设下

  • Huber 核的 \(k\) 可以取为 1.345;
  • Tukey 核的 \(k\) 可以取为 4.685;
  • Cauchy 核的 \(k\) 可以取为 2.385。

3.4 如何使用

RMVL 提供了带有 Robust 核函数的最小二乘法,可参考 rm::lsqnonlinRKF ,对于上面示例中的正弦函数拟合,可以使用下面的代码。

auto x = rm::lsqnonlinRKF(lsq_sine, {1, 0.02, 0, 1.09}, rm::RobustMode::Huber);
std::valarray< double > lsqnonlinRKF(const FuncNds &func, const std::valarray< double > &x0, RobustMode rb, const OptimalOptions &options={})
带 Robust 核函数的非线性最小二乘求解
@ Huber
Huber 核函数 huber64
定义 numcal.hpp:425