当前位置： 首页 > article >正文

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

article 2024/9/25 8:35:59

在之前的章节里面（最优化理论与自动驾驶（二）：求解算法）我们展示了最优化理论的基础求解算法，包括高斯-牛顿法（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++示例代码如下：

以指数衰减模型y=a\cdot e^{bx} 为例，通过不同方法获得最小二乘拟合参数，其中参数为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]};

        // 检查收敛条件
        double diff = std::sqrt(std::pow(params_new[0] - params[0], 2) + std::pow(params_new[1] - params[1], 2));
        if (diff < tol) {
            std::cout << "Converged after " << i + 1 << " iterations." << std::endl;
            return params_new;
        }

        params = params_new;
    }

    std::cout << "Max iterations exceeded. No solution found." << std::endl;
    return params;
}

int main() {
    // 示例数据
    std::vector<double> x_data = {0, 1, 2, 3, 4, 5};
    std::vector<double> y_data;
    for (double x : x_data) {
        y_data.push_back(2 * std::exp(-1 * x));
    }

    // 初始参数猜测 (a, b)
    std::vector<double> initial_guess = {1.0, -1.0};

    // 执行梯度下降法
    std::vector<double> solution = gradient_descent(x_data, y_data, initial_guess);

    // 输出结果
    std::cout << "Fitted parameters: a = " << solution[0] << ", b = " << solution[1] << std::endl;

    return 0;
}

1.2 使用eigen库

#include <iostream>
#include <Eigen/Dense>
#include <cmath>

using Eigen::VectorXd;

// 定义指数衰减模型函数 y = a * exp(b * x)
VectorXd model(const VectorXd& params, const VectorXd& x) {
    double a = params(0);
    double b = params(1);
    return a * (x.array() * b).exp().matrix(); // 使用 Eigen 的数组和矩阵运算
}

// 定义残差函数
VectorXd residuals(const VectorXd& params, const VectorXd& x, const VectorXd& y) {
    return model(params, x) - y;
}

// 计算目标函数（即平方和）
double objective_function(const VectorXd& params, const VectorXd& x, const VectorXd& y) {
    VectorXd res = residuals(params, x, y);
    return 0.5 * res.squaredNorm(); // 使用 Eigen 的 squaredNorm() 计算平方和
}

// 计算梯度
VectorXd compute_gradient(const VectorXd& params, const VectorXd& x, const VectorXd& y) {
    double a = params(0);
    double b = params(1);
    VectorXd res = residuals(params, x, y);

    // 梯度计算
    double grad_a = (res.array() * (b * x.array()).exp()).sum();                  // 对 a 的偏导数
    double grad_b = (res.array() * a * x.array() * (b * x.array()).exp()).sum();   // 对 b 的偏导数

    VectorXd gradient(2);
    gradient << grad_a, grad_b;

    return gradient;
}

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

    for (int i = 0; i < max_iter; ++i) {
        // 计算梯度
        VectorXd gradient = compute_gradient(params, x, y);

        // 更新参数
        VectorXd params_new = params - learning_rate * gradient;

        // 检查收敛条件
        if ((params_new - params).norm() < tol) {
            std::cout << "Converged after " << i + 1 << " iterations." << std::endl;
            return params_new;
        }

        params = params_new;
    }

    std::cout << "Max iterations exceeded. No solution found." << std::endl;
    return params;
}

int main() {
    // 示例数据
    VectorXd x_data(6);
    x_data << 0, 1, 2, 3, 4, 5;
    
    VectorXd y_data = 2 * (-1 * x_data.array()).exp();

    // 初始参数猜测 (a, b)
    VectorXd initial_guess(2);
    initial_guess << 1.0, -1.0;

    // 执行梯度下降法
    VectorXd solution = gradient_descent(x_data, y_data, initial_guess);

    // 输出结果
    std::cout << "Fitted parameters: a = " << solution(0) << ", b = " << solution(1) << std::endl;

    return 0;
}

1.3 使用eigen库QR分解

#include <iostream>
#include <Eigen/Dense>
#include <cmath>

