非线性最小二乘法之Gauss Newton、L-M、Dog-Leg

本文主要是介绍非线性最小二乘法之Gauss Newton、L-M、Dog-Leg，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

非线性最小二乘法之Gauss Newton、L-M、Dog-Leg

最快下降法

假设 $h^TF'(x) < 0$ ，则h是 $F(x)$ 下降方向，即对于任意足够小的 $\alpha > 0$ ，都满足 $F(x+ \alpha h) < F(x)$ 。
现在讨论 $F(x)$ 沿着h方向下降快慢：

lim α \to 0 F ( x ) - F ( x + α h ) α ∥ h ∥ = - 1 ∥ h ∥ h T F' (x) = - ∥ ∥ F' (x) ∥ ∥ cos θ

$\lim_{\alpha\to0}\frac{F(x)-F(x+\alpha h)}{\alpha \left \| h \right \|}=-\frac{1}{\left \| h \right \|}h^TF'(x)=-\left \|F'(x)\right\|\cos \theta$
其中

θ $\theta$ 为矢量h和

F′(x) $F'(x)$ 夹角，当

θ=π $\theta=\pi$ 时，下降最大。
即

hsd=−F′(x) $h_{sd}=-F'(x)$ ，是最快下降方向。

最小二乘问题

通常的最小二乘问题都可以表示为：

F (x) = 1 2 \sum i = 1 n (f i (x) 2) = 1 2 ∥ f (x) ∥ 2 = 1 2 f (x) T f (x)

$F(x) = \frac{1}{2}\sum_{i=1}^{n}(f_i(x)^2) = \frac{1}{2} \left \| f(x) \right \| ^2 = \frac{1}{2}f(x)^Tf(x)$
找到一个

x∗ $x^*$ 使得

x∗=argminxF(x) $x^* = argmin_x{F(x)}$ ，其中

x=[x1x2⋯xm] $x=[x_1 x_2 \cdots x_m]$ ，

f(x)=[f1(x)f2(x)⋯fn(x)] $f(x)=[f_1(x) f_2(x) \cdots f_n(x)]$ 。

假设对 $f(x)$ 第 $i$ 个分量 $f_i(x)$ 在点 $x_k$ 处Taylor展开,
$f_i(x_k+h) \approx f_i(x_k)+\nabla f_i(x_k)^Th$ ， $i=1,2\cdots n$
则 $f(x_k+h) \approx f(x_k)+J(x_k)h$ ，其中Jacobin矩阵

J (x k) = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ \nabla f 1 (x k) T \nabla f 2 (x k) T ⋮ \nabla f n (x k) ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ \partial f 1 ( x k ) \partial x 1 \partial f 2 ( x k ) \partial x 1 ⋮ \partial f n ( x k ) \partial x 1 \partial f 1 ( x k ) \partial x 2 \partial f 2 ( x k ) \partial x 2 ⋮ \partial f n ( x k ) \partial x 2 \dots \dots ⋱ \dots \partial f 1 ( x k ) \partial x m \partial f 2 ( x k ) \partial x m ⋮ \partial f n ( x k ) \partial x m ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$J(x_k)=\left[\begin{matrix} \nabla f_1(x_k)^T \\ \nabla f_2(x_k)^T \\ \vdots \\ \nabla f_n(x_k) \\ \end{matrix}\right] =\left[ \begin{matrix} \frac{\partial f_1(x_k)}{\partial x_1} &\frac{\partial f_1(x_k)}{\partial x_2} &\cdots &\frac{\partial f_1(x_k)}{\partial x_m} \\ \frac{\partial f_2(x_k)}{\partial x_1} &\frac{\partial f_2(x_k)}{\partial x_2} &\cdots &\frac{\partial f_2(x_k)}{\partial x_m} \\ \vdots &\vdots &\ddots &\vdots \\ \frac{\partial f_n(x_k)}{\partial x_1} &\frac{\partial f_n(x_k)}{\partial x_2} &\cdots &\frac{\partial f_n(x_k)}{\partial x_m} \\ \end{matrix} \right]$
通常记

fk=f(xk) $f_k=f(x_k)$ ，

Jk=J(xk) $J_k=J(x_k)$ .

