ph: the learned SE(2) port-Hamiltonian vehicle, trained natively (cvc::ph)

docs/roadmap/NAV-VEHICLE-PHYSICS-ROADMAP.md

@@ -185,6 +185,79 @@ P4 adds three things to measure and contain the drift:
 2. **A per-tick drift residual,** binned by lateral g and published next to the energy residual.
 3. **Lag-aware governors in the Jolt arm only.**
 
+### The dynamic model's residual (P8a against Jolt)
+
+`cvc::physics::one_tick_residuals` (`cvc/physics/plant_log.h`) measures the drift the same way for
+both models. Each tick, it steps P8a's `diff::dyn_plant` and the kinematic bicycle from Jolt's own
+state, with the command nav held over the tick. The models use the tick's substeps (`nsub = k`) and
+are compared with where Jolt was at the tick's end. The bins are the lateral acceleration Jolt
+measured over the tick.
+
+Ticks are left out when they have a static contact, a wheel off the ground, an impact (above
+1.5 g), or the vehicle at rest. `cvc plant-log` runs it and prints the bins.
+
+Below: nav's 0.06 s tick (6 substeps), 4 seeds, 60 s each, mu 0.9, measured 2026-10-20. Each cell
+is the position rms in mm, three ways:
+
+- the dynamic model with the spec-sheet values it had before the fit (#753);
+- the dynamic model with `dyn_bike_si_of`'s values now, fitted to Jolt;
+- the kinematic bicycle.
+
+The RoboRacer's city runs plan with 0.4 of its acceleration and braking: at its full 6 m/s^2 its
+chassis box pitches onto the ground.
+
+| lateral g | Humvee slalom | Humvee city | RoboRacer slalom | RoboRacer city |
+|---|---|---|---|---|
+| 0.0-0.1 | 2.0 / 1.2 / 3.9 | 2.0 / 1.6 / 3.4 | 0.9 / 0.3 / 0.7 | 1.9 / 1.3 / 1.9 |
+| 0.1-0.2 | 5.1 / 1.4 / 11.6 | 4.8 / 1.6 / 11.1 | 3.3 / 0.4 / 5.6 | 3.4 / 0.9 / 5.2 |
+| 0.2-0.3 | 7.1 / 1.3 / 17.9 | 7.0 / 2.0 / 18.1 | 5.0 / 0.6 / 10.7 | 4.8 / 1.0 / 8.6 |
+| 0.3-0.5 | 8.1 / 1.2 / 27.5 | 8.1 / 2.8 / 26.2 | 6.9 / 0.8 / 15.8 | 6.5 / 1.8 / 11.7 |
+| >= 0.5 | 5.2 / 4.2 / 94.0 | 4.5 / 3.8 / 52.0 | 6.5 / 0.7 / 25.1 | 12.6 / 11.5 / 30.4 |
+
+- **Position and heading:** the dynamic model is closer than the kinematic bicycle wherever the tires
+  carry load. Below 0.5 g the fitted values bring it 1.3-9x closer again.
+- **Speed:** below 0.5 g the RoboRacer's speed residual now matches the kinematic bicycle's,
+  0.001-0.035 m/s rms (it was 0.018-0.090). Two spec-sheet values had been off:
+  - its 0.1 s drive lag;
+  - the drag and rolling resistance it subtracted. The RoboRacer's driver model tracks the commanded
+    acceleration with an integral trim, so it makes up for them.
+
+  The Humvee's speed residual is unchanged, and below the kinematic bicycle's.
+- **What is left:** the RoboRacer in the city above 0.5 g (11.5 mm), and its yaw rate there,
+  0.04-0.34 rad/s rms.
+
+**Fitted to Jolt** (`cvc::physics::fit_dyn_bike`, `cvc plant-log --fit-dyn`): a coordinate descent on
+the one-tick twist residual over the knobs a spec sheet does not give. The knobs are the drive and
+steering lags, factors on the tires' B, the yaw inertia, and drag + rolling resistance. Each fit uses
+seeds 100-102 and is judged on seed 103.
+
+The first fits ran against the spec sheet: tire B x0.25-0.40, and for the RoboRacer no lag and no
+resistance. #753 adopted them as `dyn_bike_si_of`'s values. Refitted against those values:
+
+| run (50 Hz) | drive lag | tire B front / rear | yaw inertia | held-out twist residual, (m/s)^2: default -> fit | before #753 |
+|---|---|---|---|---|---|
+| Humvee slalom | 0.20 s | x1.45 / x1.20 | x1.00 | 0.00046 -> 0.00049 | 0.00287 -> 0.00045 |
+| Humvee city | 0.04 s | x1.00 / x1.00 | x1.00 | 0.00065 -> 0.00061 | 0.00280 -> 0.00060 |
+| RoboRacer slalom | 0 | x1.00 / x1.00 | x0.85 | 0.00001 -> 0.00001 | 0.00035 -> 0.00001 |
+| RoboRacer city | 0 | x1.00 / x1.20 | x1.75 | 0.00015 -> 0.00018 | 0.00229 -> 0.00018 |
+
+- **Nothing left to fit on flat ground:** on the held-out seed the defaults do as well as a fit. A
+  refit gains at most 6% (Humvee city) and loses up to 20% (RoboRacer city). The tire factors come back
+  at x1, except the Humvee slalom's, whose fit does worse held out. No steering lag is found, and drag
+  and rolling stay as they are.
+- **Weakly identified:** the Humvee's drive lag (0.04-0.20 s) and either vehicle's yaw inertia move
+  between scenes.
+- **The city-bundle fits (Austin) are not trustworthy:** they land on the grids' edges (B x0.3-0.7, yaw
+  inertia x1.75-2.1), absorbing what the planar model leaves out on rough terrain.
+- **Held out, at nav's 0.06 s tick:** position rms in mm, as fit / default now / spec sheet before #753
+  / kinematic:
+  - Humvee slalom, above 0.5 g: 2.7 / 3.4 / 6.4 / 55.2;
+  - RoboRacer city, 0.3-0.5 g: 1.1 / 1.1 / 6.1 / 11.2.
+- **The LieSPHGP datasets do not move:** Jolt drives the vehicles, and `dyn_bike_si_of` enters only
+  the residuals. The rerun's flights and pickles are byte-identical.
+
+These are the targets a plant fitted to Jolt has to beat, bin by bin.
+
 ## Status
 
 - **P0** is merged; see the clock roadmap.