using Eigen::VectorXd;
using Eigen::MatrixXd;

int main() {
    // 示例数据
    VectorXd x_data(6);
    x_data << 0, 1, 2, 3, 4, 5;
    
    VectorXd y_data(6);
    for (int i = 0; i < 6; ++i) {
        y_data(i) = 2 * std::exp(-1 * x_data(i));
    }

    // 对 y_data 取对数以转换为线性方程 ln(y) = ln(a) + b * x
    VectorXd log_y_data = y_data.array().log();

    // 构造线性方程的矩阵形式 A * params = log(y)
    // A 是 x_data 的增广矩阵 [1, x_data]
    MatrixXd A(x_data.size(), 2);
    A.col(0) = VectorXd::Ones(x_data.size()); // 第一列为 1
    A.col(1) = x_data;                        // 第二列为 x_data

    // 使用最小二乘法求解参数 [ln(a), b]
    VectorXd params = A.colPivHouseholderQr().solve(log_y_data);

    // 提取参数 ln(a) 和 b
    double log_a = params(0);
    double b = params(1);
    double a = std::exp(log_a);  // 还原 a

    // 输出拟合结果
    std::cout << "Fitted parameters: a = " << a << ", b = " << b << std::endl;

    return 0;
}

2. 牛顿法

#include <iostream>
#include <Eigen/Dense>
#include <cmath>

using Eigen::VectorXd;
using Eigen::MatrixXd;

// 定义模型函数 y = a * exp(b * x)
VectorXd model(const VectorXd& params, const VectorXd& x) {
    double a = params(0);
    double b = params(1);
    return a * (x.array() * b).exp().matrix(); // 使用 Eigen 的数组和矩阵运算
}

// 定义残差函数
VectorXd residuals(const VectorXd& params, const VectorXd& x, const VectorXd& y) {
    return model(params, x) - y;
}

// 计算梯度和 Hessian 矩阵
std::pair<VectorXd, MatrixXd> gradient_and_hessian(const VectorXd& params, const VectorXd& x, const VectorXd& y) {
    double a = params(0);
    double b = params(1);
    VectorXd res = residuals(params, x, y);
    
    // 计算雅可比矩阵 J
    MatrixXd J(x.size(), 2);
    J.col(0) = (b * x.array()).exp();            // 对 a 的偏导数
    J.col(1) = a * x.array() * (b * x.array()).exp();  // 对 b 的偏导数

    // 计算梯度：g = J.transpose() * res
    VectorXd gradient = J.transpose() * res;

    // 计算 Hessian：H = J.transpose() * J + 二阶项（这里省略二阶项，只保留 J.T * J）
    MatrixXd Hessian = J.transpose() * J;

    return {gradient, Hessian};
}

// 牛顿法求解
VectorXd newton_method(const VectorXd& x, const VectorXd& y, const VectorXd& initial_params, int max_iter = 100, double tol = 1e-6) {
    VectorXd params = initial_params;

    for (int i = 0; i < max_iter; ++i) {
        auto [gradient, Hessian] = gradient_and_hessian(params, x, y);
        
        // 检查 Hessian 是否可逆
        if (Hessian.determinant() == 0) {
            std::cerr << "Hessian matrix is singular." << std::endl;
            return VectorXd();
        }

        // 更新参数：params_new = params - H.inverse() * gradient
        VectorXd delta_params = Hessian.inverse() * gradient;
        VectorXd params_new = params - delta_params;

        // 检查收敛条件
        if (delta_params.norm() < tol) {
            std::cout << "Converged after " << i + 1 << " iterations." << std::endl;
            return params_new;
        }

        params = params_new;
    }

    std::cerr << "Max iterations exceeded. No solution found." << std::endl;
    return params;
}

