GN_MLE

引言

ch6.

1.GNpractice

注意未知数是abc.GN分析：

code：

#include <iostream>
#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 = 1.0;                        // 噪声Sigma值
    cv::RNG rng;                                 // OpenCV随机数产生器

    vector<double> x_data, y_data;      // 数据
    for (int i = 0; i < N; i++) {
        double x = i / 100.0; // scale to 0--1
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma)); // 产生測量值 y_data
    }

    // 开始Gauss-Newton迭代
    int iterations = 100;    // 迭代次数
    double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

    for (int iter = 0; iter < iterations; iter++) {

        Matrix3d H = Matrix3d::Zero();             // Hessian = J^T * J in Gauss-Newton
        Vector3d b = Vector3d::Zero();             // bias
        cost = 0;

        for (int i = 0; i < N; i++) {
            double xi = x_data[i], yi = y_data[i];  // 第i个数据点
            // start your code here
            double error = 0;   // 第i个数据点的计算误差
            error =  yi - exp(ae*xi*xi + be*xi + ce) ; // 填写计算error的表达式
            Vector3d J; // 雅可比矩阵, 是向量. [de/da, de/db, de/dc]
            J[0] = -exp(ae*xi*xi + be*xi + ce)*xi*xi;  // de/da
            J[1] = -exp(ae*xi*xi + be*xi + ce)*xi;  // de/db
            J[2] = -exp(ae*xi*xi + be*xi + ce);  // de/dc

            H += J * J.transpose(); // GN近似的H
            b += -error * J;  // See book P. 112 , eqt(6.21)
            // end your code here

            cost += error * error;
        }

        // 求解线性方程 Hx=-b，建议用ldlt
 	// start your code here
        Vector3d dx;
        LLT<MatrixXd> llt;
        llt.compute(H);
        dx = llt.solve(b); // increment dx
        //或者使用
        //dx = H.ldlt().solve(b);
        
	// end your code here

        if (std::isnan(dx[0])){
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost > lastCost) {
            // 误差增长了，说明近似的不够好
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // 更新abc估计值
        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastCost = cost;

        cout << "total cost: " << cost << endl;
    }

    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    return 0;
}

2.MaximumLikelihoodEstimation

最大似然估计：假设一个袋子里面有两种装球方案：

• 99个白球一个红球
• 99个红球一个白球

从袋子里面随机拿出一个一个球为白色，问装球方案是哪一种？（这个例子是五一前夕高工课上玩手机时听见老师讲的，很贴切！）最大似然就是解决类似问题。SLAM中理解为在什么样的状态下，最可能发生现在观测到的数据.

• 参考链接1
• 参考链接2

非线性优化，后端。