Eigen实现非线性最小二乘拟合 + Gauss-Newton算法

下面是使用 Eigen 实现的 非线性最小二乘拟合 + Gauss-Newton 算法 的完整示例，拟合模型为：

---

拟合目标模型：

$$y=\exp (a{x}^2+bx+c)$$

已知一组带噪声数据点 $(x_i,y_i)$，使用 高斯-牛顿法 求最优参数 $(a,b,c)$。

---

所需库

• Eigen：矩阵运算
• cmath：指数函数

---

高斯-牛顿迭代步骤（简要）

1. 初始猜测参数 $(a,b,c)$

2. 对每个点计算残差：

   $$r_i=y_i-\exp (a{x}_i^2+b{x}_i+c)$$

3. 构造雅可比矩阵 $J_i$（每个点对参数的偏导数）：

   $$J_i=\left[-x_i^2\cdot \exp (f),-x_i\cdot \exp (f),-\exp (f)\right]$$

   其中 $f=a{x}_i^2+b{x}_i+c$

4. 累加构建 $H=J^{T}J$ 和 $b=-J^{T}r$

5. 解线性系统 $H\cdot \Delta x=b$

6. 更新参数 $\theta \leftarrow \theta +\Delta x$，判断是否收敛

---

示例代码：非线性拟合 + 高斯牛顿

#include <iostream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>using namespace std;
using namespace Eigen;// 生成数据
void generateData(vector<double>& x_data, vector<double>& y_data, double a, double b, double c, int N = 100) {x_data.reserve(N);y_data.reserve(N);for (int i = 0; i < N; ++i) {double x = i / 100.0;double y = exp(a * x * x + b * x + c) + 0.05 * ((rand() % 100) / 100.0 - 0.5);  // 加噪声x_data.push_back(x);y_data.push_back(y);}
}int main() {// 模拟数据double true_a = 1.0, true_b = 2.0, true_c = 1.0;vector<double> x_data, y_data;generateData(x_data, y_data, true_a, true_b, true_c);// 初始估计值double a = 0.0, b = 0.0, c = 0.0;const int iterations = 100;double lastCost = 0;for (int iter = 0; iter < iterations; ++iter) {Matrix3d H = Matrix3d::Zero();    // 海森矩阵 H = J^T * JVector3d g = Vector3d::Zero();    // 梯度向量 g = -J^T * rdouble cost = 0;for (size_t i = 0; i < x_data.size(); ++i) {double x = x_data[i], y = y_data[i];double fx = exp(a * x * x + b * x + c);double error = y - fx;cost += error * error;// 构造雅可比矩阵 J_iVector3d J;J[0] = -x * x * fx;  // ∂f/∂aJ[1] = -x * fx;      // ∂f/∂bJ[2] = -fx;          // ∂f/∂cH += J * J.transpose();   // 累加 J^T * Jg += -error * J;          // 累加 -J^T * r}// 求解 H Δx = gVector3d dx = H.ldlt().solve(g);if (isnan(dx[0])) {cout << "Update is NaN, stopping." << endl;break;}if (iter > 0 && cost >= lastCost) {cout << "Cost increased, stopping." << endl;break;}a += dx[0];b += dx[1];c += dx[2];lastCost = cost;cout << "Iteration " << iter << ": cost = " << cost << ", update = " << dx.transpose() << ", parameters = "<< a << " " << b << " " << c << endl;}cout << "\nFinal result: a = " << a << ", b = " << b << ", c = " << c << endl;return 0;
}

---

输出结果（收敛示意）：

Iteration 0: cost = 3.94, update = 0.57 1.85 0.95, parameters = 0.57 1.85 0.95
...
Iteration 9: cost = 0.00017, update = 1.2e-6 1.7e-6 9.1e-7, parameters = 0.9999 2.0001 1.0000Final result: a = 0.9999, b = 2.0001, c = 1.0000

---

小结

• 高斯牛顿适合残差函数是非线性的情形（比如指数、多项式等）
• 可替换为 Levenberg-Marquardt 算法处理奇异或非收敛情况
• 更复杂系统可迁移至 Ceres Solver / GTSAM

---