高斯牛顿法在具体工程中的应用——C++版
说明:本文章没有用大量篇幅来讲述高斯牛顿的原理和数学中的应用,而是用具体的代码来说明,具体是怎么应用的。
如果对高斯牛顿法的原理比较感兴趣,可以阅读以下链接中的内容:
https://zhuanlan.zhihu.com/p/42383070
概述:高斯牛顿法解决的是工程中的非线性最小二乘问题,如SLAM。具体代码如下:
首先我们需要定义以下参数:
//设定的真实参数的值
double ar = 1.0, br = 2.0, cr = 1.0;
// 估计参数值,每一次迭代的时候,设定的参数值是不一样的,这里是初始化参数
double ae = 2.0, be = -1.0, ce = 5.0;
//设置的数据集的点数共100个数组
int N = 100;
//设置的噪声值,值越低表明对实际的影响越小
double w_sigma = 0.01;
//调用CV库里的rng函数,随机值
cv::RNG rng;
其次,我们要设置一个数据集,每一个问题的数据集是不一样的,自己设置
//定义的100个数据点,数据集
vector<double> x_data, y_data;
for (int i = 0; i < N; i++)
{
//定义的输入xi
double x = i / 100.0;
//push.函数的作用:将一个新的元素加到vector的最后面,位置为当前最后一个元素的下一个元素
x_data.push_back(x);
//返回的随机数的平均值为零,标准偏差为指定的 sigma (高斯分布的特征)
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma));
}
第三,开始具体的应用:
// 开始Gauss-Newton迭代
int iterations = 100; // 迭代次数,迭代次数应该设置的适中,太大太小都不好
double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost
for (int iter = 0; iter < iterations; iter++) {
//定义一个海塞矩阵,
Matrix3d H = Matrix3d::Zero(); // Hessian = J^T J in Gauss-Newton,这里的J为行向量,我们最终想得到的是一个矩阵
//定义一个偏置矩阵
Vector3d b = Vector3d::Zero(); // bias
cost = 0;
//每一次的迭代,都有一个对应的参数值,也就是a,b,c的值
for (int i = 0; i < N; i++) {
//xi,yi为定义的数据集,yi在这里为实际值,xi为输入值
double xi = x_data[i], yi = y_data[i]; // 第i个数据点
double error = 0; // 第i个数据点的计算误差
//每一个error的大小,一共100个
//yi为观测值
//exp(ae * xi * xi + be * xi + ce)为理论值拟合函数
error=yi - (exp(ae * xi * xi + be * xi + ce));
// error = 0; // 填写计算error的表达式
Vector3d J; // 雅可比矩阵:函数对参数求偏导
//求出每一个数值的Jacbian矩阵,对a,b,c参数分别求偏导
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);
cout<<"J"<<i<<": "<<J.transpose()<<endl;
//hessian矩阵用累加法,GN近似的H
H += J * J.transpose();
cout<<"H:"<<H.transpose()<<endl;
b += J*(-error) ;
cout<<"b: "<<b.transpose()<<endl;
//代价函数
cost += error * error;
}
// 求解线性方程 Hx=b,建议用ldlt
Vector3d dx;
cout<<"H:"<< H<<endl;
cout<<"B:"<< b<<endl;
//H的逆矩阵再乘以b
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 << endl;
break;
}
// 更新abc估计值
ae += dx[0];
be += dx[1];
ce += dx[2];
lastCost = cost;
cout << "total cost: " << cost << endl;
}
cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
return 0;
}