C++代码实现MATLAB中的lsqnonlin函数功能

// =============================================================
// lsqnonlin.cpp
// MATLAB 默认算法 trust-region-reflective 的独立实现
// 编译: g++ -O2 -std=c++17 lsqnonlin_trr.cpp -o lsqtrr
// =============================================================
#include <cstdio>
#include <cmath>
#include <vector>
#include <functional>
#include <algorithm>
#include <limits>

using Vec = std::vector<double>;

struct Matrix {
int rows = 0, cols = 0;
std::vector<double> a;
Matrix() = default;
Matrix(int r, int c) : rows(r), cols(c), a((size_t)r*c, 0.0) {}
double& operator()(int i, int j) { return a[(size_t)i*cols + j]; }
double operator()(int i, int j) const { return a[(size_t)i*cols + j]; }
};

static double dot(const Vec& a, const Vec& b) {
double s = 0.0; for (size_t i = 0; i < a.size(); ++i) s += a[i]*b[i]; return s;
}
static double nrm2(const Vec& a) { return dot(a, a); }
static double norm(const Vec& a) { return std::sqrt(dot(a, a)); }

// ———- LDLT 求解对称系统 A x = b ———-
static bool ldltSolve(const Matrix& A, const Vec& b, Vec& x) {
const int n = A.rows;
Matrix L(n, n);
Vec D(n);
for (int j = 0; j < n; ++j) {
double d = A(j, j);
for (int k = 0; k < j; ++k) d -= L(j, k)*L(j, k)*D[k];
if (!std::isfinite(d) || std::abs(d) < 1e-300) return false;
D[j] = d; L(j, j) = 1.0;
for (int i = j+1; i < n; ++i) {
double s = A(i, j);
for (int k = 0; k < j; ++k) s -= L(i, k)*L(j, k)*D[k];
L(i, j) = s / D[j];
if (!std::isfinite(L(i, j))) return false;
}
}
Vec y(n);
for (int i = 0; i < n; ++i) {
double s = b[i];
for (int k = 0; k < i; ++k) s -= L(i, k)*y[k];
y[i] = s;
}
Vec z(n);
for (int i = 0; i < n; ++i) z[i] = y[i]/D[i];
x.assign(n, 0.0);
for (int i = n–1; i >= 0; —i) {
double s = z[i];
for (int k = i+1; k < n; ++k) s -= L(k, i)*x[k];
x[i] = s;
}
for (double v : x) if (!std::isfinite(v)) return false;
return true;
}

// ———- 计算 JtJ 与 Jtf ———-
static void jtj_jtf(const Matrix& J, const Vec& f, Matrix& JtJ, Vec& Jtf) {
int m = J.rows, n = J.cols;
JtJ = Matrix(n, n);
Jtf.assign(n, 0.0);
for (int i = 0; i < n; ++i) {
for (int k = 0; k < m; ++k) Jtf[i] += J(k, i)*f[k];
for (int j = i; j < n; ++j) {
double s = 0.0;
for (int k = 0; k < m; ++k) s += J(k, i)*J(k, j);
JtJ(i, j) = JtJ(j, i) = s;
}
}
}

// ———- 高斯-牛顿步 ———-
static bool gaussNewtonStep(const Matrix& J, const Vec& f, Vec& s) {
Matrix JtJ; Vec Jtf;
jtj_jtf(J, f, JtJ, Jtf);
for (int i = 0; i < JtJ.rows; ++i) JtJ(i, i) += 1e-14;
Vec rhs(JtJ.rows);
for (int i = 0; i < JtJ.rows; ++i) rhs[i] = –Jtf[i];
return ldltSolve(JtJ, rhs, s);
}

