[数据结构与算法]高斯牛顿法非线性优化

1. 手写高斯牛顿
   原理看十四讲第6讲

   #include<iostream>
   #include<eigen3/Eigen/Core>
   #include<eigen3/Eigen/Dense>
   #include<opencv2/opencv.hpp>
   #include<vector>
   
   using namespace std;
   
   int main(int argc,char** argv)
   {
       /**
        * y = exp(ax^2 + bx + c) + w
        * 带有噪声 w - N(0,1^2)
        * 误差项 e = y-exp(ax^2 + bx + c)
        * */
       //真实参数
       double ar=1,br=2,cr=1;
       //初始估计值，很重要，有可能收敛到局部极小值
       double ae=2,be=-1,ce=5;
       //随机数产生
       cv::RNG rng;
   
       //生成样本 100个 
       int num=100;  
       //相当与100个方程，3个未知数，所以要最小二乘法
       vector<double> X, Y;
       for(int i=0;i<num;i++)
       {
           double x = i*(1.0/100);
           double y = exp(ar*x*x+br*x+cr) + rng.gaussian(1.0);
           X.push_back(x);
           Y.push_back(y);
       }
   
       double cost=0, lastcost=0;
       //跌代
       for(int iter=0;iter<100;iter++)
       {
           Eigen::Matrix<double,3,3> H = Eigen::Matrix<double,3,3>::Zero();
           Eigen::Matrix<double,3,1> g = Eigen::Matrix<double,3,1>::Zero();
           Eigen::Matrix<double,3,1> dx;
           cost = 0;///重要，别忘了，先重置为0
           double err=0;
           for(int i=0;i<num;i++)
           {
               double x=X[i];
               double y=Y[i];
               Eigen::Matrix<double,3,1> J = Eigen::Matrix<double,3,1>::Zero();  //雅可比矩阵
               /** J =d(e)/d(a,b,c) 对要求的量(a,b,c)求导
                * J[0]  -x*x*exp(a*x*x+b*x+c)
                * J[1] -x*exp(a*x*x+b*x+c)
                * J[2] -exp(a*x*x+b*x+c)
                * */
               J[0] = -x*x*exp(ae*x*x+be*x+ce);
               J[1] = -x*exp(ae*x*x+be*x+ce);
               J[2] = -exp(ae*x*x+be*x+ce);
               err = y-exp(ae*x*x+be*x+ce);
   
               H+=J*J.transpose();  //!!!!! 若干样本的 H 累加起来
               g+=-J*err;     //!!!!! 若干样本的 g 累加起来
               cost+=err*err;
           }
           dx=H.inverse()*g;
           if(iter>0&&cost>=lastcost)
           {
               cout<<"迭代次数: "<<iter<<endl;
               cout<<"cost = "<<lastcost<<endl;
               cout<<"a = "<<ae<<"\tb = "<<be<<"\tc = "<<ce<<endl;
               break;
           }
           cout<<"cost = "<<cost<<"\ta = "<<ae<<"\tb = "<<be<<"\tc = "<<ce<<endl;
           //更新估计量
           ae+=dx[0];be+=dx[1];ce+=dx[2];
           
           lastcost=cost;
       }
   
   }