#include "LM.h"
#include <opencv2/opencv.hpp>

int main()
{
	const double a = 1, b = 5, c = 2;
	double a_ = 0.9, b_ = 5.5, c_ = 2.2;

	LM::LevenbergMarquardt lm(a_, b_, c_);
	lm.SetParameters(1e-10, 1e-10, 100, true);

	const size_t N = 1000;
	cv::RNG rng(cv::getTickCount());

	for (size_t i = 0; i < N; i++)
	{
		double x = rng.uniform(0.0, 10.0);
		double y = exp(a * x * x + b * x + c) + rng.gaussian(5.0);
		lm.AddObservation(x, y);
	}

	lm.Slove();
	return 0;
}

#pragma once
#include <iostream>
#include <vector>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

namespace LM
{	
	class RunTime
	{
	public:
		void Stat();
		void Stop();
		double Duration();
	private:
		chrono::steady_clock::time_point tstat;
		chrono::steady_clock::time_point tend;
	};

	class LevenbergMarquardt
	{
	public:
		LevenbergMarquardt(double da, double db, double dc);
		void SetParameters(double sEpsilon1, double sEpsilin2, int sMaxIter, bool sIsout);
		void AddObservation(const double x, const double y);
		void CalcJ_fx();
		void CalcH_g();
		double GetCost();
		double F(double a, double b, double c);
		double L0_L(Eigen::Vector3d h);
		void Slove();
	public:
		double epsilon1, epsilon2;
		int MaxIter;
		std::vector<Eigen::Vector2d> obsPt;
		double a, b, c;
		bool isOut;
	private:			
		MatrixXd fx_;
		MatrixXd J_;
		Matrix3d H_;
		Vector3d g_;
	};
}

#include "LM.h"
#include <iomanip>

void LM::RunTime::Stat()
{
	tstat = chrono::steady_clock::now();
}

void LM::RunTime::Stop()
{
	tend = chrono::steady_clock::now();
}

double LM::RunTime::Duration()
{
	return chrono::duration_cast<chrono::duration<double>>(tend - tstat).count();
}

LM::LevenbergMarquardt::LevenbergMarquardt(double da, double db, double dc) :a(da), b(db), c(dc)
{
	epsilon1 = 1e-6;
	epsilon2 = 1e-6;
	MaxIter = 50;
	isOut = true;
}

void LM::LevenbergMarquardt::SetParameters(double sEpsilon1, double sEpsilon2, int sMaxIter, bool sIsout)
{
	epsilon1 = sEpsilon1;
	epsilon2 = sEpsilon2;
	MaxIter = sMaxIter;
	isOut = sIsout;
}

void LM::LevenbergMarquardt::AddObservation(const double x, const double y)
{
	obsPt.push_back(Eigen::Vector2d(x, y));
}

void LM::LevenbergMarquardt::CalcJ_fx()
{
	J_.resize(obsPt.size(), 3);
	fx_.resize(obsPt.size(), 1);

	for (int i = 0; i < obsPt.size(); i++)
	{
		Eigen::Vector2d obs = obsPt.at(i);
		const double x = obs(0);
		const double y = obs(1);
		double dfda = -x * x * exp(a * x * x + b * x + c);
		double dfdb = -x * exp(a * x * x + b * x + c);
		double dfdc = -exp(a * x * x + b * x + c);
		J_(i, 0) = dfda;
		J_(i, 1) = dfdb;
		J_(i, 2) = dfdc;
		fx_(i, 0) = y - exp(a * x * x + b * x + c);
	}
}

void LM::LevenbergMarquardt::CalcH_g()
{
	H_ = J_.transpose() * J_;
	g_ = - J_.transpose() * fx_;
}

double LM::LevenbergMarquardt::GetCost()
{
	MatrixXd cost = fx_.transpose() * fx_;
	return cost(0, 0);
}