\partial F ( x ) \partial x j = \sum i = 1 n f i (x) \partial f i ( x ) \partial x j

$\frac{\partial F(x)}{\partial x_j} = \sum_{i=1}^{n}f_i(x)\frac{\partial f_i(x)}{\partial x_j}$
所以

F(x) $F(x)$ 的梯度：

g = F' (x) = J (x) T f (x)

$g=F'(x)=J(x)^Tf(x)$

GaussNewton

选择 $h$ 使得 $F(x)$ 在 $x_k$ 附近二阶近似，则

F (x k + h) \approx L (h) = 1 2 f (x k + h) T f (x k + h) = 1 2 f T k f k + h T J T k f k + 1 2 h T J T k J k h = F (x k) + h T J T k f k + 1 2 h T J T k J k h

$\begin{aligned} F(x_k+h) \approx L(h) & = \frac{1}{2}f(x_k+h)^Tf(x_k+h) \\ & = \frac{1}{2}f_k^Tf_k+h^TJ_k^Tf_k+\frac{1}{2}h^TJ_k^TJ_kh \\ & = F(x_k) + h^TJ_k^Tf_k+\frac{1}{2}h^TJ_k^TJ_kh \end{aligned}$

为使 $F(x_k+h)$ 取得极小值， $L(h)$ 对h一阶导数 $\frac{\partial L(h)}{\partial h} = J_k^Tf_k+J_k^TJ_kh$ ，令 $L'(h)=0$ ，则 $(J_k^TJ_k)h_{gn}=-J_k^Tf_k$ 。
当 $J_k^TJ_k$ 非奇异的时候， $h_{gn}=-(J_k^TJ_k)^{-1}J_k^Tf_k$ ， $x_{k+1}=x_k+h_{gn}$ 。
由于GaussNewton求解过程中需要对 $J^TJ$ 求逆，所以当 $J^TJ$ 变成奇异的，GaussNewton方法失效。另外当 $x_0$ 离极小点较远时，GaussNewton算法可能会发散。

总结以上，GaussNewton法一般求解步骤：
- step1：根据 $(J_k^TJ_k)h_{gn}=-J_k^Tf_k$ ，求解迭代步长 $h_{gn}$ ；
- step2： $x_{k+1}=x_k+h_{gn}$ 进行新的迭代；
- step3：若 $\left |F(x_{k+1})-F(x_k) \right|< \epsilon$ ，其中 $\epsilon$ 足够小，则认为 $F(x)$ 以收敛，则退出迭代，否则重复step1；

通常GaussNewton法收敛较快，但是不稳定。而最快下降法稳定，但是收敛较慢。所以接下来我们介绍GaussNewton和最快下降法混合法。

LM阻尼最小二乘法

GaussNewton法是用 $(J_k^TJ_k)h=-J_k^Tf_k$ 来确定 $h$ ，现在假设在 $J_k^TJ_k$ 对角线上元素都加上同一个数 $u>0$ ，
$(J_k^TJ_k+uI)h=-J_k^Tf_k$
这样即使当 $J_k^TJ_k$ 奇异，只要 $u$ 取得充分大，总能使 $(J_k^TJ_k+uI)$ 正定，则 $(J_k^TJ_k+uI)h=-J_k^Tf_k$ 肯定有解。这个解依赖于 $u$ ，记作 $h_{lm}$ 。

⎧ ⎩ ⎨ ⎪ ⎪ 当 u = 0 当 u 充 分 大 h l m \approx h g n ， 即 为 G a u s s N e w t o n 法 . u I h l m \approx - J T k f k ， h l m = - 1 u J T k f k ， 即 为 最 快 下 降 法 . 特 别 当 u \to \infty ， ∥ h l m ∥ \to 0

$\begin{cases} 当u=0 &h_{lm}\approx h_{gn}，即为GaussNewton法.\\ 当u充分大 &uIh_{lm}\approx -J_k^Tf_k，h_{lm}=-\frac{1}{u}J_k^Tf_k ，即为最快下降法.\\ &特别当u \to \infty，\left\|h_{lm} \right\| \to 0 \\ \end{cases}$
因此

u $u$ 起着使步长