inc/cvc/ph/mat3.h

@@ -0,0 +1,158 @@
+/*
+  Copyright 2026 The University of Texas at Austin
+
+  This file is part of libcvc.
+
+  libcvc is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License version 2.1 as published by the Free Software Foundation.
+*/
+
+// cvc::ph's small linear algebra: 3x3 matrices (row-major std::array<T, 9>), the SO(2) log, and
+// the positive-definite parameterization the port-Hamiltonian models use for M^-1 and D:
+//
+//   A = L L^T,   L = [[e^l0, 0, 0], [l3, e^l1, 0], [l4, l5, e^l2]]   (six levels, any real values)
+//
+// with its reverse: for a scalar loss with dA = dLoss/dA (any 3x3, not necessarily symmetric),
+// dL = (dA + dA^T) L, and the levels get dL's lower triangle, the diagonal times e^l.
+//
+// Templated on the scalar (float or double). solve3 is Gaussian elimination with partial pivoting.
+
+#ifndef __CVC_PH_MAT3_H__
+#define __CVC_PH_MAT3_H__
+
+#include <array>
+#include <cmath>
+#include <utility>
+
+namespace cvc {
+namespace ph {
+
+template <class T> using vec3 = std::array<T, 3>;
+template <class T> using mat3 = std::array<T, 9>;
+
+template <class T> mat3<T> identity3() {
+  return {T(1), T(0), T(0), T(0), T(1), T(0), T(0), T(0), T(1)};
+}
+
+template <class T> mat3<T> mul(const mat3<T> &a, const mat3<T> &b) {
+  mat3<T> c{};
+  for (int i = 0; i < 3; ++i)
+    for (int j = 0; j < 3; ++j)
+      c[3 * i + j] = a[3 * i] * b[j] + a[3 * i + 1] * b[3 + j] + a[3 * i + 2] * b[6 + j];
+  return c;
+}
+
+template <class T> mat3<T> transpose(const mat3<T> &a) {
+  return {a[0], a[3], a[6], a[1], a[4], a[7], a[2], a[5], a[8]};
+}
+
+// A x
+template <class T> vec3<T> matvec(const mat3<T> &a, const vec3<T> &x) {
+  return {a[0] * x[0] + a[1] * x[1] + a[2] * x[2], a[3] * x[0] + a[4] * x[1] + a[5] * x[2],
+          a[6] * x[0] + a[7] * x[1] + a[8] * x[2]};
+}
+
+// A^T x
+template <class T> vec3<T> tmatvec(const mat3<T> &a, const vec3<T> &x) {
+  return {a[0] * x[0] + a[3] * x[1] + a[6] * x[2], a[1] * x[0] + a[4] * x[1] + a[7] * x[2],
+          a[2] * x[0] + a[5] * x[1] + a[8] * x[2]};
+}
+
+// acc += s x y^T
+template <class T> void add_outer(mat3<T> &acc, const vec3<T> &x, const vec3<T> &y, T s = T(1)) {
+  for (int i = 0; i < 3; ++i)
+    for (int j = 0; j < 3; ++j)
+      acc[3 * i + j] += s * x[i] * y[j];
+}
+
+// x = A^-1 b by Gaussian elimination with partial pivoting.
+template <class T> vec3<T> solve3(mat3<T> a, vec3<T> b) {
+  for (int col = 0; col < 3; ++col) {
+    int pivot = col;
+    for (int r = col + 1; r < 3; ++r)
+      if (std::abs(a[3 * r + col]) > std::abs(a[3 * pivot + col]))
+        pivot = r;
+    if (pivot != col) {
+      for (int j = 0; j < 3; ++j)
+        std::swap(a[3 * col + j], a[3 * pivot + j]);
+      std::swap(b[col], b[pivot]);
+    }
+    for (int r = col + 1; r < 3; ++r) {
+      const T f = a[3 * r + col] / a[3 * col + col];
+      for (int j = col; j < 3; ++j)
+        a[3 * r + j] -= f * a[3 * col + j];
+      b[r] -= f * b[col];
+    }
+  }
+  vec3<T> x{};
+  for (int r = 2; r >= 0; --r) {
+    T s = b[r];
+    for (int j = r + 1; j < 3; ++j)
+      s -= a[3 * r + j] * x[j];
+    x[r] = s / a[3 * r + r];
+  }
+  return x;
+}
+
+// x = A^-T b
+template <class T> vec3<T> solve3_transposed(const mat3<T> &a, const vec3<T> &b) {
+  return solve3(transpose(a), b);
+}
+
+// The lower-triangular factor of the six levels.
+template <class T> mat3<T> spd_factor(const T *l) {
+  using std::exp;
+  return {exp(l[0]), T(0), T(0), l[3], exp(l[1]), T(0), l[4], l[5], exp(l[2])};
+}
+
+// A = L L^T.
+template <class T> mat3<T> spd_from_levels(const T *l) {
+  const mat3<T> f = spd_factor(l);
+  return mul(f, transpose(f));
+}
+
+// dl += the levels' gradient for dA = dLoss/dA.
+template <class T> void spd_from_levels_vjp(const T *l, const mat3<T> &dA, T *dl) {
+  const mat3<T> f = spd_factor(l);
+  mat3<T> sym{};
+  for (int k = 0; k < 9; ++k)
+    sym[k] = dA[k];
+  const mat3<T> t = transpose(dA);
+  for (int k = 0; k < 9; ++k)
+    sym[k] += t[k];
+  const mat3<T> dL = mul(sym, f);
+  dl[0] += dL[0] * f[0];
+  dl[1] += dL[4] * f[4];
+  dl[2] += dL[8] * f[8];
+  dl[3] += dL[3];
+  dl[4] += dL[6];
+  dl[5] += dL[7];
+}
+
+// The six levels of a symmetric positive-definite A (its Cholesky factor): the inverse of
+// spd_from_levels.
+template <class T> std::array<T, 6> spd_levels_of(const mat3<T> &a) {
+  using std::log;
+  using std::sqrt;
+  const T l00 = sqrt(a[0]);
+  const T l10 = a[3] / l00, l20 = a[6] / l00;
+  const T l11 = sqrt(a[4] - l10 * l10);
+  const T l21 = (a[7] - l20 * l10) / l11;
+  const T l22 = sqrt(a[8] - l20 * l20 - l21 * l21);
+  return {log(l00), log(l11), log(l22), l10, l20, l21};
+}
+
+// Log on SO(2): the angle in (-pi, pi]. Its derivative is 1 wherever it is used (away from the
+// cut), so a reverse pass passes gradients straight through it.
+template <class T> T wrap_angle(T a) {
+  using std::atan2;
+  using std::cos;
+  using std::sin;
+  return atan2(sin(a), cos(a));
+}
+
+} // namespace ph
+} // namespace cvc
+
+#endif // __CVC_PH_MAT3_H__

