高斯牛顿法(C++实现）

来源：互联网发布：三只眼网络行为管理编辑：程序博客网时间：2024/06/01 10:28
#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 &params, VectorXd &output);void people_func(const VectorXd &input, const VectorXd &params, 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 &params,        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 &params        ){    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 &params, 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();}
阅读全文
0 0