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