01 LM 算法及 Cpp 实现

01 LM 算法及 Cpp 实现

1.1 应用

LM 算法用于解决非线性最小二乘问题

$$\underset{x}{\min }F(x)=\frac{1}{2}\Vert f(\mathbf{x}){\Vert }_2^2\text{\hspace{1em}(1)}$$

其中，$f(\mathbf{x})$ 是指各项的残差。

LM 算法有两种推导方式，即 阻尼法 和 置信域 法（见《十四讲》），这里用阻尼法进行推导。

1.2 阻尼法推导

（1）一阶泰勒展开近似

将 $f(\mathbf{x})$ 在 ${\mathbf{x}}_{\mathbf{n}}$ 处一阶泰勒展开（把 $\Delta \mathbf{x}$ 看做未知数），

$$f({\mathbf{x}}_{\mathbf{n}}+\Delta \mathbf{x})\approx f({\mathbf{x}}_{\mathbf{n}})+\mathbf{J}\left({\mathbf{x}}_n\right)\Delta \mathbf{x}\text{\hspace{1em}(2)}$$

那么问题转化为，对于每次迭代，求解最优的 $\Delta \mathbf{x}$ 。

$$\Delta {\mathbf{x}}^{\ast }=\arg \underset{\Delta \mathbf{x}}{\min }\frac{1}{2}{\Vert f\left({\mathbf{x}}_{\mathbf{n}}\right)+{\mathbf{J}}_{\mathbf{n}}\left({\mathbf{x}}_{\mathbf{n}}\right)\Delta \mathbf{x}\Vert }^2\text{\hspace{1em}(3)}$$

（2）加入阻尼项

$$\Delta {\mathbf{x}}^{\ast }=\arg \underset{\Delta \mathbf{x}}{\min }M(\Delta \mathbf{x})=\frac{1}{2}{\Vert f\left({\mathbf{x}}_{\mathbf{n}}\right)+{\mathbf{J}}_{\mathbf{n}}\left({\mathbf{x}}_{\mathbf{n}}\right)\Delta \mathbf{x}\Vert }^2+\frac{1}{2}\mu \Delta {\mathbf{x}}^T\Delta \mathbf{x}\text{\hspace{1em}(4)}$$

其中，$\mu$ 为阻尼系数，阻尼项 $\frac{1}{2}\mu \Delta {\mathbf{x}}^T\Delta \mathbf{x}$ 可以看做是对于过大的 $\Delta \mathbf{x}$ 的惩罚。

（3）求极值

对 $\Delta \mathbf{x}$ 求导，并令其等于零，

$$\mathbf{J}{\left({\mathbf{x}}_{\mathbf{n}}\right)}^{T}f\left({\mathbf{x}}_{\mathbf{n}}\right)+\mathbf{J}{\left({\mathbf{x}}_{\mathbf{n}}\right)}^T\mathbf{J}\left({\mathbf{x}}_{\mathbf{n}}\right)\Delta \mathbf{x}+\mu \Delta \mathbf{x}=0\text{\hspace{1em}(5)}$$

得

$$\Delta {\mathbf{x}}^{\ast }=-{\left(\mathbf{J}{\left({\mathbf{x}}_{\mathbf{n}}\right)}^T\mathbf{J}\left({\mathbf{x}}_{\mathbf{n}}\right)+\mu \mathbf{I}\right)}^{-1}\mathbf{J}{\left({\mathbf{x}}_{\mathbf{n}}\right)}^{T}f\left({\mathbf{x}}_{\mathbf{n}}\right)\text{\hspace{1em}(6)}$$

令

$$\begin{array}{rl}\mathbf{J}{\left({\mathbf{x}}_{\mathbf{n}}\right)}^T\mathbf{J}\left({\mathbf{x}}_{\mathbf{n}}\right)& =\mathbf{H}\\ \mathbf{J}{\left({\mathbf{x}}_{\mathbf{n}}\right)}^{T}f\left({\mathbf{x}}_{\mathbf{n}}\right)& =\mathbf{g}\end{array}$$

则式（6）可写为

$$\Delta {\mathbf{x}}^{\ast }=-(\mathbf{H}+\mu \mathbf{I}{)}^{-1}\mathbf{g}\text{\hspace{1em}(7)}$$

（4）阻尼系数 $\mu$ 的调节

定义增益系数 $\rho$

$$\rho =\frac{f(\mathbf{x}+\mathbf{\Delta }\mathbf{x})-f(\mathbf{x})}{\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}}\text{\hspace{1em}(8)}$$

其中，分子是实际下降的值，分母是近似下降的值。若 $\rho$ 接近于 1 ，则近似效果好；若 $\rho$ 太小，则说明实际减小的值远小于近似减小的值，即近似效果较差，需缩小近似范围，即增大阻尼系数 $\mu$； 若 $\rho$ 太大，则说明实际减小的值大于近似减小的值，则需放大近似范围，即减小 $\mu$。

两种调节方法：

$$\begin{array}{rl}\text{ if }& \rho <0.25\\ & \mu :=\mu \ast 2\\ \text{ else if }& \rho >0.75\\ & \mu :=\mu /3\end{array}$$

也就是，$\rho <0.25$ 时增大阻尼系数；$\rho >0.75$ 时，减小阻尼系数。

或者：

$$\begin{array}{rl}if\rho >0& \\ & \mu :=\mu \ast \max \left\{\frac{1}{3},1-(2\rho -1{)}^3\right\};\nu :=2\\ else& \\ & \mu :=\mu \ast \nu ;\nu :=2\ast \nu \end{array}$$

1.3 Cpp 算法实现

两种方法差别在于 $\rho$ 的分母的计算（我的更快？）。

优化目标是待定系数，把他们看做 未知数，需要计算的 step 就是这些待定系数的 step。

（1）我的方法

/***********************************************************                                          *
* Time: 2022/11/3
* Author: xiaocong
* Function: LM 算法** @ 解决的是最小二乘问题，也就是找到最优的系数使得残差最小* @ obj = A * sin(Bx) + C * cos(D*x) - F*      A * sin(Bx) + C * cos(D*x) 是目标函数*      F 是实际值*      目标是找到使 obj 最小的系数 A B C D
***********************************************************/#include <Eigen/Dense>      // 稠密矩阵
#include <Eigen/Sparse>      // 稀疏矩阵
#include <iostream>
#include <cmath>using namespace std;
using namespace Eigen;const double DERIV_STEP = 1e-5;
const int MAX_INTER = 100;#define max(a, b) (((a)>(b))?(a):(b))// 定义目标函数
double func(const VectorXd input, const VectorXd &output, const VectorXd &params, int objIndex)
{// obj = A * sin(Bx) + C * cos(D*x) - Fdouble x1 = params(0);   // params 中存储的是系数double x2 = params(1);double x3 = params(2);double x4 = params(3);double t = input(objIndex);    // input 中存储的是 xdouble f = output(objIndex);   // output 中存储的是实际值return x1 * sin(x2 * t) + x3 * cos(x4 * t) - f;    // 返回 objIndex 项的残差
}// 计算残差矩阵
VectorXd objF(const VectorXd &input, const VectorXd &output, const VectorXd &params)
{VectorXd obj(input.rows());    // 存储所有残差，也就是残差矩阵for (int i = 0; i < input.rows(); i++)obj(i) = func(input, output, params, i);return obj;   // 返回残差矩阵
}// 残差平方和
double Func(const VectorXd &obj)
{return obj.squaredNorm() / 2.0;
}// 求某个系数在某点的导数
double Deriv(const VectorXd &input, const VectorXd &output, int objIndex, const VectorXd &params, int paraIndex)
{VectorXd para1 = params;VectorXd para2 = params;para1(paraIndex) -= DERIV_STEP;para2(paraIndex) += DERIV_STEP;double obj1 = func(input, output, para1, objIndex);double obj2 = func(input, output, para2, objIndex);return (obj2 - obj1) / (2 * DERIV_STEP);     // 该点处的导数，为求雅克比矩阵做准备
}// 计算雅克比矩阵
/***************************** 我们优化的是系数 params，把他们看做未知数，分别求导，得到雅克比矩阵* 维度：（input.rows() x params.rows()）* [[df/dA df/dB df/dC df/dD]    <--- x1*  [df/dA df/dB df/dC df/dD]    <--- x2*  [.......................]*  [df/dA df/dB df/dC df/dD]]    <--- xn****************************/
MatrixXd Jacobian(const VectorXd &input, const VectorXd &output, const VectorXd &params)
{int rowNum = input.rows();int colNum = params.rows();MatrixXd Jac(rowNum, colNum);for (int i = 0; i < rowNum; i++)for (int j = 0; j < colNum; j++)Jac(i, j) = Deriv(input, output, i, params, j);return Jac;
}//求 Hessian 矩阵对角线最大值
// Hessian 矩阵：二阶导数
double maxMatrixDiagonale(const MatrixXd &Hessian)
{int max = 0;for (int i = 0; i < Hessian.rows(); i++){if(Hessian(i, i) > max)max = Hessian(i, i);}return max;
}// 近似下降值
double linerDeltaL(const VectorXd &step, const MatrixXd &Jac)
{VectorXd L = Jac * step;return L.norm();
}void levenMar(const VectorXd &input, const VectorXd &output, VectorXd &params)
{double epsilon = 1e-12;int iterCnt = 0;         // 迭代计数double tao = 1e-3;long long v = 2;// 求初始的 uMatrixXd Jac = Jacobian(input, output, params);    // 得到雅克比矩阵MatrixXd H = Jac.transpose() * Jac;            // 得到 Hessian 矩阵，4x4double u = tao * maxMatrixDiagonale(H);     // Hessian 矩阵对角线最大值乘 taowhile (iterCnt < MAX_INTER)     // double 类型不能直接作比较{VectorXd obj = objF(input, output, params);    // 误差矩阵MatrixXd Jac = Jacobian(input, output, params);    // 得到雅克比矩阵MatrixXd H = Jac.transpose() * Jac;            // 得到 Hessian 矩阵，4x4VectorXd g = Jac.transpose() * obj;     // 也就是 g，4x1VectorXd step = (H + u * MatrixXd::Identity(H.rows(), H.cols())).inverse() * g;if(step.norm() < epsilon)break;cout << "H：" << endl << H << endl;cout << "step: " << endl << step << endl;VectorXd paramsNew(params.rows());paramsNew = params - step;            // 更新 params// 计算 params 误差obj = objF(input, output, params);// 计算 paramsNew 误差VectorXd  obj_new = objF(input, output, paramsNew);double deltaF = Func(obj) - Func(obj_new);     // 求差double deltaL = linerDeltaL(step, Jac);// 计算增益系数 rhodouble rho = deltaF / deltaL;          // 实际下降值 / 近似下降值cout << "rho is； " << rho <<endl;if(rho > 0){params = paramsNew;u *= max(1.0 / 3.0, 1 - pow(2 * rho - 1, 3));v = 2;} else{u = u * v;v = v * 2;}cout << "u= " << u << "\tv= " << v << endl;iterCnt ++;cout << "Iteration " << iterCnt << " times, result is :" << endl<< params << endl;}
}int main()
{int params_num = 4;int total_data = 100;VectorXd input(total_data);VectorXd output(total_data);double A = 5, B = 1, C = 10, D = 2;    // 初始化// 生成数据for (int i = 0; i < total_data; i++){double x = 20.0 * ((rand() % 1000) / 1000.0) - 10.0;    // [-10, 10]double deltaY = 2.0 * (rand() % 1000) / 1000.0;         // 随机噪声，[0, 2]double y = A * sin(B*x) + C * cos(D*x) + deltaY;input(i) = x;output(i) = y;}VectorXd params_levenMar(params_num);params_levenMar << 3.6, 1.3, 7.2, 1.7;levenMar(input, output, params_levenMar);cout << "**********************************************" << endl;cout << "Levenberg-Marquardt parameter: " << endl << params_levenMar << endl;}

输出

Levenberg-Marquardt parameter:4.85628
0.99790410.05242.003

（2）网络方法

/***********************************************************                                          *
* Time: 2022/11/2
* Author: xiaocong
* Function: LM 算法** @ 解决的是最小二乘问题，也就是找到最优的系数使得残差最小* @ obj = A * sin(Bx) + C * cos(D*x) - F*      A * sin(Bx) + C * cos(D*x) 是目标函数*      F 是实际值*      目标是找到使 obj 最小的系数 A B C D
***********************************************************/#include <Eigen/Dense>      // 稠密矩阵
#include <Eigen/Sparse>      // 稀疏矩阵
#include <iostream>
#include <iomanip>    // 控制输入输出格式等
#include <cmath>using namespace std;
using namespace Eigen;const double DERIV_STEP = 1e-5;
const int MAX_INTER = 100;#define max(a, b) (((a)>(b))?(a):(b))// 定义目标函数
double func(const VectorXd input, const VectorXd &output, const VectorXd &params, int objIndex)
{// obj = A * sin(Bx) + C * cos(D*x) - Fdouble x1 = params(0);   // params 中存储的是系数double x2 = params(1);double x3 = params(2);double x4 = params(3);double t = input(objIndex);    // input 中存储的是 xdouble f = output(objIndex);   // output 中存储的是实际值return x1 * sin(x2 * t) + x3 * cos(x4 * t) - f;    // 返回 objIndex 项的残差
}// 计算残差矩阵
VectorXd objF(const VectorXd &input, const VectorXd &output, const VectorXd &params)
{VectorXd obj(input.rows());    // 存储所有残差，也就是残差矩阵for (int i = 0; i < input.rows(); i++)obj(i) = func(input, output, params, i);return obj;   // 返回残差矩阵
}// 残差平方和
double Func(const VectorXd &obj)
{return obj.squaredNorm() / 2.0;
}// 求某个系数在某点的导数
double Deriv(const VectorXd &input, const VectorXd &output, int objIndex, const VectorXd &params, int paraIndex)
{VectorXd para1 = params;VectorXd para2 = params;para1(paraIndex) -= DERIV_STEP;para2(paraIndex) += DERIV_STEP;double obj1 = func(input, output, para1, objIndex);double obj2 = func(input, output, para2, objIndex);return (obj2 - obj1) / (2 * DERIV_STEP);     // 该点处的导数，为求雅克比矩阵做准备
}// 计算雅克比矩阵
/***************************** 我们优化的是系数 params，把他们看做未知数，分别求导，得到雅克比矩阵* 维度：（input.rows() x output.rows()）* [[df/dA df/dB df/dC df/dD]    <--- x1*  [df/dA df/dB df/dC df/dD]    <--- x2*  [.......................]*  [df/dA df/dB df/dC df/dD]]    <--- xn****************************/
MatrixXd Jacobian(const VectorXd &input, const VectorXd &output, const VectorXd &params)
{int rowNum = input.rows();int colNum = params.rows();MatrixXd Jac(rowNum, colNum);for (int i = 0; i < rowNum; i++)for (int j = 0; j < colNum; j++)Jac(i, j) = Deriv(input, output, i, params, j);return Jac;
}//求 Hessian 矩阵对角线最大值
// Hessian 矩阵：二阶导数
double maxMatrixDiagonale(const MatrixXd &Hessian)
{int max = 0;for (int i = 0; i < Hessian.rows(); i++){if(Hessian(i, i) > max)max = Hessian(i, i);}return max;
}double linerDeltaL(const VectorXd &step, const VectorXd &gradient, const double u)
{double L = step.transpose() * (u * step - gradient);return L;
}void levenMar(const VectorXd &input, const VectorXd &output, VectorXd &params)
{int errNum = input.rows();int paraNum = params.rows();// initial parametersVectorXd obj = objF(input, output, params);    // 得到误差矩阵MatrixXd Jac = Jacobian(input, output, params);    // 得到雅克比矩阵MatrixXd H = Jac.transpose() * Jac;            // 得到 Hessian 矩阵，4x4VectorXd gradient = Jac.transpose() * obj;     // 也就是 g，4x1double tao = 1e-3;long  long v = 2;double epsilon1 = 1e-12, epsilon2 = 1e-12;double u = tao * maxMatrixDiagonale(H);     // Hessian 矩阵对角线最大值乘 taobool found = gradient.norm() <= epsilon1;if (found) return;        // 直接退出程序，不再执行后面的程序double last_sum = 0;int iterCnt = 0;         // 迭代计数while (iterCnt < MAX_INTER){VectorXd obj = objF(input, output, params);    // 误差矩阵MatrixXd Jac = Jacobian(input, output, params);    // 得到雅克比矩阵MatrixXd H = Jac.transpose() * Jac;            // 得到 Hessian 矩阵，4x4VectorXd gradient = Jac.transpose() * obj;     // 也就是 g，4x1if(gradient.norm() < epsilon1){cout << "stop g(x) = 0 for a local minimizer optimizer." << endl;break;}cout << "H：" << endl << H << endl;VectorXd step = (H + u * MatrixXd::Identity(paraNum, paraNum)).inverse() * gradient;// 求 Delta x = (H + uI)^{-1}g      注意：step 维度(4x1)cout << "step: " << endl << step << endl;if(step.norm() <= epsilon2 * (params.norm()) + epsilon2){cout << "stop because change in x is small" << endl;break;}VectorXd paramsNew(params.rows());paramsNew = params - step;            // 更新 params// 计算 params 误差obj = objF(input, output, params);// 计算 paramsNew 误差VectorXd  obj_new = objF(input, output, paramsNew);double deltaF = Func(obj) - Func(obj_new);     // 求差double deltaL = linerDeltaL(-1 * step, gradient, u);// 计算增益系数 rhodouble rho = deltaF / deltaL;          // 实际下降值 / 近似下降值cout << "rho is； " << rho <<endl;if(rho > 0){params = paramsNew;u *= max(1.0 / 3.0, 1 - pow(2 * rho - 1, 3));v = 2;} else{u = u * v;v = v * 2;}cout << "u= " << u << "\tv= " << v << endl;iterCnt ++;cout << "Iteration " << iterCnt << " times, result is :" << endl<< params << endl;}
}int main()
{int params_num = 4;int total_data = 100;VectorXd input(total_data);VectorXd output(total_data);double A = 5, B = 1, C = 10, D = 2;    // 初始化// 生成数据for (int i = 0; i < total_data; i++){double x = 20.0 * ((rand() % 1000) / 1000.0) - 10.0;    // [-10, 10]double deltaY = 2.0 * (rand() % 1000) / 1000.0;         // 随机噪声，[0, 2]double y = A * sin(B*x) + C * cos(D*x) + deltaY;input(i) = x;output(i) = y;}VectorXd params_levenMar(params_num);params_levenMar << 3.6, 1.3, 7.2, 1.7;levenMar(input, output, params_levenMar);cout << "Levenberg-Marquardt parameter: " << endl << params_levenMar << endl << endl << endl;cout << "**********************************************" << endl;}