double LM::LevenbergMarquardt::F(double da, double db, double dc)
{
	Eigen::VectorXd fx;
	fx.resize(obsPt.size(), 1);

	for (int i = 0; i < obsPt.size(); i++)
	{
		const Eigen::Vector2d obs = obsPt.at(i);
		const double x = obs(0);
		const double y = obs(1);
		fx(i) = y - exp(da * x * x + db * x + dc);
	}
	Eigen::MatrixXd F = 0.5 * fx.transpose() * fx;
	return F(0, 0);
}

double LM::LevenbergMarquardt::L0_L(Eigen::Vector3d h)
{
	/*Eigen::MatrixXd L = -fx_.transpose() * J_ * h - 0.5 * h.transpose() * J_ * J_.transpose() * h;*/

	Eigen::MatrixXd L = -h.transpose() * J_.transpose() * fx_ - 0.5 * h.transpose() * J_.transpose() * J_ * h;
	return L(0, 0);
}

void LM::LevenbergMarquardt::Slove()
{
	int k = 0;
	double vu = 2.0;
	CalcJ_fx();
	CalcH_g();
	bool found = (g_.lpNorm<Eigen::Infinity>() < epsilon1); //误差足够小
	std::vector<double> H_diag;
	for (int i = 0; i < 3; i++)
	{
		H_diag.push_back(H_(i, i));
	}
	auto max = std::max_element(H_diag.begin(), H_diag.end());
	double mu = *max;
	double tsum = 0;
	while (!found && k < MaxIter) // 误差足够小，增量足够小，达到迭代次数
	{
		k++;
		RunTime runtime;
		runtime.Stat();
		Eigen::Matrix3d G = H_ + mu * Eigen::Matrix3d::Identity();
		Eigen::Vector3d h = G.ldlt().solve(g_);

		if (h.norm() < epsilon2 * sqrt(a * a + b * b + c * c) + epsilon2) //增量足够小
		{
			found = true;
		}
		else
		{
			double a_new = a + h(0);
			double b_new = b + h(1);
			double c_new = c + h(2);
			double sig = (F(a, b, c) - F(a_new, b_new, c_new)) / L0_L(h);
			if (sig > 0)
			{
				a = a_new;
				b = b_new;
				c = c_new;
				CalcJ_fx();
				CalcH_g();
				found = (g_.lpNorm<Eigen::Infinity>() < epsilon1);
				mu = mu * std::max<double>(0.333, 1 - std::pow(2 * sig - 1, 3));
				vu = 2.0;
			}
			else
			{
				mu = mu * vu;
				vu *= 2;
			}
		}

		runtime.Stop();
		if (isOut)
		{
			std::cout << "Iter: " << std::left << std::setw(5) << k << std::left << std::setw(10)
				<< "Result: " << std::left << std::setw(10) << a << " " << std::left << std::setw(10) << b << " " << std::left << std::setw(10) << c << std::left << std::setw(10)
				<< "step: " << std::left << std::setw(15) << h.norm() << std::left << std::setw(10)
				<< "cost: " << std::left << std::setw(10) << GetCost() << std::left << std::setw(10)
				<< "time: " << std::left << std::setw(10) << (tsum += runtime.Duration()) << std::endl;
		}
	}

	if (found)
	{
		std::cout << "\nConverged\n\n";
	}
	else
	{
		std::cout << "\nDiverged\n\n";
	}
}

1、介绍

列文伯格(1944)和马夸尔特(1963)先后对高斯牛顿法进行了改进，求解过程中引入了阻尼因子。
将公式 36 的无约束最小二乘问题转变为公式 44 有约束最小二乘问题，其中，$\frac{1}{2}\times ({\Vert \begin{array}{c}D\Delta X_k\end{array}\Vert }^2-\mu )⩽0$ 是信赖区间对应的条件， $D$ 是系数矩阵(列文伯格方法和马夸尔特方法的主要区别就是系数矩阵 $D$ 不同)， $\mu$ 是信赖 半径。
$$F(X_k+\Delta X_k)\approx \frac{1}{2}{\Vert \begin{array}{c}\\ L(X_k)+\underset{L}{\underbrace{J(X_k)}}{}^T\Delta X_k\end{array}\Vert }^2,s.t.(\frac{1}{2}\times ({\Vert \begin{array}{c}D\Delta X_k\end{array}\Vert }^2-\mu )⩽0)(公式44)$$

