# RTK–IMU 标定链路审计与当前方案

> 2026-08-21 多源原始报文重构、R1b/R2V/R2G/R3 实现与三批数据实测结果，
> 见 [rtk_imu_multisource_v3.md](rtk_imu_multisource_v3.md)。本页保留旧式
> GGA/GNHPR 链路的审计背景；完整 HPR 手眼仍只允许作为诊断。

## 结论

当前数据可以验证“双天线基线方向与 IMU 的一致性”，但不能单独标定一个无歧义的完整三自由度 RTK 姿态外参。

G90 的 heading 表示主天线 ANT1 到从天线 ANT2 的基线方位。实车中主天线在左、从天线在右，因此该基线指向车体右侧，不是前进方向。IMU 的 +Y 指向车前；结合静止重力数据支持 IMU +Z 向上，可得到安装先验：IMU +X 指向车右，和 ANT1→ANT2 同向。

双天线只能观测这根基线的方位和仰角，绕基线自身的旋转不可观。GNHPR pitch 是横向基线的仰角，更接近车体横滚响应，不能当作坡道上的纵向车体 pitch；roll 字段也不是独立的第三姿态观测。因此，旧链路把 HPR 拼成完整 SO(3) 再做三轴手眼，是坡道 RMS、会话牵引和不稳定 yaw/pitch 的主要来源。

当前实现保留旧式完整 HPR 结果作为诊断量，但不允许它通过完整旋转门禁；平移在完整旋转通过前被硬冻结。

## 坐标系和参考点

统一输出约定：

```text
p_RTK = R_RTK_IMU p_IMU + t_RTK_IMU
```

RTK 车固坐标定义：

- +X：ANT1（主天线、左侧）→ ANT2（从天线、右侧），指向车右。
- +Y：车辆前进方向，与 IMU +Y 同向。
- +Z：车辆上方；静止重力数据支持 IMU +Z 向上。
- 该完整三轴定义包含机械安装先验；GNHPR 实际直接观测的只有 +X 基线。

位置参考点：

- GGA 为 ANT1/主天线相位中心。
- 相位中心离地高度为 1.916499878 m。

## 求解链路

### R0：协议、数据质量和时间

1. 校验 NMEA checksum。
2. GGA 位置只接收 Q=4 固定解。
3. GNHPR 姿态只接收 Q=4 固定解；Q=5 浮点解仅进入诊断统计，不参与标定。
4. 保留 GNHPR 自己的卫星数、差分龄期和基站号，不能再用 GGA 卫星数替代 HPR 质量。
5. 姿态使用 `hpr_measurement_utc_s` 映射后的 IMU 设备时间。
6. 遇到 HPR 无效、浮点、时间间隔异常或基线跳变时切断连续段，运动对不得跨断点。

### T0：残余时间偏移

只使用可观测的有符号 heading 角速度与 IMU `gyro_z` 做相关扫描。角速度模长会混入不可观的绕基线旋转，不再作为时间审计依据。

时间偏移只用于初始化/诊断。只有相关峰足够高、与次峰分离且峰宽足够窄时才应用，否则保持 0 s。

### R1：双天线基线一致性

对每个连续固定解片段构造 0.75 s、1.5 s、3.0 s 的相对运动。以安装先验 `u_IMU=[1,0,0]` 检验：

```text
angle(u_RTK(t0), u_RTK(t1))
    ≈ angle(u_IMU, ΔR_IMU(t0,t1) u_IMU)
```

该标量约束不虚构 RTK 的前向轴和上向轴。输出整体、逐会话 RMS/P95、分轴诊断及最坏时间区间。所有会话的总权重归一，避免高激励或样本更多的会话支配结果。

### R2：旧式完整 HPR 诊断

为兼容历史输出，将基线补成零 roll 的数学坐标架，再运行完整手眼。这个结果仅用于暴露符号错误、异常会话和旧结果变化，不能作为可交付外参。

LOO 删除一个会话后，会重新优化剩余会话的陀螺零偏，不再固定全量数据的 nuisance 参数。

### T1：平移

只有完整三自由度旋转“可观且通过”时，才允许进入杆臂和平移求解。当前条件不满足，因此：

- 不运行平移优化；
- 不输出 `translation_result.json`；
- `t_RTK_IMU_m` 和 `T_RTK_IMU` 为 null；
- 历史巨大 Z、米级不确定度和约 1 m/s 速度残差不再消耗优化时间。

## 当前 8 会话结果

结果目录：`artifacts/rtk_imu_calibration_v2/all_sessions`。

