高翔【自动驾驶与机器人中的SLAM技术】学习笔记（四）高斯牛顿法详解

xiaoyaolangwj

已于 2024-07-26 17:19:03 修改

阅读量1k

点赞数 12

分类专栏：高翔书《自动驾驶与机器人中的SLAM技术》学习笔记文章标签：自动驾驶机器人学习

于 2024-07-26 17:10:31 首次发布

本文链接：https://blog.csdn.net/xiaoyaolangwj/article/details/140611034

版权

高翔书《自动驾驶与机器人中的SLAM技术》学习笔记专栏收录该内容

9 篇文章 3 订阅

订阅专栏

一、高斯牛顿法详解

拓展阅读：高斯牛顿法详解_gauss-newton算法步骤-CSDN博客

1、梯度下降法

无论一阶泰勒展开，还是二阶泰勒展开都是关于增量 $\Delta x_k$ 的方程。

2、牛顿法

这个自变量增量都是可求的。但是二阶求解复杂。因此为了简化有了下面的高斯牛顿法。不过只适用于最小二乘法。

3、高斯牛顿法

最小二乘法展开的是后面的函数部分。将f(x)一阶泰勒展开（一阶就要带雅可比矩阵）。而非目标函数展开。

记住这个增量方程中的 $H(x_k)$ 。这里后面代码要用到。

缺点：当近似求解的增量过大时，算法无法收敛，我理解到是不是通俗说的SLAM飞了。

缺点：雅可比矩阵有时是奇异矩阵。从而导致增量不稳定。

补充：来源《随手笔记——如何手写高斯牛顿法》

还是那句话：高斯牛顿法是对：最小二乘法展开的是后面的函数部分。将f(x)一阶泰勒展开（一阶就要带雅可比矩阵）。而非目标函数展开。是对小f(x)（每个样本即误差项）

下面这个图是讲最小二乘法的样式：

模型函数预测值与观察值（真实值）之间的偏差的二次方的加和为目标函数。这个目标函数等效上面提到的大F(x)。
而高斯牛顿法是将这个上面这个偏差项给展开了。是对这个偏差项进行了泰拉展开，而不是对目标函数。

最优化算法之高斯牛顿法

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器

  vector<double> x_data, y_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  // 开始Gauss-Newton迭代
  int iterations = 100;    // 迭代次数
  double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (int iter = 0; iter < iterations; iter++) {

    Matrix3d H = Matrix3d::Zero();             // 黑森(海塞)矩阵：Hessian = J^T W^{-1} J in Gauss-Newton
    Vector3d b = Vector3d::Zero();             // bias
    cost = 0;

    for (int i = 0; i < N; i++) {
      double xi = x_data[i], yi = y_data[i];  // 第i个数据点
      double error = yi - exp(ae * xi * xi + be * xi + ce);
      Vector3d J; // 雅可比矩阵
      J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
      J[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/db
      J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc

      H += inv_sigma * inv_sigma * J * J.transpose();
      b += -inv_sigma * inv_sigma * error * J;

      cost += error * error;
    }

    // 求解线性方程 Hx=b
    Vector3d dx = H.ldlt().solve(b);
    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost) {
      cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
      break;
    }

    ae += dx[0];
    be += dx[1];
    ce += dx[2];

    lastCost = cost;

    cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
         "\t\testimated params: " << ae << "," << be << "," << ce << endl;
  }

  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
  return 0;
}

