5.高斯牛顿方程的具体例子

求解满足如下方程的曲线

其中为待求曲线参数，为引入的高斯分布噪声，满足,假设有对关于的观测点，要通过这些点来推测具体的曲线参数

记第对观测点误差为
定义
对该点的误差进行平方再求和写成最小二乘的形式

于是

我们根据第4节，对于
其高斯牛顿方程增量方程长下面这样

其中

于是在这里，我们令. 注意，k表示k次迭代，不是说k次方

于是

于是我们得到第个数据点的增量方程

因为最终是所有的点进行求和,得到整体所有点的单次迭代增量方程

另外，我们注意到因为误差噪声数据满足
因此实际计算的时候会对于每一个误差都除去一个标准差，这样让数据满足归一化，即满足
以防止数据越界或者各种奇怪的问题
于是最终得到

这里贴一段别人写的 eigen库实现的上面的例子的代码,可同步参考进行理解

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

using namespace std;
using namespace Eigen;

int main()
{
    /*第一部分，生成观测数据xi,yi*/
    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值
    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));//加上高斯噪声
    }

    /*第二部分，开始高斯牛顿迭代*/
    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个数据点
            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;
        }
        //如果当前迭代不能使目标函数减小，则停止迭代
        if (iter > 0 && cost >= lastCost)
        {
            std::cout << "cost: " << cost << ">= lastCost:" << lastCost << ",break." << std::endl;
            break;
        }
        //更新参数
        ae += dx[0];
        be += dx[1];
        ce += dx[2];
        //记录当前迭代次数的代价函数值
        lastCost = cost;
        //输出迭代结果
        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;
}