RTK–IMU 独立标定链

本分支将 RTK–IMU 标定与原 LiDAR–IMU Phase-A/B/C 隔离。只复用 SO(3)/SE(3)、IMU 数据契约和预积分数学，不复用 LiDAR 命名的结果对象或门限。

坐标与外参约定

唯一输出约定为 p_RTK = R_RTK_IMU p_IMU + t_RTK_IMU，即 T_RTK_IMU 把 IMU 坐标变换到 RTK 传感器坐标。杆臂 l_IMU_to_RTK_in_IMU 是“IMU 原点指向 RTK 定位参考点”的向量，表达在 IMU 坐标中，因此 t_RTK_IMU = -R_RTK_IMU @ l_IMU_to_RTK_in_IMU。

世界坐标采用局部 ENU。GGA 的第一个有效固定解作为每会话 ENU 原点。GGA 高程在局部差分中使用；因为设备输出的高程基准尚未单独核验，Z 方向使用更大的噪声并执行独立门控。

当前 GNHPR 解释候选把 heading 看作从真北顺时针，pitch 看作机头上仰为正，并按 Z-Y-X 生成 R_ENU_RTK。代码同时检查 heading/pitch 的反号候选。固定的 RTK 轴置换不能由手眼残差自行识别，因此 frame_definition 和 reference_point 为空时，最终状态必须是 diagnostic。

数据与时间规则

IMU CSV：t,gx,gy,gz,ax,ay,az，陀螺为 rad/s，加速度为 m/s²。
RTK CSV：位置使用 GGA，姿态使用 GNHPR；仅接受位置质量 4/5、姿态质量 4/5。
RTK 的 t 必须是 NMEA 测量 UTC 经 IMU device→host affine clock 反变换得到的 IMU 设备时间。禁止回退到延迟约 3–4 s 的主机接收时间。
姿态使用 hpr_measurement_utc_s 恢复的 HPR 测量时间，不挂在最近邻 GGA 时刻。这一修正把 motion_sms 的旋转 RMS 从约 4.60° 降到约 1.37°。
残余时间偏移通过 RTK/IMU 角速度模长相关扫描。相关峰不唯一时保持 0 s，并把候选峰只作为诊断值。

求解方法

R0 数据和时间审计：检查固定解比例、共同时间范围、HPR 有效性、ENU 原点和残余时间偏移。motion_sms 自动只使用固定位置且有效姿态的连续段。

R1 旋转：对 0.75 s、1.5 s、3.0 s 相对运动构造 Delta_R_RTK R_RTK_IMU = R_RTK_IMU Delta_R_IMU。IMU 侧采用陀螺预积分。先用无偏置闭式手眼解筛选四种 GNHPR 符号候选，只对最优候选联合优化共享旋转和每会话常值陀螺偏置。输出 RMS/median/P95、边缘协方差、信息奇异值、逐会话 RMS 和固定偏置 conditional LOO。

T1 杆臂和平移：不对 RTK 位置做二次差分。固定 R1 的旋转和陀螺偏置，以可配置节点构造 IMU 位置/速度预积分因子，联合求解跨会话共享杆臂、每节点速度和每会话常值加速度偏置。输出边缘协方差、信息奇异值/精度秩、位置/速度残差和 LOO。上游旋转未通过时，平移无条件标为 diagnostic。

代码结构

imu_lidar/geodesy.py：WGS84 → ECEF → 局部 ENU。
imu_lidar/rtk_io.py：RTK CSV、质量掩码、GGA/HPR 独立测量时刻。
imu_lidar/rtk_attitude.py：GNHPR 候选约定与 SO(3) 插值。
imu_lidar/rtk_imu_rotation.py：时间审计、旋转手眼、偏置、协方差、LOO。
imu_lidar/rtk_imu_translation.py：轻量全 IMU 预积分、杆臂线性因子系统、协方差、LOO。
imu_lidar/rtk_imu_replay.py：清单加载、端到端运行、结构化结果发布。
tools/run_rtk_imu_calibration.py：命令行入口，支持会话/批次筛选和 rotation-only。
tests/test_rtk_imu_calibration.py：ENU、heading 方向、HPR 时刻、预积分等价性测试。

公共 IMU 预积分的端点插值已从每小段扫描完整数组，改为当前相邻样本内的等价局部插值；公共 IMU CSV 加载器也改为先校验表头、再按列加载。完整项目测试通过。

当前结果（8 会话，2026-08-21）

结果目录：artifacts/rtk_imu_calibration_v1/all_sessions。

RPY(R_RTK_IMU) = [0.102824, -0.145072, -1.432698] deg
rotation RMS / median / P95 = 1.627683 / 0.569594 / 3.090395 deg
rotation std = [0.234013, 0.200486, 2.314692] deg
max conditional rotation LOO = 5.144191 deg（去掉 priority_175910_180530）

t_RTK_IMU = [0.771094, 0.569302, -21.073037] m
translation std = [0.367658, 0.367640, 2.960859] m
position RMS XYZ = [0.002606, 0.002617, 0.000871] m
velocity RMS XYZ = [1.034408, 1.001846, 0.081893] m/s
max translation LOO Z = 1.774476 m（去掉 loop_194223_195003）

预期 GNHPR 符号候选胜出，预筛 RMS 1.6313°；pitch 反号为 1.7376°，heading 反号候选为 7.2633°/17.7163°。heading 方向基本可判定，但固定 RTK 轴定义仍未判定。

平移明显不具机械真实性。很小的位置残差来自节点速度自由度和鲁棒权重吸收，不能抵消约 1 m/s 的水平速度残差、米级标准差与巨大 LOO 变化。该候选禁止写入配置。

逐日期旋转：0808 为 [-0.232660, -0.039716, -1.563562]°、RMS 2.121957°；0815 为 [0.188773, -0.139741, -0.582331]°、RMS 0.942835°。两批 yaw 相差约 0.98°，但 yaw 自身标准差约 3.60°/2.57°，不能据此声称安装发生变化。

当前阻断项与下一步

RTK frame_definition 和 reference_point 为空。必须从协议、天线安装方向和原 LiDAR–RTK 手眼结果确认 heading 物理轴、pitch/roll 旋转轴及 GGA 相位中心。
残余时间候选约 +0.04 s，但峰宽且次峰接近，当前按门限固定为 0 s。需要独立高动态数据或同步边沿确认。
priority_175910_180530 的 conditional LOO 牵引达到 5.14°、自身 RMS 3.10°，应检查其 HPR 突跳、IMU 丢帧、动态挠曲和安装状态。
坡道会话 RMS 2.39°，说明 pitch 数据或姿态轴/符号仍可能不完整。
平移水平速度因子 RMS 约 1 m/s、Z 杆臂漂到约 21 m。应先冻结平移，完成 RTK 帧和旋转验证，再检查加速度轴、重力符号、高程基准与噪声模型。
只有旋转、时间和 RTK 帧定义全部通过后，才重新运行平移并做严格的全 nuisance LOO/留出验证。

5.9 KiB Raw Blame History Unescape Escape