SLAM常用最小二乘最优化方法学习、分析和总结

SLAM中二维视觉的定位问题被最终归为一个最小二乘问题，那么紧随其后的就是对最小二乘的最优化求解，对其求解析解显然是不太合适的，所以就需要一些数值的最优化方法对最小二乘问题进行求解，在SLAM中，常用的算法有，梯度下降法，牛顿法，以及牛顿法的变种方法比如高斯牛顿法、Levenberg-Marquardt算法还有DogLeg算法等等，当然还有大名鼎鼎的图优化方法，图优化算法还没有搞透彻，所以就先给前面的算法做个笔记吧。

一、梯度下降法

梯度下降法又称最速下降法，应该是所有这些爬山法里面最简单了的吧，梯度的数学定义是：梯度的本意是一个向量（矢量），表示某一函数在该点处的方向导数沿着该方向取得最大值，即函数在该点处沿着该方向（此梯度的方向）变化最快，变化率最大（为该梯度的模）。

二、牛顿法

牛顿法着实困扰了我很久，计算方法中的牛顿法就是一个简单的迭代比如在单变量函数中，就是利用目标函数在该点梯度与x轴的交点作为迭代结果进行不断迭代来得到的，怎么到了最小二乘问题中，就用到了二阶的海塞矩阵了呢。

去网上找了不少资料，发现在最小二乘寻优问题中，牛顿法并不是直接对目标函数做传统意义上的牛顿法，而是利用了函数在最小值处导数为零这个性质，对目标函数的导数进行寻优，这样自然也就用到了二阶的海塞矩阵。

具体的实现方法同样移步高博的书吧（感谢高博！^-^）。

三、高斯牛顿法

高斯牛顿法其实与牛顿法类似，但是针对目标函数海塞矩阵难以得到的问题，利用数学推导的方式得到了近似的海塞矩阵。高斯牛顿法不对目标函数进行二阶泰勒展开，而对误差函数进行一阶泰勒展开，即：

而观测值被加上了一组均值为0方差为1的高斯噪声，目标是利用这组观测值得到近似的a,b,c的最优解。首先对三个参数取初值，之后取f(a,b,c)为观测值与估计值的误差，按照高斯牛顿法进行迭代。

clc;clear;close all;
syms a b c x;
parameters = [a b c];
fx = (exp(a*x^2+b*x+c));
Jco = jacobian(fx,parameters)
subs(Jco,[a b c x],[1 2 3 4])
xRange = 0:0.01:1;
dataNum = 20;
aTureth = 1;bTureth=1;cTureth=1;
yTureth = zeros(dataNum,1);
yGuss = zeros(dataNum,1);
for i=1:length(xRange)
   yTureth(i)= exp(aTureth*xRange(i)^2+bTureth*xRange(i)+cTureth);
   yGuss(i) = yTureth(i)+ rand(1)*2-1;
end

% figure;
% hold on;box on;
% plot(xRange,yTureth,'red');
% plot(xRange,yGuss,'.');
% legend('The trueth','With Guss Noise')

