手寫高斯牛頓曲線擬合（c++)

擬合曲線： 

取100個帶有高斯噪聲的數，利用高斯牛頓法進行擬合求取估計值  a ,b ,c;

主要步驟:

1.構造誤差項；

2，求雅克比，以及cost

3，判斷cost是否小於上次cost，小：執行4，大：輸出結果；

4，求H g;

5求dx；

6，更新估計量，到步驟2；

 

/*
 * 高斯牛頓法練習   created by fanguohao   19.08.17
 */
#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace Eigen;
#define DataNum 100  // 數據量；
#define itratorNum 200  // 迭代次數；

int main()
{
    vector<double> x_data;
    vector<double> y_data;

    x_data.reserve(DataNum);
    y_data.reserve(DataNum);

    double at=1.0 ,bt=2.0,ct=1.0;
    double ae=2.0,be=-1.0,ce=3.0;

    cv::RNG rng;
    double w_sigma = 1.0;

    //生成代高斯噪聲的數據；
    for(int i= 0;i<DataNum;++i)
    {
        double xt = i/100.0;
        double yt = exp(at*xt*xt + bt*xt + ct)+rng.gaussian(w_sigma);
        x_data.push_back(xt);
        y_data.push_back(yt);
    }


    double error;
    Eigen::Vector3d J;
    Eigen::Matrix3d H;
    Eigen::Vector3d dx; // 增量；
    Eigen::Vector3d b;
    double cost,lastcost;
    J = Vector3d::Zero();
    dx = Vector3d::Zero();
    for(int j= 0;j<100;++j)
    {
        // 每次迭代完成後清理H，b cost；
        H = Matrix3d::Zero();
        b = Vector3d::Zero();
        cost = 0;
        //
        for(int i=0;i<x_data.size();++i)
        {
            double x = x_data[i];
            error = y_data[i] - exp(ae*x*x + be*x + ce) ;
            J[0] =  -exp(ae*x*x + be*x + ce) * x*x;
            J[1] =  -exp(ae*x*x + be*x + ce) * x;
            J[2] =  -exp(ae*x*x + be*x + ce);

            H += J * J.transpose();
            b += -error * J;
            cost += error*error;

        }


        dx = H.ldlt().solve(b);
//        dx = H.inverse() * b;

        if (j > 0 && cost > lastcost) {
            // 誤差增長了，說明近似的不夠好
            cout << "cost: " << cost << ", last cost: " << lastcost << endl;
            break;
        }

//        cout<<dx<<endl;
        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastcost = cost;
    }


    cout<<ae<<endl;
    cout<<be<<endl;
    cout<<ce<<endl;


    return 0;
}