2、数学原理

将公式 44 的约束条件引入，可以定义拉格朗日函数$Lag(X)$：
$$Lag(\Delta X)\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{=}\frac{1}{2}{\Vert \begin{array}{c}\\ L(X_k)+\underset{L}{\underbrace{J(X_k)}}{}^T\Delta X_k\end{array}\Vert }^2+{\lambda }_k\times \frac{1}{2}\times ({\Vert \begin{array}{c}D\Delta X_k\end{array}\Vert }^2-\mu ),s.t.({\lambda }_k⩾0)(公式45)$$
公式45中$\underset{L}{\underbrace{J(X_k)}}$是残差函数$L(X_k)$的雅可比矩阵。当${\Vert \begin{array}{c}D\Delta X_k\end{array}\Vert }^2⩾0$的时候，在优化目标函数$F(X)$中引入阻尼因子${\lambda }_k$作为惩罚项。
该表达式中$Lag(\Delta X_k)$、$L(X_k)$是一个常数，$\underset{L}{\underbrace{J(X_k)}}$、$D$是一个常数矩阵，$\Delta X_k$是一个变量矩阵，即函数$Lag(\Delta X_k)$是以$\Delta X_k$为自变量的二次函数。综上所述，当$Lag(\Delta X_k)$一阶导数为0的时候，函数$Lag(\Delta X_k)$取得极值，可推得：
$$La{g}_{\Delta X_k}^{\prime }(\Delta X_k)=0(公式46)$$
由公式 45、公式 46 可推得：
$$0={\lambda }_k{D}^{T}D\Delta X_k+L(X_k)\underset{L}{\underbrace{J(X_k)}}+\underset{L}{\underbrace{J(X_k)}}\underset{L}{\underbrace{J(X_k)}}{}^T\Delta X_k(公式47)$$
即：
$$0={\lambda }_k{D}^{T}D\Delta X_k+\sum _{i=1}^m(L_i(X_k)\underset{L_i}{\underbrace{J(X_k)}}+\underset{L_i}{\underbrace{J(X_k)}}\underset{L_i}{\underbrace{J(X_k)}}{}^T\Delta X_k)(公式48)$$
由公式 47 可推得:
$$0=L(X_k)\underset{L}{\underbrace{J(X_k)}}+(\underset{L}{\underbrace{J(X_k)}}\underset{L}{\underbrace{J(X_k)}}{}^T+{\lambda }_k{D}^{T}D)\Delta X_k(公式49)$$
设：由公式49结合公式26结构形式，可近似推得函数$F(X)$的黑塞矩阵$\underset{F}{\underbrace{H(X_k)}}$和雅克比矩阵$\underset{F}{\underbrace{J(X_k)}}$：
$$\underset{F}{\underbrace{H(X_k)}}\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{\approx }\underset{L}{\underbrace{J(X_k)}}\underset{L}{\underbrace{J(X_k)}}{}^T(公式50)$$
$$\underset{F}{\underbrace{J(X_k)}}\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{\approx }L(X_k)\underset{L}{\underbrace{J(X_k)}}(公式51)$$
由公式 49、公式 50、公式 51 可推得：
$$\Delta X_k=-(\underset{F}{\underbrace{H(X_k)}}+{\lambda }_k{D}^{T}D{)}^{-1}\underset{F}{\underbrace{J(X_k)}}(公式52)$$
由公式 52 可推得目标函数$F(X)$的最优化迭代公式：
$$X_{k+1}\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{=}{X}_k-(\underset{F}{\underbrace{H(X_k)}}+{\lambda }_k{D}^{T}D{)}^{-1}\underset{F}{\underbrace{J(X_k)}}(公式53)$$
当阻尼因子${\lambda }_k>0$的时候，保证$\underset{F}{\underbrace{H(X_k)}}+{\lambda }_k{D}^{T}D$正定，迭代朝着下降方向进行；当${\lambda }_k$趋近于 0 的时候，退化为高斯牛顿算法；当 ${\lambda }_k$ 趋近于正无穷大的时候，退化为最速下降算法；

