一、高斯牛顿法详解
拓展阅读:高斯牛顿法详解_gauss-newton算法步骤-CSDN博客
1、梯度下降法
无论一阶泰勒展开,还是二阶泰勒展开都是关于增量的方程。
2、牛顿法
这个自变量增量都是可求的。但是二阶求解复杂。因此为了简化有了下面的高斯牛顿法。不过只适用于最小二乘法。
3、高斯牛顿法
最小二乘法展开的是后面的函数部分。将f(x)一阶泰勒展开(一阶就要带雅可比矩阵)。而非目标函数展开。
记住这个增量方程中的。这里后面代码要用到。
缺点:当近似求解的增量过大时,算法无法收敛,我理解到是不是通俗说的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和上一次迭代的costchrono::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-NewtonVector3d b = Vector3d::Zero(); // biascost = 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/daJ[1] = -xi * exp(ae * xi * xi + be * xi + ce); // de/dbJ[2] = -exp(ae * xi * xi + be * xi + ce); // de/dcH += inv_sigma * inv_sigma * J * J.transpose();b += -inv_sigma * inv_sigma * error * J;cost += error * error;}// 求解线性方程 Hx=bVector3d 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;//初始化数据容器,容器内元素类型为doublevector<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;//本次迭代和上次迭代的costdouble 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和costMatrix3d 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/daJ[1] = -xi * exp(ae * xi * xi + be * xi + ce); // de/dbJ[2] = -exp(ae * xi * xi + be * xi + ce); // de/dcH += inv_sigma * inv_sigma * J * J.transpose(); //这里除以sigma是归一化b += -inv_sigma * inv_sigma * error * J;cost += error * error;}//求解线性方程Hx=bVector3d 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;}//及时结束,获取当前时间赋给t2chrono::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的算法流程如下: