高斯牛顿法

高斯牛顿法

主要思想是将\(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$$
则，可得 高斯-牛顿方程：

\[J(x)J^T(x)\triangle x=-J(x)f(x) \]

这个方程是关于变量\(\delta x\)的线性方程组，我们称他为增量方程，也叫做高斯牛顿方程(Gauss-Newton equation)或者正规方程(Normal equation)。
左边的定义为\(H\)右边的定义为\(g\)：

\[H\triangle x=g \]

对比牛顿法，其实高斯-牛顿法运用\(JJ^T\)作为牛顿法中二阶Hessian矩阵的近似，从而省略了H的估计。
所以，\(H=JJ^T,g=-J*f(x)\)

算法流程：
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})