最优化理论与自动驾驶（二-补充）：求解算法（梯度下降法、牛顿法、高斯牛顿法以及LM法，C++代码）

在之前的章节里面（最优化理论与自动驾驶（二）：求解算法）我们展示了最优化理论的基础求解算法，包括高斯-牛顿法（Gauss-Newton Method）、梯度下降法（Gradient Descent Method）、牛顿法（Newton's Method）和勒文贝格-马夸尔特法（Levenberg-Marquardt Method, LM方法）法。在实际工程应用中，我们一般采用C++进行开发，所以本文补充了上述求解方法的C++代码。在实际应用中，我们既可以自己进行简单的求解，也可以采用第三方库进行求解。我们列举了三种方式：1）直接使用c++ vector容器 2）采用eigen库进行迭代计算 3）采用eigen库封装好的函数求解，工程应用中建议使用eigen库进行矩阵操作，因为底层进行了大量的优化，包括SIMD指令集优化、懒惰求值策略等。

C++示例代码如下：

以指数衰减模型 为例，通过不同方法获得最小二乘拟合参数，其中参数为a和b。

1. 梯度下降法

1.1 使用C++ vector容器

#include <iostream>
#include <vector>
#include <cmath>
#include <limits>

// 定义指数衰减模型函数 y = a * exp(b * x)
double model(const std::vector<double>& params, double x) {
    double a = params[0];
    double b = params[1];
    return a * std::exp(b * x);
}

// 定义残差函数
std::vector<double> residuals(const std::vector<double>& params, const std::vector<double>& x, const std::vector<double>& y) {
    std::vector<double> res(x.size());
    for (size_t i = 0; i < x.size(); ++i) {
        res[i] = model(params, x[i]) - y[i];
    }
    return res;
}

// 计算目标函数（即平方和）
double objective_function(const std::vector<double>& params, const std::vector<double>& x, const std::vector<double>& y) {
    std::vector<double> res = residuals(params, x, y);
    double sum = 0.0;
    for (double r : res) {
        sum += r * r;
    }
    return 0.5 * sum;
}

// 计算梯度
std::vector<double> compute_gradient(const std::vector<double>& params, const std::vector<double>& x, const std::vector<double>& y) {
    double a = params[0];
    double b = params[1];
    std::vector<double> res = residuals(params, x, y);

    // 梯度计算
    double grad_a = 0.0;
    double grad_b = 0.0;
    for (size_t i = 0; i < x.size(); ++i) {
        grad_a += res[i] * std::exp(b * x[i]);             // 对 a 的偏导数
        grad_b += res[i] * a * x[i] * std::exp(b * x[i]);   // 对 b 的偏导数
    }
    
    return {grad_a, grad_b};
}

// 梯度下降法
std::vector<double> gradient_descent(const std::vector<double>& x, const std::vector<double>& y, const std::vector<double>& initial_params, 
                                     double learning_rate = 0.01, int max_iter = 10000, double tol = 1e-6) {
    std::vector<double> params = initial_params;

    for (int i = 0; i < max_iter; ++i) {
        // 计算梯度
        std::vector<double> gradient = compute_gradient(params, x, y);

        // 更新参数
        std::vector<double> params_new = {params[0] - learning_rate * gradient[0], params[1] - learning_rate * gradient[1]};