前言

昨天写了通过牛顿法计算函数极值，也比较了牛顿法与最速下降法的求解次数与速度。

本篇记录高斯牛顿法。高斯牛顿法适合求解最小二乘形式的极值。

非线性最小二乘

考虑一个非线性标量函数 f ( x ) f({\bf x}) f(x)， R n → R R^n\to R Rn→R，其最小二乘估计形式表示为：
min ⁡ x 1 2 ∣ ∣ f ( x ) ∣ ∣ 2 \min_x \frac{1}{2}||f({\bf x})||^2 xmin​21​∣∣f(x)∣∣2
这是一个非线性的最小二乘问题。所谓非线性，就是 f ( x ) f{(\bf x)} f(x)与 x \bf x x并不是线性关系。

举个例子：SLAM中，已知一个三维世界点 P i P_i Pi​映射到相机相邻帧的坐标为 p i , p i ′ p_i,p_i' pi​,pi′​，现在需要求相机在相邻帧之间的位姿变换 T T T。由于传感器噪声、观测误差等原因， P i P_i Pi​在相机中的真实位置可能与 p i , p i ′ p_i,p_i' pi​,pi′​相差几个像素，因此我们通过最小二乘获得使这种偏差最小的位姿：
arg min ⁡ T 1 2 ∣ ∣ ∑ i = 1 n ( K T ) − 1 Z ( p i − p i ′ ) ∣ ∣ 2 2 \argmin_T \frac{1}{2}||\sum_{i=1}^n (KT)^{-1}Z(p_i-p_i')||_2^2 Targmin​21​∣∣i=1∑n​(KT)−1Z(pi​−pi′​)∣∣22​
以上表达式中的函数是一个向量的范数，是非线性的。

这也可以解释，为什么非线性最小二乘中带有一个 ∣ ∣ ⋅ ∣ ∣ ||\cdot|| ∣∣⋅∣∣标志。

高斯牛顿法

回顾梯度下降法和牛顿法的迭代思路，都是通过泰勒展开函数并求导，以此获得每次迭代的增量表达式。高斯牛顿法采用了这种思路来解决非线性最小二乘问题：
min ⁡ x 1 2 ∣ ∣ f ( x ) ∣ ∣ 2 = min ⁡ x 1 2 ∣ ∣ f ( x 0 ) + ∇ f ( x 0 ) T ( x − x 0 ) ∣ ∣ 2 = min ⁡ x 1 2 ( f ( x 0 ) + ∇ f ( x 0 ) T ( x − x 0 ) ) T ( f ( x 0 ) + ∇ f ( x 0 ) T ( x − x 0 ) ) = min ⁡ x 1 2 ( f T ( x 0 ) f ( x 0 ) + f ( x 0 ) T ∇ f ( x 0 ) T ( x − x 0 ) + ( x − x 0 ) T ∇ f ( x 0 ) f ( x 0 ) + ( x − x 0 ) T ∇ f ( x 0 ) ∇ f ( x 0 ) T ( x − x 0 ) ) \min_{x} \frac{1}{2}||f{\bf (x)}||^2=\min_x \frac{1}{2} ||f({\bf x_0)}+ \nabla f({\bf x_0})^T{\bf (x-x_0)}||^2 \\ = \min_x \frac{1}{2} (f({\bf x_0)}+ \nabla f({\bf x_0})^T{\bf (x-x_0)})^T(f({\bf x_0)}+ \nabla f({\bf x_0})^T{\bf (x-x_0)}) \\ = \min_x \frac {1}{2} (f^T({\bf x_0})f({\bf x_0)}+ f({\bf x_0})^T\nabla f({\bf x_0})^T{\bf (x-x_0)}+({\bf x-x_0})^T\nabla f({\bf x_0})f({\bf x_0})+({\bf x-x_0})^T\nabla f({\bf x_0})\nabla f({\bf x_0})^T{\bf (x-x_0)}) \\ xmin​21​∣∣f(x)∣∣2=xmin​21​∣∣f(x0​)+∇f(x0​)T(x−x0​)∣∣2=xmin​21​(f(x0​)+∇f(x0​)T(x−x0​))T(f(x0​)+∇f(x0​)T(x−x0​))=xmin​21​(fT(x0​)f(x0​)+f(x0​)T∇f(x0​)T(x−x0​)+(x−x0​)T∇f(x0​)f(x0​)+(x−x0​)T∇f(x0​)∇f(x0​)T(x−x0​))
用 J 代 替 ∇ f T J代替\nabla f^T J代替∇fT， Δ x \Delta \bf x Δx代替 x − x 0 \bf x-x_0 x−x0​，将上式对 Δ x \Delta \bf x Δx求导，构造极值条件：
f ( x 0 ) T J ( x 0 ) + Δ x T J ( x 0 ) T J ( x 0 ) = 0 J ( x 0 ) T f ( x 0 ) = − J ( x 0 ) T J ( x 0 ) Δ x Δ x = − ( J ( x 0 ) T J ( x 0 ) ) − 1 ( J ( x 0 ) T f ( x 0 ) ) f({\bf x_0})^TJ({\bf x_0})+\Delta{\bf x}^TJ({\bf x_0})^TJ({\bf x_0})={\bf 0} \\ J({\bf x_0})^Tf({\bf x_0}) = -J({\bf x_0})^TJ({\bf x_0})\Delta{\bf x} \\ \Delta{\bf x} = -(J({\bf x_0})^TJ({\bf x_0}))^{-1}(J({\bf x_0})^Tf({\bf x_0}) )\\ f(x0​)TJ(x0​)+ΔxTJ(x0​)TJ(x0​)=0J(x0​)Tf(x0​)=−J(x0​)TJ(x0​)ΔxΔx=−(J(x0​)TJ(x0​))−1(J(x0​)Tf(x0​))
得到迭代增量表达式：
x = x 0 − ( J ( x 0 ) T J ( x 0 ) ) − 1 ( J ( x 0 ) T f ( x 0 ) ) {\bf x}={\bf x_0}-(J({\bf x_0})^TJ({\bf x_0}))^{-1}(J({\bf x_0})^Tf({\bf x_0}) ) x=x0​−(J(x0​)TJ(x0​))−1(J(x0​)Tf(x0​))

牛顿法与高斯牛顿法

牛顿法的增量表达式为：
Δ x = − H ( x 0 ) − 1 J ( x 0 ) \Delta {\bf x} = -{H({\bf x_0})^{-1}J({\bf x_0})} Δx=−H(x0​)−1J(x0​)

高斯牛顿法的增量表达式为：
Δ x = − ( J ( x 0 ) T J ( x 0 ) ) − 1 J ( x 0 ) f ( x 0 ) \Delta {\bf x} = -{(J({\bf x_0})^TJ({\bf x_0}))^{-1}J({\bf x_0})}f({\bf x_0}) Δx=−(J(x0​)TJ(x0​))−1J(x0​)f(x0​)
可以看出，高斯牛顿法不再需要计算海森矩阵及其逆矩阵，而是通过 J T J J^TJ JTJ来近似表达 H H H，从而节省了每次迭代的时间。

然而，高斯牛顿法也同样具有牛顿法类似的问题。

当增量较大时， f ( x ) f(\bf x) f(x)的泰勒展开就与 f ( x ) f(\bf x) f(x)本身相差较大，局部近似就可能不成立，导致不收敛。

另外， J T J J^TJ JTJ这个矩阵是半正定的，也就是说可能出现奇异矩阵或者病态矩阵的情况，导致求逆计算的误差较大。

示例代码

这里再给出一个估计 f ( x , y ) = e x + e 0.5 y + x f(x,y)=e^x+e^{0.5y}+x f(x,y)=ex+e0.5y+x的最小平方的代码：

import numpy as np
import scipy.optimize
import time
import math


def partial_derivate_xy(x, y, F):
    dx = (F(x + 0.001, y) - F(x, y))/0.001
    dy = (F(x, y + 0.001) - F(x, y))/0.001
    return dx, dy


def non_linear_func(x, y):
    fxy = 0.5 * (x ** 2 + y ** 2)
    return fxy


def non_linear_func_2(x, y):
    fxy = x*x + 2*y*y + 2*x*y + 3*x - y - 2
    return fxy


def non_linear_func_3(x, y):
    fxy = 0.5 * (x ** 2 - y ** 2)
    return fxy


def non_linear_func_4(x, y):
    fxy = x**4 + 2*y**4 + 3*x**2*y**2 + 4*x*y**2 + x*y + x + 2*y + 0.5
    return fxy


def non_linear_func_5(x, y):
    fxy = math.exp(x) + math.exp(0.5 * y) + x
    return fxy


def non_linear_func_5_least_square(x, y):
    fxy = math.pow(math.exp(x) + math.exp(0.5 * y) + x, 2)
    return fxy


def gauss_newton_optim(x, y, F):
    dx, dy = partial_derivate_xy(x, y, F)
    fx = F(x, y)
    grad = np.array([[dx], [dy]])
    # print(np.matmul(grad, grad.T))
    H = np.matmul(grad, grad.T)
    while np.linalg.cond(H) > 10:
        H = H + 0.1 * np.eye(2)
    g = - grad * fx
    vec_delta = np.matmul(np.linalg.inv(H), g)
    vec_opt = np.array([[x], [y]]) + vec_delta
    x_opt = vec_opt[0][0]
    y_opt = vec_opt[1][0]
    return x_opt, y_opt, vec_delta


def optimizer(x0, y0, F, th=0.000001):
    x = x0
    y = y0
    counter = 0
    while True:
        x_opt, y_opt, vec_delta = gauss_newton_optim(x, y, F)
        if np.linalg.norm(vec_delta)