∥hlm∥ $\left\|h_{lm}\right\|$ 缩短的或阻尼的作用，此即为阻尼最小二乘法。

那么LM阻尼最小二乘法实际迭代过程中怎样调整 $u$ ？
假设 $\left\|h\right\|$ 足够小，对 $f(x+h)$ 一阶近似 $f(x+h) \approx \iota(h)=f(x)+J(x)h$
则对 $F(x+h)$ 二阶近似，

F (x + h) \approx L (h) = 1 2 f (x + h) T f (x + h) = 1 2 f T f + h T J T f + 1 2 h T J T J h = F (x) + h T J T f + 1 2 h T J T J h

$\begin{aligned} F(x+h) \approx L(h) & = \frac{1}{2}f(x+h)^Tf(x+h) \\ & = \frac{1}{2}f^Tf+h^TJ^Tf+\frac{1}{2}h^TJ^TJh \\ & = F(x) + h^TJ^Tf+\frac{1}{2}h^TJ^TJh \end{aligned}$
我们定义一个增益比

ρ=F(x)−F(x+hlm)L(0)−L(hlm) $\rho=\frac{F(x)-F(x+h_{lm})}{L(0)-L(h_{lm})}$
在实际中，我们选择一阶近似、二阶近似并不是在所有定义域都满足的，而是在

[x−ϵ,x+ϵ] $[x- \epsilon,x+\epsilon]$ 作用域内满足这个近似条件。
当

ρ $\rho$ 较大时，表明

F(x+h) $F(x+h)$ 的二阶近似

L(h) $L(h)$ 比

F(x+h) $F(x+h)$ 更加接近于

F(x) $F(x)$ ，因此二阶近似比较好，所以可以减小

u $u$ ，采用更大的迭代步长，接近GaussNewton法来更快收敛；
而当

ρ $\rho$ 较小时，表明采取的二阶近似较差，因此通过增大

u $u$ ，采用更小的步长，接近最快下降法来稳定的迭代。
一种比较好的阻尼系数

u $u$ 随

ρ $\rho$ 选择策略：
初始值

A0=J(x0)TJ(x0) $A_0=J(x_0)^TJ(x_0)$ ，

u0=τ∗max{aii} $u_0=\tau*max\{a_{i{i}} \}$ ，

v0=2 $v_0=2$ ，算法对

τ $\tau$ 取值不敏感，

τ $\tau$ 可以取

10−6 $10^{-6}$ 、

10−3 $10^{-3}$ 或者1都可以。

{if ρ >0 e l s e u : = u * m a x {1 3, 1 - (2 ρ - 1) 3} u : = u * v v : = 2; v : = 2 * v;

$\begin{cases} \text{if $\rho$>0} &u:=u*max\{\frac{1}{3},1-(2\rho-1)^3\} &v:=2;\\ else &u:=u*v &v:=2*v;\\ \end{cases}$

总结以上，LM阻尼最小二乘法求解步骤：
- step1：初始化 $u_0=\tau*max\{a_{i{i}} \}$ ， $v_0=2$ ；
- step2：求解梯度 $g_k=J_k^Tf_k$ ，如果 $\left\|g_k\right\|\le\epsilon_1$ ，则退出，否则继续；
- step3：根据 $(J_k^TJ_k+u_kI)h_{lm}=-J_k^Tf_k$ ，求解迭代步长 $h_{lm}$ 。若 $\left\|h_{lm} \right\| \le \epsilon_2(\left\|x\right\|+\epsilon_2)$ ，则退出迭代，否则继续;
- step4： $x_{new}=x_k+h_{lm}$ ，计算增益比 $\rho=\frac{F(x_k)-F(x_{new})}{L(0)-L(h_{lm})}$ 。
如果 $\rho>0$ ，则 $x_{k+1}=x_{new}$ ， $u_{k+1}=u_k*max\{\frac{1}{3},1-(2\rho-1)^3\}$ ， $v_{k+1}=2$ ；
否则 $u_{k+1}=u_k*v_k$ ， $v_{k+1}=2*v_k$ 。重复step2；

对于 $\epsilon_1$ 、 $\epsilon_2$ 可以选取任意小的值如 $10^{-12}$ ，只是作为迭代的终止条件，其值得选取对最终的收敛结果影响不大。

Dog-Leg最小二乘法

另外一个GaussNewton和最快下降法混合方法是Dog-Leg法，代替阻尼项而是用trust region。
回到上面讲到最快下降法，下降方向 $h_{sd}=-F'(x_k)=-J_k^Tf_k$ ，但是步进多长呢？或者沿着这个方向步进多长能使得 $F(x)$ 取得最大程度的下降呢？
假设 $f(x+\alpha h_{sd}) \approx f(x)+\alpha J(x)h_{sd}$

F (x + α h s d) \approx = 1 2 ∥ f (x) + α J (x) h s d ∥ 2 F (x) + α h T s d J (x) T f (x) + 1 2 α 2 ∥ J (x) h s d ∥ 2

$\begin{eqnarray*} F(x+\alpha h_{sd}) & \approx & \frac{1}{2}\left\|f(x)+\alpha J(x)h_{sd}\right\|^2 \\& = & F(x)+\alpha h_{sd}^TJ(x)^Tf(x)+\frac{1}{2}\alpha^2\left\|J(x)h_{sd}\right\|^2 \end{eqnarray*}$
为使得

F(x+αhsd) $F(x+\alpha h_{sd})$ 最小，对

α $\alpha$ 求导，

α = - h T s d J ( x ) T f ( x ) ∥ J ( x ) h s d ∥ 2 = ∥ g ∥ 2 ∥ J ( x ) g ∥ 2

$\alpha = -\frac{h_{sd}^TJ(x)^Tf(x)}{\left\|J(x)h_{sd}\right\|^2}=\frac{\left\|g\right\|^2}{\left\|J(x)g\right\|^2}$
则如果GaussNewton法则

xk+1−xk=hgn $x_{k+1}-x_k=h_{gn}$ ；如果最快下降法则

xk+1−xk=αhsd $x_{k+1}-x_k=\alpha h_{sd}$ 。

继续trust region是什么呢？
trust region即为在 $\left\|h\right\| \le \Delta$ 范围内， $F(x+h)= F(x) + h^TJ^Tf+\frac{1}{2}h^TJ^TJh$ 能够较好的近似，因此不管我们在选择GaussNewton还是最快下降法，必须满足 $\left\|h_{gn}\right\| \le \Delta$ 、 $\left\| \alpha h_{sd}\right\| \le \Delta$ ，二阶近似才能较好成立。
然后继续，Dog-Leg迭代步长 $h_{dl}$ 和 $h_{gn}$ 、 $\alpha h_{sd}$ 、 $\Delta$ 有什么关系？

⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ if ∥ ∥ h g n ∥ ∥ \leq Δ else if ∥ α h s d ∥ \geq Δ else h d l = h g n h d l = Δ ∥ h s d ∥ h s d h d l = α h s d + β (h g n - α h s d) 选 择 β 使 得 ∥ h d l ∥ = Δ

$\begin{cases} \text{if $\left\|h_{gn}\right\|\le\Delta$} &h_{dl}=h_{gn}\\ \text{else if $\left\|\alpha h_{sd}\right\|\ge\Delta$} &h_{dl}=\frac{\Delta}{\left\|h_{sd}\right\|} h_{sd}\\ \text{else} &h_{dl}=\alpha h_{sd}+\beta(h_{gn}-\alpha h_{sd})\\ &选择\beta使得\left\|h_{dl}\right\|=\Delta \end{cases}$

hdl $h_{dl}$ 和

hgn $h_{gn}$ 、

αhsd $\alpha h_{sd}$ 、

Δ $\Delta$ 关系示意图：

和阻尼最小二乘法类似，实际中怎样更新trust region半径呢？
继续选择增益比 $\rho=\frac{F(x)-F(x+h_{dl})}{L(0)-L(h_{dl})}$

{if ρ > 0.75 if ρ < 0.25 Δ : = m a x {Δ, 3 * ∥ h d l ∥} Δ : = Δ / 2

$\begin{cases} \text{if $\rho>0.75$} &\Delta:=max\{\Delta,3*\left\|h_{dl}\right\|\} \\ \text{if $\rho<0.25$} &\Delta:=\Delta/2 \\ \end{cases}$

总结以上，Dog-Leg最小二乘法求解步骤：
- step1：初始化 $\Delta_0$ 。
- step2：求解梯度 $g_k=J_k^Tf_k$ ，如果 $\left\|g_k\right\|\le\epsilon_1$ ，则退出，否则继续。如果 $\left\|f(x_k)\right\| \le \epsilon_3$ ，则退出，否则继续。
- step3：如果trust region半径 $\Delta_k\le \epsilon_2(\left\|x_k\right\|+\epsilon_2)$ ，则退出迭代；否则继续；
- step4：分别根据GaussNewton法和最快下降法计算 $h_{gn}$ 和 $h_{sd}$ ，然后计算最快下降法的迭代步长 $\alpha =\frac{\left\|g\right\|^2}{\left\|J(x)g\right\|^2}$ 。
- step5：根据 $h_{gn}$ 、 $h_{sd}$ 和trust region半径 $\Delta_k$ ，来计算Dog-Leg步进值 $h_{dl}$ 。若 $\left\|h_{dl} \right\| \le \epsilon_2(\left\|x_k\right\|+\epsilon_2)$ ，则退出迭代；否则继续。
- step6： $x_{new}=x_k+h_{dl}$ ，计算增益比 $\rho=\frac{F(x_k)-F(x_{new})}{L(0)-L(h_{dl})}$ 。

⎧ ⎩ ⎨ if ρ > 0 if ρ > 0.75 elseif ρ < 0.25 x k + 1 = x n e w ； Δ k + 1 = m a x {Δ k ， 3 * ∥ h d l ∥} ； Δ k + 1 = Δ k / 2 ；

$\begin{cases} \text{if $\rho > 0$} &x_{k+1}=x_{new}； \\ \text{if $\rho > 0.75$} &\Delta_{k+1}=max\{\Delta_k，3*\left\|h_{dl}\right\|\}；\\ \text{elseif $\rho <0.25$} &\Delta_{k+1}=\Delta_k/2；\\ \end{cases}$
重复step2。

对于 $\epsilon_1$ 、 $\epsilon_2$ 、 $\epsilon_3$ 可以选取任意小的值如 $10^{-12}$ ，只是作为迭代的终止条件，其值得选取对最终的收敛结果影响不大。

举例:

拟合 $f(x)=Asin(Bx)+Ccos(Dx)$ ，假设4个参数已知A=5、B=1、C=10、D=2，构造100个随机数作为x的采样值，然后计算 $f(x)$ 再在上面加一个随机扰动量作为观测值。然后，利用输入与输出，来反推四个参数。

Gauss Newton代码：

double func(const VectorXd& input, const VectorXd& output, const VectorXd& params, double objIndex)
{// obj = A * sin(Bx) + C * cos(D*x) - Fdouble x1 = params(0);double x2 = params(1);double x3 = params(2);double x4 = params(3);double t = input(objIndex);double f = output(objIndex);return x1 * sin(x2 * t) + x3 * cos( x4 * t) - f;
}//return vector make up of func() element.
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;
}//F = (f ^t * f)/2
double Func(const VectorXd& obj)
{return obj.squaredNorm()/2;
}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);
}MatrixXd Jacobin(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;
}void gaussNewton(const VectorXd& input, const VectorXd& output, VectorXd& params)
{int errNum = input.rows();      //error  numint paraNum = params.rows();    //parameter  numVectorXd obj(errNum);double last_sum = 0;int iterCnt = 0;while (iterCnt < MAX_ITER){obj = objF(input, output, params);double sum = 0;sum = Func(obj);cout << "Iterator index: " << iterCnt << endl;cout << "parameter: " << endl << params << endl;cout << "error sum: " << endl << sum << endl << endl;if (fabs(sum - last_sum) <= 1e-12)break;last_sum = sum;MatrixXd Jac = Jacobin(input, output, params);VectorXd delta(paraNum);delta = (Jac.transpose() * Jac).inverse() * Jac.transpose() * obj;params -= delta;iterCnt++;}
}

