Test norm-aware magnetic tilt weighting

Author: mgrouch

tools/ou_iii_mag_tangent_sweep.py

@@ -1,12 +1,13 @@
 #!/usr/bin/env python3
-"""Branch-only sweep of tangent-plane magnetic measurement covariance.
-
-The 3-D magnetometer residual has one radial direction parallel to the
-predicted field and two attitude-informative tangent directions.  This study
-scales the measurement sigma independently in the tangent direction excited by
-a yaw error (B x down) and in the orthogonal tangent direction.  The radial
-direction is unchanged.  At yaw_scale=tilt_scale=1 the production update is
-bit-for-bit unchanged.
+"""Branch-only sweep of norm-aware tangent-plane magnetic covariance.
+
+The magnetometer Jacobian J=-[B]x has no first-order attitude information in
+the radial field direction.  The two useful tangent directions are separated
+into the yaw-sensitive direction B x down and the complementary tilt-sensitive
+direction.  This experiment leaves yaw and radial confidence unchanged and
+inflates only tilt-sensitive covariance when the measured field norm disagrees
+with the predicted reference norm -- a direct signature of residual soft-iron
+scale/cross-axis distortion that a pure attitude rotation cannot create.
 """
 from __future__ import annotations
 
@@ -32,29 +33,34 @@
 S_OLD = """    Matrix3& S_mat = S_scratch_;\n    S_mat = Rmag;"""
 S_NEW = r'''    Matrix3 Rmag_eff = Rmag;
 #ifdef EIGEN_NON_ARDUINO
-    const T yaw_sigma_scale = []() -> T {
-        const char* s = std::getenv("OU3_MAG_YAW_SIGMA_SCALE");
-        if (!s) return T(1);
+    const T tilt_norm_gain = []() -> T {
+        const char* s = std::getenv("OU3_MAG_TILT_NORM_GAIN");
+        if (!s) return T(0);
         const T v = static_cast<T>(std::strtod(s, nullptr));
-        return (std::isfinite(v) && v > T(0)) ? v : T(1);
+        return (std::isfinite(v) && v >= T(0)) ? v : T(0);
     }();
-    const T tilt_sigma_scale = []() -> T {
-        const char* s = std::getenv("OU3_MAG_TILT_SIGMA_SCALE");
-        if (!s) return T(1);
+    const T tilt_norm_deadband = []() -> T {
+        const char* s = std::getenv("OU3_MAG_TILT_NORM_DEADBAND");
+        if (!s) return T(0);
         const T v = static_cast<T>(std::strtod(s, nullptr));
-        return (std::isfinite(v) && v > T(0)) ? v : T(1);
+        return (std::isfinite(v) && v >= T(0)) ? v : T(0);
     }();
-    if (std::abs(yaw_sigma_scale - T(1)) > T(1e-7) ||
-        std::abs(tilt_sigma_scale - T(1)) > T(1e-7)) {
-        const T bn = v2hat.norm();
-        if (std::isfinite(bn) && bn > T(1e-8)) {
-            const Vector3 b = v2hat / bn;
+    const T tilt_norm_cap = []() -> T {
+        const char* s = std::getenv("OU3_MAG_TILT_NORM_CAP");
+        if (!s) return T(1.15);
+        const T v = static_cast<T>(std::strtod(s, nullptr));
+        return (std::isfinite(v) && v >= T(1)) ? v : T(1.15);
+    }();
+    if (tilt_norm_gain > T(0)) {
+        const T bhat_norm = v2hat.norm();
+        const T bmeas_norm = v2.norm();
+        if (std::isfinite(bhat_norm) && std::isfinite(bmeas_norm) &&
+            bhat_norm > T(1e-8) && bmeas_norm > T(1e-8)) {
+            const Vector3 b = v2hat / bhat_norm;
             Vector3 down_b = R_wb() * Vector3(T(0), T(0), T(1));
             const T dn = down_b.norm();
             if (std::isfinite(dn) && dn > T(1e-8)) {
                 down_b /= dn;
-                // A small yaw error is a rotation about local down. Through
-                // J=-[B]x, its measurement residual lies along B x down.
                 Vector3 t_yaw = b.cross(down_b);
                 const T yn = t_yaw.norm();
                 if (std::isfinite(yn) && yn > T(1e-6)) {
@@ -63,12 +69,16 @@
                     const T tn = t_tilt.norm();
                     if (std::isfinite(tn) && tn > T(1e-6)) {
                         t_tilt /= tn;
-                        const T sy = std::clamp(yaw_sigma_scale, T(0.5), T(4));
-                        const T st = std::clamp(tilt_sigma_scale, T(0.5), T(4));
-                        // Congruence scaling preserves the exact configured
-                        // Rmag at sy=st=1, including a future non-isotropic Rmag.
+                        const T rel_norm_error = std::abs(bmeas_norm / bhat_norm - T(1));
+                        const T excess = std::max(T(0), rel_norm_error - tilt_norm_deadband);
+                        const T st = std::clamp(T(1) + tilt_norm_gain * excess,
+                                                T(1), tilt_norm_cap);
+                        // Radial and yaw-sensitive information are unchanged.
+                        // Only the tangent direction that primarily informs
+                        // roll/pitch is softened when |B| says the sample is
+                        // inconsistent with a rigid attitude rotation.
                         const Matrix3 M = b * b.transpose()
-                            + sy * (t_yaw * t_yaw.transpose())
+                            + t_yaw * t_yaw.transpose()
                             + st * (t_tilt * t_tilt.transpose());
                         Rmag_eff = M * Rmag * M.transpose();
                         Rmag_eff = T(0.5) * (Rmag_eff + Rmag_eff.transpose());
@@ -86,19 +96,18 @@
 
 
 def build() -> None:
-    # Do not run `make clean` here: the OU-III clean target also removes the
-    # downloaded wave_data_*.csv fixtures.  The checkout is clean and this
-    # script modifies a header, so ordinary dependency tracking rebuilds the
-    # simulator while preserving the eight downloaded records.
+    # The OU-III clean target removes the downloaded wave_data_*.csv fixtures,
+    # so rely on header dependency tracking instead of `make clean`.
     subprocess.run(["make", "kalman_ou_iii-sim"], cwd=TEST, check=True)
 
 
-def run(yaw_scale: float = 1.0, tilt_scale: float = 1.0):
+def run(gain: float = 0.0, deadband: float = 0.0, cap: float = 1.15):
     env = os.environ.copy()
     env["W3D_WRITE_TIMESERIES"] = "0"
     env["W3D_COLLECT_ALL_GATES"] = "1"
-    env["OU3_MAG_YAW_SIGMA_SCALE"] = str(yaw_scale)
-    env["OU3_MAG_TILT_SIGMA_SCALE"] = str(tilt_scale)
+    env["OU3_MAG_TILT_NORM_GAIN"] = str(gain)
+    env["OU3_MAG_TILT_NORM_DEADBAND"] = str(deadband)
+    env["OU3_MAG_TILT_NORM_CAP"] = str(cap)
     p = subprocess.run([str(BIN)], cwd=TEST, env=env, text=True,
                        stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
     ds = DS_RE.findall(p.stdout)
@@ -146,37 +155,36 @@ def main() -> None:
         HEADER.write_text(h)
         build()
 
-        base = run(1.0, 1.0)
+        base = run(0.0, 0.0, 1.15)
         print("BASELINE", flush=True)
 
-        # First sweep showed that weakening the yaw-sensitive tangent direction
-        # moves the stubborn horizontal-X metrics in the wrong direction, while
-        # weakening the orthogonal (tilt-sensitive) tangent direction produces
-        # most of the broad gains.  Search the complementary quadrant: slightly
-        # *more* yaw information with slightly *less* tilt information.
-        candidates = [
-            (0.88, 1.10),
-            (0.92, 1.06), (0.92, 1.10), (0.92, 1.14),
-            (0.96, 1.06), (0.96, 1.10), (0.96, 1.14),
-            (1.00, 1.06), (1.00, 1.10), (1.00, 1.14),
-            (1.04, 1.06), (1.04, 1.10), (1.04, 1.14),
-            (0.96, 1.18), (0.96, 1.22),
-        ]
+        # A 1--2% norm inconsistency is the expected order of residual soft-iron
+        # scale/cross-axis distortion. Gains 2--8 therefore span roughly the
+        # static 1.02--1.16 tilt-sigma range, but only on inconsistent samples.
+        candidates = []
+        for deadband in (0.0, 0.003, 0.006, 0.010):
+            for gain in (2.0, 4.0, 6.0, 8.0):
+                candidates.append((gain, deadband, 1.15))
+        candidates.extend(((4.0, 0.003, 1.10), (6.0, 0.003, 1.10),
+                           (4.0, 0.006, 1.10), (6.0, 0.006, 1.10)))
 
         results = []
-        for sy, st in candidates:
-            cand = run(sy, st)
+        for gain, deadband, cap in candidates:
+            cand = run(gain, deadband, cap)
             mx, n, geo, rows = compare(base, cand)
-            print(f"RESULT yaw={sy:.3f} tilt={st:.3f} max={mx:.9f} geo={geo:.9f} improved={n}/56", flush=True)
+            print(f"RESULT gain={gain:.2f} deadband={deadband:.3f} cap={cap:.2f} "
+                  f"max={mx:.9f} geo={geo:.9f} improved={n}/56", flush=True)
             for r, d, m, b, v in rows[:8]:
                 print(f"  {r:.9f} {m:5s} {d}: {b:.9g} -> {v:.9g}", flush=True)
-            results.append((mx, geo, -n, sy, st, rows))
+            results.append((mx, geo, -n, gain, deadband, cap, rows))
 
         print("=== RANKING ===", flush=True)
-        for mx, geo, nn, sy, st, rows in sorted(results)[:15]:
-            print(f"yaw={sy:.3f} tilt={st:.3f} max={mx:.9f} geo={geo:.9f} improved={-nn}/56", flush=True)
+        for mx, geo, nn, gain, db, cap, rows in sorted(results)[:15]:
+            print(f"gain={gain:.2f} deadband={db:.3f} cap={cap:.2f} "
+                  f"max={mx:.9f} geo={geo:.9f} improved={-nn}/56", flush=True)
         best = sorted(results)[0]
-        print(f"BEST yaw={best[3]:g} tilt={best[4]:g} max={best[0]:.9f} improved={-best[2]}/56", flush=True)
+        print(f"BEST gain={best[3]:g} deadband={best[4]:g} cap={best[5]:g} "
+              f"max={best[0]:.9f} improved={-best[2]}/56", flush=True)
         print("STRICT_DOMINANCE=PASS" if best[0] < 1.0 and -best[2] == 56
               else "STRICT_DOMINANCE=FAIL", flush=True)
     finally: