高斯牛顿法（C++实现)

#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();
}