系列文章目录
·【算法系列】卡尔曼滤波算法
·【算法系列】非线性最小二乘求解-直接求解法
·【算法系列】非线性最小二乘求解-梯度下降法
·【算法系列】非线性最小二乘-高斯牛顿法
·【算法系列】非线性最小二乘-列文伯格马夸尔和狗腿算法
文章目录
系列文章
文章目录
前言
一、梯度下降法（GD）
二、最速下降法（SD）
总结
前言
SLAM问题常规的解决思路有两种，一种是基于滤波器的状态估计，围绕着卡尔曼滤波展开；另一种则是基于图论（因子图）的状态估计，将SLAM问题建模为最小二乘问题，而且一般是非线性最小二乘估计去求解。
非线性最小二乘有多种解法，本篇博客介绍梯度下降法系列求解最小二乘问题。
非线性最小二乘的一般形式如下：
其中是非线性函数，表示协方差矩阵
为了阐述方便，进行如下表示：
一、梯度下降法（GD）
梯度下降法是使自变量x按一定步长沿梯度的反方向进行调整，对应的函数值就会下降，这样不断调整x，直到函数取值下降到最小为止，以下图进行具体说明。
这里的x是一维变量，梯度可以理解为一阶导数，初值选在x1的位置，此时一阶导数值为负，梯度的反方向为正，所以应该增加x的值，按照步长调整至x2，依次迭代；当到达x4位置时，一阶导数变为正值，梯度反方向为负，应该减小x的值，反复迭代，假设收敛到了一个最小值x5，算法结束。
算法具体表示如下：
梯度下降法的原理和实现都很简单，但它的缺点也很明显：
对初值敏感。在图中很容易发现，收敛获得的最小值，只是算法以为的最小值，是个局部最小值，而真实的最小值在橙点处，这跟初值的选取有关。步长的选择至关重要。如果步长太小，收敛速度很慢，需要迭代很多次才能到的目标点；如果步长太大，很可能错过目标点，形成在最小值附件来回震荡的情况。
在SLAM中，状态由三维坐标和空间姿态角两部分组成，空间姿态角一般用四元数表示，由于存在内部额外约束，无法进行求导和加法迭代运算，这时就要装换到李代数上进行求导和求和运算。
MATLAB实验：
主函数：
% 目标函数为 z=f(x,y)=(x^2+y^2)/2clear all;clc;%构造函数fun = inline('(x^2+y^2)/2','x','y');dfunx = inline('x','x','y');dfuny = inline('y','x','y'); x0 = 2;y0 = 2; %初值p = 0.1;%步长[x,y,n,point] = GD(fun,dfunx,dfuny,x0,y0,p); %梯度下降法%[x,y,n,point] = SD(fun,dfunx,dfuny,x0,y0); %最速下降法figurex = -1:0.1:2;y = x;[x,y] = meshgrid(x,y);z = (x.^2+y.^2)/2;surf(x,y,z) %绘制三维表面图形% hold on% plot3(point(:,1),point(:,2),point(:,3),'linewidth',1,'color','black')hold onscatter3(point(:,1),point(:,2),point(:,3),'r','*');
GD函数：
%% 梯度下降法function [x,y,n,point] = GD(fun,dfunx,dfuny,x,y,p)%输入：fun:函数形式 dfunx(y):梯度（导数） x(y):初值 p:步长%输出：x(y):计算出的自变量取值 n:迭代次数 point:迭代点集%初始化a = feval(fun,x,y);n=1;point(n,:) = [x y a];while (1) a = feval(fun,x,y); %当前时刻值x = x - p*(feval(dfunx,x,y)); %下一时刻自变量y = y - p*(feval(dfuny,x,y)); %下一时刻自变量b = feval(fun,x,y); %下一时刻值 if(b>=a)break;endn = n+1;point(n,:) = [x y b]; end
实验结果：
二、最速下降法（SD）
最速下降法解决的是梯度下降法中关于步长选取的问题，最速下降法中每次迭代都会找到一个合适的步长，使得函数沿当前梯度反方向下降，用数学语言描述如下：
如下图所示：
自变量x是二维向量，此时的梯度方向与等高线切线方向垂直，每次都会选取一个合适的步长，使得取值越来越趋近于最小值，每次的步长都不是固定值，保证了函数取值一直是下降的。
MATLAB实验：
主函数：
% 目标函数为 z=f(x,y)=(x^2+y^2)/2clear all;clc;%构造函数fun = inline('(x^2+y^2)/2','x','y');dfunx = inline('x','x','y');dfuny = inline('y','x','y'); x0 = 2;y0 = 2; %初值p = 0.1;%步长%[x,y,n,point] = GD(fun,dfunx,dfuny,x0,y0,p); %梯度下降法[x,y,n,point] = SD(fun,dfunx,dfuny,x0,y0); %最速下降法figurex = -1:0.1:2;y = x;[x,y] = meshgrid(x,y);z = (x.^2+y.^2)/2;surf(x,y,z) %绘制三维表面图形% hold on% plot3(point(:,1),point(:,2),point(:,3),'linewidth',1,'color','black')hold onscatter3(point(:,1),point(:,2),point(:,3),'r','*');
SD函数：
%% 梯度下降法function [x,y,n,point] = SD(fun,dfunx,dfuny,x,y)%输入：fun:函数形式 dfunx(y):梯度（导数） x(y):初值%输出：x(y):计算出的自变量取值 n:迭代次数 point:迭代点集%初始化a = feval(fun,x,y);n=1;point(n,:) = [x y a];p=0.01:0.01:0.1; %步长范围while (1) [m,i]=min(x - p*(feval(dfunx,x,y))); %求解合适的步长a = feval(fun,x,y); %当前时刻值x = x - p(i)*(feval(dfunx,x,y));%下一时刻自变量y = y - p(i)*(feval(dfuny,x,y));%下一时刻自变量b = feval(fun,x,y); %下一时刻值if(b>=a)break;endn = n+1;point(n,:) = [x y b]; end
实验结果：
总结
虽然最速下降法解决了步长选取的问题，但是在实际使用中，不可避免的会出现初值选取不合适导致获得局部最小值的问题，接下来将介绍高斯-牛顿的方法、裂纹伯格-马夸尔的方法及其变种。
实际应用中应对这几种方法灵活选择。