LM代码：

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;
}//L(h) = F(x) + h^t*J^t*f + h^t*J^t*J*h/2
//deltaL = h^t * (u * h - g)/2
double linerDeltaL(const VectorXd& step, const VectorXd& gradient, const double u)
{double L = step.transpose() * (u * step - gradient);return L/2;
}void levenMar(const VectorXd& input, const VectorXd& output, VectorXd& params)
{int errNum = input.rows();      //error numint paraNum = params.rows();    //parameter num//initial parameter VectorXd obj = objF(input,output,params);MatrixXd Jac = Jacobin(input, output, params);  //jacobinMatrixXd A = Jac.transpose() * Jac;             //HessianVectorXd gradient = Jac.transpose() * obj;      //gradient//initial parameter tao v epsilon1 epsilon2double tao = 1e-3;long long v = 2;double eps1 = 1e-12, eps2 = 1e-12;double u = tao * maxMatrixDiagonale(A);bool found = gradient.norm() <= eps1;if(found) return;double last_sum = 0;int iterCnt = 0;while (iterCnt < MAX_ITER){VectorXd obj = objF(input,output,params);MatrixXd Jac = Jacobin(input, output, params);  //jacobinMatrixXd A = Jac.transpose() * Jac;             //HessianVectorXd gradient = Jac.transpose() * obj;      //gradientif( gradient.norm() <= eps1 ){cout << "stop g(x) = 0 for a local minimizer optimizer." << endl;break;}cout << "A: " << endl << A << endl; VectorXd step = (A + u * MatrixXd::Identity(paraNum, paraNum)).inverse() * gradient; //negtive Hlm.cout << "step: " << endl << step << endl;if( step.norm() <= eps2*(params.norm() + eps2) ){cout << "stop because change in x is small" << endl;break;} VectorXd paramsNew(params.rows());paramsNew = params - step; //h_lm = -step;//compute f(x)obj = objF(input,output,params);//compute f(x_new)VectorXd obj_new = objF(input,output,paramsNew);double deltaF = Func(obj) - Func(obj_new);double deltaL = linerDeltaL(-1 * step, gradient, u);double roi = deltaF / deltaL;cout << "roi is : " << roi << endl;if(roi > 0){params = paramsNew;u *= max(1.0/3.0, 1-pow(2*roi-1, 3));v = 2;}else{u = u * v;v = v * 2;}cout << "u = " << u << " v = " << v << endl;iterCnt++;cout << "Iterator " << iterCnt << " times, result is :" << endl << endl;}
}