3、阻尼因子更新策略

目标函数$F(X)$在$X=X_k$处的不含皮亚诺余项的二阶泰勒公式如下：
$$F(X_k+\Delta X_k)\approx F(X_k)+\underset{F}{\underbrace{J(X_k)}}{}^T\Delta X_k＋\frac{1}{2}{\Delta X_k}^T\underset{F}{\underbrace{H(X_k)}}\Delta X_k(公式54)$$
根据公式 50、公式 51 的近似关系和公式 54 可推得：
$$U(\Delta X_k)\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{=}F(X_k+\Delta X_k)\approx F(X_k)+{\underset{L}{\underbrace{J(X_k)}}}^{T}L(X_k)\Delta X_k＋\frac{1}{2}{\Delta X_k}^T{\underset{L}{\underbrace{J(X_k)}}}^T\underset{L}{\underbrace{J(X_k)}}\Delta X_k(公式55)$$
阻尼因子${\lambda }_k$的初始值${\lambda }_0$的选取，$\underset{F}{\underbrace{H_0}}\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{\approx }\underset{L}{\underbrace{J(X_0)}}\underset{L}{\underbrace{J(X_0)}}{}^T$对角线上面的数$h_{ii}$有关。
$${\lambda }_0\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{=}\tau \times max\left\{\begin{array}{c}{h}_{ii}\end{array}\right\},s.t.(\tau \in [10^{-8},1])(公式56)$$
而阻尼因子 ${\lambda }_k$ 的更新可以由增益比 ${\beta }_k$ 来定量分析：
$${\beta }_k\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{=}\frac{F(X_k)-F(X_k+\Delta X_k)}{U(0)-U(\Delta X_k)}(公式57)$$
其中公式57的分子为目标函数 $F(X_k)$ 的值在步长 $\Delta X_k$ 下的实际变化，分母为目标函数 $F(X_k)$ 的值二阶近 似的变化，可得出：
如果 ${\beta }_k$ 的值越大，那么目标函数的二阶近似变化对实际变化效果越好，可以缩小 ${\lambda }_k$ 的值接近高斯牛顿算法;
如果 ${\beta }_k$ 的值越小，那么目标函数的二阶近似变化对实际变化效果越差，可以增大 ${\lambda }_k$ 的值接近最速下降算法;
阻尼因子 ${\lambda }_k$ 的 $\text{Nielsen}$ (1991)更新策略如下：
$$\begin{array}{rl}如果({\beta }_k>0):& \\ & {\lambda }_k\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{=}{\lambda }_{k-1}\times max\left\{\begin{array}{c}\frac{1}{3}，1-(2{\beta }_k-1{)}^3\end{array}\right\}\\ & v_k\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{=}2\\ 否则({\beta }_k⩽0):& \\ & {\lambda }_k\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{=}{\lambda }_{k-1}\times v_{k-1}\\ & v_k=2\times v_{k-1}\end{array}(公式58)$$

4、列文伯格方法

在列文伯格方法中，系数矩阵 $D$ 是单位矩阵 $I$ ，信赖区域是一个球形状。

5、马夸尔特方法

在马夸尔特方法中，系数矩阵 $D$ 是 $\underset{F}{\underbrace{H(X_k)}}\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{\approx }\underset{L}{\underbrace{J(X_k)}}\underset{L}{\underbrace{J(X_k)}}{}^T$ 对角元素的平方根，信赖区域是一个椭球形状。

6、Matlab程序

链接地址

前言

LM方法是适用于求解方程最小值的一种方法，在非线性优化的框架中，优化方法分为Line Search 和 Trust Region，也就是线搜索和信任域方法，它们是两种不同性质的方法。

不同之处:

