手写非线性最小二乘

最新推荐文章于 2022-03-23 11:13:09 发布

一朵云008

最新推荐文章于 2022-03-23 11:13:09 发布

阅读量422

点赞数

分类专栏：【SLAM探索】

本文链接：https://blog.csdn.net/David_Han008/article/details/106076786

版权

【SLAM探索】专栏收录该内容

11 篇文章 27 订阅

订阅专栏

使用梯度下降法来求解非线性最小二乘问题

方法1:梯度下降法

#include <iostream>
#include <ros/ros.h>
#include <eigen3/Eigen/Core>
#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/QR>
#include <vector>
#include <eigen3/Eigen/Dense>
class descent_method
{
private:
    /* data */

    double a_; //初始数值
    double b_;
    double c_;
    double lamda_;
    Eigen::Vector3d Gradient_;
    int max_iter_;
    double min_step_;
    //Eigen::MatrixXd Gradient_; // 梯度矩阵

    void cal_Gradient();
    void update_deltT();
    std::vector<double> x_obs_;
    std::vector<double> y_obs_;

public:
    descent_method(double a, double b, double c);
    void add_obs(double x, double y);
    void solve_cost();
    ~descent_method();
};

descent_method::descent_method(double a, double b, double c) : a_(a), b_(b), c_(c)
{
    max_iter_ = 50000;
    min_step_ = 1e-3;
    lamda_ =1e-3;
}

descent_method::~descent_method()
{
}
void descent_method::add_obs(double x, double y) //添加观测
{
    x_obs_.push_back(x);
    y_obs_.push_back(y);
}
void descent_method::solve_cost()
{
    for (int i = 0; i < max_iter_; i++) //最大的迭代次数
    {
        cal_Gradient(); //计算梯度
        //std::cout<<"success cal J"<<std::endl;
        double dg = sqrt(Gradient_.transpose() * Gradient_);
        std::cout<<"cal Gradient:"<<dg<<std::endl;
        if (dg * lamda_ <= min_step_)
        {
            std::cout << "iter:" << i << "a:" << a_ << "b:" << b_ << "c" << c_ << std::endl;
            break;
        }
        update_deltT();
    }
}
void descent_method::cal_Gradient()
{
    //Gradient_.resize(3,1); //有多少个观测数据,列数表示向量的个数
    double ga = 0;
    double gb = 0;
    double gc = 0;
    for (int i = 0; i < x_obs_.size(); i++)
    {
        //std::cout<<"i"<<i<<std::endl;
        double x = x_obs_.at(i);
        double y = y_obs_.at(i);
        //std::cout<<"原始数据:"<<x<<";"<<y<<std::endl;
        ga += x * x * (y - exp(a_ * x * x + b_ * x + c_)); //对每个点求偏导
        gb += x * (y - exp(a_ * x * x + b_ * x + c_));     //
        gc += y - exp(a_ * x * x + b_ * x + c_);           //
        //std::cout<<"雅克比:"<<J_<<std::endl;求雅克比矩阵的意义在与什么
    }
    Gradient_(0)=ga;
    Gradient_(1)=gb;
    Gradient_(2)=gc;
}
void descent_method::update_deltT()
{
    //std::cout<<"deltax_(0)"<<delt_t_(0)<<std::endl;
    a_ -= lamda_ * Gradient_(0);
    b_ -= -lamda_ * Gradient_(1);
    c_ -= -lamda_ * Gradient_(2);
}

int main(int argc, char **argv)
{
    //利用四种不同的方法,来求解非线最小二乘问题
    //第一种方法,最速下降法
    //实际方程的表达形式 y=exp(aax^2+bbx+cc)
    const double aa = 0.1, bb = 0.5, cc = 2; // 实际方程的参数
    double a = 0.0, b = 0.0, c = 0.0;        // 初值
    descent_method des_method(a, b, c);
    //添加噪声
    const size_t N = 100; //数据个数
    cv::RNG rng(cv::getTickCount());
    //已知x,y求相关曲线拟合的系数
    // 100个数据,3个未知量
    for (size_t i = 0; i < N; i++)
    {
        /* 产生带有高斯噪声的数据　*/
        double x = rng.uniform(0.0, 1.0);
        double y = exp(aa * x * x + bb * x + cc) + rng.gaussian(0.05);
        des_method.add_obs(x, y);
        //将每个数据放入其中
    }
    des_method.solve_cost();

    return 0;
}

目前梯度下降算法还存在一些问题,例如如何调lamda的参数跟min_step之间的关系

参考博客:

[1] https://www.jianshu.com/p/bf6ec56e26bd

[2] https://zhuanlan.zhihu.com/p/42383070