zoukankan      html  css  js  c++  java
  • 批量状态估计和非线性优化

     

     

     

     

     

     

     

     

     

     

     

     

     

     

     

     

     

     

     

    #include <iostream>
    #include <chrono>
    #include <opencv2/opencv.hpp>
    #include <Eigen/Core>
    #include <Eigen/Dense>
    
    using namespace std;
    using namespace Eigen;
    
    int main(int argc, char **argv) {
      double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
      double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值初始值
      int N = 100;                                 // 数据点
      double w_sigma = 2.0;                        // 噪声Sigma值
      double inv_sigma = 1.0 / w_sigma;
      cv::RNG rng;                                 // OpenCV随机数产生器
    
      vector<double> x_data, y_data;      // 真实数据
      for (int i = 0; i < N; i++) {       //生成数据
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
      }
    
      // 开始Gauss-Newton迭代
      int iterations = 1000;    // 迭代次数
      double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost
    
      chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
      for (int iter = 0; iter < iterations; iter++) {
    
        Matrix3d H = Matrix3d::Zero();             // Hessian=J*J^T  in Gauss-Newton
        Vector3d b = Vector3d::Zero();             // bias 书本中的g(x)
        cost = 0;
    
        for (int i = 0; i < N; i++) {
          double xi = x_data[i], yi = y_data[i];  // 第i个数据点
          double error = yi - exp(ae * xi * xi + be * xi + ce);
          Vector3d J; // 雅可比矩阵
          J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
          J[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/db
          J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc
    
    //      H += inv_sigma * inv_sigma * J * J.transpose();//不知道为什么要乘inv_sigma * inv_sigma
    //      b += -inv_sigma * inv_sigma * error * J;     //两边同乘一个不为0的常数,实际对结果没有影响
            H += J * J.transpose();
            b += -error * J;
          cost += error * error;
        }
    
        // 求解线性方程 Hx=b
        Vector3d dx = H.ldlt().solve(b);
        if (isnan(dx[0])) {
          cout << "result is nan!" << endl;
          break;
        }
    
        if (iter > 0 && cost >= lastCost) {
          cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
          break;
        }
    
        ae += dx[0];
        be += dx[1];
        ce += dx[2];
    
        lastCost = cost;
    
        cout << "total cost: " << cost << ", 		update: " << dx.transpose() <<
             "		estimated params: " << ae << "," << be << "," << ce << endl;
      }
    
      chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
      chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
      cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
    
      cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
      return 0;
    }

     

     

     

     

  • 相关阅读:
    HDOJ 1846 Brave Game
    并查集模板
    HDU 2102 A计划
    POJ 1426 Find The Multiple
    POJ 3278 Catch That Cow
    POJ 1321 棋盘问题
    CF 999 C.Alphabetic Removals
    CF 999 B. Reversing Encryption
    string的基础用法
    51nod 1267 4个数和为0
  • 原文地址:https://www.cnblogs.com/long5683/p/11983181.html
Copyright © 2011-2022 走看看