int main() {
    // 示例数据
    VectorXd x_data(6);
    x_data << 0, 1, 2, 3, 4, 5;

    VectorXd y_data(6);
    y_data = 2 * (-1 * x_data.array()).exp();

    // 初始参数猜测 (a, b)
    VectorXd initial_guess(2);
    initial_guess << 1.0, -1.0;

    // 执行牛顿法
    VectorXd solution = newton_method(x_data, y_data, initial_guess);

    // 输出结果
    if (solution.size() > 0) {
        std::cout << "Fitted parameters: a = " << solution(0) << ", b = " << solution(1) << std::endl;
    } else {
        std::cout << "No solution found." << std::endl;
    }

    return 0;
}

3. 高斯牛顿法

#include <iostream>
#include <Eigen/Dense>
#include <cmath>

using Eigen::VectorXd;
using Eigen::MatrixXd;

// 定义模型函数 y = a * exp(b * x)
VectorXd model(const VectorXd& params, const VectorXd& x) {
    double a = params(0);
    double b = params(1);
    return a * (b * x.array()).exp();
}

// 定义残差函数
VectorXd residuals(const VectorXd& params, const VectorXd& x, const VectorXd& y) {
    return model(params, x) - y;
}

// 定义雅可比矩阵
MatrixXd jacobian(const VectorXd& params, const VectorXd& x) {
    double a = params(0);
    double b = params(1);
    
    MatrixXd J(x.size(), 2);
    J.col(0) = (b * x.array()).exp();                 // 对 a 的偏导数
    J.col(1) = a * x.array() * (b * x.array()).exp();  // 对 b 的偏导数

    return J;
}

// 高斯牛顿法函数
VectorXd gauss_newton(const VectorXd& x, const VectorXd& y, const VectorXd& initial_params, int max_iter = 1000, double tol = 1e-6) {
    VectorXd params = initial_params;

    for (int i = 0; i < max_iter; ++i) {
        VectorXd res = residuals(params, x, y);
        MatrixXd J = jacobian(params, x);

        // 计算更新步长：delta_params = (J^T * J)^(-1) * J^T * res
        VectorXd delta_params = (J.transpose() * J).ldlt().solve(J.transpose() * res);

        // 更新参数
        VectorXd params_new = params - delta_params;

        // 检查收敛条件
        if (delta_params.norm() < tol) {
            std::cout << "Converged after " << i + 1 << " iterations." << std::endl;
            return params_new;
        }

        params = params_new;
    }

    std::cout << "Max iterations exceeded. No solution found." << std::endl;
    return params;
}

int main() {
    // 示例数据
    VectorXd x_data(6);
    x_data << 0, 1, 2, 3, 4, 5;

    VectorXd y_data(6);
    y_data = 2 * (-1 * x_data.array()).exp();

    // 初始参数猜测 (a, b)
    VectorXd initial_guess(2);
    initial_guess << 1.0, -1.0;

    // 执行高斯牛顿法
    VectorXd solution = gauss_newton(x_data, y_data, initial_guess);

    // 输出结果
    std::cout << "Fitted parameters: a = " << solution(0) << ", b = " << solution(1) << std::endl;

    return 0;
}

4. LM法

#include <iostream>
#include <Eigen/Dense>
#include <cmath>

using Eigen::VectorXd;
using Eigen::MatrixXd;

// 定义模型函数 y = a * exp(b * x)
VectorXd model(const VectorXd& params, const VectorXd& x) {
    double a = params(0);
    double b = params(1);
    return a * (b * x.array()).exp();
}

// 定义残差函数
VectorXd residuals(const VectorXd& params, const VectorXd& x, const VectorXd& y) {
    return model(params, x) - y;
}

// 定义雅可比矩阵
MatrixXd jacobian(const VectorXd& params, const VectorXd& x) {
    double a = params(0);
    double b = params(1);
    
    MatrixXd J(x.size(), 2);
    J.col(0) = (b * x.array()).exp();                 // 对 a 的偏导数
    J.col(1) = a * x.array() * (b * x.array()).exp();  // 对 b 的偏导数

    return J;
}

