LM算法探讨(附python代码)

1. 案例分析

考虑如下公式：

\begin{matrix} (1.1) & γ_{i} = \frac{2 π}{λ} \times 2 \sqrt{(x_{i} - x_{p})^{2} + (y_{i} - y_{p})^{2} + (z_{i} - z_{p})^{2}} \end{matrix}

$\gamma_i=\frac{2\pi}{\lambda}\times 2 \sqrt{(x_i-x_p)^2+(y_i-y_p)^2+(z_i-z_p)^2}\tag{1.1}$

其中 $\gamma_i$ 会随 $x_i$ 、 $y_i$ 、 $z_i$ 而改变。即我们可以将 $(x_i,y_i,z_i)$ 视为自变量， $\gamma_i$ 为因变量。而 $\lambda$ 与 $(x_p,y_p,z_p)$ 为常数。

现在通过测量，我们得出了n组 $[x_i,y_i,z_i,\lambda_i]$ 的值，并且 $\lambda$ 已知，我们需要求解常数组 $(x_p,y_p,z_p)$ 。
我们可以将问题转化为如下的最小二乘拟合：

\begin{matrix} (1.2) & m i n \sum_{i = 1}^{n} (\frac{2 π}{λ} \times 2 \sqrt{(x_{i} - x_{p})^{2} + (y_{i} - y_{p})^{2} + (z_{i} - z_{p})^{2}} - γ_{i})^{2} \end{matrix}

$min\sum_{i=1}^{n} (\frac{2\pi}{\lambda}\times 2 \sqrt{(x_i-x_p)^2+(y_i-y_p)^2+(z_i-z_p)^2}-\gamma_i)^2\tag{1.2}$

且当上式越接近于零，拟合值越好。

2.算法引入

上述问题所描述的内容，我们称之为最优化问题。即我们希望找到最优的一组 $(x_p,y_p,z_p)$ 使得式(1.2)尽可能趋近于0。解决最优化问题的算法有很多。小到梯度下降算法，大到粒子群算法等。这里我们从梯度下降算法引入，主要讲解LM算法。

2.1 梯度下降算法

梯度下降算法的核心思路是迭代。即从某一个初始值开始，沿特定的方向，逐步寻找最优解。梯度下降算法有几个核心要点，即初始点，学习率（步长），精度判断条件（何时停止）。梯度下降算法可以参考这篇文章。

优点：简单。缺点：负梯度方向，收敛速度慢。

2.2 Newton 法

Newton法与梯度下降算法类似，但不同的是，梯度下降算法相当于保留了泰勒级数的一阶项（梯度），而Newton法保留泰勒级数一阶和二阶项，拥有二次收敛速度。Newton法涉及到Hessian矩阵,其迭代思路如下：

优点：理论上比梯度下降法快。缺点：每步都计算Hessian矩阵，复杂（矩阵求逆计算量大）。

2.3 高斯牛顿法

与牛顿法类似，但采用 $H=J^TJ$ 对牛顿法中的海塞矩阵 $H(x_k)$ 进行近似，从而简化了计算量。

高斯牛顿法的算法流程：

高斯牛顿法的缺点(参考)：

$JJ^{T}$ 只有半正定的性质，在计算 $(JJ^{T})^{-1}$ 的过程中，如果 $JJ^{T}$ 为奇异矩阵或病态矩阵可能导致增量不稳定甚至算法不收敛。

2.4 L-M算法（Levenberg–Marquardt方法）

L-M算法引入了信赖域。将优化问题从无约束的最小二乘问题变成了有约束的最小二乘问题。
可以简单地理解为：迭代前期使用梯度下降法，迭代后期使用高斯牛顿法。结合前面讲到的奇异或病态的问题，LM算法的核心用信赖域限制病态的发生。

其算法流程为（参考）：

有关Levenberg–Marquardt方法的详细使用可以参考这篇IEEE论文。

3. python编程实现

（还是matlab方便 /doge/ ）

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import numpy as np
from numpy import matrix as mat
import math