inc/cvc/ph/optim.h

@@ -0,0 +1,127 @@
+/*
+  Copyright 2026 The University of Texas at Austin
+
+  This file is part of libcvc.
+
+  libcvc is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License version 2.1 as published by the Free Software Foundation.
+*/
+
+// cvc::ph's optimizers, written to match optax 0.2.8 step for step (the JAX trainer the
+// port-Hamiltonian models were first fit with): a cosine decay, global-norm clipping, Adam, and
+// Prodigy (Mishchenko and Defazio, "Prodigy: an expeditiously adaptive parameter-free learner",
+// 2023; optax.contrib.prodigy). ph_optim_test holds them to optax's own numbers.
+//
+// Each works on a flat span of doubles. `count` is the number of updates already applied, as
+// optax's schedules read it (0 on the first update).
+
+#ifndef __CVC_PH_OPTIM_H__
+#define __CVC_PH_OPTIM_H__
+
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+#include <vector>
+
+namespace cvc {
+namespace ph {
+
+// optax.cosine_decay_schedule(init, decay_steps, alpha) at `count`.
+inline double cosine_decay(double init, int decay_steps, double alpha, int count) {
+  const double t = static_cast<double>(std::min(count, decay_steps));
+  const double cosine = 0.5 * (1.0 + std::cos(3.14159265358979323846 * t / decay_steps));
+  return init * ((1.0 - alpha) * cosine + alpha);
+}
+
+// optax.clip_by_global_norm: scales g to norm `max_norm` when its norm is not below it. Returns
+// the norm before clipping.
+inline double clip_by_global_norm(double *g, std::size_t n, double max_norm) {
+  double sq = 0.0;
+  for (std::size_t i = 0; i < n; ++i)
+    sq += g[i] * g[i];
+  const double norm = std::sqrt(sq);
+  if (!(norm < max_norm))
+    for (std::size_t i = 0; i < n; ++i)
+      g[i] = g[i] / norm * max_norm;
+  return norm;
+}
+
+// optax.adam (eps outside the root, eps_root 0).
+struct adam {
+  double beta1 = 0.9, beta2 = 0.999, eps = 1e-8;
+  std::vector<double> m, v;
+  int count = 0;
+
+  void init(std::size_t n) {
+    m.assign(n, 0.0);
+    v.assign(n, 0.0);
+    count = 0;
+  }
+  // p -= lr * m_hat / (sqrt(v_hat) + eps); lr is the schedule at the count before this update.
+  void update(double *p, const double *g, std::size_t n, double lr) {
+    const int t = count + 1;
+    const double c1 = 1.0 - std::pow(beta1, t), c2 = 1.0 - std::pow(beta2, t);
+    for (std::size_t i = 0; i < n; ++i) {
+      m[i] = beta1 * m[i] + (1.0 - beta1) * g[i];
+      v[i] = beta2 * v[i] + (1.0 - beta2) * g[i] * g[i];
+      p[i] -= lr * (m[i] / c1) / (std::sqrt(v[i] / c2) + eps);
+    }
+    count = t;
+  }
+};
+
+// optax.contrib.prodigy, weight decay 0: Adam whose step size d (the distance-to-solution
+// estimate) is learned. One instance per parameter group, as the JAX trainer has one per level.
+//
+// One deliberate difference: when a group's gradients have been exactly zero since the start,
+// Prodigy's denominator is 0 and optax's d becomes NaN; here d stays where it is.
+struct prodigy {
+  double beta1 = 0.9, beta2 = 0.999, eps = 1e-8;
+  double d0 = 1e-6;  // estim_lr0, the initial (under-)estimate of d
+  double coef = 1.0; // estim_lr_coef
+  bool safeguard_warmup = false;
+  double d = 0.0; // estim_lr
+  double numerator_weighted = 0.0;
+  std::vector<double> exp_avg, exp_avg_sq, grad_sum, p0;
+  int count = 0;
+
+  void init(const double *p, std::size_t n) {
+    exp_avg.assign(n, 0.0);
+    exp_avg_sq.assign(n, 0.0);
+    grad_sum.assign(n, 0.0);
+    p0.assign(p, p + n);
+    d = d0;
+    numerator_weighted = 0.0;
+    count = 0;
+  }
+  // `sched` is the learning-rate schedule at the count before this update (1 is Prodigy's base).
+  void update(double *p, const double *g, std::size_t n, double sched) {
+    const int t = count + 1;
+    const double beta3 = std::sqrt(beta2);
+    const double bc = std::sqrt(1.0 - std::pow(beta2, t)) / (1.0 - std::pow(beta1, t));
+    const double dlr = d * sched * bc;
+    double numerator_acum = 0.0;
+    for (std::size_t i = 0; i < n; ++i)
+      numerator_acum += g[i] * (p0[i] - p[i]);
+    double denominator = 0.0;
+    for (std::size_t i = 0; i < n; ++i) {
+      const double dg = d * g[i];
+      exp_avg[i] = beta1 * exp_avg[i] + (1.0 - beta1) * dg;
+      exp_avg_sq[i] = beta2 * exp_avg_sq[i] + (1.0 - beta2) * dg * dg;
+      grad_sum[i] = beta3 * grad_sum[i] + (safeguard_warmup ? d : dlr) * dg / d0;
+      denominator += std::fabs(grad_sum[i]);
+    }
+    numerator_weighted = beta3 * numerator_weighted + (d / d0) * dlr * numerator_acum;
+    if (denominator > 0.0)
+      d = std::max(d, coef * numerator_weighted / denominator);
+    for (std::size_t i = 0; i < n; ++i)
+      p[i] -= dlr * exp_avg[i] / (std::sqrt(exp_avg_sq[i]) + d * eps);
+    count = t;
+  }
+};
+
+} // namespace ph
+} // namespace cvc
+
+#endif // __CVC_PH_OPTIM_H__