参考网址:

slambook2/ch6 at master · gaoxiang12/slambook2 (github.com)

步骤

  • 给定初值x0.
  • 对于k次迭代,求出当前雅可比矩阵J(xk)和误差发(xk).
  • 求解增量方程:Hdxk=g.
  • 若dxk足够小则停止.否则,令xk+1 = xk +dx,返回第2步.

高翔博士的

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
#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 = 1.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); // 在x_data最后边存放x值
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma)); //产生一个高斯噪声
  }

  // 开始Gauss-Newton迭代
  int iterations = 100;    // 迭代次数
  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^T W^{-1} 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个数据点
      // 对误差项error做最小二乘
      double error = yi - exp(ae * xi * xi + be * xi + ce);
      Vector3d J; // 雅可比矩阵(Jacobian可以通过matlab求解)
      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();
      b += -inv_sigma * inv_sigma * 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 << ", \t\tupdate: " << dx.transpose() <<
         "\t\testimated 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;
}

用matlab实现一次

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
clear all; close all; clc;

% 生成一组符合高斯噪声的,exp(1*X^2+2*x+1)的随机数
ar=1;
br=2;
cr=1;
n = 100;    % 样本数
w_sigma = 1;% 噪声值
inv_sigma = 1/w_sigma;
for i = 1:n
    x(i) = i/n;
    y(i) = exp(ar*x(i)^2+br*x(i)+cr)+normrnd(0,w_sigma);
end


% 设置x0初值
ae = 2.0;be=-1.0;ce=5.0;
cost = 0;lastcost=0;
iterations = 100;% 迭代次数
for iter = 1:iterations
    H = zeros(3,3);
    g = zeros(3,1);
    J = zeros(3,1);
    cost = 0;
    for j = 1:n
        error = y(j) - exp(ae*x(j)^2+be*x(j)+ce);

        J(1) = -x(j) * x(j) * exp(ae * x(j) * x(j) + be * x(j) + ce); % ae
        J(2) = -x(j) * exp(ae * x(j) * x(j) + be * x(j) + ce); % be
        J(3) = -exp(ae * x(j) * x(j) + be * x(j) + ce); % ce

        H = H+inv_sigma^2*J*J';
        g = g-inv_sigma^2*J*error;

        cost = cost + error^2;
    end
    % 求解Hx=g
    deltax = inv(H)*g;
    

    if iter>1&&abs(cost-lastcost)<0.001
        disp('cost:'+cost);
        break;
    end

    ae = ae+deltax(1);
    be = be+deltax(2);
    ce = ce+deltax(3);

    lastcost = cost;
    disp(['iter: ',num2str(iter),' ', num2str(ae),' ',num2str(be),' ',num2str(ce)]);
        
end
f = exp(ae*x.^2+be*x+ce);
plot(x,y,'bo',x,f,'r');