#include <iostream>
#include <cmath>
#include <Eigen/Eigen>
using namespace Eigen;
#define ITER_STEP (1e-5)
#define ITER_CNT (100)
typedef void (*func_ptr)(const VectorXd &input, const VectorXd ¶ms, VectorXd &output);
void people_func(const VectorXd &input, const VectorXd ¶ms, VectorXd &output)
{
double A = params(0, 0);
double B = params(1, 0);
for (int i = 0; i < input.rows(); i++) {
output(i, 0) = A * exp(input(i, 0) * B);
}
}
void get_jacobian(func_ptr func,
const VectorXd &input,
const VectorXd ¶ms,
MatrixXd &output
)
{
int m = input.rows();
int n = params.rows();
VectorXd out0(m, 1);
VectorXd out1(m, 1);
VectorXd param0(n, 1);
VectorXd param1(n, 1);
//output.resize(m, n);
for (int j = 0; j < n; j++) {
param0 = params;
param1 = params;
param0(j, 0) -= ITER_STEP;
param1(j, 0) += ITER_STEP;
func(input, param0, out0);
func(input, param1, out1);
output.block(0, j, m, 1) = (out1 - out0) / (2 * ITER_STEP);
}
}
void gauss_newton(func_ptr func,
const VectorXd &inputs,
const VectorXd &output,
VectorXd ¶ms
)
{
int m = inputs.rows();
int n = params.rows();
// jacobian
MatrixXd jmat(m, n);
VectorXd r(m, 1);
VectorXd tmp(m, 1);
double pre_mse = 0.0;
double mse;
for (int i = 0; i < ITER_CNT; i++) {
mse = 0.0;
func(inputs, params, tmp);
r = output - tmp;
get_jacobian(func, inputs, params, jmat);
mse = r.transpose() * r;
mse /= m;
if (fabs(mse - pre_mse) < 1e-8) {
break;
}
pre_mse = mse;
VectorXd delta = (jmat.transpose() * jmat).inverse() * jmat.transpose() * r;
printf ("i = %d, mse %lf.\n", i, mse);
params += delta;
}
std::cout << "params:" << params.transpose() << std::endl;
}
int people_fit()
{
// A * exp(B * x)
VectorXd inputs(8, 1);
inputs << 1, 2, 3, 4, 5, 6, 7, 8;
VectorXd output(8, 1);
output << 8.3, 11.0, 14.7, 19.7, 26.7, 35.2, 44.4, 55.9;
VectorXd params(2, 1);
params << 8, 0.7;
gauss_newton(people_func, inputs, output, params);
return 0;
}
void tri_func(const VectorXd &input, const VectorXd ¶ms, VectorXd &output)
{
double A = params(0, 0);
double B = params(1, 0);
double C = params(2, 0);
double D = params(3, 0);
for (int i = 0; i < input.rows(); i++) {
output(i, 0) = A*sin(B*input(i, 0)) + C*cos(D*input(i, 0));
}
}
int tri_func_fit()
{
// A * sin(Bx) + C * cos(Dx)
double A = 5;
double B = 1;
double C = 10;
double D = 2;
const int smp_cnt = 100;
VectorXd input(smp_cnt, 1);
VectorXd output(smp_cnt, 1);
VectorXd params(4, 1);
params << 1, 1, 9, 1;
for (int i = 0; i < smp_cnt; i++) {
input(i, 0) = i;
output(i, 0) = A*sin(B*input(i, 0))+C*cos(D*input(i, 0)) + (rand() % 255) / 255.0;
}
gauss_newton(tri_func, input, output, params);
return 0;
}
int main()
{
people_fit();
//tri_func_fit();
}
高斯牛顿法(C++实现)
最新推荐文章于 2023-05-17 15:57:03 发布