LM算法探討(附python代碼)

1. 案例分析

考慮如下公式：

\[\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)\)。
我們可以將問題轉化為如下的最小二乘擬合：

\[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)

代碼僅供參考，建議結合上面的L-M算法流程來看。（部分命名不規范還望諒解）
說明：代碼中給定的四組參數本來就是用來驗算的，其結果應該為[[0.9],[1.2],[2. ]]。而最終運算的結果也為：

也檢驗了算法的可行性。