LIne Search:不管当前迭代点X(k)到最优解X*之间的路径，每次迭代X(k)得到X(k+1),都是使用该点的反向梯度方向进行值得寻找，这就导致了这样一种可能得问题：‘在靠近X*的时候，X(k)反复震荡，不容易收敛。这种情况特别是会发生在固定迭代步长的时候，Eg:对于求解f(x)=x^2的最优解X*,当X(k)=0.01,而迭代步长为1的时候，这个时候下一个X(k+1)=-0.01，如此这就发生了震荡，无法收敛。这确实是特例，但是只要有这种情况存在，也就是我们能够找出一个情况算法有问题，我们就无法保证所需要优化的函数f(x)不存在这种性质。

代表:梯度下降法，牛顿法，高斯牛顿法。

Trust Region:对于当前迭代点X(k)，我们会找出一个合适的迭代区域||D(X(k+1)-X(k))||<=hk,hk>0，下一个X(k+1)产生于该区域内，然后寻找这个区域的内最佳的迭代向量dX(k)。同时uk会根据每次的X(k)来计算调整，这就i是信任域的普遍思想。

代表:LM法。

LM法的个人理解

具体的实现方法我不做说明，网络上已经有很多人进行了详细的介绍。

这里转载一个我认为没有问题的他人的博客。https://www.codelast.com/%e5%8e%9f%e5%88%9blm%e7%ae%97%e6%b3%95%e7%9a%84%e5%ae%9e%e7%8e%b0/

这里需要说明的是，很多博主在描述到LM具体算法实现的伪代码描述时，大多会混淆拉格朗日乘子和信任域hk这两个参量。导致在说到具体实现的时候逻辑不清，在应该让h(k+1)=2*h(k)的时候，说成h(k+1)=h(k)/2。

下面来说明一下算法和LM思想原理之间的对应关系:

高斯牛顿法优化目标是，利用求导数为0，我们得到,但是这个要求必须是可逆矩阵，要求较为严格。

而LM算法优化目标是,因为约束条件的h(k)是迭代变化的，为了保证公式一致性，把约束条件改写成是加了约束条件的优化目标.利用拉格朗日乘子法进行转换成无约束目标，令。

采用拉格朗日乘数法确实需要解出不同情况下对应的*，但是在LM法中进行了这样的操作，把看成常数，相等于把问题转化成了数学建模中的规划问题，约束条件的权重为,而高斯牛顿法优化目标为,当时，两者同比重，也就是总优化目标同时考虑到了两者。这样我们就可以减少求解乘子的步骤，得到最终的迭代向量，特别的当D为单位矩阵时，表示信任域是一个球。

接下来讲述关键问题，也就是如何迭代u(k).

LM定义了这样一个比值参量,表征了实际下降量与一阶微分量（也就是近似下降量)的比值。

在算法伪代码中有这么一个判断:

如果,那么h(k+1)=h(k)/2,也就是减小信任域，此时在具体的算法中u(k+1)=2*u(k)；

如果,那么h(k+1)=2*h(k),也就是增大信任域，此时在具体的算法中u(k+1)=u(k)/2；

这其中的逻辑关系从LM迭代公式可以知道，因为如果实际下降比一阶近似小很多，那么表示二阶以上分量占了比较大的比重，这个时候非线性比较严重，那么就应该缩小信任域，u(k)增大，而的模长变小，也就是变化较小了。同理可说明另外的情况。

最后说明一下如果u(k)的迭代搞混了，也就是之前提到的，该增大的时候变小了，而该变小的时候增大了，这个时候会出现的问题。

最容易遇到的问题就是迭代过小，通过查看的模长||||2^(-k)，就是同一个数量级，而且收敛于非最优解，过早收敛了。产生这种情况的逻辑在于:当某一次迭代过程得到,本该u(k+1)=u(k)/2,但是搞混的情况下u(k+1)=2*u(k),下次的迭代将会变得更小，因为u(k+1)=2*u(k)等价于信任域变小了。另外，这样一次比一次的迭代小了，而且还是指数变小，那么就会使得目标在非最优解上收敛。通过修改程序上的正常逻辑，我们可以验证这样的情况。

最后附上LM算法的matlab程序

链接：https://pan.baidu.com/s/1_3AF-ZRkU9DPlcgxoMY-DA 
提取码：z7y8 
复制这段内容后打开百度网盘手机App，操作更方便哦