高斯牛顿法
主要思想是将\(f(x)\)进行一阶的泰勒展开。然后求解其最小二乘解。
\[f(x_k+\triangle x_k)\approx f(x_k)+J(x_k)^T \triangle x_k \]求解问题变为：
\[\triangle x^*=argmin _{\triangle x}\frac {1}{2}\lvert f(x)+J(x)^T\triangle x\rvert ^2 \]\[\frac {1}{2}\lvert {f(x)+J(x)^T\triangle x}\rvert ^2=\frac {1}{2}(\lvert f(x) \rvert ^2+2f(x)J(x)^T\triangle x+\triangle x^TJ(x)J(x)^T\triangle x \]右侧求导，$$J(x)f(x)+J(x)J^T(x)\triangle x=0$$
则，可得 高斯-牛顿方程：
这个方程是关于变量\(\delta x\)的线性方程组，我们称他为增量方程，也叫做高斯牛顿方程(Gauss-Newton equation)或者正规方程(Normal equation)。
左边的定义为\(H\)右边的定义为\(g\)：
对比牛顿法，其实高斯-牛顿法运用\(JJ^T\)作为牛顿法中二阶Hessian矩阵的近似，从而省略了H的估计。
算法流程：
1.给定初始值\(x_0\)
2.对于第k次迭代，求出当前的雅可比矩阵\(J(x_k)\)和误差\(f(x_k)\)
3.求解增量方程：\(H\triangle x_k =g\)
4.若\(\triangle x_k\)足够小则停止。否则，令\(x_{k+1}=x_k+\triangle x_k\)，返回第2步
手写高斯牛顿法拟合曲线
#include<iostream>
#include<opencv2/opencv.hpp>
#include<Eigen/Core>
#include<Eigen/Dense>
#include <chrono>
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;
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
chrono::steady_clock::time_point t1=chrono::steady_clock::now();
for(int iter=0;iter<iterations;iter++){
Matrix3d H=Matrix3d::Zero();
Vector3d b=Vector3d::Zero();
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);
J[1]=-xi*exp(ae*xi*xi+be*xi+ce);
J[2]=-exp(ae*xi*xi+be*xi+ce);
//H+=J*J.transpose();
//b+=-error*J;//发现两种结果相同，这里应该是因为分母为1 的原因，但是究竟哪一种是对的？
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;
}
chrono::steady_clock::time_point t2=chrono::steady_clock::now();
chrono::duration<double>time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
cout<<"solve time cost"<<time_used.count()<<"seconds."<<endl;
cout<<"estimate abc="<<ae<<","<<be<<","<<ce<<endl;
return 0;
}
CMakeLists.txt文件
cmake_minimum_required(VERSION 2.8)
project(gaussNewton)
set(CMAKE_BUILD_TYPE Release)
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
include_directories("/usr/include/eigen3")
add_executable(gaussNewton gaussNewton.cpp)
target_link_libraries(gaussNewton ${OpenCV_LIBS})