IterNum = 100;
aEst=4;bEst=4;cEst =4;
Jacobian = zeros(dataNum,3);
yEst =zeros(dataNum,1);
figure;
box on;
YEst = zeros(dataNum,1);
NeededError = 1e-3;
ErrorNow=0;ErrorPast=100;
generate = 0;
for i=1:IterNum
   for j =1:length(xRange)
      Jacobian(j,:) = [ xRange(j)^2*exp(aEst*xRange(j)^2 + bEst*xRange(j) + cEst), ...
      xRange(j)*exp(aEst*xRange(j)^2 + bEst*xRange(j) + cEst), exp(aEst*xRange(j)^2 + bEst*xRange(j) + cEst)];
      yEst(j)= yGuss(j) - (exp(aEst*xRange(j)^2+bEst*xRange(j)+cEst));
   end
   Hass = Jacobian'*Jacobian;
   deta = pinv(Hass)*(Jacobian')*yEst;
   aEst = aEst+deta(1);
   bEst = bEst+deta(2);
   cEst = cEst+deta(3);
   %disp(aEst);disp(bEst);disp(cEst);
   for k=1:length(xRange)
   YEst(k)= exp(aEst*xRange(k)^2+bEst*xRange(k)+cEst);
   end
   hold off;
   ax1=plot(xRange,yTureth,'red','LineWidth',1);hold on;
   ax2=plot(xRange,yGuss,'.');
   ax3=plot(xRange,YEst,'green','LineWidth',1);
   pause(0.1);
   ErrorNow = norm(yEst);
   disp(ErrorNow - ErrorPast);
   if(ErrorNow - ErrorPast)>-1*NeededError && (ErrorNow - ErrorPast)<NeededError
       break;
   end
   %如果收敛了也要退出
   ErrorPast = ErrorNow;
   figure;
   hold on;box on;
   ax1=plot(xRange,yTureth,'red','LineWidth',1);hold on;
   ax2=plot(xRange,yGuss,'.');
   ax3=plot(xRange,YEst,'green','LineWidth',1);
   legend('Real','Observe','Estimate')
   imagename =sprintf('112233/%d',i)
   %print(1,sprintf('112233/%d',i),'-dbmp') 
   print(gcf,'-dbmp',imagename);
   close;
   generate = generate +1;
end
disp(generate);
for j=1:generate
        im=imread(sprintf('112233/%d.bmp',j));  
        [I,map]=rgb2ind(im,20);   
        if j==1  
           imwrite(I,map,'112233/GIF/meow5.gif','gif', 'Loopcount',inf,'DelayTime',0.2);%FIRST  
       else  
           imwrite(I,map,'112233/GIF/meow5.gif','gif','WriteMode','append','DelayTime',0.2);  
       end  
end

四、LW算法

LW算法其实都是高斯牛顿算法的变种算法，因为高斯牛顿算法使用了近似的海塞矩阵，因此会导致误差增大的问题，所以需要一个置信度区间来衡量这个近似是否可靠。

clc;clear;close all;
syms a b c x;
parameters = [a b c];
fx = (exp(a*x^2+b*x+c));
Jco = jacobian(fx,parameters)
subs(Jco,[a b c x],[1 2 3 4])
xRange = 0:0.01:1;
dataNum = 20;
aTureth = 1;bTureth=1;cTureth=1;
yTureth = zeros(dataNum,1);
yGuss = zeros(dataNum,1);
for i=1:length(xRange)
   yTureth(i)= exp(aTureth*xRange(i)^2+bTureth*xRange(i)+cTureth);
   yGuss(i) = yTureth(i)+ rand(1)*2-1;
end
figure;
hold on;box on;
plot(xRange,yTureth,'red');
plot(xRange,yGuss,'.');
legend('The trueth','With Guss Noise')

IterNum = 150;
aEst=0.01;bEst=0.02;cEst =0.04;
aEstSetForCopy=0;bEstSetForCopy=0;cEstSetForCopy=0
Jacobian = zeros(dataNum,3);
yEst =zeros(dataNum,1);
figure;
box on;
YEst = zeros(dataNum,1);
NeededError = 1e-3;
ErrorNow=0;ErrorPast=100;
lamda = 1e-3;
MatrixD = eye(3);
rho1 = 0;
rhoVector = zeros(dataNum,1);
generate = 0;
for i=1:IterNum
   for j =1:length(xRange)
      Jacobian(j,:) = [ xRange(j)^2*exp(aEst*xRange(j)^2 + bEst*xRange(j) + cEst), ...
      xRange(j)*exp(aEst*xRange(j)^2 + bEst*xRange(j) + cEst), exp(aEst*xRange(j)^2 + bEst*xRange(j) + cEst)];
      yEst(j)= yGuss(j) - (exp(aEst*xRange(j)^2+bEst*xRange(j)+cEst));
   end
   Hass = Jacobian'*Jacobian;
   deta = pinv(Hass)*(Jacobian')*yEst;
   
   aEstSetForCopy = aEst+deta(1);
   bEstSetForCopy = bEst+deta(2);
   cEstSetForCopy = cEst+deta(3);
   for j =1:length(xRange)
      rhoVector(j)= ((exp(aEstSetForCopy*xRange(j)^2+bEstSetForCopy*xRange(j)+cEstSetForCopy))-(exp(aEst*xRange(j)^2+bEst*xRange(j)+cEst)))...
          /(Jacobian(j,:)*deta);
   end
   rho1 = mean(rhoVector);
   if(rho1 > 0.5 && rho1 < 1.5 )
       lamda = lamda /2;
   else
       lamda = lamda *2;
   end
   
   Hass = Jacobian'*Jacobian + lamda * MatrixD;
   deta = pinv(Hass)*(Jacobian')*yEst;
   
   aEst = aEst+deta(1);
   bEst= bEst+deta(2);
   cEst = cEst+deta(3);
   %disp(aEst);disp(bEst);disp(cEst);
   for k=1:length(xRange)
   YEst(k)= exp(aEst*xRange(k)^2+bEst*xRange(k)+cEst);
   end
   hold off;
   ax1=plot(xRange,yTureth,'red','LineWidth',2);hold on;
   ax2=plot(xRange,yGuss,'.');
   ax3=plot(xRange,YEst,'green','LineWidth',2);
   pause(0.1);
   ErrorNow = norm(yEst);
   disp(ErrorNow - ErrorPast);
   if(ErrorNow - ErrorPast)>-1*NeededError && (ErrorNow - ErrorPast)<NeededError
       break;
   end
   %如果收敛了也要退出
   ErrorPast = ErrorNow;
   generate = generate+1;
end
disp(generate);