个人博客：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