// ———- Dogleg 子问题：在 ‖s‖ ≤ Δ 内 min ‖J s + f‖ ———-
static Vec solveTrustRegion(const Matrix& J, const Vec& f, double Delta) {
const int m = J.rows, n = J.cols;
Vec s_gn;
bool okGN = gaussNewtonStep(J, f, s_gn);
if (okGN && norm(s_gn) <= Delta) return s_gn; // GN 步在域内

// Cauchy 点
Vec g(n, 0.0);
for (int i = 0; i < n; ++i)
for (int k = 0; k < m; ++k) g[i] += J(k, i)*f[k];
double gg = dot(g, g);
if (gg < 1e-300) return Vec(n, 0.0);

Vec Jg(m, 0.0);
for (int k = 0; k < m; ++k)
for (int i = 0; i < n; ++i) Jg[k] += J(k, i)*g[i];
double JgJg = dot(Jg, Jg);
if (JgJg < 1e-300) return Vec(n, 0.0);

double alpha = gg / JgJg;
Vec s_sd(n);
for (int i = 0; i < n; ++i) s_sd[i] = –alpha * g[i];

double nsd = norm(s_sd);
if (nsd >= Delta || !okGN) { // SD 步出界（或 GN 不可用）→ 缩放到边界
double sc = Delta / nsd;
Vec s(n);
for (int i = 0; i < n; ++i) s[i] = sc * s_sd[i];
return s;
}

// Dogleg 插值：τ 使 ‖s_sd + τ(s_gn – s_sd)‖ = Δ
Vec d(n);
for (int i = 0; i < n; ++i) d[i] = s_gn[i] – s_sd[i];
double a2 = dot(d, d);
double b2 = 2.0 * dot(s_sd, d);
double c2 = dot(s_sd, s_sd) – Delta*Delta;
double disc = std::max(0.0, b2*b2 – 4*a2*c2);
double tau = (a2 > 0) ? (–b2 + std::sqrt(disc)) / (2*a2) : 1.0;
tau = std::max(0.0, std::min(1.0, tau));
Vec s(n);
for (int i = 0; i < n; ++i) s[i] = s_sd[i] + tau * d[i];
return s;
}

// ==================== lsqnonlin (TRR) ====================
class Lsqnonlin {
public:
using FunType = std::function<Vec(const Vec&)>;
using JacType = std::function<Matrix(const Vec&)>;

struct Options {
int maxIter = 400;
double stepTol = 1e-8;
double funcTol = 1e-12;
double optimalityTol = 1e-8;
double Delta0 = 1.0; // 初始信赖域半径
bool verbose = false;
};

struct Result {
Vec x, residual;
double cost = 0.0;
int iterations = 0;
int exitFlag = 0; // 1:最优性收敛 2:步长收敛 3:信赖域过小 0:最大迭代
bool success() const { return exitFlag > 0; }
};

explicit Lsqnonlin(FunType fun) : fun_(std::move(fun)) {}
Lsqnonlin(FunType fun, JacType jac) : fun_(std::move(fun)), jac_(std::move(jac)) {}

Result solve(const Vec& x0) { return solve(x0, Options()); }
Result solve(const Vec& x0, const Options& opts) {
const int n = (int)x0.size();
Result res;
Vec x = x0;

Vec f = fun_(x);
double cost = 0.5 * nrm2(f);
Matrix J = computeJacobian(x, f);

double Delta = opts.Delta0;
const double eta = 0.15; // 步长接受阈值

int iter = 0;
for (iter = 0; iter < opts.maxIter; ++iter) {
// 一阶最优性（无穷范数 ‖Jᵀf‖∞）
Vec g(n, 0.0);
for (int i = 0; i < n; ++i)
for (int k = 0; k < (int)f.size(); ++k)
g[i] += J(k, i)*f[k];
double gInf = 0.0;
for (double v : g) gInf = std::max(gInf, std::abs(v));

if (gInf < opts.optimalityTol) { res.exitFlag = 1; break; }
if (Delta < opts.stepTol) { res.exitFlag = 3; break; }

// 子问题
Vec s = solveTrustRegion(J, f, Delta);

// 预测下降
Vec Jsf(f.size(), 0.0);
for (int k = 0; k < (int)f.size(); ++k) {
double v = f[k];
for (int i = 0; i < n; ++i) v += J(k, i)*s[i];
Jsf[k] = v;
}
double predRed = 0.5 * (nrm2(f) – nrm2(Jsf));

// 试验点
Vec xNew(n);
for (int i = 0; i < n; ++i) xNew[i] = x[i] + s[i];
Vec fNew = fun_(xNew);
double costNew = 0.5 * nrm2(fNew);

double actRed = cost – costNew;
double rho = (predRed > 0) ? actRed / predRed : –1.0;

if (opts.verbose) {
std::printf(" Iter %3d: cost=%.10e rho=%+.4e Delta=%.4e |g|=%.4e\\n",
iter, cost, rho, Delta, gInf);
}

if (rho > eta) {
double stepNorm = norm(s);
double costRel = std::abs(cost – costNew) / std::max(1.0, cost);

x = xNew; f = fNew; cost = costNew;
J = computeJacobian(x, f);

if (rho > 0.75) Delta = std::min(2.0*Delta, 1e10);
else if (rho < 0.25) Delta *= 0.5;

if (stepNorm < opts.stepTol * (norm(x) + opts.stepTol)) {
res.exitFlag = 2; break;
}
if (costRel < opts.funcTol) { res.exitFlag = 1; break; }
} else {
Delta *= 0.5;
if (Delta < opts.stepTol) { res.exitFlag = 3; break; }
}
}

res.x = x; res.residual = f; res.cost = cost; res.iterations = iter;
return res;
}

private:
Matrix computeJacobian(const Vec& x, const Vec& f) const {
return jac_ ? jac_(x) : numericalJacobian(x, f);
}
Matrix numericalJacobian(const Vec& x, const Vec& f0) const {
int n = (int)x.size(), m = (int)f0.size();
Matrix J(m, n);
double sqEps = std::sqrt(std::numeric_limits<double>::epsilon());
for (int j = 0; j < n; ++j) {
double h = sqEps * std::max(1.0, std::abs(x[j]));
Vec xp = x; xp[j] += h;
Vec fp = fun_(xp);
for (int i = 0; i < m; ++i) J(i, j) = (fp[i] – f0[i]) / h;
}
return J;
}
FunType fun_;
JacType jac_;
};