// Levenberg-Marquardt算法
VectorXd levenberg_marquardt(const VectorXd& x, const VectorXd& y, const VectorXd& initial_params, int max_iter = 1000, double tol = 1e-6, double lambda_init = 0.01) {
    VectorXd params = initial_params;
    double lamb = lambda_init;
    
    for (int i = 0; i < max_iter; ++i) {
        VectorXd res = residuals(params, x, y);
        MatrixXd J = jacobian(params, x);
        
        // 计算 Hessian 矩阵近似 H = J.T * J
        MatrixXd H = J.transpose() * J;
        
        // 添加 lambda 倍的单位矩阵，以调整步长方向
        MatrixXd H_lm = H + lamb * MatrixXd::Identity(params.size(), params.size());
        
        // 计算梯度
        VectorXd gradient = J.transpose() * res;
        
        // 计算参数更新值
        VectorXd delta_params = H_lm.ldlt().solve(gradient);
        
        // 更新参数
        VectorXd params_new = params - delta_params;
        
        // 计算新的残差
        VectorXd res_new = residuals(params_new, x, y);
        
        // 如果新的残差平方和小于当前残差平方和，则接受新的参数，并减小 lambda
        if (res_new.squaredNorm() < res.squaredNorm()) {
            params = params_new;
            lamb /= 10;
        } else {
            // 否则增加 lambda
            lamb *= 10;
        }
        
        // 检查收敛条件
        if (delta_params.norm() < tol) {
            std::cout << "Converged after " << i + 1 << " iterations." << std::endl;
            return params;
        }
    }

    std::cout << "Max iterations exceeded. No solution found." << std::endl;
    return params;
}

int main() {
    // 示例数据
    VectorXd x_data(6);
    x_data << 0, 1, 2, 3, 4, 5;

    VectorXd y_data(6);
    y_data = 2 * (-1 * x_data.array()).exp();

    // 初始参数猜测 (a, b)
    VectorXd initial_guess(2);
    initial_guess << 1.0, -1.0;

    // 执行 Levenberg-Marquardt 法
    VectorXd solution = levenberg_marquardt(x_data, y_data, initial_guess);

    // 输出结果
    std::cout << "Fitted parameters: a = " << solution(0) << ", b = " << solution(1) << std::endl;

    return 0;
}

http://www.kler.cn/news/318664.html

相关文章：

  • Java-数据结构-排序(三) |ू･ω･` )
  • 【网络安全】密码学的新进展
  • Nginx 如何开启压缩
  • 伊犁云计算22-1 rhel8 dhcp 配置
  • YOLOv10改进，YOLOv10主干网络替换为VanillaNet( CVPR 2023 华为提出的全新轻量化架构)，大幅度涨点
  • 操作系统知识3
  • 华为全联接大会HUAWEI Connect 2024印象（一）：OpenEuler
  • uniapp沉浸式导航栏+自定义导航栏组件
  • 深入理解端口、端口号及FTP的基本工作原理
  • CREO教程——2 绘制标准图纸
  • python/requests库的使用/爬虫基础工具/
  • 最新版C/C++通过CLion2024进行Linux远程开发保姆级教学
  • 【Docker】基于docker compose部署artifactory-cpp-ce服务
  • 【车联网安全】车端知识调研
  • 产品经理面试整理-软件产品经理的常用工具
  • SpringBoot框架在文档管理中的创新应用
  • 系统架构笔记-3-信息系统基础知识
  • 探讨MySQL中的GROUP BY语句大小写敏感性
  • SegFormer网络结构的学习和重构
  • CSP-S 2024 提高级 第一轮（初赛） 阅读程序（2）
  • 【OSS安全最佳实践】降低因操作失误等原因导致数据丢失的风险
  • 【C++笔试强训】如何成为算法糕手Day2
  • 【c++】知识点
  • 分布式光伏监控系统 在鄂尔多斯市鄂托克旗某煤矿项目中的应用
  • GPU高性能编程CUDA入门
  • 拦截器filter
  • 【ShuQiHere】 探索自然语言处理的世界：从基础到应用
  • flutter中常见的跨组件通讯方式
  • Redis 分布式缓存服务（集群）
  • str函数的模拟（包括strn函数的模拟）