考虑满足以下方程的曲线
其中a,b,c是曲线参数,w是高斯噪声,w~(0,σ^2)(服从零均值的高斯分布)。
假设有N个关于x,y的观测数据点,通过根据这些数据求解下面最小二乘问题,求出曲线参数
在求解这个问题时,程序里先根据模型生成x,y的真值,然后在真值中添加高斯分布的噪声,再使用高斯牛顿法从带噪声的数据拟合参数模型
定义误差为:
计算每个误差项对状态变量的导数:
则
高斯牛顿法的增量方程:
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
#define N 100 //数据点
int main(void)
{
double ar = 1.0, br = 2.0, cr = 1.0; //真实参数
double ae = 2.0, be = -1.0, ce = 5.0; //估计参数
double w_sigma = 1.0; //噪声sigma值
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));
}
//高斯牛顿迭代
int iterations = 50; //迭代次数
double cost = 0, lastCost = 0; //本次迭代和上次迭代
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]; //从数据集取出数据点
double err = yi - exp(ae*xi*xi + be*xi + ce); //误差
Vector3d J; //雅可比阵
//因为待估计状态是a,b,c,因此J的各元素是e对a,b,c的偏导
J[0] = -xi*xi*exp(ae*xi*xi + be*xi + ce); //误差对a的偏导
J[1] = -xi*exp(ae*xi*xi + be*xi + ce); //误差对b的偏导
J[2] = -exp(ae*xi*xi + be*xi + ce); //误差对c的偏导
H += inv_sigma*inv_sigma*J*J.transpose();
b += -inv_sigma*inv_sigma*err*J;
cost += err*err;
}
//求解Hx=b
Vector3d dx = H.ldlt().solve(b); //解△x,各元素表示状态的增量
if(isnan(dx[0])) //无解
{
cout << iter << ":" << "result is nan!" << endl;
break;
}
if(iter > 0 && cost >= lastCost) //目标函数经过迭代不再下降,已经达到局部极小值
{
cout << iter << ":" << "cost:" << cost << ">= last cost:" << lastCost << ",break." << endl;
break;
}
ae += dx[0];
be += dx[1];
ce += dx[2];
lastCost = cost;
cout << iter << ":" << "total cost : " << cost << ", \tupdate: " << dx.transpose() << "\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 = " << time_used.count() << "s." << endl;
cout << "estimated a = " << ae << ", b = " << be << ", c = " << ce << endl;
return 0;
}