重构RTK-IMU标定链路并完成机械先验工程验证
This commit is contained in:
lichun.qu
+19
@@ -8,3 +8,22 @@ build/
|
||||
examples/synthetic_session/
|
||||
.venv/
|
||||
venv/
|
||||
|
||||
# Local IDE state
|
||||
.vs/
|
||||
|
||||
# RTK-IMU calibration process artifacts stay local. Keep only the reviewed
|
||||
# V3 result bundle explicitly listed below under version control.
|
||||
artifacts/rtk_imu_calibration_v2/
|
||||
artifacts/rtk_imu_calibration_v3/*
|
||||
!artifacts/rtk_imu_calibration_v3/README.md
|
||||
!artifacts/rtk_imu_calibration_v3/engineering_release_decision.json
|
||||
!artifacts/rtk_imu_calibration_v3/heldout_independent_innovation.json
|
||||
!artifacts/rtk_imu_calibration_v3/heldout_nonconverged_retry.json
|
||||
!artifacts/rtk_imu_calibration_v3/lever_information_window_selection.json
|
||||
!artifacts/rtk_imu_calibration_v3/lever_information_window_selection_refined.json
|
||||
!artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_47_window.json
|
||||
!artifacts/rtk_imu_calibration_v3/mechanical_prior_engineering_heldout.json
|
||||
!artifacts/rtk_imu_calibration_v3/mechanical_prior_rotation_sensitivity.json
|
||||
!artifacts/rtk_imu_calibration_v3/node_graph_free_information_selected_mechanical.json
|
||||
!artifacts/rtk_imu_calibration_v3/propagation_bias_root_cause_audit.json
|
||||
|
||||
@@ -0,0 +1,24 @@
|
||||
# RTK-IMU V3 正式结果集
|
||||
|
||||
本目录只提交复现工程结论所需的审核结果。运行过程中的调试输出、状态快照、逐窗口 checkpoint 和旧版结果保留在本机，由仓库根目录 `.gitignore` 排除。
|
||||
|
||||
## 当前结论
|
||||
|
||||
- 固定旋转来源：`R2G_gravity_level_prior`。
|
||||
- 工程候选杆臂：`l_I = p_ANT1^I = [-0.4518015159, -0.2644749820, 0.7314656115] m`。
|
||||
- `data_only_translation_accepted=false`。
|
||||
- `engineering_translation_accepted=false`，原因是独立传播验证仍受公共加速度偏差影响。
|
||||
- `independent_extrinsic_sensitive_validation_passed=true`；当前结果可描述为机械杆臂经动态数据一致性验证的工程候选，不得描述为 data-only 平移标定结果。
|
||||
|
||||
## 文件说明
|
||||
|
||||
- `engineering_release_decision.json`：最终状态、变换和放行判定。
|
||||
- `mechanical_prior_engineering_47_window.json`：相同 47 个非重叠窗口上的 free/fixed/prior 对比。
|
||||
- `node_graph_free_information_selected_mechanical.json`：冻结的无先验 free-solve 基线。
|
||||
- `lever_information_window_selection*.json`：窗口选择及实际边缘信息复核。
|
||||
- `mechanical_prior_engineering_heldout.json`：未参与标定窗口的 held-out 验证。
|
||||
- `heldout_independent_innovation.json`、`heldout_nonconverged_retry.json`：独立创新与失败窗口重试结果。
|
||||
- `mechanical_prior_rotation_sensitivity.json`：固定候选杆臂的旋转扰动敏感性结果。
|
||||
- `propagation_bias_root_cause_audit.json`：独立传播公共加速度误差根因审计。
|
||||
|
||||
结果 JSON 是审核快照；需要重新生成时应通过 `tools/` 中相应入口运行，且不得提交运行产生的 checkpoint 或 `.npz` 状态文件。
|
||||
@@ -0,0 +1,128 @@
|
||||
{
|
||||
"scope": "final mechanical-prior RTK-IMU engineering release decision",
|
||||
"no_refit_performed": true,
|
||||
"data_only_full_free_called": false,
|
||||
"bootstrap_called": false,
|
||||
"loo_called": false,
|
||||
"covariance_retuned": false,
|
||||
"new_window_selection_called": false,
|
||||
"parser_R0_modified": false,
|
||||
"data_only_translation_accepted": false,
|
||||
"translation_refined_by_data": false,
|
||||
"mechanical_prior_consistent_with_calibration": true,
|
||||
"heldout_physical_validation_passed": true,
|
||||
"heldout_physical_gate_checks": {
|
||||
"all_267_converged_after_retry": true,
|
||||
"BEST_position_vector_p95_le_0p20_m": true,
|
||||
"Doppler_vector_p95_le_0p50_m_s": true,
|
||||
"HPR_normalized_p95_le_4": true,
|
||||
"preintegration_normalized_p95_le_3": true
|
||||
},
|
||||
"heldout_statistical_scale_passed": false,
|
||||
"heldout_postfit_chi_square_per_dof": 0.12733913101711092,
|
||||
"heldout_covariance_underdispersion_warning": true,
|
||||
"independent_heldout_innovation_passed": false,
|
||||
"common_constant_acceleration_error_detected": true,
|
||||
"independent_propagation_validation_passed": false,
|
||||
"independent_extrinsic_sensitive_validation_passed": true,
|
||||
"rotation_sensitivity_passed": true,
|
||||
"engineering_translation_acceptance_formula": "mechanical_prior_consistent_with_calibration AND heldout_physical_validation_passed AND independent_heldout_innovation_passed AND rotation_sensitivity_passed",
|
||||
"engineering_translation_accepted": false,
|
||||
"result_nature": "mechanically anchored + dynamically validated",
|
||||
"summary": "Translation is mechanically anchored and dynamically validated. The current dataset does not independently observe translation accurately enough for data-only calibration, and does not provide meaningful refinement beyond the mechanical prior.",
|
||||
"forbidden_descriptions": [
|
||||
"data-only calibrated translation",
|
||||
"dynamically refined mechanical lever"
|
||||
],
|
||||
"candidate_l_I_engineering_m": [
|
||||
-0.45180151590212486,
|
||||
-0.26447498198536895,
|
||||
0.7314656114613277
|
||||
],
|
||||
"candidate_T_RTK_IMU": [
|
||||
[
|
||||
0.9999999761265028,
|
||||
-0.00021395911860711003,
|
||||
-4.436766104011944e-05,
|
||||
0.45177737170031523
|
||||
],
|
||||
[
|
||||
0.00021360059847267874,
|
||||
0.9999685415936667,
|
||||
-0.007929072948303898,
|
||||
0.27036301129056084
|
||||
],
|
||||
[
|
||||
4.606276276360097e-05,
|
||||
0.007929063282050246,
|
||||
0.9999685634227163,
|
||||
-0.7293247665913783
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"candidate_T_IMU_RTK": [
|
||||
[
|
||||
0.9999999761265029,
|
||||
0.00021360059847267876,
|
||||
4.606276276360098e-05,
|
||||
-0.45180151590212486
|
||||
],
|
||||
[
|
||||
-0.00021395911860711008,
|
||||
0.9999685415936669,
|
||||
0.007929063282050248,
|
||||
-0.264474981985369
|
||||
],
|
||||
[
|
||||
-4.436766104011945e-05,
|
||||
-0.0079290729483039,
|
||||
0.9999685634227164,
|
||||
0.7314656114613278
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"candidate_transform_inverse_error_norm": 1.3597553244868544e-16,
|
||||
"l_I_engineering_m": null,
|
||||
"T_RTK_IMU": null,
|
||||
"T_IMU_RTK": null,
|
||||
"transform_convention": {
|
||||
"equation": "p_RTK = R_RTK_IMU * p_IMU + t_RTK_IMU",
|
||||
"translation": "t_RTK_IMU = -R_RTK_IMU * l_I",
|
||||
"RTK_origin": "ANT1 phase center"
|
||||
},
|
||||
"rotation_source": "R2G_gravity_level_prior",
|
||||
"translation_conditional_on_rotation": true,
|
||||
"evidence": {
|
||||
"calibration_path": "artifacts\\rtk_imu_calibration_v3\\mechanical_prior_engineering_47_window.json",
|
||||
"heldout_postfit_path": "artifacts\\rtk_imu_calibration_v3\\mechanical_prior_engineering_heldout.json",
|
||||
"innovation_path": "artifacts\\rtk_imu_calibration_v3\\heldout_independent_innovation.json",
|
||||
"sensitivity_path": "artifacts\\rtk_imu_calibration_v3\\mechanical_prior_rotation_sensitivity.json",
|
||||
"convergence_retry_path": "artifacts\\rtk_imu_calibration_v3\\heldout_nonconverged_retry.json",
|
||||
"propagation_root_cause_path": "artifacts\\rtk_imu_calibration_v3\\propagation_bias_root_cause_audit.json",
|
||||
"posterior_prior_variance_ratio": [
|
||||
0.9841028247436912,
|
||||
0.9831529061304226,
|
||||
0.9921155953185713
|
||||
],
|
||||
"heldout_convergence_after_retry": 1.0,
|
||||
"rotation_sensitivity_summary": {
|
||||
"max_abs_delta_l_xyz_m": [
|
||||
0.0024980444199615426,
|
||||
0.0006588674774769543,
|
||||
0.0007172660986609625
|
||||
],
|
||||
"max_delta_l_norm_m": 0.0025582764807350012,
|
||||
"max_transform_translation_delta_norm_m": 0.00686033304738171
|
||||
}
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
+1302
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
+152
-56
@@ -1,72 +1,168 @@
|
||||
# RTK–IMU 独立标定链
|
||||
# RTK–IMU 标定链路审计与当前方案
|
||||
|
||||
本分支将 RTK–IMU 标定与原 LiDAR–IMU Phase-A/B/C 隔离。只复用 SO(3)/SE(3)、IMU 数据契约和预积分数学，不复用 LiDAR 命名的结果对象或门限。
|
||||
> 2026-08-21 多源原始报文重构、R1b/R2V/R2G/R3 实现与三批数据实测结果，
|
||||
> 见 [rtk_imu_multisource_v3.md](rtk_imu_multisource_v3.md)。本页保留旧式
|
||||
> GGA/GNHPR 链路的审计背景；完整 HPR 手眼仍只允许作为诊断。
|
||||
|
||||
## 坐标与外参约定
|
||||
## 结论
|
||||
|
||||
唯一输出约定为 `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`。
|
||||
当前数据可以验证“双天线基线方向与 IMU 的一致性”，但不能单独标定一个无歧义的完整三自由度 RTK 姿态外参。
|
||||
|
||||
世界坐标采用局部 ENU。GGA 的第一个有效固定解作为每会话 ENU 原点。GGA 高程在局部差分中使用；因为设备输出的高程基准尚未单独核验，Z 方向使用更大的噪声并执行独立门控。
|
||||
G90 的 heading 表示主天线 ANT1 到从天线 ANT2 的基线方位。实车中主天线在左、从天线在右，因此该基线指向车体右侧，不是前进方向。IMU 的 +Y 指向车前；结合静止重力数据支持 IMU +Z 向上，可得到安装先验：IMU +X 指向车右，和 ANT1→ANT2 同向。
|
||||
|
||||
当前 GNHPR 解释候选把 heading 看作从真北顺时针，pitch 看作机头上仰为正，并按 Z-Y-X 生成 `R_ENU_RTK`。代码同时检查 heading/pitch 的反号候选。固定的 RTK 轴置换不能由手眼残差自行识别，因此 `frame_definition` 和 `reference_point` 为空时，最终状态必须是 diagnostic。
|
||||
双天线只能观测这根基线的方位和仰角，绕基线自身的旋转不可观。GNHPR pitch 是横向基线的仰角，更接近车体横滚响应，不能当作坡道上的纵向车体 pitch；roll 字段也不是独立的第三姿态观测。因此，旧链路把 HPR 拼成完整 SO(3) 再做三轴手眼，是坡道 RMS、会话牵引和不稳定 yaw/pitch 的主要来源。
|
||||
|
||||
## 数据与时间规则
|
||||
当前实现保留旧式完整 HPR 结果作为诊断量，但不允许它通过完整旋转门禁；平移在完整旋转通过前被硬冻结。
|
||||
|
||||
- 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`。
|
||||
统一输出约定：
|
||||
|
||||
```text
|
||||
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）
|
||||
p_RTK = R_RTK_IMU p_IMU + t_RTK_IMU
|
||||
```
|
||||
|
||||
预期 GNHPR 符号候选胜出，预筛 RMS 1.6313°；pitch 反号为 1.7376°，heading 反号候选为 7.2633°/17.7163°。heading 方向基本可判定，但固定 RTK 轴定义仍未判定。
|
||||
RTK 车固坐标定义：
|
||||
|
||||
平移明显不具机械真实性。很小的位置残差来自节点速度自由度和鲁棒权重吸收，不能抵消约 1 m/s 的水平速度残差、米级标准差与巨大 LOO 变化。该候选禁止写入配置。
|
||||
- +X：ANT1（主天线、左侧）→ ANT2（从天线、右侧），指向车右。
|
||||
- +Y：车辆前进方向，与 IMU +Y 同向。
|
||||
- +Z：车辆上方；静止重力数据支持 IMU +Z 向上。
|
||||
- 该完整三轴定义包含机械安装先验；GNHPR 实际直接观测的只有 +X 基线。
|
||||
|
||||
逐日期旋转：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°，不能据此声称安装发生变化。
|
||||
位置参考点：
|
||||
|
||||
## 当前阻断项与下一步
|
||||
- GGA 为 ANT1/主天线相位中心。
|
||||
- 相位中心离地高度为 1.916499878 m。
|
||||
|
||||
1. RTK `frame_definition` 和 `reference_point` 为空。必须从协议、天线安装方向和原 LiDAR–RTK 手眼结果确认 heading 物理轴、pitch/roll 旋转轴及 GGA 相位中心。
|
||||
2. 残余时间候选约 +0.04 s，但峰宽且次峰接近，当前按门限固定为 0 s。需要独立高动态数据或同步边沿确认。
|
||||
3. `priority_175910_180530` 的 conditional LOO 牵引达到 5.14°、自身 RMS 3.10°，应检查其 HPR 突跳、IMU 丢帧、动态挠曲和安装状态。
|
||||
4. 坡道会话 RMS 2.39°，说明 pitch 数据或姿态轴/符号仍可能不完整。
|
||||
5. 平移水平速度因子 RMS 约 1 m/s、Z 杆臂漂到约 21 m。应先冻结平移，完成 RTK 帧和旋转验证，再检查加速度轴、重力符号、高程基准与噪声模型。
|
||||
6. 只有旋转、时间和 RTK 帧定义全部通过后，才重新运行平移并做严格的全 nuisance LOO/留出验证。
|
||||
## 求解链路
|
||||
|
||||
### 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
|
||||
|
||||
## 代码入口
|
||||
|
||||
- `imu_lidar/rtk_attitude.py`：GNHPR 基线语义及零 roll 数学补全。
|
||||
- `imu_lidar/rtk_io.py`：RTK CSV、Q4/Q5 和 checksum 质量门禁。
|
||||
- `imu_lidar/rtk_imu_rotation.py`：时间审计、连续段、基线审计、旧式诊断和重优化 LOO。
|
||||
- `imu_lidar/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、时间和预积分回归测试。
|
||||
|
||||
@@ -0,0 +1,108 @@
|
||||
# RTK–IMU Engineering 6DoF 分支
|
||||
|
||||
该分支与 V3 `data_only` 严格链路并列，不改变其门禁结论：
|
||||
|
||||
```text
|
||||
data_only_6dof_accepted = false
|
||||
```
|
||||
|
||||
工程分支固定使用真实水平静止场地得到的 R2G 旋转，默认参考值为
|
||||
`RPY=[0.4543066, -0.0026392, 0.0122384] deg`，输出始终记录：
|
||||
|
||||
```text
|
||||
rotation_source = R2G_gravity_level_prior
|
||||
translation_conditional_on_rotation = true
|
||||
```
|
||||
|
||||
## 条件模型
|
||||
|
||||
估计量是 `l_I = p_ANT1^I`。每个连续质量段使用 HI13 设备时间、raw gyro/acc 动态预积分，融合 RTK Fixed 的 GGA/BESTNAVA 位置、BESTNAVA Doppler velocity 和 Q4 GNHPR 的 ANT1→ANT2 基线。逐段 nuisance state 包含初始姿态、IMU 原点初始位置、速度、gyro bias 和 acc bias。HI13 absolute quaternion/RPY（尤其 absolute yaw）不进入机械外参因子；R2V 仅作诊断。
|
||||
|
||||
静止相关因子分为两类：
|
||||
|
||||
- `gravity_candidate`：连续约 1.5 s 的低角速度、gyro/acc 方差稳定且加速度模长接近重力；它不能等价为静止。
|
||||
- `zupt_static`：在 gravity candidate 基础上，必须同时有至少约 1 s 的连续 BESTNAVA Doppler 速度接近 0。高速或匀速直线不会加入 ZUPT。
|
||||
|
||||
连续段复用 R0 质量断点：checksum 无效、定位非 Fixed、GNHPR 非 Q4、设备时间回跳、HPR 测量间隙、baseline jump、位置测量间隙或 IMU gap 都会切段，禁止跨断点预积分。 断后片段还必须至少包含 6 个求解节点且持续不少于 5 s；更短的局部欠约束微段不会跨断点拼接，而是直接不进入杆臂优化。
|
||||
|
||||
## 高程与残差口径
|
||||
|
||||
- GGA 只约束 XY；GGA MSL altitude 不定义也不参与 ENU-Z。
|
||||
- ENU-Z reference 只来自有效 Fixed BESTNAVA altitude；BESTNAVA 才约束 XYZ。
|
||||
- 输出分别为 `gga_xy_residual`、`bestnava_xyz_residual` 和 `doppler_velocity_residual`。未进入 Z factor 的 GGA 高度不进入垂向残差统计。
|
||||
|
||||
## 可观性与验收
|
||||
|
||||
杆臂可观性不再使用全状态最小奇异向量。状态分为杆臂 `l` 与 nuisance state，对优化 Hessian 计算 Schur complement：
|
||||
|
||||
```text
|
||||
H_l_marg = H_ll - H_ln pinv(H_nn) H_nl
|
||||
```
|
||||
|
||||
只对该 3×3 marginal lever information 做 SVD，并输出：
|
||||
|
||||
- `l_I_marginal_covariance_m2`、`l_I_std_m`
|
||||
- `lever_information_singular_values`
|
||||
- `lever_information_condition_number`
|
||||
- `lever_precision_rank`
|
||||
- `weakest_lever_direction_I`
|
||||
|
||||
门禁分为两层：
|
||||
|
||||
- `solver_health_gates` 只判断优化是否收敛、数值是否有限且残差未发散。
|
||||
- `engineering_acceptance_gates` 使用更严格的杆臂 marginal std/information/rank/condition、BESTNAVA XYZ、GGA XY、Doppler velocity、LOO、bootstrap、旋转敏感性和可选手量一致性。
|
||||
|
||||
任一核心门禁失败时，`engineering_6dof_accepted=false`。没有完整执行 bootstrap 和 18 组旋转敏感性时，两项门禁明确为 false，不会把阶段性 base/LOO 结果误标为正式放行。
|
||||
|
||||
Bootstrap 按 session 有放回抽样，并保留重复 session 的 multiplicity；重复抽中的 session 会重复贡献其全部连续段。
|
||||
|
||||
## 机械杆臂软先验与双解输出
|
||||
|
||||
`--manual-l-i-m` 仅在同时提供 `--manual-l-i-std-m` 或完整
|
||||
`--manual-l-i-covariance-m2` 时才成为白化高斯软因子。求解器始终先运行无先验
|
||||
`free_solution`，再运行 `prior_constrained_solution`；输出还包含机械参考及两者到
|
||||
参考的差值。当前活动 engineering 解为 prior-constrained 解（若启用），但 acceptance
|
||||
额外要求 free-solve 也与机械参考一致，软先验不能掩盖不可观或数据矛盾。
|
||||
|
||||
示例（数值需使用实际机械测量的 1σ，不可把示例值当作默认）：
|
||||
|
||||
```powershell
|
||||
--manual-l-i-m -0.45072 -0.25682 0.73208 `
|
||||
--manual-l-i-std-m <sigma_x_m> <sigma_y_m> <sigma_z_m>
|
||||
```
|
||||
## 坐标转换
|
||||
|
||||
```text
|
||||
T_RTK_IMU: p_RTK = R_RTK_IMU p_IMU - R_RTK_IMU l_I
|
||||
T_IMU_RTK: p_IMU = R_RTK_IMU^T p_RTK + l_I
|
||||
```
|
||||
|
||||
RTK 原点是 ANT1，因此 `T_IMU_RTK.translation == l_I`，两矩阵必须互逆。
|
||||
|
||||
## 分阶段运行
|
||||
|
||||
第一阶段默认只运行 base fit、marginal observability、residual audit 和 LOO：
|
||||
|
||||
```powershell
|
||||
python tools/run_rtk_imu_engineering_6dof.py `
|
||||
--manifest D:\data\rtk_imu_unified_v3\manifest.json `
|
||||
--output artifacts/rtk_imu_calibration_v3/engineering_6dof_base_loo.json `
|
||||
--session <session-a> `
|
||||
--session <session-b> `
|
||||
--session <session-c>
|
||||
```
|
||||
|
||||
只有 base/LOO 合理后，才显式启动全量验证：
|
||||
|
||||
```powershell
|
||||
python tools/run_rtk_imu_engineering_6dof.py `
|
||||
--manifest D:\data\rtk_imu_unified_v3\manifest.json `
|
||||
--output artifacts/rtk_imu_calibration_v3/engineering_6dof_full.json `
|
||||
--session <session-a> `
|
||||
--session <session-b> `
|
||||
--session <session-c> `
|
||||
--run-bootstrap --bootstrap-repetitions 40 --bootstrap-seed 0 `
|
||||
--run-rotation-sensitivity
|
||||
```
|
||||
|
||||
正式会话应覆盖直行加减速、左右转、坡道和明确水平静止段。不得为了运行时间降低验收门禁或把未完成验证标为成功。
|
||||
@@ -0,0 +1,175 @@
|
||||
# RTK–IMU 多源原始数据与旋转标定 V3
|
||||
|
||||
## 可行性结论
|
||||
|
||||
重构方向正确且必要。原始捕获中确实存在旧导出链路忽略的 BESTNAVA、
|
||||
PVTSLNA 和 HI13 姿态/四元数。它们可以补充速度、质量、绝对姿态和静止
|
||||
重力约束，但本批数据尚不能通过完整旋转门禁：
|
||||
|
||||
- BESTNAVA 约 0.8–1 Hz，严格高速样本很少。
|
||||
- 当前动态主要是平面 yaw，R1b 的两个倾斜自由度仍弱可观。
|
||||
- HI13 四元数与 GNSS 真北存在稳定但非机械的 yaw 偏差，可能来自磁偏角或
|
||||
内部导航融合。
|
||||
- 两个明确水平静止会话的 R2G 很稳定，但它是水平/重力先验约束解。
|
||||
|
||||
因此，本次重构显著提高了诊断能力，也防止错误外参进入平移；它没有把信息
|
||||
不足包装成“标定成功”。
|
||||
|
||||
## 三批原始数据
|
||||
|
||||
扫描根目录：
|
||||
|
||||
- 0808：D:\data\raw_serial_capture_v2，仅选 20260808。
|
||||
- 0815：D:\data\0815\raw_serial_capture_v2；目录实际包含 20260812–14。
|
||||
- 0819：D:\data\0819\raw_serial_capture_v2。
|
||||
|
||||
按文件名捕获开始时间在 1.5 s 内一一配对：
|
||||
|
||||
| 批次 | G90/HI13 配对 | GNHPR | BESTNAVA | PVTSLNA | Q4 HPR | Fixed Doppler |
|
||||
|---|---:|---:|---:|---:|---:|---:|
|
||||
| 0808 | 18 | 46,180 | 8,291 | 5,993 | 45,606 | 8,128 |
|
||||
| 0815 | 28 | 96,241 | 12,660 | 9,638 | 27,045 | 3,385 |
|
||||
| 0819 | 9 | 15,717 | 2,029 | 1,427 | 14,976 | 1,930 |
|
||||
|
||||
共完成 55 组统一导出。0815 有一组 G90 没有匹配 HI13，manifest 将其列为
|
||||
unmatched。发现一条 NMEA 时间为 0913A：保留原始行，设备时间留空并隔离，
|
||||
没有回退到 host receive time。
|
||||
|
||||
速度不等于高速激励。增加速度 ≥1.5 m/s、水平速度标准差 ≤0.25 m/s 后，
|
||||
三个批次分别只剩 44、8、0 条候选。
|
||||
|
||||
## 统一导出
|
||||
|
||||
输出根目录：D:\data\rtk_imu_unified_v3。
|
||||
|
||||
每组捕获：
|
||||
|
||||
- imu.npz：HI13 system_time、gyro、accel、RPY、WXYZ quaternion、磁场、
|
||||
PPS stamp、温度、气压和 host receive UTC。
|
||||
- rtk.csv：每条原生异步 GGA/GNHPR/BESTNAVA/PVTSLNA 单独成行，不再把
|
||||
HPR/BEST/PVT 最近邻挂到 GGA。
|
||||
- export_summary.json：原始捕获摘要、报文数量、四元数范数和设备到 host
|
||||
的仿射时钟诊断。
|
||||
- manifest.json：全部会话、批次、目录和 unmatched 记录。
|
||||
|
||||
时间规则：
|
||||
|
||||
1. HI13 system_time 是 IMU 主时间轴。
|
||||
2. BESTNAVA/PVTSLNA 使用 GNSS week/TOW 和 leap seconds。
|
||||
3. GGA/GNHPR 使用报文自己的 UTC time-of-day。
|
||||
4. RTK GNSS 测量时刻通过 HI13 device→host 仿射模型映射进 HI13 设备时钟。
|
||||
5. host receive time 只用于跨时钟桥接、延迟和抖动诊断，绝不替代采样时刻。
|
||||
6. checksum 无效、时间畸形或非固定解记录保留，但不进入求解。
|
||||
|
||||
## 新旋转链路
|
||||
|
||||
### R1b：基线在 IMU 中的 2DoF 方向
|
||||
|
||||
对连续 Q4 GNHPR 基线和 HI13 陀螺相对旋转使用不变量：
|
||||
|
||||
angle(b_ENU(t0), b_ENU(t1))
|
||||
= angle(b_IMU, DeltaR_IMU b_IMU)
|
||||
|
||||
估计 b_IMU 的两个倾斜自由度及逐会话陀螺零偏。安装信息只用于选择 +X
|
||||
半球，并施加明确记录的弱 20° 先验。按会话等权，输出残差、协方差和信息
|
||||
奇异值。
|
||||
|
||||
### R2V：基线 + Doppler velocity
|
||||
|
||||
筛选条件：
|
||||
|
||||
- BESTNAVA position 为 SOL_COMPUTED/NARROW_INT；
|
||||
- velocity 为 SOL_COMPUTED/DOPPLER_VELOCITY；
|
||||
- checksum 有效；
|
||||
- 水平速度 ≥1.5 m/s、速度标准差 ≤0.25 m/s；
|
||||
- |IMU gyro_z| ≤3°/s；
|
||||
- 速度方向与横向基线接近正交；
|
||||
- GNHPR Q4，并有时间邻近的合法 HI13 quaternion。
|
||||
|
||||
基线给车右，Doppler velocity 给车前，叉积给车上。HI13 quaternion 的
|
||||
body/world 和 ENU/NED 候选全部评分，只保留残差最小者。该方法会把 HI13
|
||||
导航 yaw 偏差带入候选，因此必须和 R2G 交叉验证。
|
||||
|
||||
### R2G：基线 + 水平静止重力
|
||||
|
||||
只允许调用方明确标记的水平静止会话。本次使用：
|
||||
|
||||
- 0819_20260819_072130（flat_static_hdg207）
|
||||
- 0819_20260819_073045（flat_static_hdg082）
|
||||
|
||||
每 10 s 分块，要求 gyro norm ≤0.35°/s，且加速度模长距标准重力不超过
|
||||
0.15 m/s²。R1b 提供车右，加速度中值提供车上，叉积得到车前。
|
||||
|
||||
### R3：稳定性与正式门禁
|
||||
|
||||
R2V/R2G 都输出：
|
||||
|
||||
- 样本和会话数量；
|
||||
- SO(3) RMS/P95；
|
||||
- 旋转向量样本标准差和均值协方差；
|
||||
- leave-one-session；
|
||||
- 10 样本或 10 s block-out。
|
||||
|
||||
只有 R1b 可观、R2V 和 R2G 各自稳定、二者差异 ≤2° 时，才将
|
||||
translation_unlocked 设为 true。旧 GNHPR 三轴手眼不参与正式门禁。
|
||||
|
||||
## 首轮实测
|
||||
|
||||
R1b：
|
||||
|
||||
b_IMU = [0.999999976, -0.000213959, -0.000044368]
|
||||
tilt_yz = [-0.0123, -0.0025] deg
|
||||
pairs = 2091
|
||||
RMS / P95 = 0.8086 / 1.7105 deg
|
||||
information singular values = [1.176e-2, 8.476e-4]
|
||||
std = [27.77, 7.47] deg
|
||||
gate = failed
|
||||
|
||||
点估计接近 +X 是安装先验与名义轴一致的结果；巨大协方差说明不能宣称
|
||||
数据独立估出了这两个小角。
|
||||
|
||||
R2V：
|
||||
|
||||
qualifying samples / sessions = 41 / 2
|
||||
selected quaternion convention = HI13_q_body_to_ENU
|
||||
RPY = [0.7532, 0.0708, -9.2944] deg
|
||||
RMS / P95 = 1.8436 / 3.4975 deg
|
||||
max leave-one-session = 3.4519 deg
|
||||
gate = failed
|
||||
|
||||
R2G：
|
||||
|
||||
level-static blocks / sessions = 53 / 2
|
||||
RPY = [0.4543, -0.0026, 0.0122] deg
|
||||
RMS / P95 = 0.2542 / 0.2732 deg
|
||||
max leave-one-session = 0.2688 deg
|
||||
gate = passed (level/gravity-prior constrained)
|
||||
|
||||
R2V 与 R2G 的 SO(3) 差异为 9.3119°。R3 最终：
|
||||
|
||||
full_rotation_accepted = false
|
||||
translation_unlocked = false
|
||||
|
||||
## 下一轮数据要求
|
||||
|
||||
1. BESTNAVA 改为至少 10 Hz，并确认 Doppler velocity 与 GNHPR 使用同一
|
||||
GNSS week/TOW 输出周期。
|
||||
2. 每个日期都录制多段 ≥3 m/s、持续 20–30 s 的正向直线；包含不同方位，
|
||||
避免单一磁环境和单一会话支配。
|
||||
3. 为 R1b 增加可控的 roll/pitch 激励；只有平面 yaw 无法稳定估出横向
|
||||
基线的两个微小倾斜角。
|
||||
4. 每个日期至少录两种车头方位的明确水平静止段，检验 HI13 重力和绝对
|
||||
quaternion 的跨日期稳定性。
|
||||
5. 若 HI13 quaternion 的 yaw 来自磁融合，应获取其导航坐标定义、磁偏角
|
||||
设置和融合状态；否则 R2V 只使用其 roll/pitch，yaw 由 GNSS 基线和速度
|
||||
决定。
|
||||
6. 上述门禁通过前继续冻结杆臂和平移。
|
||||
|
||||
## 实现入口
|
||||
|
||||
- tools/rscap_v2/g90_rtk.py：GGA/GNHPR/BESTNAVA/PVTSLNA 和双 checksum。
|
||||
- tools/rscap_v2/hi13_imu.py：HI91 system_time、惯性、姿态和四元数。
|
||||
- tools/export_rtk_imu_unified.py：55 组配对、原生异步导出和断点续导。
|
||||
- imu_lidar/rtk_imu_multisource.py：R1b/R2V/R2G/R3。
|
||||
- tools/run_rtk_imu_multisource.py：多源旋转命令行入口。
|
||||
- artifacts/rtk_imu_calibration_v3/multisource_result.json：首轮 R3 结果。
|
||||
@@ -1,4 +1,4 @@
|
||||
"""GNHPR attitude conventions and SO(3) interpolation."""
|
||||
"""GNHPR dual-antenna baseline conventions and SO(3) interpolation."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
@@ -9,12 +9,14 @@ from scipy.spatial.transform import Rotation, Slerp
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GnhprConvention:
|
||||
"""Interpretation of GNHPR angles as ``R_ENU_RTK``.
|
||||
"""Interpretation of the GNHPR ANT1-to-ANT2 baseline.
|
||||
|
||||
Heading is normally clockwise from north. With ENU and an x-forward RTK
|
||||
frame this becomes yaw ``90 deg - heading``. Aircraft-positive pitch is
|
||||
nose-up, which is the negative mathematical Y rotation in an FLU frame.
|
||||
Alternative signs are retained for empirical protocol validation.
|
||||
Heading is clockwise from north. In this vehicle the RTK ``+X`` axis is
|
||||
the baseline from the main/left antenna (ANT1) to the secondary/right
|
||||
antenna (ANT2), so it points to vehicle right rather than vehicle forward.
|
||||
GNHPR pitch is the elevation of that baseline. A two-antenna receiver
|
||||
cannot observe rotation about the baseline; the reported roll is therefore
|
||||
not treated as a third independent attitude measurement.
|
||||
"""
|
||||
|
||||
name: str
|
||||
@@ -23,7 +25,7 @@ class GnhprConvention:
|
||||
roll_sign: float = 1.0
|
||||
|
||||
|
||||
EXPECTED_GNHPR = GnhprConvention("north_cw__pitch_nose_up__roll_right_down")
|
||||
EXPECTED_GNHPR = GnhprConvention("ant1_to_ant2__north_cw__elevation_up")
|
||||
GNHPR_CANDIDATES = (
|
||||
EXPECTED_GNHPR,
|
||||
GnhprConvention("north_cw__pitch_opposite", -1.0, 1.0, 1.0),
|
||||
@@ -38,7 +40,12 @@ def gnhpr_to_rotation_enu_rtk(
|
||||
roll_deg: np.ndarray,
|
||||
convention: GnhprConvention = EXPECTED_GNHPR,
|
||||
) -> np.ndarray:
|
||||
"""Build body-to-ENU matrices with an extrinsic Z-Y-X Euler sequence."""
|
||||
"""Build a zero-roll mathematical completion of the baseline frame.
|
||||
|
||||
Only the first column (the ANT1-to-ANT2 unit vector) is physically observed
|
||||
by GNHPR. The remaining columns are a convenient gauge completion and must
|
||||
not be used as a measured full vehicle attitude.
|
||||
"""
|
||||
|
||||
heading = np.asarray(heading_deg, dtype=float).reshape(-1)
|
||||
pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
|
||||
@@ -47,11 +54,36 @@ def gnhpr_to_rotation_enu_rtk(
|
||||
raise ValueError("heading, pitch and roll must have equal length")
|
||||
yaw_rad = np.deg2rad(90.0 + convention.heading_sign * heading)
|
||||
pitch_rad = np.deg2rad(convention.pitch_sign * pitch)
|
||||
roll_rad = np.deg2rad(convention.roll_sign * roll)
|
||||
# A dual-antenna baseline has no independent roll observation. Keep the
|
||||
# argument for wire-format compatibility, but never inject it into SO(3).
|
||||
roll_rad = np.zeros_like(roll)
|
||||
angles = np.column_stack([yaw_rad, pitch_rad, roll_rad])
|
||||
return Rotation.from_euler('ZYX', angles).as_matrix()
|
||||
|
||||
|
||||
def gnhpr_to_baseline_enu(
|
||||
heading_deg: np.ndarray,
|
||||
pitch_deg: np.ndarray,
|
||||
convention: GnhprConvention = EXPECTED_GNHPR,
|
||||
) -> np.ndarray:
|
||||
"""Return ANT1-to-ANT2 unit vectors expressed in ENU.
|
||||
|
||||
The result is independent of the unobservable GNHPR roll field.
|
||||
"""
|
||||
|
||||
heading = np.asarray(heading_deg, dtype=float).reshape(-1)
|
||||
pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
|
||||
if heading.size != pitch.size:
|
||||
raise ValueError("heading and pitch must have equal length")
|
||||
azimuth = np.deg2rad(heading)
|
||||
elevation = np.deg2rad(-convention.pitch_sign * pitch)
|
||||
horizontal = np.cos(elevation)
|
||||
east = np.sin(azimuth) * horizontal
|
||||
north = np.cos(azimuth) * horizontal
|
||||
up = np.sin(elevation)
|
||||
return np.column_stack([east, north, up])
|
||||
|
||||
|
||||
def interpolate_rotations(
|
||||
source_t_s: np.ndarray,
|
||||
rotations: np.ndarray,
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,558 @@
|
||||
"""Observable RTK--IMU rotation stages using native asynchronous measurements."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
from dataclasses import asdict, dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from .contracts import ImuSeries
|
||||
from .geometry import so3_exp, so3_log
|
||||
from .imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
|
||||
from .rtk_attitude import gnhpr_to_baseline_enu
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class UnifiedSession:
|
||||
session_id: str
|
||||
batch_id: str
|
||||
imu: ImuSeries
|
||||
imu_rpy_deg: np.ndarray
|
||||
imu_quaternion_wxyz: np.ndarray
|
||||
imu_host_receive_utc_s: np.ndarray
|
||||
rtk_by_type: dict[str, list[dict[str, str]]]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class R1bResult:
|
||||
baseline_axis_imu: np.ndarray
|
||||
tilt_yz_deg: np.ndarray
|
||||
pair_count: int
|
||||
residual_rms_deg: float
|
||||
residual_p95_deg: float
|
||||
covariance_deg2: np.ndarray
|
||||
std_deg: np.ndarray
|
||||
information_singular_values: np.ndarray
|
||||
per_session_rms_deg: dict[str, float]
|
||||
gyro_bias_by_session_rad_s: dict[str, np.ndarray]
|
||||
ok: bool
|
||||
notes: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CompletedRotationResult:
|
||||
method: str
|
||||
R_RTK_IMU: np.ndarray | None
|
||||
rpy_deg: np.ndarray | None
|
||||
sample_count: int
|
||||
session_count: int
|
||||
residual_rms_deg: float
|
||||
residual_p95_deg: float
|
||||
covariance_deg2: np.ndarray
|
||||
std_deg: np.ndarray
|
||||
per_session_rms_deg: dict[str, float]
|
||||
leave_one_session_delta_deg: dict[str, float]
|
||||
block_out_delta_deg: dict[str, float]
|
||||
convention: str
|
||||
ok: bool
|
||||
notes: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class R3Result:
|
||||
r1b: R1bResult
|
||||
r2v: CompletedRotationResult
|
||||
r2g: CompletedRotationResult
|
||||
r2v_r2g_delta_deg: float
|
||||
full_rotation_accepted: bool
|
||||
translation_unlocked: bool
|
||||
blockers: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _BaselinePair:
|
||||
session_index: int
|
||||
session_id: str
|
||||
world_angle_rad: float
|
||||
delta_R: np.ndarray
|
||||
J_bg: np.ndarray
|
||||
|
||||
|
||||
def _f(row: dict[str, str], key: str, default: float = np.nan) -> float:
|
||||
try:
|
||||
return float(row.get(key, ""))
|
||||
except (TypeError, ValueError):
|
||||
return default
|
||||
|
||||
|
||||
def _truth(row: dict[str, str], key: str) -> bool:
|
||||
return str(row.get(key, "")).strip().lower() in {"1", "true", "yes"}
|
||||
|
||||
|
||||
def load_unified_sessions(
|
||||
manifest_path: Path | str,
|
||||
*,
|
||||
selected_session_ids: set[str] | None = None,
|
||||
) -> list[UnifiedSession]:
|
||||
manifest_source = Path(manifest_path)
|
||||
manifest = json.loads(manifest_source.read_text(encoding="utf-8"))
|
||||
sessions: list[UnifiedSession] = []
|
||||
for entry in manifest["sessions"]:
|
||||
session_id = str(entry["session_id"])
|
||||
if selected_session_ids and session_id not in selected_session_ids:
|
||||
continue
|
||||
directory = Path(entry["directory"])
|
||||
if not directory.is_absolute():
|
||||
directory = manifest_source.parent / directory
|
||||
with np.load(directory / "imu.npz") as payload:
|
||||
t = np.asarray(payload["system_time_s"], dtype=float)
|
||||
gyro = np.asarray(payload["gyro_rad_s"], dtype=float)
|
||||
accel = np.asarray(payload["accel_m_s2"], dtype=float)
|
||||
rpy = np.asarray(payload["rpy_deg"], dtype=float)
|
||||
quaternion = np.asarray(payload["quaternion_wxyz"], dtype=float)
|
||||
host = np.asarray(payload["host_receive_utc_s"], dtype=float)
|
||||
by_type: dict[str, list[dict[str, str]]] = {}
|
||||
with (directory / "rtk.csv").open("r", encoding="utf-8", newline="") as stream:
|
||||
for row in csv.DictReader(stream):
|
||||
by_type.setdefault(row["message_type"], []).append(row)
|
||||
sessions.append(
|
||||
UnifiedSession(
|
||||
session_id=session_id,
|
||||
batch_id=str(entry["batch_id"]),
|
||||
imu=ImuSeries(t_s=t, gyro_rad_s=gyro, acc_m_s2=accel),
|
||||
imu_rpy_deg=rpy,
|
||||
imu_quaternion_wxyz=quaternion,
|
||||
imu_host_receive_utc_s=host,
|
||||
rtk_by_type=by_type,
|
||||
)
|
||||
)
|
||||
return sessions
|
||||
|
||||
|
||||
def _valid_hpr(session: UnifiedSession) -> tuple[np.ndarray, np.ndarray]:
|
||||
rows = [
|
||||
row for row in session.rtk_by_type.get("GNHPR", [])
|
||||
if _truth(row, "checksum_valid") and int(_f(row, "heading_quality", -1)) == 4
|
||||
]
|
||||
if not rows:
|
||||
return np.zeros(0), np.zeros((0, 3))
|
||||
t = np.asarray([_f(row, "t_device_s") for row in rows])
|
||||
baseline = gnhpr_to_baseline_enu(
|
||||
np.asarray([_f(row, "heading_deg") for row in rows]),
|
||||
np.asarray([_f(row, "pitch_deg") for row in rows]),
|
||||
)
|
||||
finite = np.isfinite(t) & np.all(np.isfinite(baseline), axis=1)
|
||||
t, baseline = t[finite], baseline[finite]
|
||||
order = np.argsort(t)
|
||||
t, baseline = t[order], baseline[order]
|
||||
unique, indices = np.unique(t, return_index=True)
|
||||
return unique, baseline[indices]
|
||||
|
||||
|
||||
def _baseline_pairs(sessions: list[UnifiedSession]) -> list[_BaselinePair]:
|
||||
pairs: list[_BaselinePair] = []
|
||||
for session_index, session in enumerate(sessions):
|
||||
t, baseline = _valid_hpr(session)
|
||||
if t.size < 3:
|
||||
continue
|
||||
dt = np.diff(t)
|
||||
jump = np.degrees(
|
||||
np.arccos(np.clip(np.sum(baseline[:-1] * baseline[1:], axis=1), -1.0, 1.0))
|
||||
)
|
||||
continuous = (dt >= 0.03) & (dt <= 0.25) & (jump / np.maximum(dt, 1e-6) <= 45.0)
|
||||
last_anchor = -np.inf
|
||||
for index, t0 in enumerate(t[:-1]):
|
||||
if t0 - last_anchor < 1.0:
|
||||
continue
|
||||
last_anchor = t0
|
||||
for duration in (0.75, 1.5, 3.0):
|
||||
target = t0 + duration
|
||||
end = int(np.searchsorted(t, target))
|
||||
candidates = [candidate for candidate in (end - 1, end) if index < candidate < t.size]
|
||||
if not candidates:
|
||||
continue
|
||||
j = min(candidates, key=lambda candidate: abs(t[candidate] - target))
|
||||
if abs((t[j] - t0) - duration) > 0.12 or not np.all(continuous[index:j]):
|
||||
continue
|
||||
try:
|
||||
pre = preintegrate_gyro(
|
||||
session.imu.t_s, session.imu.gyro_rad_s, float(t0), float(t[j])
|
||||
)
|
||||
except ValueError:
|
||||
continue
|
||||
world_angle = float(
|
||||
np.arccos(np.clip(np.dot(baseline[index], baseline[j]), -1.0, 1.0))
|
||||
)
|
||||
if np.degrees(world_angle) < 0.4:
|
||||
continue
|
||||
pairs.append(
|
||||
_BaselinePair(
|
||||
session_index=session_index,
|
||||
session_id=session.session_id,
|
||||
world_angle_rad=world_angle,
|
||||
delta_R=pre.delta_R,
|
||||
J_bg=pre.J_bg,
|
||||
)
|
||||
)
|
||||
return pairs
|
||||
|
||||
|
||||
def _axis_from_parameters(parameters: np.ndarray) -> np.ndarray:
|
||||
axis = np.asarray([1.0, parameters[0], parameters[1]], dtype=float)
|
||||
return axis / np.linalg.norm(axis)
|
||||
|
||||
|
||||
def solve_r1b(sessions: list[UnifiedSession]) -> R1bResult:
|
||||
pairs = _baseline_pairs(sessions)
|
||||
if len(pairs) < 20:
|
||||
raise ValueError("R1b needs at least 20 continuous baseline/gyro motion pairs")
|
||||
session_count = len(sessions)
|
||||
pair_counts = {
|
||||
session.session_id: sum(pair.session_id == session.session_id for pair in pairs)
|
||||
for session in sessions
|
||||
}
|
||||
active_sessions = sum(count > 0 for count in pair_counts.values())
|
||||
target_count = len(pairs) / max(active_sessions, 1)
|
||||
|
||||
def residual(parameters: np.ndarray) -> np.ndarray:
|
||||
axis = _axis_from_parameters(parameters[:2])
|
||||
biases = parameters[2:].reshape(session_count, 3)
|
||||
values = []
|
||||
for pair in pairs:
|
||||
corrected = apply_bias_jacobian_correction(
|
||||
pair.delta_R, pair.J_bg, biases[pair.session_index]
|
||||
)
|
||||
body_angle = np.arccos(
|
||||
np.clip(np.dot(axis, corrected @ axis), -1.0, 1.0)
|
||||
)
|
||||
weight = np.sqrt(target_count / pair_counts[pair.session_id])
|
||||
values.append(weight * (body_angle - pair.world_angle_rad))
|
||||
values.extend((biases / 0.01).reshape(-1))
|
||||
# Weak 20-degree installation prior selects the physically known +X hemisphere.
|
||||
values.extend(np.asarray(parameters[:2]) / np.tan(np.deg2rad(20.0)))
|
||||
return np.asarray(values)
|
||||
|
||||
initial = np.zeros(2 + 3 * session_count)
|
||||
optimum = least_squares(
|
||||
residual, initial, loss="huber", f_scale=np.deg2rad(0.25), max_nfev=120
|
||||
)
|
||||
axis = _axis_from_parameters(optimum.x[:2])
|
||||
biases = optimum.x[2:].reshape(session_count, 3)
|
||||
errors = []
|
||||
per_session_values: dict[str, list[float]] = {}
|
||||
for pair in pairs:
|
||||
corrected = apply_bias_jacobian_correction(
|
||||
pair.delta_R, pair.J_bg, biases[pair.session_index]
|
||||
)
|
||||
body_angle = np.arccos(np.clip(np.dot(axis, corrected @ axis), -1.0, 1.0))
|
||||
error = float(np.degrees(body_angle - pair.world_angle_rad))
|
||||
errors.append(error)
|
||||
per_session_values.setdefault(pair.session_id, []).append(error)
|
||||
errors_array = np.asarray(errors)
|
||||
data_rows = len(pairs)
|
||||
jacobian = optimum.jac[:data_rows, :2]
|
||||
information = jacobian.T @ jacobian
|
||||
residual_variance = float(np.mean(np.deg2rad(errors_array) ** 2))
|
||||
covariance = residual_variance * np.linalg.pinv(information, rcond=1e-12)
|
||||
covariance_deg2 = np.degrees(1.0) ** 2 * covariance
|
||||
std_deg = np.sqrt(np.maximum(np.diag(covariance_deg2), 0.0))
|
||||
singular = np.linalg.svd(information, compute_uv=False)
|
||||
rms = float(np.sqrt(np.mean(errors_array**2)))
|
||||
p95 = float(np.percentile(np.abs(errors_array), 95.0))
|
||||
return R1bResult(
|
||||
baseline_axis_imu=axis,
|
||||
tilt_yz_deg=np.degrees(np.arctan(optimum.x[:2])),
|
||||
pair_count=len(pairs),
|
||||
residual_rms_deg=rms,
|
||||
residual_p95_deg=p95,
|
||||
covariance_deg2=covariance_deg2,
|
||||
std_deg=std_deg,
|
||||
information_singular_values=singular,
|
||||
per_session_rms_deg={
|
||||
key: float(np.sqrt(np.mean(np.asarray(value) ** 2)))
|
||||
for key, value in per_session_values.items()
|
||||
},
|
||||
gyro_bias_by_session_rad_s={
|
||||
session.session_id: biases[index] for index, session in enumerate(sessions)
|
||||
},
|
||||
ok=bool(rms <= 1.0 and p95 <= 2.0 and np.min(singular) >= 1e-3),
|
||||
notes=(
|
||||
"2DoF ANT1-to-ANT2 direction; +X hemisphere selected by installation knowledge",
|
||||
"weak 20 deg prior is reported and prevents sign/gauge branch switching",
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def _rotation_mean(matrices: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
mean = Rotation.from_matrix(matrices).mean().as_matrix()
|
||||
errors = np.asarray(
|
||||
[np.degrees(np.linalg.norm(so3_log(mean.T @ matrix))) for matrix in matrices]
|
||||
)
|
||||
return mean, errors
|
||||
|
||||
|
||||
def _result_from_samples(
|
||||
method: str,
|
||||
samples: list[tuple[str, str, np.ndarray]],
|
||||
convention: str,
|
||||
notes: tuple[str, ...],
|
||||
) -> CompletedRotationResult:
|
||||
if not samples:
|
||||
return CompletedRotationResult(
|
||||
method, None, None, 0, 0, np.nan, np.nan, np.full((3, 3), np.nan),
|
||||
np.full(3, np.nan),
|
||||
{}, {}, {}, convention, False, notes + ("no qualifying samples",)
|
||||
)
|
||||
matrices = np.asarray([item[2] for item in samples])
|
||||
mean, errors = _rotation_mean(matrices)
|
||||
rotvec = np.asarray([so3_log(mean.T @ matrix) for matrix in matrices])
|
||||
rotvec_deg = np.degrees(rotvec)
|
||||
std = np.std(rotvec_deg, axis=0, ddof=1) if len(samples) > 1 else np.full(3, np.nan)
|
||||
covariance = (
|
||||
np.cov(rotvec_deg, rowvar=False, ddof=1) / len(samples)
|
||||
if len(samples) > 1 else np.full((3, 3), np.nan)
|
||||
)
|
||||
ids = sorted({item[0] for item in samples})
|
||||
per_session = {}
|
||||
loo = {}
|
||||
for session_id in ids:
|
||||
selected = [item[2] for item in samples if item[0] == session_id]
|
||||
_, local_errors = _rotation_mean(np.asarray(selected))
|
||||
per_session[session_id] = float(np.sqrt(np.mean(local_errors**2)))
|
||||
kept = np.asarray([item[2] for item in samples if item[0] != session_id])
|
||||
if kept.size:
|
||||
kept_mean, _ = _rotation_mean(kept)
|
||||
loo[session_id] = float(np.degrees(np.linalg.norm(so3_log(mean.T @ kept_mean))))
|
||||
block_groups = sorted({(item[0], item[1]) for item in samples})
|
||||
block_out = {}
|
||||
for session_id, block_id in block_groups:
|
||||
kept = np.asarray(
|
||||
[item[2] for item in samples if (item[0], item[1]) != (session_id, block_id)]
|
||||
)
|
||||
if kept.size:
|
||||
kept_mean, _ = _rotation_mean(kept)
|
||||
block_out[f"{session_id}:{block_id}"] = float(
|
||||
np.degrees(np.linalg.norm(so3_log(mean.T @ kept_mean)))
|
||||
)
|
||||
rms = float(np.sqrt(np.mean(errors**2)))
|
||||
p95 = float(np.percentile(errors, 95.0))
|
||||
max_loo = max(loo.values(), default=np.inf)
|
||||
ok = bool(
|
||||
len(samples) >= 10 and len(ids) >= 2 and rms <= 2.0 and p95 <= 3.0
|
||||
and np.nanmax(std) <= 1.0 and max_loo <= 1.0
|
||||
)
|
||||
return CompletedRotationResult(
|
||||
method=method,
|
||||
R_RTK_IMU=mean,
|
||||
rpy_deg=Rotation.from_matrix(mean).as_euler("xyz", degrees=True),
|
||||
sample_count=len(samples),
|
||||
session_count=len(ids),
|
||||
residual_rms_deg=rms,
|
||||
residual_p95_deg=p95,
|
||||
covariance_deg2=covariance,
|
||||
std_deg=std,
|
||||
per_session_rms_deg=per_session,
|
||||
leave_one_session_delta_deg=loo,
|
||||
block_out_delta_deg=block_out,
|
||||
convention=convention,
|
||||
ok=ok,
|
||||
notes=notes,
|
||||
)
|
||||
|
||||
|
||||
def solve_r2g(
|
||||
sessions: list[UnifiedSession],
|
||||
r1b: R1bResult,
|
||||
*,
|
||||
level_static_session_ids: set[str],
|
||||
block_duration_s: float = 10.0,
|
||||
) -> CompletedRotationResult:
|
||||
samples: list[tuple[str, str, np.ndarray]] = []
|
||||
right = r1b.baseline_axis_imu
|
||||
for session in sessions:
|
||||
if session.session_id not in level_static_session_ids:
|
||||
continue
|
||||
gyro_norm = np.linalg.norm(session.imu.gyro_rad_s, axis=1)
|
||||
accel_norm = np.linalg.norm(session.imu.acc_m_s2, axis=1)
|
||||
valid = (gyro_norm <= np.deg2rad(0.35)) & (np.abs(accel_norm - 9.80665) <= 0.15)
|
||||
block = np.floor(
|
||||
(session.imu.t_s - session.imu.t_s[0]) / block_duration_s
|
||||
).astype(int)
|
||||
for block_id in np.unique(block[valid]):
|
||||
selected = valid & (block == block_id)
|
||||
if np.count_nonzero(selected) < 200:
|
||||
continue
|
||||
up = np.median(session.imu.acc_m_s2[selected], axis=0)
|
||||
up /= np.linalg.norm(up)
|
||||
up -= right * np.dot(up, right)
|
||||
if np.linalg.norm(up) < 0.9:
|
||||
continue
|
||||
up /= np.linalg.norm(up)
|
||||
forward = np.cross(up, right)
|
||||
forward /= np.linalg.norm(forward)
|
||||
C_IMU_RTK = np.column_stack([right, forward, up])
|
||||
samples.append((session.session_id, str(int(block_id)), C_IMU_RTK.T))
|
||||
return _result_from_samples(
|
||||
"R2G_baseline_plus_level_gravity",
|
||||
samples,
|
||||
"accelerometer specific-force points vehicle up on explicit level-static blocks",
|
||||
(
|
||||
"only caller-declared level-static sessions are eligible",
|
||||
"result is level/gravity-prior constrained, not dual-antenna-only",
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def _nearest_index(t: np.ndarray, value: float, tolerance: float) -> int | None:
|
||||
index = int(np.searchsorted(t, value))
|
||||
candidates = [item for item in (index - 1, index) if 0 <= item < t.size]
|
||||
if not candidates:
|
||||
return None
|
||||
best = min(candidates, key=lambda item: abs(t[item] - value))
|
||||
return best if abs(t[best] - value) <= tolerance else None
|
||||
|
||||
|
||||
def solve_r2v(
|
||||
sessions: list[UnifiedSession],
|
||||
*,
|
||||
min_speed_m_s: float = 1.5,
|
||||
max_yaw_rate_deg_s: float = 3.0,
|
||||
max_baseline_course_error_deg: float = 15.0,
|
||||
) -> CompletedRotationResult:
|
||||
candidates: dict[str, list[tuple[str, str, np.ndarray]]] = {
|
||||
"HI13_q_body_to_ENU": [],
|
||||
"NED_to_ENU_times_HI13_q": [],
|
||||
"HI13_q_inverse_as_body_to_ENU": [],
|
||||
"NED_to_ENU_times_HI13_q_inverse": [],
|
||||
}
|
||||
NED_TO_ENU = np.asarray([[0.0, 1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, -1.0]])
|
||||
for session in sessions:
|
||||
hpr_t, hpr_baseline = _valid_hpr(session)
|
||||
if hpr_t.size == 0:
|
||||
continue
|
||||
quaternion = session.imu_quaternion_wxyz
|
||||
norm = np.linalg.norm(quaternion, axis=1)
|
||||
valid_quaternion = np.isfinite(norm) & (np.abs(norm - 1.0) <= 0.02)
|
||||
normalized = quaternion / np.maximum(norm[:, None], 1e-12)
|
||||
imu_rotations = Rotation.from_quat(normalized[:, [1, 2, 3, 0]]).as_matrix()
|
||||
for row_index, row in enumerate(session.rtk_by_type.get("BESTNAVA", [])):
|
||||
if not (
|
||||
_truth(row, "checksum_valid")
|
||||
and _truth(row, "position_fixed")
|
||||
and _truth(row, "doppler_velocity_valid")
|
||||
and _f(row, "horizontal_speed_m_s") >= min_speed_m_s
|
||||
and _f(row, "horizontal_speed_std_m_s") <= 0.25
|
||||
):
|
||||
continue
|
||||
t = _f(row, "t_device_s")
|
||||
hpr_index = _nearest_index(hpr_t, t, 0.15)
|
||||
imu_index = _nearest_index(session.imu.t_s, t, 0.03)
|
||||
if hpr_index is None or imu_index is None or not valid_quaternion[imu_index]:
|
||||
continue
|
||||
if abs(np.degrees(session.imu.gyro_rad_s[imu_index, 2])) > max_yaw_rate_deg_s:
|
||||
continue
|
||||
right = hpr_baseline[hpr_index]
|
||||
forward = np.asarray(
|
||||
[_f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"), 0.0]
|
||||
)
|
||||
forward /= np.linalg.norm(forward)
|
||||
course_error = np.degrees(
|
||||
np.arcsin(np.clip(abs(np.dot(right, forward)), 0.0, 1.0))
|
||||
)
|
||||
if course_error > max_baseline_course_error_deg:
|
||||
continue
|
||||
forward -= right * np.dot(right, forward)
|
||||
forward /= np.linalg.norm(forward)
|
||||
up = np.cross(right, forward)
|
||||
if up[2] < 0:
|
||||
forward = -forward
|
||||
up = -up
|
||||
up /= np.linalg.norm(up)
|
||||
R_ENU_RTK = np.column_stack([right, forward, up])
|
||||
q = imu_rotations[imu_index]
|
||||
world_candidates = {
|
||||
"HI13_q_body_to_ENU": q,
|
||||
"NED_to_ENU_times_HI13_q": NED_TO_ENU @ q,
|
||||
"HI13_q_inverse_as_body_to_ENU": q.T,
|
||||
"NED_to_ENU_times_HI13_q_inverse": NED_TO_ENU @ q.T,
|
||||
}
|
||||
block_id = str(row_index // 10)
|
||||
for name, R_ENU_IMU in world_candidates.items():
|
||||
candidates[name].append(
|
||||
(session.session_id, block_id, R_ENU_RTK.T @ R_ENU_IMU)
|
||||
)
|
||||
diagnostics = {
|
||||
name: _result_from_samples(
|
||||
"R2V_baseline_plus_doppler_velocity",
|
||||
values,
|
||||
name,
|
||||
(
|
||||
"RTK fixed + Doppler velocity + speed + low-yaw + baseline/course gates",
|
||||
"HI13 absolute quaternion may contain magnetic/navigation yaw bias",
|
||||
),
|
||||
)
|
||||
for name, values in candidates.items()
|
||||
}
|
||||
finite = [result for result in diagnostics.values() if result.sample_count]
|
||||
if not finite:
|
||||
return diagnostics["HI13_q_body_to_ENU"]
|
||||
return min(finite, key=lambda result: result.residual_rms_deg)
|
||||
|
||||
|
||||
def solve_r3(
|
||||
sessions: list[UnifiedSession],
|
||||
*,
|
||||
level_static_session_ids: set[str],
|
||||
) -> R3Result:
|
||||
dynamic_sessions = [
|
||||
session for session in sessions if session.session_id not in level_static_session_ids
|
||||
]
|
||||
r1b = solve_r1b(dynamic_sessions)
|
||||
r2v = solve_r2v(dynamic_sessions)
|
||||
r2g = solve_r2g(sessions, r1b, level_static_session_ids=level_static_session_ids)
|
||||
if r2v.R_RTK_IMU is None or r2g.R_RTK_IMU is None:
|
||||
delta = np.nan
|
||||
else:
|
||||
delta = float(
|
||||
np.degrees(np.linalg.norm(so3_log(r2v.R_RTK_IMU.T @ r2g.R_RTK_IMU)))
|
||||
)
|
||||
blockers = []
|
||||
if not r1b.ok:
|
||||
blockers.append("R1b baseline direction failed residual/observability gates")
|
||||
if not r2v.ok:
|
||||
blockers.append("R2V velocity-completed rotation failed stability gates")
|
||||
if not r2g.ok:
|
||||
blockers.append("R2G gravity-completed rotation failed stability gates")
|
||||
if not np.isfinite(delta) or delta > 2.0:
|
||||
blockers.append("R2V and R2G disagree by more than 2 deg")
|
||||
accepted = not blockers
|
||||
return R3Result(
|
||||
r1b=r1b,
|
||||
r2v=r2v,
|
||||
r2g=r2g,
|
||||
r2v_r2g_delta_deg=delta,
|
||||
full_rotation_accepted=accepted,
|
||||
translation_unlocked=accepted,
|
||||
blockers=tuple(blockers),
|
||||
)
|
||||
|
||||
|
||||
def result_to_jsonable(result: R3Result) -> dict:
|
||||
def convert(value):
|
||||
if isinstance(value, np.ndarray):
|
||||
return value.tolist()
|
||||
if isinstance(value, np.generic):
|
||||
return value.item()
|
||||
if hasattr(value, "__dataclass_fields__"):
|
||||
return {key: convert(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): convert(item) for key, item in value.items()}
|
||||
if isinstance(value, (tuple, list)):
|
||||
return [convert(item) for item in value]
|
||||
return value
|
||||
return convert(result)
|
||||
@@ -0,0 +1,529 @@
|
||||
'''Per-GNSS-node RTK/IMU state graph used after legacy propagation deprecation.'''
|
||||
from __future__ import annotations
|
||||
from dataclasses import dataclass
|
||||
import numpy as np
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.sparse import lil_matrix
|
||||
from .geometry import so3_exp, so3_log
|
||||
from .imu_preintegration import apply_bias_correction_imu, residual_whiten_matrix
|
||||
from .rtk_imu_engineering import (
|
||||
G_ENU, HPR_DIRECT_ANGULAR_SIGMA_RAD, _Segment, _world_rtk)
|
||||
|
||||
NODE_DOF = 15
|
||||
SIGMA_BG_RW = 1e-5
|
||||
SIGMA_BA_RW = 1e-3
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class NodeGraphProblem:
|
||||
segment: _Segment
|
||||
R_seed_WI: tuple[np.ndarray,...]
|
||||
fixed_l_I_m: np.ndarray
|
||||
R_RTK_IMU: np.ndarray
|
||||
hpr_direct_angular_sigma_rad: float = HPR_DIRECT_ANGULAR_SIGMA_RAD
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class NodeGraphResult:
|
||||
success: bool
|
||||
message: str
|
||||
node_count: int
|
||||
duration_s: float
|
||||
fixed_l_I_m: np.ndarray
|
||||
initial_cost: float
|
||||
final_cost: float
|
||||
cost_reduction: float
|
||||
nfev: int
|
||||
optimality: float
|
||||
gradient_norm: float
|
||||
residual_dimension: int
|
||||
state_dimension: int
|
||||
statistical_dof: int
|
||||
total_nis: float
|
||||
chi_square_per_dof: float
|
||||
cost_per_dof: float
|
||||
initial_residual_by_factor: dict[str,dict[str,float]]
|
||||
final_residual_by_factor: dict[str,dict[str,float]]
|
||||
final_position_residual_m: dict[str,object]
|
||||
final_velocity_residual_m_s: dict[str,object]
|
||||
max_bg_step_rad_s: float
|
||||
max_ba_step_m_s2: float
|
||||
preintegration_covariance_sigma: dict[str,dict[str,float]]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FreeLeverResult:
|
||||
success: bool
|
||||
message: str
|
||||
initial_l_I_m: np.ndarray
|
||||
final_l_I_m: np.ndarray
|
||||
lever_step_norm_m: float
|
||||
initial_cost: float
|
||||
final_cost: float
|
||||
nfev: int
|
||||
optimality: float
|
||||
chi_square_per_dof: float
|
||||
position_residual_m: dict[str,object]
|
||||
velocity_residual_m_s: dict[str,object]
|
||||
residual_by_factor: dict[str,dict[str,float]]
|
||||
lever_covariance_m2: np.ndarray
|
||||
lever_information_singular_values: np.ndarray
|
||||
lever_information_condition_number: float
|
||||
lever_precision_rank: int
|
||||
weakest_lever_direction_I: np.ndarray
|
||||
|
||||
def _rotation(problem,index,x):
|
||||
offset = NODE_DOF*index
|
||||
return problem.R_seed_WI[index] @ so3_exp(x[offset:offset+3])
|
||||
|
||||
def initial_parameters(problem, lever_override=None):
|
||||
nodes = problem.segment.nodes
|
||||
x = np.zeros(NODE_DOF*len(nodes))
|
||||
lever = (problem.fixed_l_I_m if lever_override is None
|
||||
else np.asarray(lever_override,dtype=float))
|
||||
previous_p, previous_v = np.zeros(3), np.zeros(3)
|
||||
for index,node in enumerate(nodes):
|
||||
offset = NODE_DOF*index
|
||||
R = problem.R_seed_WI[index]
|
||||
previous_p = node.p_enu_m-R@lever
|
||||
if index and not node.position_mask[2]:
|
||||
previous_p[2] = x[offset-NODE_DOF+5]
|
||||
if node.velocity_enu_m_s is not None:
|
||||
previous_v = node.velocity_enu_m_s-R@np.cross(node.gyro_rad_s,lever)
|
||||
x[offset+3:offset+6] = previous_p
|
||||
x[offset+6:offset+9] = previous_v
|
||||
return x
|
||||
|
||||
def _stats(values, effective_dof=None):
|
||||
a = np.asarray(values,dtype=float).reshape(-1)
|
||||
if not a.size:
|
||||
return {'count':0,'dof':0,'rms':np.nan,'p50_abs':np.nan,
|
||||
'p95_abs':np.nan,'p99_abs':np.nan,'nis':np.nan,
|
||||
'chi_square_per_dof':np.nan}
|
||||
nis = float(np.dot(a,a))
|
||||
dof = int(a.size if effective_dof is None else effective_dof)
|
||||
return {'count':int(a.size),'dof':dof,
|
||||
'rms':float(np.sqrt(np.mean(a*a))),
|
||||
'p50_abs':float(np.percentile(np.abs(a),50.)),
|
||||
'p95_abs':float(np.percentile(np.abs(a),95.)),
|
||||
'p99_abs':float(np.percentile(np.abs(a),99.)),
|
||||
'nis':nis,'chi_square_per_dof':nis/max(dof,1)}
|
||||
|
||||
def _hpr_sigma(problem, node):
|
||||
old = float(node.hpr_angular_sigma_rad)
|
||||
extra_var = max(old*old-HPR_DIRECT_ANGULAR_SIGMA_RAD**2, 0.)
|
||||
return float(np.sqrt(problem.hpr_direct_angular_sigma_rad**2+extra_var))
|
||||
|
||||
|
||||
def _distribution(values):
|
||||
a = np.asarray(values,dtype=float).reshape(-1)
|
||||
if not a.size:
|
||||
return {'count':0,'rms':np.nan,'p50':np.nan,'p95':np.nan,'p99':np.nan}
|
||||
return {'count':len(a),'rms':float(np.sqrt(np.mean(a*a))),
|
||||
'p50':float(np.percentile(a,50.)),'p95':float(np.percentile(a,95.)),
|
||||
'p99':float(np.percentile(a,99.))}
|
||||
|
||||
def _vector_stats(values):
|
||||
a = np.asarray(values,dtype=float).reshape(-1,3)
|
||||
if not a.size:
|
||||
return {'count':0,'axis_rms':[np.nan]*3,'axis_p95_abs':[np.nan]*3,
|
||||
'vector_rms':np.nan,'vector_p95':np.nan}
|
||||
norm = np.linalg.norm(a,axis=1)
|
||||
return {'count':len(a),'axis_rms':np.sqrt(np.mean(a*a,axis=0)),
|
||||
'axis_p95_abs':np.percentile(np.abs(a),95.,axis=0),
|
||||
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
|
||||
'vector_p95':float(np.percentile(norm,95.))}
|
||||
|
||||
def residual(problem,x,details=None,dependencies=None,lever_override=None):
|
||||
nodes, preints = problem.segment.nodes, problem.segment.preintegrations
|
||||
lever = (problem.fixed_l_I_m if lever_override is None
|
||||
else np.asarray(lever_override,dtype=float))
|
||||
baseline_I = problem.R_RTK_IMU.T[:,0]
|
||||
values = []
|
||||
def add(value,label,node_indices,raw=None):
|
||||
a = np.asarray(value,dtype=float).reshape(-1)
|
||||
values.extend(a)
|
||||
if dependencies is not None:
|
||||
dependencies.extend([tuple(node_indices)]*len(a))
|
||||
if details is not None:
|
||||
details.setdefault(label,[]).extend(a.tolist())
|
||||
if raw is not None: details.setdefault(label+'_physical',[]).append(np.asarray(raw))
|
||||
for index,node in enumerate(nodes):
|
||||
offset = NODE_DOF*index
|
||||
R = _rotation(problem,index,x)
|
||||
p, v = x[offset+3:offset+6], x[offset+6:offset+9]
|
||||
bg, ba = x[offset+9:offset+12], x[offset+12:offset+15]
|
||||
p_error = p+R@lever-node.p_enu_m
|
||||
if node.source == 'GGA':
|
||||
add(p_error[:2]/.06,'gga_xy',(index,))
|
||||
if details is not None: details.setdefault('position_physical',[]).append(
|
||||
np.array([p_error[0],p_error[1],np.nan]))
|
||||
else:
|
||||
add(p_error/np.array([.06,.06,.12]),'best_position',(index,),p_error)
|
||||
if details is not None: details.setdefault('position_physical',[]).append(p_error)
|
||||
if node.velocity_enu_m_s is not None:
|
||||
v_error = v+R@np.cross(node.gyro_rad_s-bg,lever)-node.velocity_enu_m_s
|
||||
add(v_error/np.array([.15,.15,.30]),'doppler',(index,),v_error)
|
||||
if details is not None: details.setdefault('velocity_physical',[]).append(v_error)
|
||||
if node.hpr_factor_valid:
|
||||
hpr_error=np.cross(R@baseline_I,node.baseline_enu)
|
||||
add(hpr_error/_hpr_sigma(problem,node),'hpr',(index,),hpr_error)
|
||||
if node.gravity_candidate:
|
||||
gravity = node.accel_m_s2-ba-R.T@(-G_ENU)
|
||||
add(gravity/.12,'gravity',(index,))
|
||||
if index == len(nodes)-1: continue
|
||||
right = index+1
|
||||
right_offset = NODE_DOF*right
|
||||
Rj = _rotation(problem,right,x)
|
||||
pj = x[right_offset+3:right_offset+6]
|
||||
vj = x[right_offset+6:right_offset+9]
|
||||
bgj = x[right_offset+9:right_offset+12]
|
||||
baj = x[right_offset+12:right_offset+15]
|
||||
pre = preints[index]
|
||||
dR,dv,dp = apply_bias_correction_imu(pre,bg,ba)
|
||||
dt = pre.duration_s
|
||||
imu_error = np.concatenate([
|
||||
so3_log(dR.T@R.T@Rj),
|
||||
R.T@(vj-v-G_ENU*dt)-dv,
|
||||
R.T@(pj-p-v*dt-.5*G_ENU*dt*dt)-dp])
|
||||
add(residual_whiten_matrix(pre.cov)@imu_error,'imu_preintegration',
|
||||
(index,right),imu_error)
|
||||
add((bgj-bg)/(SIGMA_BG_RW*np.sqrt(dt)),'gyro_bias_random_walk',(index,right))
|
||||
add((baj-ba)/(SIGMA_BA_RW*np.sqrt(dt)),'accel_bias_random_walk',(index,right))
|
||||
add(x[:3]/np.deg2rad(5.),'initial_attitude_gauge',(0,))
|
||||
add(x[9:12]/.02,'initial_gyro_bias',(0,))
|
||||
add(x[12:15]/.5,'initial_accel_bias',(0,))
|
||||
return np.asarray(values)
|
||||
|
||||
def jacobian_sparsity(problem,x):
|
||||
dependencies = []
|
||||
base = residual(problem,x,dependencies=dependencies)
|
||||
sparsity = lil_matrix((len(base),len(x)),dtype=int)
|
||||
for row,node_indices in enumerate(dependencies):
|
||||
for index in node_indices:
|
||||
start = NODE_DOF*index
|
||||
sparsity[row,start:start+NODE_DOF] = 1
|
||||
return sparsity.tocsr()
|
||||
|
||||
def _factor_stats(details):
|
||||
return {key:_stats(value,2*len(value)//3 if key=='hpr' else None)
|
||||
for key,value in details.items()
|
||||
if not key.endswith('_physical') and key not in ('position_physical','velocity_physical')}
|
||||
|
||||
|
||||
def _effective_residual_dimension(details):
|
||||
return sum(2*len(v)//3 if k=='hpr' else len(v)
|
||||
for k,v in details.items() if not k.endswith('_physical')
|
||||
and k not in ('position_physical','velocity_physical'))
|
||||
|
||||
def _preintegration_covariance_stats(problem):
|
||||
blocks = {'rotation_rad':[],'velocity_m_s':[],'position_m':[]}
|
||||
for pre in problem.segment.preintegrations:
|
||||
sigma = np.sqrt(np.maximum(np.diag(pre.cov),0.))
|
||||
blocks['rotation_rad'].extend(sigma[:3])
|
||||
blocks['velocity_m_s'].extend(sigma[3:6])
|
||||
blocks['position_m'].extend(sigma[6:9])
|
||||
return {key:_distribution(value) for key,value in blocks.items()}
|
||||
|
||||
def build_problem(segment,R_RTK_IMU,fixed_l_I_m,
|
||||
hpr_direct_angular_sigma_rad=HPR_DIRECT_ANGULAR_SIGMA_RAD):
|
||||
seeds = []
|
||||
for index,node in enumerate(segment.nodes):
|
||||
if node.hpr_factor_valid:
|
||||
seeds.append(_world_rtk(node.baseline_enu)@R_RTK_IMU)
|
||||
elif index:
|
||||
seeds.append(seeds[-1]@segment.preintegrations[index-1].delta_R)
|
||||
else:
|
||||
seeds.append(segment.R_WRTK_initial@R_RTK_IMU)
|
||||
return NodeGraphProblem(segment,tuple(seeds),np.asarray(fixed_l_I_m,dtype=float),
|
||||
np.asarray(R_RTK_IMU,dtype=float),
|
||||
float(hpr_direct_angular_sigma_rad))
|
||||
|
||||
def solve_fixed_lever(problem,max_nfev=30):
|
||||
x0 = initial_parameters(problem)
|
||||
initial_detail = {}
|
||||
r0 = residual(problem,x0,initial_detail)
|
||||
fit = least_squares(
|
||||
lambda value:residual(problem,value),x0,jac='2-point',
|
||||
jac_sparsity=jacobian_sparsity(problem,x0),method='trf',
|
||||
tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
|
||||
x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
final_detail = {}
|
||||
rf = residual(problem,fit.x,final_detail)
|
||||
statistical_dof = max(_effective_residual_dimension(final_detail)-len(fit.x),1)
|
||||
bg = fit.x.reshape(-1,NODE_DOF)[:,9:12]
|
||||
ba = fit.x.reshape(-1,NODE_DOF)[:,12:15]
|
||||
bg_step = np.diff(bg,axis=0)
|
||||
ba_step = np.diff(ba,axis=0)
|
||||
return NodeGraphResult(
|
||||
success=bool(fit.success),message=str(fit.message),
|
||||
node_count=len(problem.segment.nodes),
|
||||
duration_s=problem.segment.nodes[-1].t_s-problem.segment.nodes[0].t_s,
|
||||
fixed_l_I_m=problem.fixed_l_I_m.copy(),
|
||||
initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
|
||||
cost_reduction=.5*float(np.dot(r0,r0)-np.dot(rf,rf)),
|
||||
nfev=int(fit.nfev),optimality=float(fit.optimality),
|
||||
gradient_norm=float(np.linalg.norm(fit.grad)),
|
||||
residual_dimension=len(rf),state_dimension=len(fit.x),
|
||||
statistical_dof=statistical_dof,total_nis=float(np.dot(rf,rf)),
|
||||
chi_square_per_dof=float(np.dot(rf,rf)/statistical_dof),
|
||||
cost_per_dof=.5*float(np.dot(rf,rf)/statistical_dof),
|
||||
initial_residual_by_factor=_factor_stats(initial_detail),
|
||||
final_residual_by_factor=_factor_stats(final_detail),
|
||||
final_position_residual_m=_vector_stats(final_detail.get('position_physical',[])),
|
||||
final_velocity_residual_m_s=_vector_stats(final_detail.get('velocity_physical',[])),
|
||||
max_bg_step_rad_s=float(np.max(np.linalg.norm(bg_step,axis=1))) if len(bg_step) else 0.,
|
||||
max_ba_step_m_s2=float(np.max(np.linalg.norm(ba_step,axis=1))) if len(ba_step) else 0.,
|
||||
preintegration_covariance_sigma=_preintegration_covariance_stats(problem))
|
||||
|
||||
|
||||
def fit_states_at_fixed_lever(problem,l_I_m,max_nfev=50,initial_state_values=None):
|
||||
lever=np.asarray(l_I_m,dtype=float)
|
||||
x0=(initial_parameters(problem,lever) if initial_state_values is None
|
||||
else np.asarray(initial_state_values,dtype=float))
|
||||
r0=residual(problem,x0,lever_override=lever)
|
||||
fit=least_squares(
|
||||
lambda value:residual(problem,value,lever_override=lever),x0,jac='2-point',
|
||||
jac_sparsity=jacobian_sparsity(problem,x0),method='trf',tr_solver='lsmr',
|
||||
loss='linear',max_nfev=max_nfev,x_scale='jac',
|
||||
ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
return fit.x,{'success':bool(fit.success),'message':str(fit.message),
|
||||
'nfev':int(fit.nfev),'initial_cost':.5*float(r0@r0),
|
||||
'cost':float(fit.cost)}
|
||||
|
||||
|
||||
def summarize_fixed_state_values(problems,state_values,l_I_m):
|
||||
details={}; residuals=[]
|
||||
for problem,value in zip(problems,state_values):
|
||||
local={}
|
||||
residuals.append(residual(problem,np.asarray(value),details=local,
|
||||
lever_override=l_I_m))
|
||||
for key,items in local.items(): details.setdefault(key,[]).extend(items)
|
||||
joined=np.concatenate(residuals)
|
||||
state_dimension=sum(len(value) for value in state_values)
|
||||
dof=max(_effective_residual_dimension(details)-state_dimension,1)
|
||||
return {'cost':.5*float(np.dot(joined,joined)),
|
||||
'total_nis':float(np.dot(joined,joined)),
|
||||
'chi_square_per_dof':float(np.dot(joined,joined)/dof),
|
||||
'statistical_dof':dof,'residual_by_factor':_factor_stats(details),
|
||||
'best_position_physical_m':_vector_stats(details.get('best_position_physical',[])),
|
||||
'doppler_physical_m_s':_vector_stats(details.get('doppler_physical',[])),
|
||||
'hpr_physical_rad':_vector_stats(details.get('hpr_physical',[]))}
|
||||
|
||||
def solve_fixed_lever_many(problems,max_nfev=30):
|
||||
problems = tuple(problems)
|
||||
sizes = [NODE_DOF*len(problem.segment.nodes) for problem in problems]
|
||||
offsets = np.cumsum([0,*sizes])
|
||||
x0 = np.concatenate([initial_parameters(problem) for problem in problems])
|
||||
def evaluate(value,details=None):
|
||||
chunks = []
|
||||
for index,problem in enumerate(problems):
|
||||
local_details = {} if details is not None else None
|
||||
chunks.append(residual(problem,value[offsets[index]:offsets[index+1]],
|
||||
local_details))
|
||||
if details is not None:
|
||||
for key,items in local_details.items():
|
||||
details.setdefault(key,[]).extend(items)
|
||||
return np.concatenate(chunks)
|
||||
initial_detail = {}
|
||||
r0 = evaluate(x0,initial_detail)
|
||||
sparsity = lil_matrix((len(r0),len(x0)),dtype=int)
|
||||
row = 0
|
||||
for index,problem in enumerate(problems):
|
||||
local_x = x0[offsets[index]:offsets[index+1]]
|
||||
local = jacobian_sparsity(problem,local_x)
|
||||
sparsity[row:row+local.shape[0],offsets[index]:offsets[index+1]] = local
|
||||
row += local.shape[0]
|
||||
fit = least_squares(
|
||||
lambda value:evaluate(value),x0,jac='2-point',jac_sparsity=sparsity.tocsr(),
|
||||
method='trf',tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
|
||||
x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
final_detail = {}
|
||||
rf = evaluate(fit.x,final_detail)
|
||||
bg_steps, ba_steps = [], []
|
||||
for index,problem in enumerate(problems):
|
||||
states = fit.x[offsets[index]:offsets[index+1]].reshape(-1,NODE_DOF)
|
||||
bg_steps.extend(np.linalg.norm(np.diff(states[:,9:12],axis=0),axis=1))
|
||||
ba_steps.extend(np.linalg.norm(np.diff(states[:,12:15],axis=0),axis=1))
|
||||
covariance = {'rotation_rad':[],'velocity_m_s':[],'position_m':[]}
|
||||
for problem in problems:
|
||||
for pre in problem.segment.preintegrations:
|
||||
sigma = np.sqrt(np.maximum(np.diag(pre.cov),0.))
|
||||
covariance['rotation_rad'].extend(sigma[:3])
|
||||
covariance['velocity_m_s'].extend(sigma[3:6])
|
||||
covariance['position_m'].extend(sigma[6:9])
|
||||
dof = max(_effective_residual_dimension(final_detail)-len(fit.x),1)
|
||||
return NodeGraphResult(
|
||||
success=bool(fit.success),message=str(fit.message),
|
||||
node_count=sum(len(problem.segment.nodes) for problem in problems),
|
||||
duration_s=sum(problem.segment.nodes[-1].t_s-problem.segment.nodes[0].t_s
|
||||
for problem in problems),
|
||||
fixed_l_I_m=problems[0].fixed_l_I_m.copy(),
|
||||
initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
|
||||
cost_reduction=.5*float(np.dot(r0,r0)-np.dot(rf,rf)),
|
||||
nfev=int(fit.nfev),optimality=float(fit.optimality),
|
||||
gradient_norm=float(np.linalg.norm(fit.grad)),
|
||||
residual_dimension=len(rf),state_dimension=len(fit.x),
|
||||
statistical_dof=dof,total_nis=float(np.dot(rf,rf)),
|
||||
chi_square_per_dof=float(np.dot(rf,rf)/dof),
|
||||
cost_per_dof=.5*float(np.dot(rf,rf)/dof),
|
||||
initial_residual_by_factor=_factor_stats(initial_detail),
|
||||
final_residual_by_factor=_factor_stats(final_detail),
|
||||
final_position_residual_m=_vector_stats(final_detail.get('position_physical',[])),
|
||||
final_velocity_residual_m_s=_vector_stats(final_detail.get('velocity_physical',[])),
|
||||
max_bg_step_rad_s=float(max(bg_steps,default=0.)),
|
||||
max_ba_step_m_s2=float(max(ba_steps,default=0.)),
|
||||
preintegration_covariance_sigma={key:_distribution(value) for key,value in covariance.items()})
|
||||
|
||||
|
||||
def _free_residual(problem,value,details=None):
|
||||
return residual(problem,value[3:],details=details,lever_override=value[:3])
|
||||
|
||||
|
||||
def _free_sparsity(problem,value):
|
||||
local = jacobian_sparsity(problem,value[3:])
|
||||
result = lil_matrix((local.shape[0],local.shape[1]+3),dtype=int)
|
||||
result[:,:3] = 1
|
||||
result[:,3:] = local
|
||||
return result.tocsr()
|
||||
|
||||
|
||||
def _marginal_lever_information(jacobian):
|
||||
J = jacobian.toarray() if hasattr(jacobian,'toarray') else np.asarray(jacobian)
|
||||
H = J.T@J
|
||||
Hll,Hln,Hnn = H[:3,:3],H[:3,3:],H[3:,3:]
|
||||
marginal = Hll-Hln@np.linalg.pinv(Hnn,rcond=1e-10)@Hln.T
|
||||
return .5*(marginal+marginal.T)
|
||||
|
||||
|
||||
def _additive_marginal_lever_information(jacobian,row_offsets,state_offsets):
|
||||
total=np.zeros((3,3))
|
||||
for index in range(len(row_offsets)-1):
|
||||
rows=slice(row_offsets[index],row_offsets[index+1])
|
||||
columns=np.r_[0:3,state_offsets[index]:state_offsets[index+1]]
|
||||
local=jacobian[rows,:][:,columns]
|
||||
total+=_marginal_lever_information(local)
|
||||
return .5*(total+total.T)
|
||||
|
||||
|
||||
def linearized_lever_information(problem,l_I_m):
|
||||
lever=np.asarray(l_I_m,dtype=float)
|
||||
value=np.concatenate([lever,initial_parameters(problem,lever)])
|
||||
fit=least_squares(lambda x:_free_residual(problem,x),value,jac='2-point',
|
||||
jac_sparsity=_free_sparsity(problem,value),method='trf',tr_solver='lsmr',
|
||||
loss='linear',max_nfev=1,x_scale='jac')
|
||||
information=_marginal_lever_information(fit.jac)
|
||||
_,singular,Vt=np.linalg.svd(information)
|
||||
covariance=np.linalg.pinv(information,rcond=1e-9)
|
||||
return information,covariance,singular,Vt[-1]
|
||||
|
||||
|
||||
def solve_free_lever(problem,initial_l_I_m,max_nfev=120):
|
||||
initial_l = np.asarray(initial_l_I_m,dtype=float)
|
||||
x0 = np.concatenate([initial_l,initial_parameters(problem,initial_l)])
|
||||
r0 = _free_residual(problem,x0)
|
||||
fit = least_squares(
|
||||
lambda value:_free_residual(problem,value),x0,jac='2-point',
|
||||
jac_sparsity=_free_sparsity(problem,x0),method='trf',tr_solver='lsmr',
|
||||
loss='linear',max_nfev=max_nfev,x_scale='jac',
|
||||
ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
detail = {}
|
||||
rf = _free_residual(problem,fit.x,detail)
|
||||
information = _marginal_lever_information(fit.jac)
|
||||
_,singular_values,Vt = np.linalg.svd(information)
|
||||
tolerance = max(singular_values[0]*1e-9,1e-10)
|
||||
rank = int(np.sum(singular_values>tolerance))
|
||||
covariance = np.linalg.pinv(information,rcond=1e-9)
|
||||
dof = max(_effective_residual_dimension(detail)-len(fit.x),1)
|
||||
condition = (float(singular_values[0]/singular_values[-1])
|
||||
if singular_values[-1]>tolerance else np.inf)
|
||||
return FreeLeverResult(
|
||||
success=bool(fit.success),message=str(fit.message),
|
||||
initial_l_I_m=initial_l,final_l_I_m=fit.x[:3].copy(),
|
||||
lever_step_norm_m=float(np.linalg.norm(fit.x[:3]-initial_l)),
|
||||
initial_cost=.5*float(np.dot(r0,r0)),
|
||||
final_cost=.5*float(np.dot(rf,rf)),nfev=int(fit.nfev),
|
||||
optimality=float(fit.optimality),chi_square_per_dof=float(np.dot(rf,rf)/dof),
|
||||
position_residual_m=_vector_stats(detail.get('position_physical',[])),
|
||||
velocity_residual_m_s=_vector_stats(detail.get('velocity_physical',[])),
|
||||
residual_by_factor=_factor_stats(detail),lever_covariance_m2=covariance,
|
||||
lever_information_singular_values=singular_values,
|
||||
lever_information_condition_number=condition,lever_precision_rank=rank,
|
||||
weakest_lever_direction_I=Vt[-1].copy())
|
||||
|
||||
|
||||
def solve_free_lever_many(problems,initial_l_I_m,max_nfev=120,
|
||||
initial_state_values=None,lever_prior_mean_m=None,
|
||||
lever_prior_covariance_m2=None,return_state_values=False):
|
||||
problems=tuple(problems)
|
||||
initial_l=np.asarray(initial_l_I_m,dtype=float)
|
||||
sizes=[NODE_DOF*len(problem.segment.nodes) for problem in problems]
|
||||
offsets=np.cumsum([3,*sizes])
|
||||
states=([initial_parameters(problem,initial_l) for problem in problems]
|
||||
if initial_state_values is None else
|
||||
[np.asarray(value,dtype=float) for value in initial_state_values])
|
||||
prior_mean=(None if lever_prior_mean_m is None else
|
||||
np.asarray(lever_prior_mean_m,dtype=float))
|
||||
prior_cov=(None if lever_prior_covariance_m2 is None else
|
||||
np.asarray(lever_prior_covariance_m2,dtype=float))
|
||||
prior_whitener=(None if prior_cov is None else
|
||||
np.linalg.inv(np.linalg.cholesky(prior_cov)))
|
||||
x0=np.concatenate([initial_l,*states])
|
||||
def evaluate(value,details=None):
|
||||
chunks=[]
|
||||
for index,problem in enumerate(problems):
|
||||
local={} if details is not None else None
|
||||
chunks.append(residual(problem,value[offsets[index]:offsets[index+1]],
|
||||
details=local,lever_override=value[:3]))
|
||||
if details is not None:
|
||||
for key,items in local.items(): details.setdefault(key,[]).extend(items)
|
||||
if prior_whitener is not None:
|
||||
prior_error=prior_whitener@(value[:3]-prior_mean)
|
||||
chunks.append(prior_error)
|
||||
if details is not None:
|
||||
details.setdefault('lever_prior',[]).extend(prior_error.tolist())
|
||||
return np.concatenate(chunks)
|
||||
r0=evaluate(x0)
|
||||
sparsity=lil_matrix((len(r0),len(x0)),dtype=int)
|
||||
row=0; row_offsets=[0]
|
||||
for index,problem in enumerate(problems):
|
||||
local=jacobian_sparsity(problem,states[index])
|
||||
sparsity[row:row+local.shape[0],:3]=1
|
||||
sparsity[row:row+local.shape[0],offsets[index]:offsets[index+1]]=local
|
||||
row+=local.shape[0]
|
||||
row_offsets.append(row)
|
||||
if prior_whitener is not None:
|
||||
sparsity[row:row+3,:3]=1
|
||||
fit=least_squares(
|
||||
lambda value:evaluate(value),x0,jac='2-point',jac_sparsity=sparsity.tocsr(),
|
||||
method='trf',tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
|
||||
x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
detail={}
|
||||
rf=evaluate(fit.x,detail)
|
||||
information=_additive_marginal_lever_information(
|
||||
fit.jac,row_offsets,offsets)
|
||||
if prior_cov is not None:
|
||||
information+=np.linalg.inv(prior_cov)
|
||||
_,singular_values,Vt=np.linalg.svd(information)
|
||||
tolerance=max(singular_values[0]*1e-9,1e-10)
|
||||
rank=int(np.sum(singular_values>tolerance))
|
||||
covariance=np.linalg.pinv(information,rcond=1e-9)
|
||||
dof=max(_effective_residual_dimension(detail)-len(fit.x),1)
|
||||
condition=(float(singular_values[0]/singular_values[-1])
|
||||
if singular_values[-1]>tolerance else np.inf)
|
||||
result=FreeLeverResult(
|
||||
success=bool(fit.success),message=str(fit.message),
|
||||
initial_l_I_m=initial_l,final_l_I_m=fit.x[:3].copy(),
|
||||
lever_step_norm_m=float(np.linalg.norm(fit.x[:3]-initial_l)),
|
||||
initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
|
||||
nfev=int(fit.nfev),optimality=float(fit.optimality),
|
||||
chi_square_per_dof=float(np.dot(rf,rf)/dof),
|
||||
position_residual_m=_vector_stats(detail.get('position_physical',[])),
|
||||
velocity_residual_m_s=_vector_stats(detail.get('velocity_physical',[])),
|
||||
residual_by_factor=_factor_stats(detail),lever_covariance_m2=covariance,
|
||||
lever_information_singular_values=singular_values,
|
||||
lever_information_condition_number=condition,lever_precision_rank=rank,
|
||||
weakest_lever_direction_I=Vt[-1].copy())
|
||||
if return_state_values:
|
||||
states=[fit.x[offsets[i]:offsets[i+1]].copy()
|
||||
for i in range(len(problems))]
|
||||
return result,states
|
||||
return result
|
||||
@@ -16,6 +16,15 @@ from .rtk_imu_translation import TranslationCalibrationResult, solve_rtk_imu_tra
|
||||
from .rtk_io import load_rtk_csv
|
||||
|
||||
|
||||
DEFAULT_RTK_FRAME_DEFINITION = (
|
||||
"right-handed vehicle-fixed frame: +X ANT1(main,left)->ANT2(secondary,right), "
|
||||
"+Y vehicle forward/IMU +Y, +Z vehicle up/IMU +Z"
|
||||
)
|
||||
DEFAULT_RTK_REFERENCE_POINT = (
|
||||
"GGA ANT1/main-antenna phase center, 1.916499878 m above ground"
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class InventoryEntry:
|
||||
session_id: str
|
||||
@@ -82,6 +91,7 @@ def dataset_audit(sessions: list[RotationSession]) -> dict[str, Any]:
|
||||
rtk = session.rtk
|
||||
valid_position = rtk.position_valid
|
||||
valid_attitude = rtk.attitude_valid & valid_position
|
||||
float_attitude = rtk.attitude_float & valid_position
|
||||
rows.append(
|
||||
{
|
||||
"session_id": session.session_id,
|
||||
@@ -90,6 +100,8 @@ def dataset_audit(sessions: list[RotationSession]) -> dict[str, Any]:
|
||||
"rtk_samples": int(rtk.t_s.size),
|
||||
"fixed_position_ratio": float(np.mean(valid_position)),
|
||||
"fixed_attitude_ratio": float(np.mean(valid_attitude)),
|
||||
"float_attitude_ratio": float(np.mean(float_attitude)),
|
||||
"checksum_valid_ratio": float(np.mean(rtk.checksum_valid)),
|
||||
"common_time_span_s": [
|
||||
float(max(session.imu.t_s[0], rtk.t_s[0])),
|
||||
float(min(session.imu.t_s[-1], rtk.t_s[-1])),
|
||||
@@ -109,8 +121,8 @@ def run_calibration(
|
||||
rotation_only: bool = False,
|
||||
compute_loo: bool = True,
|
||||
knot_step_s: float = 2.0,
|
||||
rtk_frame_definition: str = '',
|
||||
rtk_reference_point: str = '',
|
||||
rtk_frame_definition: str = DEFAULT_RTK_FRAME_DEFINITION,
|
||||
rtk_reference_point: str = DEFAULT_RTK_REFERENCE_POINT,
|
||||
) -> tuple[RotationCalibrationResult, TranslationCalibrationResult | None]:
|
||||
"""Run calibration and publish human-readable JSON artifacts."""
|
||||
|
||||
@@ -118,7 +130,7 @@ def run_calibration(
|
||||
output.mkdir(parents=True, exist_ok=True)
|
||||
rotation = solve_rtk_imu_rotation(sessions, compute_loo=compute_loo)
|
||||
translation = None
|
||||
if not rotation_only:
|
||||
if not rotation_only and rotation.ok:
|
||||
translation = solve_rtk_imu_translation(
|
||||
sessions,
|
||||
rotation,
|
||||
@@ -144,9 +156,9 @@ def run_calibration(
|
||||
)
|
||||
blockers = []
|
||||
if not rotation.ok:
|
||||
blockers.append('rotation quality gates failed')
|
||||
blockers.append('full RTK-to-IMU rotation is not observable from the lateral dual-antenna baseline')
|
||||
if translation is None or not translation.ok:
|
||||
blockers.append('translation quality gates failed or were not run')
|
||||
blockers.append('translation is frozen until a full rotation is observable and accepted')
|
||||
if not rtk_frame_definition.strip():
|
||||
blockers.append('RTK frame_definition is empty')
|
||||
if not rtk_reference_point.strip():
|
||||
|
||||
+301
-52
@@ -2,7 +2,7 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from dataclasses import dataclass, replace
|
||||
|
||||
import numpy as np
|
||||
from scipy.optimize import least_squares
|
||||
@@ -31,6 +31,28 @@ class TimeOffsetAudit:
|
||||
second_best_correlation: float
|
||||
evaluated_samples: int
|
||||
reliable: bool
|
||||
method: str
|
||||
peak_width_s: tuple[float, float]
|
||||
per_session_offset_s: dict[str, float]
|
||||
per_session_peak_correlation: dict[str, float]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class BaselineConsistencyAudit:
|
||||
"""Two-DOF audit using only the physically observed ANT1-to-ANT2 axis."""
|
||||
|
||||
baseline_axis_imu: np.ndarray
|
||||
pair_count: int
|
||||
residual_rms_deg: float
|
||||
residual_median_deg: float
|
||||
residual_p95_deg: float
|
||||
per_session_rms_deg: dict[str, float]
|
||||
per_session_p95_deg: dict[str, float]
|
||||
per_session_axis_rms_deg: dict[str, np.ndarray]
|
||||
gyro_bias_by_session_rad_s: dict[str, np.ndarray]
|
||||
worst_pairs: tuple[dict[str, object], ...]
|
||||
ok: bool
|
||||
notes: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -50,6 +72,10 @@ class RotationCalibrationResult:
|
||||
information_singular_values: np.ndarray
|
||||
per_session_rms_deg: dict[str, float]
|
||||
loo_delta_deg: dict[str, float]
|
||||
baseline_consistency: BaselineConsistencyAudit
|
||||
observable_rotation_dof: int
|
||||
full_attitude_observable: bool
|
||||
legacy_full_attitude_numeric_ok: bool
|
||||
ok: bool
|
||||
notes: tuple[str, ...]
|
||||
|
||||
@@ -62,6 +88,8 @@ class _Pair:
|
||||
delta_R_zero_bias: np.ndarray
|
||||
J_bg: np.ndarray
|
||||
weight: float
|
||||
t0_s: float
|
||||
t1_s: float
|
||||
|
||||
|
||||
def _attitude_rows(rtk: RtkSeries, convention: GnhprConvention) -> tuple[np.ndarray, np.ndarray]:
|
||||
@@ -91,14 +119,25 @@ def _attitude_rows(rtk: RtkSeries, convention: GnhprConvention) -> tuple[np.ndar
|
||||
return unique_t, rotations
|
||||
|
||||
|
||||
def _rtk_angular_speed(t: np.ndarray, rotations: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
def _rtk_heading_rate(t: np.ndarray, rotations: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Return signed vehicle yaw rate from the ANT1-to-ANT2 azimuth.
|
||||
|
||||
ANT1-to-ANT2 points vehicle-right, so its clockwise heading increases when
|
||||
mathematical body yaw decreases. Only this signed heading channel is
|
||||
compared with IMU gyro_z; rotation about the baseline is unobservable.
|
||||
"""
|
||||
|
||||
dt = np.diff(t)
|
||||
valid = (dt >= 0.03) & (dt <= 0.5)
|
||||
midpoint = 0.5 * (t[:-1] + t[1:])
|
||||
speed = np.array(
|
||||
[np.linalg.norm(so3_log(rotations[i].T @ rotations[i + 1])) for i in range(t.size - 1)]
|
||||
) / np.maximum(dt, 1e-6)
|
||||
return midpoint[valid], speed[valid]
|
||||
baseline = rotations[:, :, 0]
|
||||
heading = np.unwrap(np.arctan2(baseline[:, 0], baseline[:, 1]))
|
||||
rate = -np.diff(heading) / np.maximum(dt, 1e-6)
|
||||
valid = (
|
||||
(dt >= 0.03)
|
||||
& (dt <= 0.25)
|
||||
& (np.abs(rate) >= np.deg2rad(0.5))
|
||||
& (np.abs(rate) <= np.deg2rad(30.0))
|
||||
)
|
||||
return 0.5 * (t[:-1] + t[1:])[valid], rate[valid]
|
||||
|
||||
|
||||
def _correlation(a: np.ndarray, b: np.ndarray) -> float:
|
||||
@@ -115,45 +154,102 @@ def audit_time_offset(
|
||||
search_half_width_s: float = 0.30,
|
||||
step_s: float = 0.005,
|
||||
) -> TimeOffsetAudit:
|
||||
"""Estimate residual ``t_IMU - t_RTK`` from invariant angular-speed norms."""
|
||||
"""Audit residual t_IMU - t_RTK from signed heading rate.
|
||||
|
||||
A broad correlation peak remains diagnostic. It is never applied unless
|
||||
both peak separation and peak width pass.
|
||||
"""
|
||||
|
||||
offsets = np.arange(-search_half_width_s, search_half_width_s + 0.5 * step_s, step_s)
|
||||
session_series: list[tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]] = []
|
||||
session_series: list[tuple[str, np.ndarray, np.ndarray, np.ndarray, np.ndarray]] = []
|
||||
total_samples = 0
|
||||
for session in sessions:
|
||||
t_rtk, rotations = _attitude_rows(session.rtk, GNHPR_CANDIDATES[0])
|
||||
midpoint, rtk_speed = _rtk_angular_speed(t_rtk, rotations)
|
||||
imu_speed = np.linalg.norm(session.imu.gyro_rad_s, axis=1)
|
||||
motion = rtk_speed > np.deg2rad(0.5)
|
||||
midpoint = midpoint[motion]
|
||||
rtk_speed = rtk_speed[motion]
|
||||
midpoint, rtk_rate = _rtk_heading_rate(t_rtk, rotations)
|
||||
imu_rate = session.imu.gyro_rad_s[:, 2]
|
||||
if midpoint.size >= 20:
|
||||
session_series.append((midpoint, rtk_speed, session.imu.t_s, imu_speed))
|
||||
session_series.append(
|
||||
(session.session_id, midpoint, rtk_rate, session.imu.t_s, imu_rate)
|
||||
)
|
||||
total_samples += int(midpoint.size)
|
||||
if not session_series:
|
||||
return TimeOffsetAudit(0.0, np.nan, np.nan, 0, False)
|
||||
return TimeOffsetAudit(
|
||||
0.0,
|
||||
np.nan,
|
||||
np.nan,
|
||||
0,
|
||||
False,
|
||||
"signed_heading_rate_vs_imu_gyro_z",
|
||||
(np.nan, np.nan),
|
||||
{},
|
||||
{},
|
||||
)
|
||||
scores = []
|
||||
per_session_scores: dict[str, list[float]] = {
|
||||
session_id: [] for session_id, *_ in session_series
|
||||
}
|
||||
for offset in offsets:
|
||||
per_session = []
|
||||
for midpoint, rtk_speed, imu_t, imu_speed in session_series:
|
||||
for session_id, midpoint, rtk_rate, imu_t, imu_rate in session_series:
|
||||
query = midpoint + offset
|
||||
inside = (query >= imu_t[0]) & (query <= imu_t[-1])
|
||||
if np.count_nonzero(inside) < 20:
|
||||
per_session_scores[session_id].append(np.nan)
|
||||
continue
|
||||
interpolated = np.interp(query[inside], imu_t, imu_speed)
|
||||
value = _correlation(rtk_speed[inside], interpolated)
|
||||
interpolated = np.interp(query[inside], imu_t, imu_rate)
|
||||
value = _correlation(rtk_rate[inside], interpolated)
|
||||
per_session_scores[session_id].append(value)
|
||||
if np.isfinite(value):
|
||||
per_session.append(value)
|
||||
scores.append(float(np.median(per_session)) if per_session else np.nan)
|
||||
values = np.asarray(scores, dtype=float)
|
||||
if not np.any(np.isfinite(values)):
|
||||
return TimeOffsetAudit(0.0, np.nan, np.nan, total_samples, False)
|
||||
return TimeOffsetAudit(
|
||||
0.0,
|
||||
np.nan,
|
||||
np.nan,
|
||||
total_samples,
|
||||
False,
|
||||
"signed_heading_rate_vs_imu_gyro_z",
|
||||
(np.nan, np.nan),
|
||||
{},
|
||||
{},
|
||||
)
|
||||
best_index = int(np.nanargmax(values))
|
||||
exclusion = np.abs(offsets - offsets[best_index]) >= 0.03
|
||||
second = float(np.nanmax(values[exclusion])) if np.any(np.isfinite(values[exclusion])) else np.nan
|
||||
peak = float(values[best_index])
|
||||
reliable = bool(peak >= 0.35 and (not np.isfinite(second) or peak - second >= 0.015))
|
||||
return TimeOffsetAudit(float(offsets[best_index]), peak, second, total_samples, reliable)
|
||||
near_peak = np.flatnonzero(values >= peak - 0.005)
|
||||
peak_width = (
|
||||
(float(offsets[near_peak[0]]), float(offsets[near_peak[-1]]))
|
||||
if near_peak.size
|
||||
else (np.nan, np.nan)
|
||||
)
|
||||
per_session_offset = {}
|
||||
per_session_peak = {}
|
||||
for session_id, session_values in per_session_scores.items():
|
||||
array = np.asarray(session_values, dtype=float)
|
||||
if np.any(np.isfinite(array)):
|
||||
index = int(np.nanargmax(array))
|
||||
per_session_offset[session_id] = float(offsets[index])
|
||||
per_session_peak[session_id] = float(array[index])
|
||||
reliable = bool(
|
||||
peak >= 0.5
|
||||
and (not np.isfinite(second) or peak - second >= 0.015)
|
||||
and np.isfinite(peak_width[0])
|
||||
and peak_width[1] - peak_width[0] <= 0.03
|
||||
)
|
||||
return TimeOffsetAudit(
|
||||
float(offsets[best_index]),
|
||||
peak,
|
||||
second,
|
||||
total_samples,
|
||||
reliable,
|
||||
"signed_heading_rate_vs_imu_gyro_z",
|
||||
peak_width,
|
||||
per_session_offset,
|
||||
per_session_peak,
|
||||
)
|
||||
|
||||
|
||||
def _nearest_index(times: np.ndarray, target: float) -> int:
|
||||
@@ -175,6 +271,17 @@ def _make_pairs(
|
||||
cache = {} if preintegration_cache is None else preintegration_cache
|
||||
for session_index, session in enumerate(sessions):
|
||||
t, rotations = _attitude_rows(session.rtk, convention)
|
||||
dt = np.diff(t)
|
||||
baseline = rotations[:, :, 0]
|
||||
baseline_step = np.arccos(
|
||||
np.clip(np.sum(baseline[:-1] * baseline[1:], axis=1), -1.0, 1.0)
|
||||
)
|
||||
broken_edge = (
|
||||
(dt < 0.03)
|
||||
| (dt > 0.25)
|
||||
| (baseline_step / np.maximum(dt, 1e-6) > np.deg2rad(45.0))
|
||||
)
|
||||
broken_prefix = np.concatenate([[0], np.cumsum(broken_edge.astype(int))])
|
||||
next_anchor = float(t[0])
|
||||
for i in range(t.size - 1):
|
||||
if t[i] + 1e-9 < next_anchor:
|
||||
@@ -184,6 +291,8 @@ def _make_pairs(
|
||||
j = _nearest_index(t, float(t[i] + duration))
|
||||
if j <= i or abs(float(t[j] - t[i]) - duration) > 0.18:
|
||||
continue
|
||||
if broken_prefix[j] - broken_prefix[i] != 0:
|
||||
continue
|
||||
imu_t0 = float(t[i] + time_offset_s)
|
||||
imu_t1 = float(t[j] + time_offset_s)
|
||||
if imu_t0 < session.imu.t_s[0] or imu_t1 > session.imu.t_s[-1]:
|
||||
@@ -212,9 +321,25 @@ def _make_pairs(
|
||||
delta_R_zero_bias=preint.delta_R,
|
||||
J_bg=preint.J_bg,
|
||||
weight=weight,
|
||||
t0_s=float(t[i]),
|
||||
t1_s=float(t[j]),
|
||||
)
|
||||
)
|
||||
return pairs
|
||||
# Equalize total influence per session. Pair count and excitation otherwise
|
||||
# let long/high-motion sessions dominate the shared rotation.
|
||||
totals = {
|
||||
session.session_id: sum(
|
||||
pair.weight for pair in pairs if pair.session_id == session.session_id
|
||||
)
|
||||
for session in sessions
|
||||
}
|
||||
nonzero = [value for value in totals.values() if value > 0.0]
|
||||
target = float(np.mean(nonzero)) if nonzero else 1.0
|
||||
return [
|
||||
replace(pair, weight=pair.weight * target / totals[pair.session_id])
|
||||
for pair in pairs
|
||||
if totals[pair.session_id] > 0.0
|
||||
]
|
||||
|
||||
|
||||
def _solve_core(sessions: list[RotationSession], pairs: list[_Pair]) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
|
||||
@@ -290,6 +415,129 @@ def _solve_core(sessions: list[RotationSession], pairs: list[_Pair]) -> tuple[np
|
||||
return r_x, biases, np.asarray(errors), covariance
|
||||
|
||||
|
||||
def _solve_baseline_consistency(
|
||||
sessions: list[RotationSession],
|
||||
pairs: list[_Pair],
|
||||
) -> BaselineConsistencyAudit:
|
||||
"""Audit the two physically observable dual-antenna rotation DOFs.
|
||||
|
||||
The confirmed ANT1-to-ANT2 axis is IMU +X. For every interval, the angle
|
||||
swept by the GNSS baseline must equal the angle swept by IMU +X under gyro
|
||||
preintegration. Rotation about +X cancels from this invariant and is not
|
||||
falsely scored as an RTK attitude residual.
|
||||
"""
|
||||
|
||||
baseline_axis = np.array([1.0, 0.0, 0.0])
|
||||
session_count = len(sessions)
|
||||
|
||||
def raw_errors(parameters: np.ndarray) -> np.ndarray:
|
||||
biases = parameters.reshape(session_count, 3)
|
||||
values = []
|
||||
for pair in pairs:
|
||||
corrected = apply_bias_jacobian_correction(
|
||||
pair.delta_R_zero_bias,
|
||||
pair.J_bg,
|
||||
biases[pair.session_index],
|
||||
)
|
||||
observed = np.arccos(np.clip(pair.R_A[0, 0], -1.0, 1.0))
|
||||
predicted = np.arccos(
|
||||
np.clip(baseline_axis @ corrected @ baseline_axis, -1.0, 1.0)
|
||||
)
|
||||
values.append(predicted - observed)
|
||||
return np.asarray(values)
|
||||
|
||||
def residual(parameters: np.ndarray) -> np.ndarray:
|
||||
errors = raw_errors(parameters)
|
||||
weighted = errors * np.sqrt(np.asarray([pair.weight for pair in pairs]))
|
||||
return np.concatenate([weighted, parameters / 0.003])
|
||||
|
||||
initial = np.zeros(3 * session_count)
|
||||
opt = least_squares(
|
||||
residual,
|
||||
initial,
|
||||
loss="huber",
|
||||
f_scale=np.deg2rad(0.25),
|
||||
max_nfev=60,
|
||||
)
|
||||
biases = opt.x.reshape(session_count, 3)
|
||||
errors_deg = np.degrees(raw_errors(opt.x))
|
||||
per_session_rms = {}
|
||||
per_session_p95 = {}
|
||||
per_session_axis_rms = {}
|
||||
for session in sessions:
|
||||
selection = np.asarray(
|
||||
[pair.session_id == session.session_id for pair in pairs], dtype=bool
|
||||
)
|
||||
values = errors_deg[selection]
|
||||
per_session_rms[session.session_id] = (
|
||||
float(np.sqrt(np.mean(values**2))) if values.size else np.nan
|
||||
)
|
||||
per_session_p95[session.session_id] = (
|
||||
float(np.percentile(np.abs(values), 95.0)) if values.size else np.nan
|
||||
)
|
||||
axis_errors = []
|
||||
for pair in np.asarray(pairs, dtype=object)[selection]:
|
||||
corrected = apply_bias_jacobian_correction(
|
||||
pair.delta_R_zero_bias,
|
||||
pair.J_bg,
|
||||
biases[pair.session_index],
|
||||
)
|
||||
axis_errors.append(np.degrees(so3_log(pair.R_A @ corrected.T)))
|
||||
per_session_axis_rms[session.session_id] = (
|
||||
np.sqrt(np.mean(np.asarray(axis_errors) ** 2, axis=0))
|
||||
if axis_errors
|
||||
else np.full(3, np.nan)
|
||||
)
|
||||
worst_indices = np.argsort(np.abs(errors_deg))[-20:][::-1]
|
||||
worst_pairs = tuple(
|
||||
{
|
||||
"session_id": pairs[index].session_id,
|
||||
"t0_s": pairs[index].t0_s,
|
||||
"t1_s": pairs[index].t1_s,
|
||||
"duration_s": pairs[index].t1_s - pairs[index].t0_s,
|
||||
"baseline_angle_residual_deg": float(errors_deg[index]),
|
||||
}
|
||||
for index in worst_indices
|
||||
)
|
||||
rms = float(np.sqrt(np.mean(errors_deg**2)))
|
||||
median = float(np.median(np.abs(errors_deg)))
|
||||
p95 = float(np.percentile(np.abs(errors_deg), 95.0))
|
||||
finite_session_rms = [
|
||||
value for value in per_session_rms.values() if np.isfinite(value)
|
||||
]
|
||||
finite_session_p95 = [
|
||||
value for value in per_session_p95.values() if np.isfinite(value)
|
||||
]
|
||||
ok = bool(
|
||||
len(pairs) >= 20
|
||||
and rms <= 1.0
|
||||
and p95 <= 2.0
|
||||
and (not finite_session_rms or max(finite_session_rms) <= 1.5)
|
||||
and (not finite_session_p95 or max(finite_session_p95) <= 3.0)
|
||||
)
|
||||
return BaselineConsistencyAudit(
|
||||
baseline_axis_imu=baseline_axis,
|
||||
pair_count=len(pairs),
|
||||
residual_rms_deg=rms,
|
||||
residual_median_deg=median,
|
||||
residual_p95_deg=p95,
|
||||
per_session_rms_deg=per_session_rms,
|
||||
per_session_p95_deg=per_session_p95,
|
||||
per_session_axis_rms_deg=per_session_axis_rms,
|
||||
gyro_bias_by_session_rad_s={
|
||||
session.session_id: biases[index].copy()
|
||||
for index, session in enumerate(sessions)
|
||||
},
|
||||
worst_pairs=worst_pairs,
|
||||
ok=ok,
|
||||
notes=(
|
||||
"ANT1(main,left)->ANT2(secondary,right) is fixed to IMU +X",
|
||||
"axis residual XYZ labels are baseline-spin(unobservable), baseline-elevation, heading",
|
||||
"full rotation about the baseline is not identifiable from two antennas",
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def solve_rtk_imu_rotation(
|
||||
sessions: list[RotationSession] | tuple[RotationSession, ...],
|
||||
*,
|
||||
@@ -351,41 +599,28 @@ def solve_rtk_imu_rotation(
|
||||
per_session[session.session_id] = (
|
||||
float(np.sqrt(np.mean(np.asarray(values) ** 2))) if values else np.nan
|
||||
)
|
||||
baseline_audit = _solve_baseline_consistency(items, pairs)
|
||||
loo = {}
|
||||
if compute_loo and len(items) >= 3:
|
||||
for omitted in items:
|
||||
kept_items = [item for item in items if item.session_id != omitted.session_id]
|
||||
kept_index = {item.session_id: index for index, item in enumerate(kept_items)}
|
||||
kept_pairs = [
|
||||
pair
|
||||
replace(pair, session_index=kept_index[pair.session_id])
|
||||
for pair in pairs
|
||||
if pair.session_id != omitted.session_id
|
||||
]
|
||||
if len(kept_pairs) < 6:
|
||||
loo[omitted.session_id] = np.nan
|
||||
continue
|
||||
def loo_residual(rotvec: np.ndarray) -> np.ndarray:
|
||||
candidate = orthonormalize_rotation(so3_exp(rotvec))
|
||||
rows = []
|
||||
for pair in kept_pairs:
|
||||
corrected = apply_bias_jacobian_correction(
|
||||
pair.delta_R_zero_bias,
|
||||
pair.J_bg,
|
||||
biases[pair.session_index],
|
||||
)
|
||||
rows.append(
|
||||
np.sqrt(pair.weight)
|
||||
* so3_log(candidate.T @ pair.R_A @ candidate @ corrected.T)
|
||||
)
|
||||
return np.concatenate(rows)
|
||||
|
||||
loo_opt = least_squares(
|
||||
loo_residual,
|
||||
so3_log(rotation),
|
||||
loss='huber',
|
||||
f_scale=np.deg2rad(0.5),
|
||||
max_nfev=30,
|
||||
try:
|
||||
loo_rotation, _, _, _ = _solve_core(kept_items, kept_pairs)
|
||||
except ValueError:
|
||||
loo[omitted.session_id] = np.nan
|
||||
continue
|
||||
loo[omitted.session_id] = float(
|
||||
np.degrees(np.linalg.norm(so3_log(rotation.T @ loo_rotation)))
|
||||
)
|
||||
loo_rotation = orthonormalize_rotation(so3_exp(loo_opt.x))
|
||||
loo[omitted.session_id] = float(np.degrees(np.linalg.norm(so3_log(rotation.T @ loo_rotation))))
|
||||
rotation_cov = covariance[:3, :3]
|
||||
std_deg = np.degrees(np.sqrt(np.maximum(np.diag(rotation_cov), 0.0)))
|
||||
singular_values = np.linalg.svd(np.linalg.pinv(rotation_cov, rcond=1e-12), compute_uv=False)
|
||||
@@ -395,7 +630,7 @@ def solve_rtk_imu_rotation(
|
||||
finite_loo = [value for value in loo.values() if np.isfinite(value)]
|
||||
sorted_scores = sorted(scores.values())
|
||||
convention_gap = sorted_scores[1] - sorted_scores[0] if len(sorted_scores) > 1 else np.inf
|
||||
ok = bool(
|
||||
legacy_numeric_ok = bool(
|
||||
len(pairs) >= 20
|
||||
and rms <= 1.0
|
||||
and p95 <= 2.0
|
||||
@@ -403,19 +638,29 @@ def solve_rtk_imu_rotation(
|
||||
and (not finite_loo or max(finite_loo) <= 1.0)
|
||||
and convention_gap >= 0.05
|
||||
)
|
||||
# GNHPR supplies the ANT1-to-ANT2 direction but no independent rotation
|
||||
# about that direction. A completed 3-D attitude is useful diagnostically,
|
||||
# but cannot pass the full extrinsic-rotation gate from this dataset alone.
|
||||
full_attitude_observable = False
|
||||
ok = False
|
||||
notes = [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
f"residual time convention: t_IMU = t_RTK + {offset:+.6f} s",
|
||||
f"GNHPR convention score gap={convention_gap:.4f} deg",
|
||||
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||
"LOO is conditional: per-session gyro biases are held at their all-session estimates",
|
||||
"LOO re-optimizes the remaining per-session gyro biases",
|
||||
"legacy full-HPR rotation uses a zero-roll gauge completion and is diagnostic only",
|
||||
"dual antennas do not observe rotation about the ANT1-to-ANT2 baseline",
|
||||
]
|
||||
if not time_audit.reliable:
|
||||
notes.append("time-offset correlation was ambiguous; held residual offset at zero")
|
||||
if convention is not GNHPR_CANDIDATES[0]:
|
||||
notes.append("empirical best GNHPR convention differs from protocol expectation; manual verification required")
|
||||
if not ok:
|
||||
notes.append("rotation failed one or more strict acceptance gates")
|
||||
if not baseline_audit.ok:
|
||||
notes.append("the physically observable baseline consistency failed strict gates")
|
||||
if not legacy_numeric_ok:
|
||||
notes.append("the legacy gauge-completed rotation failed one or more numeric gates")
|
||||
notes.append("full rotation is not accepted; translation must remain frozen")
|
||||
return RotationCalibrationResult(
|
||||
R_RTK_IMU=rotation,
|
||||
rpy_deg=rpy_deg_xyz(rotation),
|
||||
@@ -434,6 +679,10 @@ def solve_rtk_imu_rotation(
|
||||
information_singular_values=singular_values,
|
||||
per_session_rms_deg=per_session,
|
||||
loo_delta_deg=loo,
|
||||
baseline_consistency=baseline_audit,
|
||||
observable_rotation_dof=2,
|
||||
full_attitude_observable=full_attitude_observable,
|
||||
legacy_full_attitude_numeric_ok=legacy_numeric_ok,
|
||||
ok=ok,
|
||||
notes=tuple(notes),
|
||||
)
|
||||
|
||||
+26
-3
@@ -22,23 +22,43 @@ class RtkSeries:
|
||||
roll_deg: np.ndarray
|
||||
fix_quality: np.ndarray
|
||||
heading_quality: np.ndarray
|
||||
heading_satellites: np.ndarray
|
||||
heading_age_s: np.ndarray
|
||||
hdop: np.ndarray
|
||||
checksum_valid: np.ndarray
|
||||
origin_geodetic: tuple[float, float, float]
|
||||
source: Path
|
||||
|
||||
@property
|
||||
def attitude_valid(self) -> np.ndarray:
|
||||
"""Strict fixed dual-antenna solutions suitable for calibration."""
|
||||
|
||||
return (
|
||||
np.isfinite(self.heading_deg)
|
||||
& np.isfinite(self.pitch_deg)
|
||||
& np.isfinite(self.roll_deg)
|
||||
& np.isin(self.heading_quality, (4.0, 5.0))
|
||||
& (self.heading_quality == 4.0)
|
||||
& self.checksum_valid
|
||||
)
|
||||
|
||||
@property
|
||||
def attitude_float(self) -> np.ndarray:
|
||||
"""Float solutions retained for diagnostics but never calibration."""
|
||||
|
||||
return (
|
||||
np.isfinite(self.heading_deg)
|
||||
& np.isfinite(self.pitch_deg)
|
||||
& np.isfinite(self.roll_deg)
|
||||
& (self.heading_quality == 5.0)
|
||||
& self.checksum_valid
|
||||
)
|
||||
|
||||
@property
|
||||
def position_valid(self) -> np.ndarray:
|
||||
return np.all(np.isfinite(self.position_enu_m), axis=1) & np.isin(
|
||||
self.fix_quality, (4.0, 5.0)
|
||||
return (
|
||||
np.all(np.isfinite(self.position_enu_m), axis=1)
|
||||
& (self.fix_quality == 4.0)
|
||||
& self.checksum_valid
|
||||
)
|
||||
|
||||
|
||||
@@ -88,7 +108,10 @@ def load_rtk_csv(path: Path | str) -> RtkSeries:
|
||||
roll_deg=_column(data, "roll_deg")[order],
|
||||
fix_quality=_column(data, "fix_quality", default=0.0)[order],
|
||||
heading_quality=_column(data, "heading_quality", default=0.0)[order],
|
||||
heading_satellites=_column(data, "heading_satellites")[order],
|
||||
heading_age_s=_column(data, "heading_age_s")[order],
|
||||
hdop=_column(data, "hdop")[order],
|
||||
checksum_valid=_column(data, "checksum_valid", default=1.0)[order] == 1.0,
|
||||
origin_geodetic=origin,
|
||||
source=source,
|
||||
)
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from tools.rscap_v2.g90_rtk import (
|
||||
parse_bestnava,
|
||||
parse_pvtslna,
|
||||
unicore_checksum_valid,
|
||||
)
|
||||
|
||||
|
||||
BESTNAVA = (
|
||||
'#BESTNAVA,48,GPS,FINE,2430,552524000,0,0,18,8;SOL_COMPUTED,NARROW_INT,'
|
||||
'30.46551864298,114.09274943336,29.6465,-15.1091,WGS84,0.0100,0.0091,'
|
||||
'0.0279,"547",1.000,176.000,38,31,31,31,0,01,03,f3,SOL_COMPUTED,'
|
||||
'DOPPLER_VELOCITY,0.000,0.000,1.6286,81.665533,-0.0015,0.0320,0.0167'
|
||||
'*3e7e4885'
|
||||
)
|
||||
PVTSLNA = (
|
||||
'#PVTSLNA,49,GPS,FINE,2430,552523000,0,0,18,42;NARROW_INT,29.6427,'
|
||||
'30.46551666558,114.09273316982,0.0288,0.0098,0.0094,1.000,PSRDIFF,'
|
||||
'29.2291,30.46551517805,114.09273145886,-15.1092,38,31,38,28,0.2276,'
|
||||
'1.5569,-0.0014,NARROW_INT,0.8328,350.8610,-1.9697,37,31,31,31,'
|
||||
'1.6227,1.3605,0.6398,1.0915,0.8843,5.0,28,10,25*00000000'
|
||||
)
|
||||
|
||||
|
||||
def test_bestnava_preserves_gnss_time_and_doppler_velocity() -> None:
|
||||
assert unicore_checksum_valid(BESTNAVA)
|
||||
row = parse_bestnava(BESTNAVA)
|
||||
assert row["gnss_week"] == 2430
|
||||
assert row["gnss_tow_ms"] == 552524000
|
||||
assert row["position_fixed"]
|
||||
assert row["doppler_velocity_valid"]
|
||||
assert np.isclose(row["horizontal_speed_m_s"], 1.6286)
|
||||
assert np.isclose(
|
||||
np.hypot(row["velocity_east_m_s"], row["velocity_north_m_s"]), 1.6286
|
||||
)
|
||||
|
||||
|
||||
def test_pvtslna_preserves_quality_baseline_and_velocity() -> None:
|
||||
row = parse_pvtslna(PVTSLNA)
|
||||
assert row["position_fixed"]
|
||||
assert row["heading_type"] == "NARROW_INT"
|
||||
assert np.isclose(row["baseline_length_m"], 0.8328)
|
||||
assert np.isclose(row["heading_deg"], 350.8610)
|
||||
assert np.isclose(row["horizontal_speed_m_s"], np.hypot(0.2276, 1.5569))
|
||||
+16
-2
@@ -5,7 +5,12 @@ from __future__ import annotations
|
||||
import struct
|
||||
|
||||
from tools.rscap_v2.capture_format_v2 import CaptureFile, CaptureHeader, RawChunk
|
||||
from tools.rscap_v2.hi13_imu import crc16_hi13, iter_hi13_imu_samples, parse_hi91_frame
|
||||
from tools.rscap_v2.hi13_imu import (
|
||||
crc16_hi13,
|
||||
iter_hi13_imu_samples,
|
||||
parse_hi91_frame,
|
||||
parse_hi91_sample,
|
||||
)
|
||||
|
||||
|
||||
def _hi91_frame(
|
||||
@@ -13,6 +18,8 @@ def _hi91_frame(
|
||||
device_ms: int = 123456,
|
||||
accel_g=(0.0, 0.0, 1.0),
|
||||
gyro_dps=(1.0, -2.0, 3.0),
|
||||
rpy_deg=(4.0, 5.0, 6.0),
|
||||
quaternion_wxyz=(1.0, 0.0, 0.0, 0.0),
|
||||
) -> bytes:
|
||||
payload = bytearray(76)
|
||||
payload[0] = 0x91
|
||||
@@ -22,7 +29,8 @@ def _hi91_frame(
|
||||
struct.pack_into("<I", payload, 8, device_ms)
|
||||
struct.pack_into("<fff", payload, 12, *accel_g)
|
||||
struct.pack_into("<fff", payload, 24, *gyro_dps)
|
||||
# remaining mag/rpy/quat left zero
|
||||
struct.pack_into("<fff", payload, 48, *rpy_deg)
|
||||
struct.pack_into("<ffff", payload, 60, *quaternion_wxyz)
|
||||
payload_length = len(payload)
|
||||
header = bytearray(6)
|
||||
header[0] = 0x5A
|
||||
@@ -48,6 +56,12 @@ def test_parse_hi91_units():
|
||||
assert device_ms == 5000
|
||||
assert abs(accel[2] - 9.80665) < 1e-4
|
||||
assert abs(gyro[0] - 1.0) < 1e-5
|
||||
full = parse_hi91_sample(frame, host_receive_utc_ticks=123)
|
||||
assert full is not None
|
||||
assert full.system_time_ms == 5000
|
||||
assert full.host_receive_utc_ticks == 123
|
||||
assert full.rpy_deg == (4.0, 5.0, 6.0)
|
||||
assert full.quaternion_wxyz == (1.0, 0.0, 0.0, 0.0)
|
||||
|
||||
|
||||
def test_iter_hi13_from_capture():
|
||||
|
||||
@@ -7,7 +7,7 @@ import numpy as np
|
||||
from imu_lidar.geodesy import geodetic_to_enu
|
||||
from imu_lidar.geometry import so3_exp
|
||||
from imu_lidar.imu_preintegration import preintegrate_gyro, preintegrate_imu
|
||||
from imu_lidar.rtk_attitude import gnhpr_to_rotation_enu_rtk
|
||||
from imu_lidar.rtk_attitude import gnhpr_to_baseline_enu, gnhpr_to_rotation_enu_rtk
|
||||
from imu_lidar.rtk_imu_translation import _preintegrate_translation_interval
|
||||
from imu_lidar.rtk_io import load_rtk_csv
|
||||
|
||||
@@ -34,6 +34,22 @@ def test_gnhpr_heading_maps_north_clockwise_into_enu() -> None:
|
||||
assert np.allclose(rotations[1][:, 0], [1.0, 0.0, 0.0], atol=1e-12)
|
||||
|
||||
|
||||
def test_gnhpr_baseline_is_main_to_secondary_and_ignores_roll() -> None:
|
||||
baseline = gnhpr_to_baseline_enu(
|
||||
np.array([0.0, 90.0]),
|
||||
np.array([30.0, 0.0]),
|
||||
)
|
||||
assert np.allclose(baseline[0], [0.0, np.sqrt(0.75), 0.5], atol=1e-12)
|
||||
assert np.allclose(baseline[1], [1.0, 0.0, 0.0], atol=1e-12)
|
||||
|
||||
rotations = gnhpr_to_rotation_enu_rtk(
|
||||
np.array([90.0, 90.0]),
|
||||
np.zeros(2),
|
||||
np.array([0.0, 45.0]),
|
||||
)
|
||||
assert np.allclose(rotations[0], rotations[1], atol=1e-12)
|
||||
|
||||
|
||||
def test_rtk_loader_uses_hpr_measurement_time_for_attitude(tmp_path: Path) -> None:
|
||||
path = tmp_path / "rtk.csv"
|
||||
path.write_text(
|
||||
@@ -49,6 +65,21 @@ def test_rtk_loader_uses_hpr_measurement_time_for_attitude(tmp_path: Path) -> No
|
||||
assert np.all(loaded.attitude_valid)
|
||||
|
||||
|
||||
def test_rtk_loader_accepts_only_checksum_valid_fixed_hpr(tmp_path: Path) -> None:
|
||||
path = tmp_path / "rtk.csv"
|
||||
path.write_text(
|
||||
"t,lat_deg,lon_deg,altitude_m,fix_quality,heading_deg,pitch_deg,roll_deg,"
|
||||
"heading_quality,checksum_valid\n"
|
||||
"0.0,30.0,114.0,20.0,4,10.0,0.0,0.0,4,1\n"
|
||||
"0.1,30.0,114.0,20.0,4,11.0,0.0,0.0,5,1\n"
|
||||
"0.2,30.0,114.0,20.0,4,12.0,0.0,0.0,4,0\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
loaded = load_rtk_csv(path)
|
||||
assert loaded.attitude_valid.tolist() == [True, False, False]
|
||||
assert loaded.attitude_float.tolist() == [False, True, False]
|
||||
|
||||
|
||||
def test_fast_local_preintegration_matches_reference() -> None:
|
||||
times = np.linspace(0.0, 1.0, 101)
|
||||
gyro = np.tile(np.array([0.03, -0.02, 0.15]), (times.size, 1))
|
||||
|
||||
@@ -0,0 +1,535 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from imu_lidar.contracts import ImuSeries
|
||||
from imu_lidar.rtk_imu_engineering import (
|
||||
G0,
|
||||
HPR_DIRECT_ANGULAR_SIGMA_RAD,
|
||||
ResidualAudit,
|
||||
_all_hpr,
|
||||
_engineering_gates,
|
||||
_enu,
|
||||
_height_reference,
|
||||
_hpr_angular_sigma_rad,
|
||||
_marginal_lever_information,
|
||||
_motion_flags,
|
||||
_nodes,
|
||||
_resample_segments_with_multiplicity,
|
||||
_segments,
|
||||
solve_engineering_6dof,
|
||||
)
|
||||
from imu_lidar.rtk_imu_multisource import UnifiedSession
|
||||
from imu_lidar.rtk_imu_node_graph import (
|
||||
_additive_marginal_lever_information,
|
||||
_free_residual,
|
||||
_free_sparsity,
|
||||
_hpr_sigma,
|
||||
build_problem,
|
||||
fit_states_at_fixed_lever,
|
||||
initial_parameters as node_graph_initial_parameters,
|
||||
jacobian_sparsity as node_graph_jacobian_sparsity,
|
||||
residual as node_graph_residual,
|
||||
solve_free_lever_many,
|
||||
)
|
||||
from tools.run_rtk_imu_mechanical_prior_heldout import _gate as heldout_gate
|
||||
from tools.audit_rtk_imu_innovation_noise import _summarize as innovation_summary
|
||||
from tools.audit_rtk_imu_propagation_bias_root_cause import _root_report
|
||||
|
||||
EARTH_RADIUS_M = 6_378_137.0
|
||||
|
||||
|
||||
def _rows_from_motion(
|
||||
session_id: str,
|
||||
lever: np.ndarray,
|
||||
R_RTK_IMU: np.ndarray,
|
||||
rotation_at,
|
||||
*,
|
||||
duration_s: float = 10.0,
|
||||
gga_altitude_offset_m: float | None = None,
|
||||
) -> UnifiedSession:
|
||||
imu_t = np.arange(0.0, duration_s + 0.005, 0.01)
|
||||
rotations = Rotation.from_matrix(np.asarray([rotation_at(t) for t in imu_t]))
|
||||
matrices = rotations.as_matrix()
|
||||
relative = Rotation.from_matrix(np.einsum("nij,njk->nik", matrices[:-1].transpose(0, 2, 1), matrices[1:]))
|
||||
gyro = np.empty((imu_t.size, 3))
|
||||
gyro[:-1] = relative.as_rotvec() / 0.01
|
||||
gyro[-1] = gyro[-2]
|
||||
accel = np.einsum("nji,j->ni", matrices, np.array([0.0, 0.0, G0]))
|
||||
|
||||
hpr_t = np.arange(0.0, duration_s + 0.001, 0.1)
|
||||
rows_hpr: list[dict[str, str]] = []
|
||||
baseline_I = R_RTK_IMU.T[:, 0]
|
||||
for t in hpr_t:
|
||||
baseline = rotation_at(t) @ baseline_I
|
||||
heading = np.degrees(np.arctan2(baseline[0], baseline[1])) % 360.0
|
||||
pitch = np.degrees(np.arcsin(np.clip(baseline[2], -1.0, 1.0)))
|
||||
rows_hpr.append({
|
||||
"checksum_valid": "1", "heading_quality": "4", "t_device_s": str(t),
|
||||
"heading_deg": str(heading), "pitch_deg": str(pitch),
|
||||
})
|
||||
|
||||
rows_best: list[dict[str, str]] = []
|
||||
rows_gga: list[dict[str, str]] = []
|
||||
best_t = np.arange(0.0, duration_s + 0.001, 0.5)
|
||||
dt_velocity = 1e-3
|
||||
for t in best_t:
|
||||
R_WI = rotation_at(t)
|
||||
position = R_WI @ lever
|
||||
before = rotation_at(max(t - dt_velocity, 0.0)) @ lever
|
||||
after = rotation_at(min(t + dt_velocity, duration_s)) @ lever
|
||||
denominator = min(t + dt_velocity, duration_s) - max(t - dt_velocity, 0.0)
|
||||
velocity = (after - before) / denominator
|
||||
lat = position[1] / EARTH_RADIUS_M * 180.0 / np.pi
|
||||
lon = position[0] / EARTH_RADIUS_M * 180.0 / np.pi
|
||||
rows_best.append({
|
||||
"checksum_valid": "1", "position_fixed": "1", "t_device_s": str(t),
|
||||
"lat_deg": str(lat), "lon_deg": str(lon), "altitude_m": str(50.0 + position[2]),
|
||||
"doppler_velocity_valid": "1", "velocity_east_m_s": str(velocity[0]),
|
||||
"velocity_north_m_s": str(velocity[1]), "vertical_speed_m_s": str(velocity[2]),
|
||||
})
|
||||
if gga_altitude_offset_m is not None:
|
||||
rows_gga.append({
|
||||
"checksum_valid": "1", "fix_quality": "4", "t_device_s": str(t),
|
||||
"lat_deg": str(lat), "lon_deg": str(lon),
|
||||
"altitude_msl_m": str(50.0 + position[2] + gga_altitude_offset_m),
|
||||
})
|
||||
|
||||
rtk = {"BESTNAVA": rows_best, "GNHPR": rows_hpr}
|
||||
if rows_gga:
|
||||
rtk["GGA"] = rows_gga
|
||||
return UnifiedSession(
|
||||
session_id=session_id,
|
||||
batch_id="synthetic",
|
||||
imu=ImuSeries(t_s=imu_t, gyro_rad_s=gyro, acc_m_s2=accel),
|
||||
imu_rpy_deg=np.zeros((imu_t.size, 3)),
|
||||
imu_quaternion_wxyz=np.tile([1.0, 0.0, 0.0, 0.0], (imu_t.size, 1)),
|
||||
imu_host_receive_utc_s=imu_t,
|
||||
rtk_by_type=rtk,
|
||||
)
|
||||
|
||||
|
||||
def _constant_velocity_session(speed_m_s: float = 3.0) -> UnifiedSession:
|
||||
imu_t = np.arange(0.0, 5.01, 0.01)
|
||||
rows_best = []
|
||||
rows_hpr = []
|
||||
for t in np.arange(0.0, 5.01, 0.1):
|
||||
rows_hpr.append({
|
||||
"checksum_valid": "1", "heading_quality": "4", "t_device_s": str(t),
|
||||
"heading_deg": "90", "pitch_deg": "0",
|
||||
})
|
||||
for t in np.arange(0.0, 5.01, 0.5):
|
||||
rows_best.append({
|
||||
"checksum_valid": "1", "position_fixed": "1", "t_device_s": str(t),
|
||||
"lat_deg": "0", "lon_deg": str(speed_m_s * t / EARTH_RADIUS_M * 180.0 / np.pi),
|
||||
"altitude_m": "50", "doppler_velocity_valid": "1",
|
||||
"velocity_east_m_s": str(speed_m_s), "velocity_north_m_s": "0",
|
||||
"vertical_speed_m_s": "0",
|
||||
})
|
||||
return UnifiedSession(
|
||||
session_id="constant_velocity", batch_id="synthetic",
|
||||
imu=ImuSeries(
|
||||
t_s=imu_t, gyro_rad_s=np.zeros((imu_t.size, 3)),
|
||||
acc_m_s2=np.tile([0.0, 0.0, G0], (imu_t.size, 1)),
|
||||
),
|
||||
imu_rpy_deg=np.zeros((imu_t.size, 3)),
|
||||
imu_quaternion_wxyz=np.tile([1.0, 0.0, 0.0, 0.0], (imu_t.size, 1)),
|
||||
imu_host_receive_utc_s=imu_t,
|
||||
rtk_by_type={"BESTNAVA": rows_best, "GNHPR": rows_hpr},
|
||||
)
|
||||
|
||||
|
||||
def _audit(values: list[float]) -> ResidualAudit:
|
||||
vector = np.asarray(values, dtype=float)
|
||||
return ResidualAudit(20, vector, vector, float(np.linalg.norm(vector)), float(np.linalg.norm(vector)))
|
||||
|
||||
|
||||
def test_constant_speed_straight_is_gravity_candidate_but_not_zupt() -> None:
|
||||
session = _constant_velocity_session()
|
||||
times = np.asarray([float(row["t_device_s"]) for row in session.rtk_by_type["BESTNAVA"]])
|
||||
velocities = np.asarray([[3.0, 0.0, 0.0]] * len(times))
|
||||
gravity_candidate, zupt_static = _motion_flags(session, 2.5, times, velocities)
|
||||
assert gravity_candidate
|
||||
assert not zupt_static
|
||||
|
||||
|
||||
def test_bestnava_is_not_suppressed_by_earlier_gga_epochs() -> None:
|
||||
session = _rows_from_motion(
|
||||
"best_preferred", np.array([0.3, -0.2, 0.1]), np.eye(3),
|
||||
lambda _: np.eye(3), gga_altitude_offset_m=100.0,
|
||||
)
|
||||
for row in session.rtk_by_type["GGA"]:
|
||||
row["t_device_s"] = str(float(row["t_device_s"]) - 0.05)
|
||||
reference = _height_reference([session])
|
||||
assert reference is not None
|
||||
nodes = _nodes(session, reference, 0.5)
|
||||
assert nodes and all(node.source == "BESTNAVA" for node in nodes)
|
||||
assert all(node.velocity_enu_m_s is not None for node in nodes)
|
||||
|
||||
def test_gga_msl_altitude_never_enters_bestnava_z_reference() -> None:
|
||||
lever = np.array([0.3, -0.2, 0.4])
|
||||
session = _rows_from_motion("mixed_height", lever, np.eye(3), lambda _: np.eye(3),
|
||||
gga_altitude_offset_m=123.0)
|
||||
reference = _height_reference([session])
|
||||
assert reference is not None and reference[2] == pytest.approx(50.4)
|
||||
best, best_mask = _enu(session.rtk_by_type["BESTNAVA"][1], "BESTNAVA", reference)
|
||||
gga, gga_mask = _enu(session.rtk_by_type["GGA"][1], "GGA", reference)
|
||||
assert best_mask.tolist() == [True, True, True]
|
||||
assert gga_mask.tolist() == [True, True, False]
|
||||
assert best[2] == pytest.approx(0.0)
|
||||
assert gga[2] == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_schur_observability_uses_marginal_lever_information() -> None:
|
||||
J_l = np.array([
|
||||
[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 0.1],
|
||||
[1.0, 1.0, 0.0], [1.0, 0.0, 0.0],
|
||||
])
|
||||
J_n = np.array([[1.0], [0.0], [0.0], [1.0], [0.0]])
|
||||
J = np.column_stack([J_l, J_n])
|
||||
marginal, singular, condition, rank, weakest, covariance = _marginal_lever_information(J, np.ones(4))
|
||||
H = J.T @ J
|
||||
expected = H[:3, :3] - H[:3, 3:] @ np.linalg.pinv(H[3:, 3:]) @ H[3:, :3]
|
||||
assert np.allclose(marginal, expected)
|
||||
assert singular.shape == (3,) and rank == 3 and np.isfinite(condition)
|
||||
assert abs(weakest[2]) > 0.99
|
||||
assert covariance.shape == (3, 3)
|
||||
|
||||
|
||||
def test_bootstrap_resampling_preserves_session_multiplicity() -> None:
|
||||
segments = [SimpleNamespace(session_id="a"), SimpleNamespace(session_id="b")]
|
||||
sampled = _resample_segments_with_multiplicity(segments, ["a", "a", "b"])
|
||||
assert [segment.session_id for segment in sampled] == ["a", "a", "b"]
|
||||
|
||||
|
||||
@pytest.mark.parametrize("fault", ["q5", "time_gap", "baseline_jump"])
|
||||
def test_isolated_hpr_faults_do_not_split_r0_trajectory(fault: str) -> None:
|
||||
session = _constant_velocity_session(0.0)
|
||||
if fault == "q5":
|
||||
session.rtk_by_type["GNHPR"][25]["heading_quality"] = "5"
|
||||
elif fault == "time_gap":
|
||||
session.rtk_by_type["GNHPR"] = [
|
||||
row for row in session.rtk_by_type["GNHPR"]
|
||||
if not 2.1 <= float(row["t_device_s"]) <= 2.9
|
||||
]
|
||||
else:
|
||||
session.rtk_by_type["GNHPR"][25]["heading_deg"] = "270"
|
||||
reference = _height_reference([session])
|
||||
assert reference is not None
|
||||
nodes = _nodes(session, reference, 0.5)
|
||||
assert nodes
|
||||
assert all(
|
||||
right.continuity_id == left.continuity_id
|
||||
for left, right in zip(nodes[:-1], nodes[1:])
|
||||
)
|
||||
if fault == "baseline_jump":
|
||||
assert any(not node.hpr_factor_valid and node.hpr_factor_method == "isolated_outlier" for node in nodes)
|
||||
|
||||
|
||||
def test_hpr_bridge_covariance_is_weaker_than_direct_and_limited_to_half_second() -> None:
|
||||
assert _hpr_angular_sigma_rad("nearest_q4", 0.0) == pytest.approx(HPR_DIRECT_ANGULAR_SIGMA_RAD)
|
||||
assert _hpr_angular_sigma_rad("bracket_interpolation", 0.2) > HPR_DIRECT_ANGULAR_SIGMA_RAD
|
||||
assert _hpr_angular_sigma_rad("bracket_interpolation", 0.5) >= _hpr_angular_sigma_rad(
|
||||
"bracket_interpolation", 0.2
|
||||
)
|
||||
assert np.isinf(_hpr_angular_sigma_rad("bracket_interpolation", 0.500001))
|
||||
|
||||
def test_short_hpr_dropout_uses_interpolated_factor_without_splitting_r0() -> None:
|
||||
session = _constant_velocity_session(0.0)
|
||||
session.rtk_by_type["GNHPR"] = [
|
||||
row for row in session.rtk_by_type["GNHPR"]
|
||||
if not 2.4 <= float(row["t_device_s"]) <= 2.6
|
||||
]
|
||||
reference = _height_reference([session])
|
||||
assert reference is not None
|
||||
nodes = _nodes(session, reference, 0.5)
|
||||
bridged = [node for node in nodes if abs(node.t_s - 2.5) < 1e-9]
|
||||
assert len(bridged) == 1
|
||||
assert bridged[0].hpr_factor_valid
|
||||
assert bridged[0].hpr_factor_method == "bracket_interpolation"
|
||||
assert all(right.continuity_id == left.continuity_id for left, right in zip(nodes[:-1], nodes[1:]))
|
||||
|
||||
def test_position_time_backwards_still_splits_r0_trajectory() -> None:
|
||||
session = _constant_velocity_session(0.0)
|
||||
session.rtk_by_type["BESTNAVA"][5]["t_device_s"] = "1.75"
|
||||
reference = _height_reference([session])
|
||||
assert reference is not None
|
||||
nodes = _nodes(session, reference, 0.5)
|
||||
assert any(
|
||||
right.continuity_id != left.continuity_id
|
||||
for left, right in zip(nodes[:-1], nodes[1:])
|
||||
)
|
||||
|
||||
|
||||
def test_one_hz_timestamp_jitter_does_not_create_artificial_two_second_r0_gaps() -> None:
|
||||
session = _constant_velocity_session(0.0)
|
||||
rows = session.rtk_by_type["BESTNAVA"]
|
||||
# Retain strict device-time monotonicity while making every second sample early.
|
||||
for index, row in enumerate(rows):
|
||||
row["t_device_s"] = str(float(row["t_device_s"]) - (0.002 if index % 2 else 0.0))
|
||||
reference = _height_reference([session])
|
||||
assert reference is not None
|
||||
nodes = _nodes(session, reference, 1.0)
|
||||
intervals = np.diff([node.t_s for node in nodes])
|
||||
assert len(nodes) == 6
|
||||
assert np.all(intervals > 0.75)
|
||||
assert np.all(intervals < 1.25)
|
||||
assert all(right.continuity_id == left.continuity_id for left, right in zip(nodes[:-1], nodes[1:]))
|
||||
|
||||
def test_large_residuals_fail_engineering_acceptance() -> None:
|
||||
gates = _engineering_gates(
|
||||
np.array([0.01, 0.01, 0.01]), np.array([100.0, 10.0, 1.0]), 3, 100.0,
|
||||
_audit([1.0, 1.0]), _audit([1.0, 1.0, 1.0]), _audit([2.0, 2.0, 2.0]),
|
||||
{"a": 0.01, "b": 0.02, "c": 0.03}, np.array([0.01, 0.01, 0.01]), True,
|
||||
0.01, True, None, False, None,
|
||||
)
|
||||
assert not gates["gga_xy_rms_p95"]
|
||||
assert not gates["bestnava_xyz_rms_p95"]
|
||||
assert not gates["doppler_velocity_rms_p95"]
|
||||
assert not all(gates.values())
|
||||
|
||||
|
||||
def test_unobservable_free_solution_does_not_block_mechanical_prior_with_euclidean_delta() -> None:
|
||||
gates = _engineering_gates(
|
||||
np.array([0.01, 0.01, 0.01]), np.array([100.0, 10.0, 1.0]), 3, 100.0,
|
||||
_audit([0.01, 0.01]), _audit([0.01, 0.01, 0.01]), _audit([0.01, 0.01, 0.01]),
|
||||
{"a": 0.01, "b": 0.02, "c": 0.03}, np.array([0.01, 0.01, 0.01]), True,
|
||||
0.01, True, np.array([10.0, 10.0, 10.0]), False, None,
|
||||
)
|
||||
assert gates["manual_lever_consistency"] is False
|
||||
assert gates["free_manual_mahalanobis_consistency"] is True
|
||||
observable_gates = _engineering_gates(
|
||||
np.array([0.01, 0.01, 0.01]), np.array([100.0, 10.0, 1.0]), 3, 100.0,
|
||||
_audit([0.01, 0.01]), _audit([0.01, 0.01, 0.01]), _audit([0.01, 0.01, 0.01]),
|
||||
{"a": 0.01, "b": 0.02, "c": 0.03}, np.array([0.01, 0.01, 0.01]), True,
|
||||
0.01, True, None, True, 12.0,
|
||||
)
|
||||
assert observable_gates["free_manual_mahalanobis_consistency"] is False
|
||||
def test_yaw_only_recovers_xy_but_weak_z_is_rejected_and_transforms_are_inverse() -> None:
|
||||
lever = np.array([0.30, -0.20, 0.10])
|
||||
session = _rows_from_motion(
|
||||
"yaw", lever, np.eye(3), lambda t: Rotation.from_euler("z", 0.20 * t).as_matrix()
|
||||
)
|
||||
result = solve_engineering_6dof(
|
||||
[session], R_RTK_IMU=np.eye(3), sample_period_s=0.5,
|
||||
run_loo=False, run_bootstrap=False, run_rotation_sensitivity=False,
|
||||
)
|
||||
assert result.l_I_m is not None
|
||||
assert np.allclose(result.l_I_m[:2], lever[:2], atol=0.05)
|
||||
assert result.lever_precision_rank < 3 or not result.engineering_acceptance_gates["lever_marginal_std"]
|
||||
assert not result.engineering_6dof_accepted
|
||||
assert result.T_RTK_IMU is not None and result.T_IMU_RTK is not None
|
||||
assert np.allclose(result.T_RTK_IMU @ result.T_IMU_RTK, np.eye(4), atol=1e-8)
|
||||
|
||||
|
||||
def test_pitch_excitation_recovers_vertical_lever_arm() -> None:
|
||||
lever = np.array([0.28, -0.16, 0.42])
|
||||
rotation_at = lambda t: Rotation.from_euler(
|
||||
"xyz", [0.0, 0.12 * np.sin(0.55 * t), 0.16 * t]
|
||||
).as_matrix()
|
||||
session = _rows_from_motion("pitch", lever, np.eye(3), rotation_at)
|
||||
result = solve_engineering_6dof(
|
||||
[session], R_RTK_IMU=np.eye(3), sample_period_s=0.5,
|
||||
run_loo=False, run_bootstrap=False, run_rotation_sensitivity=False,
|
||||
)
|
||||
assert result.l_I_m is not None
|
||||
assert result.l_I_m[2] == pytest.approx(lever[2], abs=0.08)
|
||||
|
||||
|
||||
def test_full_3d_excitation_with_nonzero_fixed_r2g_recovers_l_i() -> None:
|
||||
lever = np.array([0.24, -0.18, 0.36])
|
||||
fixed_rotation = Rotation.from_euler("xyz", [0.454, -0.003, 0.012], degrees=True).as_matrix()
|
||||
rotation_at = lambda t: Rotation.from_euler(
|
||||
"xyz", [0.16 * np.sin(0.37 * t), 0.18 * np.sin(0.51 * t), 0.18 * t]
|
||||
).as_matrix()
|
||||
session = _rows_from_motion(
|
||||
"full_3d", lever, fixed_rotation, rotation_at, duration_s=15.0
|
||||
)
|
||||
result = solve_engineering_6dof(
|
||||
[session], R_RTK_IMU=fixed_rotation, sample_period_s=0.5,
|
||||
run_loo=False, run_bootstrap=False, run_rotation_sensitivity=False,
|
||||
)
|
||||
assert result.l_I_m is not None
|
||||
assert np.allclose(result.l_I_m, lever, atol=0.08)
|
||||
assert np.allclose(result.R_RTK_IMU, fixed_rotation)
|
||||
assert result.T_RTK_IMU is not None and result.T_IMU_RTK is not None
|
||||
assert np.allclose(result.T_RTK_IMU @ result.T_IMU_RTK, np.eye(4), atol=1e-8)
|
||||
|
||||
def test_mechanical_soft_prior_keeps_free_solution_and_reports_prior_solution() -> None:
|
||||
lever = np.array([0.24, -0.18, 0.36])
|
||||
reference = np.array([0.30, -0.18, 0.36])
|
||||
rotation_at = lambda t: Rotation.from_euler(
|
||||
"xyz", [0.14 * np.sin(0.37 * t), 0.16 * np.sin(0.51 * t), 0.18 * t]
|
||||
).as_matrix()
|
||||
session = _rows_from_motion("prior", lever, np.eye(3), rotation_at, duration_s=12.0)
|
||||
result = solve_engineering_6dof(
|
||||
[session], R_RTK_IMU=np.eye(3),
|
||||
manual_l_I_m=reference,
|
||||
manual_l_I_covariance_m2=np.diag([0.02**2, 0.02**2, 0.02**2]),
|
||||
sample_period_s=0.5, run_loo=False, run_bootstrap=False,
|
||||
run_rotation_sensitivity=False,
|
||||
)
|
||||
assert result.free_solution is not None
|
||||
assert result.prior_constrained_solution is not None
|
||||
assert result.translation_prior_applied
|
||||
assert np.allclose(result.mechanical_reference_l_I_m, reference)
|
||||
assert np.linalg.norm(result.prior_to_mechanical_delta_m) < np.linalg.norm(result.free_to_mechanical_delta_m)
|
||||
assert np.allclose(result.l_I_m, result.prior_constrained_solution.l_I_m)
|
||||
assert result.mechanical_reference_solution is not None
|
||||
assert result.residual_comparison is not None
|
||||
assert result.posterior_to_prior_covariance_ratio is not None
|
||||
|
||||
|
||||
def test_manual_mean_and_covariance_must_be_provided_together() -> None:
|
||||
session = _constant_velocity_session(0.0)
|
||||
with pytest.raises(ValueError, match="must be provided together"):
|
||||
solve_engineering_6dof([session], R_RTK_IMU=np.eye(3), manual_l_I_m=np.zeros(3))
|
||||
|
||||
|
||||
def test_blockwise_schur_information_is_additive_across_independent_segments() -> None:
|
||||
rng = np.random.default_rng(7)
|
||||
lever_blocks = []
|
||||
nuisance_blocks = []
|
||||
row_count = 30
|
||||
for scale in (1.0, 1e-3, 20.0):
|
||||
lever_blocks.append(rng.normal(size=(row_count, 3)) * scale)
|
||||
nuisance_blocks.append(rng.normal(size=(row_count, 15)) * scale)
|
||||
jacobian = np.zeros((3 * row_count, 3 + 3 * 15))
|
||||
for index, (lever, nuisance) in enumerate(zip(lever_blocks, nuisance_blocks)):
|
||||
rows = slice(index * row_count, (index + 1) * row_count)
|
||||
columns = slice(3 + index * 15, 3 + (index + 1) * 15)
|
||||
jacobian[rows, :3] = lever
|
||||
jacobian[rows, columns] = nuisance
|
||||
residual = np.ones(jacobian.shape[0])
|
||||
combined = _marginal_lever_information(jacobian, residual)[0]
|
||||
expected = np.zeros((3, 3))
|
||||
for lever, nuisance in zip(lever_blocks, nuisance_blocks):
|
||||
local = np.column_stack([lever, nuisance])
|
||||
expected += _marginal_lever_information(local, np.ones(row_count))[0]
|
||||
assert np.allclose(combined, expected, rtol=1e-9, atol=1e-9)
|
||||
|
||||
|
||||
def test_per_node_graph_has_finite_residual_and_matching_sparse_structure() -> None:
|
||||
lever = np.array([0.24,-0.18,0.36])
|
||||
rotation_at = lambda t: Rotation.from_euler('z',.12*t).as_matrix()
|
||||
session = _rows_from_motion('node_graph',lever,np.eye(3),rotation_at,duration_s=10.)
|
||||
segments = _segments([session],.5)
|
||||
assert segments
|
||||
problem = build_problem(segments[0],np.eye(3),lever)
|
||||
x0 = node_graph_initial_parameters(problem)
|
||||
value = node_graph_residual(problem,x0)
|
||||
sparsity = node_graph_jacobian_sparsity(problem,x0)
|
||||
assert x0.size == 15*len(problem.segment.nodes)
|
||||
assert np.all(np.isfinite(value))
|
||||
assert sparsity.shape == (value.size,x0.size)
|
||||
assert sparsity.nnz > value.size
|
||||
|
||||
|
||||
def test_node_graph_hpr_override_retains_bridge_extra_variance() -> None:
|
||||
problem=SimpleNamespace(hpr_direct_angular_sigma_rad=.006)
|
||||
direct=SimpleNamespace(hpr_angular_sigma_rad=HPR_DIRECT_ANGULAR_SIGMA_RAD)
|
||||
assert _hpr_sigma(problem,direct)==pytest.approx(.006)
|
||||
old=_hpr_angular_sigma_rad('bracket_interpolation',.4)
|
||||
bridge=SimpleNamespace(hpr_angular_sigma_rad=old)
|
||||
expected=np.sqrt(.006**2+old**2-HPR_DIRECT_ANGULAR_SIGMA_RAD**2)
|
||||
assert _hpr_sigma(problem,bridge)==pytest.approx(expected)
|
||||
|
||||
|
||||
def test_node_graph_free_lever_layout_extends_fixed_state_by_three() -> None:
|
||||
lever=np.array([.24,-.18,.36])
|
||||
rotation_at=lambda t: Rotation.from_euler('z',.12*t).as_matrix()
|
||||
session=_rows_from_motion('node_graph_free',lever,np.eye(3),rotation_at,duration_s=5.)
|
||||
problem=build_problem(_segments([session],.5)[0],np.eye(3),lever,.006)
|
||||
value=np.concatenate([np.zeros(3),node_graph_initial_parameters(problem,np.zeros(3))])
|
||||
residual=_free_residual(problem,value)
|
||||
sparsity=_free_sparsity(problem,value)
|
||||
assert np.all(np.isfinite(residual))
|
||||
assert sparsity.shape==(residual.size,value.size)
|
||||
|
||||
|
||||
def test_node_graph_block_schur_information_is_additive() -> None:
|
||||
rng=np.random.default_rng(7); rows=24; nuisance=5
|
||||
local=[rng.normal(size=(rows,3+nuisance)) for _ in range(2)]
|
||||
global_jac=np.zeros((2*rows,3+2*nuisance))
|
||||
global_jac[:rows,:3]=local[0][:,:3]
|
||||
global_jac[:rows,3:3+nuisance]=local[0][:,3:]
|
||||
global_jac[rows:,:3]=local[1][:,:3]
|
||||
global_jac[rows:,3+nuisance:]=local[1][:,3:]
|
||||
actual=_additive_marginal_lever_information(
|
||||
global_jac,[0,rows,2*rows],[3,3+nuisance,3+2*nuisance])
|
||||
expected=np.zeros((3,3))
|
||||
for jac in local:
|
||||
H=jac.T@jac
|
||||
expected+=H[:3,:3]-H[:3,3:]@np.linalg.pinv(H[3:,3:])@H[3:,:3]
|
||||
assert np.allclose(actual,expected,rtol=1e-10,atol=1e-10)
|
||||
|
||||
|
||||
def test_node_graph_soft_lever_prior_adds_exact_information() -> None:
|
||||
lever=np.array([.24,-.18,.36])
|
||||
rotation_at=lambda t: Rotation.from_euler('z',.12*t).as_matrix()
|
||||
session=_rows_from_motion('node_graph_prior',lever,np.eye(3),rotation_at,
|
||||
duration_s=5.)
|
||||
problem=build_problem(_segments([session],.5)[0],np.eye(3),lever,.006)
|
||||
free=solve_free_lever_many([problem],lever,max_nfev=1)
|
||||
covariance=np.diag([.02**2,.02**2,.03**2])
|
||||
prior=solve_free_lever_many([problem],lever,max_nfev=1,
|
||||
lever_prior_mean_m=lever,lever_prior_covariance_m2=covariance)
|
||||
free_information=np.linalg.pinv(free.lever_covariance_m2,rcond=1e-9)
|
||||
prior_information=np.linalg.pinv(prior.lever_covariance_m2,rcond=1e-9)
|
||||
assert np.allclose(prior_information-free_information,
|
||||
np.linalg.inv(covariance),rtol=1e-6,atol=1e-5)
|
||||
|
||||
|
||||
def test_heldout_gate_does_not_accept_underdispersed_statistics() -> None:
|
||||
vector={'count':1,'axis_rms':[0.,0.,0.],'axis_p95_abs':[0.,0.,0.],
|
||||
'vector_rms':.01,'vector_p95':.02}
|
||||
summary={'optimizer_converged_fraction':1.,
|
||||
'global_chi_square_per_dof':.127,
|
||||
'best_position_physical_m':vector,
|
||||
'doppler_physical_m_s':vector,
|
||||
'residual_by_factor':{
|
||||
'hpr':{'p95_abs':1.3},
|
||||
'imu_preintegration':{'p95_abs':.28}}}
|
||||
result=heldout_gate(summary)
|
||||
assert not result['passed']
|
||||
assert not result['checks']['global_chi_square_per_dof_in_0p25_4']
|
||||
|
||||
|
||||
def test_fixed_lever_retry_starts_from_previous_final_state() -> None:
|
||||
lever=np.array([.24,-.18,.36])
|
||||
rotation_at=lambda t: Rotation.from_euler('z',.12*t).as_matrix()
|
||||
session=_rows_from_motion('node_graph_retry',lever,np.eye(3),rotation_at,
|
||||
duration_s=5.)
|
||||
problem=build_problem(_segments([session],.5)[0],np.eye(3),lever,.006)
|
||||
state,first=fit_states_at_fixed_lever(problem,lever,max_nfev=1)
|
||||
_,retry=fit_states_at_fixed_lever(
|
||||
problem,lever,max_nfev=1,initial_state_values=state)
|
||||
assert retry['initial_cost']==pytest.approx(first['cost'])
|
||||
|
||||
|
||||
def test_innovation_summary_reports_vector_and_temporal_metrics() -> None:
|
||||
records=[{'residual':np.array([float(i),0.,0.]),
|
||||
'normalized':np.array([float(i),0.,0.]),
|
||||
'normalized_factor_only':np.array([float(i),0.,0.]),
|
||||
't_s':float(i)} for i in range(4)]
|
||||
result=innovation_summary(records)
|
||||
assert result['vector_p95']>result['vector_p50']
|
||||
assert result['temporal_linear_drift_per_s'][0]==pytest.approx(1.)
|
||||
assert np.asarray(result['empirical_covariance']).shape==(3,3)
|
||||
|
||||
|
||||
def test_propagation_root_report_detects_common_constant_acceleration() -> None:
|
||||
acceleration=np.array([.2,-.02,-.01]); position=[]; velocity=[]
|
||||
for index,dt in enumerate((.8,1.,1.2,1.4)):
|
||||
base={'interval_id':str(index),'session':'s','motion':'m','t_s':index,
|
||||
'speed_bin':'slow','gyro_bin':'low','dt_s':dt,'R0_WI':np.eye(3)}
|
||||
position.append({**base,'residual':.5*acceleration*dt*dt})
|
||||
velocity.append({**base,'residual':acceleration*dt})
|
||||
report,_=_root_report(position,velocity)
|
||||
assert report['common_constant_acceleration_error_detected']
|
||||
assert np.allclose(
|
||||
report['overall']['difference_velocity_minus_position_m_s2']['bias'],0.)
|
||||
@@ -0,0 +1,46 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from imu_lidar.contracts import ImuSeries
|
||||
from imu_lidar.rtk_imu_multisource import R1bResult, UnifiedSession, solve_r2g
|
||||
|
||||
|
||||
def _level_session(session_id: str) -> UnifiedSession:
|
||||
t = np.arange(0.0, 25.0, 0.01)
|
||||
return UnifiedSession(
|
||||
session_id=session_id,
|
||||
batch_id="test",
|
||||
imu=ImuSeries(
|
||||
t_s=t,
|
||||
gyro_rad_s=np.zeros((t.size, 3)),
|
||||
acc_m_s2=np.tile([0.0, 0.0, 9.80665], (t.size, 1)),
|
||||
),
|
||||
imu_rpy_deg=np.zeros((t.size, 3)),
|
||||
imu_quaternion_wxyz=np.tile([1.0, 0.0, 0.0, 0.0], (t.size, 1)),
|
||||
imu_host_receive_utc_s=t,
|
||||
rtk_by_type={},
|
||||
)
|
||||
|
||||
|
||||
def test_r2g_baseline_plus_level_gravity_completes_identity() -> None:
|
||||
r1b = R1bResult(
|
||||
baseline_axis_imu=np.array([1.0, 0.0, 0.0]),
|
||||
tilt_yz_deg=np.zeros(2),
|
||||
pair_count=100,
|
||||
residual_rms_deg=0.1,
|
||||
residual_p95_deg=0.2,
|
||||
covariance_deg2=np.eye(2) * 0.01,
|
||||
std_deg=np.ones(2) * 0.1,
|
||||
information_singular_values=np.ones(2),
|
||||
per_session_rms_deg={},
|
||||
gyro_bias_by_session_rad_s={},
|
||||
ok=True,
|
||||
notes=(),
|
||||
)
|
||||
sessions = [_level_session("a"), _level_session("b")]
|
||||
result = solve_r2g(sessions, r1b, level_static_session_ids={"a", "b"})
|
||||
assert result.R_RTK_IMU is not None
|
||||
assert np.allclose(result.R_RTK_IMU, np.eye(3), atol=1e-12)
|
||||
assert result.sample_count == 6
|
||||
assert result.session_count == 2
|
||||
@@ -0,0 +1,313 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Audit RTK/IMU factor conventions without running an optimizer.'''
|
||||
from __future__ import annotations
|
||||
import argparse, json, math, sys
|
||||
from dataclasses import asdict, is_dataclass
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0, str(ROOT))
|
||||
from imu_lidar.imu_preintegration import preintegrate_imu
|
||||
from imu_lidar.rtk_imu_engineering import (
|
||||
G_ENU, MIN_SEGMENT_DURATION_S, MIN_SEGMENT_NODE_COUNT,
|
||||
NUISANCE_DOF_PER_SEGMENT, _Segment, _all_hpr, _audit,
|
||||
_dense_colored_jacobian, _enu, _height_reference,
|
||||
_hpr_factor_observation, _initial_parameters, _marginal_lever_information,
|
||||
_motion_flags, _nodes, _position_valid, _residual,
|
||||
_segment_residual_size, _world_rtk)
|
||||
from imu_lidar.rtk_imu_multisource import _f, _truth, load_unified_sessions
|
||||
MECHANICAL_L_I_M = np.array([-0.45072, -0.25682, 0.73208])
|
||||
|
||||
def _jsonable(value):
|
||||
if isinstance(value, np.ndarray): return _jsonable(value.tolist())
|
||||
if isinstance(value, np.generic): return _jsonable(value.item())
|
||||
if isinstance(value, float): return value if math.isfinite(value) else None
|
||||
if is_dataclass(value): return _jsonable(asdict(value))
|
||||
if isinstance(value, dict): return {str(k): _jsonable(v) for k, v in value.items()}
|
||||
if isinstance(value, (list, tuple)): return [_jsonable(v) for v in value]
|
||||
return value
|
||||
|
||||
def _summary(error):
|
||||
a = np.asarray(error, dtype=float)
|
||||
return _jsonable(_audit(list(a.reshape(-1, 3)), 3)) if a.size else _jsonable(_audit([], 3))
|
||||
|
||||
def _corr(a, b):
|
||||
out = np.full(3, np.nan)
|
||||
for axis in range(3):
|
||||
if len(a) >= 3 and np.std(a[:, axis]) > 1e-10 and np.std(b[:, axis]) > 1e-10:
|
||||
out[axis] = np.corrcoef(a[:, axis], b[:, axis])[0, 1]
|
||||
return out
|
||||
|
||||
def _best_arrays(session, reference):
|
||||
rows = []
|
||||
for row in session.rtk_by_type.get('BESTNAVA', []):
|
||||
v = np.array([_f(row, 'velocity_east_m_s'), _f(row, 'velocity_north_m_s'),
|
||||
_f(row, 'vertical_speed_m_s')])
|
||||
if _position_valid(row, 'BESTNAVA') and _truth(row, 'doppler_velocity_valid') and np.all(np.isfinite(v)):
|
||||
rows.append(row)
|
||||
rows.sort(key=lambda row: _f(row, 't_device_s'))
|
||||
rows = [row for i, row in enumerate(rows) if i == 0 or _f(row, 't_device_s') > _f(rows[i-1], 't_device_s')]
|
||||
t = np.asarray([_f(row, 't_device_s') for row in rows])
|
||||
p = np.asarray([_enu(row, 'BESTNAVA', reference)[0] for row in rows]).reshape(-1, 3)
|
||||
v = np.asarray([[_f(row, 'velocity_east_m_s'), _f(row, 'velocity_north_m_s'),
|
||||
_f(row, 'vertical_speed_m_s')] for row in rows]).reshape(-1, 3)
|
||||
return rows, t, p, v
|
||||
|
||||
def _gnss_audit(sessions, reference, max_dt):
|
||||
all_dpdt, all_v, per_session = [], [], {}
|
||||
for session in sessions:
|
||||
_, t, p, v = _best_arrays(session, reference)
|
||||
dt = np.diff(t); keep = (dt > 0) & (dt <= max_dt)
|
||||
dpdt = np.diff(p, axis=0)[keep] / dt[keep, None]
|
||||
v_avg = .5 * (v[:-1] + v[1:])[keep]
|
||||
all_dpdt.append(dpdt); all_v.append(v_avg)
|
||||
per_session[session.session_id] = {
|
||||
'interval_count': len(dpdt), 'nominal_correlation_xyz': _corr(dpdt, v_avg),
|
||||
'nominal_error_m_s': _summary(dpdt-v_avg)}
|
||||
dpdt, velocity = np.vstack(all_dpdt), np.vstack(all_v)
|
||||
hypotheses = []
|
||||
for swap in (False, True):
|
||||
base = velocity[:, [1,0,2]] if swap else velocity
|
||||
for sx in (-1.,1.):
|
||||
for sy in (-1.,1.):
|
||||
for sz in (-1.,1.):
|
||||
transformed = base * [sx,sy,sz]
|
||||
hypotheses.append({
|
||||
'mapping': ('[N,E,Z]' if swap else '[E,N,Z]')+f'*[{sx:+.0f},{sy:+.0f},{sz:+.0f}]',
|
||||
'correlation_xyz': _corr(dpdt, transformed),
|
||||
'error_m_s': _summary(dpdt-transformed)})
|
||||
hypotheses.sort(key=lambda item: item['error_m_s']['vector_rms'])
|
||||
nominal = next(x for x in hypotheses if x['mapping'] == '[E,N,Z]*[+1,+1,+1]')
|
||||
return {'interval_count': len(dpdt), 'nominal': nominal, 'best_mapping': hypotheses[0],
|
||||
'all_hypotheses': hypotheses, 'per_session': per_session}
|
||||
|
||||
def _nearest_imu(session, t, tolerance=.03):
|
||||
right = int(np.searchsorted(session.imu.t_s, t))
|
||||
candidates = [i for i in (right-1,right) if 0 <= i < len(session.imu.t_s)]
|
||||
if not candidates: return None
|
||||
index = min(candidates, key=lambda i: abs(session.imu.t_s[i]-t))
|
||||
return index if abs(session.imu.t_s[index]-t) <= tolerance else None
|
||||
|
||||
def _static_audit(sessions, rotation):
|
||||
current, opposite, per_session = [], [], {}
|
||||
for session in sessions:
|
||||
hpr, local = _all_hpr(session), []
|
||||
last_t = -np.inf
|
||||
for hpr_index in hpr.valid_indices:
|
||||
t = float(hpr.t_s[hpr_index])
|
||||
if t-last_t < 1.: continue
|
||||
last_t = t
|
||||
gravity, _ = _motion_flags(session,t,np.zeros(0),np.zeros((0,3)))
|
||||
baseline, _, valid, _, _ = _hpr_factor_observation(hpr,t)
|
||||
imu_index = _nearest_imu(session,t)
|
||||
if not (gravity and valid and imu_index is not None):
|
||||
continue
|
||||
R_WI = _world_rtk(baseline) @ rotation
|
||||
accel = session.imu.acc_m_s2[imu_index]
|
||||
value = R_WI @ accel + G_ENU
|
||||
local.append(value); current.append(value); opposite.append(R_WI @ accel-G_ENU)
|
||||
per_session[session.session_id] = {'count':len(local),'current_formula_m_s2':_summary(local)}
|
||||
return {'formula':'a_W_linear = R_WI @ specific_force_I + G_ENU',
|
||||
'current_formula_m_s2':_summary(current),
|
||||
'opposite_gravity_sign_m_s2':_summary(opposite),'per_session':per_session}
|
||||
|
||||
def _closure_audit(sessions, reference, rotation, lever, max_dt):
|
||||
all_p, all_v, per_session = [], [], {}
|
||||
for session in sessions:
|
||||
_, t, p_ant, v_ant = _best_arrays(session, reference)
|
||||
hpr, local_p, local_v = _all_hpr(session), [], []
|
||||
for index, dt in enumerate(np.diff(t)):
|
||||
if not .5 <= dt <= max_dt: continue
|
||||
baseline, _, valid, _, _ = _hpr_factor_observation(hpr, t[index])
|
||||
i0, i1 = _nearest_imu(session, t[index]), _nearest_imu(session, t[index+1])
|
||||
if not valid or i0 is None or i1 is None: continue
|
||||
pre = preintegrate_imu(session.imu.t_s, session.imu.gyro_rad_s,
|
||||
session.imu.acc_m_s2, t[index], t[index+1])
|
||||
if pre.duration_s <= 0 or abs(pre.duration_s-dt) > 1e-6: continue
|
||||
R0 = _world_rtk(baseline) @ rotation
|
||||
p_i0 = p_ant[index] - R0 @ lever
|
||||
v_i0 = v_ant[index] - R0 @ np.cross(session.imu.gyro_rad_s[i0], lever)
|
||||
p_i1 = p_i0 + v_i0*dt + .5*G_ENU*dt**2 + R0@pre.delta_p
|
||||
v_i1 = v_i0 + G_ENU*dt + R0@pre.delta_v
|
||||
R1 = R0 @ pre.delta_R
|
||||
local_p.append(p_i1 + R1@lever - p_ant[index+1])
|
||||
local_v.append(v_i1 + R1@np.cross(session.imu.gyro_rad_s[i1],lever) - v_ant[index+1])
|
||||
all_p.extend(local_p); all_v.extend(local_v)
|
||||
per_session[session.session_id] = {
|
||||
'interval_count':len(local_p),'position_m':_summary(local_p),
|
||||
'velocity_m_s':_summary(local_v)}
|
||||
return {'method':'single-step forward closure; fixed R2G/mechanical lever/BEST p-v; no least_squares',
|
||||
'position_m':_summary(all_p),'velocity_m_s':_summary(all_v),
|
||||
'per_session':per_session}
|
||||
|
||||
def _qualified_runs(sessions, reference, period):
|
||||
result = []
|
||||
for session in sessions:
|
||||
nodes, start, number = _nodes(session, reference, period), 0, 0
|
||||
for end in range(1,len(nodes)+1):
|
||||
if end != len(nodes) and nodes[end].continuity_id == nodes[end-1].continuity_id:
|
||||
continue
|
||||
run, start = tuple(nodes[start:end]), end
|
||||
if (len(run) >= MIN_SEGMENT_NODE_COUNT
|
||||
and run[-1].t_s-run[0].t_s >= MIN_SEGMENT_DURATION_S
|
||||
and any(node.hpr_factor_valid for node in run)):
|
||||
result.append((session,run,f'{session.session_id}:{number:02d}'))
|
||||
number += 1
|
||||
return result
|
||||
|
||||
def _first_node_audit(runs, rotation, lever):
|
||||
records, ranges = [], {}
|
||||
for session, run, segment_id in runs:
|
||||
node = run[0]
|
||||
hpr_node = next(item for item in run if item.hpr_factor_valid)
|
||||
R_WI = _world_rtk(hpr_node.baseline_enu) @ rotation
|
||||
velocity = node.velocity_enu_m_s
|
||||
v = np.full(3,np.nan) if velocity is None else velocity
|
||||
records.append({
|
||||
'segment_id':segment_id,'source':node.source,'t_s':node.t_s,
|
||||
'first_position_global_enu_m':node.p_enu_m,'first_velocity_enu_m_s':v,
|
||||
'free_x0_l0_position_residual_m':np.zeros(3),
|
||||
'free_x0_l0_velocity_residual_m_s':-v,
|
||||
'mechanical_l_unshifted_x0_position_residual_m':R_WI@lever,
|
||||
'mechanical_l_unshifted_x0_velocity_residual_m_s':R_WI@np.cross(node.gyro_rad_s,lever)-v,
|
||||
'observation_seeded_mechanical_position_residual_m':np.zeros(3),
|
||||
'observation_seeded_mechanical_velocity_residual_m_s':
|
||||
np.zeros(3) if velocity is not None else np.full(3,np.nan)})
|
||||
ranges.setdefault(session.session_id,[]).append(node.p_enu_m)
|
||||
ranges = {key:{'count':len(value),'min_global_enu_m':np.min(value,axis=0),
|
||||
'max_global_enu_m':np.max(value,axis=0)} for key,value in ranges.items()}
|
||||
return {'common_global_enu_reference':True,'segment_state_is_global_imu_position':True,
|
||||
'per_session_first_node_ranges':ranges,'segments':records}
|
||||
|
||||
def _gyro_score(session, run, category):
|
||||
keep = (session.imu.t_s >= run[0].t_s) & (session.imu.t_s <= run[-1].t_s)
|
||||
t, gyro = session.imu.t_s[keep], session.imu.gyro_rad_s[keep]
|
||||
if len(t) < 2: return 0.
|
||||
value = np.trapezoid(np.abs(gyro),t,axis=0)
|
||||
return float(value[2] if category != 'slope' else np.hypot(value[0],value[1]))
|
||||
|
||||
def _build_segment(session, run, segment_id):
|
||||
pre = tuple(preintegrate_imu(session.imu.t_s,session.imu.gyro_rad_s,
|
||||
session.imu.acc_m_s2,a.t_s,b.t_s)
|
||||
for a,b in zip(run[:-1],run[1:]))
|
||||
hpr = next(node for node in run if node.hpr_factor_valid)
|
||||
return _Segment(segment_id,session.session_id,run,pre,_world_rtk(hpr.baseline_enu))
|
||||
|
||||
def _subset_marginal(jacobian,residual,segments,indices):
|
||||
rows, row0 = [], 0
|
||||
for index,segment in enumerate(segments):
|
||||
count = _segment_residual_size(segment)
|
||||
if index in indices: rows.extend(range(row0,row0+count))
|
||||
row0 += count
|
||||
columns = [0,1,2]
|
||||
for index in indices:
|
||||
start = 3 + NUISANCE_DOF_PER_SEGMENT*index
|
||||
columns.extend(range(start,start+NUISANCE_DOF_PER_SEGMENT))
|
||||
rows, columns = np.asarray(rows), np.asarray(columns)
|
||||
return _marginal_lever_information(
|
||||
jacobian[np.ix_(rows,columns)],residual[rows])[0]
|
||||
|
||||
def _jacobian_schur_audit(runs,categories,rotation,lever):
|
||||
segments, indices = [], {}
|
||||
for category in ('circle','left_right','slope'):
|
||||
choices = [item for item in runs if categories[item[0].session_id] == category]
|
||||
if not choices:
|
||||
indices[category] = []; continue
|
||||
best = max(choices,key=lambda item:_gyro_score(item[0],item[1],category))
|
||||
indices[category] = [len(segments)]
|
||||
segments.append(_build_segment(*best))
|
||||
x = _initial_parameters(segments); x[:3] = lever
|
||||
residual = _residual(x,segments,rotation)
|
||||
jacobian = _dense_colored_jacobian(x,segments,rotation)
|
||||
comparisons = []
|
||||
for axis in range(3):
|
||||
plus, minus = x.copy(), x.copy()
|
||||
plus[axis] += .001; minus[axis] -= .001
|
||||
numeric = (_residual(plus,segments,rotation)-_residual(minus,segments,rotation))/.002
|
||||
current = jacobian[:,axis]; delta = numeric-current
|
||||
comparisons.append({
|
||||
'axis':'XYZ'[axis],'perturbation_m':.001,
|
||||
'relative_difference':float(np.linalg.norm(delta)/max(np.linalg.norm(numeric),1e-12)),
|
||||
'difference_norm':float(np.linalg.norm(delta)),
|
||||
'max_absolute_difference':float(np.max(np.abs(delta))),
|
||||
'correlation':float(np.corrcoef(numeric,current)[0,1])})
|
||||
full = _marginal_lever_information(jacobian,residual)[0]
|
||||
parts = {key:_subset_marginal(jacobian,residual,segments,value)
|
||||
for key,value in indices.items() if value}
|
||||
summed = sum(parts.values(),np.zeros((3,3)))
|
||||
error = np.linalg.norm(summed-full,ord='fro')
|
||||
return {'linearization':'same observation-seeded nuisance state and mechanical lever; no optimizer',
|
||||
'selected_segments':[segment.segment_id for segment in segments],
|
||||
'finite_difference_vs_solver_jacobian':comparisons,
|
||||
'full_marginal_information':full,'category_marginal_information':parts,
|
||||
'category_sum':summed,'additivity_error_fro':float(error),
|
||||
'additivity_relative_error':float(error/max(np.linalg.norm(full,ord='fro'),1e-12))}
|
||||
|
||||
def _legacy_diagnostics(path):
|
||||
if path is None or not path.exists():
|
||||
return {'available':False,'reason':'no prior artifact supplied'}
|
||||
payload = json.loads(path.read_text(encoding='utf-8'))
|
||||
final_l = np.asarray(payload.get('free_solution',{}).get('l_I_m',[np.nan]*3))
|
||||
return {'available':False,'source':str(path),'initial_cost':None,'final_cost':None,
|
||||
'cost_reduction':None,'nfev':None,'optimality':None,'gradient_norm':None,
|
||||
'initial_l_I_m':[0.,0.,0.],'final_l_I_m':final_l,
|
||||
'l_step_norm_m':float(np.linalg.norm(final_l)) if np.all(np.isfinite(final_l)) else None,
|
||||
'reason':'legacy artifact did not persist optimizer state; prohibited solve was not rerun'}
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--circle-session',required=True)
|
||||
parser.add_argument('--left-right-session',required=True)
|
||||
parser.add_argument('--slope-session',required=True)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--max-best-interval-s',type=float,default=1.75)
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
|
||||
default=MECHANICAL_L_I_M.tolist())
|
||||
parser.add_argument('--previous-free-result',type=Path)
|
||||
parser.add_argument('--level-static-session',action='append',
|
||||
default=['0819_20260819_072130','0819_20260819_073045'])
|
||||
args = parser.parse_args()
|
||||
categories = {args.circle_session:'circle',args.left_right_session:'left_right',
|
||||
args.slope_session:'slope'}
|
||||
selected_ids = set(categories) | set(args.level_static_session)
|
||||
all_sessions = load_unified_sessions(args.manifest,selected_session_ids=selected_ids)
|
||||
sessions = [session for session in all_sessions if session.session_id in categories]
|
||||
static_sessions = [session for session in all_sessions
|
||||
if session.session_id in set(args.level_static_session)]
|
||||
reference = _height_reference(sessions)
|
||||
if reference is None: raise RuntimeError('no valid BESTNAVA height reference')
|
||||
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
lever = np.asarray(args.mechanical_l_I_m)
|
||||
runs = _qualified_runs(sessions,reference,args.sample_period_s)
|
||||
payload = {
|
||||
'scope':'factor consistency only; no free solve/LOO/prior/bootstrap/sensitivity',
|
||||
'least_squares_called':False,'rotation_source':'R2G_gravity_level_prior',
|
||||
'rotation_rpy_deg':args.rotation_rpy_deg,'mechanical_l_I_m':lever,
|
||||
'common_enu_reference':reference,
|
||||
'gnss_position_difference_vs_doppler':_gnss_audit(
|
||||
sessions,reference,args.max_best_interval_s),
|
||||
'static_specific_force_gravity_sign':_static_audit(static_sessions,rotation),
|
||||
'one_step_imu_preintegration_closure':_closure_audit(
|
||||
sessions,reference,rotation,lever,args.max_best_interval_s),
|
||||
'first_node_origin_and_initial_residual':_first_node_audit(runs,rotation,lever),
|
||||
'lever_jacobian_and_category_schur':_jacobian_schur_audit(
|
||||
runs,categories,rotation,lever),
|
||||
'previous_free_fit_solver_diagnostics':_legacy_diagnostics(args.previous_free_result)}
|
||||
args.output.parent.mkdir(parents=True,exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable({
|
||||
'gnss':payload['gnss_position_difference_vs_doppler'],
|
||||
'static':payload['static_specific_force_gravity_sign'],
|
||||
'closure':payload['one_step_imu_preintegration_closure'],
|
||||
'jacobian_schur':payload['lever_jacobian_and_category_schur']}),
|
||||
ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
if __name__ == '__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,151 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Audit BESTNAVA/Doppler factor yield for every unified RTK--IMU session."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import sys
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.rtk_imu_engineering import (
|
||||
_all_hpr,
|
||||
_height_reference,
|
||||
_nodes,
|
||||
_position_valid,
|
||||
)
|
||||
from imu_lidar.rtk_imu_multisource import _f, _nearest_index, _truth, load_unified_sessions
|
||||
|
||||
|
||||
def _finite_doppler(row: dict[str, str]) -> bool:
|
||||
value = np.asarray([
|
||||
_f(row, "velocity_east_m_s"),
|
||||
_f(row, "velocity_north_m_s"),
|
||||
_f(row, "vertical_speed_m_s"),
|
||||
])
|
||||
return bool(_truth(row, "doppler_velocity_valid") and np.all(np.isfinite(value)))
|
||||
|
||||
|
||||
def _source_only_nodes(session, source: str, period_s: float):
|
||||
rows = {source: session.rtk_by_type.get(source, [])}
|
||||
if "GNHPR" in session.rtk_by_type:
|
||||
rows["GNHPR"] = session.rtk_by_type["GNHPR"]
|
||||
return _nodes(replace(session, rtk_by_type=rows), _height_reference([session]), period_s)
|
||||
|
||||
|
||||
def _audit_session(session, period_s: float) -> dict[str, object]:
|
||||
best = session.rtk_by_type.get("BESTNAVA", [])
|
||||
hpr = _all_hpr(session)
|
||||
counts = {
|
||||
"raw_bestnava": len(best),
|
||||
"checksum_valid": 0,
|
||||
"fixed": 0,
|
||||
"finite_position": 0,
|
||||
"finite_doppler": 0,
|
||||
"hpr_near": 0,
|
||||
"hpr_q4": 0,
|
||||
"imu_near": 0,
|
||||
"raw_bestnava_candidate": 0,
|
||||
}
|
||||
for row in best:
|
||||
if not _truth(row, "checksum_valid"):
|
||||
continue
|
||||
counts["checksum_valid"] += 1
|
||||
if not _truth(row, "position_fixed"):
|
||||
continue
|
||||
counts["fixed"] += 1
|
||||
if not _position_valid(row, "BESTNAVA"):
|
||||
continue
|
||||
counts["finite_position"] += 1
|
||||
if not _finite_doppler(row):
|
||||
continue
|
||||
counts["finite_doppler"] += 1
|
||||
t_s = _f(row, "t_device_s")
|
||||
hpr_index = _nearest_index(hpr.t_s, t_s, 0.12)
|
||||
if hpr_index is None:
|
||||
continue
|
||||
counts["hpr_near"] += 1
|
||||
if not hpr.valid[hpr_index]:
|
||||
continue
|
||||
counts["hpr_q4"] += 1
|
||||
if _nearest_index(session.imu.t_s, t_s, 0.03) is None:
|
||||
continue
|
||||
counts["imu_near"] += 1
|
||||
counts["raw_bestnava_candidate"] += 1
|
||||
|
||||
reference = _height_reference([session])
|
||||
combined_nodes = _nodes(session, reference, period_s) if reference is not None else []
|
||||
best_nodes = _source_only_nodes(session, "BESTNAVA", period_s) if reference is not None else []
|
||||
combined_best = [node for node in combined_nodes if node.source == "BESTNAVA"]
|
||||
best_velocity_nodes = [node for node in combined_best if node.velocity_enu_m_s is not None]
|
||||
source_best_velocity = [node for node in best_nodes if node.velocity_enu_m_s is not None]
|
||||
run_nodes: dict[int, list] = {}
|
||||
for node in combined_nodes:
|
||||
run_nodes.setdefault(node.continuity_id, []).append(node)
|
||||
qualifying_runs = [
|
||||
run for run in run_nodes.values()
|
||||
if len(run) >= 6 and run[-1].t_s - run[0].t_s >= 5.0
|
||||
]
|
||||
qualifying_best = [
|
||||
node for run in qualifying_runs for node in run if node.source == "BESTNAVA"
|
||||
]
|
||||
qualifying_doppler = [node for node in qualifying_best if node.velocity_enu_m_s is not None]
|
||||
return {
|
||||
"session_id": session.session_id,
|
||||
"batch_id": session.batch_id,
|
||||
"counts": counts,
|
||||
"combined_selected_nodes": len(combined_nodes),
|
||||
"combined_selected_bestnava": len(combined_best),
|
||||
"combined_selected_doppler": len(best_velocity_nodes),
|
||||
"best_only_selected_nodes": len(best_nodes),
|
||||
"best_only_selected_doppler": len(source_best_velocity),
|
||||
"best_suppressed_by_mixed_selection": max(0, len(best_nodes) - len(combined_best)),
|
||||
"doppler_suppressed_by_mixed_selection": max(0, len(source_best_velocity) - len(best_velocity_nodes)),
|
||||
"continuous_run_count": len(run_nodes),
|
||||
"qualified_run_count": len(qualifying_runs),
|
||||
"qualified_bestnava_factors": len(qualifying_best),
|
||||
"qualified_doppler_factors": len(qualifying_doppler),
|
||||
}
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--sample-period-s", type=float, default=1.0)
|
||||
args = parser.parse_args(argv)
|
||||
sessions = load_unified_sessions(args.manifest)
|
||||
audit = [_audit_session(session, args.sample_period_s) for session in sessions]
|
||||
totals: dict[str, int] = {}
|
||||
for item in audit:
|
||||
for key, value in item["counts"].items():
|
||||
totals[key] = totals.get(key, 0) + int(value)
|
||||
for key in (
|
||||
"combined_selected_nodes", "combined_selected_bestnava", "combined_selected_doppler",
|
||||
"best_only_selected_nodes", "best_only_selected_doppler",
|
||||
"best_suppressed_by_mixed_selection", "doppler_suppressed_by_mixed_selection",
|
||||
"continuous_run_count", "qualified_run_count", "qualified_bestnava_factors",
|
||||
"qualified_doppler_factors",
|
||||
):
|
||||
totals[key] = totals.get(key, 0) + int(item[key])
|
||||
payload = {
|
||||
"sample_period_s": args.sample_period_s,
|
||||
"session_count": len(audit),
|
||||
"totals": totals,
|
||||
"sessions": audit,
|
||||
}
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
args.output.write_text(json.dumps(payload, ensure_ascii=False, indent=2, allow_nan=False) + "\n", encoding="utf-8")
|
||||
print(json.dumps({"session_count": len(audit), "totals": totals}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,158 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Independent prediction innovations on the frozen 267 held-out windows.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
from tools.audit_rtk_imu_innovation_noise import (
|
||||
_factor_report,_hpr_innovations,_interval_innovations)
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
|
||||
LIMITS={
|
||||
'best_position':{'bias_norm':.10,'vector_p95':.50},
|
||||
'doppler':{'bias_norm':.10,'vector_p95':.50},
|
||||
'hpr':{'bias_norm':.01,'vector_p95':.05},
|
||||
}
|
||||
|
||||
def _finite_max_abs(values):
|
||||
a=np.asarray(values,dtype=float)
|
||||
return float(np.max(np.abs(a[np.isfinite(a)]))) if np.any(np.isfinite(a)) else np.inf
|
||||
|
||||
def _summary_gate(summary,limits,min_samples=20):
|
||||
if summary.get('sample_count',0)<min_samples:
|
||||
return {'evaluated':False,'passed':True,'reason':'insufficient_samples'}
|
||||
checks={'bias_norm':float(np.linalg.norm(summary['innovation_bias']))<=limits['bias_norm'],
|
||||
'vector_p95':summary['vector_p95']<=limits['vector_p95'],
|
||||
'lag1_autocorrelation_abs':_finite_max_abs(
|
||||
summary['temporal_autocorrelation_lag1'])<=.95}
|
||||
return {'evaluated':True,'thresholds':{**limits,'lag1_abs_max':.95},
|
||||
'checks':checks,'passed':bool(all(checks.values()))}
|
||||
|
||||
def _factor_gate(report,key):
|
||||
limits=LIMITS[key]; overall=_summary_gate(report['overall'],limits,1)
|
||||
grouped={}
|
||||
for group_name in ('by_session','by_motion','by_speed','by_gyro_norm'):
|
||||
grouped[group_name]={name:_summary_gate(value,limits)
|
||||
for name,value in report[group_name].items()}
|
||||
evaluated=[gate for group in grouped.values() for gate in group.values()
|
||||
if gate['evaluated']]
|
||||
passed=overall['passed'] and all(gate['passed'] for gate in evaluated)
|
||||
return {'overall':overall,'grouped':grouped,'passed':bool(passed)}
|
||||
|
||||
def _calibration_biases(engineering,problems):
|
||||
source=engineering['prior_constrained_solution'].get(
|
||||
'calibration_only_frozen_bias_by_session')
|
||||
if not source:
|
||||
raise RuntimeError('engineering result lacks calibration-only frozen bias')
|
||||
by_date={}
|
||||
for session_id,value in source.items():
|
||||
by_date.setdefault(session_id.split('_')[1],[]).append(value)
|
||||
result={}
|
||||
for problem in problems:
|
||||
session_id=problem.segment.session_id
|
||||
if session_id in result: continue
|
||||
if session_id in source:
|
||||
value=source[session_id]; origin='same_calibration_session'
|
||||
else:
|
||||
values=by_date.get(session_id.split('_')[1],list(source.values()))
|
||||
weights=np.asarray([x['node_count'] for x in values],dtype=float)
|
||||
value={'gyro_bias_rad_s':np.average(
|
||||
[x['gyro_bias_rad_s'] for x in values],axis=0,weights=weights),
|
||||
'accel_bias_m_s2':np.average(
|
||||
[x['accel_bias_m_s2'] for x in values],axis=0,weights=weights)}
|
||||
origin=('calibration_date_weighted_mean' if
|
||||
session_id.split('_')[1] in by_date else 'calibration_global_weighted_mean')
|
||||
result[session_id]={'gyro_bias_rad_s':value['gyro_bias_rad_s'],
|
||||
'accel_bias_m_s2':value['accel_bias_m_s2'],'source':origin}
|
||||
return result
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--manifest',type=Path,required=True)
|
||||
p.add_argument('--calibration-selection',type=Path,required=True)
|
||||
p.add_argument('--all-selection',type=Path,required=True)
|
||||
p.add_argument('--engineering-result',type=Path,required=True)
|
||||
p.add_argument('--output',type=Path,required=True)
|
||||
p.add_argument('--sample-period-s',type=float,default=1.)
|
||||
p.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
p.add_argument('--circle-session',default='0808_20260808_092827')
|
||||
p.add_argument('--left-right-session',default='0808_20260808_082148')
|
||||
p.add_argument('--slope-session',default='0815_20260812_123424')
|
||||
args=p.parse_args()
|
||||
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
|
||||
calibration=json.loads(args.calibration_selection.read_text(encoding='utf-8'))
|
||||
selected=json.loads(args.all_selection.read_text(encoding='utf-8'))
|
||||
calibration_ids={x['candidate_id'] for x in calibration['selected_windows']}
|
||||
heldout=[x for x in selected['selected_windows']
|
||||
if x['candidate_id'] not in calibration_ids]
|
||||
if len(calibration_ids)!=47 or len(heldout)!=267:
|
||||
raise RuntimeError(f'expected 47+267 windows, got {len(calibration_ids)}+{len(heldout)}')
|
||||
shared=sum(x.get('shared_sample_count_with_previous',{}).get(k,0)
|
||||
for x in selected['selected_windows'] for k in ('imu','gnss','hpr'))
|
||||
if shared: raise RuntimeError(f'selection contains {shared} shared samples')
|
||||
lever=np.asarray(engineering.get('engineering_l_I_m',
|
||||
engineering['prior_constrained_solution']['result']['final_l_I_m']),dtype=float)
|
||||
sessions=load_unified_sessions(
|
||||
args.manifest,selected_session_ids={x['session_id'] for x in heldout})
|
||||
reference=_height_reference(sessions)
|
||||
segments=_restore_segments(sessions,reference,heldout,args.sample_period_s)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
problems=[build_problem(segment,rotation,lever,args.hpr_direct_sigma_rad)
|
||||
for segment in segments]
|
||||
session_by_id={session.session_id:session for session in sessions}
|
||||
motion_by_session={args.circle_session:'circle',
|
||||
args.left_right_session:'left_right',args.slope_session:'slope'}
|
||||
records={key:[] for key in ('best_position','doppler','hpr','imu_preintegration')}
|
||||
biases=_calibration_biases(engineering,problems)
|
||||
for problem in problems:
|
||||
motion=motion_by_session.get(problem.segment.session_id,'other_recovered_dynamic')
|
||||
bias=biases[problem.segment.session_id]
|
||||
interval=_interval_innovations(problem,motion,
|
||||
bias['gyro_bias_rad_s'],bias['accel_bias_m_s2'])
|
||||
for key,value in interval.items(): records[key].extend(value)
|
||||
records['hpr'].extend(_hpr_innovations(
|
||||
session_by_id[problem.segment.session_id],problem,motion))
|
||||
factors={key:_factor_report(value) for key,value in records.items()}
|
||||
gates={key:_factor_gate(factors[key],key)
|
||||
for key in ('best_position','doppler','hpr')}
|
||||
passed=bool(all(value['passed'] for value in gates.values()))
|
||||
payload={'scope':'267-window independent held-out prediction innovations',
|
||||
'least_squares_called':False,'target_observation_used_by_predictor':False,
|
||||
'lever_reoptimized':False,'rotation_reoptimized':False,
|
||||
'covariance_parameters_modified':False,
|
||||
'fixed_l_I_m':lever,'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
|
||||
'calibration_window_count':47,'heldout_window_count':267,
|
||||
'calibration_heldout_overlap_count':0,
|
||||
'motion_class_mapping':motion_by_session,
|
||||
'prediction_definition':{
|
||||
'BEST_position':'previous GNSS p/v + IMU preintegration + fixed lever',
|
||||
'Doppler':'previous GNSS velocity + IMU preintegration + omega-cross-lever',
|
||||
'HPR':'withheld direct HPR predicted by LOO interpolation and gyro propagation',
|
||||
'IMU_preintegration':'two independently GNSS/HPR-anchored endpoints'},
|
||||
'bias_source':('frozen prior-node bias from the disjoint 47-window calibration set; '
|
||||
'same-session when available, otherwise calibration date-weighted mean; '
|
||||
'no held-out target observation and no covariance writeback'),
|
||||
'per_session_frozen_bias':biases,
|
||||
'factors':factors,'validation_limits_are_physical_not_covariance_retuning':LIMITS,
|
||||
'factor_gates':gates,'independent_heldout_innovation_passed':passed}
|
||||
args.output.parent.mkdir(parents=True,exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
compact={key:{'samples':value['overall'].get('sample_count',0),
|
||||
'bias':value['overall'].get('innovation_bias'),
|
||||
'vector_p95':value['overall'].get('vector_p95'),
|
||||
'nis_per_dof':value['overall'].get('nis_per_dof'),
|
||||
'gate':gates.get(key)} for key,value in factors.items()}
|
||||
print(json.dumps(_jsonable({'factors':compact,
|
||||
'independent_heldout_innovation_passed':passed}),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
if __name__=='__main__': raise SystemExit(main())
|
||||
@@ -0,0 +1,286 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Independent prediction/innovation noise audit for node-state RTK/IMU factors.'''
|
||||
from __future__ import annotations
|
||||
import argparse, json, math, sys
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.geometry import so3_log
|
||||
from imu_lidar.imu_preintegration import apply_bias_correction_imu,preintegrate_gyro
|
||||
from imu_lidar.rtk_imu_engineering import (
|
||||
G_ENU,HPR_DIRECT_ANGULAR_SIGMA_RAD,_all_hpr,_height_reference,
|
||||
_world_rtk)
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem
|
||||
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
|
||||
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
|
||||
|
||||
BEST_SIGMA = np.array([.06,.06,.12])
|
||||
DOPPLER_SIGMA = np.array([.15,.15,.30])
|
||||
|
||||
def _whiten(value,covariance):
|
||||
covariance = .5*(covariance+covariance.T)+np.eye(len(value))*1e-12
|
||||
return np.linalg.solve(np.linalg.cholesky(covariance),value)
|
||||
|
||||
def _bin_speed(value):
|
||||
if value < .2: return 'speed_lt_0p2'
|
||||
if value < 1.: return 'speed_0p2_to_1'
|
||||
return 'speed_ge_1'
|
||||
|
||||
def _bin_gyro(value):
|
||||
if value < .02: return 'gyro_lt_0p02'
|
||||
if value < .10: return 'gyro_0p02_to_0p10'
|
||||
return 'gyro_ge_0p10'
|
||||
|
||||
def _bin_gap(value):
|
||||
if not np.isfinite(value): return 'gap_unavailable'
|
||||
if value <= .12: return 'gap_direct'
|
||||
if value <= .3: return 'gap_0p12_to_0p3'
|
||||
if value <= .5: return 'gap_0p3_to_0p5'
|
||||
return 'gap_gt_0p5'
|
||||
|
||||
def _distribution(values):
|
||||
a = np.asarray(values,dtype=float).reshape(-1)
|
||||
if not a.size:
|
||||
return {'count':0,'rms':np.nan,'p50_abs':np.nan,'p95_abs':np.nan,'p99_abs':np.nan}
|
||||
return {'count':len(a),'rms':float(np.sqrt(np.mean(a*a))),
|
||||
'p50_abs':float(np.percentile(np.abs(a),50.)),
|
||||
'p95_abs':float(np.percentile(np.abs(a),95.)),
|
||||
'p99_abs':float(np.percentile(np.abs(a),99.))}
|
||||
|
||||
def _autocorrelation(vectors):
|
||||
a = np.asarray(vectors,dtype=float)
|
||||
result = np.full(a.shape[1] if a.ndim == 2 else 0,np.nan)
|
||||
if a.ndim != 2 or len(a) < 3: return result
|
||||
for axis in range(a.shape[1]):
|
||||
left,right = a[:-1,axis],a[1:,axis]
|
||||
if np.std(left)>1e-12 and np.std(right)>1e-12:
|
||||
result[axis] = np.corrcoef(left,right)[0,1]
|
||||
return result
|
||||
|
||||
def _summarize(records):
|
||||
if not records: return {'sample_count':0}
|
||||
residual = np.asarray([item['residual'] for item in records])
|
||||
normalized = np.asarray([item['normalized'] for item in records])
|
||||
factor_only = np.asarray([item['normalized_factor_only'] for item in records])
|
||||
nis = np.sum(normalized*normalized,axis=1)
|
||||
dimension = residual.shape[1]
|
||||
effective_dimension = int(records[0].get('effective_dimension',dimension))
|
||||
total_dof = len(records)*effective_dimension
|
||||
norm = np.linalg.norm(residual,axis=1)
|
||||
centered_t = np.asarray([item['t_s'] for item in records],dtype=float)
|
||||
centered_t -= np.mean(centered_t)
|
||||
temporal_drift = np.zeros(dimension)
|
||||
if len(records)>=3 and np.ptp(centered_t)>1e-9:
|
||||
temporal_drift = np.asarray([
|
||||
np.polyfit(centered_t,residual[:,axis],1)[0]
|
||||
for axis in range(dimension)])
|
||||
return {'sample_count':len(records),'dimension':dimension,
|
||||
'effective_dof_per_sample':effective_dimension,
|
||||
'innovation_bias':np.mean(residual,axis=0),
|
||||
'innovation_distribution':_distribution(residual),
|
||||
'axis_rms':np.sqrt(np.mean(residual*residual,axis=0)),
|
||||
'axis_p50_abs':np.percentile(np.abs(residual),50.,axis=0),
|
||||
'axis_p95_abs':np.percentile(np.abs(residual),95.,axis=0),
|
||||
'axis_p99_abs':np.percentile(np.abs(residual),99.,axis=0),
|
||||
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
|
||||
'vector_p50':float(np.percentile(norm,50.)),
|
||||
'vector_p95':float(np.percentile(norm,95.)),
|
||||
'vector_p99':float(np.percentile(norm,99.)),
|
||||
'normalized_distribution':_distribution(normalized),
|
||||
'empirical_covariance':np.cov(residual,rowvar=False),
|
||||
'nis_distribution':_distribution(nis),
|
||||
'nis_total':float(np.sum(nis)),'nis_per_dof':float(np.sum(nis)/total_dof),
|
||||
'alpha_factor':float(np.sum(nis)/total_dof),
|
||||
'alpha_composite_prediction':float(np.sum(nis)/total_dof),
|
||||
'alpha_factor_only_upper_bound':float(np.sum(factor_only*factor_only)/total_dof),
|
||||
'temporal_autocorrelation_lag1':_autocorrelation(residual),
|
||||
'temporal_linear_drift_per_s':temporal_drift}
|
||||
|
||||
def _group(records,key):
|
||||
values = {}
|
||||
for item in records: values.setdefault(str(item[key]),[]).append(item)
|
||||
return {name:_summarize(items) for name,items in values.items()}
|
||||
|
||||
def _base_record(session_id,motion,t,speed,gyro_norm,gap):
|
||||
return {'session':session_id,'motion':motion,'t_s':float(t),
|
||||
'speed_bin':_bin_speed(speed),'gyro_bin':_bin_gyro(gyro_norm),
|
||||
'hpr_gap_bin':_bin_gap(gap)}
|
||||
|
||||
def _interval_innovations(problem,motion,bg=None,ba=None):
|
||||
records = {'best_position':[],'doppler':[],'imu_preintegration':[]}
|
||||
lever = problem.fixed_l_I_m
|
||||
bg=np.zeros(3) if bg is None else np.asarray(bg,dtype=float)
|
||||
ba=np.zeros(3) if ba is None else np.asarray(ba,dtype=float)
|
||||
nodes = problem.segment.nodes
|
||||
for index,(left,right,pre) in enumerate(zip(nodes[:-1],nodes[1:],problem.segment.preintegrations)):
|
||||
if not (left.hpr_factor_valid and right.hpr_factor_valid): continue
|
||||
if left.velocity_enu_m_s is None or right.velocity_enu_m_s is None: continue
|
||||
R0 = _world_rtk(left.baseline_enu)@problem.R_RTK_IMU
|
||||
R1 = _world_rtk(right.baseline_enu)@problem.R_RTK_IMU
|
||||
p_i0 = left.p_enu_m-R0@lever
|
||||
p_i1 = right.p_enu_m-R1@lever
|
||||
v_i0 = left.velocity_enu_m_s-R0@np.cross(left.gyro_rad_s-bg,lever)
|
||||
v_i1 = right.velocity_enu_m_s-R1@np.cross(right.gyro_rad_s-bg,lever)
|
||||
dt = pre.duration_s
|
||||
delta_R,delta_v,delta_p=apply_bias_correction_imu(pre,bg,ba)
|
||||
pred_p_i1 = p_i0+v_i0*dt+.5*G_ENU*dt*dt+R0@delta_p
|
||||
pred_v_i1 = v_i0+G_ENU*dt+R0@delta_v
|
||||
pred_R1 = R0@delta_R
|
||||
p_innovation = pred_p_i1+pred_R1@lever-right.p_enu_m
|
||||
v_innovation = pred_v_i1+pred_R1@np.cross(
|
||||
right.gyro_rad_s-bg,lever)-right.velocity_enu_m_s
|
||||
speed = float(np.linalg.norm(left.velocity_enu_m_s))
|
||||
gyro_norm = float(pre.mean_gyro_norm)
|
||||
gap = max(left.hpr_support_gap_s,right.hpr_support_gap_s)
|
||||
base = _base_record(problem.segment.session_id,motion,right.t_s,speed,gyro_norm,gap)
|
||||
base.update({'interval_id':f'{problem.segment.segment_id}:{index}',
|
||||
'dt_s':float(dt),'R0_WI':R0})
|
||||
p_cov = (np.diag(BEST_SIGMA**2)+dt*dt*np.diag(DOPPLER_SIGMA**2)
|
||||
+R0@pre.cov[6:9,6:9]@R0.T+np.diag(BEST_SIGMA**2))
|
||||
v_cov = (np.diag(DOPPLER_SIGMA**2)+R0@pre.cov[3:6,3:6]@R0.T
|
||||
+np.diag(DOPPLER_SIGMA**2))
|
||||
records['best_position'].append({**base,'residual':p_innovation,
|
||||
'normalized':_whiten(p_innovation,p_cov),
|
||||
'normalized_factor_only':p_innovation/BEST_SIGMA})
|
||||
records['doppler'].append({**base,'residual':v_innovation,
|
||||
'normalized':_whiten(v_innovation,v_cov),
|
||||
'normalized_factor_only':v_innovation/DOPPLER_SIGMA})
|
||||
imu_error = np.concatenate([
|
||||
so3_log(delta_R.T@R0.T@R1),
|
||||
R0.T@(v_i1-v_i0-G_ENU*dt)-delta_v,
|
||||
R0.T@(p_i1-p_i0-v_i0*dt-.5*G_ENU*dt*dt)-delta_p])
|
||||
anchored_cov = pre.cov.copy()
|
||||
hpr_var = left.hpr_angular_sigma_rad**2+right.hpr_angular_sigma_rad**2
|
||||
anchored_cov[:3,:3] += np.eye(3)*hpr_var
|
||||
anchored_cov[3:6,3:6] += R0.T@np.diag(2.*DOPPLER_SIGMA**2)@R0
|
||||
anchored_cov[6:9,6:9] += R0.T@np.diag(
|
||||
2.*BEST_SIGMA**2+dt*dt*DOPPLER_SIGMA**2)@R0
|
||||
records['imu_preintegration'].append({**base,'residual':imu_error,
|
||||
'normalized':_whiten(imu_error,anchored_cov),
|
||||
'normalized_factor_only':_whiten(imu_error,pre.cov)})
|
||||
return records
|
||||
|
||||
def _hpr_innovations(session,problem,motion):
|
||||
hpr = _all_hpr(session)
|
||||
indices = [int(i) for i in hpr.valid_indices
|
||||
if problem.segment.nodes[0].t_s <= hpr.t_s[i] <= problem.segment.nodes[-1].t_s]
|
||||
records = []
|
||||
baseline_I = problem.R_RTK_IMU.T[:,0]
|
||||
node_times = np.asarray([node.t_s for node in problem.segment.nodes])
|
||||
for position in range(1,len(indices)-1):
|
||||
left,index,right = indices[position-1],indices[position],indices[position+1]
|
||||
t0,t,t1 = hpr.t_s[left],hpr.t_s[index],hpr.t_s[right]
|
||||
total_gap = float(t1-t0)
|
||||
if not 0. < total_gap <= .5: continue
|
||||
fraction = float((t-t0)/total_gap)
|
||||
predicted = (1.-fraction)*hpr.baseline_enu[left]+fraction*hpr.baseline_enu[right]
|
||||
predicted /= np.linalg.norm(predicted)
|
||||
innovation = np.cross(predicted,hpr.baseline_enu[index])
|
||||
sigma = HPR_DIRECT_ANGULAR_SIGMA_RAD*np.sqrt(
|
||||
1.+(1.-fraction)**2+fraction**2)
|
||||
nearest = int(np.argmin(np.abs(node_times-t)))
|
||||
node = problem.segment.nodes[nearest]
|
||||
speed = float(np.linalg.norm(node.velocity_enu_m_s)) if node.velocity_enu_m_s is not None else 0.
|
||||
gyro_norm = float(np.linalg.norm(node.gyro_rad_s))
|
||||
base = _base_record(session.session_id,motion,t,speed,gyro_norm,total_gap)
|
||||
records.append({**base,'method':'leave_one_out_interpolation',
|
||||
'effective_dimension':2,'residual':innovation,
|
||||
'normalized':innovation/sigma,
|
||||
'normalized_factor_only':innovation/HPR_DIRECT_ANGULAR_SIGMA_RAD})
|
||||
gyro_pre = preintegrate_gyro(session.imu.t_s,session.imu.gyro_rad_s,float(t0),float(t))
|
||||
R0 = _world_rtk(hpr.baseline_enu[left])@problem.R_RTK_IMU
|
||||
propagated = R0@gyro_pre.delta_R@baseline_I
|
||||
gyro_innovation = np.cross(propagated,hpr.baseline_enu[index])
|
||||
gyro_sigma = np.sqrt(2.*HPR_DIRECT_ANGULAR_SIGMA_RAD**2
|
||||
+float(np.trace(gyro_pre.cov))/3.)
|
||||
records.append({**base,'method':'gyro_propagation',
|
||||
'effective_dimension':2,'residual':gyro_innovation,
|
||||
'normalized':gyro_innovation/gyro_sigma,
|
||||
'normalized_factor_only':gyro_innovation/HPR_DIRECT_ANGULAR_SIGMA_RAD})
|
||||
return records
|
||||
|
||||
def _factor_report(records):
|
||||
return {'overall':_summarize(records),
|
||||
'by_session':_group(records,'session'),'by_motion':_group(records,'motion'),
|
||||
'by_speed':_group(records,'speed_bin'),'by_gyro_norm':_group(records,'gyro_bin'),
|
||||
'by_hpr_bridge_gap':_group(records,'hpr_gap_bin'),
|
||||
'by_method':_group(records,'method') if records and 'method' in records[0] else {}}
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--circle-session',required=True)
|
||||
parser.add_argument('--left-right-session',required=True)
|
||||
parser.add_argument('--slope-session',required=True)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--target-duration-s',type=float,default=15.)
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
|
||||
default=MECHANICAL_L_I_M.tolist())
|
||||
args = parser.parse_args()
|
||||
categories = {'circle':args.circle_session,'left_right':args.left_right_session,
|
||||
'slope':args.slope_session}
|
||||
sessions = load_unified_sessions(args.manifest,selected_session_ids=set(categories.values()))
|
||||
reference = _height_reference(sessions)
|
||||
if reference is None: raise RuntimeError('no BEST reference')
|
||||
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
lever = np.asarray(args.mechanical_l_I_m)
|
||||
selections, all_records = {}, {
|
||||
'best_position':[],'doppler':[],'hpr':[],'imu_preintegration':[]}
|
||||
for motion,session_id in categories.items():
|
||||
session = next(item for item in sessions if item.session_id == session_id)
|
||||
segment,selection = _select_window(
|
||||
[session],reference,args.sample_period_s,args.target_duration_s)
|
||||
selections[motion] = selection
|
||||
problem = build_problem(segment,rotation,lever)
|
||||
interval = _interval_innovations(problem,motion)
|
||||
for key,records in interval.items(): all_records[key].extend(records)
|
||||
all_records['hpr'].extend(_hpr_innovations(session,problem,motion))
|
||||
factors = {key:_factor_report(records) for key,records in all_records.items()}
|
||||
payload = {
|
||||
'scope':'independent prediction innovations; no node optimization/covariance writeback',
|
||||
'least_squares_called':False,'covariance_parameters_modified':False,
|
||||
'fixed_l_I_m':lever,'selections':selections,
|
||||
'current_physical_sigma':{
|
||||
'best_position_xyz_m':BEST_SIGMA,
|
||||
'doppler_xyz_m_s':DOPPLER_SIGMA,
|
||||
'hpr_direct_angular_rad':HPR_DIRECT_ANGULAR_SIGMA_RAD,
|
||||
'bias_random_walk_source':'unchanged device/static/Allan noise model'},
|
||||
'alpha_semantics':{
|
||||
'alpha_factor':'innovation NIS/dof using composite prediction covariance',
|
||||
'alpha_factor_only_upper_bound':(
|
||||
'innovation divided only by current factor covariance; includes predictor and '
|
||||
'endpoint-anchor noise and must not be written back directly')},
|
||||
'factors':factors,
|
||||
'recommended_covariance_writeback':False,
|
||||
'covariance_freeze_assessment':{
|
||||
'best_position':'predictor-confounded; unchanged',
|
||||
'doppler':'predictor-confounded; unchanged',
|
||||
'imu_preintegration':'endpoint-anchor dominated; unchanged',
|
||||
'bias_random_walk':'static/Allan/device-model based; unchanged',
|
||||
'hpr':{
|
||||
'identifiable':True,
|
||||
'evidence':'264 withheld samples; LOO and gyro predictions agree',
|
||||
'frozen_direct_sigma_rad':.006,
|
||||
'writeback_policy':'explicit audit decision, not automatic alpha'}}}
|
||||
args.output.parent.mkdir(parents=True,exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
compact = {key:{'count':value['overall'].get('sample_count',0),
|
||||
'bias':value['overall'].get('innovation_bias'),
|
||||
'rms':value['overall'].get('innovation_distribution',{}).get('rms'),
|
||||
'p95':value['overall'].get('innovation_distribution',{}).get('p95_abs'),
|
||||
'alpha_composite':value['overall'].get('alpha_composite_prediction'),
|
||||
'alpha_factor_only_upper_bound':value['overall'].get('alpha_factor_only_upper_bound'),
|
||||
'autocorrelation':value['overall'].get('temporal_autocorrelation_lag1')}
|
||||
for key,value in factors.items()}
|
||||
print(json.dumps(_jsonable(compact),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
if __name__ == '__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,145 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Audit strict RTK--IMU segments for lever-arm excitation and marginal information."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.rtk_imu_engineering import _fit_segments, _fit_summary, _segments
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
|
||||
|
||||
def _jsonable(value):
|
||||
if isinstance(value, np.ndarray):
|
||||
return _jsonable(value.tolist())
|
||||
if isinstance(value, np.generic):
|
||||
return _jsonable(value.item())
|
||||
if isinstance(value, float):
|
||||
return value if math.isfinite(value) else None
|
||||
if hasattr(value, "__dataclass_fields__"):
|
||||
return {key: _jsonable(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): _jsonable(item) for key, item in value.items()}
|
||||
if isinstance(value, (tuple, list)):
|
||||
return [_jsonable(item) for item in value]
|
||||
return value
|
||||
|
||||
|
||||
def _orientation_spans_deg(segment, rotation_rtk_imu: np.ndarray) -> np.ndarray:
|
||||
matrices = [segment.R_WRTK_initial @ rotation_rtk_imu]
|
||||
for pre in segment.preintegrations:
|
||||
matrices.append(matrices[-1] @ pre.delta_R)
|
||||
euler = Rotation.from_matrix(np.asarray(matrices)).as_euler("xyz", degrees=False)
|
||||
return np.degrees(np.ptp(np.unwrap(euler, axis=0), axis=0))
|
||||
|
||||
|
||||
def _audit_segment(segment, rotation_rtk_imu: np.ndarray, max_nfev: int) -> dict[str, object]:
|
||||
gyro = np.asarray([node.gyro_rad_s for node in segment.nodes])
|
||||
gyro_norm = np.linalg.norm(gyro, axis=1)
|
||||
best_count = sum(node.source == "BESTNAVA" for node in segment.nodes)
|
||||
doppler_count = sum(
|
||||
node.source == "BESTNAVA" and node.velocity_enu_m_s is not None
|
||||
for node in segment.nodes
|
||||
)
|
||||
fit, residual, detail = _fit_segments([segment], rotation_rtk_imu, max_nfev=max_nfev)
|
||||
summary = _fit_summary(fit, residual, detail)
|
||||
item: dict[str, object] = {
|
||||
"segment_id": segment.segment_id,
|
||||
"session_id": segment.session_id,
|
||||
"node_count": len(segment.nodes),
|
||||
"duration_s": float(segment.nodes[-1].t_s - segment.nodes[0].t_s),
|
||||
"yaw_pitch_roll_span_deg": _orientation_spans_deg(segment, rotation_rtk_imu)[[2, 1, 0]],
|
||||
"gyro_rms_deg_s": np.degrees(np.sqrt(np.mean(gyro ** 2, axis=0))),
|
||||
"gyro_peak_deg_s": np.degrees(np.max(np.abs(gyro), axis=0)),
|
||||
"gyro_norm_rms_deg_s": float(np.degrees(np.sqrt(np.mean(gyro_norm ** 2)))),
|
||||
"gyro_norm_peak_deg_s": float(np.degrees(np.max(gyro_norm))),
|
||||
"bestnava_count": best_count,
|
||||
"doppler_count": doppler_count,
|
||||
"fit": None,
|
||||
}
|
||||
if summary is not None:
|
||||
item["fit"] = {
|
||||
"optimizer_converged": summary.optimizer_converged,
|
||||
"lever_information_singular_values": summary.lever_information_singular_values,
|
||||
"lever_information_condition_number": summary.lever_information_condition_number,
|
||||
"lever_precision_rank": summary.lever_precision_rank,
|
||||
"weakest_lever_direction_I": summary.weakest_lever_direction_I,
|
||||
"lever_std_m": summary.l_I_std_m,
|
||||
}
|
||||
singular = summary.lever_information_singular_values
|
||||
item["information_score"] = float(singular[-1]) if summary.lever_precision_rank == 3 else 0.0
|
||||
else:
|
||||
item["information_score"] = 0.0
|
||||
spans = np.asarray(item["yaw_pitch_roll_span_deg"])
|
||||
# Short strict runs rarely accumulate a full vehicle turn; retain clearly non-straight motion.
|
||||
item["turn_or_slope"] = bool(spans[0] >= 3.0 or abs(spans[1]) >= 0.5 or abs(spans[2]) >= 0.5)
|
||||
return item
|
||||
|
||||
|
||||
def _recommended(items: list[dict[str, object]]) -> list[str]:
|
||||
candidates = [
|
||||
item for item in items
|
||||
if item["turn_or_slope"] and int(item["bestnava_count"]) >= 6
|
||||
and int(item["doppler_count"]) >= 6
|
||||
and item["fit"] is not None
|
||||
and int(item["fit"]["lever_precision_rank"]) == 3
|
||||
]
|
||||
candidates.sort(key=lambda item: float(item["information_score"]), reverse=True)
|
||||
selected: list[str] = []
|
||||
per_session: dict[str, int] = {}
|
||||
for item in candidates:
|
||||
session_id = str(item["session_id"])
|
||||
if per_session.get(session_id, 0) >= 2:
|
||||
continue
|
||||
selected.append(str(item["segment_id"]))
|
||||
per_session[session_id] = per_session.get(session_id, 0) + 1
|
||||
return selected
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--sample-period-s", type=float, default=1.0)
|
||||
parser.add_argument("--rotation-rpy-deg", nargs=3, type=float,
|
||||
default=[0.4543066225, -0.0026392019, 0.0122384129])
|
||||
parser.add_argument("--max-nfev", type=int, default=80)
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
sessions = load_unified_sessions(args.manifest)
|
||||
rotation = Rotation.from_euler("xyz", args.rotation_rpy_deg, degrees=True).as_matrix()
|
||||
items = [
|
||||
_audit_segment(segment, rotation, args.max_nfev)
|
||||
for segment in _segments(sessions, args.sample_period_s)
|
||||
]
|
||||
payload = {
|
||||
"session_count": len(sessions),
|
||||
"strict_segment_count": len(items),
|
||||
"sample_period_s": args.sample_period_s,
|
||||
"rotation_rpy_deg": args.rotation_rpy_deg,
|
||||
"recommended_segment_ids": _recommended(items),
|
||||
"segments": items,
|
||||
}
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload), ensure_ascii=False, indent=2, allow_nan=False) + "\n", encoding="utf-8")
|
||||
print(json.dumps({
|
||||
"strict_segment_count": len(items),
|
||||
"recommended_segment_ids": payload["recommended_segment_ids"],
|
||||
}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,668 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Penetration audit for RTK--IMU motion excitation.
|
||||
|
||||
This is read-only diagnostics. It never applies a lever prior, solves a lever arm,
|
||||
or changes continuity/acceptance thresholds. Gyro trajectory integrals are the
|
||||
primary excitation metrics; start/end Euler differences are deliberately absent.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
from typing import Iterable
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.imu_audit import audit_imu
|
||||
from imu_lidar.rtk_imu_engineering import (
|
||||
MIN_SEGMENT_DURATION_S,
|
||||
MIN_SEGMENT_NODE_COUNT,
|
||||
_all_hpr,
|
||||
_height_reference,
|
||||
_hpr_factor_observation,
|
||||
_node_interval_threshold_s,
|
||||
_trajectory_continuity_reasons,
|
||||
_nearest_index,
|
||||
_nodes,
|
||||
_position_valid,
|
||||
_segments,
|
||||
_source_nodes,
|
||||
)
|
||||
from imu_lidar.rtk_imu_multisource import _f, _truth, load_unified_sessions
|
||||
|
||||
|
||||
RAW_GAP_S = 1.5
|
||||
|
||||
|
||||
def _jsonable(value):
|
||||
if isinstance(value, np.ndarray):
|
||||
return _jsonable(value.tolist())
|
||||
if isinstance(value, np.generic):
|
||||
return _jsonable(value.item())
|
||||
if isinstance(value, float):
|
||||
return value if math.isfinite(value) else None
|
||||
if hasattr(value, "__dataclass_fields__"):
|
||||
return {key: _jsonable(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): _jsonable(item) for key, item in value.items()}
|
||||
if isinstance(value, (tuple, list)):
|
||||
return [_jsonable(item) for item in value]
|
||||
return value
|
||||
|
||||
|
||||
def _trapz(values: np.ndarray, t_s: np.ndarray) -> np.ndarray:
|
||||
if t_s.size < 2:
|
||||
return np.zeros(values.shape[1], dtype=float)
|
||||
return np.trapezoid(values, t_s, axis=0)
|
||||
|
||||
|
||||
def _interval_imu(session, start_s: float, end_s: float) -> tuple[np.ndarray, np.ndarray]:
|
||||
mask = (session.imu.t_s >= start_s) & (session.imu.t_s <= end_s)
|
||||
return session.imu.t_s[mask], session.imu.gyro_rad_s[mask]
|
||||
|
||||
|
||||
def _gyro_metrics(session, start_s: float, end_s: float, gyro_offset_rad_s: np.ndarray | None = None) -> dict[str, object]:
|
||||
t_s, gyro = _interval_imu(session, start_s, end_s)
|
||||
if gyro_offset_rad_s is not None:
|
||||
gyro = gyro - np.asarray(gyro_offset_rad_s, dtype=float).reshape(1, 3)
|
||||
if t_s.size < 2:
|
||||
nan = np.full(3, np.nan)
|
||||
return {
|
||||
"sample_count": int(t_s.size), "net_rotation_xyz_deg": nan,
|
||||
"unwrap_rotation_range_xyz_deg": nan,
|
||||
"cumulative_absolute_rotation_xyz_deg": nan,
|
||||
"gyro_integral_squared_xyz_rad2_s": nan,
|
||||
"gyro_rms_xyz_deg_s": nan, "gyro_peak_xyz_deg_s": nan,
|
||||
}
|
||||
dt = np.diff(t_s)
|
||||
midpoint = 0.5 * (gyro[:-1] + gyro[1:])
|
||||
trajectory = np.vstack([np.zeros(3), np.cumsum(midpoint * dt[:, None], axis=0)])
|
||||
# The integrated trajectory is continuous. Explicit unwrap documents that
|
||||
# the yaw range is never inferred from a wrapped heading/Euler endpoint.
|
||||
trajectory[:, 2] = np.unwrap(trajectory[:, 2])
|
||||
duration = float(t_s[-1] - t_s[0])
|
||||
return {
|
||||
"sample_count": int(t_s.size),
|
||||
"net_rotation_xyz_deg": np.degrees(trajectory[-1]),
|
||||
"unwrap_rotation_range_xyz_deg": np.degrees(np.ptp(trajectory, axis=0)),
|
||||
"cumulative_absolute_rotation_xyz_deg": np.degrees(_trapz(np.abs(gyro), t_s)),
|
||||
"gyro_integral_squared_xyz_rad2_s": _trapz(gyro * gyro, t_s),
|
||||
"gyro_rms_xyz_deg_s": np.degrees(np.sqrt(_trapz(gyro * gyro, t_s) / duration)),
|
||||
"gyro_peak_xyz_deg_s": np.degrees(np.max(np.abs(gyro), axis=0)),
|
||||
}
|
||||
|
||||
|
||||
def _interval_summary(session, start_s: float, end_s: float, *, label: str,
|
||||
best_rows: Iterable[dict[str, str]], hpr) -> dict[str, object]:
|
||||
best = list(best_rows)
|
||||
in_range = [row for row in best if start_s <= _f(row, "t_device_s") <= end_s]
|
||||
doppler = [
|
||||
row for row in in_range
|
||||
if _truth(row, "doppler_velocity_valid") and np.all(np.isfinite([
|
||||
_f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"),
|
||||
_f(row, "vertical_speed_m_s"),
|
||||
]))
|
||||
]
|
||||
q4 = hpr.valid & (hpr.t_s >= start_s) & (hpr.t_s <= end_s)
|
||||
return {
|
||||
"label": label,
|
||||
"start_s": float(start_s), "end_s": float(end_s),
|
||||
"duration_s": float(max(0.0, end_s - start_s)),
|
||||
"bestnava_count": len(in_range), "doppler_count": len(doppler),
|
||||
"q4_hpr_count": int(np.count_nonzero(q4)),
|
||||
"gyro": _gyro_metrics(session, start_s, end_s),
|
||||
}
|
||||
|
||||
|
||||
def _coalesce(records: list[dict[str, object]], *, include: bool, label: str,
|
||||
session, best_rows, hpr) -> list[dict[str, object]]:
|
||||
result: list[dict[str, object]] = []
|
||||
current: list[dict[str, object]] = []
|
||||
key: tuple[str, ...] | None = None
|
||||
for record in records:
|
||||
active = bool(record["accepted"]) == include
|
||||
reasons = tuple(record["reasons"])
|
||||
same = (
|
||||
current and active and key == reasons
|
||||
and float(record["t_s"]) - float(current[-1]["t_s"]) <= RAW_GAP_S
|
||||
)
|
||||
if active and (not current or same):
|
||||
current.append(record)
|
||||
key = reasons
|
||||
continue
|
||||
if current:
|
||||
summary = _interval_summary(
|
||||
session, float(current[0]["t_s"]), float(current[-1]["t_s"]),
|
||||
label=label, best_rows=best_rows, hpr=hpr,
|
||||
)
|
||||
if not include:
|
||||
summary["cut_reason"] = list(key or ())
|
||||
result.append(summary)
|
||||
current = [record] if active else []
|
||||
key = reasons if active else None
|
||||
if current:
|
||||
summary = _interval_summary(
|
||||
session, float(current[0]["t_s"]), float(current[-1]["t_s"]),
|
||||
label=label, best_rows=best_rows, hpr=hpr,
|
||||
)
|
||||
if not include:
|
||||
summary["cut_reason"] = list(key or ())
|
||||
result.append(summary)
|
||||
return result
|
||||
|
||||
|
||||
def _raw_records(session) -> list[dict[str, object]]:
|
||||
"""Raw-valid is position/IMU validity; HPR support remains a separate factor audit."""
|
||||
hpr = _all_hpr(session)
|
||||
rows = session.rtk_by_type.get("BESTNAVA", [])
|
||||
ordered = sorted(rows, key=lambda row: _f(row, "t_device_s"))
|
||||
records: list[dict[str, object]] = []
|
||||
last_t = -np.inf
|
||||
for row in ordered:
|
||||
t_s = _f(row, "t_device_s")
|
||||
reasons: list[str] = []
|
||||
if not np.isfinite(t_s):
|
||||
reasons.append("position_device_time_invalid")
|
||||
elif t_s <= last_t:
|
||||
reasons.append("position_device_time_nonmonotonic")
|
||||
if np.isfinite(t_s):
|
||||
last_t = max(last_t, t_s)
|
||||
if not _truth(row, "checksum_valid"):
|
||||
reasons.append("position_checksum_invalid")
|
||||
if not _truth(row, "position_fixed"):
|
||||
reasons.append("position_not_fixed")
|
||||
if not _position_valid(row, "BESTNAVA"):
|
||||
reasons.append("position_required_field_invalid")
|
||||
imu_index = _nearest_index(session.imu.t_s, t_s, 0.03) if np.isfinite(t_s) else None
|
||||
if imu_index is None:
|
||||
reasons.append("imu_missing_near")
|
||||
_, _, hpr_factor_valid, hpr_method, hpr_gap = _hpr_factor_observation(hpr, t_s)
|
||||
doppler_ok = bool(
|
||||
_truth(row, "doppler_velocity_valid") and np.all(np.isfinite([
|
||||
_f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"),
|
||||
_f(row, "vertical_speed_m_s"),
|
||||
]))
|
||||
)
|
||||
records.append({
|
||||
"t_s": t_s, "row": row, "accepted": not reasons,
|
||||
"reasons": sorted(set(reasons)), "doppler_valid": doppler_ok,
|
||||
"hpr_factor_valid": hpr_factor_valid, "hpr_factor_method": hpr_method,
|
||||
"hpr_support_gap_s": hpr_gap,
|
||||
})
|
||||
return records
|
||||
def _r0_runs_and_cuts(session, nodes, hpr, best_rows, period_s: float) -> tuple[list[dict[str, object]], list[dict[str, object]], list[dict[str, object]]]:
|
||||
runs: list[list] = []
|
||||
cuts: list[dict[str, object]] = []
|
||||
intervals: list[dict[str, object]] = []
|
||||
if not nodes:
|
||||
return [], [], []
|
||||
current = [nodes[0]]
|
||||
threshold = _node_interval_threshold_s(period_s)
|
||||
for previous, node in zip(nodes[:-1], nodes[1:]):
|
||||
dt = float(node.t_s - previous.t_s)
|
||||
structural_reasons = list(_trajectory_continuity_reasons(session, previous.t_s, node.t_s, period_s))
|
||||
continuity_break = node.continuity_id != previous.continuity_id
|
||||
reasons = structural_reasons or (["position_source_quality_or_merge_break"] if continuity_break else [])
|
||||
intervals.append({
|
||||
"left_t_s": float(previous.t_s), "right_t_s": float(node.t_s),
|
||||
"dt_s": dt, "threshold_s": threshold,
|
||||
"trajectory_continuous": not structural_reasons,
|
||||
"continuity_id_changed": continuity_break,
|
||||
"cut_reason": reasons,
|
||||
"left_hpr_factor": {"valid": previous.hpr_factor_valid, "method": previous.hpr_factor_method,
|
||||
"support_gap_s": previous.hpr_support_gap_s},
|
||||
"right_hpr_factor": {"valid": node.hpr_factor_valid, "method": node.hpr_factor_method,
|
||||
"support_gap_s": node.hpr_support_gap_s},
|
||||
})
|
||||
if not continuity_break:
|
||||
current.append(node)
|
||||
continue
|
||||
runs.append(current)
|
||||
cuts.append({
|
||||
**_interval_summary(session, previous.t_s, node.t_s, label="r0_cut", best_rows=best_rows, hpr=hpr),
|
||||
"dt_s": dt, "threshold_s": threshold, "cut_reason": reasons,
|
||||
})
|
||||
current = [node]
|
||||
runs.append(current)
|
||||
summaries = [
|
||||
_interval_summary(session, run[0].t_s, run[-1].t_s, label="R0_after_cuts", best_rows=best_rows, hpr=hpr)
|
||||
| {
|
||||
"node_count": len(run), "continuity_id": int(run[0].continuity_id),
|
||||
"bestnava_count": sum(node.source == "BESTNAVA" for node in run),
|
||||
"doppler_count": sum(node.velocity_enu_m_s is not None for node in run),
|
||||
"hpr_factor_count": sum(node.hpr_factor_valid for node in run),
|
||||
"hpr_factor_rejected_count": sum(not node.hpr_factor_valid for node in run),
|
||||
}
|
||||
for run in runs
|
||||
]
|
||||
return summaries, cuts, intervals
|
||||
def _qualified_summary(session, segments, hpr, best_rows) -> list[dict[str, object]]:
|
||||
return [
|
||||
_interval_summary(session, segment.nodes[0].t_s, segment.nodes[-1].t_s,
|
||||
label="qualified_segment", best_rows=best_rows, hpr=hpr)
|
||||
| {
|
||||
"segment_id": segment.segment_id, "node_count": len(segment.nodes),
|
||||
"bestnava_count": sum(node.source == "BESTNAVA" for node in segment.nodes),
|
||||
"doppler_count": sum(node.velocity_enu_m_s is not None for node in segment.nodes),
|
||||
"hpr_factor_count": sum(node.hpr_factor_valid for node in segment.nodes),
|
||||
"hpr_factor_rejected_count": sum(not node.hpr_factor_valid for node in segment.nodes),
|
||||
}
|
||||
for segment in segments
|
||||
]
|
||||
|
||||
def _dropped_r0_runs(session, r0_nodes, qualified, hpr, best_rows) -> list[dict[str, object]]:
|
||||
qualified_ranges = [(s.nodes[0].t_s, s.nodes[-1].t_s) for s in qualified]
|
||||
result: list[dict[str, object]] = []
|
||||
by_id: dict[int, list] = {}
|
||||
for node in r0_nodes:
|
||||
by_id.setdefault(node.continuity_id, []).append(node)
|
||||
for run in by_id.values():
|
||||
start_s, end_s = run[0].t_s, run[-1].t_s
|
||||
retained = any(abs(start_s - left) < 1e-6 and abs(end_s - right) < 1e-6 for left, right in qualified_ranges)
|
||||
if retained:
|
||||
continue
|
||||
reasons = []
|
||||
if len(run) < MIN_SEGMENT_NODE_COUNT:
|
||||
reasons.append("qualified_min_node_count")
|
||||
if end_s - start_s < MIN_SEGMENT_DURATION_S:
|
||||
reasons.append("qualified_min_duration")
|
||||
if not reasons:
|
||||
reasons.append("preintegration_or_segment_validation")
|
||||
result.append({
|
||||
**_interval_summary(session, start_s, end_s, label="dropped_before_qualified",
|
||||
best_rows=best_rows, hpr=hpr),
|
||||
"node_count": len(run), "cut_reason": reasons,
|
||||
})
|
||||
return result
|
||||
|
||||
|
||||
def _interval_overlap(left: dict[str, object], right: dict[str, object]) -> float:
|
||||
return max(0.0, min(float(left["end_s"]), float(right["end_s"])) - max(float(left["start_s"]), float(right["start_s"])))
|
||||
|
||||
|
||||
def _interval_penetration(raw_intervals, r0_intervals, qualified_intervals, cut_intervals):
|
||||
"""Link every raw-valid dynamic interval to its downstream R0/qualified survivors."""
|
||||
result = []
|
||||
for raw in raw_intervals:
|
||||
r0 = [item for item in r0_intervals if _interval_overlap(raw, item) > 0.0 or (
|
||||
item["start_s"] == item["end_s"] and raw["start_s"] <= item["start_s"] <= raw["end_s"]
|
||||
)]
|
||||
qualified = [item for item in qualified_intervals if _interval_overlap(raw, item) > 0.0]
|
||||
cuts = [item for item in cut_intervals if _interval_overlap(raw, item) > 0.0]
|
||||
raw_best = max(int(raw["bestnava_count"]), 1)
|
||||
raw_doppler = max(int(raw["doppler_count"]), 1)
|
||||
r0_duration = sum(_interval_overlap(raw, item) for item in r0)
|
||||
qualified_duration = sum(_interval_overlap(raw, item) for item in qualified)
|
||||
cut_reasons = sorted({reason for item in cuts for reason in item.get("cut_reason", [])})
|
||||
result.append({
|
||||
"raw_start_s": raw["start_s"], "raw_end_s": raw["end_s"],
|
||||
"raw_duration_s": raw["duration_s"], "raw_bestnava_count": raw["bestnava_count"],
|
||||
"raw_doppler_count": raw["doppler_count"], "raw_gyro": raw["gyro"],
|
||||
"R0_overlap_duration_s": r0_duration,
|
||||
"qualified_overlap_duration_s": qualified_duration,
|
||||
"R0_bestnava_count": sum(int(item["bestnava_count"]) for item in r0),
|
||||
"R0_doppler_count": sum(int(item["doppler_count"]) for item in r0),
|
||||
"qualified_bestnava_count": sum(int(item["bestnava_count"]) for item in qualified),
|
||||
"qualified_doppler_count": sum(int(item["doppler_count"]) for item in qualified),
|
||||
"retention": {
|
||||
"raw_to_R0_bestnava": sum(int(item["bestnava_count"]) for item in r0) / raw_best,
|
||||
"raw_to_R0_doppler": sum(int(item["doppler_count"]) for item in r0) / raw_doppler,
|
||||
"raw_to_R0_duration": r0_duration / max(float(raw["duration_s"]), 1e-9),
|
||||
"raw_to_qualified_bestnava": sum(int(item["bestnava_count"]) for item in qualified) / raw_best,
|
||||
"raw_to_qualified_doppler": sum(int(item["doppler_count"]) for item in qualified) / raw_doppler,
|
||||
"raw_to_qualified_duration": qualified_duration / max(float(raw["duration_s"]), 1e-9),
|
||||
},
|
||||
"cut_reason": cut_reasons,
|
||||
})
|
||||
return result
|
||||
|
||||
|
||||
def _union_time_intervals(intervals: list[dict[str, object]]) -> list[tuple[float, float]]:
|
||||
ordered = sorted(
|
||||
(float(item["start_s"]), float(item["end_s"])) for item in intervals
|
||||
if np.isfinite(float(item["start_s"])) and np.isfinite(float(item["end_s"]))
|
||||
)
|
||||
merged: list[list[float]] = []
|
||||
for start_s, end_s in ordered:
|
||||
if end_s < start_s:
|
||||
continue
|
||||
if not merged or start_s > merged[-1][1]:
|
||||
merged.append([start_s, end_s])
|
||||
else:
|
||||
merged[-1][1] = max(merged[-1][1], end_s)
|
||||
return [(start_s, end_s) for start_s, end_s in merged]
|
||||
|
||||
|
||||
def _stage_statistics(session, intervals: list[dict[str, object]]) -> dict[str, object]:
|
||||
"""Audit each stage on unique IMU samples over the union of its time ranges."""
|
||||
total = _stage_total(intervals)
|
||||
union = _union_time_intervals(intervals)
|
||||
selected_count = 0
|
||||
unique_mask = np.zeros(session.imu.t_s.size, dtype=bool)
|
||||
for item in intervals:
|
||||
mask = (session.imu.t_s >= float(item["start_s"])) & (session.imu.t_s <= float(item["end_s"]))
|
||||
selected_count += int(np.count_nonzero(mask))
|
||||
unique_mask |= mask
|
||||
unique_count = int(np.count_nonzero(unique_mask))
|
||||
input_duration = float(sum(max(0.0, float(item["end_s"]) - float(item["start_s"])) for item in intervals))
|
||||
union_duration = float(sum(end_s - start_s for start_s, end_s in union))
|
||||
net = np.zeros(3)
|
||||
unwrap_range_sum = np.zeros(3)
|
||||
cumulative_abs = np.zeros(3)
|
||||
energy = np.zeros(3)
|
||||
peak = np.zeros(3)
|
||||
metric_duration = 0.0
|
||||
for start_s, end_s in union:
|
||||
gyro = _gyro_metrics(session, start_s, end_s)
|
||||
current_net = np.asarray(gyro["net_rotation_xyz_deg"], dtype=float)
|
||||
if not np.all(np.isfinite(current_net)):
|
||||
continue
|
||||
net += current_net
|
||||
unwrap_range_sum += np.asarray(gyro["unwrap_rotation_range_xyz_deg"], dtype=float)
|
||||
cumulative_abs += np.asarray(gyro["cumulative_absolute_rotation_xyz_deg"], dtype=float)
|
||||
energy += np.asarray(gyro["gyro_integral_squared_xyz_rad2_s"], dtype=float)
|
||||
peak = np.maximum(peak, np.asarray(gyro["gyro_peak_xyz_deg_s"], dtype=float))
|
||||
metric_duration += max(0.0, end_s - start_s)
|
||||
total["unique_imu_coverage"] = {
|
||||
"input_interval_count": len(intervals),
|
||||
"union_interval_count": len(union),
|
||||
"input_duration_s": input_duration,
|
||||
"union_duration_s": union_duration,
|
||||
"overlap_duration_s": max(0.0, input_duration - union_duration),
|
||||
"selected_imu_sample_count_before_dedup": selected_count,
|
||||
"unique_imu_sample_count": unique_count,
|
||||
"duplicate_imu_sample_count": selected_count - unique_count,
|
||||
}
|
||||
total["gyro"] = {
|
||||
"net_rotation_xyz_deg": net,
|
||||
"sum_interval_unwrap_rotation_range_xyz_deg": unwrap_range_sum,
|
||||
"cumulative_absolute_rotation_xyz_deg": cumulative_abs,
|
||||
"gyro_integral_squared_xyz_rad2_s": energy,
|
||||
"gyro_rms_xyz_deg_s": np.degrees(np.sqrt(energy / max(metric_duration, 1e-9))),
|
||||
"gyro_peak_xyz_deg_s": peak,
|
||||
}
|
||||
return total
|
||||
|
||||
def _stage_total(intervals: list[dict[str, object]]) -> dict[str, object]:
|
||||
total = {"interval_count": len(intervals), "duration_s": 0.0, "bestnava_count": 0,
|
||||
"doppler_count": 0, "q4_hpr_count": 0}
|
||||
for item in intervals:
|
||||
for key in ("duration_s", "bestnava_count", "doppler_count", "q4_hpr_count"):
|
||||
total[key] += item[key]
|
||||
return total
|
||||
|
||||
|
||||
def _hpr_chain_diagnostics(hpr) -> dict[str, object]:
|
||||
if hpr.t_s.size < 2:
|
||||
return {"sample_count": int(hpr.t_s.size), "pair_count": 0}
|
||||
dt = np.diff(hpr.t_s)
|
||||
finite_vector = np.all(np.isfinite(hpr.baseline_enu), axis=1)
|
||||
dot = np.sum(hpr.baseline_enu[:-1] * hpr.baseline_enu[1:], axis=1)
|
||||
jump_deg = np.degrees(np.arccos(np.clip(dot, -1.0, 1.0)))
|
||||
rate = jump_deg / np.maximum(dt, 1e-12)
|
||||
return {
|
||||
"sample_count": int(hpr.t_s.size),
|
||||
"q4_valid_sample_count": int(np.count_nonzero(hpr.valid)),
|
||||
"pair_count": int(dt.size),
|
||||
"pair_with_invalid_endpoint_count": int(np.count_nonzero(~(hpr.valid[:-1] & hpr.valid[1:]))),
|
||||
"dt_s_p50_p95_max": np.percentile(dt[np.isfinite(dt)], [50.0, 95.0, 100.0]),
|
||||
"dt_too_short_count": int(np.count_nonzero(dt < 0.03)),
|
||||
"dt_too_long_count": int(np.count_nonzero(dt > 0.25)),
|
||||
"baseline_jump_rate_over_45deg_s_count": int(np.count_nonzero(
|
||||
finite_vector[:-1] & finite_vector[1:] & (rate > 45.0)
|
||||
)),
|
||||
}
|
||||
|
||||
|
||||
def _hpr_axis_mapping(session, hpr) -> dict[str, object]:
|
||||
valid = hpr.valid & np.isfinite(hpr.t_s)
|
||||
if np.count_nonzero(valid) < 8:
|
||||
return {"available": False, "reason": "fewer_than_8_q4_hpr_samples"}
|
||||
t = hpr.t_s[valid]
|
||||
dt = np.diff(t)
|
||||
keep = np.r_[True, (dt > 0.03) & (dt <= 0.25)]
|
||||
t = t[keep]
|
||||
# hpr arrays preserve GNHPR order after time sorting; heading must unwrap.
|
||||
hpr_rows = sorted(session.rtk_by_type.get("GNHPR", []), key=lambda row: _f(row, "t_device_s"))
|
||||
heading = np.unwrap(np.deg2rad(np.asarray([_f(row, "heading_deg") for row in hpr_rows])))[valid][keep]
|
||||
pitch = np.asarray([_f(row, "pitch_deg") for row in hpr_rows])[valid][keep]
|
||||
if t.size < 8:
|
||||
return {"available": False, "reason": "insufficient_contiguous_q4_hpr"}
|
||||
heading_rate = np.gradient(heading, t)
|
||||
gyro = np.column_stack([np.interp(t, session.imu.t_s, session.imu.gyro_rad_s[:, axis]) for axis in range(3)])
|
||||
correlation = []
|
||||
for axis in range(3):
|
||||
value = np.corrcoef(heading_rate, gyro[:, axis])[0, 1]
|
||||
correlation.append(float(value) if np.isfinite(value) else np.nan)
|
||||
best_axis = int(np.nanargmax(np.abs(correlation))) if np.any(np.isfinite(correlation)) else None
|
||||
return {
|
||||
"available": best_axis is not None,
|
||||
"hpr_heading_unwrapped_range_deg": float(np.degrees(np.ptp(heading))),
|
||||
"hpr_heading_net_rotation_deg": float(np.degrees(heading[-1] - heading[0])),
|
||||
"hpr_heading_cumulative_absolute_rotation_deg": float(np.degrees(np.sum(np.abs(np.diff(heading))))),
|
||||
"hpr_pitch_range_deg": float(np.ptp(pitch)),
|
||||
"hpr_pitch_cumulative_absolute_change_deg": float(np.sum(np.abs(np.diff(pitch)))),
|
||||
"heading_rate_to_imu_gyro_correlation_xyz": np.asarray(correlation),
|
||||
"best_correlated_imu_axis": best_axis,
|
||||
"expected_z_axis_correlation": correlation[2],
|
||||
"note": "heading is unwrapped; sign depends on GNHPR clockwise-from-north convention",
|
||||
}
|
||||
|
||||
|
||||
def _bias_absorption_check(session, intervals: list[dict[str, object]]) -> dict[str, object]:
|
||||
report = audit_imu(session.imu)
|
||||
checks = []
|
||||
for item in intervals:
|
||||
if item["duration_s"] < 1.0:
|
||||
continue
|
||||
start_s, end_s = item["start_s"], item["end_s"]
|
||||
raw = _gyro_metrics(session, start_s, end_s)
|
||||
static_corrected = _gyro_metrics(session, start_s, end_s, report.gyro_bias_rad_s)
|
||||
t, gyro = _interval_imu(session, start_s, end_s)
|
||||
mean = np.mean(gyro, axis=0) if gyro.size else np.zeros(3)
|
||||
mean_removed = _gyro_metrics(session, start_s, end_s, mean)
|
||||
raw_abs = np.asarray(raw["cumulative_absolute_rotation_xyz_deg"])
|
||||
removed_abs = np.asarray(mean_removed["cumulative_absolute_rotation_xyz_deg"])
|
||||
ratio = removed_abs / np.maximum(raw_abs, 1e-9)
|
||||
checks.append({
|
||||
"start_s": start_s, "end_s": end_s,
|
||||
"static_bias_rad_s": report.gyro_bias_rad_s,
|
||||
"segment_mean_gyro_rad_s": mean,
|
||||
"segment_mean_removed_to_raw_abs_rotation_ratio_xyz": ratio,
|
||||
"static_bias_corrected": static_corrected,
|
||||
"mean_removal_would_absorb_motion": bool(np.any(ratio < 0.5)),
|
||||
})
|
||||
return {"imu_static_audit": report, "interval_checks": checks,
|
||||
"note": "Engineering audit integrates raw gyro; it does not subtract a segment mean."}
|
||||
|
||||
|
||||
def _unit_check(session) -> dict[str, object]:
|
||||
gyro = session.imu.gyro_rad_s
|
||||
norm = np.linalg.norm(gyro, axis=1)
|
||||
p99 = float(np.percentile(norm, 99.0)) if norm.size else np.nan
|
||||
return {
|
||||
"gyro_p99_norm_rad_s": p99,
|
||||
"gyro_p99_norm_deg_s": float(np.degrees(p99)),
|
||||
"gyro_peak_norm_rad_s": float(np.max(norm)) if norm.size else np.nan,
|
||||
"suspect_deg_per_second_stored_as_rad_per_second": bool(np.isfinite(p99) and p99 > 20.0),
|
||||
"suspect_near_zero_gyro_scale": bool(np.isfinite(p99) and p99 < 1e-4),
|
||||
"unit_contract": "unified imu.npz gyro_rad_s is radians per second",
|
||||
}
|
||||
|
||||
|
||||
def _candidate_scores(session_audit: dict[str, object]) -> dict[str, float]:
|
||||
raw = session_audit["raw_valid_intervals"]
|
||||
if not raw:
|
||||
return {"circle": 0.0, "left_right": 0.0, "slope": 0.0}
|
||||
cumulative = np.zeros(3)
|
||||
net = np.zeros(3)
|
||||
for interval in raw:
|
||||
gyro = interval["gyro"]
|
||||
value = np.asarray(gyro["cumulative_absolute_rotation_xyz_deg"], dtype=float)
|
||||
signed = np.asarray(gyro["net_rotation_xyz_deg"], dtype=float)
|
||||
if np.all(np.isfinite(value)):
|
||||
cumulative += value
|
||||
if np.all(np.isfinite(signed)):
|
||||
net += signed
|
||||
axis = session_audit["axis_mapping"]
|
||||
heading_range = abs(float(axis.get("hpr_heading_unwrapped_range_deg", 0.0) or 0.0))
|
||||
heading_abs = abs(float(axis.get("hpr_heading_cumulative_absolute_rotation_deg", 0.0) or 0.0))
|
||||
heading_net = abs(float(axis.get("hpr_heading_net_rotation_deg", 0.0) or 0.0))
|
||||
pitch_range = abs(float(axis.get("hpr_pitch_range_deg", 0.0) or 0.0))
|
||||
pitch_abs = abs(float(axis.get("hpr_pitch_cumulative_absolute_change_deg", 0.0) or 0.0))
|
||||
return {
|
||||
"circle": max(heading_range, cumulative[2]),
|
||||
"left_right": max(0.0, heading_abs - heading_net, cumulative[2] - abs(net[2])),
|
||||
"slope": max(cumulative[0], cumulative[1]) ** 2 / max(1.0, cumulative[2]),
|
||||
}
|
||||
|
||||
|
||||
def _select_candidates(audits: list[dict[str, object]]) -> dict[str, dict[str, object] | None]:
|
||||
remaining = list(audits)
|
||||
chosen: dict[str, dict[str, object] | None] = {}
|
||||
for kind in ("circle", "left_right", "slope"):
|
||||
ranked = sorted(remaining, key=lambda item: item["candidate_scores"][kind], reverse=True)
|
||||
choice = ranked[0] if ranked and ranked[0]["candidate_scores"][kind] > 0.0 else None
|
||||
chosen[kind] = None if choice is None else {
|
||||
"session_id": choice["session_id"], "score_deg": choice["candidate_scores"][kind],
|
||||
"selection_metric": {
|
||||
"circle": "max(unwrapped HPR heading range, raw gyro-z net/absolute rotation)",
|
||||
"left_right": "unwrapped HPR heading cumulative change minus net change, cross-checked with gyro-z",
|
||||
"slope": "tilt-dominance: max(raw gyro-x/y cumulative rotation)^2 / raw gyro-z cumulative rotation",
|
||||
}[kind],
|
||||
}
|
||||
if choice is not None:
|
||||
remaining.remove(choice)
|
||||
return chosen
|
||||
|
||||
|
||||
def _audit_session(session, period_s: float) -> dict[str, object]:
|
||||
reference = _height_reference([session])
|
||||
hpr = _all_hpr(session)
|
||||
best_rows = session.rtk_by_type.get("BESTNAVA", [])
|
||||
records = _raw_records(session)
|
||||
raw_valid = _coalesce(records, include=True, label="raw_valid", session=session,
|
||||
best_rows=best_rows, hpr=hpr)
|
||||
raw_rejected = _coalesce(records, include=False, label="dropped_before_raw_valid", session=session,
|
||||
best_rows=best_rows, hpr=hpr)
|
||||
r0_nodes = [] if reference is None else _nodes(session, reference, period_s)
|
||||
r0_intervals, r0_cuts, r0_node_intervals = _r0_runs_and_cuts(
|
||||
session, r0_nodes, hpr, best_rows, period_s
|
||||
)
|
||||
all_qualified = _segments([session], period_s)
|
||||
qualified = [segment for segment in all_qualified if segment.session_id == session.session_id]
|
||||
qualified_intervals = _qualified_summary(session, qualified, hpr, best_rows)
|
||||
dropped_r0 = _dropped_r0_runs(session, r0_nodes, qualified, hpr, best_rows)
|
||||
r0_selected_times = np.asarray([node.t_s for node in r0_nodes])
|
||||
decimated = []
|
||||
for record in records:
|
||||
if not record["accepted"]:
|
||||
continue
|
||||
t_s = float(record["t_s"])
|
||||
selected = r0_selected_times.size and np.min(np.abs(r0_selected_times - t_s)) < 1e-8
|
||||
if not selected:
|
||||
decimated.append({**record, "accepted": False, "reasons": ["sample_period_decimation"]})
|
||||
decimation_cuts = _coalesce(decimated, include=False, label="dropped_raw_to_R0", session=session,
|
||||
best_rows=best_rows, hpr=hpr)
|
||||
raw_total, r0_total, qualified_total = map(_stage_total, (raw_valid, r0_intervals, qualified_intervals))
|
||||
retention = {
|
||||
"raw_to_R0": {
|
||||
"bestnava_count_ratio": r0_total["bestnava_count"] / max(raw_total["bestnava_count"], 1),
|
||||
"doppler_count_ratio": r0_total["doppler_count"] / max(raw_total["doppler_count"], 1),
|
||||
"duration_ratio": r0_total["duration_s"] / max(raw_total["duration_s"], 1e-9),
|
||||
},
|
||||
"R0_to_qualified": {
|
||||
"bestnava_count_ratio": qualified_total["bestnava_count"] / max(r0_total["bestnava_count"], 1),
|
||||
"doppler_count_ratio": qualified_total["doppler_count"] / max(r0_total["doppler_count"], 1),
|
||||
"duration_ratio": qualified_total["duration_s"] / max(r0_total["duration_s"], 1e-9),
|
||||
},
|
||||
}
|
||||
audit = {
|
||||
"session_id": session.session_id, "batch_id": session.batch_id,
|
||||
"raw_valid_intervals": raw_valid, "R0_after_cuts_intervals": r0_intervals,
|
||||
"qualified_segments": qualified_intervals,
|
||||
"stage_statistics": {
|
||||
"raw_valid": _stage_statistics(session, raw_valid),
|
||||
"R0_after_cuts": _stage_statistics(session, r0_intervals),
|
||||
"qualified": _stage_statistics(session, qualified_intervals),
|
||||
},
|
||||
"retention": retention,
|
||||
"cut_intervals": [*raw_rejected, *decimation_cuts, *r0_cuts, *dropped_r0],
|
||||
"r0_consecutive_node_intervals": r0_node_intervals,
|
||||
"interval_penetration": _interval_penetration(
|
||||
raw_valid, r0_intervals, qualified_intervals,
|
||||
[*raw_rejected, *decimation_cuts, *r0_cuts, *dropped_r0],
|
||||
),
|
||||
"hpr_chain_diagnostics": _hpr_chain_diagnostics(hpr),
|
||||
"axis_mapping": _hpr_axis_mapping(session, hpr),
|
||||
"unit_check": _unit_check(session),
|
||||
"bias_absorption_check": _bias_absorption_check(session, raw_valid),
|
||||
}
|
||||
audit["candidate_scores"] = _candidate_scores(audit)
|
||||
return audit
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--sample-period-s", type=float, default=1.0)
|
||||
parser.add_argument("--session", action="append", help="Optional session id; may repeat.")
|
||||
parser.add_argument(
|
||||
"--inventory-only", action="store_true",
|
||||
help="Only scan raw-valid gyro/HPR excitation; skip R0 and qualified-segment work.",
|
||||
)
|
||||
args = parser.parse_args(argv)
|
||||
sessions = load_unified_sessions(
|
||||
args.manifest,
|
||||
selected_session_ids=None if args.session is None else set(args.session),
|
||||
)
|
||||
if args.inventory_only:
|
||||
audits = []
|
||||
for session in sessions:
|
||||
hpr = _all_hpr(session)
|
||||
best_rows = session.rtk_by_type.get("BESTNAVA", [])
|
||||
records = _raw_records(session)
|
||||
raw_valid = _coalesce(records, include=True, label="raw_valid", session=session,
|
||||
best_rows=best_rows, hpr=hpr)
|
||||
audit = {
|
||||
"session_id": session.session_id,
|
||||
"batch_id": session.batch_id,
|
||||
"raw_valid_intervals": raw_valid,
|
||||
"hpr_chain_diagnostics": _hpr_chain_diagnostics(hpr),
|
||||
"axis_mapping": _hpr_axis_mapping(session, hpr),
|
||||
"unit_check": _unit_check(session),
|
||||
}
|
||||
audit["candidate_scores"] = _candidate_scores(audit)
|
||||
audits.append(audit)
|
||||
scope = "raw-valid motion inventory only; no R0/qualified work or optimisation"
|
||||
else:
|
||||
audits = [_audit_session(session, args.sample_period_s) for session in sessions]
|
||||
scope = "motion-excitation penetration audit only; no lever fit/prior/bootstrap/sensitivity"
|
||||
payload = {
|
||||
"scope": scope,
|
||||
"sample_period_s": args.sample_period_s,
|
||||
"session_count": len(audits),
|
||||
"selected_dynamic_candidates": _select_candidates(audits),
|
||||
"sessions": audits,
|
||||
}
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload), ensure_ascii=False, indent=2, allow_nan=False) + "\n", encoding="utf-8")
|
||||
print(json.dumps({
|
||||
"session_count": len(audits),
|
||||
"selected_dynamic_candidates": payload["selected_dynamic_candidates"],
|
||||
}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,149 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Multi-horizon open-loop RTK/IMU propagation audit without optimization.'''
|
||||
from __future__ import annotations
|
||||
import argparse, json, sys
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.imu_preintegration import preintegrate_imu
|
||||
from imu_lidar.rtk_imu_engineering import (
|
||||
G_ENU, _all_hpr, _height_reference, _hpr_factor_observation, _world_rtk)
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from tools.audit_rtk_imu_factor_consistency import (
|
||||
MECHANICAL_L_I_M, _best_arrays, _jsonable, _nearest_imu, _summary)
|
||||
|
||||
HORIZONS_S = (1.,2.,5.,10.,20.)
|
||||
|
||||
def _angle_deg(left,right):
|
||||
return float(np.degrees(np.arccos(np.clip(np.dot(left,right),-1.,1.))))
|
||||
|
||||
def _scalar_summary(values):
|
||||
a = np.asarray(values,dtype=float)
|
||||
if not a.size:
|
||||
return {'count':0,'rms':np.nan,'p95_abs':np.nan}
|
||||
return {'count':len(a),'rms':float(np.sqrt(np.mean(a*a))),
|
||||
'p95_abs':float(np.percentile(np.abs(a),95.))}
|
||||
|
||||
def _evaluate_session(session,reference,rotation,lever,bg,ba):
|
||||
_,times,p_ant,v_ant = _best_arrays(session,reference)
|
||||
hpr = _all_hpr(session)
|
||||
baseline_I = rotation.T[:,0]
|
||||
values = {h:{'position':[],'velocity':[],'attitude':[],'baseline':[]}
|
||||
for h in HORIZONS_S}
|
||||
pair_pre = []
|
||||
for left,right in zip(times[:-1],times[1:]):
|
||||
if not .5 <= right-left <= 1.75:
|
||||
pair_pre.append(None)
|
||||
else:
|
||||
pair_pre.append(preintegrate_imu(
|
||||
session.imu.t_s,session.imu.gyro_rad_s,session.imu.acc_m_s2,
|
||||
float(left),float(right),bg,ba))
|
||||
for start,t0 in enumerate(times):
|
||||
baseline0,_,valid0,_,_ = _hpr_factor_observation(hpr,float(t0))
|
||||
imu0 = _nearest_imu(session,float(t0))
|
||||
if not valid0 or imu0 is None: continue
|
||||
R0 = _world_rtk(baseline0) @ rotation
|
||||
p_i0 = p_ant[start]-R0@lever
|
||||
v_i0 = v_ant[start]-R0@np.cross(session.imu.gyro_rad_s[imu0]-bg,lever)
|
||||
delta_R, delta_v, delta_p = np.eye(3), np.zeros(3), np.zeros(3)
|
||||
elapsed, matched = 0., set()
|
||||
for end in range(start+1,len(times)):
|
||||
pre = pair_pre[end-1]
|
||||
if pre is None: break
|
||||
delta_p = delta_p + delta_v*pre.duration_s + delta_R@pre.delta_p
|
||||
delta_v = delta_v + delta_R@pre.delta_v
|
||||
delta_R = delta_R@pre.delta_R
|
||||
elapsed = float(times[end]-t0)
|
||||
if elapsed > max(HORIZONS_S)+.25: break
|
||||
candidates = [h for h in HORIZONS_S if h not in matched and abs(elapsed-h) <= .25]
|
||||
if not candidates: continue
|
||||
horizon = min(candidates,key=lambda h:abs(elapsed-h))
|
||||
matched.add(horizon)
|
||||
baseline1,_,valid1,_,_ = _hpr_factor_observation(hpr,float(times[end]))
|
||||
imu1 = _nearest_imu(session,float(times[end]))
|
||||
if not valid1 or imu1 is None: continue
|
||||
p_i1 = p_i0+v_i0*elapsed+.5*G_ENU*elapsed**2+R0@delta_p
|
||||
v_i1 = v_i0+G_ENU*elapsed+R0@delta_v
|
||||
R1 = R0@delta_R
|
||||
predicted_p = p_i1+R1@lever
|
||||
predicted_v = v_i1+R1@np.cross(session.imu.gyro_rad_s[imu1]-bg,lever)
|
||||
observed_R1 = _world_rtk(baseline1)@rotation
|
||||
values[horizon]['position'].append(predicted_p-p_ant[end])
|
||||
values[horizon]['velocity'].append(predicted_v-v_ant[end])
|
||||
values[horizon]['attitude'].append(
|
||||
np.degrees(Rotation.from_matrix(observed_R1.T@R1).magnitude()))
|
||||
values[horizon]['baseline'].append(_angle_deg(R1@baseline_I,baseline1))
|
||||
summary = {str(int(h)):{
|
||||
'position_error_m':_summary(values[h]['position']),
|
||||
'velocity_error_m_s':_summary(values[h]['velocity']),
|
||||
'attitude_level_completed_error_deg':_scalar_summary(values[h]['attitude']),
|
||||
'observable_baseline_angular_error_deg':_scalar_summary(values[h]['baseline'])}
|
||||
for h in HORIZONS_S}
|
||||
return summary, values
|
||||
|
||||
def _summarize_values(values):
|
||||
return {str(int(h)):{
|
||||
'position_error_m':_summary(values[h]['position']),
|
||||
'velocity_error_m_s':_summary(values[h]['velocity']),
|
||||
'attitude_level_completed_error_deg':_scalar_summary(values[h]['attitude']),
|
||||
'observable_baseline_angular_error_deg':_scalar_summary(values[h]['baseline'])}
|
||||
for h in HORIZONS_S}
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--session',action='append',required=True)
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
|
||||
default=MECHANICAL_L_I_M.tolist())
|
||||
parser.add_argument('--gyro-bias-rad-s',nargs=3,type=float,default=[0.,0.,0.])
|
||||
parser.add_argument('--accel-bias-m-s2',nargs=3,type=float,default=[0.,0.,0.])
|
||||
args = parser.parse_args()
|
||||
sessions = load_unified_sessions(args.manifest,selected_session_ids=set(args.session))
|
||||
reference = _height_reference(sessions)
|
||||
if reference is None: raise RuntimeError('no BEST height reference')
|
||||
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
lever = np.asarray(args.mechanical_l_I_m)
|
||||
bg, ba = np.asarray(args.gyro_bias_rad_s), np.asarray(args.accel_bias_m_s2)
|
||||
combined = {h:{'position':[],'velocity':[],'attitude':[],'baseline':[]}
|
||||
for h in HORIZONS_S}
|
||||
per_session = {}
|
||||
for session in sessions:
|
||||
per_session[session.session_id], raw = _evaluate_session(
|
||||
session,reference,rotation,lever,bg,ba)
|
||||
for h in HORIZONS_S:
|
||||
for key in combined[h]: combined[h][key].extend(raw[h][key])
|
||||
aggregate = _summarize_values(combined)
|
||||
one, twenty = aggregate['1'], aggregate['20']
|
||||
growth = {
|
||||
'position_p95_ratio_20s_over_1s':twenty['position_error_m']['vector_p95']/one['position_error_m']['vector_p95'],
|
||||
'velocity_p95_ratio_20s_over_1s':twenty['velocity_error_m_s']['vector_p95']/one['velocity_error_m_s']['vector_p95'],
|
||||
'attitude_p95_ratio_20s_over_1s':twenty['observable_baseline_angular_error_deg']['p95_abs']/one['observable_baseline_angular_error_deg']['p95_abs']}
|
||||
significant = bool(
|
||||
growth['position_p95_ratio_20s_over_1s'] >= 3.
|
||||
and twenty['position_error_m']['vector_p95']-one['position_error_m']['vector_p95'] >= .5)
|
||||
payload = {
|
||||
'scope':'open-loop only; no least_squares/free/prior/LOO/bootstrap/sensitivity',
|
||||
'least_squares_called':False,'rotation_source':'R2G_gravity_level_prior',
|
||||
'mechanical_l_I_m':lever,'gyro_bias_rad_s':bg,'accel_bias_m_s2':ba,
|
||||
'bias_source':'current nominal engineering bias; configurable CLI; no fit artifact bias was persisted',
|
||||
'aggregate_by_horizon_s':aggregate,'per_session_by_horizon_s':per_session,
|
||||
'one_second_reanchored_control':{
|
||||
'definition':'each interval restarts from observed BEST p/v and HPR-completed R',
|
||||
'result':one},
|
||||
'growth_20s_over_1s':growth,
|
||||
'significant_accumulated_drift':significant,
|
||||
'legacy_long_segment_deterministic_model_deprecated':significant}
|
||||
args.output.parent.mkdir(parents=True,exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable({'aggregate':aggregate,'growth':growth,
|
||||
'legacy_deprecated':significant}),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
if __name__ == '__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,304 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Root-cause audit for paired BEST/Doppler propagation innovations.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.rtk_imu_engineering import G0,G_ENU,_height_reference,_nodes,_world_rtk
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
from tools.audit_rtk_imu_heldout_innovation import _calibration_biases
|
||||
from tools.audit_rtk_imu_innovation_noise import _factor_report,_interval_innovations
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
|
||||
def _vector_summary(values):
|
||||
a=np.asarray(values,dtype=float).reshape(-1,3)
|
||||
if not len(a): return {'count':0}
|
||||
norm=np.linalg.norm(a,axis=1)
|
||||
return {'count':len(a),'bias':np.mean(a,axis=0),
|
||||
'axis_rms':np.sqrt(np.mean(a*a,axis=0)),
|
||||
'axis_p95_abs':np.percentile(np.abs(a),95,axis=0),
|
||||
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
|
||||
'vector_p95':float(np.percentile(norm,95)),
|
||||
'empirical_covariance':np.cov(a,rowvar=False)}
|
||||
|
||||
def _correlation(left,right):
|
||||
a,b=np.asarray(left),np.asarray(right); result=np.full(3,np.nan)
|
||||
for axis in range(3):
|
||||
if len(a)>2 and np.std(a[:,axis])>1e-12 and np.std(b[:,axis])>1e-12:
|
||||
result[axis]=np.corrcoef(a[:,axis],b[:,axis])[0,1]
|
||||
return result
|
||||
|
||||
def _root_report(position,velocity):
|
||||
p={x['interval_id']:x for x in position}; v={x['interval_id']:x for x in velocity}
|
||||
ids=sorted(set(p)&set(v)); records=[]
|
||||
for key in ids:
|
||||
left,right=p[key],v[key]; dt=float(left['dt_s'])
|
||||
a_p=2.*np.asarray(left['residual'])/(dt*dt)
|
||||
a_v=np.asarray(right['residual'])/dt
|
||||
R=np.asarray(left['R0_WI'])
|
||||
records.append({**{k:left[k] for k in (
|
||||
'interval_id','session','motion','t_s','speed_bin','gyro_bin')},
|
||||
'dt_s':dt,'a_position':a_p,'a_velocity':a_v,
|
||||
'difference':a_v-a_p,'a_common':.5*(a_p+a_v),
|
||||
'a_common_body':R.T@(.5*(a_p+a_v))})
|
||||
def summarize(items):
|
||||
if not items:
|
||||
return {'interval_count':0,
|
||||
'a_err_from_position_m_s2':{'count':0},
|
||||
'a_err_from_velocity_m_s2':{'count':0},
|
||||
'per_axis_correlation':[np.nan]*3,
|
||||
'mean_direction_cosine':np.nan,
|
||||
'mean_magnitude_ratio_velocity_over_position':np.nan,
|
||||
'difference_velocity_minus_position_m_s2':{'count':0},
|
||||
'common_acceleration_world_m_s2':{'count':0},
|
||||
'common_acceleration_body_m_s2':{'count':0},
|
||||
'equivalent_horizontal_tilt_rad':np.nan,
|
||||
'equivalent_horizontal_tilt_deg':np.nan}
|
||||
ap=[x['a_position'] for x in items]; av=[x['a_velocity'] for x in items]
|
||||
diff=[x['difference'] for x in items]; common=[x['a_common'] for x in items]
|
||||
body=[x['a_common_body'] for x in items]
|
||||
mean_p=np.mean(ap,axis=0); mean_v=np.mean(av,axis=0)
|
||||
denom=np.linalg.norm(mean_p)*np.linalg.norm(mean_v)
|
||||
cosine=float(mean_p@mean_v/denom) if denom>1e-12 else np.nan
|
||||
ratio=float(np.linalg.norm(mean_v)/max(np.linalg.norm(mean_p),1e-12))
|
||||
body_bias=np.mean(body,axis=0)
|
||||
body_std=np.std(body,axis=0)
|
||||
horizontal=float(np.linalg.norm(body_bias[:2]))
|
||||
return {'interval_count':len(items),
|
||||
'a_err_from_position_m_s2':_vector_summary(ap),
|
||||
'a_err_from_velocity_m_s2':_vector_summary(av),
|
||||
'per_axis_correlation':_correlation(ap,av),
|
||||
'mean_direction_cosine':cosine,'mean_magnitude_ratio_velocity_over_position':ratio,
|
||||
'difference_velocity_minus_position_m_s2':_vector_summary(diff),
|
||||
'common_acceleration_world_m_s2':_vector_summary(common),
|
||||
'common_acceleration_body_m_s2':_vector_summary(body),
|
||||
'body_bias_stability_std_over_bias_norm':float(
|
||||
np.linalg.norm(body_std)/max(np.linalg.norm(body_bias),1e-12)),
|
||||
'constant_body_accelerometer_bias_direction_stable':bool(
|
||||
np.linalg.norm(body_std)<=np.linalg.norm(body_bias)),
|
||||
'equivalent_horizontal_tilt_rad':horizontal/G0,
|
||||
'equivalent_horizontal_tilt_deg':float(np.degrees(horizontal/G0)),
|
||||
'gravity_tilt_leakage_magnitude_le_0p5deg':bool(
|
||||
np.degrees(horizontal/G0)<=.5)}
|
||||
def grouped(key):
|
||||
groups={}
|
||||
for record in records: groups.setdefault(record[key],[]).append(record)
|
||||
return {name:summarize(items) for name,items in groups.items()}
|
||||
overall=summarize(records)
|
||||
detection_checks={}
|
||||
if records:
|
||||
mp=np.asarray(overall['a_err_from_position_m_s2']['bias'])
|
||||
mv=np.asarray(overall['a_err_from_velocity_m_s2']['bias'])
|
||||
detection_checks={'mean_direction_cosine_ge_0p95':
|
||||
overall['mean_direction_cosine']>=.95,
|
||||
'mean_magnitude_ratio_in_0p75_1p25':
|
||||
.75<=overall['mean_magnitude_ratio_velocity_over_position']<=1.25,
|
||||
'mean_acceleration_difference_norm_le_0p05_m_s2':
|
||||
np.linalg.norm(mv-mp)<=.05}
|
||||
detected=bool(all(detection_checks.values()))
|
||||
else:
|
||||
detected=False
|
||||
return {'overall':overall,'per_session':grouped('session'),
|
||||
'by_motion_class':grouped('motion'),'by_speed':grouped('speed_bin'),
|
||||
'by_gyro_norm':grouped('gyro_bin'),
|
||||
'common_acceleration_detection_gate':{
|
||||
'thresholds':{'direction_cosine_min':.95,
|
||||
'magnitude_ratio_range':[.75,1.25],
|
||||
'mean_difference_norm_max_m_s2':.05},
|
||||
'checks':detection_checks,'passed':detected},
|
||||
'common_constant_acceleration_error_detected':detected},records
|
||||
|
||||
def _outside_targets(t,intervals):
|
||||
return all(not (start-1.<=t<=end+1.) for start,end in intervals)
|
||||
|
||||
def _physical_static_biases(sessions,reference,R,heldout):
|
||||
ranges={}
|
||||
for item in heldout:
|
||||
ranges.setdefault(item['session_id'],[]).append((item['start_s'],item['end_s']))
|
||||
result={}
|
||||
for session in sessions:
|
||||
estimates=[]; times=[]
|
||||
for node in _nodes(session,reference,1.):
|
||||
if (node.zupt_static and node.hpr_factor_valid and
|
||||
_outside_targets(node.t_s,ranges.get(session.session_id,[]))):
|
||||
R_WI=_world_rtk(node.baseline_enu)@R
|
||||
estimates.append(node.accel_m_s2-R_WI.T@(-G_ENU)); times.append(node.t_s)
|
||||
if len(estimates)>=3:
|
||||
a=np.asarray(estimates)
|
||||
result[session.session_id]={'available':True,'sample_count':len(a),
|
||||
'time_min_s':min(times),'time_max_s':max(times),
|
||||
'physical_accel_bias_m_s2':np.median(a,axis=0),
|
||||
'sample_axis_std_m_s2':np.std(a,axis=0),
|
||||
'method':('target-excluded zupt_static + fixed R2G/HPR gravity; '
|
||||
'horizontal components remain gravity-tilt confounded')}
|
||||
else:
|
||||
result[session.session_id]={'available':False,'sample_count':len(estimates),
|
||||
'reason':'fewer than 3 target-excluded independent static nodes'}
|
||||
return result
|
||||
|
||||
def _bias_distribution(values,key):
|
||||
a=np.asarray([x[key] for x in values.values()],dtype=float)
|
||||
return {'session_count':len(a),'mean':np.mean(a,axis=0),'std':np.std(a,axis=0),
|
||||
'min':np.min(a,axis=0),'max':np.max(a,axis=0),
|
||||
'peak_to_peak':np.ptp(a,axis=0)}
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--manifest',type=Path,required=True)
|
||||
p.add_argument('--calibration-selection',type=Path,required=True)
|
||||
p.add_argument('--all-selection',type=Path,required=True)
|
||||
p.add_argument('--engineering-result',type=Path,required=True)
|
||||
p.add_argument('--output',type=Path,required=True)
|
||||
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
args=p.parse_args()
|
||||
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
|
||||
calibration=json.loads(args.calibration_selection.read_text(encoding='utf-8'))
|
||||
selected=json.loads(args.all_selection.read_text(encoding='utf-8'))
|
||||
calibration_ids={x['candidate_id'] for x in calibration['selected_windows']}
|
||||
heldout=[x for x in selected['selected_windows']
|
||||
if x['candidate_id'] not in calibration_ids]
|
||||
if len(calibration_ids)!=47 or len(heldout)!=267:
|
||||
raise RuntimeError(f'expected 47+267 windows, got {len(calibration_ids)}+{len(heldout)}')
|
||||
lever=np.asarray(engineering['prior_constrained_solution']['result']['final_l_I_m'])
|
||||
frozen=np.array([-.4518015159,-.2644749820,.7314656115])
|
||||
if not np.allclose(lever,frozen,atol=1e-10):
|
||||
raise RuntimeError('candidate lever differs from frozen root-cause value')
|
||||
sessions=load_unified_sessions(
|
||||
args.manifest,selected_session_ids={x['session_id'] for x in heldout})
|
||||
reference=_height_reference(sessions)
|
||||
segments=_restore_segments(sessions,reference,heldout,1.)
|
||||
R=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
problems=[build_problem(segment,R,lever,.006) for segment in segments]
|
||||
calibration_bias=_calibration_biases(engineering,problems)
|
||||
physical_bias=_physical_static_biases(sessions,reference,R,heldout)
|
||||
motion_map={'0808_20260808_092827':'circle',
|
||||
'0808_20260808_082148':'left_right',
|
||||
'0815_20260812_123424':'slope'}
|
||||
|
||||
def evaluate(mode):
|
||||
output={'best_position':[],'doppler':[],'imu_preintegration':[]}
|
||||
for problem in problems:
|
||||
session_id=problem.segment.session_id
|
||||
if mode=='calibration_frozen':
|
||||
bg=calibration_bias[session_id]['gyro_bias_rad_s']
|
||||
ba=calibration_bias[session_id]['accel_bias_m_s2']
|
||||
elif mode=='zero':
|
||||
bg=np.zeros(3); ba=np.zeros(3)
|
||||
else:
|
||||
source=physical_bias[session_id]
|
||||
if not source['available']: continue
|
||||
bg=np.zeros(3); ba=source['physical_accel_bias_m_s2']
|
||||
motion=motion_map.get(session_id,'other_recovered_dynamic')
|
||||
records=_interval_innovations(problem,motion,bg,ba)
|
||||
for key,value in records.items(): output[key].extend(value)
|
||||
root,pairs=_root_report(output['best_position'],output['doppler'])
|
||||
return {'bias_source':mode,
|
||||
'BEST_position_innovation':_factor_report(output['best_position']),
|
||||
'Doppler_innovation':_factor_report(output['doppler']),
|
||||
'IMU_preintegration_innovation':_factor_report(output['imu_preintegration']),
|
||||
'propagation_acceleration_consistency':root},pairs,output
|
||||
|
||||
modes={}; pairs={}; raw={}
|
||||
for mode in ('calibration_frozen','zero','session_static_physical'):
|
||||
modes[mode],pairs[mode],raw[mode]=evaluate(mode)
|
||||
static_ids={x['interval_id'] for x in pairs['session_static_physical']}
|
||||
common_subset={}
|
||||
for mode in ('calibration_frozen','zero','session_static_physical'):
|
||||
pos=[x for x in raw[mode]['best_position'] if x['interval_id'] in static_ids]
|
||||
vel=[x for x in raw[mode]['doppler'] if x['interval_id'] in static_ids]
|
||||
root,_=_root_report(pos,vel)
|
||||
common_subset[mode]={'BEST_position_innovation':_factor_report(pos),
|
||||
'Doppler_innovation':_factor_report(vel),
|
||||
'propagation_acceleration_consistency':root}
|
||||
def bias_norm(report,factor):
|
||||
return float(np.linalg.norm(report[factor]['overall']['innovation_bias']))
|
||||
A,C=common_subset['calibration_frozen'],common_subset['session_static_physical']
|
||||
static_available=bool(static_ids)
|
||||
best_reduction=(bias_norm(C,'BEST_position_innovation')/
|
||||
max(bias_norm(A,'BEST_position_innovation'),1e-12)
|
||||
if static_available else np.inf)
|
||||
doppler_reduction=(bias_norm(C,'Doppler_innovation')/
|
||||
max(bias_norm(A,'Doppler_innovation'),1e-12)
|
||||
if static_available else np.inf)
|
||||
nuisance_transfer=bool(static_available and best_reduction<=.5 and doppler_reduction<=.5)
|
||||
high_gyro={}
|
||||
for factor in ('BEST_position_innovation','Doppler_innovation'):
|
||||
summary=modes['calibration_frozen'][factor]['by_gyro_norm'].get('gyro_ge_0p10',{})
|
||||
limit=.5
|
||||
high_gyro[factor]={'sample_count':summary.get('sample_count',0),
|
||||
'bias_norm':float(np.linalg.norm(summary.get('innovation_bias',[np.inf]*3))),
|
||||
'vector_p95':summary.get('vector_p95',np.inf),
|
||||
'passed':bool(summary.get('sample_count',0)>=20 and
|
||||
np.linalg.norm(summary['innovation_bias'])<=.10 and
|
||||
summary['vector_p95']<=limit)}
|
||||
common_detected=modes['calibration_frozen'][
|
||||
'propagation_acceleration_consistency'][
|
||||
'common_constant_acceleration_error_detected']
|
||||
extrinsic_sensitive=bool(common_detected and
|
||||
all(x['passed'] for x in high_gyro.values()))
|
||||
propagation_passed=bool(nuisance_transfer)
|
||||
graph_bias=engineering['prior_constrained_solution'][
|
||||
'calibration_only_frozen_bias_by_session']
|
||||
graph_distribution=_bias_distribution(graph_bias,'accel_bias_m_s2')
|
||||
physical_available={k:v for k,v in physical_bias.items() if v['available']}
|
||||
physical_values={k:{'accel_bias_m_s2':v['physical_accel_bias_m_s2']}
|
||||
for k,v in physical_available.items()}
|
||||
payload={'scope':'propagation bias root-cause only; frozen extrinsic and covariance',
|
||||
'lever_reoptimized':False,'R2G_refit':False,'covariance_retuned':False,
|
||||
'R0_parser_modified':False,'new_window_selection':False,
|
||||
'data_only_free_bootstrap_LOO_called':False,
|
||||
'fixed_l_I_m':lever,'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
|
||||
'calibration_window_count':47,'heldout_window_count':267,
|
||||
'target_GNSS_observation_used_for_bias_estimation':False,
|
||||
'bias_semantics':{
|
||||
'graph_nuisance_accel_bias':(
|
||||
'node-graph nuisance absorbing IMU/model/attitude effects; '
|
||||
'not assumed transferable physical sensor zero bias'),
|
||||
'physical_IMU_accel_bias':(
|
||||
'target-excluded session static estimate; horizontal components '
|
||||
'remain gravity-tilt confounded')},
|
||||
'graph_nuisance_accel_bias_distribution_m_s2':graph_distribution,
|
||||
'per_session_graph_nuisance_bias':graph_bias,
|
||||
'per_session_static_physical_bias':physical_bias,
|
||||
'static_physical_bias_distribution_m_s2':(
|
||||
_bias_distribution(physical_values,'accel_bias_m_s2')
|
||||
if physical_values else {'session_count':0}),
|
||||
'bias_source_ablation':modes,
|
||||
'common_static_interval_subset_ablation':common_subset,
|
||||
'static_common_subset_interval_count':len(static_ids),
|
||||
'physical_over_calibration_bias_norm_ratio':{
|
||||
'BEST_position':best_reduction,'Doppler':doppler_reduction},
|
||||
'common_constant_acceleration_error_detected':common_detected,
|
||||
'nuisance_bias_transfer_failure_detected':nuisance_transfer,
|
||||
'physical_ba_ablation_available':static_available,
|
||||
'nuisance_bias_transfer_assessment':(
|
||||
'confirmed' if nuisance_transfer else
|
||||
'not_testable_no_independent_static_segments' if not static_available
|
||||
else 'not_confirmed_by_static_ablation'),
|
||||
'lever_sensitive_high_gyro_validation':high_gyro,
|
||||
'independent_propagation_validation_passed':propagation_passed,
|
||||
'independent_extrinsic_sensitive_validation_passed':extrinsic_sensitive,
|
||||
'engineering_translation_accepted':False,
|
||||
'acceptance_modified_by_this_audit':False}
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
compact={'common_constant_acceleration_error_detected':common_detected,
|
||||
'static_session_count':len(physical_available),
|
||||
'static_common_subset_interval_count':len(static_ids),
|
||||
'bias_norm_ratio':payload['physical_over_calibration_bias_norm_ratio'],
|
||||
'nuisance_bias_transfer_failure_detected':nuisance_transfer,
|
||||
'high_gyro':high_gyro,
|
||||
'independent_propagation_validation_passed':propagation_passed,
|
||||
'independent_extrinsic_sensitive_validation_passed':extrinsic_sensitive,
|
||||
'engineering_translation_accepted':False,
|
||||
'calibration_frozen_acceleration':
|
||||
modes['calibration_frozen']['propagation_acceleration_consistency']['overall']}
|
||||
print(json.dumps(_jsonable(compact),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
if __name__=='__main__': raise SystemExit(main())
|
||||
@@ -46,6 +46,9 @@ CSV_FIELDS = [
|
||||
"pitch_deg",
|
||||
"roll_deg",
|
||||
"heading_quality",
|
||||
"heading_satellites",
|
||||
"heading_age_s",
|
||||
"heading_station_id",
|
||||
"heading_valid",
|
||||
"gga_utc",
|
||||
"hpr_utc",
|
||||
@@ -170,6 +173,9 @@ def merged_row(
|
||||
"pitch_deg": hpr_fields.get("pitch_deg"),
|
||||
"roll_deg": hpr_fields.get("roll_deg"),
|
||||
"heading_quality": hpr_fields.get("heading_quality"),
|
||||
"heading_satellites": hpr_fields.get("heading_satellites"),
|
||||
"heading_age_s": hpr_fields.get("heading_age_s"),
|
||||
"heading_station_id": hpr_fields.get("heading_station_id"),
|
||||
"heading_valid": hpr_fields.get("heading_valid"),
|
||||
"gga_utc": fields.get("position_time_utc"),
|
||||
"hpr_utc": hpr_fields.get("position_time_utc"),
|
||||
|
||||
@@ -0,0 +1,290 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Export paired G90/HI13 captures without replacing sensor time by host time."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
import re
|
||||
import sys
|
||||
from collections import Counter
|
||||
from datetime import datetime, timedelta, timezone
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from tools.h32_dlog.timeutil import utc_dotnet_ticks_to_unix_s
|
||||
from tools.rscap_v2.capture_format_v2 import file_summary, read_capture
|
||||
from tools.rscap_v2.g90_rtk import RtkSentence, iter_g90_sentences
|
||||
from tools.rscap_v2.hi13_imu import Hi13Sample, iter_hi13_imu_samples
|
||||
from tools.time_alignment import AffineClockModel, fit_affine_clock
|
||||
|
||||
GPS_EPOCH_UNIX_S = datetime(1980, 1, 6, tzinfo=timezone.utc).timestamp()
|
||||
DEFAULT_SOURCES = (
|
||||
("0808", Path("D:/data/raw_serial_capture_v2"), "20260808"),
|
||||
("0815", Path("D:/data/0815/raw_serial_capture_v2"), None),
|
||||
("0819", Path("D:/data/0819/raw_serial_capture_v2"), None),
|
||||
)
|
||||
RTK_FIELDS = [
|
||||
"message_type", "t_device_s", "measurement_utc_s", "gnss_time_s",
|
||||
"gnss_week", "gnss_tow_ms", "leap_seconds", "host_receive_utc_s",
|
||||
"host_minus_measurement_s", "receive_utc_ticks", "checksum_valid",
|
||||
"position_time_utc", "lat_deg", "lon_deg", "altitude_m",
|
||||
"position_status", "position_type", "position_fixed", "fix_quality",
|
||||
"satellites", "solution_satellites", "hdop", "undulation_m",
|
||||
"lat_std_m", "lon_std_m", "altitude_std_m", "differential_age_s",
|
||||
"solution_age_s", "station_id", "heading_deg", "pitch_deg", "roll_deg",
|
||||
"heading_quality", "heading_satellites", "heading_age_s",
|
||||
"heading_station_id", "heading_valid", "baseline_length_m",
|
||||
"heading_type", "heading_solution_satellites", "velocity_status",
|
||||
"velocity_type", "doppler_velocity_valid", "velocity_latency_s",
|
||||
"velocity_age_s", "horizontal_speed_m_s", "track_ground_deg",
|
||||
"velocity_east_m_s", "velocity_north_m_s", "vertical_speed_m_s",
|
||||
"horizontal_speed_std_m_s", "vertical_speed_std_m_s", "raw_line",
|
||||
]
|
||||
|
||||
|
||||
def _capture_time(path: Path) -> datetime:
|
||||
match = re.search(r"_(\d{8}-\d{6}\.\d+)_", path.name)
|
||||
if not match:
|
||||
raise ValueError(f"capture filename has no timestamp: {path}")
|
||||
return datetime.strptime(match.group(1), "%Y%m%d-%H%M%S.%f")
|
||||
|
||||
|
||||
def discover_pairs(
|
||||
sources: tuple[tuple[str, Path, str | None], ...],
|
||||
*,
|
||||
max_start_delta_s: float = 1.5,
|
||||
) -> tuple[list[dict], list[dict]]:
|
||||
pairs: list[dict] = []
|
||||
unmatched: list[dict] = []
|
||||
for batch, folder, date_prefix in sources:
|
||||
rtk = sorted(folder.glob("wheeltec-g90*.rscap"))
|
||||
imu = sorted(folder.glob("hi13*.rscap"))
|
||||
if date_prefix:
|
||||
rtk = [path for path in rtk if date_prefix in path.name]
|
||||
imu = [path for path in imu if date_prefix in path.name]
|
||||
available = set(imu)
|
||||
for rtk_path in rtk:
|
||||
candidates = sorted(
|
||||
(
|
||||
(abs((_capture_time(path) - _capture_time(rtk_path)).total_seconds()), path)
|
||||
for path in available
|
||||
),
|
||||
key=lambda item: item[0],
|
||||
)
|
||||
if not candidates or candidates[0][0] > max_start_delta_s:
|
||||
unmatched.append({"batch": batch, "rtk_rscap": str(rtk_path)})
|
||||
continue
|
||||
delta, imu_path = candidates[0]
|
||||
available.remove(imu_path)
|
||||
stamp = _capture_time(rtk_path).strftime("%Y%m%d_%H%M%S")
|
||||
pairs.append(
|
||||
{
|
||||
"session_id": f"{batch}_{stamp}",
|
||||
"batch_id": batch,
|
||||
"rtk_rscap": rtk_path,
|
||||
"imu_rscap": imu_path,
|
||||
"capture_start_delta_s": delta,
|
||||
}
|
||||
)
|
||||
return pairs, unmatched
|
||||
|
||||
|
||||
def _fit_clock(samples: list[Hi13Sample]) -> AffineClockModel:
|
||||
stride = max(1, len(samples) // 20000)
|
||||
selected = samples[::stride]
|
||||
device = np.asarray([sample.t_s for sample in selected], dtype=float)
|
||||
host = np.asarray(
|
||||
[utc_dotnet_ticks_to_unix_s(sample.host_receive_utc_ticks) for sample in selected],
|
||||
dtype=float,
|
||||
)
|
||||
return fit_affine_clock(device, host)
|
||||
|
||||
|
||||
def _nmea_utc_to_unix_s(value: str, host_s: float) -> float:
|
||||
packed = float(value)
|
||||
hour = int(packed // 10000)
|
||||
minute = int((packed - hour * 10000) // 100)
|
||||
second = packed - hour * 10000 - minute * 100
|
||||
receive = datetime.fromtimestamp(host_s, tz=timezone.utc)
|
||||
midnight = datetime(receive.year, receive.month, receive.day, tzinfo=timezone.utc).timestamp()
|
||||
same_day = midnight + hour * 3600 + minute * 60 + second
|
||||
return min((same_day - 86400.0, same_day, same_day + 86400.0),
|
||||
key=lambda candidate: abs(candidate - host_s))
|
||||
|
||||
|
||||
def _measurement_time(row: RtkSentence) -> tuple[float, float | None]:
|
||||
fields = row.fields
|
||||
week = fields.get("gnss_week")
|
||||
tow_ms = fields.get("gnss_tow_ms")
|
||||
leap = fields.get("leap_seconds")
|
||||
if week is not None and tow_ms is not None:
|
||||
gnss_s = float(week) * 604800.0 + float(tow_ms) * 1e-3
|
||||
utc_s = GPS_EPOCH_UNIX_S + gnss_s - float(leap or 0)
|
||||
return utc_s, gnss_s
|
||||
value = fields.get("position_time_utc")
|
||||
if value is None:
|
||||
raise ValueError(f"{row.sentence_type} has no sensor measurement time")
|
||||
host_s = utc_dotnet_ticks_to_unix_s(row.receive_utc_ticks)
|
||||
return _nmea_utc_to_unix_s(str(value), host_s), None
|
||||
|
||||
|
||||
def _format(value: object) -> object:
|
||||
if value is None:
|
||||
return ""
|
||||
if isinstance(value, bool):
|
||||
return int(value)
|
||||
return value
|
||||
|
||||
|
||||
def _write_imu_npz(path: Path, samples: list[Hi13Sample]) -> None:
|
||||
np.savez_compressed(
|
||||
path,
|
||||
system_time_s=np.asarray([row.t_s for row in samples], dtype=np.float64),
|
||||
system_time_ms=np.asarray([row.system_time_ms for row in samples], dtype=np.uint32),
|
||||
host_receive_utc_s=np.asarray(
|
||||
[utc_dotnet_ticks_to_unix_s(row.host_receive_utc_ticks) for row in samples],
|
||||
dtype=np.float64,
|
||||
),
|
||||
gyro_rad_s=np.asarray([row.gyro_rad_s for row in samples], dtype=np.float64),
|
||||
accel_m_s2=np.asarray([row.accel_m_s2 for row in samples], dtype=np.float64),
|
||||
rpy_deg=np.asarray([row.rpy_deg for row in samples], dtype=np.float64),
|
||||
quaternion_wxyz=np.asarray([row.quaternion_wxyz for row in samples], dtype=np.float64),
|
||||
mag_ut=np.asarray([row.mag_ut for row in samples], dtype=np.float64),
|
||||
pps_sync_stamp_ms=np.asarray([row.pps_sync_stamp_ms for row in samples], dtype=np.uint16),
|
||||
temperature_c=np.asarray([row.temperature_c for row in samples], dtype=np.int16),
|
||||
air_pressure_pa=np.asarray([row.air_pressure_pa for row in samples], dtype=np.float64),
|
||||
frame_tag=np.asarray([row.frame_tag for row in samples], dtype=np.uint8),
|
||||
)
|
||||
|
||||
|
||||
def _write_rtk_csv(
|
||||
path: Path,
|
||||
rows: list[RtkSentence],
|
||||
clock: AffineClockModel,
|
||||
) -> Counter:
|
||||
counts: Counter = Counter()
|
||||
with path.open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=RTK_FIELDS)
|
||||
writer.writeheader()
|
||||
for row in rows:
|
||||
host_s = utc_dotnet_ticks_to_unix_s(row.receive_utc_ticks)
|
||||
try:
|
||||
measurement_s, gnss_s = _measurement_time(row)
|
||||
t_device_s = clock.inverse(measurement_s)
|
||||
host_minus_measurement_s = host_s - measurement_s
|
||||
except (TypeError, ValueError):
|
||||
measurement_s = None
|
||||
gnss_s = None
|
||||
t_device_s = None
|
||||
host_minus_measurement_s = None
|
||||
counts[f"{row.sentence_type}_invalid_measurement_time"] += 1
|
||||
fields = dict(row.fields)
|
||||
fields.update(
|
||||
{
|
||||
"message_type": row.sentence_type,
|
||||
"t_device_s": t_device_s,
|
||||
"measurement_utc_s": measurement_s,
|
||||
"gnss_time_s": gnss_s,
|
||||
"host_receive_utc_s": host_s,
|
||||
"host_minus_measurement_s": host_minus_measurement_s,
|
||||
"receive_utc_ticks": row.receive_utc_ticks,
|
||||
"checksum_valid": row.checksum_valid,
|
||||
"raw_line": row.raw_line,
|
||||
}
|
||||
)
|
||||
writer.writerow({name: _format(fields.get(name)) for name in RTK_FIELDS})
|
||||
counts[row.sentence_type] += 1
|
||||
return counts
|
||||
|
||||
|
||||
def export_pair(pair: dict, output_root: Path, *, overwrite: bool) -> dict:
|
||||
destination = output_root / str(pair["session_id"])
|
||||
if destination.exists() and not overwrite:
|
||||
raise FileExistsError(f"{destination} exists; pass --overwrite")
|
||||
destination.mkdir(parents=True, exist_ok=True)
|
||||
imu_capture = read_capture(pair["imu_rscap"])
|
||||
rtk_capture = read_capture(pair["rtk_rscap"])
|
||||
imu_rows = iter_hi13_imu_samples(imu_capture)
|
||||
if len(imu_rows) < 2:
|
||||
raise ValueError(f"not enough CRC-valid HI91 rows: {pair['imu_rscap']}")
|
||||
if np.any(np.diff(np.asarray([row.t_s for row in imu_rows])) <= 0):
|
||||
raise ValueError(f"HI13 system_time is not strictly increasing: {pair['imu_rscap']}")
|
||||
clock = _fit_clock(imu_rows)
|
||||
rtk_rows = iter_g90_sentences(rtk_capture)
|
||||
_write_imu_npz(destination / "imu.npz", imu_rows)
|
||||
counts = _write_rtk_csv(destination / "rtk.csv", rtk_rows, clock)
|
||||
quaternion = np.asarray([row.quaternion_wxyz for row in imu_rows], dtype=float)
|
||||
quaternion_norm = np.linalg.norm(quaternion, axis=1)
|
||||
summary = {
|
||||
**{key: str(value) if isinstance(value, Path) else value for key, value in pair.items()},
|
||||
"time_policy": {
|
||||
"master": "HI13 system_time; RTK GNSS measurement time mapped into that clock",
|
||||
"host_receive_time": "diagnostic and affine cross-clock bridge only",
|
||||
},
|
||||
"imu_capture": file_summary(imu_capture),
|
||||
"rtk_capture": file_summary(rtk_capture),
|
||||
"imu_valid_rows": len(imu_rows),
|
||||
"imu_time_span_s": float(imu_rows[-1].t_s - imu_rows[0].t_s),
|
||||
"quaternion_norm_p01_p50_p99": [
|
||||
float(np.percentile(quaternion_norm, percentile)) for percentile in (1, 50, 99)
|
||||
],
|
||||
"rtk_records": dict(counts),
|
||||
"clock_model_device_to_host": clock.to_dict(),
|
||||
}
|
||||
(destination / "export_summary.json").write_text(
|
||||
json.dumps(summary, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
return summary
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--output-root", type=Path, required=True)
|
||||
parser.add_argument("--overwrite", action="store_true")
|
||||
parser.add_argument("--resume", action="store_true")
|
||||
parser.add_argument("--session", action="append")
|
||||
args = parser.parse_args(argv)
|
||||
pairs, unmatched = discover_pairs(DEFAULT_SOURCES)
|
||||
if args.session:
|
||||
selected = set(args.session)
|
||||
pairs = [pair for pair in pairs if pair["session_id"] in selected]
|
||||
args.output_root.mkdir(parents=True, exist_ok=True)
|
||||
summaries = []
|
||||
for index, pair in enumerate(pairs, 1):
|
||||
print(f"[{index}/{len(pairs)}] {pair['session_id']}", flush=True)
|
||||
summary_path = args.output_root / pair["session_id"] / "export_summary.json"
|
||||
if args.resume and summary_path.is_file():
|
||||
summaries.append(json.loads(summary_path.read_text(encoding="utf-8")))
|
||||
continue
|
||||
summaries.append(export_pair(pair, args.output_root, overwrite=args.overwrite))
|
||||
manifest = {
|
||||
"schema_version": 3,
|
||||
"session_count": len(summaries),
|
||||
"unmatched_rtk": unmatched,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": item["session_id"],
|
||||
"batch_id": item["batch_id"],
|
||||
"directory": str((args.output_root / item["session_id"]).resolve()),
|
||||
"rtk_records": item["rtk_records"],
|
||||
"imu_valid_rows": item["imu_valid_rows"],
|
||||
}
|
||||
for item in summaries
|
||||
],
|
||||
}
|
||||
(args.output_root / "manifest.json").write_text(
|
||||
json.dumps(manifest, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
print(json.dumps({"output_root": str(args.output_root), **manifest}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,139 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Combine final mechanical-prior engineering release gates without refitting.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.geometry import make_transform
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
|
||||
SUMMARY=('Translation is mechanically anchored and dynamically validated. '
|
||||
'The current dataset does not independently observe translation accurately '
|
||||
'enough for data-only calibration, and does not provide meaningful refinement '
|
||||
'beyond the mechanical prior.')
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--calibration',type=Path,required=True)
|
||||
p.add_argument('--heldout-postfit',type=Path,required=True)
|
||||
p.add_argument('--innovation',type=Path,required=True)
|
||||
p.add_argument('--sensitivity',type=Path,required=True)
|
||||
p.add_argument('--convergence-retry',type=Path,required=True)
|
||||
p.add_argument('--propagation-root-cause',type=Path)
|
||||
p.add_argument('--output',type=Path,required=True)
|
||||
args=p.parse_args()
|
||||
calibration=json.loads(args.calibration.read_text(encoding='utf-8'))
|
||||
heldout=json.loads(args.heldout_postfit.read_text(encoding='utf-8'))
|
||||
innovation=json.loads(args.innovation.read_text(encoding='utf-8'))
|
||||
sensitivity=json.loads(args.sensitivity.read_text(encoding='utf-8'))
|
||||
retry=json.loads(args.convergence_retry.read_text(encoding='utf-8'))
|
||||
root_cause=(None if args.propagation_root_cause is None else
|
||||
json.loads(args.propagation_root_cause.read_text(encoding='utf-8')))
|
||||
lever=np.asarray(calibration['prior_constrained_solution']['result']['final_l_I_m'])
|
||||
R=Rotation.from_euler('xyz',[.4543066225,-.0026392019,.0122384129],
|
||||
degrees=True).as_matrix()
|
||||
candidate_T=make_transform(-R@lever,R)
|
||||
candidate_inverse=np.linalg.inv(candidate_T)
|
||||
inverse_error=float(np.linalg.norm(candidate_T@candidate_inverse-np.eye(4)))
|
||||
if inverse_error>=1e-10: raise RuntimeError('candidate transforms are not inverse')
|
||||
comparison=calibration['comparisons']
|
||||
def nonconflicting(name):
|
||||
value=comparison[name]
|
||||
return (value['relative_cost_delta']<=.05 and
|
||||
all(x['p95_abs_delta']<=.25
|
||||
for x in value['factor_residual_delta'].values()))
|
||||
mechanical_consistent=bool(
|
||||
nonconflicting('fixed_vs_free') and nonconflicting('prior_data_vs_free'))
|
||||
overall=heldout['heldout_validation']['overall']
|
||||
physical_checks={
|
||||
'all_267_converged_after_retry':
|
||||
retry['heldout_convergence_after_retry']==1.,
|
||||
'BEST_position_vector_p95_le_0p20_m':
|
||||
overall['best_position_physical_m']['vector_p95']<=.20,
|
||||
'Doppler_vector_p95_le_0p50_m_s':
|
||||
overall['doppler_physical_m_s']['vector_p95']<=.50,
|
||||
'HPR_normalized_p95_le_4':
|
||||
overall['residual_by_factor']['hpr']['p95_abs']<=4.,
|
||||
'preintegration_normalized_p95_le_3':
|
||||
overall['residual_by_factor']['imu_preintegration']['p95_abs']<=3.}
|
||||
physical_passed=bool(all(physical_checks.values()))
|
||||
statistical_passed=bool(.25<=overall['global_chi_square_per_dof']<=4.)
|
||||
underdispersion=bool(physical_passed and not statistical_passed and
|
||||
overall['global_chi_square_per_dof']<.25)
|
||||
independent=bool(innovation['independent_heldout_innovation_passed'])
|
||||
rotation=bool(sensitivity['rotation_sensitivity_passed'])
|
||||
accepted=bool(mechanical_consistent and physical_passed and independent and rotation)
|
||||
payload={'scope':'final mechanical-prior RTK-IMU engineering release decision',
|
||||
'no_refit_performed':True,'data_only_full_free_called':False,
|
||||
'bootstrap_called':False,'loo_called':False,
|
||||
'covariance_retuned':False,'new_window_selection_called':False,
|
||||
'parser_R0_modified':False,
|
||||
'data_only_translation_accepted':False,
|
||||
'translation_refined_by_data':False,
|
||||
'mechanical_prior_consistent_with_calibration':mechanical_consistent,
|
||||
'heldout_physical_validation_passed':physical_passed,
|
||||
'heldout_physical_gate_checks':physical_checks,
|
||||
'heldout_statistical_scale_passed':statistical_passed,
|
||||
'heldout_postfit_chi_square_per_dof':
|
||||
overall['global_chi_square_per_dof'],
|
||||
'heldout_covariance_underdispersion_warning':underdispersion,
|
||||
'independent_heldout_innovation_passed':independent,
|
||||
'common_constant_acceleration_error_detected':(
|
||||
None if root_cause is None else
|
||||
root_cause['common_constant_acceleration_error_detected']),
|
||||
'independent_propagation_validation_passed':(
|
||||
None if root_cause is None else
|
||||
root_cause['independent_propagation_validation_passed']),
|
||||
'independent_extrinsic_sensitive_validation_passed':(
|
||||
None if root_cause is None else
|
||||
root_cause['independent_extrinsic_sensitive_validation_passed']),
|
||||
'rotation_sensitivity_passed':rotation,
|
||||
'engineering_translation_acceptance_formula':(
|
||||
'mechanical_prior_consistent_with_calibration AND '
|
||||
'heldout_physical_validation_passed AND '
|
||||
'independent_heldout_innovation_passed AND rotation_sensitivity_passed'),
|
||||
'engineering_translation_accepted':accepted,
|
||||
'result_nature':'mechanically anchored + dynamically validated',
|
||||
'summary':SUMMARY,'forbidden_descriptions':[
|
||||
'data-only calibrated translation','dynamically refined mechanical lever'],
|
||||
'candidate_l_I_engineering_m':lever,
|
||||
'candidate_T_RTK_IMU':candidate_T,
|
||||
'candidate_T_IMU_RTK':candidate_inverse,
|
||||
'candidate_transform_inverse_error_norm':inverse_error,
|
||||
'l_I_engineering_m':lever if accepted else None,
|
||||
'T_RTK_IMU':candidate_T if accepted else None,
|
||||
'T_IMU_RTK':candidate_inverse if accepted else None,
|
||||
'transform_convention':{
|
||||
'equation':'p_RTK = R_RTK_IMU * p_IMU + t_RTK_IMU',
|
||||
'translation':'t_RTK_IMU = -R_RTK_IMU * l_I',
|
||||
'RTK_origin':'ANT1 phase center'},
|
||||
'rotation_source':'R2G_gravity_level_prior',
|
||||
'translation_conditional_on_rotation':True,
|
||||
'evidence':{
|
||||
'calibration_path':str(args.calibration),
|
||||
'heldout_postfit_path':str(args.heldout_postfit),
|
||||
'innovation_path':str(args.innovation),
|
||||
'sensitivity_path':str(args.sensitivity),
|
||||
'convergence_retry_path':str(args.convergence_retry),
|
||||
'propagation_root_cause_path':(
|
||||
None if args.propagation_root_cause is None
|
||||
else str(args.propagation_root_cause)),
|
||||
'posterior_prior_variance_ratio':
|
||||
calibration['prior_constrained_solution']['posterior_prior_variance_ratio'],
|
||||
'heldout_convergence_after_retry':
|
||||
retry['heldout_convergence_after_retry'],
|
||||
'rotation_sensitivity_summary':sensitivity['summary']}}
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable({key:payload[key] for key in (
|
||||
'data_only_translation_accepted','translation_refined_by_data',
|
||||
'mechanical_prior_consistent_with_calibration',
|
||||
'heldout_physical_validation_passed','heldout_statistical_scale_passed',
|
||||
'heldout_covariance_underdispersion_warning',
|
||||
'independent_heldout_innovation_passed','rotation_sensitivity_passed',
|
||||
'engineering_translation_accepted')}),indent=2))
|
||||
return 0
|
||||
if __name__=='__main__': raise SystemExit(main())
|
||||
@@ -0,0 +1,137 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Refine non-overlapping windows after comparing predicted and actual lever information.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem,linearized_lever_information
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
from tools.select_rtk_imu_windows_by_lever_information import (
|
||||
STD_GATE,_overlap,_sample_keys,_summary,_window_candidates)
|
||||
|
||||
|
||||
def _sqrt_psd(matrix,inverse=False):
|
||||
values,vectors=np.linalg.eigh(.5*(matrix+matrix.T))
|
||||
values=np.maximum(values,1e-12)
|
||||
scale=1./np.sqrt(values) if inverse else np.sqrt(values)
|
||||
return (vectors*scale)@vectors.T
|
||||
|
||||
|
||||
def main():
|
||||
parser=argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--base-selection',type=Path,required=True)
|
||||
parser.add_argument('--actual-result',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--window-duration-s',type=float,default=15.)
|
||||
parser.add_argument('--max-additional-windows',type=int,default=400)
|
||||
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
args=parser.parse_args()
|
||||
base=json.loads(args.base_selection.read_text(encoding='utf-8'))
|
||||
actual=json.loads(args.actual_result.read_text(encoding='utf-8'))
|
||||
actual_solution=next(iter(actual['solutions'].values()))
|
||||
lever=np.asarray(actual_solution['final_l_I_m'],dtype=float)
|
||||
actual_cov=np.asarray(actual_solution['lever_covariance_m2'],dtype=float)
|
||||
actual_information=np.linalg.pinv(actual_cov,rcond=1e-9)
|
||||
predicted_information=sum((np.asarray(item['single_window_information'])
|
||||
for item in base['selected_windows']),np.zeros((3,3)))
|
||||
correction=_sqrt_psd(actual_information)@_sqrt_psd(predicted_information,True)
|
||||
sessions=load_unified_sessions(args.manifest)
|
||||
reference=_height_reference(sessions)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
restored=_restore_segments(sessions,reference,base['selected_windows'],
|
||||
args.sample_period_s)
|
||||
session_by_id={session.session_id:session for session in sessions}
|
||||
selected=[]
|
||||
for item,segment in zip(base['selected_windows'],restored):
|
||||
selected.append({**item,'segment':segment,
|
||||
'session':session_by_id[item['session_id']]})
|
||||
candidates=[entry for entry in _window_candidates(
|
||||
sessions,reference,args.sample_period_s,args.window_duration_s)
|
||||
if not any(_overlap(entry,item) for item in selected)]
|
||||
for entry in candidates:
|
||||
problem=build_problem(entry['segment'],rotation,lever,args.hpr_direct_sigma_rad)
|
||||
raw,_,singular,weak=linearized_lever_information(problem,lever)
|
||||
entry['raw_information']=raw
|
||||
entry['information']=correction@raw@correction.T
|
||||
entry['single_window_singular_values']=singular
|
||||
entry['single_window_weakest_direction_I']=weak
|
||||
total=actual_information.copy()
|
||||
curve=[{'window_count':len(selected),'added_candidate_id':'actual_base',
|
||||
'information_source':'actual_joint_free_schur',**_summary(total)}]
|
||||
remaining=list(candidates); saturation_count=0
|
||||
stop_reason='candidate_exhausted'
|
||||
while remaining and len(selected)<len(base['selected_windows'])+args.max_additional_windows:
|
||||
feasible=[entry for entry in remaining
|
||||
if not any(_overlap(entry,item) for item in selected)]
|
||||
if not feasible: break
|
||||
ranked=[]
|
||||
for entry in feasible:
|
||||
summary=_summary(total+entry['information'])
|
||||
ranked.append((summary['lambda_min'],summary['logdet'],entry,summary))
|
||||
_,_,choice,summary=max(ranked,key=lambda item:(item[0],item[1]))
|
||||
gain=summary['lambda_min']-curve[-1]['lambda_min']
|
||||
selected.append(choice); remaining.remove(choice); total+=choice['information']
|
||||
curve.append({'window_count':len(selected),'added_candidate_id':choice['candidate_id'],
|
||||
'delta_lambda_min':gain,'information_source':'actual-calibrated prediction',
|
||||
**summary})
|
||||
saturation_count=saturation_count+1 if gain<.01 else 0
|
||||
if np.all(np.asarray(summary['std_m'])<=STD_GATE):
|
||||
stop_reason='actual_calibrated_observability_gate_reached'; break
|
||||
if saturation_count>=3:
|
||||
stop_reason='incremental_lambda_min_gain_saturated'; break
|
||||
else:
|
||||
if len(selected)>=len(base['selected_windows'])+args.max_additional_windows:
|
||||
stop_reason='max_additional_windows_reached'
|
||||
seen={'imu':set(),'gnss':set(),'hpr':set()}; duplicate={key:0 for key in seen}
|
||||
output=[]
|
||||
for order,entry in enumerate(selected):
|
||||
keys=_sample_keys(entry); shared={key:len(value&seen[key]) for key,value in keys.items()}
|
||||
for key,value in keys.items(): duplicate[key]+=shared[key]; seen[key].update(value)
|
||||
information=np.asarray(entry['information'] if 'information' in entry
|
||||
else entry['single_window_information'])
|
||||
output.append({'selection_order':order,'candidate_id':entry['candidate_id'],
|
||||
'session_id':entry['session_id'],'start_s':entry['start_s'],
|
||||
'end_s':entry['end_s'],'duration_s':entry['duration_s'],
|
||||
'node_count':entry['node_count'],'seed_window':order<len(base['selected_windows']),
|
||||
'single_window_information':information,
|
||||
'raw_single_window_information':entry.get('raw_information'),
|
||||
'sample_count':{key:len(value) for key,value in keys.items()},
|
||||
'shared_sample_count_with_previous':shared})
|
||||
overlap={'duplicate_sample_count':duplicate,
|
||||
'all_selected_windows_time_nonoverlapping_within_session':not any(
|
||||
_overlap(a,b) for i,a in enumerate(selected) for b in selected[i+1:]),
|
||||
'unique_sample_count':{key:len(value) for key,value in seen.items()}}
|
||||
payload={'scope':'actual-H calibrated incremental lever-information selection',
|
||||
'manual_prior_used':False,'base_window_count':len(base['selected_windows']),
|
||||
'candidate_count':len(candidates),'selected_window_count':len(selected),
|
||||
'actual_base_l_I_m':lever,'actual_base_information':actual_information,
|
||||
'predicted_base_information':predicted_information,
|
||||
'information_congruence_correction':correction,
|
||||
'selection_objective':'maximize lambda_min(H_l); break ties by logdet(H_l)',
|
||||
'std_gate_m':STD_GATE,'stop_reason':stop_reason,
|
||||
'selected_windows':output,'lever_std_vs_information_curve':curve,
|
||||
'sample_overlap_audit':overlap,
|
||||
'final_calibrated_linearized_observable':bool(
|
||||
np.all(np.asarray(curve[-1]['std_m'])<=STD_GATE))}
|
||||
args.output.parent.mkdir(parents=True,exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable({'selected_window_count':len(selected),
|
||||
'additional_window_count':len(selected)-len(base['selected_windows']),
|
||||
'stop_reason':stop_reason,'final_curve':curve[-1],
|
||||
'sample_overlap_audit':overlap}),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,104 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Retry only held-out fixed-lever windows that ended at max_nfev.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from concurrent.futures import ProcessPoolExecutor
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import (
|
||||
build_problem,fit_states_at_fixed_lever,summarize_fixed_state_values)
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
|
||||
def _retry(task):
|
||||
index,problem,lever,state,max_nfev=task
|
||||
value,meta=fit_states_at_fixed_lever(
|
||||
problem,lever,max_nfev,initial_state_values=state)
|
||||
return index,value,meta
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--manifest',type=Path,required=True)
|
||||
p.add_argument('--calibration-selection',type=Path,required=True)
|
||||
p.add_argument('--all-selection',type=Path,required=True)
|
||||
p.add_argument('--engineering-result',type=Path,required=True)
|
||||
p.add_argument('--checkpoint-dir',type=Path,required=True)
|
||||
p.add_argument('--output',type=Path,required=True)
|
||||
p.add_argument('--retry-checkpoint-dir',type=Path,required=True)
|
||||
p.add_argument('--max-nfev',type=int,default=120)
|
||||
p.add_argument('--workers',type=int,default=4)
|
||||
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
args=p.parse_args()
|
||||
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
|
||||
calibration=json.loads(args.calibration_selection.read_text(encoding='utf-8'))
|
||||
selected=json.loads(args.all_selection.read_text(encoding='utf-8'))
|
||||
ids={x['candidate_id'] for x in calibration['selected_windows']}
|
||||
heldout=[x for x in selected['selected_windows'] if x['candidate_id'] not in ids]
|
||||
lever=np.asarray(engineering['engineering_l_I_m'],dtype=float)
|
||||
bad=[]
|
||||
for index,item in enumerate(heldout):
|
||||
data=np.load(args.checkpoint_dir/f'{index:04d}.npz',allow_pickle=False)
|
||||
meta=json.loads(str(data['optimizer']))
|
||||
if not meta['success']:
|
||||
bad.append((index,item,data['state'].copy(),meta))
|
||||
if len(bad)!=10: raise RuntimeError(f'expected 10 non-converged windows, got {len(bad)}')
|
||||
sessions=load_unified_sessions(
|
||||
args.manifest,selected_session_ids={item['session_id'] for _,item,_,_ in bad})
|
||||
reference=_height_reference(sessions)
|
||||
selections=[item for _,item,_,_ in bad]
|
||||
segments=_restore_segments(sessions,reference,selections,1.)
|
||||
R=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
problems=[build_problem(segment,R,lever,.006) for segment in segments]
|
||||
tasks=[(local,problem,lever,bad[local][2],args.max_nfev)
|
||||
for local,problem in enumerate(problems)]
|
||||
with ProcessPoolExecutor(max_workers=args.workers) as executor:
|
||||
retried=list(executor.map(_retry,tasks))
|
||||
retried.sort(key=lambda x:x[0])
|
||||
args.retry_checkpoint_dir.mkdir(parents=True,exist_ok=True)
|
||||
mapping={'0808_20260808_092827':'circle',
|
||||
'0808_20260808_082148':'left_right',
|
||||
'0815_20260812_123424':'slope'}
|
||||
results=[]
|
||||
for local,state,meta in retried:
|
||||
original_index,item,_,previous=bad[local]
|
||||
summary=summarize_fixed_state_values([problems[local]],[state],lever)
|
||||
np.savez_compressed(args.retry_checkpoint_dir/f'{original_index:04d}.npz',
|
||||
state=state,optimizer=json.dumps(meta))
|
||||
results.append({'heldout_index':original_index,
|
||||
'candidate_id':item['candidate_id'],'session_id':item['session_id'],
|
||||
'time_range_s':[item['start_s'],item['end_s']],
|
||||
'motion_class':mapping.get(item['session_id'],'other_recovered_dynamic'),
|
||||
'original_termination_reason':previous['message'],
|
||||
'original_nfev':previous['nfev'],'original_final_cost':previous['cost'],
|
||||
'retry_started_from_original_final_state':True,
|
||||
'retry_termination_reason':meta['message'],'retry_nfev':meta['nfev'],
|
||||
'retry_initial_cost':meta['initial_cost'],'retry_final_cost':meta['cost'],
|
||||
'retry_hit_max_nfev':bool(not meta['success'] and meta['nfev']>=args.max_nfev),
|
||||
'retry_success':meta['success'],'residual':summary})
|
||||
converged=sum(x['retry_success'] for x in results)
|
||||
payload={'scope':'10 held-out max_nfev windows; exact-model continuation retry',
|
||||
'lever_reoptimized':False,'covariance_parameters_modified':False,
|
||||
'parser_R0_modified':False,'windows_removed':False,
|
||||
'fixed_l_I_m':lever,'retry_max_nfev':args.max_nfev,
|
||||
'initial_nonconverged_count':len(results),
|
||||
'converged_after_retry_count':converged,
|
||||
'heldout_convergence_after_retry':(257+converged)/267.,
|
||||
'remaining_nonconverged_count':len(results)-converged,
|
||||
'windows':results}
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable({'converged_after_retry':converged,
|
||||
'remaining':len(results)-converged,
|
||||
'heldout_convergence_after_retry':payload['heldout_convergence_after_retry'],
|
||||
'windows':[{'index':x['heldout_index'],'session':x['session_id'],
|
||||
'motion':x['motion_class'],'success':x['retry_success'],
|
||||
'nfev':x['retry_nfev'],'cost':x['retry_final_cost']}
|
||||
for x in results]}),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
if __name__=='__main__': raise SystemExit(main())
|
||||
+163
-10
@@ -1,4 +1,9 @@
|
||||
"""Decode Wheeltec G90 NMEA (GGA / GNHPR) from a V2 .rscap capture."""
|
||||
"""Decode calibration-relevant Wheeltec G90 logs from a V2 capture.
|
||||
|
||||
GNSS-owned measurement time is preserved for every record. Host receive time
|
||||
only identifies the chunk that completed the line and must not be substituted
|
||||
for the measurement timestamp.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
@@ -23,6 +28,32 @@ def nmea_checksum_valid(line: str) -> bool:
|
||||
return value == expected
|
||||
|
||||
|
||||
def unicore_checksum_valid(line: str) -> bool:
|
||||
"""Validate the CRC32 suffix used by Unicore hash-prefixed logs."""
|
||||
|
||||
star = line.rfind("*")
|
||||
if star < 0:
|
||||
return False
|
||||
try:
|
||||
expected = int(line[star + 1 : star + 9], 16)
|
||||
except ValueError:
|
||||
return False
|
||||
crc = 0
|
||||
for value in line[1:star].encode("ascii", "replace"):
|
||||
crc ^= value
|
||||
for _ in range(8):
|
||||
crc = (crc >> 1) ^ (0xEDB88320 if crc & 1 else 0)
|
||||
return (crc & 0xFFFFFFFF) == expected
|
||||
|
||||
|
||||
def g90_checksum_valid(line: str) -> bool:
|
||||
if line.startswith("$"):
|
||||
return nmea_checksum_valid(line)
|
||||
if line.startswith("#"):
|
||||
return unicore_checksum_valid(line)
|
||||
return False
|
||||
|
||||
|
||||
def _safe_float(value: str):
|
||||
try:
|
||||
return float(value)
|
||||
@@ -76,8 +107,124 @@ def parse_gnhpr(line: str) -> dict:
|
||||
"pitch_deg": _safe_float(fields[3]),
|
||||
"roll_deg": _safe_float(fields[4]),
|
||||
"heading_quality": quality,
|
||||
"satellites": _safe_int(fields[6]),
|
||||
"heading_valid": quality in {4, 5},
|
||||
"heading_satellites": _safe_int(fields[6]),
|
||||
"heading_age_s": _safe_float(fields[7]) if len(fields) > 7 else None,
|
||||
"heading_station_id": fields[8] if len(fields) > 8 else None,
|
||||
"heading_valid": quality == 4,
|
||||
}
|
||||
|
||||
|
||||
def _split_unicore(line: str) -> tuple[list[str], list[str]]:
|
||||
before_checksum = line[: line.rfind("*")]
|
||||
header, payload = before_checksum.split(";", 1)
|
||||
return header[1:].split(","), payload.split(",")
|
||||
|
||||
|
||||
def _parse_unicore_header(fields: list[str]) -> dict:
|
||||
if len(fields) < 9:
|
||||
raise ValueError("Unicore ASCII header is incomplete")
|
||||
return {
|
||||
"gnss_week": _safe_int(fields[4]),
|
||||
"gnss_tow_ms": _safe_int(fields[5]),
|
||||
"leap_seconds": _safe_int(fields[8]),
|
||||
}
|
||||
|
||||
|
||||
def parse_bestnava(line: str) -> dict:
|
||||
"""Parse BESTNAVA position and Doppler-velocity fields."""
|
||||
|
||||
header, fields = _split_unicore(line)
|
||||
if len(fields) < 30:
|
||||
raise ValueError("BESTNAVA has too few fields")
|
||||
result = {
|
||||
"type": "BESTNAVA",
|
||||
**_parse_unicore_header(header),
|
||||
"position_status": fields[0],
|
||||
"position_type": fields[1],
|
||||
"lat_deg": _safe_float(fields[2]),
|
||||
"lon_deg": _safe_float(fields[3]),
|
||||
"altitude_m": _safe_float(fields[4]),
|
||||
"undulation_m": _safe_float(fields[5]),
|
||||
"lat_std_m": _safe_float(fields[7]),
|
||||
"lon_std_m": _safe_float(fields[8]),
|
||||
"altitude_std_m": _safe_float(fields[9]),
|
||||
"station_id": fields[10].strip('"'),
|
||||
"differential_age_s": _safe_float(fields[11]),
|
||||
"solution_age_s": _safe_float(fields[12]),
|
||||
"satellites": _safe_int(fields[13]),
|
||||
"solution_satellites": _safe_int(fields[14]),
|
||||
"velocity_status": fields[21],
|
||||
"velocity_type": fields[22],
|
||||
"velocity_latency_s": _safe_float(fields[23]),
|
||||
"velocity_age_s": _safe_float(fields[24]),
|
||||
"horizontal_speed_m_s": _safe_float(fields[25]),
|
||||
"track_ground_deg": _safe_float(fields[26]),
|
||||
"vertical_speed_m_s": _safe_float(fields[27]),
|
||||
"vertical_speed_std_m_s": _safe_float(fields[28]),
|
||||
"horizontal_speed_std_m_s": _safe_float(fields[29]),
|
||||
}
|
||||
speed = result["horizontal_speed_m_s"]
|
||||
track = result["track_ground_deg"]
|
||||
if speed is not None and track is not None:
|
||||
angle = math.radians(track)
|
||||
result["velocity_east_m_s"] = speed * math.sin(angle)
|
||||
result["velocity_north_m_s"] = speed * math.cos(angle)
|
||||
else:
|
||||
result["velocity_east_m_s"] = None
|
||||
result["velocity_north_m_s"] = None
|
||||
result["position_fixed"] = (
|
||||
result["position_status"] == "SOL_COMPUTED"
|
||||
and result["position_type"] == "NARROW_INT"
|
||||
)
|
||||
result["doppler_velocity_valid"] = (
|
||||
result["velocity_status"] == "SOL_COMPUTED"
|
||||
and result["velocity_type"] == "DOPPLER_VELOCITY"
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
def parse_pvtslna(line: str) -> dict:
|
||||
"""Parse PVTSLNA as a quality-rich fallback/diagnostic record."""
|
||||
|
||||
header, fields = _split_unicore(line)
|
||||
if len(fields) < 34:
|
||||
raise ValueError("PVTSLNA has too few fields")
|
||||
speed_north = _safe_float(fields[17])
|
||||
speed_east = _safe_float(fields[18])
|
||||
return {
|
||||
"type": "PVTSLNA",
|
||||
**_parse_unicore_header(header),
|
||||
"position_type": fields[0],
|
||||
"altitude_m": _safe_float(fields[1]),
|
||||
"lat_deg": _safe_float(fields[2]),
|
||||
"lon_deg": _safe_float(fields[3]),
|
||||
"altitude_std_m": _safe_float(fields[4]),
|
||||
"lat_std_m": _safe_float(fields[5]),
|
||||
"lon_std_m": _safe_float(fields[6]),
|
||||
"differential_age_s": _safe_float(fields[7]),
|
||||
"psr_position_type": fields[8],
|
||||
"undulation_m": _safe_float(fields[12]),
|
||||
"satellites": _safe_int(fields[13]),
|
||||
"solution_satellites": _safe_int(fields[14]),
|
||||
"velocity_north_m_s": speed_north,
|
||||
"velocity_east_m_s": speed_east,
|
||||
"horizontal_speed_m_s": (
|
||||
None if speed_north is None or speed_east is None
|
||||
else math.hypot(speed_north, speed_east)
|
||||
),
|
||||
"vertical_speed_m_s": _safe_float(fields[19]),
|
||||
"heading_type": fields[20],
|
||||
"baseline_length_m": _safe_float(fields[21]),
|
||||
"heading_deg": _safe_float(fields[22]),
|
||||
"pitch_deg": _safe_float(fields[23]),
|
||||
"heading_satellites": _safe_int(fields[24]),
|
||||
"heading_solution_satellites": _safe_int(fields[25]),
|
||||
"gdop": _safe_float(fields[28]),
|
||||
"pdop": _safe_float(fields[29]),
|
||||
"hdop": _safe_float(fields[30]),
|
||||
"htdop": _safe_float(fields[31]),
|
||||
"tdop": _safe_float(fields[32]),
|
||||
"position_fixed": fields[0] == "NARROW_INT",
|
||||
}
|
||||
|
||||
|
||||
@@ -105,7 +252,7 @@ class RtkSentence:
|
||||
|
||||
|
||||
def iter_g90_sentences(capture: CaptureFile) -> list[RtkSentence]:
|
||||
"""Parse GGA/GNHPR lines; host time comes from the containing serial chunk."""
|
||||
"""Parse native asynchronous GGA/GNHPR/BESTNAVA/PVTSLNA records."""
|
||||
|
||||
rows: list[RtkSentence] = []
|
||||
for _segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
@@ -122,21 +269,27 @@ def iter_g90_sentences(capture: CaptureFile) -> list[RtkSentence]:
|
||||
if not raw_line:
|
||||
continue
|
||||
line = raw_line.decode("ascii", "replace")
|
||||
if not (line.startswith("$GNGGA") or line.startswith("$GPGGA") or line.startswith("$GNHPR")):
|
||||
parser = None
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
parser = parse_gga
|
||||
elif line.startswith("$GNHPR"):
|
||||
parser = parse_gnhpr
|
||||
elif line.startswith("#BESTNAVA"):
|
||||
parser = parse_bestnava
|
||||
elif line.startswith("#PVTSLNA"):
|
||||
parser = parse_pvtslna
|
||||
if parser is None:
|
||||
continue
|
||||
ticks = _host_ticks_for_span(chunks, starts, end)
|
||||
try:
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
fields = parse_gga(line)
|
||||
else:
|
||||
fields = parse_gnhpr(line)
|
||||
fields = parser(line)
|
||||
except ValueError:
|
||||
continue
|
||||
rows.append(
|
||||
RtkSentence(
|
||||
sentence_type=str(fields["type"]),
|
||||
receive_utc_ticks=int(ticks),
|
||||
checksum_valid=nmea_checksum_valid(line),
|
||||
checksum_valid=g90_checksum_valid(line),
|
||||
fields=fields,
|
||||
raw_line=line,
|
||||
)
|
||||
|
||||
+57
-18
@@ -12,6 +12,7 @@ HI91 (preferred for calibration):
|
||||
from __future__ import annotations
|
||||
|
||||
import struct
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
@@ -22,6 +23,29 @@ G0 = 9.80665
|
||||
DEG2RAD = np.pi / 180.0
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Hi13Sample:
|
||||
"""One CRC-valid native HI91 record.
|
||||
|
||||
t_s/system_time_ms are sensor-owned. Host receive ticks are retained only
|
||||
for clock diagnostics and cross-clock fitting.
|
||||
"""
|
||||
|
||||
t_s: float
|
||||
system_time_ms: int
|
||||
device_timestamp_us: int
|
||||
host_receive_utc_ticks: int
|
||||
gyro_rad_s: tuple[float, float, float]
|
||||
accel_m_s2: tuple[float, float, float]
|
||||
rpy_deg: tuple[float, float, float]
|
||||
quaternion_wxyz: tuple[float, float, float, float]
|
||||
mag_ut: tuple[float, float, float]
|
||||
pps_sync_stamp_ms: int
|
||||
temperature_c: int
|
||||
air_pressure_pa: float
|
||||
frame_tag: int = 0x91
|
||||
|
||||
|
||||
def crc16_hi13(frame: bytes, payload_length: int) -> int:
|
||||
crc = 0
|
||||
for value in frame[:4]:
|
||||
@@ -41,8 +65,8 @@ def _update_crc16(crc: int, value: int) -> int:
|
||||
return crc
|
||||
|
||||
|
||||
def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[float, float, float], int] | None:
|
||||
"""Return (gyro_rad_s, accel_m_s2, device_timestamp_ms) for a CRC-valid HI91 frame."""
|
||||
def parse_hi91_sample(raw: bytes, host_receive_utc_ticks: int = 0) -> Hi13Sample | None:
|
||||
"""Decode every calibration-relevant field from a CRC-valid HI91 frame."""
|
||||
|
||||
if len(raw) < 6 + 76:
|
||||
return None
|
||||
@@ -54,12 +78,34 @@ def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[floa
|
||||
expected = raw[4] | (raw[5] << 8)
|
||||
if crc16_hi13(raw, payload_length) != expected:
|
||||
return None
|
||||
device_ms = struct.unpack_from("<I", raw, 14)[0]
|
||||
device_ms = int(struct.unpack_from("<I", raw, 14)[0])
|
||||
ax, ay, az = struct.unpack_from("<fff", raw, 18)
|
||||
gx, gy, gz = struct.unpack_from("<fff", raw, 30)
|
||||
gyro = (gx * DEG2RAD, gy * DEG2RAD, gz * DEG2RAD)
|
||||
accel = (ax * G0, ay * G0, az * G0)
|
||||
return gyro, accel, int(device_ms)
|
||||
return Hi13Sample(
|
||||
t_s=float(device_ms) * 1e-3,
|
||||
system_time_ms=device_ms,
|
||||
device_timestamp_us=device_ms * 1000,
|
||||
host_receive_utc_ticks=int(host_receive_utc_ticks),
|
||||
gyro_rad_s=gyro,
|
||||
accel_m_s2=accel,
|
||||
rpy_deg=struct.unpack_from("<fff", raw, 54),
|
||||
quaternion_wxyz=struct.unpack_from("<ffff", raw, 66),
|
||||
mag_ut=struct.unpack_from("<fff", raw, 42),
|
||||
pps_sync_stamp_ms=int(struct.unpack_from("<H", raw, 7)[0]),
|
||||
temperature_c=int(struct.unpack_from("<b", raw, 9)[0]),
|
||||
air_pressure_pa=float(struct.unpack_from("<f", raw, 10)[0]),
|
||||
)
|
||||
|
||||
|
||||
def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[float, float, float], int] | None:
|
||||
"""Backward-compatible compact HI91 decoder."""
|
||||
|
||||
sample = parse_hi91_sample(raw)
|
||||
if sample is None:
|
||||
return None
|
||||
return sample.gyro_rad_s, sample.accel_m_s2, sample.system_time_ms
|
||||
|
||||
|
||||
def iter_hi13_imu_samples(
|
||||
@@ -67,14 +113,14 @@ def iter_hi13_imu_samples(
|
||||
*,
|
||||
host_utc_ticks_min: int | None = None,
|
||||
host_utc_ticks_max: int | None = None,
|
||||
) -> list[ImuSample]:
|
||||
) -> list[Hi13Sample]:
|
||||
"""Return CRC-valid HI91 samples sorted by device timestamp.
|
||||
|
||||
Streams chunk-by-chunk (no giant join) and can skip whole chunks outside the
|
||||
host UTC receive window before parsing.
|
||||
"""
|
||||
|
||||
samples: list[ImuSample] = []
|
||||
samples: list[Hi13Sample] = []
|
||||
carry = b""
|
||||
for chunk in capture.chunks:
|
||||
if host_utc_ticks_min is not None and chunk.receive_utc_ticks < host_utc_ticks_min:
|
||||
@@ -102,25 +148,16 @@ def iter_hi13_imu_samples(
|
||||
if end > len(stream):
|
||||
carry = stream[sync:]
|
||||
break
|
||||
parsed = parse_hi91_frame(stream[sync:end])
|
||||
host_ticks = chunk.receive_utc_ticks
|
||||
parsed = parse_hi91_sample(stream[sync:end], host_ticks)
|
||||
cursor = end
|
||||
if parsed is None:
|
||||
continue
|
||||
gyro, accel, device_ms = parsed
|
||||
host_ticks = chunk.receive_utc_ticks
|
||||
if host_utc_ticks_min is not None and host_ticks < host_utc_ticks_min:
|
||||
continue
|
||||
if host_utc_ticks_max is not None and host_ticks > host_utc_ticks_max:
|
||||
continue
|
||||
samples.append(
|
||||
ImuSample(
|
||||
t_s=float(device_ms) * 1e-3,
|
||||
gyro_rad_s=gyro,
|
||||
accel_m_s2=accel,
|
||||
host_receive_utc_ticks=host_ticks,
|
||||
device_timestamp_us=int(device_ms) * 1000,
|
||||
)
|
||||
)
|
||||
samples.append(parsed)
|
||||
else:
|
||||
carry = b""
|
||||
samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
|
||||
@@ -129,8 +166,10 @@ def iter_hi13_imu_samples(
|
||||
|
||||
__all__ = [
|
||||
"ImuSample",
|
||||
"Hi13Sample",
|
||||
"crc16_hi13",
|
||||
"iter_hi13_imu_samples",
|
||||
"parse_hi91_frame",
|
||||
"parse_hi91_sample",
|
||||
"samples_to_arrays",
|
||||
]
|
||||
|
||||
@@ -12,7 +12,13 @@ ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.rtk_imu_replay import load_inventory, load_sessions, run_calibration
|
||||
from imu_lidar.rtk_imu_replay import (
|
||||
DEFAULT_RTK_FRAME_DEFINITION,
|
||||
DEFAULT_RTK_REFERENCE_POINT,
|
||||
load_inventory,
|
||||
load_sessions,
|
||||
run_calibration,
|
||||
)
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
@@ -33,8 +39,8 @@ def main(argv: list[str] | None = None) -> int:
|
||||
parser.add_argument("--no-loo", action="store_true")
|
||||
parser.add_argument("--knot-step-s", type=float, default=2.0)
|
||||
parser.add_argument("--per-batch", action="store_true")
|
||||
parser.add_argument("--rtk-frame-definition", default='')
|
||||
parser.add_argument("--rtk-reference-point", default='')
|
||||
parser.add_argument("--rtk-frame-definition", default=DEFAULT_RTK_FRAME_DEFINITION)
|
||||
parser.add_argument("--rtk-reference-point", default=DEFAULT_RTK_REFERENCE_POINT)
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
entries = load_inventory(args.inventory)
|
||||
|
||||
@@ -0,0 +1,125 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run the conditional engineering RTK--IMU 6DoF lever-arm branch."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.rtk_imu_engineering import (
|
||||
DEFAULT_MANUAL_L_I_M,
|
||||
DEFAULT_MANUAL_L_I_COVARIANCE_M2,
|
||||
solve_engineering_6dof,
|
||||
)
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
|
||||
|
||||
def _jsonable(value):
|
||||
if isinstance(value, np.ndarray):
|
||||
return _jsonable(value.tolist())
|
||||
if isinstance(value, np.generic):
|
||||
return _jsonable(value.item())
|
||||
if isinstance(value, float):
|
||||
return value if math.isfinite(value) else None
|
||||
if hasattr(value, "__dataclass_fields__"):
|
||||
return {key: _jsonable(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): _jsonable(item) for key, item in value.items()}
|
||||
if isinstance(value, (list, tuple)):
|
||||
return [_jsonable(item) for item in value]
|
||||
return value
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument(
|
||||
"--session", action="append", required=True,
|
||||
help="Dynamic/static sessions intentionally admitted to this engineering run.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--rotation-rpy-deg", nargs=3, type=float,
|
||||
default=[0.4543066225, -0.0026392019, 0.0122384129],
|
||||
)
|
||||
parser.add_argument(
|
||||
"--manual-l-i-m", nargs=3, type=float, default=DEFAULT_MANUAL_L_I_M.tolist(),
|
||||
help="Mechanical ANT1 phase-centre lever arm p_ANT1^I in metres.",
|
||||
)
|
||||
prior_group = parser.add_mutually_exclusive_group()
|
||||
prior_group.add_argument(
|
||||
"--manual-l-i-std-m", nargs=3, type=float,
|
||||
default=np.sqrt(np.diag(DEFAULT_MANUAL_L_I_COVARIANCE_M2)).tolist(),
|
||||
help="Mechanical 1-sigma prior standard deviation for lx, ly, lz in metres.",
|
||||
)
|
||||
prior_group.add_argument(
|
||||
"--manual-l-i-covariance-m2", nargs=9, type=float,
|
||||
help="Row-major 3x3 mechanical prior covariance in m^2.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-manual-prior", action="store_true",
|
||||
help="Run a free solve only; retain the mechanical reference in no optimisation factor.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--segment-id", action="append",
|
||||
help="Strict-continuity segment id from the excitation audit; may be repeated.",
|
||||
)
|
||||
parser.add_argument("--sample-period-s", type=float, default=0.5)
|
||||
parser.add_argument("--skip-loo", action="store_true")
|
||||
parser.add_argument("--run-bootstrap", action="store_true")
|
||||
parser.add_argument("--bootstrap-repetitions", type=int, default=40)
|
||||
parser.add_argument("--bootstrap-seed", type=int, default=0)
|
||||
parser.add_argument("--run-rotation-sensitivity", action="store_true")
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
manual_l_i_m = None if args.no_manual_prior else args.manual_l_i_m
|
||||
prior_covariance = None
|
||||
if not args.no_manual_prior:
|
||||
if args.manual_l_i_std_m is not None:
|
||||
prior_covariance = np.diag(np.square(args.manual_l_i_std_m))
|
||||
elif args.manual_l_i_covariance_m2 is not None:
|
||||
prior_covariance = np.asarray(args.manual_l_i_covariance_m2, dtype=float).reshape(3, 3)
|
||||
else:
|
||||
parser.error("mechanical prior requires --manual-l-i-std-m or --manual-l-i-covariance-m2")
|
||||
|
||||
sessions = load_unified_sessions(args.manifest, selected_session_ids=set(args.session))
|
||||
rotation = Rotation.from_euler("xyz", args.rotation_rpy_deg, degrees=True).as_matrix()
|
||||
result = solve_engineering_6dof(
|
||||
sessions,
|
||||
R_RTK_IMU=rotation,
|
||||
manual_l_I_m=manual_l_i_m,
|
||||
manual_l_I_covariance_m2=prior_covariance,
|
||||
sample_period_s=args.sample_period_s,
|
||||
run_loo=not args.skip_loo,
|
||||
run_bootstrap=args.run_bootstrap,
|
||||
bootstrap_repetitions=args.bootstrap_repetitions,
|
||||
bootstrap_seed=args.bootstrap_seed,
|
||||
run_rotation_sensitivity=args.run_rotation_sensitivity,
|
||||
selected_segment_ids=None if args.segment_id is None else set(args.segment_id),
|
||||
)
|
||||
payload = {
|
||||
"data_only_6dof_accepted": False,
|
||||
"data_only_translation_accepted": result.data_only_translation_accepted,
|
||||
"engineering_6dof": _jsonable(result),
|
||||
}
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
args.output.write_text(
|
||||
json.dumps(payload, ensure_ascii=False, indent=2, allow_nan=False) + "\n", encoding="utf-8"
|
||||
)
|
||||
print(json.dumps(payload, ensure_ascii=False, indent=2, allow_nan=False))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,254 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run one prior-free engineering base fit on high-excitation qualified segments.
|
||||
|
||||
This diagnostic never adds a mechanical lever factor, never profiles the
|
||||
mechanical reference, and never runs LOO/bootstrap/rotation sensitivity.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.rtk_imu_engineering import (
|
||||
MIN_SEGMENT_DURATION_S,
|
||||
MIN_SEGMENT_NODE_COUNT,
|
||||
NUISANCE_DOF_PER_SEGMENT,
|
||||
_Segment,
|
||||
_fit_segments,
|
||||
_fit_summary,
|
||||
_height_reference,
|
||||
_initial_parameters,
|
||||
_marginal_lever_information,
|
||||
_nodes,
|
||||
_residual,
|
||||
_segment_residual_size,
|
||||
_world_rtk,
|
||||
)
|
||||
from imu_lidar.imu_preintegration import preintegrate_imu
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
|
||||
MANUAL_REFERENCE_L_I_M = np.array([-0.45072, -0.25682, 0.73208], dtype=float)
|
||||
|
||||
|
||||
def _jsonable(value):
|
||||
if isinstance(value, np.ndarray):
|
||||
return _jsonable(value.tolist())
|
||||
if isinstance(value, np.generic):
|
||||
return _jsonable(value.item())
|
||||
if isinstance(value, float):
|
||||
return value if math.isfinite(value) else None
|
||||
if hasattr(value, "__dataclass_fields__"):
|
||||
return {key: _jsonable(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): _jsonable(item) for key, item in value.items()}
|
||||
if isinstance(value, (tuple, list)):
|
||||
return [_jsonable(item) for item in value]
|
||||
return value
|
||||
|
||||
|
||||
def _r0_segment_candidates(sessions, period_s: float) -> list[tuple[object, tuple, str]]:
|
||||
"""Split R0 first; defer expensive preintegration until a candidate is selected."""
|
||||
reference = _height_reference(sessions)
|
||||
if reference is None:
|
||||
return []
|
||||
candidates: list[tuple[object, tuple, str]] = []
|
||||
for session in sessions:
|
||||
nodes = _nodes(session, reference, period_s)
|
||||
start = 0
|
||||
qualifying_index = 0
|
||||
for end in range(1, len(nodes) + 1):
|
||||
if end != len(nodes) and nodes[end].continuity_id == nodes[end - 1].continuity_id:
|
||||
continue
|
||||
run = tuple(nodes[start:end])
|
||||
start = end
|
||||
if len(run) < MIN_SEGMENT_NODE_COUNT or run[-1].t_s - run[0].t_s < MIN_SEGMENT_DURATION_S:
|
||||
continue
|
||||
if not any(node.hpr_factor_valid for node in run):
|
||||
continue
|
||||
candidates.append((session, run, f"{session.session_id}:{qualifying_index:02d}"))
|
||||
qualifying_index += 1
|
||||
return candidates
|
||||
|
||||
|
||||
def _build_segment(session, run: tuple, segment_id: str) -> _Segment | None:
|
||||
pre = tuple(preintegrate_imu(
|
||||
session.imu.t_s, session.imu.gyro_rad_s, session.imu.acc_m_s2, left.t_s, right.t_s,
|
||||
) for left, right in zip(run[:-1], run[1:]))
|
||||
if any(item.duration_s <= 0.0 for item in pre):
|
||||
return None
|
||||
initial_hpr = next((node for node in run if node.hpr_factor_valid), None)
|
||||
if initial_hpr is None:
|
||||
return None
|
||||
return _Segment(segment_id, session.session_id, run, pre, _world_rtk(initial_hpr.baseline_enu))
|
||||
|
||||
def _gyro_abs_rotation_deg(session, segment) -> np.ndarray:
|
||||
start_s, end_s = segment.nodes[0].t_s, segment.nodes[-1].t_s
|
||||
mask = (session.imu.t_s >= start_s) & (session.imu.t_s <= end_s)
|
||||
t_s = session.imu.t_s[mask]
|
||||
gyro = session.imu.gyro_rad_s[mask]
|
||||
if t_s.size < 2:
|
||||
return np.zeros(3)
|
||||
return np.degrees(np.trapezoid(np.abs(gyro), t_s, axis=0))
|
||||
|
||||
|
||||
def _category_score(category: str, gyro_abs_deg: np.ndarray) -> float:
|
||||
if category in {"circle", "left_right"}:
|
||||
return float(gyro_abs_deg[2])
|
||||
return float(np.hypot(gyro_abs_deg[0], gyro_abs_deg[1]))
|
||||
|
||||
|
||||
def _marginal_for_segment_indices(jacobian: np.ndarray, residual: np.ndarray,
|
||||
segments, indices: list[int]) -> tuple[np.ndarray, np.ndarray, float, int, np.ndarray]:
|
||||
row = 0
|
||||
rows: list[np.ndarray] = []
|
||||
for index, segment in enumerate(segments):
|
||||
count = _segment_residual_size(segment)
|
||||
if index in indices:
|
||||
rows.append(np.arange(row, row + count))
|
||||
row += count
|
||||
selected_rows = np.concatenate(rows) if rows else np.empty(0, dtype=int)
|
||||
columns = [0, 1, 2]
|
||||
for index in indices:
|
||||
offset = 3 + NUISANCE_DOF_PER_SEGMENT * index
|
||||
columns.extend(range(offset, offset + NUISANCE_DOF_PER_SEGMENT))
|
||||
marginal, singular, condition, rank, weakest, _ = _marginal_lever_information(
|
||||
jacobian[np.ix_(selected_rows, columns)], residual[selected_rows]
|
||||
)
|
||||
return marginal, singular, condition, rank, weakest
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--circle-session", required=True)
|
||||
parser.add_argument("--left-right-session", required=True)
|
||||
parser.add_argument("--slope-session", required=True)
|
||||
parser.add_argument("--top-per-category", type=int, default=10)
|
||||
parser.add_argument("--sample-period-s", type=float, default=1.0)
|
||||
parser.add_argument("--rotation-rpy-deg", nargs=3, type=float,
|
||||
default=[0.4543066225, -0.0026392019, 0.0122384129])
|
||||
args = parser.parse_args()
|
||||
category_by_session = {
|
||||
args.circle_session: "circle",
|
||||
args.left_right_session: "left_right",
|
||||
args.slope_session: "slope",
|
||||
}
|
||||
sessions = load_unified_sessions(args.manifest, selected_session_ids=set(category_by_session))
|
||||
candidates: dict[str, list[tuple[float, object, tuple, str, np.ndarray]]] = {
|
||||
key: [] for key in category_by_session.values()
|
||||
}
|
||||
all_candidates = _r0_segment_candidates(sessions, args.sample_period_s)
|
||||
for session, run, segment_id in all_candidates:
|
||||
category = category_by_session[session.session_id]
|
||||
gyro_abs = _gyro_abs_rotation_deg(session, type("Run", (), {"nodes": run})())
|
||||
candidates[category].append((_category_score(category, gyro_abs), session, run, segment_id, gyro_abs))
|
||||
selected: list[object] = []
|
||||
selected_entries: list[dict[str, object]] = []
|
||||
category_indices: dict[str, list[int]] = {}
|
||||
for category, entries in candidates.items():
|
||||
selected_before = len(selected)
|
||||
for score, session, run, segment_id, gyro_abs in sorted(entries, key=lambda item: item[0], reverse=True):
|
||||
segment = _build_segment(session, run, segment_id)
|
||||
if segment is None:
|
||||
continue
|
||||
selected.append(segment)
|
||||
selected_entries.append({
|
||||
"category": category, "segment_id": segment.segment_id,
|
||||
"session_id": segment.session_id, "duration_s": segment.nodes[-1].t_s - segment.nodes[0].t_s,
|
||||
"node_count": len(segment.nodes), "excitation_score_deg": score,
|
||||
"cumulative_absolute_gyro_rotation_xyz_deg": gyro_abs,
|
||||
})
|
||||
if len(selected) - selected_before >= args.top_per_category:
|
||||
break
|
||||
category_indices[category] = list(range(selected_before, len(selected)))
|
||||
rotation = Rotation.from_euler("xyz", args.rotation_rpy_deg, degrees=True).as_matrix()
|
||||
initial_parameters = _initial_parameters(selected)
|
||||
initial_residual = _residual(initial_parameters, selected, rotation)
|
||||
fit, residual, detail = _fit_segments(selected, rotation)
|
||||
summary = _fit_summary(fit, residual, detail)
|
||||
if fit is None or summary is None:
|
||||
raise RuntimeError("no selected qualified segment could be fit")
|
||||
contributions: dict[str, dict[str, object]] = {}
|
||||
contribution_sum = np.zeros((3, 3))
|
||||
for category, indices in category_indices.items():
|
||||
marginal, singular, condition, rank, weakest = _marginal_for_segment_indices(
|
||||
fit.jac, residual, selected, indices
|
||||
)
|
||||
contribution_sum += marginal
|
||||
contributions[category] = {
|
||||
"selected_segment_count": len(indices),
|
||||
"lever_marginal_information": marginal,
|
||||
"lever_information_singular_values": singular,
|
||||
"condition_number": condition,
|
||||
"precision_rank": rank,
|
||||
"weakest_direction_I": weakest,
|
||||
"axis_information_diagonal_I": np.diag(marginal),
|
||||
}
|
||||
total_diag = np.diag(summary.lever_marginal_information)
|
||||
for category, item in contributions.items():
|
||||
item["axis_information_fraction_of_total_I"] = np.divide(
|
||||
item["axis_information_diagonal_I"], total_diag,
|
||||
out=np.full(3, np.nan), where=np.abs(total_diag) > 1e-12,
|
||||
)
|
||||
payload = {
|
||||
'solver_diagnostics': {
|
||||
'initial_cost': 0.5 * float(np.dot(initial_residual, initial_residual)),
|
||||
'final_cost': 0.5 * float(np.dot(residual, residual)),
|
||||
'cost_reduction': 0.5 * float(
|
||||
np.dot(initial_residual, initial_residual) - np.dot(residual, residual)
|
||||
),
|
||||
'cost_definition': '0.5 * unmodified residual squared norm, comparable initial/final',
|
||||
'scipy_final_huber_cost': float(fit.cost),
|
||||
'nfev': int(fit.nfev), 'optimality': float(fit.optimality),
|
||||
'gradient_norm': float(np.linalg.norm(fit.grad)),
|
||||
'initial_l_I_m': initial_parameters[:3], 'final_l_I_m': fit.x[:3],
|
||||
'l_step_norm_m': float(np.linalg.norm(fit.x[:3] - initial_parameters[:3])),
|
||||
},
|
||||
"scope": "prior-free free base fit only; no mechanical factor/LOO/bootstrap/rotation sensitivity",
|
||||
"rotation_source": "R2G_gravity_level_prior",
|
||||
"translation_conditional_on_rotation": True,
|
||||
"manual_reference_comparison_only": {
|
||||
"manual_l_I_m": MANUAL_REFERENCE_L_I_M,
|
||||
"free_minus_manual_l_I_m": summary.l_I_m - MANUAL_REFERENCE_L_I_M,
|
||||
"euclidean_delta_m": float(np.linalg.norm(summary.l_I_m - MANUAL_REFERENCE_L_I_M)),
|
||||
},
|
||||
"sample_period_s": args.sample_period_s,
|
||||
"all_qualified_segment_count": len(all_candidates),
|
||||
"selected_segment_count": len(selected),
|
||||
"selected_segments": selected_entries,
|
||||
"free_solution": _jsonable(summary),
|
||||
"motion_category_information_contributions": _jsonable(contributions),
|
||||
"axis_information_total_I": np.diag(summary.lever_marginal_information),
|
||||
"marginal_additivity_error_fro": float(np.linalg.norm(
|
||||
contribution_sum - summary.lever_marginal_information, ord="fro"
|
||||
)),
|
||||
}
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload), ensure_ascii=False, indent=2, allow_nan=False) + "\n", encoding="utf-8")
|
||||
print(json.dumps({
|
||||
"selected_segment_count": len(selected),
|
||||
"free_l_I_m": _jsonable(summary.l_I_m),
|
||||
"free_l_I_std_m": _jsonable(summary.l_I_std_m),
|
||||
"lever_information_singular_values": _jsonable(summary.lever_information_singular_values),
|
||||
"condition_number": summary.lever_information_condition_number,
|
||||
"precision_rank": summary.lever_precision_rank,
|
||||
"bestnava_xyz_vector_rms_p95_m": [summary.bestnava_xyz_residual.vector_rms, summary.bestnava_xyz_residual.vector_p95],
|
||||
"doppler_vector_rms_p95_m_s": [summary.doppler_velocity_residual.vector_rms, summary.doppler_velocity_residual.vector_p95],
|
||||
}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,144 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Run fixed-mechanical and soft-prior solutions on immutable 47-window baseline.'''
|
||||
from __future__ import annotations
|
||||
import argparse,hashlib,json,sys
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import (
|
||||
NODE_DOF,build_problem,fit_states_at_fixed_lever,solve_free_lever_many,
|
||||
summarize_fixed_state_values)
|
||||
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
|
||||
MANUAL_STD_M=np.array([.02,.02,.03])
|
||||
MANUAL_COVARIANCE_M2=np.diag(MANUAL_STD_M**2)
|
||||
|
||||
|
||||
def _factor_delta(candidate,baseline):
|
||||
output={}
|
||||
for key in ('best_position','doppler','hpr','imu_preintegration'):
|
||||
left,right=candidate['residual_by_factor'][key],baseline['residual_by_factor'][key]
|
||||
output[key]={'rms_delta':left['rms']-right['rms'],
|
||||
'p95_abs_delta':left['p95_abs']-right['p95_abs'],
|
||||
'nis_per_dof_delta':left['chi_square_per_dof']-right['chi_square_per_dof']}
|
||||
return output
|
||||
|
||||
|
||||
def main():
|
||||
parser=argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--selection',type=Path,required=True)
|
||||
parser.add_argument('--free-baseline',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--state-output',type=Path)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--fixed-max-nfev',type=int,default=120)
|
||||
parser.add_argument('--prior-max-nfev',type=int,default=120)
|
||||
parser.add_argument('--workers',type=int,default=4)
|
||||
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
args=parser.parse_args()
|
||||
selection=json.loads(args.selection.read_text(encoding='utf-8'))
|
||||
baseline=json.loads(args.free_baseline.read_text(encoding='utf-8'))
|
||||
if len(selection['selected_windows'])!=47:
|
||||
raise RuntimeError('engineering branch requires immutable 47-window selection')
|
||||
ids={item['session_id'] for item in selection['selected_windows']}
|
||||
sessions=load_unified_sessions(args.manifest,selected_session_ids=ids)
|
||||
reference=_height_reference(sessions)
|
||||
segments=_restore_segments(sessions,reference,selection['selected_windows'],
|
||||
args.sample_period_s)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
mechanical=MECHANICAL_L_I_M.copy()
|
||||
problems=[build_problem(segment,rotation,mechanical,args.hpr_direct_sigma_rad)
|
||||
for segment in segments]
|
||||
with ThreadPoolExecutor(max_workers=args.workers) as executor:
|
||||
fitted=list(executor.map(lambda problem:fit_states_at_fixed_lever(
|
||||
problem,mechanical,args.fixed_max_nfev),problems))
|
||||
fixed_states=[item[0] for item in fitted]
|
||||
fixed_optimizer=[item[1] for item in fitted]
|
||||
fixed_summary=summarize_fixed_state_values(problems,fixed_states,mechanical)
|
||||
prior_result,prior_states=solve_free_lever_many(
|
||||
problems,mechanical,args.prior_max_nfev,fixed_states,
|
||||
lever_prior_mean_m=mechanical,
|
||||
lever_prior_covariance_m2=MANUAL_COVARIANCE_M2,
|
||||
return_state_values=True)
|
||||
prior_result=asdict(prior_result)
|
||||
prior_data=summarize_fixed_state_values(
|
||||
problems,prior_states,np.asarray(prior_result['final_l_I_m']))
|
||||
bias_values={}
|
||||
for segment,state in zip(segments,prior_states):
|
||||
states=np.asarray(state).reshape(-1,NODE_DOF)
|
||||
values=bias_values.setdefault(segment.session_id,{'bg':[],'ba':[]})
|
||||
values['bg'].extend(states[:,9:12])
|
||||
values['ba'].extend(states[:,12:15])
|
||||
calibration_bias={session_id:{
|
||||
'gyro_bias_rad_s':np.median(values['bg'],axis=0),
|
||||
'accel_bias_m_s2':np.median(values['ba'],axis=0),
|
||||
'node_count':len(values['bg'])}
|
||||
for session_id,values in bias_values.items()}
|
||||
free=baseline['solutions']['mechanical']
|
||||
posterior_cov=np.asarray(prior_result['lever_covariance_m2'])
|
||||
variance_ratio=np.diag(posterior_cov)/np.diag(MANUAL_COVARIANCE_M2)
|
||||
prior_pull=(np.asarray(prior_result['final_l_I_m'])-mechanical)/MANUAL_STD_M
|
||||
translation_refined=bool(np.all(variance_ratio<=.90))
|
||||
comparisons={'fixed_vs_free':{
|
||||
'cost_delta':fixed_summary['cost']-free['final_cost'],
|
||||
'relative_cost_delta':fixed_summary['cost']/free['final_cost']-1.,
|
||||
'factor_residual_delta':_factor_delta(fixed_summary,free)},
|
||||
'prior_data_vs_free':{
|
||||
'cost_delta':prior_data['cost']-free['final_cost'],
|
||||
'relative_cost_delta':prior_data['cost']/free['final_cost']-1.,
|
||||
'factor_residual_delta':_factor_delta(prior_data,free)}}
|
||||
baseline_hash=hashlib.sha256(args.free_baseline.read_bytes()).hexdigest()
|
||||
payload={'scope':'47-window mechanical-prior engineering branch',
|
||||
'data_only_translation_accepted':False,
|
||||
'immutable_free_baseline':{'path':str(args.free_baseline),
|
||||
'sha256':baseline_hash,'solution':free},
|
||||
'selected_window_count':len(segments),'selection_path':str(args.selection),
|
||||
'manual_l_I_m':mechanical,'manual_l_I_std_m':MANUAL_STD_M,
|
||||
'manual_l_I_covariance_m2':MANUAL_COVARIANCE_M2,
|
||||
'fixed_mechanical_solution':{'l_I_m':mechanical,
|
||||
'window_optimizer':fixed_optimizer,'data_summary':fixed_summary},
|
||||
'prior_constrained_solution':{'result':prior_result,
|
||||
'data_only_summary_excluding_prior_factor':prior_data,
|
||||
'posterior_covariance_m2':posterior_cov,
|
||||
'posterior_prior_variance_ratio':variance_ratio,
|
||||
'prior_pull_sigma':prior_pull,
|
||||
'calibration_only_frozen_bias_by_session':calibration_bias},
|
||||
'comparisons':comparisons,
|
||||
'translation_refinement_gate':{
|
||||
'required_max_axis_variance_ratio':.90,
|
||||
'passed':translation_refined},
|
||||
'translation_refined_by_data':translation_refined,
|
||||
'heldout_validation_called':False,
|
||||
'engineering_translation_accepted':False}
|
||||
args.output.parent.mkdir(parents=True,exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
if args.state_output is not None:
|
||||
sizes=np.asarray([len(value) for value in prior_states],dtype=int)
|
||||
args.state_output.parent.mkdir(parents=True,exist_ok=True)
|
||||
np.savez_compressed(args.state_output,
|
||||
states=np.concatenate(prior_states),offsets=np.cumsum(np.r_[0,sizes]),
|
||||
candidate_ids=np.asarray(
|
||||
[item['candidate_id'] for item in selection['selected_windows']]))
|
||||
print(json.dumps(_jsonable({'fixed':fixed_summary,
|
||||
'prior_l_I_m':prior_result['final_l_I_m'],
|
||||
'prior_data_cost':prior_data['cost'],'prior_map_cost':prior_result['final_cost'],
|
||||
'variance_ratio':variance_ratio,'prior_pull_sigma':prior_pull,
|
||||
'translation_refined_by_data':translation_refined,
|
||||
'fixed_optimizer_failed_count':sum(not item['success'] for item in fixed_optimizer)}),
|
||||
ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,212 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Validate an engineering lever on disjoint held-out windows.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from concurrent.futures import ProcessPoolExecutor,as_completed
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.geometry import make_transform
|
||||
from imu_lidar.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem,fit_states_at_fixed_lever,residual
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
|
||||
FACTORS=('best_position','doppler','hpr','imu_preintegration')
|
||||
PHYSICAL=('best_position_physical','doppler_physical','hpr_physical')
|
||||
|
||||
def _fit_checkpoint(task):
|
||||
index,problem,lever,max_nfev,path_text=task
|
||||
path=Path(path_text)
|
||||
if path.exists():
|
||||
data=np.load(path,allow_pickle=False)
|
||||
return index,data['state'],json.loads(str(data['optimizer']))
|
||||
state,meta=fit_states_at_fixed_lever(problem,lever,max_nfev)
|
||||
np.savez_compressed(path,state=state,optimizer=json.dumps(meta))
|
||||
return index,state,meta
|
||||
|
||||
def _stats(values,dof=None):
|
||||
a=np.asarray(values,dtype=float).reshape(-1)
|
||||
if not a.size:
|
||||
return {'count':0,'dof':0,'rms':np.nan,'p50_abs':np.nan,
|
||||
'p95_abs':np.nan,'p99_abs':np.nan,'nis':np.nan,
|
||||
'chi_square_per_dof':np.nan}
|
||||
dof=len(a) if dof is None else max(int(dof),1); nis=float(a@a)
|
||||
return {'count':len(a),'dof':dof,'rms':float(np.sqrt(np.mean(a*a))),
|
||||
'p50_abs':float(np.percentile(np.abs(a),50)),
|
||||
'p95_abs':float(np.percentile(np.abs(a),95)),
|
||||
'p99_abs':float(np.percentile(np.abs(a),99)),
|
||||
'nis':nis,'chi_square_per_dof':nis/dof}
|
||||
|
||||
def _vectors(values):
|
||||
a=np.asarray(values,dtype=float).reshape(-1,3)
|
||||
if not a.size:
|
||||
return {'count':0,'axis_rms':[np.nan]*3,'axis_p95_abs':[np.nan]*3,
|
||||
'vector_rms':np.nan,'vector_p95':np.nan}
|
||||
norm=np.linalg.norm(a,axis=1)
|
||||
return {'count':len(a),'axis_rms':np.sqrt(np.mean(a*a,axis=0)),
|
||||
'axis_p95_abs':np.percentile(np.abs(a),95,axis=0),
|
||||
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
|
||||
'vector_p95':float(np.percentile(norm,95))}
|
||||
|
||||
def _summarize(records):
|
||||
factor={key:[] for key in FACTORS}; physical={key:[] for key in PHYSICAL}
|
||||
joined=[]; state_dimension=0; other_effective=0
|
||||
for record in records:
|
||||
joined.extend(record['all']); state_dimension+=record['state_dimension']
|
||||
other_effective+=record['other_effective']
|
||||
for key in FACTORS: factor[key].extend(record['factor'].get(key,[]))
|
||||
for key in PHYSICAL: physical[key].extend(record['physical'].get(key,[]))
|
||||
effective=sum(2*len(v)//3 if k=='hpr' else len(v)
|
||||
for k,v in factor.items())+other_effective
|
||||
dof=max(effective-state_dimension,1); a=np.asarray(joined,dtype=float)
|
||||
converged=sum(bool(x['optimizer']['success']) for x in records)
|
||||
return {'window_count':len(records),'optimizer_converged_count':converged,
|
||||
'optimizer_converged_fraction':converged/max(len(records),1),
|
||||
'total_residual_dimension':len(a),'state_dimension':state_dimension,
|
||||
'statistical_dof':dof,'total_nis':float(a@a),
|
||||
'global_chi_square_per_dof':float((a@a)/dof),
|
||||
'residual_by_factor':{key:_stats(value,2*len(value)//3 if key=='hpr' else None)
|
||||
for key,value in factor.items()},
|
||||
'best_position_physical_m':_vectors(physical['best_position_physical']),
|
||||
'doppler_physical_m_s':_vectors(physical['doppler_physical']),
|
||||
'hpr_physical_rad':_vectors(physical['hpr_physical'])}
|
||||
|
||||
def _gate(summary):
|
||||
factor=summary['residual_by_factor']
|
||||
checks={'optimizer_converged_fraction_ge_0p90':
|
||||
summary['optimizer_converged_fraction']>=.90,
|
||||
'global_chi_square_per_dof_in_0p25_4':
|
||||
.25<=summary['global_chi_square_per_dof']<=4.,
|
||||
'best_vector_p95_le_0p20_m':
|
||||
summary['best_position_physical_m']['vector_p95']<=.20,
|
||||
'doppler_vector_p95_le_0p50_m_s':
|
||||
summary['doppler_physical_m_s']['vector_p95']<=.50,
|
||||
'hpr_normalized_p95_le_4':factor['hpr']['p95_abs']<=4.,
|
||||
'preintegration_normalized_p95_le_3':
|
||||
factor['imu_preintegration']['p95_abs']<=3.}
|
||||
return {'uses_existing_P0p5_physical_and_statistical_health_gates':True,
|
||||
'checks':checks,'passed':bool(all(checks.values()))}
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--manifest',type=Path,required=True)
|
||||
p.add_argument('--calibration-selection',type=Path,required=True)
|
||||
p.add_argument('--all-selection',type=Path,required=True)
|
||||
p.add_argument('--engineering-result',type=Path,required=True)
|
||||
p.add_argument('--output',type=Path,required=True)
|
||||
p.add_argument('--checkpoint-dir',type=Path,required=True)
|
||||
p.add_argument('--sample-period-s',type=float,default=1.)
|
||||
p.add_argument('--max-nfev',type=int,default=120)
|
||||
p.add_argument('--workers',type=int,default=4)
|
||||
p.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
p.add_argument('--circle-session',default='0808_20260808_092827')
|
||||
p.add_argument('--left-right-session',default='0808_20260808_082148')
|
||||
p.add_argument('--slope-session',default='0815_20260812_123424')
|
||||
args=p.parse_args()
|
||||
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
|
||||
calibration=json.loads(args.calibration_selection.read_text(encoding='utf-8'))
|
||||
all_windows=json.loads(args.all_selection.read_text(encoding='utf-8'))
|
||||
calibration_ids={x['candidate_id'] for x in calibration['selected_windows']}
|
||||
heldout=[x for x in all_windows['selected_windows']
|
||||
if x['candidate_id'] not in calibration_ids]
|
||||
if len(calibration_ids)!=47 or len(heldout)!=267:
|
||||
raise RuntimeError(f'expected 47+267 windows, got {len(calibration_ids)}+{len(heldout)}')
|
||||
shared=sum(x.get('shared_sample_count_with_previous',{}).get(k,0)
|
||||
for x in all_windows['selected_windows'] for k in ('imu','gnss','hpr'))
|
||||
if shared: raise RuntimeError(f'selection contains {shared} shared samples')
|
||||
lever=np.asarray(engineering['prior_constrained_solution']['result']['final_l_I_m'])
|
||||
sessions=load_unified_sessions(
|
||||
args.manifest,selected_session_ids={x['session_id'] for x in heldout})
|
||||
reference=_height_reference(sessions)
|
||||
segments=_restore_segments(sessions,reference,heldout,args.sample_period_s)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
problems=[build_problem(x,rotation,lever,args.hpr_direct_sigma_rad) for x in segments]
|
||||
args.checkpoint_dir.mkdir(parents=True,exist_ok=True)
|
||||
fitted=[None]*len(problems)
|
||||
tasks=[(i,problem,lever,args.max_nfev,
|
||||
str(args.checkpoint_dir/f'{i:04d}.npz'))
|
||||
for i,problem in enumerate(problems)]
|
||||
with ProcessPoolExecutor(max_workers=args.workers) as executor:
|
||||
futures=[executor.submit(_fit_checkpoint,task) for task in tasks]
|
||||
completed=0
|
||||
for future in as_completed(futures):
|
||||
index,state,meta=future.result(); fitted[index]=(state,meta); completed+=1
|
||||
if completed%10==0: print(f'held-out checkpoint {completed}/{len(problems)}',flush=True)
|
||||
mapping={args.circle_session:'circle',args.left_right_session:'left_right',
|
||||
args.slope_session:'slope'}
|
||||
records=[]
|
||||
for item,problem,fit in zip(heldout,problems,fitted):
|
||||
state,optimizer=fit; detail={}
|
||||
all_residual=residual(problem,state,details=detail,lever_override=lever)
|
||||
known=sum(len(detail.get(k,[])) for k in FACTORS)
|
||||
records.append({'candidate_id':item['candidate_id'],
|
||||
'session_id':item['session_id'],
|
||||
'motion_class':mapping.get(item['session_id'],'other_recovered_dynamic'),
|
||||
'start_s':item['start_s'],'end_s':item['end_s'],
|
||||
'state_dimension':len(state),'optimizer':optimizer,
|
||||
'all':all_residual.tolist(),'other_effective':len(all_residual)-known,
|
||||
'factor':{k:detail.get(k,[]) for k in FACTORS},
|
||||
'physical':{k:detail.get(k,[]) for k in PHYSICAL}})
|
||||
overall=_summarize(records)
|
||||
by_session={key:_summarize([x for x in records if x['session_id']==key])
|
||||
for key in sorted({x['session_id'] for x in records})}
|
||||
by_motion={key:_summarize([x for x in records if x['motion_class']==key])
|
||||
for key in sorted({x['motion_class'] for x in records})}
|
||||
overall_gate=_gate(overall)
|
||||
session_gates={k:_gate(v) for k,v in by_session.items()}
|
||||
motion_gates={k:_gate(v) for k,v in by_motion.items()}
|
||||
heldout_passed=bool(overall_gate['passed'] and
|
||||
all(x['passed'] for x in session_gates.values()) and
|
||||
all(x['passed'] for x in motion_gates.values()))
|
||||
comparisons=engineering['comparisons']
|
||||
def nonconflicting(name):
|
||||
c=comparisons[name]
|
||||
return (c['relative_cost_delta']<=.05 and
|
||||
all(x['p95_abs_delta']<=.25 for x in c['factor_residual_delta'].values()))
|
||||
fit_nonconflict=nonconflicting('fixed_vs_free') and nonconflicting('prior_data_vs_free')
|
||||
T_rtk_imu=make_transform(-rotation@lever,rotation)
|
||||
T_imu_rtk=np.linalg.inv(T_rtk_imu)
|
||||
accepted=bool(fit_nonconflict and heldout_passed)
|
||||
payload={**engineering,
|
||||
'scope':'47-window mechanical-prior engineering branch + disjoint held-out validation',
|
||||
'heldout_validation_called':True,
|
||||
'engineering_lever_source':'prior_constrained_solution',
|
||||
'engineering_l_I_m':lever,'calibration_heldout_overlap_count':0,
|
||||
'heldout_window_count':len(heldout),
|
||||
'heldout_validation':{'lever_reoptimized':False,
|
||||
'nuisance_states_optimized_per_window':True,
|
||||
'motion_class_mapping':mapping,
|
||||
'other_sessions_class':'other_recovered_dynamic',
|
||||
'overall':overall,'by_session':by_session,'by_motion_class':by_motion,
|
||||
'overall_gate':overall_gate,'session_gates':session_gates,
|
||||
'motion_class_gates':motion_gates,'passed':heldout_passed},
|
||||
'calibration_fit_nonconflict_gate':{'relative_cost_increase_max':.05,
|
||||
'per_factor_normalized_p95_increase_max':.25,
|
||||
'fixed_passed':nonconflicting('fixed_vs_free'),
|
||||
'prior_passed':nonconflicting('prior_data_vs_free'),
|
||||
'passed':fit_nonconflict},
|
||||
'engineering_translation_accepted':accepted,
|
||||
'data_only_translation_accepted':False,
|
||||
'result_nature':'mechanical lever + dynamic-data consistency validation; not data-only translation calibration',
|
||||
'rotation_source':'R2G_gravity_level_prior',
|
||||
'translation_conditional_on_rotation':True,
|
||||
'candidate_T_RTK_IMU':T_rtk_imu,'candidate_T_IMU_RTK':T_imu_rtk,
|
||||
'T_RTK_IMU':T_rtk_imu if accepted else None,
|
||||
'T_IMU_RTK':T_imu_rtk if accepted else None,
|
||||
'transform_convention':('T_RTK_IMU maps IMU coordinates into ANT1 RTK frame; '
|
||||
'l_I=p_ANT1^I; T_IMU_RTK translation equals l_I')}
|
||||
args.output.parent.mkdir(parents=True,exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable({'engineering_l_I_m':lever,
|
||||
'fit_nonconflict':fit_nonconflict,'heldout_overall':overall,
|
||||
'heldout_passed':heldout_passed,
|
||||
'engineering_translation_accepted':accepted}),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
if __name__=='__main__': raise SystemExit(main())
|
||||
@@ -0,0 +1,153 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Run 18 fixed-R2G perturbation prior-constrained node-graph solves.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from concurrent.futures import ProcessPoolExecutor,as_completed
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.geometry import make_transform
|
||||
from imu_lidar.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import (
|
||||
build_problem,solve_free_lever_many,summarize_fixed_state_values)
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
|
||||
_CONTEXT={}
|
||||
MANUAL=np.array([-.45072,-.25682,.73208])
|
||||
STD=np.array([.02,.02,.03])
|
||||
COV=np.diag(STD**2)
|
||||
|
||||
def _initialize_worker(segments,states,rpy,max_nfev,sigma,checkpoint_dir):
|
||||
_CONTEXT.update(segments=segments,states=states,rpy=np.asarray(rpy),
|
||||
max_nfev=max_nfev,sigma=sigma,checkpoint_dir=Path(checkpoint_dir))
|
||||
|
||||
def _solve(task):
|
||||
axis,sign,angle=task
|
||||
name=f'{axis}_{sign*angle:+.1f}deg'
|
||||
path=_CONTEXT['checkpoint_dir']/f'{name}.json'
|
||||
if path.exists(): return json.loads(path.read_text(encoding='utf-8'))
|
||||
vector=np.zeros(3); vector['xyz'.index(axis)]=np.deg2rad(sign*angle)
|
||||
nominal_R=Rotation.from_euler('xyz',_CONTEXT['rpy'],degrees=True).as_matrix()
|
||||
perturbed_R=Rotation.from_rotvec(vector).as_matrix()@nominal_R
|
||||
problems=[build_problem(segment,perturbed_R,MANUAL,_CONTEXT['sigma'])
|
||||
for segment in _CONTEXT['segments']]
|
||||
result,states=solve_free_lever_many(problems,MANUAL,_CONTEXT['max_nfev'],
|
||||
_CONTEXT['states'],lever_prior_mean_m=MANUAL,
|
||||
lever_prior_covariance_m2=COV,return_state_values=True)
|
||||
result=asdict(result); lever=np.asarray(result['final_l_I_m'])
|
||||
data=summarize_fixed_state_values(problems,states,lever)
|
||||
transform=make_transform(-perturbed_R@lever,perturbed_R)
|
||||
payload={'name':name,'axis':axis,'signed_angle_deg':sign*angle,
|
||||
'success':result['success'],'message':result['message'],'nfev':result['nfev'],
|
||||
'final_l_I_m':lever,'delta_l_from_nominal_m':None,
|
||||
'map_cost':result['final_cost'],'data_cost':data['cost'],
|
||||
'global_chi_square_per_dof':data['chi_square_per_dof'],
|
||||
'residual_by_factor':data['residual_by_factor'],
|
||||
'physical_residual':{
|
||||
'BEST_position_m':data['best_position_physical_m'],
|
||||
'Doppler_m_s':data['doppler_physical_m_s'],
|
||||
'HPR_rad':data['hpr_physical_rad']},
|
||||
'posterior_covariance_m2':result['lever_covariance_m2'],
|
||||
'posterior_std_m':np.sqrt(np.diag(result['lever_covariance_m2'])),
|
||||
'prior_pull_sigma':(lever-MANUAL)/STD,
|
||||
'T_RTK_IMU':transform}
|
||||
path.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
return _jsonable(payload)
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--manifest',type=Path,required=True)
|
||||
p.add_argument('--selection',type=Path,required=True)
|
||||
p.add_argument('--engineering-result',type=Path,required=True)
|
||||
p.add_argument('--nominal-states',type=Path,required=True)
|
||||
p.add_argument('--output',type=Path,required=True)
|
||||
p.add_argument('--checkpoint-dir',type=Path,required=True)
|
||||
p.add_argument('--workers',type=int,default=3)
|
||||
p.add_argument('--max-nfev',type=int,default=120)
|
||||
p.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
args=p.parse_args()
|
||||
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
|
||||
selection=json.loads(args.selection.read_text(encoding='utf-8'))
|
||||
if len(selection['selected_windows'])!=47:
|
||||
raise RuntimeError('sensitivity requires immutable 47-window selection')
|
||||
sessions=load_unified_sessions(args.manifest,
|
||||
selected_session_ids={x['session_id'] for x in selection['selected_windows']})
|
||||
segments=_restore_segments(sessions,_height_reference(sessions),
|
||||
selection['selected_windows'],1.)
|
||||
saved=np.load(args.nominal_states,allow_pickle=False)
|
||||
offsets=saved['offsets']; flat=saved['states']
|
||||
states=[flat[offsets[i]:offsets[i+1]] for i in range(len(offsets)-1)]
|
||||
if list(saved['candidate_ids'])!=[x['candidate_id'] for x in selection['selected_windows']]:
|
||||
raise RuntimeError('nominal state checkpoint does not match selection order')
|
||||
args.checkpoint_dir.mkdir(parents=True,exist_ok=True)
|
||||
tasks=[(axis,sign,angle) for axis in 'xyz'
|
||||
for angle in (.1,.3,.5) for sign in (-1.,1.)]
|
||||
results=[]
|
||||
with ProcessPoolExecutor(max_workers=args.workers,initializer=_initialize_worker,
|
||||
initargs=(segments,states,args.rotation_rpy_deg,args.max_nfev,
|
||||
args.hpr_direct_sigma_rad,str(args.checkpoint_dir))) as executor:
|
||||
futures=[executor.submit(_solve,task) for task in tasks]
|
||||
for future in as_completed(futures):
|
||||
result=future.result(); results.append(result)
|
||||
print('completed',result['name'],flush=True)
|
||||
results.sort(key=lambda x:('xyz'.index(x['axis']),x['signed_angle_deg']))
|
||||
nominal=engineering['prior_constrained_solution']
|
||||
nominal_l=np.asarray(nominal['result']['final_l_I_m'])
|
||||
nominal_data=nominal['data_only_summary_excluding_prior_factor']
|
||||
nominal_R=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
nominal_T=make_transform(-nominal_R@nominal_l,nominal_R)
|
||||
for result in results:
|
||||
result['delta_l_from_nominal_m']=np.asarray(result['final_l_I_m'])-nominal_l
|
||||
result['delta_l_norm_m']=float(np.linalg.norm(result['delta_l_from_nominal_m']))
|
||||
result['transform_translation_delta_m']=(
|
||||
np.asarray(result['T_RTK_IMU'])[:3,3]-nominal_T[:3,3])
|
||||
result['transform_translation_delta_norm_m']=float(np.linalg.norm(
|
||||
result['transform_translation_delta_m']))
|
||||
result['transform_rotation_delta_deg']=abs(result['signed_angle_deg'])
|
||||
result['relative_data_cost_delta']=result['data_cost']/nominal_data['cost']-1.
|
||||
result['factor_p95_delta_sigma']={key:
|
||||
result['residual_by_factor'][key]['p95_abs']-
|
||||
nominal_data['residual_by_factor'][key]['p95_abs']
|
||||
for key in ('best_position','doppler','hpr','imu_preintegration')}
|
||||
delta=np.asarray([x['delta_l_from_nominal_m'] for x in results])
|
||||
at_point3=[x for x in results if abs(x['signed_angle_deg'])==.3]
|
||||
checks={'all_18_complete_and_converged':
|
||||
len(results)==18 and all(x['success'] for x in results),
|
||||
'max_delta_norm_at_0p3deg_le_0p10m':
|
||||
max(x['delta_l_norm_m'] for x in at_point3)<=.10,
|
||||
'max_delta_norm_all_le_0p15m':
|
||||
max(x['delta_l_norm_m'] for x in results)<=.15,
|
||||
'relative_data_cost_increase_all_le_0p05':
|
||||
max(x['relative_data_cost_delta'] for x in results)<=.05,
|
||||
'factor_normalized_p95_increase_all_le_0p5sigma':
|
||||
max(v for x in results for v in x['factor_p95_delta_sigma'].values())<=.5}
|
||||
passed=bool(all(checks.values()))
|
||||
payload={'scope':'18 prior-constrained fixed-R2G perturbation solves',
|
||||
'data_only_free_called':False,'bootstrap_called':False,'loo_called':False,
|
||||
'parser_R0_covariance_modified':False,'calibration_window_count':47,
|
||||
'nominal_rotation_rpy_deg':args.rotation_rpy_deg,
|
||||
'nominal_l_I_m':nominal_l,'nominal_T_RTK_IMU':nominal_T,
|
||||
'perturbations':results,'summary':{
|
||||
'max_abs_delta_l_xyz_m':np.max(np.abs(delta),axis=0),
|
||||
'max_delta_l_norm_m':float(np.max(np.linalg.norm(delta,axis=1))),
|
||||
'max_transform_translation_delta_norm_m':max(
|
||||
x['transform_translation_delta_norm_m'] for x in results)},
|
||||
'diagnostic_gate_thresholds_frozen_before_run':{
|
||||
'max_delta_norm_at_0p3deg_m':.10,'max_delta_norm_all_m':.15,
|
||||
'relative_data_cost_increase':.05,
|
||||
'factor_normalized_p95_increase_sigma':.5},
|
||||
'gate_checks':checks,'rotation_sensitivity_passed':passed}
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable({'summary':payload['summary'],
|
||||
'checks':checks,'rotation_sensitivity_passed':passed}),indent=2))
|
||||
return 0
|
||||
if __name__=='__main__': raise SystemExit(main())
|
||||
@@ -0,0 +1,42 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run R1b/R2V/R2G/R3 on unified native-time G90/HI13 exports."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.rtk_imu_multisource import (
|
||||
load_unified_sessions,
|
||||
result_to_jsonable,
|
||||
solve_r3,
|
||||
)
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--session", action="append")
|
||||
parser.add_argument("--level-static", action="append", required=True)
|
||||
args = parser.parse_args(argv)
|
||||
selected = None if not args.session else set(args.session) | set(args.level_static)
|
||||
sessions = load_unified_sessions(args.manifest, selected_session_ids=selected)
|
||||
result = solve_r3(sessions, level_static_session_ids=set(args.level_static))
|
||||
payload = result_to_jsonable(result)
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
args.output.write_text(
|
||||
json.dumps(payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
print(json.dumps(payload, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,75 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Run one 10-20 s per-node state graph with a fixed mechanical lever.'''
|
||||
from __future__ import annotations
|
||||
import argparse, json, sys
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem, solve_fixed_lever
|
||||
from tools.audit_rtk_imu_factor_consistency import (
|
||||
MECHANICAL_L_I_M, _build_segment, _jsonable, _qualified_runs)
|
||||
|
||||
def _select_window(sessions,reference,period,target_duration):
|
||||
best = None
|
||||
for session,run,segment_id in _qualified_runs(sessions,reference,period):
|
||||
for start in range(len(run)):
|
||||
target_t = run[start].t_s+target_duration
|
||||
end = int(np.searchsorted([node.t_s for node in run],target_t))
|
||||
if end >= len(run): continue
|
||||
window = tuple(run[start:end+1])
|
||||
duration = window[-1].t_s-window[0].t_s
|
||||
if not 10. <= duration <= 20.: continue
|
||||
best_count = sum(node.source == 'BESTNAVA' for node in window)
|
||||
doppler_count = sum(node.velocity_enu_m_s is not None for node in window)
|
||||
hpr_count = sum(node.hpr_factor_valid for node in window)
|
||||
gyro_score = sum(np.linalg.norm(node.gyro_rad_s) for node in window)
|
||||
score = 10.*best_count+10.*doppler_count+2.*hpr_count+gyro_score
|
||||
candidate = (score,session,window,f'{segment_id}:window_{start:03d}',
|
||||
best_count,doppler_count,hpr_count)
|
||||
if best is None or candidate[0] > best[0]: best = candidate
|
||||
if best is None: raise RuntimeError('no 10-20 s qualified window')
|
||||
score,session,window,segment_id,best_count,doppler_count,hpr_count = best
|
||||
segment = _build_segment(session,window,segment_id)
|
||||
if segment is None: raise RuntimeError('selected window preintegration failed')
|
||||
return segment,{'score':score,'session_id':session.session_id,
|
||||
'segment_id':segment_id,'start_s':window[0].t_s,'end_s':window[-1].t_s,
|
||||
'duration_s':window[-1].t_s-window[0].t_s,'node_count':len(window),
|
||||
'best_count':best_count,'doppler_count':doppler_count,'hpr_factor_count':hpr_count}
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--session',required=True)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--target-duration-s',type=float,default=15.)
|
||||
parser.add_argument('--max-nfev',type=int,default=30)
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
|
||||
default=MECHANICAL_L_I_M.tolist())
|
||||
args = parser.parse_args()
|
||||
sessions = load_unified_sessions(args.manifest,selected_session_ids={args.session})
|
||||
reference = _height_reference(sessions)
|
||||
if reference is None: raise RuntimeError('no BEST reference')
|
||||
segment,selection = _select_window(
|
||||
sessions,reference,args.sample_period_s,args.target_duration_s)
|
||||
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
problem = build_problem(segment,rotation,np.asarray(args.mechanical_l_I_m))
|
||||
result = solve_fixed_lever(problem,args.max_nfev)
|
||||
payload = {'scope':'single 10-20 s node-state graph; fixed mechanical lever',
|
||||
'translation_variable_enabled':False,'free_prior_loo_bootstrap_sensitivity_called':False,
|
||||
'selection':selection,'result':asdict(result)}
|
||||
args.output.parent.mkdir(parents=True,exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
if __name__ == '__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,79 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Three-start, no-prior free-lever solve on the frozen P0.5 circle window.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem,solve_free_lever
|
||||
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
|
||||
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
|
||||
|
||||
|
||||
def main():
|
||||
parser=argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--circle-session',required=True)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--target-duration-s',type=float,default=15.)
|
||||
parser.add_argument('--max-nfev',type=int,default=120)
|
||||
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
|
||||
default=MECHANICAL_L_I_M.tolist())
|
||||
parser.add_argument('--large-perturbation-m',nargs=3,type=float,
|
||||
default=[.5,-.5,.5])
|
||||
args=parser.parse_args()
|
||||
sessions=load_unified_sessions(
|
||||
args.manifest,selected_session_ids={args.circle_session})
|
||||
reference=_height_reference(sessions)
|
||||
segment,selection=_select_window(
|
||||
sessions,reference,args.sample_period_s,args.target_duration_s)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
mechanical=np.asarray(args.mechanical_l_I_m,dtype=float)
|
||||
problem=build_problem(segment,rotation,mechanical,args.hpr_direct_sigma_rad)
|
||||
starts={'zero':np.zeros(3),'mechanical':mechanical,
|
||||
'mechanical_large_perturbation':mechanical+args.large_perturbation_m}
|
||||
results={name:asdict(solve_free_lever(problem,value,args.max_nfev))
|
||||
for name,value in starts.items()}
|
||||
for result in results.values():
|
||||
covariance=np.asarray(result['lever_covariance_m2'])
|
||||
result['lever_std_m']=np.sqrt(np.maximum(np.diag(covariance),0.))
|
||||
result['delta_to_mechanical_m']=np.asarray(result['final_l_I_m'])-mechanical
|
||||
result['xy_marginal_covariance_m2']=covariance[:2,:2]
|
||||
result['xy_information_singular_values']=np.linalg.svd(
|
||||
np.linalg.pinv(covariance[:2,:2],rcond=1e-9),compute_uv=False)
|
||||
solutions=np.asarray([value['final_l_I_m'] for value in results.values()])
|
||||
spread=float(max(np.linalg.norm(a-b) for a in solutions for b in solutions))
|
||||
xy_spread=float(max(np.linalg.norm(a[:2]-b[:2]) for a in solutions for b in solutions))
|
||||
circle_xy_observable=bool(all(value['success'] and
|
||||
np.all(np.asarray(value['lever_std_m'])[:2]<=.15) for value in results.values()))
|
||||
payload={'scope':'circle single-segment no-prior three-start free lever',
|
||||
'translation_variable_enabled':True,'manual_prior_used':False,
|
||||
'loo_bootstrap_sensitivity_called':False,'selection':selection,
|
||||
'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
|
||||
'hpr_direct_sigma_rad':args.hpr_direct_sigma_rad,
|
||||
'mechanical_reference_m':mechanical,'large_perturbation_m':args.large_perturbation_m,
|
||||
'solutions':results,'maximum_solution_spread_m':spread,
|
||||
'maximum_xy_solution_spread_m':xy_spread,
|
||||
'circle_only_observability_gate':{
|
||||
'xy_marginal_std_max_m':.15,'passed':circle_xy_observable},
|
||||
'circle_only_data_only_accepted':circle_xy_observable,
|
||||
'joint_free_blocked_by_circle_gate':False,
|
||||
'covariance_postfit_scaled':False}
|
||||
args.output.parent.mkdir(parents=True,exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,85 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Three-start, no-prior joint free-lever solve on fixed P0.5 motion windows.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem,solve_free_lever_many
|
||||
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
|
||||
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
|
||||
|
||||
|
||||
def main():
|
||||
parser=argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--circle-session',required=True)
|
||||
parser.add_argument('--left-right-session',required=True)
|
||||
parser.add_argument('--slope-session',required=True)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--target-duration-s',type=float,default=15.)
|
||||
parser.add_argument('--max-nfev',type=int,default=120)
|
||||
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
|
||||
default=MECHANICAL_L_I_M.tolist())
|
||||
parser.add_argument('--large-perturbation-m',nargs=3,type=float,
|
||||
default=[.5,-.5,.5])
|
||||
args=parser.parse_args()
|
||||
categories={'circle':args.circle_session,'left_right':args.left_right_session,
|
||||
'slope':args.slope_session}
|
||||
sessions=load_unified_sessions(
|
||||
args.manifest,selected_session_ids=set(categories.values()))
|
||||
reference=_height_reference(sessions)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
mechanical=np.asarray(args.mechanical_l_I_m,dtype=float)
|
||||
problems,selections=[],{}
|
||||
for motion,session_id in categories.items():
|
||||
segment,selection=_select_window(
|
||||
[s for s in sessions if s.session_id==session_id],reference,
|
||||
args.sample_period_s,args.target_duration_s)
|
||||
problems.append(build_problem(
|
||||
segment,rotation,mechanical,args.hpr_direct_sigma_rad))
|
||||
selections[motion]=selection
|
||||
starts={'zero':np.zeros(3),'mechanical':mechanical,
|
||||
'mechanical_large_perturbation':mechanical+args.large_perturbation_m}
|
||||
results={name:asdict(solve_free_lever_many(problems,value,args.max_nfev))
|
||||
for name,value in starts.items()}
|
||||
for result in results.values():
|
||||
covariance=np.asarray(result['lever_covariance_m2'])
|
||||
result['lever_std_m']=np.sqrt(np.maximum(np.diag(covariance),0.))
|
||||
result['delta_to_mechanical_m']=np.asarray(result['final_l_I_m'])-mechanical
|
||||
solutions=np.asarray([value['final_l_I_m'] for value in results.values()])
|
||||
spread=float(max(np.linalg.norm(a-b) for a in solutions for b in solutions))
|
||||
gates={name:{'optimizer_converged':value['success'],
|
||||
'lever_marginal_std':bool(np.all(np.asarray(value['lever_std_m'])<=[.15,.15,.20])),
|
||||
'lever_information_rank':value['lever_precision_rank']==3,
|
||||
'lever_information_condition':value['lever_information_condition_number']<=1e6,
|
||||
'lever_min_information':min(value['lever_information_singular_values'])>=1e-3}
|
||||
for name,value in results.items()}
|
||||
observable=bool(all(all(gate.values()) for gate in gates.values()))
|
||||
payload={'scope':'three-motion joint no-prior three-start free lever',
|
||||
'translation_variable_enabled':True,'manual_prior_used':False,
|
||||
'loo_bootstrap_sensitivity_called':False,'selections':selections,
|
||||
'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
|
||||
'hpr_direct_sigma_rad':args.hpr_direct_sigma_rad,
|
||||
'mechanical_reference_m':mechanical,'solutions':results,
|
||||
'maximum_solution_spread_m':spread,'observability_gates':gates,
|
||||
'joint_data_only_translation_observable':observable,
|
||||
'manual_prior_started':False,'covariance_postfit_scaled':False}
|
||||
args.output.parent.mkdir(parents=True,exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,120 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Three-start joint free solve on information-selected windows.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import (
|
||||
build_problem,fit_states_at_fixed_lever,solve_free_lever_many)
|
||||
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_build_segment,_jsonable,_qualified_runs
|
||||
|
||||
|
||||
def _restore_segments(sessions,reference,selections,period_s):
|
||||
runs=list(_qualified_runs(sessions,reference,period_s)); restored=[]
|
||||
for item in selections:
|
||||
match=None
|
||||
for session,run,_ in runs:
|
||||
if session.session_id!=item['session_id']: continue
|
||||
contained=(run[0].t_s<=item['start_s']+1e-6 and
|
||||
run[-1].t_s>=item['end_s']-1e-6)
|
||||
if not contained: continue
|
||||
nodes=tuple(node for node in run
|
||||
if item['start_s']-1e-6<=node.t_s<=item['end_s']+1e-6)
|
||||
if len(nodes)==item['node_count']:
|
||||
match=_build_segment(session,nodes,item['candidate_id']); break
|
||||
if match is None: raise RuntimeError('cannot restore '+item['candidate_id'])
|
||||
restored.append(match)
|
||||
return restored
|
||||
|
||||
|
||||
def main():
|
||||
parser=argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--selection',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--max-nfev',type=int,default=120)
|
||||
parser.add_argument('--prefit-max-nfev',type=int,default=50)
|
||||
parser.add_argument('--prefit-workers',type=int,default=4)
|
||||
parser.add_argument('--start-name',choices=['all','zero','mechanical',
|
||||
'mechanical_large_perturbation'],default='all')
|
||||
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
|
||||
default=MECHANICAL_L_I_M.tolist())
|
||||
parser.add_argument('--large-perturbation-m',nargs=3,type=float,
|
||||
default=[.5,-.5,.5])
|
||||
args=parser.parse_args()
|
||||
selection=json.loads(args.selection.read_text(encoding='utf-8'))
|
||||
ids={item['session_id'] for item in selection['selected_windows']}
|
||||
sessions=load_unified_sessions(args.manifest,selected_session_ids=ids)
|
||||
reference=_height_reference(sessions)
|
||||
segments=_restore_segments(sessions,reference,selection['selected_windows'],
|
||||
args.sample_period_s)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
mechanical=np.asarray(args.mechanical_l_I_m,dtype=float)
|
||||
problems=[build_problem(segment,rotation,mechanical,args.hpr_direct_sigma_rad)
|
||||
for segment in segments]
|
||||
starts={'zero':np.zeros(3),'mechanical':mechanical,
|
||||
'mechanical_large_perturbation':mechanical+args.large_perturbation_m}
|
||||
if args.start_name!='all': starts={args.start_name:starts[args.start_name]}
|
||||
results={}; prefit={}
|
||||
for name,value in starts.items():
|
||||
with ThreadPoolExecutor(max_workers=args.prefit_workers) as executor:
|
||||
fitted=list(executor.map(
|
||||
lambda problem:fit_states_at_fixed_lever(
|
||||
problem,value,args.prefit_max_nfev),problems))
|
||||
state_values=[item[0] for item in fitted]
|
||||
prefit[name]=[item[1] for item in fitted]
|
||||
results[name]=asdict(solve_free_lever_many(
|
||||
problems,value,args.max_nfev,state_values))
|
||||
for result in results.values():
|
||||
covariance=np.asarray(result['lever_covariance_m2'])
|
||||
result['lever_std_m']=np.sqrt(np.maximum(np.diag(covariance),0.))
|
||||
result['delta_to_mechanical_m']=np.asarray(result['final_l_I_m'])-mechanical
|
||||
solutions=np.asarray([value['final_l_I_m'] for value in results.values()])
|
||||
spread=float(max(np.linalg.norm(a-b) for a in solutions for b in solutions))
|
||||
gates={name:{'optimizer_converged':value['success'],
|
||||
'lever_marginal_std':bool(np.all(np.asarray(value['lever_std_m'])<=[.15,.15,.20])),
|
||||
'lever_information_rank':value['lever_precision_rank']==3,
|
||||
'lever_information_condition':value['lever_information_condition_number']<=1e6,
|
||||
'lever_min_information':min(value['lever_information_singular_values'])>=1e-3}
|
||||
for name,value in results.items()}
|
||||
observable=bool(all(all(gate.values()) for gate in gates.values()))
|
||||
payload={'scope':'information-selected multi-window no-prior three-start free lever',
|
||||
'translation_variable_enabled':True,'manual_prior_used':False,
|
||||
'loo_bootstrap_sensitivity_called':False,
|
||||
'selection_artifact':str(args.selection),'selected_window_count':len(segments),
|
||||
'start_name':args.start_name,'nuisance_prefit':prefit,
|
||||
'nuisance_prefit_is_not_lever_prior':True,
|
||||
'sample_overlap_audit':selection['sample_overlap_audit'],
|
||||
'information_curve':selection['lever_std_vs_information_curve'],
|
||||
'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
|
||||
'hpr_direct_sigma_rad':args.hpr_direct_sigma_rad,
|
||||
'mechanical_reference_m':mechanical,'solutions':results,
|
||||
'maximum_solution_spread_m':spread,'observability_gates':gates,
|
||||
'data_only_translation_observable':observable,
|
||||
'manual_prior_started':False,'covariance_postfit_scaled':False}
|
||||
args.output.parent.mkdir(parents=True,exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable({'selected_window_count':len(segments),
|
||||
'solutions':{name:{'success':value['success'],'l_I_m':value['final_l_I_m'],
|
||||
'std_m':value['lever_std_m'],'cost':value['final_cost'],
|
||||
'chi_square_per_dof':value['chi_square_per_dof'],
|
||||
'singular_values':value['lever_information_singular_values']}
|
||||
for name,value in results.items()},'maximum_solution_spread_m':spread,
|
||||
'data_only_translation_observable':observable}),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,106 @@
|
||||
#!/usr/bin/env python3
|
||||
'''P0.5 fixed-lever node-graph covariance and motion-window audit.'''
|
||||
from __future__ import annotations
|
||||
import argparse, json, sys
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import (
|
||||
build_problem,solve_fixed_lever,solve_fixed_lever_many)
|
||||
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
|
||||
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--circle-session',required=True)
|
||||
parser.add_argument('--left-right-session',required=True)
|
||||
parser.add_argument('--slope-session',required=True)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--target-duration-s',type=float,default=15.)
|
||||
parser.add_argument('--max-nfev',type=int,default=50)
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
|
||||
default=MECHANICAL_L_I_M.tolist())
|
||||
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
args = parser.parse_args()
|
||||
categories = {'circle':args.circle_session,'left_right':args.left_right_session,
|
||||
'slope':args.slope_session}
|
||||
sessions = load_unified_sessions(args.manifest,selected_session_ids=set(categories.values()))
|
||||
reference = _height_reference(sessions)
|
||||
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
lever = np.asarray(args.mechanical_l_I_m)
|
||||
problems, selections, separate = [], {}, {}
|
||||
for category,session_id in categories.items():
|
||||
local = [session for session in sessions if session.session_id == session_id]
|
||||
segment,selection = _select_window(
|
||||
local,reference,args.sample_period_s,args.target_duration_s)
|
||||
problem = build_problem(segment,rotation,lever,args.hpr_direct_sigma_rad)
|
||||
problems.append(problem); selections[category] = selection
|
||||
separate[category] = asdict(solve_fixed_lever(problem,args.max_nfev))
|
||||
joint = asdict(solve_fixed_lever_many(problems,args.max_nfev))
|
||||
fixed_pass = bool(
|
||||
all(value['success'] for value in separate.values()) and joint['success']
|
||||
and joint['chi_square_per_dof'] >= .25
|
||||
and joint['chi_square_per_dof'] <= 4.
|
||||
and joint['final_position_residual_m']['vector_p95'] <= .20
|
||||
and joint['final_velocity_residual_m_s']['vector_p95'] <= .50)
|
||||
payload = {'scope':'P0.5 three-motion fixed-lever only',
|
||||
'translation_variable_enabled':False,
|
||||
'manual_prior_loo_bootstrap_sensitivity_called':False,
|
||||
'covariance_model':{
|
||||
'source':'independent_innovation_audit_three_motion',
|
||||
'best_position_xyz_m':[.06,.06,.12],
|
||||
'doppler_xyz_m_s':[.15,.15,.30],
|
||||
'hpr_direct_angular_rad':args.hpr_direct_sigma_rad,
|
||||
'hpr_bridge_rule':'sqrt(direct_sigma^2 + dropout_extra_variance)',
|
||||
'imu_preintegration':'unchanged physical covariance',
|
||||
'bias_random_walk':'unchanged static/Allan/device model',
|
||||
'postfit_global_scale_applied':False},
|
||||
'fixed_l_I_m':lever,'selections':selections,
|
||||
'separate_fixed_lever':separate,'joint_fixed_lever':joint,
|
||||
'fixed_lever_covariance_gate':{
|
||||
'chi_square_per_dof_range':[.25,4.],
|
||||
'position_vector_p95_max_m':.20,
|
||||
'velocity_vector_p95_max_m_s':.50,
|
||||
'passed':fixed_pass},
|
||||
'whitening_diagnosis':{
|
||||
'normalized_scale_consistent':joint['chi_square_per_dof'] >= .25,
|
||||
'joint_preintegration_nis_per_dof':
|
||||
joint['final_residual_by_factor']['imu_preintegration']['chi_square_per_dof'],
|
||||
'joint_preintegration_normalized_p95':
|
||||
joint['final_residual_by_factor']['imu_preintegration']['p95_abs'],
|
||||
'preintegration_sigma_distribution':joint['preintegration_covariance_sigma'],
|
||||
'interpretation':(
|
||||
'absolute preintegration sigma is small, but per-node states satisfy process '
|
||||
'factors almost exactly while BEST/Doppler/HPR normalized residuals are also '
|
||||
'well below one; current factor covariance set is collectively overconservative'
|
||||
)},
|
||||
'free_lever_unlocked':fixed_pass}
|
||||
args.output.parent.mkdir(parents=True,exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
compact = {'selections':selections,
|
||||
'separate':{key:{'success':value['success'],
|
||||
'chi_square_per_dof':value['chi_square_per_dof'],
|
||||
'position_p95_m':value['final_position_residual_m']['vector_p95'],
|
||||
'velocity_p95_m_s':value['final_velocity_residual_m_s']['vector_p95'],
|
||||
'factor_stats':value['final_residual_by_factor']}
|
||||
for key,value in separate.items()},
|
||||
'joint':{'success':joint['success'],'chi_square_per_dof':joint['chi_square_per_dof'],
|
||||
'position_p95_m':joint['final_position_residual_m']['vector_p95'],
|
||||
'velocity_p95_m_s':joint['final_velocity_residual_m_s']['vector_p95'],
|
||||
'factor_stats':joint['final_residual_by_factor']},
|
||||
'free_lever_unlocked':fixed_pass}
|
||||
print(json.dumps(_jsonable(compact),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
if __name__ == '__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,174 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Select non-overlapping qualified windows by incremental lever information.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
||||
from imu_lidar.rtk_imu_engineering import _all_hpr,_height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem,linearized_lever_information
|
||||
from tools.audit_rtk_imu_factor_consistency import _build_segment,_jsonable,_qualified_runs
|
||||
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
|
||||
|
||||
|
||||
STD_GATE=np.array([.15,.15,.20])
|
||||
|
||||
|
||||
def _overlap(left,right):
|
||||
return left['session_id']==right['session_id'] and not (
|
||||
left['end_s']<right['start_s'] or right['end_s']<left['start_s'])
|
||||
|
||||
|
||||
def _summary(information):
|
||||
covariance=np.linalg.pinv(information,rcond=1e-9)
|
||||
singular=np.linalg.svd(information,compute_uv=False)
|
||||
sign,logdet=np.linalg.slogdet(information)
|
||||
return {'std_m':np.sqrt(np.maximum(np.diag(covariance),0.)),
|
||||
'singular_values':singular,'lambda_min':singular[-1],
|
||||
'logdet':float(logdet) if sign>0 else -np.inf}
|
||||
|
||||
|
||||
def _window_candidates(sessions,reference,period_s,duration_s):
|
||||
candidates=[]
|
||||
for session,run,segment_id in _qualified_runs(sessions,reference,period_s):
|
||||
times=np.asarray([node.t_s for node in run])
|
||||
start=0
|
||||
while start<len(run):
|
||||
end=int(np.searchsorted(times,times[start]+duration_s))
|
||||
if end>=len(run): break
|
||||
window=tuple(run[start:end+1])
|
||||
if 10.<=window[-1].t_s-window[0].t_s<=20.:
|
||||
candidate_id=f'{segment_id}:info_window_{start:04d}'
|
||||
segment=_build_segment(session,window,candidate_id)
|
||||
if segment is not None:
|
||||
candidates.append({'candidate_id':candidate_id,
|
||||
'session_id':session.session_id,'start_s':window[0].t_s,
|
||||
'end_s':window[-1].t_s,'duration_s':window[-1].t_s-window[0].t_s,
|
||||
'node_count':len(window),'segment':segment,'session':session})
|
||||
start=end+1
|
||||
return candidates
|
||||
|
||||
|
||||
def _sample_keys(entry):
|
||||
session=entry['session']; start,end=entry['start_s'],entry['end_s']
|
||||
imu=set((session.session_id,int(round(t*1e6))) for t in session.imu.t_s
|
||||
if start<=t<=end)
|
||||
gnss=set((session.session_id,int(round(node.t_s*1e6)),node.source)
|
||||
for node in entry['segment'].nodes)
|
||||
hpr=_all_hpr(session)
|
||||
hpr_keys=set((session.session_id,int(round(hpr.t_s[i]*1e6)))
|
||||
for i in hpr.valid_indices if start<=hpr.t_s[i]<=end)
|
||||
return {'imu':imu,'gnss':gnss,'hpr':hpr_keys}
|
||||
|
||||
|
||||
def main():
|
||||
parser=argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--circle-session',required=True)
|
||||
parser.add_argument('--left-right-session',required=True)
|
||||
parser.add_argument('--slope-session',required=True)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--window-duration-s',type=float,default=15.)
|
||||
parser.add_argument('--max-additional-windows',type=int,default=100)
|
||||
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
parser.add_argument('--linearization-l-I-m',nargs=3,type=float,
|
||||
default=[-.53243,-.42662,.74660])
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
args=parser.parse_args()
|
||||
sessions=load_unified_sessions(args.manifest)
|
||||
reference=_height_reference(sessions)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
lever=np.asarray(args.linearization_l_I_m,dtype=float)
|
||||
categories={'circle':args.circle_session,'left_right':args.left_right_session,
|
||||
'slope':args.slope_session}
|
||||
selected=[]
|
||||
for motion,session_id in categories.items():
|
||||
session=[s for s in sessions if s.session_id==session_id]
|
||||
segment,meta=_select_window(
|
||||
session,reference,args.sample_period_s,args.window_duration_s)
|
||||
selected.append({**meta,'candidate_id':meta['segment_id'],'motion':motion,
|
||||
'segment':segment,'session':session[0],'seed_window':True})
|
||||
candidates=[entry for entry in _window_candidates(
|
||||
sessions,reference,args.sample_period_s,args.window_duration_s)
|
||||
if not any(_overlap(entry,seed) for seed in selected)]
|
||||
for entry in [*selected,*candidates]:
|
||||
problem=build_problem(entry['segment'],rotation,lever,args.hpr_direct_sigma_rad)
|
||||
information,_,singular,weak=linearized_lever_information(problem,lever)
|
||||
entry['problem']=problem; entry['information']=information
|
||||
entry['single_window_singular_values']=singular
|
||||
entry['single_window_weakest_direction_I']=weak
|
||||
total=sum((entry['information'] for entry in selected),np.zeros((3,3)))
|
||||
curve=[{'window_count':len(selected),'added_candidate_id':'seed_three_motion',
|
||||
**_summary(total)}]
|
||||
remaining=list(candidates)
|
||||
saturation_count=0
|
||||
stop_reason='candidate_exhausted'
|
||||
while remaining and len(selected)<3+args.max_additional_windows:
|
||||
feasible=[entry for entry in remaining
|
||||
if not any(_overlap(entry,item) for item in selected)]
|
||||
if not feasible: break
|
||||
ranked=[]
|
||||
for entry in feasible:
|
||||
summary=_summary(total+entry['information'])
|
||||
ranked.append((summary['lambda_min'],summary['logdet'],entry,summary))
|
||||
_,_,choice,summary=max(ranked,key=lambda item:(item[0],item[1]))
|
||||
gain=summary['lambda_min']-curve[-1]['lambda_min']
|
||||
selected.append(choice); remaining.remove(choice); total+=choice['information']
|
||||
curve.append({'window_count':len(selected),'added_candidate_id':choice['candidate_id'],
|
||||
'delta_lambda_min':gain,**summary})
|
||||
saturation_count=saturation_count+1 if gain<.01 else 0
|
||||
if np.all(np.asarray(summary['std_m'])<=STD_GATE):
|
||||
stop_reason='linearized_observability_gate_reached'; break
|
||||
if saturation_count>=3:
|
||||
stop_reason='incremental_lambda_min_gain_saturated'; break
|
||||
else:
|
||||
if len(selected)>=3+args.max_additional_windows:
|
||||
stop_reason='max_additional_windows_reached'
|
||||
seen={'imu':set(),'gnss':set(),'hpr':set()}
|
||||
duplicate={'imu':0,'gnss':0,'hpr':0}
|
||||
selected_output=[]
|
||||
for order,entry in enumerate(selected):
|
||||
keys=_sample_keys(entry)
|
||||
shared={name:len(value&seen[name]) for name,value in keys.items()}
|
||||
for name,value in keys.items():
|
||||
duplicate[name]+=shared[name]; seen[name].update(value)
|
||||
selected_output.append({'selection_order':order,
|
||||
'candidate_id':entry['candidate_id'],'session_id':entry['session_id'],
|
||||
'start_s':entry['start_s'],'end_s':entry['end_s'],
|
||||
'duration_s':entry['duration_s'],'node_count':entry['node_count'],
|
||||
'seed_window':entry.get('seed_window',False),
|
||||
'single_window_information':entry['information'],
|
||||
'single_window_singular_values':entry['single_window_singular_values'],
|
||||
'single_window_weakest_direction_I':entry['single_window_weakest_direction_I'],
|
||||
'sample_count':{name:len(value) for name,value in keys.items()},
|
||||
'shared_sample_count_with_previous':shared})
|
||||
payload={'scope':'all recovered qualified dynamics; information-based window selection',
|
||||
'manual_prior_used':False,'linearization_l_I_m':lever,
|
||||
'linearization_point_is_not_prior_factor':True,
|
||||
'candidate_count':len(candidates),'selected_window_count':len(selected),
|
||||
'selection_objective':'maximize lambda_min(H_l); break ties by logdet(H_l)',
|
||||
'std_gate_m':STD_GATE,'stop_reason':stop_reason,
|
||||
'selected_windows':selected_output,'lever_std_vs_information_curve':curve,
|
||||
'sample_overlap_audit':{'duplicate_sample_count':duplicate,
|
||||
'all_selected_windows_time_nonoverlapping_within_session':not any(
|
||||
_overlap(a,b) for i,a in enumerate(selected) for b in selected[i+1:]),
|
||||
'unique_sample_count':{name:len(value) for name,value in seen.items()}},
|
||||
'final_linearized_observable':bool(np.all(
|
||||
np.asarray(curve[-1]['std_m'])<=STD_GATE))}
|
||||
args.output.parent.mkdir(parents=True,exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable({'candidate_count':len(candidates),
|
||||
'selected_window_count':len(selected),'stop_reason':stop_reason,
|
||||
'final_curve':curve[-1],'sample_overlap_audit':payload['sample_overlap_audit']}),
|
||||
ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,182 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Read-only artificial GNHPR dropout validation for the engineering R0 bridge.
|
||||
|
||||
For each requested gap, Q4 HPR samples inside an otherwise 0.1 s contiguous
|
||||
run are withheld. Their true baseline directions are compared with normalized
|
||||
linear interpolation and short-horizon HI13 gyro propagation. No lever-arm
|
||||
solve, prior, bootstrap, sensitivity run, or threshold update is performed.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.rtk_imu_engineering import _all_hpr, _baseline_angle_deg, _interpolate_baseline, _world_rtk
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
|
||||
GAPS_S = (0.2, 0.3, 0.4, 0.5, 0.6, 0.8)
|
||||
R2G_RPY_DEG = (0.4543066225, -0.0026392019, 0.0122384129)
|
||||
BASELINE_FACTOR_SIGMA_DEG = float(np.degrees(0.035))
|
||||
|
||||
|
||||
def _jsonable(value):
|
||||
if isinstance(value, np.ndarray):
|
||||
return _jsonable(value.tolist())
|
||||
if isinstance(value, np.generic):
|
||||
return _jsonable(value.item())
|
||||
if isinstance(value, float):
|
||||
return value if math.isfinite(value) else None
|
||||
if isinstance(value, dict):
|
||||
return {str(k): _jsonable(v) for k, v in value.items()}
|
||||
if isinstance(value, (tuple, list)):
|
||||
return [_jsonable(v) for v in value]
|
||||
return value
|
||||
|
||||
|
||||
def _contiguous_runs(times: np.ndarray, valid: np.ndarray) -> list[np.ndarray]:
|
||||
indices = np.flatnonzero(valid)
|
||||
if indices.size == 0:
|
||||
return []
|
||||
runs: list[list[int]] = [[int(indices[0])]]
|
||||
for previous, index in zip(indices[:-1], indices[1:]):
|
||||
dt = float(times[index] - times[previous])
|
||||
if 0.075 <= dt <= 0.125:
|
||||
runs[-1].append(int(index))
|
||||
else:
|
||||
runs.append([int(index)])
|
||||
return [np.asarray(run, dtype=int) for run in runs if len(run) >= 3]
|
||||
|
||||
|
||||
def _propagate_baseline(session, R_WI_start: np.ndarray, baseline_I: np.ndarray,
|
||||
start_s: float, targets_s: np.ndarray) -> list[np.ndarray]:
|
||||
"""Propagate with vectorized gyro interpolation over the short withheld gap."""
|
||||
targets = np.asarray(targets_s, dtype=float)
|
||||
if targets.size == 0:
|
||||
return []
|
||||
grid = np.unique(np.concatenate(([start_s], session.imu.t_s[
|
||||
(session.imu.t_s > start_s) & (session.imu.t_s < targets[-1])], targets)))
|
||||
gyro = np.column_stack([
|
||||
np.interp(grid, session.imu.t_s, session.imu.gyro_rad_s[:, axis]) for axis in range(3)
|
||||
])
|
||||
R_WI = np.asarray(R_WI_start, dtype=float).copy()
|
||||
predicted: list[np.ndarray] = []
|
||||
target_index = 0
|
||||
for index, (left, right) in enumerate(zip(grid[:-1], grid[1:])):
|
||||
omega = 0.5 * (gyro[index] + gyro[index + 1])
|
||||
R_WI = R_WI @ Rotation.from_rotvec(omega * float(right - left)).as_matrix()
|
||||
while target_index < targets.size and abs(targets[target_index] - right) < 1e-9:
|
||||
predicted.append(R_WI @ baseline_I)
|
||||
target_index += 1
|
||||
return predicted
|
||||
|
||||
def _session_validation(session, rotation: np.ndarray, max_windows: int) -> dict[str, object]:
|
||||
hpr = _all_hpr(session)
|
||||
runs = _contiguous_runs(hpr.t_s, hpr.valid)
|
||||
baseline_I = rotation.T[:, 0]
|
||||
result: dict[str, object] = {"session_id": session.session_id, "q4_runs": len(runs), "gaps": {}}
|
||||
for requested_gap in GAPS_S:
|
||||
interpolation_errors: list[float] = []
|
||||
propagation_errors: list[float] = []
|
||||
actual_gaps: list[float] = []
|
||||
windows = 0
|
||||
for run in runs:
|
||||
nominal_dt = float(np.median(np.diff(hpr.t_s[run])))
|
||||
step_count = max(2, int(round(requested_gap / nominal_dt)))
|
||||
# The endpoints remain observed, every internal Q4 point is withheld.
|
||||
for start in range(0, run.size - step_count, max(1, step_count)):
|
||||
subset = run[start:start + step_count + 1]
|
||||
if subset.size != step_count + 1:
|
||||
continue
|
||||
left, right = int(subset[0]), int(subset[-1])
|
||||
actual_gap = float(hpr.t_s[right] - hpr.t_s[left])
|
||||
if abs(actual_gap - requested_gap) > 0.08:
|
||||
continue
|
||||
withheld = subset[1:-1]
|
||||
if withheld.size == 0:
|
||||
continue
|
||||
left_b, right_b = hpr.baseline_enu[left], hpr.baseline_enu[right]
|
||||
targets = hpr.t_s[withheld]
|
||||
predicted_prop = _propagate_baseline(
|
||||
session, _world_rtk(left_b) @ rotation, baseline_I, float(hpr.t_s[left]), targets
|
||||
)
|
||||
for index, predicted in zip(withheld, predicted_prop):
|
||||
fraction = float((hpr.t_s[index] - hpr.t_s[left]) / actual_gap)
|
||||
interpolation_errors.append(_baseline_angle_deg(
|
||||
_interpolate_baseline(left_b, right_b, fraction), hpr.baseline_enu[index]
|
||||
))
|
||||
propagation_errors.append(_baseline_angle_deg(predicted, hpr.baseline_enu[index]))
|
||||
actual_gaps.append(actual_gap)
|
||||
windows += 1
|
||||
if windows >= max_windows:
|
||||
break
|
||||
if windows >= max_windows:
|
||||
break
|
||||
def summary(errors: list[float]) -> dict[str, float | int | None]:
|
||||
finite = np.asarray([e for e in errors if np.isfinite(e)], dtype=float)
|
||||
if finite.size == 0:
|
||||
return {"count": 0, "p50_deg": None, "p95_deg": None, "max_deg": None}
|
||||
return {"count": int(finite.size), "p50_deg": float(np.percentile(finite, 50)),
|
||||
"p95_deg": float(np.percentile(finite, 95)), "max_deg": float(np.max(finite))}
|
||||
interp, prop = summary(interpolation_errors), summary(propagation_errors)
|
||||
choice = "linear_baseline_interpolation" if (interp["p95_deg"] or np.inf) <= (prop["p95_deg"] or np.inf) else "imu_gyro_propagation"
|
||||
selected_p95 = interp["p95_deg"] if choice.startswith("linear") else prop["p95_deg"]
|
||||
result["gaps"][f"{requested_gap:.1f}"] = {
|
||||
"requested_gap_s": requested_gap,
|
||||
"actual_gap_p50_s": float(np.median(actual_gaps)) if actual_gaps else None,
|
||||
"window_count": windows,
|
||||
"linear_baseline_interpolation": interp,
|
||||
"imu_gyro_propagation": prop,
|
||||
"preferred_method_by_p95": choice,
|
||||
"selected_p95_deg": selected_p95,
|
||||
"within_baseline_factor_sigma": bool(selected_p95 is not None and selected_p95 <= BASELINE_FACTOR_SIGMA_DEG),
|
||||
}
|
||||
return result
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--session", action="append")
|
||||
parser.add_argument("--max-windows", type=int, default=500)
|
||||
args = parser.parse_args()
|
||||
sessions = load_unified_sessions(args.manifest, selected_session_ids=None if args.session is None else set(args.session))
|
||||
rotation = Rotation.from_euler("xyz", R2G_RPY_DEG, degrees=True).as_matrix()
|
||||
session_results = [_session_validation(session, rotation, args.max_windows) for session in sessions]
|
||||
recommendation: dict[str, object] = {}
|
||||
for gap in GAPS_S:
|
||||
entries = [item["gaps"][f"{gap:.1f}"] for item in session_results if item["gaps"].get(f"{gap:.1f}")]
|
||||
p95 = [entry["selected_p95_deg"] for entry in entries if entry["selected_p95_deg"] is not None]
|
||||
recommendation[f"{gap:.1f}"] = {
|
||||
"sessions_with_samples": len(p95),
|
||||
"worst_session_selected_p95_deg": float(max(p95)) if p95 else None,
|
||||
"passes_all_sessions_factor_sigma": bool(p95 and max(p95) <= BASELINE_FACTOR_SIGMA_DEG),
|
||||
}
|
||||
passing = [float(key) for key, value in recommendation.items() if value["passes_all_sessions_factor_sigma"]]
|
||||
payload = {
|
||||
"scope": "artificial HPR dropout validation only; no solve/prior/bootstrap/sensitivity/threshold change",
|
||||
"r2g_rotation_rpy_deg": R2G_RPY_DEG,
|
||||
"baseline_factor_sigma_deg": BASELINE_FACTOR_SIGMA_DEG,
|
||||
"sessions": session_results,
|
||||
"bridge_recommendation": recommendation,
|
||||
"largest_gap_passing_all_sessions_s": max(passing) if passing else None,
|
||||
}
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload), ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
|
||||
print(json.dumps({"largest_gap_passing_all_sessions_s": payload["largest_gap_passing_all_sessions_s"],
|
||||
"bridge_recommendation": recommendation}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Reference in New Issue
Block a user