Dog-Leg代码：

void dogLeg(const VectorXd& input, const VectorXd& output, VectorXd& params)
{int errNum = input.rows();      //error numint paraNum = params.rows();    //parameter numVectorXd obj = objF(input, output, params);MatrixXd Jac = Jacobin(input, output, params);  //jacobinVectorXd gradient = Jac.transpose() * obj;      //gradient//initial parameter tao v epsilon1 epsilon2double eps1 = 1e-12, eps2 = 1e-12, eps3 = 1e-12;double radius = 1.0;bool found  = obj.norm() <= eps3 || gradient.norm() <= eps1;if(found) return;double last_sum = 0;int iterCnt = 0;while(iterCnt < MAX_ITER){VectorXd obj = objF(input, output, params);MatrixXd Jac = Jacobin(input, output, params);  //jacobinVectorXd gradient = Jac.transpose() * obj;      //gradientif( gradient.norm() <= eps1 ){cout << "stop F'(x) = g(x) = 0 for a global minimizer optimizer." << endl;break;}if(obj.norm() <= eps3){cout << "stop f(x) = 0 for f(x) is so small";break;}//compute how far go along stepest descent direction.double alpha = gradient.squaredNorm() / (Jac * gradient).squaredNorm();//compute gauss newton step and stepest descent step.VectorXd stepest_descent = -alpha * gradient;VectorXd gauss_newton = (Jac.transpose() * Jac).inverse() * Jac.transpose() * obj * (-1);double beta = 0;//compute dog-leg step.VectorXd dog_leg(params.rows());if(gauss_newton.norm() <= radius)dog_leg = gauss_newton;else if(alpha * stepest_descent.norm() >= radius)dog_leg = (radius / stepest_descent.norm()) * stepest_descent;else{VectorXd a = alpha * stepest_descent;VectorXd b = gauss_newton;double c = a.transpose() * (b - a);beta = (sqrt(c*c + (b-a).squaredNorm()*(radius*radius-a.squaredNorm()))-c)/(b-a).squaredNorm();dog_leg = alpha * stepest_descent + beta * (gauss_newton - alpha * stepest_descent);}cout << "dog-leg: " << endl << dog_leg << endl;if(dog_leg.norm() <= eps2 *(params.norm() + eps2)){cout << "stop because change in x is small" << endl;break;}VectorXd new_params(params.rows());new_params = params + dog_leg;cout << "new parameter is: " << endl << new_params << endl;//compute f(x)obj = objF(input,output,params);//compute f(x_new)VectorXd obj_new = objF(input,output,new_params);//compute delta F = F(x) - F(x_new)double deltaF = Func(obj) - Func(obj_new);//compute delat L =L(0)-L(dog_leg)double deltaL = 0;if(gauss_newton.norm() <= radius)deltaL = Func(obj);else if(alpha * stepest_descent.norm() >= radius)deltaL = radius*(2*alpha*gradient.norm() - radius)/(2.0*alpha);else{VectorXd a = alpha * stepest_descent;VectorXd b = gauss_newton;double c = a.transpose() * (b - a);beta = (sqrt(c*c + (b-a).squaredNorm()*(radius*radius-a.squaredNorm()))-c)/(b-a).squaredNorm();deltaL = alpha*(1-beta)*(1-beta)*gradient.squaredNorm()/2.0 + beta*(2.0-beta)*Func(obj);}double roi = deltaF / deltaL;if(roi > 0){params = new_params;}if(roi > 0.75){radius = max(radius, 3.0 * dog_leg.norm());}else if(roi < 0.25){radius = radius / 2.0;if(radius <= eps2*(params.norm()+eps2)){cout << "trust region radius is too small." << endl;break;}}cout << "roi: " << roi << " dog-leg norm: " << dog_leg.norm() << endl;cout << "radius: " << radius << endl;iterCnt++;cout << "Iterator " << iterCnt << " times" << endl << endl;}
}

