完善Phase-A会话级联合优化并修正雷达相位中心高度先验

imu_lidar/observability.py

@@ -57,43 +57,53 @@ def analyze_observability(
     if j_r.size == 0:
         return ObservabilityReport(False, False, 1e9, 1e9, ("empty rotation jacobian",))
 
-    # Normalize columns.
-    col_norm = np.linalg.norm(j_r, axis=0) + 1e-12
-    j_r_n = j_r / col_norm
-    singular = np.linalg.svd(j_r_n, compute_uv=False)
+    singular = np.linalg.svd(j_r, compute_uv=False)
     cond_r = float(singular[0] / max(singular[-1], 1e-12))
-    rotation_ok = cond_r < condition_threshold and singular[-1] > 1e-3
+    rotation_information = float(singular[-1] / np.sqrt(max(len(usable), 1)))
+    rotation_ok = (
+        cond_r < condition_threshold
+        and rotation_information > 1e-3
+        and singular[-1] > 1e-6
+    )
 
-    # Translation observability proxy: diversity of rotation axes and presence of translation in B.
-    axes = []
-    translations = []
-    for pair in usable:
-        axis = so3_log(pair.R_B)
-        n = np.linalg.norm(axis)
-        if n > 1e-8:
-            axes.append(axis / n)
-        if pair.t_B_m is not None:
-            translations.append(pair.t_B_m)
-    axis_rank = 0
-    if axes:
-        axis_mat = np.asarray(axes, dtype=float)
-        axis_rank = int(np.linalg.matrix_rank(axis_mat, tol=0.1))
-    trans_span = 0.0
-    if translations:
-        tmat = np.asarray(translations, dtype=float)
-        trans_span = float(np.linalg.norm(np.std(tmat, axis=0)))
-    # For planar yaw-mostly motion, translation z is typically weak.
-    translation_ok = axis_rank >= 2 and trans_span > 0.2 and len(translations) >= 5
-    cond_t = 1e9 if not translation_ok else float(max(3, 10 - axis_rank * 2) * (0.5 / max(trans_span, 1e-3)))
+    # Translation lever arm is observable through stacked (R_A - I). Pure
+    # planar yaw leaves its vertical column in the nullspace and must fail.
+    translation_rows = [
+        np.asarray(pair.R_A, dtype=float).reshape(3, 3) - np.eye(3)
+        for pair in usable
+        if pair.t_B_m is not None
+    ]
+    if translation_rows:
+        j_t = np.vstack(translation_rows)
+        singular_t = np.linalg.svd(j_t, compute_uv=False)
+        cond_t = float(singular_t[0] / max(singular_t[-1], 1e-12))
+        translation_information = float(
+            singular_t[-1] / np.sqrt(max(len(translation_rows), 1))
+        )
+    else:
+        cond_t = 1e9
+        translation_information = 0.0
+    translation_ok = (
+        len(translation_rows) >= 5
+        and cond_t < condition_threshold
+        and translation_information > 0.02
+    )
 
     if not rotation_ok:
-        notes.append(f"rotation condition {cond_r:.1f} exceeds threshold {condition_threshold}")
+        notes.append(
+            f"rotation not observable: condition={cond_r:.1f}, "
+            f"min_information={rotation_information:.3e}"
+        )
     else:
-        notes.append(f"rotation condition {cond_r:.1f}")
+        notes.append(
+            f"rotation observable: condition={cond_r:.1f}, "
+            f"min_information={rotation_information:.3e}"
+        )
     if not translation_ok:
         notes.append(
-            f"translation not observable (axis_rank={axis_rank}, trans_span={trans_span:.3f} m); "
-            "V1 will reject full SE3 without strong priors"
+            f"translation not observable: condition={cond_t:.1f}, "
+            f"min_information={translation_information:.3e}; "
+            "full SE3 will be rejected"
         )
     return ObservabilityReport(
         rotation_observable=rotation_ok,