個人博客：http://www.chenjianqu.com/
原文鏈接：http://www.chenjianqu.com/show-85.html
最小二乘法
最小二乘法（又稱最小平方法）是一種數學優化技術。它通過最小化誤差的平方和尋找數據的最佳函數匹配。利用最小二乘法可以簡便地求得未知的數據，並使得這些求得的數據與實際數據之間誤差的平方和爲最小。最小二乘法還可用於曲線擬合。數學表示：
其中yi是第i個實際觀測到的值，或叫真實值或者目標值；fi(x)則可以看作是通過x這個參數預測得到的第i個值，是對yi的估計。最小二乘的目標估計未知的參數使得觀測值（真實值）和估計值之間的差距最小。
線性最小二乘
所謂線性，指f(x)是x的線性函數，f(x)=x0+t1*x1+...+tq*xq。線性最小二乘的解法相對簡單。
非線性最小二乘
所謂非線性，就是f(x)無法表示爲的線性關係，而是某種非線性關係。考慮一個簡單的非線性最小二乘問題：
上式的f(x)是要擬合的函數，可以是任意標量非線性函數，F(x)是目標函數，我們希望找到一個x(即這裏的x是待優化參數)令目標函數F(x)最小。求解這個問題有幾種方法：
1.直接求導，令dF/dx=0，但這通常難以實現。
2.迭代法：
這讓求解導函數爲零的問題變成了一個不斷尋找下降增量 ∆xk 的問題。以下就是尋找增量的方法。
一階和二階梯度法
考慮第k次迭代，想要尋找∆xk，最直接的方法是將目標函數F(x)在xk附近進行泰勒展開：
其中J(xk)是F(x)關於x的一階導數(梯度、雅可比(Jacobian)矩陣)，H(xk)則是二階導數(海塞(Hessian)矩陣)。
如果上式中只保留一階項，則稱爲一階梯度法或最快下降法，取∆x=-J(xk)，即增量的方向爲梯度的反方向，通常還設置一個步長λ。
如果保留二階項，則此增量方程爲：
求右側關於∆x的導數並令它爲0，得：J+H∆x=0，即 H∆x=-J 。求解這個線性方程，就得到了增量，該方法稱爲二階梯度法或牛頓法。
一階梯度法過於貪心，容易走出鋸齒路線，反而增加了迭代次數；而二階梯度法則需要計算目標函數的 H 矩陣，這在問題規模較大時非常困難，我們通常傾向於避免 H 的計算。
高斯牛頓法
將要擬合的函數f(x)(不是目標函數F(x)，不然就成了牛頓法)進行一階泰勒展開：
這裏的J(x)是f(x)關於x的導數，爲nx1的列向量。我們的目標是尋找增量∆x，使得|f(x+∆x)|2最小。爲了求∆x，需要解一個線性最小二乘問題：
將上述目標函數對∆x求導，並令導數等於0。爲此，先展開目標函數的平方項：
再對∆x求導，令其爲0,得：J(x)f(x) + J(x)JT(x)∆x = 0，即：
J(x)JT(x)∆x = - J(x)f(x)
該式是關於變量∆x的線性方程組，稱之爲增量方程，或稱之爲高斯牛頓方程(Gauss-Newton equation)或正規方程(Normal equation)。令H=J(x)JT(x)，g=-J(x)f(x)，則高斯牛頓方程變爲：
H∆x=g
對比牛頓法中H∆x=-J，高斯牛頓法用J(x)JT(x)作爲牛頓法中Hessian矩陣的近似，從而省略了H的計算。求解高斯牛頓方程是整個優化問題的核心所在，如果能解出該方程，則高斯牛頓法的步驟如下：
爲了求解增量方程，需要解出H-1，這需要H矩陣可逆。但是實際上H只有半正定，也就是H可能會是奇異矩陣或ill-condition的情況，此時增量的穩定性較差，導致算法不收斂。就算H非奇異也非病態，但是如果求出來的步長∆x太大，也無法保證能迭代收斂。
列文伯格—馬夸爾特方法
該方法的收斂速度比高斯牛頓法慢，也被稱爲阻尼牛頓法。高斯牛頓法中採用的近似二階泰勒展開只能在展開點附近有較好的近似效果，所以很自然地想到給 ∆x 添加一個範圍，稱爲信賴區域(Trust Region)。這個範圍定義了在什麼情況下二階近似是有效的，這類方法也稱爲信賴區域方法(Trust Region Method)。在信賴區域裏邊，我們認爲近似是有效的；出了這個區域，近似可能會出問題。
那麼如何確定這個信賴區域的範圍呢？一個比較好的方法是根據我們的近似模型跟實際函數之間的差異來確定：如果差異小，說明近似效果好，我們擴大近似的範圍；反之，如果差異大，就縮小近似的範圍。我們定義一個指標 ρ 來刻畫近似的好壞程度，下式爲6.34：
ρ 的分子是實際函數下降的值，分母是近似模型下降的值。如果 ρ 接近於 1，則近似是好的。如果 ρ太小，說明實際減小的值遠少於近似減小的值，則認爲近似比較差，需要縮小近似範圍。反之，如果 ρ 比較大，則說明實際下降的比預計的更大，我們可以放大近似範圍。因此新的步驟如下：
這裏近似範圍µ擴大的倍數和閾值都是經驗值。在式(6.35)中，把增量限定於一個半徑爲 µ 的球中，只在這個球內纔是有效的。帶上 D 之後，這個球可以看成一個橢球。在列文伯格提出的優化方法中，把 D 取成單位陣 I，相當於直接把 ∆xk 約束在一個球中。馬夸爾特提出將 D 取成非負數對角陣——實際中通常用 JTJ 的對角元素平方根，使得在梯度小的維度上約束範圍更大一些。
梯度的獲得需要求解式(6.35)，這個子問題是帶不等式約束的優化問題，我們用拉格朗日乘子把約束項放到目標函數中，構成拉格朗日函數：
λ爲拉格朗日乘子。類似於高斯牛頓法中的做法，令該拉格朗日函數關於 ∆x 的導數爲零，它的核心仍是計算增量的線性方程：(H + λDTD) ∆xk = g 。這裏的增量方程相比於高斯牛頓法多了一項λDTD，若 D=I，則求解的是：
(H + λI) ∆xk = g
當參數 λ 比較小時，H 佔主要地位，這說明二次近似模型在該範圍內是比較好的，列文伯格—馬夸爾特方法更接近於高斯牛頓法。當 λ 比較大時，λI 佔據主要地位，列文伯格—馬夸爾特方法更接近於一階梯度下降法(即最速下降)，這說明附近的二次近似不夠好。
列文伯格—馬夸爾特方法的求解方式，可在一定程度上避免線性方程組的係數矩陣的非奇異和病態問題，提供更穩定、更準確的增量 ∆x。在實際中，還存在許多其他的方式來求解增量，例如 Dog-Leg 等方法。
曲線擬合
要擬合的曲線：y = exp(ax2 + bx + c) + w，其中a、b、c爲曲線參數，w爲0均值、σ標準差的高斯噪聲。假設有N個觀測點，則使用高斯牛頓法求解下面的最小二乘問題以估計曲線參數：
定義誤差爲：ei = yi - exp(axi2 + bxi + c)，這裏的狀態變量爲a,b,c，求出每個誤差項對於狀態變量的導數：
於是雅可比矩陣爲：
得高斯牛頓方程：
代碼如下：
CMakeLists.txt
cmake_minimum_required(VERSION 2.6)
project(gaussnewtontest)
# 添加c++ 11標準支持
set( CMAKE_CXX_FLAGS "-std=c++11" )
include_directories("/usr/include/eigen3")
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )
add_executable(gaussnewtontest main.cpp)
target_link_libraries(gaussnewtontest ${OpenCV_LIBS} )
install(TARGETS gaussnewtontest RUNTIME DESTINATION bin)
main.cpp
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0; // 真實參數值
double ae = 2.0, be = -1.0, ce = 5.0; // 估計參數值
int N = 100; // 數據點
double w_sigma = 1.0; // 噪聲Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng;
// OpenCV隨機數產生器
vector<double> x_data, y_data; // 數據
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}
// 開始Gauss-Newton迭代
int iterations = 100; // 迭代次數
double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost
for (int iter = 0; iter < iterations; iter++) {
Matrix3d H = Matrix3d::Zero(); // Hessian = J^T W^{-1} J in Gauss-Newton
Vector3d b = Vector3d::Zero(); // bias
cost = 0;
for (int i = 0; i < N; i++) {
double xi = x_data[i], yi = y_data[i]; // 第i個數據點
double error = yi - exp(ae * xi * xi + be * xi + ce);
Vector3d J; // 雅可比矩陣
J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce); // de/da
J[1] = -xi * exp(ae * xi * xi + be * xi + ce); // de/db
J[2] = -exp(ae * xi * xi + be * xi + ce); // de/dc
H += inv_sigma * inv_sigma * J * J.transpose();
b += -inv_sigma * inv_sigma * error * J;
cost += error * error;
}
// 求解線性方程 Hx=b
Vector3d dx = H.ldlt().solve(b);
if (isnan(dx[0])) {
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost >= lastCost) {
cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
break;
}
ae += dx[0];
be += dx[1];
ce += dx[2];
lastCost = cost;
cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
"\t\testimated params: " << ae << "," << be << "," << ce << endl;
}
cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
return 0;
}
文獻參考
[1]高翔.視覺SLAM14講
[2]皮皮蔣.線性最小二乘和非線性最小二乘. https://www.jianshu.com/p/bf6ec56e26bd