手写高斯牛顿法-CSDN博客

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main() {
    //设定曲线真实参数
    double ar = 1.0, br = 2.0, cr = 1.0;
    //给定曲线参数优化初始估计值
    double ae = 2.0, be = -1.0, ce = 5.0;
    //设定数据点个数
    int N = 100;
    //设定噪声服从的正态分布的sigma值
    double w_sigma = 1.0;
    //计算sigma的倒数，之后用于误差归一化
    double inv_sigma = 1.0 / w_sigma;
    //OpenCV随机数产生器
    cv::RNG rng;

    //初始化数据容器，容器内元素类型为double
    vector<double> x_data, y_data;
    //生成N个数据点
    for (int i=0; i < N; ++i){
        //x在0-1之间均匀取100个值
        double x = i / 100.0;
        x_data.push_back(x);
        //y用真实函数生成再加上高斯噪声
        y_data.push_back(exp(ar*x*x+br*x+cr)+rng.gaussian(w_sigma * w_sigma));
    }

    //开始高斯牛顿迭代
    //设定迭代次数
    int iterations = 100;
    //本次迭代和上次迭代的cost
    double cost = 0, lastcost = 0;

    //开始及时，当前时间点存储到t1中
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    //牛顿高斯算法迭代iterations次
    for (int iter = 0; iter < iterations; ++iter) {



        //初始化H矩阵，b矩阵,雅克比矩阵J和cost
        Matrix3d H = Matrix3d::Zero();
        Vector3d b = Vector3d::Zero();
        cost = 0;

        //对N个数据点进行处理，列出总的增量方程，计算初始误差
        for (int i = 0; i < N; ++i) {
            double xi = x_data[i], yi = y_data[i];
            double error = yi - exp(ae * xi * xi + be * xi + ce);
            //计算雅克比矩阵在该点取值
            Vector3d J;
            J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
            J[1] = -xi * exp(ae * xi * xi + be * xi + ce);   // de/db
            J[2] = -exp(ae * xi * xi + be * xi + ce);   // de/dc

            H += inv_sigma * inv_sigma * J * J.transpose();   //这里除以sigma是归一化
            b += -inv_sigma * inv_sigma * error * J;

            cost += error * error;
        }

        //求解线性方程Hx=b
        Vector3d dx = H.ldlt().solve(b);
        //如果方程无解，那么dx[0]是非法字符nan，退出迭代
        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        //如果本次迭代误差大于上次误差，算法结束，退出迭代
        if(iter > 0 && cost >= lastcost){
            cout << "cost:" << cost << ">=" << lastcost << ",break." << endl;
            break;
        }

        //进行估计参数的增量更新，存储本次代价
        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastcost = cost;
        //输出本次迭代信息
        cout << "total cost:" << cost << ",\t\tupdate:" << dx.transpose() << "\t\testimatec:" << ae << "," << be <<
        "," << ce << endl;

    }
    //及时结束，获取当前时间赋给t2
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    //计算算法耗时并输出
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

    //输出最终算法迭代结果
    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    return 0;
}

cmake_minimum_required(VERSION 3.15)
project(GuassNewton)

set(CMAKE_CXX_STANDARD 14)

#OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

#Eigen
include_directories("/usr/include/eigen3")

add_executable(GuassNewton main.cpp)
target_link_libraries(GuassNewton ${OpenCV_LIBS})

4、列文伯格-马夸尔特方法

因为高斯牛顿更新时增量可能会不稳定，甚至太大。所以为了使增量的更加稳定可靠，对其做了限制，增加了置信域。

5、拟牛顿法

为了解决牛顿法中海森矩阵H计算复杂的问题，拟牛顿法提供了另外一种解决思路：

通过使用不含有二阶导数的矩阵U代替牛顿法中的H，根据矩阵U构造的不同，具有不同的拟牛顿法。

1、拟牛顿法的基本原理

2、DFP

为了方便区分，下面把U称作D（表示DFP）

1）DFP结果

DFP算法的问题在于在求取增量的时候D矩阵仍要求逆

3、BFGS

2)BFGS算步骤

4、L-BFGS

1）L-BFGS原理

2）L-BFGS应用

因此，L-BFGS的算法流程如下：

xiaoyaolangwj

关注

12
点赞
踩
10

收藏

觉得还不错? 一键收藏
0
评论
高翔【自动驾驶与机器人中的SLAM技术】学习笔记（四）高斯牛顿法详解

还是那句话：高斯牛顿法是对：最小二乘法展开的是后面的函数部分。将f(x)一阶泰勒展开（一阶就要带雅可比矩阵）。这个自变量增量都是可求的。因此为了简化有了下面的高斯牛顿法。不过只适用于最小二乘法。最小二乘法展开的是后面的函数部分。将f(x)一阶泰勒展开（一阶就要带雅可比矩阵）。通过使用不含有二阶导数的矩阵U代替牛顿法中的H，根据矩阵U构造的不同，具有不同的拟牛顿法。所以为了使增量的更加稳定可靠，对其做了限制，增加了置信域。无论一阶泰勒展开，还是二阶泰勒展开都是关于增量。缺点：雅可比矩阵有时是。
复制链接

扫一扫