非线性最小二乘法之Gauss Newton、L-M、Dog-Leg9

于 2021-06-28 22:18:27 发布
阅读量650 收藏 5
点赞数 2
分类专栏： 数学基础 文章标签： matlab 线性代数 算法
原文链接：https://blog.csdn.net/stihy/article/details/52737723?hmsr=toutiao.io&utm_medium=toutiao.io&utm_source=toutiao.io
版权

最优化理论·非线性最小二乘

最优化理论·非线性最小二乘_努力努力努力-CSDN博客

最快下降法

理解：

        F(x)^{'} = \lim_{\Delta x\rightarrow 0}\frac{F(x+\Delta x)-F(x)}{\Delta x}

         a*b = |a|*|b|*cos(\theta)

最小二乘问题

通常的最小二乘问题都可以表示为：

GaussNewton

close all;
clc;

a=1;b=2;c=1;               %待求解的系数

x=(0:0.01:1)';
w=rand(length(x),1)*2-1;   %生成噪声
y_noise=exp(a*x.^2+b*x+c)+w;     %带噪声的模型 

pre=rand(3,1);  %步骤1，待拟合的参数，初始值是随机数；

for i=1:1000 
    
    f = exp(pre(1)*x.^2+pre(2)*x+pre(3));   % 
    g = y_noise-f;                                % 步骤2，构建拟合的函数和真实函数的误差；视觉中，也是根据观测的值减去预测的值，然后优化其中的参数，使两者最小； 
    
    p1 = exp(pre(1)*x.^2+pre(2)*x+pre(3)).*x.^2;    % 残差函数g对a求偏导，在ceres中也是有对应的偏导项的
    p2 = exp(pre(1)*x.^2+pre(2)*x+pre(3)).*x;       % 残差函数g对b求偏导；
    p3 = exp(pre(1)*x.^2+pre(2)*x+pre(3));          % 残差函数g对c求偏导；
    J = [p1 p2 p3];                                 % 步骤2中的雅克比矩阵
    
    delta = inv(J'*J)*J'* g;                        %步骤3，inv(J'*J)*J'为H的逆
    
    pcur = pre+delta;                               %步骤4
    if norm(delta) <1e-16
        break;
    end
    pre = pcur;
end



 

yreal = exp(a*x.^2+b*x+c);
ypre=exp(pre(1)*x.^2+pre(2)*x+pre(3));  % 拟合的曲线

figure(1)
plot(x,y_noise,'b')
hold on
plot(x,yreal,'r')
hold on
plot(x,ypre,'k');
grid on;
set(gca, 'GridLineStyle', '--');   % 设置为虚线
set(gca, 'GridAlpha', 1.0);        % 设置透明度
xlabel('X');
ylabel('Y');
legend('带有噪声的真实曲线','不带噪声的真实曲线','拟合曲线')

figure(2)
plot(x,yreal - ypre,'b')
grid on;
set(gca, 'GridLineStyle', '--');   % 设置为虚线
set(gca, 'GridAlpha', 1.0);        % 设置透明度
xlabel('X');
ylabel('Y');
legend('真值和优化值之间的误差曲线')


%比较一下
[a b c]
pre'

LM阻尼最小二乘法

Dog-Leg最小二乘法

举例

Gauss Newton代码：

double func(const VectorXd& input, const VectorXd& output, const VectorXd& params, double objIndex)
{
    // obj = A * sin(Bx) + C * cos(D*x) - F
    double x1 = params(0);
    double x2 = params(1);
    double x3 = params(2);
    double x4 = params(3);

    double t = input(objIndex);
    double f = output(objIndex);

    return x1 * sin(x2 * t) + x3 * cos( x4 * t) - f;
}

//return vector make up of func() element.
VectorXd objF(const VectorXd& input, const VectorXd& output, const VectorXd& params)
{
    VectorXd obj(input.rows());
    for(int i = 0; i < input.rows(); i++)
        obj(i) = func(input, output, params, i);

    return obj;
}

//F = (f ^t * f)/2
double Func(const VectorXd& obj)
{
    return obj.squaredNorm()/2;
}

double Deriv(const VectorXd& input, const VectorXd& output, int objIndex, const VectorXd& params,
                 int paraIndex)
{
    VectorXd para1 = params;
    VectorXd para2 = params;

    para1(paraIndex) -= DERIV_STEP;
    para2(paraIndex) += DERIV_STEP;

    double obj1 = func(input, output, para1, objIndex);
    double obj2 = func(input, output, para2, objIndex);

    return (obj2 - obj1) / (2 * DERIV_STEP);
}

MatrixXd Jacobin(const VectorXd& input, const VectorXd& output, const VectorXd& params)
{
    int rowNum = input.rows();
    int colNum = params.rows();

    MatrixXd Jac(rowNum, colNum);

    for (int i = 0; i < rowNum; i++)
    {
        for (int j = 0; j < colNum; j++)
        {
            Jac(i,j) = Deriv(input, output, i, params, j);
        }
    }
    return Jac;
}

void gaussNewton(const VectorXd& input, const VectorXd& output, VectorXd& params)
{
    int errNum = input.rows();      //error  num
    int paraNum = params.rows();    //parameter  num

    VectorXd obj(errNum);

    double last_sum = 0;

    int iterCnt = 0;
    while (iterCnt < MAX_ITER)
    {
        obj = objF(input, output, params);

        double sum = 0;
        sum = Func(obj);

        cout << "Iterator index: " << iterCnt << endl;
        cout << "parameter: " << endl << params << endl;
        cout << "error sum: " << endl << sum << endl << endl;

        if (fabs(sum - last_sum) <= 1e-12)
            break;
        last_sum = sum;

        MatrixXd Jac = Jacobin(input, output, params);
        VectorXd delta(paraNum);
        delta = (Jac.transpose() * Jac).inverse() * Jac.transpose() * obj;

        params -= delta;
        iterCnt++;
    }
}

LM代码：

double maxMatrixDiagonale(const MatrixXd& Hessian)
{
    int max = 0;
    for(int i = 0; i < Hessian.rows(); i++)
    {
        if(Hessian(i,i) > max)
            max = Hessian(i,i);
    }

    return max;
}

//L(h) = F(x) + h^t*J^t*f + h^t*J^t*J*h/2
//deltaL = h^t * (u * h - g)/2
double linerDeltaL(const VectorXd& step, const VectorXd& gradient, const double u)
{
    double L = step.transpose() * (u * step - gradient);
    return L/2;
}

void levenMar(const VectorXd& input, const VectorXd& output, VectorXd& params)
{
    int errNum = input.rows();      //error num
    int paraNum = params.rows();    //parameter num

    //initial parameter 
    VectorXd obj = objF(input,output,params);
    MatrixXd Jac = Jacobin(input, output, params);  //jacobin
    MatrixXd A = Jac.transpose() * Jac;             //Hessian
    VectorXd gradient = Jac.transpose() * obj;      //gradient

    //initial parameter tao v epsilon1 epsilon2
    double tao = 1e-3;
    long long v = 2;
    double eps1 = 1e-12, eps2 = 1e-12;
    double u = tao * maxMatrixDiagonale(A);
    bool found = gradient.norm() <= eps1;
    if(found) return;

    double last_sum = 0;
    int iterCnt = 0;

    while (iterCnt < MAX_ITER)
    {
        VectorXd obj = objF(input,output,params);

        MatrixXd Jac = Jacobin(input, output, params);  //jacobin
        MatrixXd A = Jac.transpose() * Jac;             //Hessian
        VectorXd gradient = Jac.transpose() * obj;      //gradient

        if( gradient.norm() <= eps1 )
        {
            cout << "stop g(x) = 0 for a local minimizer optimizer." << endl;
            break;
        }

        cout << "A: " << endl << A << endl; 

        VectorXd step = (A + u * MatrixXd::Identity(paraNum, paraNum)).inverse() * gradient; //negtive Hlm.

        cout << "step: " << endl << step << endl;

        if( step.norm() <= eps2*(params.norm() + eps2) )
        {
            cout << "stop because change in x is small" << endl;
            break;
        } 

        VectorXd paramsNew(params.rows());
        paramsNew = params - step; //h_lm = -step;

        //compute f(x)
        obj = objF(input,output,params);

        //compute f(x_new)
        VectorXd obj_new = objF(input,output,paramsNew);

        double deltaF = Func(obj) - Func(obj_new);
        double deltaL = linerDeltaL(-1 * step, gradient, u);

        double roi = deltaF / deltaL;
        cout << "roi is : " << roi << endl;
        if(roi > 0)
        {
            params = paramsNew;
            u *= max(1.0/3.0, 1-pow(2*roi-1, 3));
            v = 2;
        }
        else
        {
            u = u * v;
            v = v * 2;
        }

        cout << "u = " << u << " v = " << v << endl;

        iterCnt++;
        cout << "Iterator " << iterCnt << " times, result is :" << endl << endl;
    }
}

Dog-Leg代码：

void dogLeg(const VectorXd& input, const VectorXd& output, VectorXd& params)
{
    int errNum = input.rows();      //error num
    int paraNum = params.rows();    //parameter num

    VectorXd obj = objF(input, output, params);
    MatrixXd Jac = Jacobin(input, output, params);  //jacobin
    VectorXd gradient = Jac.transpose() * obj;      //gradient

    //initial parameter tao v epsilon1 epsilon2
    double eps1 = 1e-12, eps2 = 1e-12, eps3 = 1e-12;
    double radius = 1.0;

    bool found  = obj.norm() <= eps3 || gradient.norm() <= eps1;
    if(found) return;

    double last_sum = 0;
    int iterCnt = 0;
    while(iterCnt < MAX_ITER)
    {
        VectorXd obj = objF(input, output, params);
        MatrixXd Jac = Jacobin(input, output, params);  //jacobin
        VectorXd gradient = Jac.transpose() * obj;      //gradient

        if( gradient.norm() <= eps1 )
        {
            cout << "stop F'(x) = g(x) = 0 for a global minimizer optimizer." << endl;
            break;
        }
        if(obj.norm() <= eps3)
        {
            cout << "stop f(x) = 0 for f(x) is so small";
            break;
        }

        //compute how far go along stepest descent direction.
        double alpha = gradient.squaredNorm() / (Jac * gradient).squaredNorm();
        //compute gauss newton step and stepest descent step.
        VectorXd stepest_descent = -alpha * gradient;
        VectorXd gauss_newton = (Jac.transpose() * Jac).inverse() * Jac.transpose() * obj * (-1);

        double beta = 0;

        //compute dog-leg step.
        VectorXd dog_leg(params.rows());
        if(gauss_newton.norm() <= radius)
            dog_leg = gauss_newton;
        else if(alpha * stepest_descent.norm() >= radius)
            dog_leg = (radius / stepest_descent.norm()) * stepest_descent;
        else
        {
            VectorXd a = alpha * stepest_descent;
            VectorXd b = gauss_newton;
            double c = a.transpose() * (b - a);
            beta = (sqrt(c*c + (b-a).squaredNorm()*(radius*radius-a.squaredNorm()))-c)
                    /(b-a).squaredNorm();

            dog_leg = alpha * stepest_descent + beta * (gauss_newton - alpha * stepest_descent);

        }

        cout << "dog-leg: " << endl << dog_leg << endl;

        if(dog_leg.norm() <= eps2 *(params.norm() + eps2))
        {
            cout << "stop because change in x is small" << endl;
            break;
        }

        VectorXd new_params(params.rows());
        new_params = params + dog_leg;

        cout << "new parameter is: " << endl << new_params << endl;

        //compute f(x)
        obj = objF(input,output,params);
        //compute f(x_new)
        VectorXd obj_new = objF(input,output,new_params);

        //compute delta F = F(x) - F(x_new)
        double deltaF = Func(obj) - Func(obj_new);

        //compute delat L =L(0)-L(dog_leg)
        double deltaL = 0;
        if(gauss_newton.norm() <= radius)
            deltaL = Func(obj);
        else if(alpha * stepest_descent.norm() >= radius)
            deltaL = radius*(2*alpha*gradient.norm() - radius)/(2.0*alpha);
        else
        {
            VectorXd a = alpha * stepest_descent;
            VectorXd b = gauss_newton;
            double c = a.transpose() * (b - a);
            beta = (sqrt(c*c + (b-a).squaredNorm()*(radius*radius-a.squaredNorm()))-c)
                    /(b-a).squaredNorm();

            deltaL = alpha*(1-beta)*(1-beta)*gradient.squaredNorm()/2.0 + beta*(2.0-beta)*Func(obj);

        }

        double roi = deltaF / deltaL;
        if(roi > 0)
        {
            params = new_params;
        }
        if(roi > 0.75)
        {
            radius = max(radius, 3.0 * dog_leg.norm());
        }
        else if(roi < 0.25)
        {
            radius = radius / 2.0;
            if(radius <= eps2*(params.norm()+eps2))
            {
                cout << "trust region radius is too small." << endl;
                break;
            }
        }

        cout << "roi: " << roi << " dog-leg norm: " << dog_leg.norm() << endl;
        cout << "radius: " << radius << endl;

        iterCnt++;
        cout << "Iterator " << iterCnt << " times" << endl << endl;
    }
}

main()

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Sparse>
#include <iostream>
#include <iomanip>
#include <math.h>

using namespace std;
using namespace Eigen;

const double DERIV_STEP = 1e-5;
const int MAX_ITER = 100;

#define max(a,b) (((a)>(b))?(a):(b))

int main(int argc, char* argv[])
{
    // obj = A * sin(Bx) + C * cos(D*x) - F
    //there are 4 parameter: A, B, C, D.
    int num_params = 4;

    //generate random data using these parameter
    int total_data = 100;

    VectorXd input(total_data);
    VectorXd output(total_data);

    double A = 5, B= 1, C = 10, D = 2;
    //load observation data
    for (int i = 0; i < total_data; i++)
    {
        //generate a random variable [-10 10]
        double x = 20.0 * ((random() % 1000) / 1000.0) - 10.0;
        double deltaY = 2.0 * (random() % 1000) /1000.0;
        double y = A*sin(B*x)+C*cos(D*x) + deltaY;

        input(i) = x;
        output(i) = y;
    }

    //gauss the parameters
    VectorXd params_gaussNewton(num_params);
    //init gauss
    params_gaussNewton << 1.6, 1.4, 6.2, 1.7;

    VectorXd params_levenMar = params_gaussNewton;
    VectorXd params_dogLeg   = params_gaussNewton;

    gaussNewton(input, output, params_gaussNewton);
    levenMar(input, output, params_levenMar);
    dogLeg(input, output, params_dogLeg);

    cout << "gauss newton parameter: " << endl << params_gaussNewton << endl << endl << endl;
    cout << "Levenberg-Marquardt parameter: " << endl << params_levenMar << endl << endl << endl;
    cout << "dog-leg parameter: " << endl << params_dogLeg << endl << endl << endl;
}

least square

相关资料

最小二乘问题的四种解法——牛顿法，梯度下降法，高斯牛顿法和列文伯格-马夸特法的区别和联系 - 知乎

他人是一面镜子，保持谦虚的态度
关注
  • 2
    点赞
  • 踩
  • 5
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值