求解滿足如下方程的曲線

其中為待求曲線參數(shù)，為引入的高斯分布噪聲，滿足,假設(shè)有對關(guān)于的觀測點卤橄，觀測結(jié)果是,要通過這些點來推測具體的曲線參數(shù)

記第對觀測點誤差為
,此處的函數(shù)為確定函數(shù)，無需進行泰勒展開
可直接定義函數(shù)
對該點的誤差函數(shù)進行平方再求和寫成最小二乘的形式

于是

我們根據(jù)第4節(jié)臂外，對于
其高斯牛頓方程增量方程長下面這樣

其中

于是在這里窟扑，我們令. 注意，k表示k次迭代漏健，不是說k次方

于是

于是我們得到第個數(shù)據(jù)點的增量方程

因為最終是所有的點進行求和,得到整體所有點的單次迭代增量方程

另外嚎货，我們注意到因為誤差噪聲數(shù)據(jù)滿足
因此實際計算的時候會對于每一個誤差都除去一個標(biāo)準(zhǔn)差，這樣讓數(shù)據(jù)滿足歸一化漾肮，即滿足
以防止數(shù)據(jù)越界或者各種奇怪的問題
于是最終得到

這里貼一段別人寫的 eigen庫實現(xiàn)的上面的例子的代碼,可同步參考進行理解

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main()
{
    /*第一部分厂抖，生成觀測數(shù)據(jù)xi,yi*/
    double ar = 1.0, br = 2.0, cr = 1.0;//真實參數(shù)值
    double ae = 2.0, be = -1.0, ce = 5.0;//估計參數(shù)值
    int N = 100;//數(shù)據(jù)點
    double w_sigma = 1.0;//噪聲的sigma值
    cv::RNG rng;//opencv隨機數(shù)產(chǎn)生器
    vector<double> x_data, y_data;//數(shù)據(jù)
    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));//加上高斯噪聲
    }

    /*第二部分，開始高斯牛頓迭代*/
    int iterations = 100;
    double cost = 0, lastCost = 0;//本次迭代的cost和上一次迭代的cost
    //開始計時間
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    //迭代iterations次
    for (int iter = 0; iter < iterations; iter++)
    {
        Matrix3d H = Matrix3d::Zero();//H=JxJ^T
        Vector3d b = Vector3d::Zero();//b=-J*f
        cost = 0;
        //求解每個觀測點的損失克懊，即F=1/2*||f||^2方程中的f
        for (int i = 0; i < N; i++)
        {
            double xi = x_data[i], yi = y_data[i];//第i個數(shù)據(jù)點
            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 +=  J*J.transpose();
            b += -error*J;

            cost += error * error;
        }

        //求解線性方程組Hx=b
        Vector3d dx = H.ldlt().solve(b);
        if (isnan(dx[0]))
        {
            std::cout << "result is nan!" << std::endl;
            break;
        }
        //如果當(dāng)前迭代不能使目標(biāo)函數(shù)減小忱辅，則停止迭代
        if (iter > 0 && cost >= lastCost)
        {
            std::cout << "cost: " << cost << ">= lastCost:" << lastCost << ",break." << std::endl;
            break;
        }
        //更新參數(shù)
        ae += dx[0];
        be += dx[1];
        ce += dx[2];
        //記錄當(dāng)前迭代次數(shù)的代價函數(shù)值
        lastCost = cost;
        //輸出迭代結(jié)果
        std::cout << "iteration:"<<iter+1<<"\ttotal cost:" << cost << "\t\tupdata:" << 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);
    std::cout << "solve time cost=" << time_used.count() << "seconds." << endl;
    std::cout << "estimated abc=" << ae << "," << be << "," << ce << endl;
    return 0;
}