// ==================== 测试主程序 ====================
static void printVec(const char* name, const Vec& v) {
std::printf("%s = [", name);
for (size_t i = 0; i < v.size(); ++i)
std::printf("%s%.12g", i ? ", " : "", v[i]);
std::printf("]\\n");
}

int main() {
std::printf("=========================================================\\n");
std::printf(" MATLAB lsqnonlin 默认算法: trust-region-reflective\\n");
std::printf("=========================================================\\n\\n");

// ———- 用例 1: Rosenbrock 最小二乘 ———-
{
std::printf("———- 用例 1: Rosenbrock 最小二乘 ———-\\n");
std::printf(" r1 = 10*(x2 – x1^2)\\n");
std::printf(" r2 = 1 – x1\\n");
std::printf(" x0 = [-1.2, 1.0]，期望最优 x* = [1, 1]\\n\\n");

auto fun = [](const Vec& x) -> Vec {
return { 10.0*(x[1] – x[0]*x[0]), 1.0 – x[0] };
};
auto jac = [](const Vec& x) -> Matrix {
Matrix J(2, 2);
J(0,0) = –20.0*x[0]; J(0,1) = 10.0;
J(1,0) = –1.0; J(1,1) = 0.0;
return J;
};

Lsqnonlin::Options opts;
opts.verbose = true;

Lsqnonlin solver(fun, jac);
Vec x0 = { –1.2, 1.0 };
auto res = solver.solve(x0, opts);

std::printf("\\n ———- 求解结束 ———-\\n");
printVec(" x", res.x);
printVec(" residual", res.residual);
std::printf(" cost = %.6e\\n", res.cost);
std::printf(" iterations = %d\\n", res.iterations);
std::printf(" exitFlag = %d\\n\\n", res.exitFlag);
}

// ———- 用例 2: 指数拟合（数值雅可比）———-
{
std::printf("———- 用例 2: 指数拟合 y = a*exp(b*t) ———-\\n");
const std::vector<double> t = {0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0};
const std::vector<double> y = {2.60, 1.80, 1.30, 0.91, 0.63, 0.44, 0.31};
std::printf(" 真值 a≈2.5, b≈-0.7，初值 p0 = [1.0, 0.0]\\n\\n");

auto fun = [&](const Vec& p) -> Vec {
Vec r(t.size());
for (size_t i = 0; i < t.size(); ++i)
r[i] = p[0]*std::exp(p[1]*t[i]) – y[i];
return r;
};

Lsqnonlin::Options opts;
opts.verbose = true;
Lsqnonlin solver(fun); // 使用数值雅可比
Vec p0 = { 1.0, 0.0 };
auto res = solver.solve(p0, opts);

std::printf("\\n ———- 求解结束 ———-\\n");
printVec(" p (a, b)", res.x);
std::printf(" cost = %.6e\\n", res.cost);
std::printf(" iterations = %d\\n", res.iterations);
std::printf(" exitFlag = %d\\n\\n", res.exitFlag);
}
return 0;
}