# 导入数据
Label_location = [0.9, 1.2, 2.0]
theta_data = [60.31857894892403, 48.25486315913922, 80.4247719318987, 80.4247719318987]
lambda_data = [0.3125, 0.3125, 0.3125, 0.3125]
xi_data = [0.0, 0.9, 2.5, 0.9]
yi_data = [0.0, 0.0, 0.0, 0.0]
zi_data = [2.0, 2.0, 2.0, 0.4]
# 合并为一个矩阵，然后转置,每一行为一组λ，xi,yi,zi。
Variable_Matrix = mat([lambda_data, xi_data, yi_data, zi_data]).T


def Func(parameter, iput):  # 需要拟合的函数，abc是包含三个参数的一个矩阵[[a],[b],[c]]
    x = parameter[0, 0]
    y = parameter[1, 0]
    z = parameter[2, 0]
    residual = mat((4*np.pi / iput[0, 0])*np.sqrt(np.square(iput[0, 1]-x)+np.square(iput[0, 2]-y)+np.square(iput[0, 3]-z)))
    return residual


def Deriv(parameter, iput):  # 对函数求偏导
    x = parameter[0, 0]
    y = parameter[1, 0]
    z = parameter[2, 0]
    x_deriv = -4*np.pi*(iput[0, 1]-x) / (iput[0, 0] * np.sqrt(np.square(iput[0, 1]-x)+np.square(iput[0, 2]-y) + np.square(iput[0, 3]-z)))
    y_deriv = -4*np.pi*(iput[0, 2]-y) / (iput[0, 0] * np.sqrt(np.square(iput[0, 1]-x)+np.square(iput[0, 2]-y) + np.square(iput[0, 3]-z)))
    z_deriv = -4*np.pi*(iput[0, 3]-z) / (iput[0, 0] * np.sqrt(np.square(iput[0, 1]-x)+np.square(iput[0, 2]-y) + np.square(iput[0, 3]-z)))
    deriv_matrix = mat([x_deriv, y_deriv, z_deriv])
    return deriv_matrix


n = len(theta_data)
J = mat(np.zeros((n, 3)))  # 雅克比矩阵
fx = mat(np.zeros((n, 1)))  # f(x)  3*1  误差
fx_tmp = mat(np.zeros((n, 1)))
initialization_parameters = mat([[10], [400], [30]])  # 参数初始化
lase_mse = 0.0
step = 0.0
u, v = 1.0, 2.0
conve = 100


while conve:
    mse, mse_tmp = 0.0, 0.0
    step += 1
    for i in range(len(theta_data)):
        fx[i] = Func(initialization_parameters, Variable_Matrix[i]) - theta_data[i]  # 注意不能写成  y - Func  ,否则发散
        # print(fx[i])
        mse += fx[i, 0] ** 2
        J[i] = Deriv(initialization_parameters, Variable_Matrix[i])  # 数值求导
    mse = mse/n  # 范围约束
    H = J.T * J + u * np.eye(3)  # 3*3
    dx = -H.I * J.T * fx  # 注意这里有一个负号，和fx = Func - y的符号要对应

    initial_parameters_tmp = initialization_parameters.copy()
    initial_parameters_tmp = initial_parameters_tmp + dx
    for j in range(len(theta_data)):
        fx_tmp[j] = Func(initial_parameters_tmp, Variable_Matrix[j]) - theta_data[j]
        mse_tmp += fx_tmp[j, 0] ** 2
    mse_tmp = mse_tmp/n

    q = (mse - mse_tmp) / ((0.5 * dx.T * (u * dx - J.T * fx))[0, 0])
    print(q)
    if q > 0:
        s = 1.0 / 3.0
        v = 2
        mse = mse_tmp
        initialization_parameters = initial_parameters_tmp
        temp = 1 - pow(2 * q - 1, 3)
        if s > temp:
            u = u * s
        else:
            u = u * temp
    else:
        u = u * v
        v = 2 * v
        mse = mse_tmp
        initialization_parameters = initial_parameters_tmp
    print("step = %d,parameters(mse-lase_mse) = " % step, abs(mse - lase_mse))
    if abs(mse - lase_mse) < math.pow(0.1, 14):
        break
    lase_mse = mse  # 记录上一个 mse 的位置
    conve -= 1
print(lase_mse)
print(initialization_parameters)