main()

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Sparse>
#include <iostream>
#include <iomanip>
#include <math.h>using namespace std;
using namespace Eigen;const double DERIV_STEP = 1e-5;
const int MAX_ITER = 100;#define max(a,b) (((a)>(b))?(a):(b))int main(int argc, char* argv[])
{// obj = A * sin(Bx) + C * cos(D*x) - F//there are 4 parameter: A, B, C, D.int num_params = 4;//generate random data using these parameterint total_data = 100;VectorXd input(total_data);VectorXd output(total_data);double A = 5, B= 1, C = 10, D = 2;//load observation datafor (int i = 0; i < total_data; i++){//generate a random variable [-10 10]double x = 20.0 * ((random() % 1000) / 1000.0) - 10.0;double deltaY = 2.0 * (random() % 1000) /1000.0;double y = A*sin(B*x)+C*cos(D*x) + deltaY;input(i) = x;output(i) = y;}//gauss the parametersVectorXd params_gaussNewton(num_params);//init gaussparams_gaussNewton << 1.6, 1.4, 6.2, 1.7;VectorXd params_levenMar = params_gaussNewton;VectorXd params_dogLeg   = params_gaussNewton;gaussNewton(input, output, params_gaussNewton);levenMar(input, output, params_levenMar);dogLeg(input, output, params_dogLeg);cout << "gauss newton parameter: " << endl << params_gaussNewton << endl << endl << endl;cout << "Levenberg-Marquardt parameter: " << endl << params_levenMar << endl << endl << endl;cout << "dog-leg parameter: " << endl << params_dogLeg << endl << endl << endl;
}

least square
通常对于GaussNewton、LM、Dog-Leg，如果初始化的参数离真实值较近时，这三种方法都能收敛到真实值，而像例子中初始化参数 $[1.6,1.4,6.2,1.7]^T$ 与真实参数 $[5,1,10,2]^T$ 差距较大时，最后收敛的结果与真实值有一定的偏差。因为最小二乘法依赖于初始化参数，最后收敛的只能保证是一个极值点，但是不能保证是全局最优点，图中可以看到LM、Dog-Leg收敛的结果明显优于GaussNewton。