手写高斯牛顿求解非线性最小二乘问题

leeayu

于 2024-08-04 00:33:36 发布

阅读量289

点赞数 2

分类专栏：状态估计文章标签：高斯牛顿优化算法曲线拟合

本文链接：https://blog.youkuaiyun.com/qq_23225073/article/details/140899490

版权

在这里插入图片描述

容易写错的地方：

注意你的residual定义是 z-h(x)，还是 h(x) - z，这会影响到Hx=b的符号。（自己推一遍就知道了），我的习惯性定义是z-h(x)

class GaussianNewtonOptimizer {
   
  // Observation: [x, y]
  // y = std::exp(a*x^2 + b*x + c)
  // state: [a, b, c]
  // J = std::exp(a*x^2 + b*x + c) * [x^2, x, 1]
  // residual = y - f(x; a, b, c)

 public:
  void Initialize(const Eigen::Vector3d &state, const Eigen::Vector3d &state_gt,
                  int max_iteration) {
   
    state_ = state;
    state_gt_ = state_gt;
    max_iteration_ = max_iteration;

  double MeasurementModelFunc(double observation) {
   
    return std::exp(state_[0] * observation * observation +
                    state_[1] * observation + state_[2]);
  }

  Eige