```text
可观测基线一致性：
RMS / median / P95 = 0.593665 / 0.146040 / 1.212972 deg
priority_175910 RMS / P95 = 1.316292 / 2.472570 deg
slope          RMS / P95 = 1.264514 / 2.475899 deg
最坏区间 = priority_175910，约 5.084 deg
baseline gate = passed

时间偏移：
全局候选 = +0.005 s
峰值相关 = 0.909019
近峰宽度 = [-0.110, +0.170] s
实际应用 = 0 s

旧式完整 HPR 诊断：
RPY = [-0.132765, -0.297433, -1.558358] deg
RMS / P95 = 1.172397 / 2.404977 deg
std = [0.615416, 0.437419, 3.687145] deg
priority_175910 re-optimized LOO = 1.698555 deg
max re-optimized LOO = 1.699427 deg（删除 slope）
legacy numeric gate = failed
full attitude observable = false

平移：
frozen / not run
```

旧结果中的 `priority_175910` 条件 LOO 为 5.14°。严格剔除 Q5、会话等权、断段保护以及 LOO 重估零偏后，该会话完整诊断 LOO 降为 1.699°。它和坡道会话仍是主要异常源，但现在异常集中在基线仰角通道，而不是 heading：这更符合原始 HPR 中 Q5、低卫星数和 pitch 大幅波动的事实。

## 已排除或仍存在的漏洞

- 已修复：heading 误当车前方向。
- 已修复：GNHPR pitch 误当纵向车体 pitch。
- 已修复：不可观 roll 注入完整姿态。
- 已修复：Q5 浮点 HPR 进入标定。
- 已修复：HPR 质量字段在导出时丢失。
- 已修复：相对运动跨越无效段或跳变。
- 已修复：样本多的会话权重过大。
- 已修复：LOO 固定全量会话零偏。
- 已修复：时间相关使用三轴角速度模长。
- 已修复：旋转未通过仍继续优化平移。
- 仍存在：当前运动不能稳定地从 GGA 速度补全车前/车上方向。
- 仍存在：基线绕轴自由度没有独立传感器观测。
- 仍存在：`priority_175910` 和坡道的基线仰角存在局部异常。

## 如何得到可交付的完整旋转

按优先级建议：

1. 采一组专用数据：空旷区域、全程 RTK fixed、较长直线加减速、左右转、坡道上下行，保留原始 GGA/GNHPR/IMU 时间和全部质量字段。
2. 用高质量前向速度补第二根轴。只在速度足够高、航向变化平缓的区间用 GGA course，并在同一优化中建模 ANT1 杆臂、非完整车辆侧向速度约束和时间偏移。
3. 用静止重力补上向轴时，必须把“地面水平/车辆静止”写成显式先验，并将结果标记为安装先验约束解，而不是双天线数据独立解。
4. 若能取得 G90 内部融合后的完整 INS 姿态、第三天线、轮速/转角或可靠车体姿态源，优先作为第二独立方向。
5. 完整旋转通过留出验证后，才恢复平移；平移应同时估计杆臂、速度、加计偏置，并检查垂向高程基准。

当前数据上尝试用平滑 GGA 速度直接补全姿态，结果随平滑窗口明显变化，受低速、转弯杆臂和高程差分噪声影响，不能进入正式结果。

## 与主流开源方法的对应

- Kalibr：角速度相关适合作为时间偏移初始化，不应在宽峰时强行采用候选值。
- iKalibr：采用连续时间轨迹联合估计时空参数，并强调充分激励；适合后续专用数据。
- MINS：异步测量插值并把传感器外参、时间和 nuisance 状态一起估计。
- GICI-LIB：因子图中显式进行 GNSS/INS 初始化、质量控制和异常值处理。

本项目暂不直接引入这些大型框架，而是吸收其原则：先保证物理可观测性和数据质量，再进行联合优化；不能用自由状态吸收错误模型。

参考：

- Unicore N4 Reference Commands Manual
- Unicore UM982 User Manual
- https://github.com/Unsigned-Long/iKalibr
- https://github.com/ethz-asl/kalibr
- https://github.com/rpng/MINS
- https://github.com/chichengcn/gici-open

## 代码入口

- `rtk_imu/rtk_attitude.py`：GNHPR 基线语义及零 roll 数学补全。
- `rtk_imu/rtk_io.py`：RTK CSV、Q4/Q5 和 checksum 质量门禁。
- `rtk_imu/rtk_imu_rotation.py`：时间审计、连续段、基线审计、旧式诊断和重优化 LOO。
- `rtk_imu/rtk_imu_replay.py`：坐标定义、参考点、平移冻结和 JSON 输出。
- `tools/rscap_v2/g90_rtk.py`：原始 GNHPR 解析及质量字段。
- `tools/export_g90_rtk_to_sessions.py`：GNHPR 质量字段导出。
- `tools/run_rtk_imu_calibration.py`：端到端命令行入口。
- `tests/test_rtk_imu_calibration.py`：轴定义、Q4/Q5、时间和预积分回归测试。