修正安装Z离地先验，并改进旋转可视化模式4避免坏IMU位移误导。

Co-authored-by: Cursor <cursoragent@cursor.com>

config/vehicle_hi13_h32_20260808.yaml

@@ -4,19 +4,23 @@ vehicle:
   vehicle_id: "outdoor_usable_20260808"
   body_frame:
     name: "base_link"
-    # CAD / 后轮轴中心测量系（与安装图 dX/dY/dZ 一致）
+    # 车体约定：后轮轴中心在地面投影为原点附近参考；X 前 / Y 左 / Z 上
+    # translation_m 的 Z 使用「离地高度」；后轮轴中心离地 294 mm
     axes: "X forward, Y left, Z up"
     unit: m
-    reference_point: "rear_axle_center"
+    reference_point: "rear_axle_center_xy__z_above_ground"
+    rear_axle_height_above_ground_m: 0.294
 
 installation:
   installation_id: "20260808_priority_windows"
   installed_at: "2026-08-08"
   notes: >
-    HI13R4 + H32 DLogCapture. CAD mounts are origins vs rear axle center
-    (translation only). LiDAR phase-center Z = CAD dZ + 63.5 mm.
+    HI13R4 + H32 DLogCapture. Body +X forward: LiDAR and IMU at positive X.
+    CAD sheet may draw +X rearward; numbers below are body-frame.
+    Z is height above ground = CAD height at axle + 0.294 m (axle AGL).
+    LiDAR phase-center AGL is the measured 1.9165 m (not CAD dZ+63.5).
     IMU axes: HI13R4 manual §2.4 RFU (X right, Y forward, Z up).
-    LiDAR Cartesian assumed body-aligned.
+    LiDAR Cartesian in NPZ assumed body-aligned (X forward).
 
 sensors:
   imu:
@@ -26,14 +30,13 @@ sensors:
       axes: "X right, Y forward, Z up (RFU)"
       driver_axis_remapped: false
     mount_in_body:
-      # CAD 图二：后轮轴中心 → IMU，单位 m（mm/1000）
-      # dX=2574.126255, dY=36.5, dZ=892.5
-      translation_m: [2.574126255, 0.0365, 0.8925]
+      # X/Y：后轮轴中心 → IMU；Z：离地 = CAD 0.8925 + 0.294
+      translation_m: [2.574126255, 0.0365, 1.1865]
       # body <- imu : p_body = R_body_imu * p_imu
       # R_body_imu = [[0,1,0],[-1,0,0],[0,0,1]]  (fwd=imu_y, left=-imu_x, up=imu_z)
       rotation_matrix_body_imu: [[0.0, 1.0, 0.0], [-1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
       rotation_quaternion_xyzw: null
-      source: "CAD dX/dY/dZ + HI13R4 manual RFU"
+      source: "CAD X/Y in body (+X forward); Z = CAD axle-height + 294mm AGL + HI13R4 RFU"
 
   lidar:
     model: "RSLidarH32"
@@ -42,13 +45,11 @@ sensors:
       axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)"
       driver_axis_remapped: false
     mount_in_body:
-      # CAD 图一：后轮轴中心 → 雷达安装点，再加相位中心 +63.5 mm（仅 Z）
-      # dX=2522.276859, dY=0.020526, dZ=1637.499879+63.5=1700.999879
-      translation_m: [2.522276859, 0.000020526, 1.700999879]
-      # 假设雷达系与车体 CAD 轴一致（导出 XYZ 已按此约定）
+      # X/Y：后轮轴中心 → 雷达；Z：相位中心真实离地 1.9165 m
+      translation_m: [2.522276859, 0.000020526, 1.9165]
       rotation_matrix_body_lidar: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
       rotation_quaternion_xyzw: null
-      source: "CAD dX/dY/dZ + phase-center +63.5mm on Z; attitude assumed = body"
+      source: "CAD X/Y in body (+X forward); Z = measured phase-center AGL 1.9165 m; attitude = body"
 
   rtk:
     frame_definition: ""
@@ -66,18 +67,17 @@ time:
 # t_IMU_lidar = R_IMU_body * t_body,  R_IMU_lidar = R_IMU_body * R_body_lidar
 derived_T_IMU_lidar_prior:
   R_IMU_lidar: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
-  t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
-  t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.808499879]
+  t_IMU_lidar_m: [0.036479474, -0.051849396, 0.730]
+  t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.730]
   notes: >
-    Rotation prior is ~90 deg yaw between body/lidar (X-fwd) and IMU RFU (Y-fwd).
-    Translation prior from CAD + LiDAR phase-center offset; use for full_se3 /
-    sanity, not as hard lock for rotation_only.
+    Rotation prior ~90 deg yaw (body/lidar X-fwd vs IMU Y-fwd).
+    Relative Z = 1.9165 - 1.1865 = 0.730 m (was 0.8085 m with old CAD+63.5mm Z).
 
 initialization:
   translation_prior:
     enabled: true
     sigma_m: [0.05, 0.05, 0.05]
-    t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
+    t_IMU_lidar_m: [0.036479474, -0.051849396, 0.730]
   rotation_prior:
     enabled: true
     sigma_deg: 15.0