新增独立RTK与IMU外参标定流程及质量验证

This commit is contained in:
lichun.qu
2026-08-21 10:04:41 +08:00
parent 1233f8aafd
commit c2da6dd192
38 changed files with 3937 additions and 17 deletions
@@ -0,0 +1,17 @@
# RTKIMU 标定产物说明
- `all_sessions/`:8 会话、5 s 平移节点、带 conditional rotation LOO 和 translation LOO 的当前完整基线。
- `rotation_hpr_time/`:改用 GNHPR 自带测量时刻后的全量 rotation-only 对照。
- `rotation_smoke/`:较早的 GGA 最近邻姿态时刻对照,不作为当前结果。
- `batch_0808_full_smoke/`0808 三会话完整诊断。
- `batch_0815_rotation/`0815 四会话 rotation-only 诊断。
- `single_smoke/`:早期单会话性能/数值冒烟,不作为当前结果。
每个正式运行目录包含:
- `dataset_audit.json`:样本数、固定解比例、共同时间范围和 ENU 原点。
- `rotation_result.json`:旋转、RPY、时间审计、GNHPR 候选、偏置、残差、协方差、逐会话指标和 LOO。
- `translation_result.json`:杆臂、平移、齐次矩阵、协方差/秩、位置/速度残差、偏置和 LOO。
- `summary.json`:供程序读取的最终状态和候选矩阵。
当前 `all_sessions/summary.json``diagnostic_not_accepted`。其中平移约 `[0.771, 0.569, -21.073] m` 明显不具机械真实性,禁止用于车辆配置。完整解释见 `docs/rtk_imu_calibration.md`
@@ -0,0 +1,157 @@
{
"session_count": 8,
"sessions": [
{
"session_id": "priority_174005_174515",
"batch_id": "0808",
"imu_samples": 31000,
"rtk_samples": 4780,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.854602510460251,
"common_time_span_s": [
16265.2325992,
16575.0298107
],
"origin_geodetic": [
30.465514786,
114.092169888,
29.4695
],
"imu_source": "31000 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
},
{
"session_id": "priority_174905_175450",
"batch_id": "0808",
"imu_samples": 34499,
"rtk_samples": 5211,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8547303780464403,
"common_time_span_s": [
16805.1862481,
17150.0907328
],
"origin_geodetic": [
30.4653424457,
114.092237385,
29.5366
],
"imu_source": "34499 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
},
{
"session_id": "priority_175910_180530",
"batch_id": "0808",
"imu_samples": 37998,
"rtk_samples": 5786,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8567231247839613,
"common_time_span_s": [
17410.2121738,
17790.0458228
],
"origin_geodetic": [
30.4654151935,
114.090796384,
29.5317
],
"imu_source": "37998 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
},
{
"session_id": "slope_190548_190730",
"batch_id": "0815",
"imu_samples": 10199,
"rtk_samples": 1578,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8757921419518377,
"common_time_span_s": [
36466.328875,
36568.2301441
],
"origin_geodetic": [
30.4652183602,
114.090839984,
29.9891
],
"imu_source": "10199 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
},
{
"session_id": "circle_193412_193642",
"batch_id": "0815",
"imu_samples": 14989,
"rtk_samples": 2162,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8288621646623496,
"common_time_span_s": [
38170.3385255,
38317.8571971
],
"origin_geodetic": [
30.4654799242,
114.092155912,
29.4779
],
"imu_source": "14989 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
},
{
"session_id": "loop_194223_195003",
"batch_id": "0815",
"imu_samples": 45801,
"rtk_samples": 6741,
"fixed_position_ratio": 0.9998516540572615,
"fixed_attitude_ratio": 0.8377095386441181,
"common_time_span_s": [
38661.2831251,
39121.1709553
],
"origin_geodetic": [
30.4654856957,
114.092151015,
29.4651
],
"imu_source": "45801 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
},
{
"session_id": "accel_195608_195958",
"batch_id": "0815",
"imu_samples": 23002,
"rtk_samples": 3456,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8457754629629629,
"common_time_span_s": [
39486.3574509,
39716.3199405
],
"origin_geodetic": [
30.465443014,
114.092179168,
29.5309
],
"imu_source": "23002 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
},
{
"session_id": "motion_sms_154023_154359",
"batch_id": "0819",
"imu_samples": 21556,
"rtk_samples": 3294,
"fixed_position_ratio": 0.49271402550091076,
"fixed_attitude_ratio": 0.4344262295081967,
"common_time_span_s": [
25829.8725525,
26027.4217529
],
"origin_geodetic": [
30.4651580863,
114.090773052,
36.5626
],
"imu_source": "21556 normalized samples",
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
}
]
}
@@ -0,0 +1,130 @@
{
"R_RTK_IMU": [
[
0.9996841792313951,
0.02499811511405725,
-0.002576050309343804
],
[
-0.025002617750669198,
0.9996858874629868,
-0.0017307550243271697
],
[
0.002531975526313299,
0.0017946164171362589,
0.9999951842143289
]
],
"rpy_deg": [
0.1028243313393031,
-0.1450716664951083,
-1.43269836393699
],
"gyro_bias_by_session_rad_s": {
"priority_174005_174515": [
-8.90944066727157e-05,
7.078609864979291e-05,
0.0001460390779870233
],
"priority_174905_175450": [
-4.327374730750894e-05,
-0.0001097548390022348,
3.000549725593387e-05
],
"priority_175910_180530": [
-7.036551812132934e-05,
-0.0008054961048184173,
1.3026193503057623e-05
],
"slope_190548_190730": [
6.035222904568755e-05,
-8.913884157613711e-06,
0.00015470949872434692
],
"circle_193412_193642": [
-3.696799268832588e-05,
-0.00019200076224600124,
-5.9567987995056394e-05
],
"loop_194223_195003": [
-0.0001979655352189002,
-0.00015937450336643818,
-4.343089303706036e-05
],
"accel_195608_195958": [
-7.841839817698704e-05,
-2.604932294598783e-06,
5.8586555655011475e-05
],
"motion_sms_154023_154359": [
0.00011745666467620586,
0.00021434926285955486,
-4.5147354343760625e-05
]
},
"time_offset": {
"offset_s": 0.04000000000000031,
"peak_correlation": 0.5788788671828708,
"second_best_correlation": 0.5773967219219845,
"evaluated_samples": 12394,
"reliable": false
},
"applied_time_offset_s": 0.0,
"convention": {
"name": "north_cw__pitch_nose_up__roll_right_down",
"heading_sign": -1.0,
"pitch_sign": -1.0,
"roll_sign": 1.0
},
"convention_scores_deg": {
"north_cw__pitch_nose_up__roll_right_down": 1.631322241364994,
"north_cw__pitch_opposite": 1.737582794831228,
"heading_opposite__pitch_nose_up": 7.263259116648845,
"heading_opposite__pitch_opposite": 17.716319437929055
},
"pair_count": 731,
"residual_rms_deg": 1.6276839301413086,
"residual_median_deg": 0.5695938849052221,
"residual_p95_deg": 3.0903953005641345,
"rotation_std_deg": [
0.2340126126406699,
0.20048574131667432,
2.314692068045375
],
"information_singular_values": [
82178.77940373585,
60441.86300479194,
612.6358640387364
],
"per_session_rms_deg": {
"priority_174005_174515": 0.6134232617562615,
"priority_174905_175450": 2.2485932625979106,
"priority_175910_180530": 3.095861798757508,
"slope_190548_190730": 2.388478834012087,
"circle_193412_193642": 0.5629885032374518,
"loop_194223_195003": 0.6250976606325254,
"accel_195608_195958": 0.975856888271022,
"motion_sms_154023_154359": 1.3698317592842066
},
"loo_delta_deg": {
"priority_174005_174515": 0.7895996403426686,
"priority_174905_175450": 0.9224102138653388,
"priority_175910_180530": 5.144190932547379,
"slope_190548_190730": 0.24769087330915346,
"circle_193412_193642": 1.2637729378484346,
"loop_194223_195003": 0.4844375238260164,
"accel_195608_195958": 1.0976412402728102,
"motion_sms_154023_154359": 0.6896079810115167
},
"ok": false,
"notes": [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
"residual time convention: t_IMU = t_RTK + +0.000000 s",
"GNHPR convention score gap=0.1063 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",
"time-offset correlation was ambiguous; held residual offset at zero",
"rotation failed one or more strict acceptance gates"
]
}
@@ -0,0 +1,65 @@
{
"status": "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"rtk_frame_definition": "",
"rtk_reference_point": "",
"interpretation_blockers": [
"rotation quality gates failed",
"translation quality gates failed or were not run",
"RTK frame_definition is empty",
"RTK reference_point is empty"
],
"R_RTK_IMU": [
[
0.9996841792313951,
0.02499811511405725,
-0.002576050309343804
],
[
-0.025002617750669198,
0.9996858874629868,
-0.0017307550243271697
],
[
0.002531975526313299,
0.0017946164171362589,
0.9999951842143289
]
],
"t_RTK_IMU_m": [
0.7710937276592442,
0.5693018309069646,
-21.07303743539233
],
"T_RTK_IMU": [
[
0.9996841792313951,
0.02499811511405725,
-0.002576050309343804,
0.7710937276592442
],
[
-0.025002617750669198,
0.9996858874629868,
-0.0017307550243271697,
0.5693018309069646
],
[
0.002531975526313299,
0.0017946164171362589,
0.9999951842143289,
-21.07303743539233
],
[
0.0,
0.0,
0.0,
1.0
]
],
"rotation_ok": false,
"translation_ok": false,
"rotation_result": "rotation_result.json",
"translation_result": "translation_result.json",
"dataset_audit": "dataset_audit.json"
}
@@ -0,0 +1,161 @@
{
"lever_IMU_to_RTK_in_IMU_m": [
-0.7032597491310882,
-0.5505808768918061,
21.07590765040047
],
"t_RTK_IMU_m": [
0.7710937276592442,
0.5693018309069646,
-21.07303743539233
],
"T_RTK_IMU": [
[
0.9996841792313951,
0.02499811511405725,
-0.002576050309343804,
0.7710937276592442
],
[
-0.025002617750669198,
0.9996858874629868,
-0.0017307550243271697,
0.5693018309069646
],
[
0.002531975526313299,
0.0017946164171362589,
0.9999951842143289,
-21.07303743539233
],
[
0.0,
0.0,
0.0,
1.0
]
],
"translation_std_m": [
0.367657527272358,
0.36764001580136974,
2.960858932902556
],
"lever_information_singular_values": [
7.467287688797444,
7.419771560016465,
0.11404687553142893
],
"lever_precision_rank": 3,
"position_residual_rms_xyz_m": [
0.0026057774092802665,
0.0026172203224947795,
0.0008707288139887198
],
"velocity_residual_rms_xyz_m_s": [
1.0344084464154262,
1.0018456371907998,
0.08189262443294992
],
"accel_bias_by_session_m_s2": {
"priority_174005_174515": [
-0.0033446448235936025,
0.04850567116751809,
0.014864908749227664
],
"priority_174905_175450": [
0.051484854191344014,
0.009750775340658668,
0.016837766756117954
],
"priority_175910_180530": [
-0.0012098409852818557,
0.04428237322299069,
0.015044444493724668
],
"slope_190548_190730": [
0.007720083607057322,
-0.07045791547639069,
0.011096872492579776
],
"circle_193412_193642": [
0.010695150095323893,
0.07124837152622766,
0.012181970165526993
],
"loop_194223_195003": [
0.017798177348527063,
0.026531175431114495,
0.013517325106459492
],
"accel_195608_195958": [
0.010843370014374661,
0.0021847264023688797,
0.012720554759472001
],
"motion_sms_154023_154359": [
-0.01917068926914634,
0.04878246850510192,
0.011986246228615108
]
},
"knot_count_by_session": {
"priority_174005_174515": 62,
"priority_174905_175450": 69,
"priority_175910_180530": 76,
"slope_190548_190730": 21,
"circle_193412_193642": 30,
"loop_194223_195003": 92,
"accel_195608_195958": 46,
"motion_sms_154023_154359": 20
},
"loo_delta_m": {
"priority_174005_174515": [
-0.08631101169626099,
-0.16878866579372004,
-0.1969063529933237
],
"priority_174905_175450": [
0.052982832020427084,
0.23694572858391594,
0.5921699991558107
],
"priority_175910_180530": [
-0.03728037456994515,
-0.0587695607071852,
0.36745157813128415
],
"slope_190548_190730": [
0.06820599394639204,
0.13646009238200618,
0.8245922313333871
],
"circle_193412_193642": [
-0.06745036261863124,
-0.18645839932341524,
-0.8300513550738842
],
"loop_194223_195003": [
0.12045919921905746,
0.06572884705689208,
-1.774476169745249
],
"accel_195608_195958": [
0.019243586803303625,
-0.002985337002138544,
0.3653882998138158
],
"motion_sms_154023_154359": [
-0.0383153365081248,
0.027686850248512473,
0.507160468738487
]
},
"ok": false,
"notes": [
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
"upstream rotation is not accepted, so translation is diagnostic only",
"translation failed one or more strict acceptance gates"
]
}
@@ -0,0 +1,62 @@
{
"session_count": 3,
"sessions": [
{
"session_id": "priority_174005_174515",
"batch_id": "0808",
"imu_samples": 31000,
"rtk_samples": 4780,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.854602510460251,
"common_time_span_s": [
16265.2325992,
16575.0298107
],
"origin_geodetic": [
30.465514786,
114.092169888,
29.4695
],
"imu_source": "31000 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
},
{
"session_id": "priority_174905_175450",
"batch_id": "0808",
"imu_samples": 34499,
"rtk_samples": 5211,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8547303780464403,
"common_time_span_s": [
16805.1862481,
17150.0907328
],
"origin_geodetic": [
30.4653424457,
114.092237385,
29.5366
],
"imu_source": "34499 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
},
{
"session_id": "priority_175910_180530",
"batch_id": "0808",
"imu_samples": 37998,
"rtk_samples": 5786,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8567231247839613,
"common_time_span_s": [
17410.2121738,
17790.0458228
],
"origin_geodetic": [
30.4654151935,
114.090796384,
29.5317
],
"imu_source": "37998 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
}
]
}
@@ -0,0 +1,89 @@
{
"R_RTK_IMU": [
[
0.9996274297857513,
0.0272885099045028,
-0.0005821057674967719
],
[
-0.027285914629035343,
0.9996193516550798,
0.0040780705652228135
],
[
0.0006931686589101494,
-0.004060667909321538,
0.9999915152106744
]
],
"rpy_deg": [
-0.23265982849493025,
-0.03971564182674239,
-1.5635621825359411
],
"gyro_bias_by_session_rad_s": {
"priority_174005_174515": [
-9.355487703345326e-05,
6.022278219673199e-05,
0.0001599879605718389
],
"priority_174905_175450": [
-6.763782096065313e-05,
-0.00013271246982721386,
2.7304435743171578e-05
],
"priority_175910_180530": [
-4.9469524877665416e-05,
-0.0005605818923478396,
4.72341323992995e-06
]
},
"time_offset": {
"offset_s": 0.07500000000000034,
"peak_correlation": 0.2075894836191556,
"second_best_correlation": 0.2075830757517665,
"evaluated_samples": 6286,
"reliable": false
},
"convention": {
"name": "north_cw__pitch_nose_up__roll_right_down",
"heading_sign": -1.0,
"pitch_sign": -1.0,
"roll_sign": 1.0
},
"convention_scores_deg": {
"north_cw__pitch_nose_up__roll_right_down": 2.124836124403544,
"north_cw__pitch_opposite": 2.246244073697405,
"heading_opposite__pitch_nose_up": 14.081722937345578,
"heading_opposite__pitch_opposite": 14.187723789068325
},
"pair_count": 347,
"residual_rms_deg": 2.121956852743709,
"residual_median_deg": 0.6620410270629032,
"residual_p95_deg": 4.932454679529714,
"rotation_std_deg": [
0.5378381823216046,
0.43991569563643235,
3.604673882923754
],
"information_singular_values": [
17058.156967522238,
11392.471369813427,
252.6038959367189
],
"per_session_rms_deg": {
"priority_174005_174515": 0.6074812737155098,
"priority_174905_175450": 2.2498284381524494,
"priority_175910_180530": 3.09642157433063
},
"loo_delta_deg": {},
"ok": false,
"notes": [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
"residual time convention: t_IMU = t_RTK + +0.000000 s",
"GNHPR convention score gap=0.1214 deg",
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
"time-offset correlation was ambiguous; held residual offset at zero",
"rotation failed one or more strict acceptance gates"
]
}
@@ -0,0 +1,57 @@
{
"status": "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"R_RTK_IMU": [
[
0.9996274297857513,
0.0272885099045028,
-0.0005821057674967719
],
[
-0.027285914629035343,
0.9996193516550798,
0.0040780705652228135
],
[
0.0006931686589101494,
-0.004060667909321538,
0.9999915152106744
]
],
"t_RTK_IMU_m": [
1.017036626166409,
0.7992624136748191,
-22.250154010194304
],
"T_RTK_IMU": [
[
0.9996274297857513,
0.0272885099045028,
-0.0005821057674967719,
1.017036626166409
],
[
-0.027285914629035343,
0.9996193516550798,
0.0040780705652228135,
0.7992624136748191
],
[
0.0006931686589101494,
-0.004060667909321538,
0.9999915152106744,
-22.250154010194304
],
[
0.0,
0.0,
0.0,
1.0
]
],
"rotation_ok": false,
"translation_ok": false,
"rotation_result": "rotation_result.json",
"translation_result": "translation_result.json",
"dataset_audit": "dataset_audit.json"
}
@@ -0,0 +1,90 @@
{
"lever_IMU_to_RTK_in_IMU_m": [
-0.979425993211181,
-0.9170620761729408,
22.247297796687818
],
"t_RTK_IMU_m": [
1.017036626166409,
0.7992624136748191,
-22.250154010194304
],
"T_RTK_IMU": [
[
0.9996274297857513,
0.0272885099045028,
-0.0005821057674967719,
1.017036626166409
],
[
-0.027285914629035343,
0.9996193516550798,
0.0040780705652228135,
0.7992624136748191
],
[
0.0006931686589101494,
-0.004060667909321538,
0.9999915152106744,
-22.250154010194304
],
[
0.0,
0.0,
0.0,
1.0
]
],
"translation_std_m": [
0.6410402171684149,
0.6447113787519164,
3.8963000265795396
],
"lever_information_singular_values": [
2.4406600443109308,
2.412462748498746,
0.06586096829714262
],
"lever_precision_rank": 3,
"position_residual_rms_xyz_m": [
0.0023101392989052683,
0.002794889168410421,
0.0005246719035281052
],
"velocity_residual_rms_xyz_m_s": [
1.1059333756244145,
1.368704336610928,
0.11603076744633863
],
"accel_bias_by_session_m_s2": {
"priority_174005_174515": [
0.015165392523580496,
0.10623623265620695,
0.015221547657584966
],
"priority_174905_175450": [
0.06725309871307823,
0.06785033078439083,
0.0171641139816057
],
"priority_175910_180530": [
0.015302724485125336,
0.10160587655627046,
0.015281702682303734
]
},
"knot_count_by_session": {
"priority_174005_174515": 62,
"priority_174905_175450": 69,
"priority_175910_180530": 76
},
"loo_delta_m": {},
"ok": false,
"notes": [
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
"upstream rotation is not accepted, so translation is diagnostic only",
"translation failed one or more strict acceptance gates"
]
}
@@ -0,0 +1,81 @@
{
"session_count": 4,
"sessions": [
{
"session_id": "slope_190548_190730",
"batch_id": "0815",
"imu_samples": 10199,
"rtk_samples": 1578,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8757921419518377,
"common_time_span_s": [
36466.328875,
36568.2301441
],
"origin_geodetic": [
30.4652183602,
114.090839984,
29.9891
],
"imu_source": "10199 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
},
{
"session_id": "circle_193412_193642",
"batch_id": "0815",
"imu_samples": 14989,
"rtk_samples": 2162,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8288621646623496,
"common_time_span_s": [
38170.3385255,
38317.8571971
],
"origin_geodetic": [
30.4654799242,
114.092155912,
29.4779
],
"imu_source": "14989 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
},
{
"session_id": "loop_194223_195003",
"batch_id": "0815",
"imu_samples": 45801,
"rtk_samples": 6741,
"fixed_position_ratio": 0.9998516540572615,
"fixed_attitude_ratio": 0.8377095386441181,
"common_time_span_s": [
38661.2831251,
39121.1709553
],
"origin_geodetic": [
30.4654856957,
114.092151015,
29.4651
],
"imu_source": "45801 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
},
{
"session_id": "accel_195608_195958",
"batch_id": "0815",
"imu_samples": 23002,
"rtk_samples": 3456,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8457754629629629,
"common_time_span_s": [
39486.3574509,
39716.3199405
],
"origin_geodetic": [
30.465443014,
114.092179168,
29.5309
],
"imu_source": "23002 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
}
]
}
@@ -0,0 +1,96 @@
{
"R_RTK_IMU": [
[
0.9999453769836999,
0.010155336209751328,
-0.002472285459453144
],
[
-0.010163396322394882,
0.9999430054296535,
-0.003269750373547743
],
[
0.002438939138240274,
0.003294698586865735,
0.9999915982332558
]
],
"rpy_deg": [
0.18877322677308359,
-0.13974105765052158,
-0.5823314723802753
],
"gyro_bias_by_session_rad_s": {
"slope_190548_190730": [
5.517563410757097e-05,
-1.0668622945796592e-05,
0.00015474647141927933
],
"circle_193412_193642": [
-4.092656265416791e-05,
-0.00017676217236757262,
-5.914138553067106e-05
],
"loop_194223_195003": [
-0.000196149370561509,
-0.0001486662046316796,
-3.6375768145656054e-05
],
"accel_195608_195958": [
-7.483644204529091e-05,
-4.17320520562614e-06,
5.861303135056825e-05
]
},
"time_offset": {
"offset_s": 0.04000000000000031,
"peak_correlation": 0.7578237026149359,
"second_best_correlation": 0.757682618781891,
"evaluated_samples": 5443,
"reliable": false
},
"convention": {
"name": "north_cw__pitch_nose_up__roll_right_down",
"heading_sign": -1.0,
"pitch_sign": -1.0,
"roll_sign": 1.0
},
"convention_scores_deg": {
"north_cw__pitch_nose_up__roll_right_down": 0.9457675569035681,
"north_cw__pitch_opposite": 1.064327146928918,
"heading_opposite__pitch_nose_up": 5.753424726280977,
"heading_opposite__pitch_opposite": 13.362267974377751
},
"pair_count": 351,
"residual_rms_deg": 0.9428350359826574,
"residual_median_deg": 0.4993317818354644,
"residual_p95_deg": 1.5023594730593923,
"rotation_std_deg": [
0.191917244945292,
0.16245924204992213,
2.5710977852526464
],
"information_singular_values": [
125441.66767899474,
90572.5161990362,
496.5407001788204
],
"per_session_rms_deg": {
"slope_190548_190730": 2.386329048920402,
"circle_193412_193642": 0.560395991888806,
"loop_194223_195003": 0.6239401903763562,
"accel_195608_195958": 0.9797575195873669
},
"loo_delta_deg": {},
"ok": false,
"notes": [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
"residual time convention: t_IMU = t_RTK + +0.000000 s",
"GNHPR convention score gap=0.1186 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",
"time-offset correlation was ambiguous; held residual offset at zero",
"rotation failed one or more strict acceptance gates"
]
}
@@ -0,0 +1,28 @@
{
"status": "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"R_RTK_IMU": [
[
0.9999453769836999,
0.010155336209751328,
-0.002472285459453144
],
[
-0.010163396322394882,
0.9999430054296535,
-0.003269750373547743
],
[
0.002438939138240274,
0.003294698586865735,
0.9999915982332558
]
],
"t_RTK_IMU_m": null,
"T_RTK_IMU": null,
"rotation_ok": false,
"translation_ok": null,
"rotation_result": "rotation_result.json",
"translation_result": null,
"dataset_audit": "dataset_audit.json"
}
@@ -0,0 +1,157 @@
{
"session_count": 8,
"sessions": [
{
"session_id": "priority_174005_174515",
"batch_id": "0808",
"imu_samples": 31000,
"rtk_samples": 4780,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.854602510460251,
"common_time_span_s": [
16265.2325992,
16575.0298107
],
"origin_geodetic": [
30.465514786,
114.092169888,
29.4695
],
"imu_source": "31000 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
},
{
"session_id": "priority_174905_175450",
"batch_id": "0808",
"imu_samples": 34499,
"rtk_samples": 5211,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8547303780464403,
"common_time_span_s": [
16805.1862481,
17150.0907328
],
"origin_geodetic": [
30.4653424457,
114.092237385,
29.5366
],
"imu_source": "34499 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
},
{
"session_id": "priority_175910_180530",
"batch_id": "0808",
"imu_samples": 37998,
"rtk_samples": 5786,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8567231247839613,
"common_time_span_s": [
17410.2121738,
17790.0458228
],
"origin_geodetic": [
30.4654151935,
114.090796384,
29.5317
],
"imu_source": "37998 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
},
{
"session_id": "slope_190548_190730",
"batch_id": "0815",
"imu_samples": 10199,
"rtk_samples": 1578,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8757921419518377,
"common_time_span_s": [
36466.328875,
36568.2301441
],
"origin_geodetic": [
30.4652183602,
114.090839984,
29.9891
],
"imu_source": "10199 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
},
{
"session_id": "circle_193412_193642",
"batch_id": "0815",
"imu_samples": 14989,
"rtk_samples": 2162,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8288621646623496,
"common_time_span_s": [
38170.3385255,
38317.8571971
],
"origin_geodetic": [
30.4654799242,
114.092155912,
29.4779
],
"imu_source": "14989 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
},
{
"session_id": "loop_194223_195003",
"batch_id": "0815",
"imu_samples": 45801,
"rtk_samples": 6741,
"fixed_position_ratio": 0.9998516540572615,
"fixed_attitude_ratio": 0.8377095386441181,
"common_time_span_s": [
38661.2831251,
39121.1709553
],
"origin_geodetic": [
30.4654856957,
114.092151015,
29.4651
],
"imu_source": "45801 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
},
{
"session_id": "accel_195608_195958",
"batch_id": "0815",
"imu_samples": 23002,
"rtk_samples": 3456,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8457754629629629,
"common_time_span_s": [
39486.3574509,
39716.3199405
],
"origin_geodetic": [
30.465443014,
114.092179168,
29.5309
],
"imu_source": "23002 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
},
{
"session_id": "motion_sms_154023_154359",
"batch_id": "0819",
"imu_samples": 21556,
"rtk_samples": 3294,
"fixed_position_ratio": 0.49271402550091076,
"fixed_attitude_ratio": 0.4344262295081967,
"common_time_span_s": [
25829.8725525,
26027.4217529
],
"origin_geodetic": [
30.4651580863,
114.090773052,
36.5626
],
"imu_source": "21556 normalized samples",
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
}
]
}
@@ -0,0 +1,119 @@
{
"R_RTK_IMU": [
[
0.9996841792313951,
0.02499811511405725,
-0.002576050309343804
],
[
-0.025002617750669198,
0.9996858874629868,
-0.0017307550243271697
],
[
0.002531975526313299,
0.0017946164171362589,
0.9999951842143289
]
],
"rpy_deg": [
0.1028243313393031,
-0.1450716664951083,
-1.43269836393699
],
"gyro_bias_by_session_rad_s": {
"priority_174005_174515": [
-8.90944066727157e-05,
7.078609864979291e-05,
0.0001460390779870233
],
"priority_174905_175450": [
-4.327374730750894e-05,
-0.0001097548390022348,
3.000549725593387e-05
],
"priority_175910_180530": [
-7.036551812132934e-05,
-0.0008054961048184173,
1.3026193503057623e-05
],
"slope_190548_190730": [
6.035222904568755e-05,
-8.913884157613711e-06,
0.00015470949872434692
],
"circle_193412_193642": [
-3.696799268832588e-05,
-0.00019200076224600124,
-5.9567987995056394e-05
],
"loop_194223_195003": [
-0.0001979655352189002,
-0.00015937450336643818,
-4.343089303706036e-05
],
"accel_195608_195958": [
-7.841839817698704e-05,
-2.604932294598783e-06,
5.8586555655011475e-05
],
"motion_sms_154023_154359": [
0.00011745666467620586,
0.00021434926285955486,
-4.5147354343760625e-05
]
},
"time_offset": {
"offset_s": 0.04000000000000031,
"peak_correlation": 0.5788788671828708,
"second_best_correlation": 0.5773967219219845,
"evaluated_samples": 12394,
"reliable": false
},
"convention": {
"name": "north_cw__pitch_nose_up__roll_right_down",
"heading_sign": -1.0,
"pitch_sign": -1.0,
"roll_sign": 1.0
},
"convention_scores_deg": {
"north_cw__pitch_nose_up__roll_right_down": 1.631322241364994,
"north_cw__pitch_opposite": 1.737582794831228,
"heading_opposite__pitch_nose_up": 7.263259116648845,
"heading_opposite__pitch_opposite": 17.716319437929055
},
"pair_count": 731,
"residual_rms_deg": 1.6276839301413086,
"residual_median_deg": 0.5695938849052221,
"residual_p95_deg": 3.0903953005641345,
"rotation_std_deg": [
0.2340126126406699,
0.20048574131667432,
2.314692068045375
],
"information_singular_values": [
82178.77940373585,
60441.86300479194,
612.6358640387364
],
"per_session_rms_deg": {
"priority_174005_174515": 0.6134232617562615,
"priority_174905_175450": 2.2485932625979106,
"priority_175910_180530": 3.095861798757508,
"slope_190548_190730": 2.388478834012087,
"circle_193412_193642": 0.5629885032374518,
"loop_194223_195003": 0.6250976606325254,
"accel_195608_195958": 0.975856888271022,
"motion_sms_154023_154359": 1.3698317592842066
},
"loo_delta_deg": {},
"ok": false,
"notes": [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
"residual time convention: t_IMU = t_RTK + +0.000000 s",
"GNHPR convention score gap=0.1063 deg",
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
"time-offset correlation was ambiguous; held residual offset at zero",
"rotation failed one or more strict acceptance gates"
]
}
@@ -0,0 +1,28 @@
{
"status": "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"R_RTK_IMU": [
[
0.9996841792313951,
0.02499811511405725,
-0.002576050309343804
],
[
-0.025002617750669198,
0.9996858874629868,
-0.0017307550243271697
],
[
0.002531975526313299,
0.0017946164171362589,
0.9999951842143289
]
],
"t_RTK_IMU_m": null,
"T_RTK_IMU": null,
"rotation_ok": false,
"translation_ok": null,
"rotation_result": "rotation_result.json",
"translation_result": null,
"dataset_audit": "dataset_audit.json"
}
@@ -0,0 +1,157 @@
{
"session_count": 8,
"sessions": [
{
"session_id": "priority_174005_174515",
"batch_id": "0808",
"imu_samples": 31000,
"rtk_samples": 4780,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.854602510460251,
"common_time_span_s": [
16265.2325992,
16575.0298107
],
"origin_geodetic": [
30.465514786,
114.092169888,
29.4695
],
"imu_source": "31000 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
},
{
"session_id": "priority_174905_175450",
"batch_id": "0808",
"imu_samples": 34499,
"rtk_samples": 5211,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8547303780464403,
"common_time_span_s": [
16805.1862481,
17150.0907328
],
"origin_geodetic": [
30.4653424457,
114.092237385,
29.5366
],
"imu_source": "34499 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
},
{
"session_id": "priority_175910_180530",
"batch_id": "0808",
"imu_samples": 37998,
"rtk_samples": 5786,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8567231247839613,
"common_time_span_s": [
17410.2121738,
17790.0458228
],
"origin_geodetic": [
30.4654151935,
114.090796384,
29.5317
],
"imu_source": "37998 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
},
{
"session_id": "slope_190548_190730",
"batch_id": "0815",
"imu_samples": 10199,
"rtk_samples": 1578,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8757921419518377,
"common_time_span_s": [
36466.328875,
36568.2301441
],
"origin_geodetic": [
30.4652183602,
114.090839984,
29.9891
],
"imu_source": "10199 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
},
{
"session_id": "circle_193412_193642",
"batch_id": "0815",
"imu_samples": 14989,
"rtk_samples": 2162,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8288621646623496,
"common_time_span_s": [
38170.3385255,
38317.8571971
],
"origin_geodetic": [
30.4654799242,
114.092155912,
29.4779
],
"imu_source": "14989 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
},
{
"session_id": "loop_194223_195003",
"batch_id": "0815",
"imu_samples": 45801,
"rtk_samples": 6741,
"fixed_position_ratio": 0.9998516540572615,
"fixed_attitude_ratio": 0.8377095386441181,
"common_time_span_s": [
38661.2831251,
39121.1709553
],
"origin_geodetic": [
30.4654856957,
114.092151015,
29.4651
],
"imu_source": "45801 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
},
{
"session_id": "accel_195608_195958",
"batch_id": "0815",
"imu_samples": 23002,
"rtk_samples": 3456,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8457754629629629,
"common_time_span_s": [
39486.3574509,
39716.3199405
],
"origin_geodetic": [
30.465443014,
114.092179168,
29.5309
],
"imu_source": "23002 normalized samples",
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
},
{
"session_id": "motion_sms_154023_154359",
"batch_id": "0819",
"imu_samples": 21556,
"rtk_samples": 3294,
"fixed_position_ratio": 0.49271402550091076,
"fixed_attitude_ratio": 0.4344262295081967,
"common_time_span_s": [
25829.8725525,
26027.4217529
],
"origin_geodetic": [
30.4651580863,
114.090773052,
36.5626
],
"imu_source": "21556 normalized samples",
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
}
]
}
@@ -0,0 +1,119 @@
{
"R_RTK_IMU": [
[
-0.999754381938718,
0.021803011844914608,
0.003975483470215314
],
[
0.021793866945340735,
0.9997597719617833,
-0.0023293197487744975
],
[
-0.0040253146336934244,
-0.002242106467980424,
-0.9999893848440022
]
],
"rpy_deg": [
-179.8715356137635,
0.23063416255936714,
178.75119441523574
],
"gyro_bias_by_session_rad_s": {
"priority_174005_174515": [
-0.00018079953472427648,
-9.3706764772758e-05,
-1.6471539455612585e-05
],
"priority_174905_175450": [
-0.0001527801061740737,
-0.0001164065722445753,
-5.5002757975692905e-05
],
"priority_175910_180530": [
0.00010732176614734042,
-9.217694744503191e-05,
0.00037171055222583177
],
"slope_190548_190730": [
-0.0001400513048024026,
-9.490085614046164e-05,
-7.313488292136488e-05
],
"circle_193412_193642": [
-5.9488404294769446e-05,
-0.00029434501957892675,
0.000102661594942897
],
"loop_194223_195003": [
-0.0002980828626939094,
9.137138703447405e-05,
4.430930647242609e-05
],
"accel_195608_195958": [
5.9388956272647935e-05,
9.900621296014341e-06,
8.065768586636302e-05
],
"motion_sms_154023_154359": [
0.0016442968972331072,
0.00012387305081956675,
8.435528364463815e-08
]
},
"time_offset": {
"offset_s": 0.08500000000000035,
"peak_correlation": 0.46191352508231565,
"second_best_correlation": 0.460889710037625,
"evaluated_samples": 12384,
"reliable": false
},
"convention": {
"name": "heading_opposite__pitch_nose_up",
"heading_sign": 1.0,
"pitch_sign": -1.0,
"roll_sign": 1.0
},
"convention_scores_deg": {
"north_cw__pitch_nose_up__roll_right_down": 1.6671304616155285,
"north_cw__pitch_opposite": 1.7955511909491206,
"heading_opposite__pitch_nose_up": 1.6669796415371239,
"heading_opposite__pitch_opposite": 19.709790964275243
},
"pair_count": 6164,
"residual_rms_deg": 1.6669796415371239,
"residual_median_deg": 0.6097138083592458,
"residual_p95_deg": 2.9952512236242987,
"rotation_std_deg": [
1.0204560247927847,
0.06460681216547948,
0.11838973994567728
],
"information_singular_values": [
818353.8002481281,
235774.70975965378,
3151.7390576048515
],
"per_session_rms_deg": {
"priority_174005_174515": 0.7068100152136895,
"priority_174905_175450": 1.7590065687347056,
"priority_175910_180530": 3.0674316757114104,
"slope_190548_190730": 2.07580304513421,
"circle_193412_193642": 0.6574485751543728,
"loop_194223_195003": 0.6594363752510715,
"accel_195608_195958": 0.7631418065216562,
"motion_sms_154023_154359": 3.709386878989403
},
"loo_delta_deg": {},
"ok": false,
"notes": [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
"residual time convention: t_IMU = t_RTK + +0.000000 s",
"GNHPR convention score gap=0.0002 deg",
"time-offset correlation was ambiguous; held residual offset at zero",
"empirical best GNHPR convention differs from protocol expectation; manual verification required",
"rotation failed one or more strict acceptance gates"
]
}
@@ -0,0 +1,28 @@
{
"status": "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"R_RTK_IMU": [
[
-0.999754381938718,
0.021803011844914608,
0.003975483470215314
],
[
0.021793866945340735,
0.9997597719617833,
-0.0023293197487744975
],
[
-0.0040253146336934244,
-0.002242106467980424,
-0.9999893848440022
]
],
"t_RTK_IMU_m": null,
"T_RTK_IMU": null,
"rotation_ok": false,
"translation_ok": null,
"rotation_result": "rotation_result.json",
"translation_result": null,
"dataset_audit": "dataset_audit.json"
}
@@ -0,0 +1,24 @@
{
"session_count": 1,
"sessions": [
{
"session_id": "priority_175910_180530",
"batch_id": "0808",
"imu_samples": 37998,
"rtk_samples": 5786,
"fixed_position_ratio": 1.0,
"fixed_attitude_ratio": 0.8567231247839613,
"common_time_span_s": [
17410.2121738,
17790.0458228
],
"origin_geodetic": [
30.4654151935,
114.090796384,
29.5317
],
"imu_source": "37998 normalized samples",
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
}
]
}
@@ -0,0 +1,76 @@
{
"R_RTK_IMU": [
[
0.9974493913373024,
-0.07135460605626225,
0.0017977528752911507
],
[
0.0713431523484532,
0.997435050761847,
0.005785682733905998
],
[
-0.0022059768426676615,
-0.00564266836413856,
0.9999816468115312
]
],
"rpy_deg": [
-0.32330358477785043,
0.1263932653005637,
4.0911470587568495
],
"gyro_bias_by_session_rad_s": {
"priority_175910_180530": [
-0.00010509442179307857,
-0.0001426783269500731,
3.086292346981605e-05
]
},
"time_offset": {
"offset_s": -0.0949999999999998,
"peak_correlation": 0.1329249352233157,
"second_best_correlation": 0.13247995527618098,
"evaluated_samples": 2316,
"reliable": false
},
"convention": {
"name": "north_cw__pitch_nose_up__roll_right_down",
"heading_sign": -1.0,
"pitch_sign": -1.0,
"roll_sign": 1.0
},
"convention_scores_deg": {
"north_cw__pitch_nose_up__roll_right_down": 2.8406184024910064,
"north_cw__pitch_opposite": 2.8942770966870084,
"heading_opposite__pitch_nose_up": 3.8836333647028383,
"heading_opposite__pitch_opposite": 8.244842702849073
},
"pair_count": 91,
"residual_rms_deg": 2.8406184024910064,
"residual_median_deg": 1.624215282655001,
"residual_p95_deg": 6.087913500787023,
"rotation_std_deg": [
1.6029337786454214,
1.0761604678920118,
6.305203755276785
],
"information_singular_values": [
2941.184567996004,
1268.1548949190246,
82.52753961323997
],
"per_session_rms_deg": {
"priority_175910_180530": 2.8406184024910064
},
"loo_delta_deg": {},
"ok": false,
"notes": [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
"residual time convention: t_IMU = t_RTK + +0.000000 s",
"GNHPR convention score gap=0.0537 deg",
"time-offset correlation was ambiguous; held residual offset at zero",
"rotation failed one or more strict acceptance gates"
]
}
@@ -0,0 +1,28 @@
{
"status": "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"R_RTK_IMU": [
[
0.9974493913373024,
-0.07135460605626225,
0.0017977528752911507
],
[
0.0713431523484532,
0.997435050761847,
0.005785682733905998
],
[
-0.0022059768426676615,
-0.00564266836413856,
0.9999816468115312
]
],
"t_RTK_IMU_m": null,
"T_RTK_IMU": null,
"rotation_ok": false,
"translation_ok": null,
"rotation_result": "rotation_result.json",
"translation_result": null,
"dataset_audit": "dataset_audit.json"
}
+52
View File
@@ -0,0 +1,52 @@
# RTKIMU 候选数据清点(2026-08-20
本目录记录 0808、0815、0819 三批 LiDAR/IMU 会话所对应的 G90 RTK 原始记录与当前导出状态。此处只做数据血缘和可用性评估,尚未求解 `T_RTK_IMU`
## 时间与质量约定
- 原始 RTK 为 Wheeltec G90 V2 `.rscap`,包含 `$GNGGA/$GPGGA` 位置和 `$GNHPR` heading/pitch/roll。
- 切窗使用 NMEA 报文自带的测量 UTC,再通过每个会话的 IMU device→host affine clock 映射到 IMU 设备时间。
- 主机接收时间比 NMEA 测量时间晚约 3–4 s,且有波动;不能按接收时间直接切窗。
- 当前项目门控按 GGA `fix_quality=4` 和 HPR `heading_quality∈{4,5}` 判断固定位置/有效航向。
- `heading_valid` 未覆盖全部 GGA 行主要因为 GGA 与 HPR 频率不同、最近邻匹配阈值为 80 ms;不代表该会话航向整体失效。
## 数据映射与质量
详细机器可读清单见 `rtk_session_inventory.csv`
| 会话 | RTK结论 | 适合的标定作用 |
| --- | --- | --- |
| `priority_174005_174515` | 100% GGA质量4;有效航向约309 syaw变化约485° | 多圈 yaw 与 XY 杠杆臂 |
| `priority_174905_175450` | 100% GGA质量4Pitch跨度约11.4°;XY约58×22 m | 强候选:yaw、pitch、XY/Z耦合解除 |
| `priority_175910_180530` | 100% GGA质量4Pitch跨度约13.6°;XY约144×59 m | 最强候选:长基线、pitch、平移 |
| `slope_190548_190730` | 100% GGA质量4Pitch跨度约7.4°;约102 s | 坡度/Pitch补充 |
| `circle_193412_193642` | 100% GGA质量4yaw变化约445° | 平面旋转与XY杠杆臂 |
| `loop_194223_195003` | 仅1个异常GGAyaw累计变化约1203°;约460 s | 最强 yaw/多圈转弯候选 |
| `accel_195608_195958` | 100% GGA质量4XY约46×23 m;约230 s | 加减速、速度和水平杠杆臂 |
| `motion_sms_154023_154359` | 全窗仅约49%为质量4;可用连续子段约100 s | 仅用15:41:28.215:43:08.15固定解/有效航向段 |
## 导出状态
- 0808 当前 `sessions_v2_device_affine` 原先没有 RTK CSV,本次已从原始 G90 `.rscap` 按 NMEA 测量 UTC补导三个会话,未覆盖旧文件。
- 0815 `sessions_v2_device_affine` 与 dense5 slope 已经采用同一测量时间导出规则,无需重导。
- 0819 `sessions_v2_device_affine` 与 dense5 motion 已经采用同一规则;需要在求解器中按质量与时间连续段过滤,而不是重新解释为全窗固定解。
- 0808 旧 `sessions_v1_host_aligned_00` RTK CSV 不含 `t_measurement_utc_s` 字段,只保留作历史对照;新求解应使用 `sessions_v2_device_affine`
## 已发现的 LiDARRTK 资料边界
`D:\data\calibration_usable_20260808\rtk_lidar_station_report*` 保存的是静止站点候选:27个站点、29个候选段,并非包含 `T_RTK_lidar`、协方差和留一验证的正式手眼结果。本轮在 0808 数据目录的 JSON/YAML/Markdown/CSV/日志中没有找到 `T_RTK_lidar` 矩阵。若要通过链式关系得到 LiDAR–IMU,需要继续定位原手眼结果及其坐标约定:
```text
T_IMU_lidar = inverse(T_RTK_IMU) @ T_RTK_lidar
```
## 初步可行性判断
这些数据足以启动直接 RTK–IMU 标定,且比当前纯 LiDARIMU Phase-B 更有希望约束 XYRTK 提供绝对位置,GNHPR 提供航向和 Pitch,多会话包含长基线、转弯、加减速与坡度。仍需注意:
1. GNHPR roll 的变化仅约0.006°–0.065°,不能指望它提供有效 roll 激励。
2. 应先用 RTK heading/pitch 角速度与 IMU gyro 做残余时间偏置和坐标轴验证,再求旋转。
3. 平移应使用 RTK绝对位置 + IMU预积分的联合状态模型,估计共享 `T_RTK_IMU`、每会话速度/bias;不应把RTK轨迹简单二次差分后直接最小二乘。
4. `motion_sms` 必须仅使用其连续固定解子段。
5. 跨0808/0815/0819时应使用每会话IMU bias,外参共享,并检查安装期间是否发生机械变动。
@@ -0,0 +1,9 @@
session,batch,raw_rtk_rscap,current_rtk_csv,rows,fixed_gga_ratio,fixed_heading_valid_rows,valid_duration_s,xy_robust_span_x_m,xy_robust_span_y_m,altitude_robust_span_m,heading_unwrapped_span_deg,pitch_robust_span_deg,roll_robust_span_deg,notes
priority_174005_174515,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174005_174515\rtk.csv,4780,1.0000,4085,309.45,12.045,17.380,0.112,485.411,3.462,0.012,newly exported from NMEA measurement UTC
priority_174905_175450,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174905_175450\rtk.csv,5211,1.0000,4454,344.90,58.436,21.923,0.277,350.616,11.379,0.008,newly exported from NMEA measurement UTC
priority_175910_180530,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_175910_180530\rtk.csv,5786,1.0000,4957,379.85,143.775,58.657,0.971,260.213,13.574,0.065,newly exported from NMEA measurement UTC
slope_190548_190730,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-110519.251_e0edf32b-c82b-4a38-b5df-2c0e4cac364f.rscap,D:\data\0815\sessions_v2_device_affine\slope_190548_190730\rtk.csv,1578,1.0000,1382,101.90,11.354,18.567,0.714,155.356,7.399,0.026,root is named 0815 but raw measurement date is 2026-08-14 local
circle_193412_193642,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-113355.356_b4b37794-d4d6-433e-87e9-037cad5517d1.rscap,D:\data\0815\sessions_v2_device_affine\circle_193412_193642\rtk.csv,2162,1.0000,1792,147.25,10.105,10.277,0.097,444.994,2.834,0.006,raw capture has truncated tail but target messages are checksum-valid
loop_194223_195003,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-114200.914_36911c82-f8c0-451b-99e3-f5b663ec6115.rscap,D:\data\0815\sessions_v2_device_affine\loop_194223_195003\rtk.csv,6741,0.9999,5647,459.90,11.555,18.800,0.104,1202.601,2.727,0.009,one malformed/non-fixed GGA excluded
accel_195608_195958,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-115547.472_8722f326-8314-4db1-9ec4-bce185c54a78.rscap,D:\data\0815\sessions_v2_device_affine\accel_195608_195958\rtk.csv,3456,1.0000,2923,229.90,45.542,22.610,0.130,188.236,3.248,0.010,measurement-time export already present
motion_sms_154023_154359,0819,D:\data\0819\raw_serial_capture_v2\wheeltec-g90_20260819-074023.329_3d9da6eb-7ef8-4f7b-9262-9193328238b0.rscap,D:\data\0819\dense5\sessions_v2_device_affine\motion_sms_154023_154359\rtk.csv,3294,0.4924,1431,99.95,12.679,17.356,0.839,308.864,8.985,0.020,use only 15:41:28.200-15:43:08.150 fixed+valid sub-window
1 session batch raw_rtk_rscap current_rtk_csv rows fixed_gga_ratio fixed_heading_valid_rows valid_duration_s xy_robust_span_x_m xy_robust_span_y_m altitude_robust_span_m heading_unwrapped_span_deg pitch_robust_span_deg roll_robust_span_deg notes
2 priority_174005_174515 0808 D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174005_174515\rtk.csv 4780 1.0000 4085 309.45 12.045 17.380 0.112 485.411 3.462 0.012 newly exported from NMEA measurement UTC
3 priority_174905_175450 0808 D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174905_175450\rtk.csv 5211 1.0000 4454 344.90 58.436 21.923 0.277 350.616 11.379 0.008 newly exported from NMEA measurement UTC
4 priority_175910_180530 0808 D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_175910_180530\rtk.csv 5786 1.0000 4957 379.85 143.775 58.657 0.971 260.213 13.574 0.065 newly exported from NMEA measurement UTC
5 slope_190548_190730 0815 D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-110519.251_e0edf32b-c82b-4a38-b5df-2c0e4cac364f.rscap D:\data\0815\sessions_v2_device_affine\slope_190548_190730\rtk.csv 1578 1.0000 1382 101.90 11.354 18.567 0.714 155.356 7.399 0.026 root is named 0815 but raw measurement date is 2026-08-14 local
6 circle_193412_193642 0815 D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-113355.356_b4b37794-d4d6-433e-87e9-037cad5517d1.rscap D:\data\0815\sessions_v2_device_affine\circle_193412_193642\rtk.csv 2162 1.0000 1792 147.25 10.105 10.277 0.097 444.994 2.834 0.006 raw capture has truncated tail but target messages are checksum-valid
7 loop_194223_195003 0815 D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-114200.914_36911c82-f8c0-451b-99e3-f5b663ec6115.rscap D:\data\0815\sessions_v2_device_affine\loop_194223_195003\rtk.csv 6741 0.9999 5647 459.90 11.555 18.800 0.104 1202.601 2.727 0.009 one malformed/non-fixed GGA excluded
8 accel_195608_195958 0815 D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-115547.472_8722f326-8314-4db1-9ec4-bce185c54a78.rscap D:\data\0815\sessions_v2_device_affine\accel_195608_195958\rtk.csv 3456 1.0000 2923 229.90 45.542 22.610 0.130 188.236 3.248 0.010 measurement-time export already present
9 motion_sms_154023_154359 0819 D:\data\0819\raw_serial_capture_v2\wheeltec-g90_20260819-074023.329_3d9da6eb-7ef8-4f7b-9262-9193328238b0.rscap D:\data\0819\dense5\sessions_v2_device_affine\motion_sms_154023_154359\rtk.csv 3294 0.4924 1431 99.95 12.679 17.356 0.839 308.864 8.985 0.020 use only 15:41:28.200-15:43:08.150 fixed+valid sub-window
+72
View File
@@ -0,0 +1,72 @@
# RTKIMU 独立标定链
本分支将 RTKIMU 标定与原 LiDARIMU Phase-A/B/C 隔离。只复用 SO(3)/SE(3)、IMU 数据契约和预积分数学,不复用 LiDAR 命名的结果对象或门限。
## 坐标与外参约定
唯一输出约定为 `p_RTK = R_RTK_IMU p_IMU + t_RTK_IMU`,即 `T_RTK_IMU` 把 IMU 坐标变换到 RTK 传感器坐标。杆臂 `l_IMU_to_RTK_in_IMU` 是“IMU 原点指向 RTK 定位参考点”的向量,表达在 IMU 坐标中,因此 `t_RTK_IMU = -R_RTK_IMU @ l_IMU_to_RTK_in_IMU`
世界坐标采用局部 ENU。GGA 的第一个有效固定解作为每会话 ENU 原点。GGA 高程在局部差分中使用;因为设备输出的高程基准尚未单独核验,Z 方向使用更大的噪声并执行独立门控。
当前 GNHPR 解释候选把 heading 看作从真北顺时针,pitch 看作机头上仰为正,并按 Z-Y-X 生成 `R_ENU_RTK`。代码同时检查 heading/pitch 的反号候选。固定的 RTK 轴置换不能由手眼残差自行识别,因此 `frame_definition``reference_point` 为空时,最终状态必须是 diagnostic。
## 数据与时间规则
- IMU CSV`t,gx,gy,gz,ax,ay,az`,陀螺为 rad/s,加速度为 m/s²。
- RTK CSV:位置使用 GGA,姿态使用 GNHPR;仅接受位置质量 4/5、姿态质量 4/5。
- RTK 的 `t` 必须是 NMEA 测量 UTC 经 IMU device→host affine clock 反变换得到的 IMU 设备时间。禁止回退到延迟约 3–4 s 的主机接收时间。
- 姿态使用 `hpr_measurement_utc_s` 恢复的 HPR 测量时间,不挂在最近邻 GGA 时刻。这一修正把 `motion_sms` 的旋转 RMS 从约 4.60° 降到约 1.37°。
- 残余时间偏移通过 RTK/IMU 角速度模长相关扫描。相关峰不唯一时保持 0 s,并把候选峰只作为诊断值。
## 求解方法
R0 数据和时间审计:检查固定解比例、共同时间范围、HPR 有效性、ENU 原点和残余时间偏移。`motion_sms` 自动只使用固定位置且有效姿态的连续段。
R1 旋转:对 0.75 s、1.5 s、3.0 s 相对运动构造 `Delta_R_RTK R_RTK_IMU = R_RTK_IMU Delta_R_IMU`。IMU 侧采用陀螺预积分。先用无偏置闭式手眼解筛选四种 GNHPR 符号候选,只对最优候选联合优化共享旋转和每会话常值陀螺偏置。输出 RMS/median/P95、边缘协方差、信息奇异值、逐会话 RMS 和固定偏置 conditional LOO。
T1 杆臂和平移:不对 RTK 位置做二次差分。固定 R1 的旋转和陀螺偏置,以可配置节点构造 IMU 位置/速度预积分因子,联合求解跨会话共享杆臂、每节点速度和每会话常值加速度偏置。输出边缘协方差、信息奇异值/精度秩、位置/速度残差和 LOO。上游旋转未通过时,平移无条件标为 diagnostic。
## 代码结构
- `imu_lidar/geodesy.py`WGS84 → ECEF → 局部 ENU。
- `imu_lidar/rtk_io.py`RTK CSV、质量掩码、GGA/HPR 独立测量时刻。
- `imu_lidar/rtk_attitude.py`GNHPR 候选约定与 SO(3) 插值。
- `imu_lidar/rtk_imu_rotation.py`:时间审计、旋转手眼、偏置、协方差、LOO。
- `imu_lidar/rtk_imu_translation.py`:轻量全 IMU 预积分、杆臂线性因子系统、协方差、LOO。
- `imu_lidar/rtk_imu_replay.py`:清单加载、端到端运行、结构化结果发布。
- `tools/run_rtk_imu_calibration.py`:命令行入口,支持会话/批次筛选和 rotation-only。
- `tests/test_rtk_imu_calibration.py`ENU、heading 方向、HPR 时刻、预积分等价性测试。
公共 IMU 预积分的端点插值已从每小段扫描完整数组,改为当前相邻样本内的等价局部插值;公共 IMU CSV 加载器也改为先校验表头、再按列加载。完整项目测试通过。
## 当前结果(8 会话,2026-08-21
结果目录:`artifacts/rtk_imu_calibration_v1/all_sessions`
```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
```
预期 GNHPR 符号候选胜出,预筛 RMS 1.6313°;pitch 反号为 1.7376°,heading 反号候选为 7.2633°/17.7163°。heading 方向基本可判定,但固定 RTK 轴定义仍未判定。
平移明显不具机械真实性。很小的位置残差来自节点速度自由度和鲁棒权重吸收,不能抵消约 1 m/s 的水平速度残差、米级标准差与巨大 LOO 变化。该候选禁止写入配置。
逐日期旋转:0808 为 `[-0.232660, -0.039716, -1.563562]°`、RMS 2.121957°;0815 为 `[0.188773, -0.139741, -0.582331]°`、RMS 0.942835°。两批 yaw 相差约 0.98°,但 yaw 自身标准差约 3.60°/2.57°,不能据此声称安装发生变化。
## 当前阻断项与下一步
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/留出验证。
+74
View File
@@ -0,0 +1,74 @@
"""Small WGS84 geodesy helpers used by the RTK--IMU calibration path."""
from __future__ import annotations
import numpy as np
WGS84_A_M = 6378137.0
WGS84_F = 1.0 / 298.257223563
WGS84_E2 = WGS84_F * (2.0 - WGS84_F)
def geodetic_to_ecef(
latitude_deg: np.ndarray,
longitude_deg: np.ndarray,
altitude_m: np.ndarray,
) -> np.ndarray:
"""Convert WGS84 latitude/longitude/ellipsoidal height to ECEF metres."""
latitude = np.deg2rad(np.asarray(latitude_deg, dtype=float))
longitude = np.deg2rad(np.asarray(longitude_deg, dtype=float))
altitude = np.asarray(altitude_m, dtype=float)
latitude, longitude, altitude = np.broadcast_arrays(latitude, longitude, altitude)
sin_lat = np.sin(latitude)
cos_lat = np.cos(latitude)
radius = WGS84_A_M / np.sqrt(1.0 - WGS84_E2 * sin_lat**2)
x = (radius + altitude) * cos_lat * np.cos(longitude)
y = (radius + altitude) * cos_lat * np.sin(longitude)
z = (radius * (1.0 - WGS84_E2) + altitude) * sin_lat
return np.stack([x, y, z], axis=-1)
def geodetic_to_enu(
latitude_deg: np.ndarray,
longitude_deg: np.ndarray,
altitude_m: np.ndarray,
*,
origin_latitude_deg: float | None = None,
origin_longitude_deg: float | None = None,
origin_altitude_m: float | None = None,
) -> tuple[np.ndarray, tuple[float, float, float]]:
"""Convert WGS84 samples to a local east/north/up frame.
When no origin is supplied, the first finite sample is used. The returned
origin tuple is ``(latitude_deg, longitude_deg, altitude_m)``.
"""
lat = np.asarray(latitude_deg, dtype=float).reshape(-1)
lon = np.asarray(longitude_deg, dtype=float).reshape(-1)
alt = np.asarray(altitude_m, dtype=float).reshape(-1)
if not (lat.size == lon.size == alt.size):
raise ValueError("latitude, longitude and altitude must have equal length")
finite = np.isfinite(lat) & np.isfinite(lon) & np.isfinite(alt)
if not np.any(finite):
raise ValueError("no finite geodetic sample")
first = int(np.flatnonzero(finite)[0])
lat0 = float(lat[first] if origin_latitude_deg is None else origin_latitude_deg)
lon0 = float(lon[first] if origin_longitude_deg is None else origin_longitude_deg)
alt0 = float(alt[first] if origin_altitude_m is None else origin_altitude_m)
ecef = geodetic_to_ecef(lat, lon, alt)
ecef0 = geodetic_to_ecef(np.array(lat0), np.array(lon0), np.array(alt0)).reshape(3)
delta = ecef - ecef0
phi = np.deg2rad(lat0)
lam = np.deg2rad(lon0)
rotation = np.array(
[
[-np.sin(lam), np.cos(lam), 0.0],
[-np.sin(phi) * np.cos(lam), -np.sin(phi) * np.sin(lam), np.cos(phi)],
[np.cos(phi) * np.cos(lam), np.cos(phi) * np.sin(lam), np.sin(phi)],
],
dtype=float,
)
return delta @ rotation.T, (lat0, lon0, alt0)
+12 -10
View File
@@ -15,6 +15,7 @@ Accepted inputs
from __future__ import annotations
import csv
from pathlib import Path
import numpy as np
@@ -36,16 +37,17 @@ def load_imu_samples(path: Path | str) -> ImuSeries:
def _load_imu_csv(path: Path) -> ImuSeries:
data = np.genfromtxt(path, delimiter=",", names=True, dtype=float)
if data.ndim == 0:
data = np.array([data])
names = set(data.dtype.names or ())
required = {"t", "gx", "gy", "gz", "ax", "ay", "az"}
if not required.issubset(names):
raise ValueError(f"IMU CSV must contain columns {sorted(required)}, got {sorted(names)}")
t = np.asarray(data["t"], dtype=float).reshape(-1)
gyro = np.column_stack([data["gx"], data["gy"], data["gz"]]).astype(float)
acc = np.column_stack([data["ax"], data["ay"], data["az"]]).astype(float)
required_order = ["t", "gx", "gy", "gz", "ax", "ay", "az"]
with path.open("r", encoding="utf-8-sig", newline="") as handle:
header = next(csv.reader(handle), [])
names = set(header)
if not set(required_order).issubset(names):
raise ValueError(f"IMU CSV must contain columns {sorted(required_order)}, got {sorted(names)}")
usecols = [header.index(name) for name in required_order]
data = np.loadtxt(path, delimiter=",", skiprows=1, usecols=usecols, ndmin=2)
t = np.asarray(data[:, 0], dtype=float).reshape(-1)
gyro = np.asarray(data[:, 1:4], dtype=float)
acc = np.asarray(data[:, 4:7], dtype=float)
order = np.argsort(t)
return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
+15 -7
View File
@@ -158,9 +158,14 @@ def preintegrate_gyro(
if dt <= 0:
continue
# Exact endpoint gyro via linear interpolation inside the sample interval.
g_a = _interp_gyro(times_s, gyro_rad_s, seg0)
g_b = _interp_gyro(times_s, gyro_rad_s, seg1)
# Exact local endpoint interpolation. ``seg0`` and ``seg1`` are inside
# this adjacent sample interval, so scanning the full series with
# np.interp here would turn pair construction into quadratic work.
sample_dt = max(t_b - t_a, 1e-12)
u0 = (seg0 - t_a) / sample_dt
u1 = (seg1 - t_a) / sample_dt
g_a = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
g_b = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
omega = 0.5 * (g_a + g_b) - bias
gyro_norms.append(float(np.linalg.norm(omega)))
@@ -279,10 +284,13 @@ def preintegrate_imu(
if dt <= 0:
continue
g_a = _interp_vec(times_s, gyro_rad_s, seg0)
g_b = _interp_vec(times_s, gyro_rad_s, seg1)
a_a = _interp_vec(times_s, acc_m_s2, seg0)
a_b = _interp_vec(times_s, acc_m_s2, seg1)
sample_dt = max(t_b - t_a, 1e-12)
u0 = (seg0 - t_a) / sample_dt
u1 = (seg1 - t_a) / sample_dt
g_a = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
g_b = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
a_a = (1.0 - u0) * acc_m_s2[index] + u0 * acc_m_s2[index + 1]
a_b = (1.0 - u1) * acc_m_s2[index] + u1 * acc_m_s2[index + 1]
omega = 0.5 * (g_a + g_b) - bg
acc = 0.5 * (a_a + a_b) - ba
gyro_norms.append(float(np.linalg.norm(omega)))
+73
View File
@@ -0,0 +1,73 @@
"""GNHPR attitude conventions and SO(3) interpolation."""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
from scipy.spatial.transform import Rotation, Slerp
@dataclass(frozen=True)
class GnhprConvention:
"""Interpretation of GNHPR angles as ``R_ENU_RTK``.
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.
"""
name: str
heading_sign: float = -1.0
pitch_sign: float = -1.0
roll_sign: float = 1.0
EXPECTED_GNHPR = GnhprConvention("north_cw__pitch_nose_up__roll_right_down")
GNHPR_CANDIDATES = (
EXPECTED_GNHPR,
GnhprConvention("north_cw__pitch_opposite", -1.0, 1.0, 1.0),
GnhprConvention("heading_opposite__pitch_nose_up", 1.0, -1.0, 1.0),
GnhprConvention("heading_opposite__pitch_opposite", 1.0, 1.0, 1.0),
)
def gnhpr_to_rotation_enu_rtk(
heading_deg: np.ndarray,
pitch_deg: np.ndarray,
roll_deg: np.ndarray,
convention: GnhprConvention = EXPECTED_GNHPR,
) -> np.ndarray:
"""Build body-to-ENU matrices with an extrinsic Z-Y-X Euler sequence."""
heading = np.asarray(heading_deg, dtype=float).reshape(-1)
pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
roll = np.asarray(roll_deg, dtype=float).reshape(-1)
if not (heading.size == pitch.size == roll.size):
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)
angles = np.column_stack([yaw_rad, pitch_rad, roll_rad])
return Rotation.from_euler('ZYX', angles).as_matrix()
def interpolate_rotations(
source_t_s: np.ndarray,
rotations: np.ndarray,
query_t_s: np.ndarray,
) -> np.ndarray:
"""Slerp a monotonic SO(3) series without extrapolation."""
source_t = np.asarray(source_t_s, dtype=float).reshape(-1)
query_t = np.asarray(query_t_s, dtype=float).reshape(-1)
matrices = np.asarray(rotations, dtype=float).reshape(-1, 3, 3)
if source_t.size < 2 or matrices.shape[0] != source_t.size:
raise ValueError("need at least two timestamped rotations")
if np.any(np.diff(source_t) <= 0):
unique_t, unique_indices = np.unique(source_t, return_index=True)
source_t = unique_t
matrices = matrices[unique_indices]
if np.any(query_t < source_t[0]) or np.any(query_t > source_t[-1]):
raise ValueError("rotation interpolation does not extrapolate")
return Slerp(source_t, Rotation.from_matrix(matrices))(query_t).as_matrix()
+172
View File
@@ -0,0 +1,172 @@
"""End-to-end orchestration and JSON reporting for RTK--IMU calibration."""
from __future__ import annotations
import csv
import json
from dataclasses import asdict, dataclass
from pathlib import Path
from typing import Any
import numpy as np
from .imu_io import load_imu_samples
from .rtk_imu_rotation import RotationCalibrationResult, RotationSession, solve_rtk_imu_rotation
from .rtk_imu_translation import TranslationCalibrationResult, solve_rtk_imu_translation
from .rtk_io import load_rtk_csv
@dataclass(frozen=True)
class InventoryEntry:
session_id: str
batch_id: str
imu_csv: Path
rtk_csv: Path
def load_inventory(path: Path | str) -> list[InventoryEntry]:
"""Load the project RTK inventory and derive each paired IMU path."""
source = Path(path)
entries: list[InventoryEntry] = []
with source.open("r", encoding="utf-8-sig", newline="") as handle:
for row in csv.DictReader(handle):
rtk_csv = Path(row["current_rtk_csv"])
imu_csv = rtk_csv.with_name("imu.csv")
entries.append(
InventoryEntry(
session_id=row["session"],
batch_id=row["batch"],
imu_csv=imu_csv,
rtk_csv=rtk_csv,
)
)
if not entries:
raise ValueError(f"empty RTK inventory: {source}")
return entries
def load_sessions(entries: list[InventoryEntry] | tuple[InventoryEntry, ...]) -> list[RotationSession]:
sessions = []
for entry in entries:
sessions.append(
RotationSession(
session_id=entry.session_id,
batch_id=entry.batch_id,
imu=load_imu_samples(entry.imu_csv),
rtk=load_rtk_csv(entry.rtk_csv),
)
)
return sessions
def _jsonable(value: Any) -> Any:
if isinstance(value, np.ndarray):
return value.tolist()
if isinstance(value, np.generic):
return value.item()
if isinstance(value, Path):
return str(value)
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 dataset_audit(sessions: list[RotationSession]) -> dict[str, Any]:
rows = []
for session in sessions:
rtk = session.rtk
valid_position = rtk.position_valid
valid_attitude = rtk.attitude_valid & valid_position
rows.append(
{
"session_id": session.session_id,
"batch_id": session.batch_id,
"imu_samples": int(session.imu.t_s.size),
"rtk_samples": int(rtk.t_s.size),
"fixed_position_ratio": float(np.mean(valid_position)),
"fixed_attitude_ratio": float(np.mean(valid_attitude)),
"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])),
],
"origin_geodetic": list(rtk.origin_geodetic),
"imu_source": str(session.imu.t_s.size) + " normalized samples",
"rtk_source": str(rtk.source),
}
)
return {"session_count": len(sessions), "sessions": rows}
def run_calibration(
sessions: list[RotationSession],
output_directory: Path | str,
*,
rotation_only: bool = False,
compute_loo: bool = True,
knot_step_s: float = 2.0,
rtk_frame_definition: str = '',
rtk_reference_point: str = '',
) -> tuple[RotationCalibrationResult, TranslationCalibrationResult | None]:
"""Run calibration and publish human-readable JSON artifacts."""
output = Path(output_directory)
output.mkdir(parents=True, exist_ok=True)
rotation = solve_rtk_imu_rotation(sessions, compute_loo=compute_loo)
translation = None
if not rotation_only:
translation = solve_rtk_imu_translation(
sessions,
rotation,
knot_step_s=knot_step_s,
compute_loo=compute_loo,
)
audit_payload = dataset_audit(sessions)
rotation_payload = _jsonable(rotation)
translation_payload = None if translation is None else _jsonable(translation)
(output / "dataset_audit.json").write_text(
json.dumps(audit_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
)
(output / "rotation_result.json").write_text(
json.dumps(rotation_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
)
if translation_payload is not None:
(output / "translation_result.json").write_text(
json.dumps(translation_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
)
interpretation_complete = bool(rtk_frame_definition.strip() and rtk_reference_point.strip())
accepted = bool(
rotation.ok and translation is not None and translation.ok and interpretation_complete
)
blockers = []
if not rotation.ok:
blockers.append('rotation quality gates failed')
if translation is None or not translation.ok:
blockers.append('translation quality gates failed or were not run')
if not rtk_frame_definition.strip():
blockers.append('RTK frame_definition is empty')
if not rtk_reference_point.strip():
blockers.append('RTK reference_point is empty')
summary = {
"status": "accepted" if accepted else "diagnostic_not_accepted",
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
"rtk_frame_definition": rtk_frame_definition,
"rtk_reference_point": rtk_reference_point,
"interpretation_blockers": blockers,
"R_RTK_IMU": rotation.R_RTK_IMU.tolist(),
"t_RTK_IMU_m": None if translation is None else translation.t_RTK_IMU_m.tolist(),
"T_RTK_IMU": None if translation is None else translation.T_RTK_IMU.tolist(),
"rotation_ok": rotation.ok,
"translation_ok": None if translation is None else translation.ok,
"rotation_result": "rotation_result.json",
"translation_result": None if translation is None else "translation_result.json",
"dataset_audit": "dataset_audit.json",
}
(output / "summary.json").write_text(
json.dumps(summary, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
)
return rotation, translation
+439
View File
@@ -0,0 +1,439 @@
"""Rotation and residual time-offset calibration between G90 RTK and HI13 IMU."""
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 .contracts import ImuSeries, MotionPair
from .geometry import orthonormalize_rotation, rpy_deg_xyz, so3_exp, so3_log
from .imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
from .rotation_handeye import estimate_rotation_handeye_initial
from .rtk_attitude import GNHPR_CANDIDATES, GnhprConvention, gnhpr_to_rotation_enu_rtk
from .rtk_io import RtkSeries
@dataclass(frozen=True)
class RotationSession:
session_id: str
batch_id: str
imu: ImuSeries
rtk: RtkSeries
@dataclass(frozen=True)
class TimeOffsetAudit:
offset_s: float
peak_correlation: float
second_best_correlation: float
evaluated_samples: int
reliable: bool
@dataclass(frozen=True)
class RotationCalibrationResult:
R_RTK_IMU: np.ndarray
rpy_deg: np.ndarray
gyro_bias_by_session_rad_s: dict[str, np.ndarray]
time_offset: TimeOffsetAudit
applied_time_offset_s: float
convention: GnhprConvention
convention_scores_deg: dict[str, float]
pair_count: int
residual_rms_deg: float
residual_median_deg: float
residual_p95_deg: float
rotation_std_deg: np.ndarray
information_singular_values: np.ndarray
per_session_rms_deg: dict[str, float]
loo_delta_deg: dict[str, float]
ok: bool
notes: tuple[str, ...]
@dataclass(frozen=True)
class _Pair:
session_index: int
session_id: str
R_A: np.ndarray
delta_R_zero_bias: np.ndarray
J_bg: np.ndarray
weight: float
def _attitude_rows(rtk: RtkSeries, convention: GnhprConvention) -> tuple[np.ndarray, np.ndarray]:
valid = rtk.attitude_valid & rtk.position_valid
t = rtk.attitude_t_s[valid]
angles = np.column_stack(
[rtk.heading_deg[valid], rtk.pitch_deg[valid], rtk.roll_deg[valid]]
)
if t.size < 2:
raise ValueError(f"not enough valid RTK attitude rows: {rtk.source}")
# GGA is faster than HPR, so nearest-neighbour export repeats attitude rows.
# Keep only changes and place them at the first associated GGA measurement.
changed = np.ones(t.size, dtype=bool)
changed[1:] = np.any(np.abs(np.diff(angles, axis=0)) > 1e-10, axis=1)
t = t[changed]
angles = angles[changed]
order = np.argsort(t)
t = t[order]
angles = angles[order]
unique_t, unique_indices = np.unique(t, return_index=True)
rotations = gnhpr_to_rotation_enu_rtk(
angles[unique_indices, 0],
angles[unique_indices, 1],
angles[unique_indices, 2],
convention,
)
return unique_t, rotations
def _rtk_angular_speed(t: np.ndarray, rotations: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
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]
def _correlation(a: np.ndarray, b: np.ndarray) -> float:
a = np.asarray(a, dtype=float)
b = np.asarray(b, dtype=float)
if a.size < 20 or np.std(a) < 1e-5 or np.std(b) < 1e-5:
return np.nan
return float(np.corrcoef(a, b)[0, 1])
def audit_time_offset(
sessions: list[RotationSession] | tuple[RotationSession, ...],
*,
search_half_width_s: float = 0.30,
step_s: float = 0.005,
) -> TimeOffsetAudit:
"""Estimate residual ``t_IMU - t_RTK`` from invariant angular-speed norms."""
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]] = []
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]
if midpoint.size >= 20:
session_series.append((midpoint, rtk_speed, session.imu.t_s, imu_speed))
total_samples += int(midpoint.size)
if not session_series:
return TimeOffsetAudit(0.0, np.nan, np.nan, 0, False)
scores = []
for offset in offsets:
per_session = []
for midpoint, rtk_speed, imu_t, imu_speed in session_series:
query = midpoint + offset
inside = (query >= imu_t[0]) & (query <= imu_t[-1])
if np.count_nonzero(inside) < 20:
continue
interpolated = np.interp(query[inside], imu_t, imu_speed)
value = _correlation(rtk_speed[inside], interpolated)
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)
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)
def _nearest_index(times: np.ndarray, target: float) -> int:
index = int(np.searchsorted(times, target))
candidates = [max(0, index - 1), min(times.size - 1, index)]
return min(candidates, key=lambda item: abs(float(times[item]) - target))
def _make_pairs(
sessions: list[RotationSession],
convention: GnhprConvention,
time_offset_s: float,
*,
anchor_step_s: float = 5.0,
intervals_s: tuple[float, ...] = (0.75, 1.5, 3.0),
preintegration_cache: dict[tuple[str, float, float], object] | None = None,
) -> list[_Pair]:
pairs: list[_Pair] = []
cache = {} if preintegration_cache is None else preintegration_cache
for session_index, session in enumerate(sessions):
t, rotations = _attitude_rows(session.rtk, convention)
next_anchor = float(t[0])
for i in range(t.size - 1):
if t[i] + 1e-9 < next_anchor:
continue
next_anchor = float(t[i] + anchor_step_s)
for duration in intervals_s:
j = _nearest_index(t, float(t[i] + duration))
if j <= i or abs(float(t[j] - t[i]) - duration) > 0.18:
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]:
continue
r_a = orthonormalize_rotation(rotations[i].T @ rotations[j])
cache_key = (session.session_id, round(imu_t0, 6), round(imu_t1, 6))
preint = cache.get(cache_key)
if preint is None:
preint = preintegrate_gyro(
session.imu.t_s,
session.imu.gyro_rad_s,
imu_t0,
imu_t1,
)
cache[cache_key] = preint
angle_a = np.linalg.norm(so3_log(r_a))
angle_b = np.linalg.norm(so3_log(preint.delta_R))
if min(angle_a, angle_b) < np.deg2rad(0.8):
continue
weight = float(np.clip(min(angle_a, angle_b) / np.deg2rad(5.0), 0.2, 3.0))
pairs.append(
_Pair(
session_index=session_index,
session_id=session.session_id,
R_A=r_a,
delta_R_zero_bias=preint.delta_R,
J_bg=preint.J_bg,
weight=weight,
)
)
return pairs
def _solve_core(sessions: list[RotationSession], pairs: list[_Pair]) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
if len(pairs) < 6:
raise ValueError("need at least 6 excited RTK--IMU rotation pairs")
generic = [
MotionPair(
session_id=pair.session_id,
i=index,
j=index + 1,
t_i_s=0.0,
t_j_s=1.0,
R_A=pair.R_A,
R_B=pair.delta_R_zero_bias,
metadata={"weight": pair.weight},
)
for index, pair in enumerate(pairs)
]
r0 = estimate_rotation_handeye_initial(generic, min_rotation_deg=0.5)
session_count = len(sessions)
def unpack(parameters: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
return orthonormalize_rotation(so3_exp(parameters[:3])), parameters[3:].reshape(session_count, 3)
def residual(parameters: np.ndarray) -> np.ndarray:
r_x, biases = unpack(parameters)
rows = []
for pair in pairs:
corrected = apply_bias_jacobian_correction(
pair.delta_R_zero_bias,
pair.J_bg,
biases[pair.session_index],
)
error = so3_log(r_x.T @ pair.R_A @ r_x @ corrected.T)
rows.append(np.sqrt(pair.weight) * error)
# HI13 bias is session-specific; this weak prior only removes degenerate
# bias/extrinsic trades and is much looser than observed static bias.
rows.append((biases / 0.03).reshape(-1))
return np.concatenate(rows)
initial = np.concatenate([so3_log(r0), np.zeros(3 * session_count)])
jacobian_pattern = lil_matrix((3 * len(pairs) + 3 * session_count, initial.size), dtype=int)
for pair_index, pair in enumerate(pairs):
row = 3 * pair_index
jacobian_pattern[row : row + 3, 0:3] = 1
bias_col = 3 + 3 * pair.session_index
jacobian_pattern[row : row + 3, bias_col : bias_col + 3] = 1
prior_row = 3 * len(pairs)
jacobian_pattern[prior_row:, 3:] = 1
opt = least_squares(
residual,
initial,
loss="huber",
f_scale=np.deg2rad(0.5),
jac_sparsity=jacobian_pattern.tocsr(),
tr_solver='lsmr',
max_nfev=40,
)
r_x, biases = unpack(opt.x)
errors = []
for pair in pairs:
corrected = apply_bias_jacobian_correction(
pair.delta_R_zero_bias,
pair.J_bg,
biases[pair.session_index],
)
errors.append(np.degrees(np.linalg.norm(so3_log(r_x.T @ pair.R_A @ r_x @ corrected.T))))
jacobian = opt.jac.toarray() if hasattr(opt.jac, 'toarray') else np.asarray(opt.jac, dtype=float)
information = jacobian.T @ jacobian
dof = max(residual(opt.x).size - opt.x.size, 1)
variance = float(np.sum(residual(opt.x) ** 2) / dof)
covariance = np.linalg.pinv(information, rcond=1e-10) * variance
return r_x, biases, np.asarray(errors), covariance
def solve_rtk_imu_rotation(
sessions: list[RotationSession] | tuple[RotationSession, ...],
*,
compute_loo: bool = True,
) -> RotationCalibrationResult:
"""Solve shared ``R_RTK_IMU`` and per-session gyro biases."""
items = list(sessions)
if not items:
raise ValueError("at least one RTK--IMU session is required")
time_audit = audit_time_offset(items)
offset = time_audit.offset_s if time_audit.reliable else 0.0
candidates: list[tuple[GnhprConvention, list[_Pair]]] = []
scores: dict[str, float] = {}
preintegration_cache: dict[tuple[str, float, float], object] = {}
for convention in GNHPR_CANDIDATES:
pairs = _make_pairs(items, convention, offset, preintegration_cache=preintegration_cache)
if len(pairs) < 6:
scores[convention.name] = 1e9
continue
generic = [
MotionPair(
session_id=pair.session_id,
i=index,
j=index + 1,
t_i_s=0.0,
t_j_s=1.0,
R_A=pair.R_A,
R_B=pair.delta_R_zero_bias,
metadata={'weight': pair.weight},
)
for index, pair in enumerate(pairs)
]
initial_rotation = estimate_rotation_handeye_initial(generic, min_rotation_deg=0.5)
preliminary_errors = np.asarray(
[
np.degrees(
np.linalg.norm(
so3_log(
initial_rotation.T
@ pair.R_A
@ initial_rotation
@ pair.delta_R_zero_bias.T
)
)
)
for pair in pairs
]
)
scores[convention.name] = float(np.sqrt(np.mean(preliminary_errors**2)))
candidates.append((convention, pairs))
if not candidates:
raise ValueError("no GNHPR convention produced enough rotation pairs")
convention, pairs = min(candidates, key=lambda item: scores[item[0].name])
rotation, biases, errors, covariance = _solve_core(items, pairs)
per_session = {}
for session in items:
values = [error for pair, error in zip(pairs, errors) if pair.session_id == session.session_id]
per_session[session.session_id] = (
float(np.sqrt(np.mean(np.asarray(values) ** 2))) if values else np.nan
)
loo = {}
if compute_loo and len(items) >= 3:
for omitted in items:
kept_pairs = [
pair
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,
)
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)
rms = float(np.sqrt(np.mean(errors**2)))
median = float(np.median(errors))
p95 = float(np.percentile(errors, 95.0))
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(
len(pairs) >= 20
and rms <= 1.0
and p95 <= 2.0
and float(np.max(std_deg)) <= 0.5
and (not finite_loo or max(finite_loo) <= 1.0)
and convention_gap >= 0.05
)
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",
]
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")
return RotationCalibrationResult(
R_RTK_IMU=rotation,
rpy_deg=rpy_deg_xyz(rotation),
gyro_bias_by_session_rad_s={
session.session_id: biases[index].copy() for index, session in enumerate(items)
},
time_offset=time_audit,
applied_time_offset_s=offset,
convention=convention,
convention_scores_deg=scores,
pair_count=len(pairs),
residual_rms_deg=rms,
residual_median_deg=median,
residual_p95_deg=p95,
rotation_std_deg=std_deg,
information_singular_values=singular_values,
per_session_rms_deg=per_session,
loo_delta_deg=loo,
ok=ok,
notes=tuple(notes),
)
+389
View File
@@ -0,0 +1,389 @@
"""Lever-arm calibration from RTK positions and full IMU preintegration."""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
from scipy.sparse import coo_matrix, csr_matrix, eye
from scipy.sparse.linalg import lsqr, splu
from scipy.spatial.transform import Rotation, Slerp
from .geometry import make_transform, orthonormalize_rotation, so3_exp
from .rtk_imu_rotation import RotationCalibrationResult, RotationSession, _attitude_rows
@dataclass(frozen=True)
class TranslationCalibrationResult:
lever_IMU_to_RTK_in_IMU_m: np.ndarray
t_RTK_IMU_m: np.ndarray
T_RTK_IMU: np.ndarray
translation_std_m: np.ndarray
lever_information_singular_values: np.ndarray
lever_precision_rank: int
position_residual_rms_xyz_m: np.ndarray
velocity_residual_rms_xyz_m_s: np.ndarray
accel_bias_by_session_m_s2: dict[str, np.ndarray]
knot_count_by_session: dict[str, int]
loo_delta_m: dict[str, np.ndarray]
ok: bool
notes: tuple[str, ...]
@dataclass(frozen=True)
class _SessionFactors:
session: RotationSession
knot_t_s: np.ndarray
position_enu_m: np.ndarray
R_ENU_IMU: np.ndarray
delta_p: tuple[np.ndarray, ...]
delta_v: tuple[np.ndarray, ...]
J_p_ba: tuple[np.ndarray, ...]
J_v_ba: tuple[np.ndarray, ...]
duration_s: np.ndarray
def _preintegrate_translation_interval(
times_s: np.ndarray,
gyro_rad_s: np.ndarray,
acc_m_s2: np.ndarray,
t0: float,
t1: float,
gyro_bias_rad_s: np.ndarray,
) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, float]:
"""Fast nominal ``delta_p/delta_v`` and accel-bias Jacobians.
Rotation covariance and gyro-bias Jacobians are deliberately omitted here:
rotation and gyro bias have already been fixed by Phase R1, while the
translation linear system only consumes the accelerometer-bias Jacobians.
"""
left = max(int(np.searchsorted(times_s, t0, side='left') - 1), 0)
right = min(int(np.searchsorted(times_s, t1, side='right')), times_s.size - 1)
delta_r = np.eye(3)
delta_v = np.zeros(3)
delta_p = np.zeros(3)
j_v_ba = np.zeros((3, 3))
j_p_ba = np.zeros((3, 3))
for index in range(left, right):
sample_t0 = float(times_s[index])
sample_t1 = float(times_s[index + 1])
if sample_t1 <= t0 or sample_t0 >= t1:
continue
segment_t0 = max(sample_t0, t0)
segment_t1 = min(sample_t1, t1)
dt = segment_t1 - segment_t0
if dt <= 0.0:
continue
sample_dt = max(sample_t1 - sample_t0, 1e-12)
u0 = (segment_t0 - sample_t0) / sample_dt
u1 = (segment_t1 - sample_t0) / sample_dt
gyro0 = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
gyro1 = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
acc0 = (1.0 - u0) * acc_m_s2[index] + u0 * acc_m_s2[index + 1]
acc1 = (1.0 - u1) * acc_m_s2[index] + u1 * acc_m_s2[index + 1]
omega = 0.5 * (gyro0 + gyro1) - gyro_bias_rad_s
acc = 0.5 * (acc0 + acc1)
r_i = delta_r
delta_p = delta_p + delta_v * dt + 0.5 * r_i @ acc * dt**2
delta_v = delta_v + r_i @ acc * dt
j_p_ba = j_p_ba + j_v_ba * dt - 0.5 * r_i * dt**2
j_v_ba = j_v_ba - r_i * dt
delta_r = orthonormalize_rotation(delta_r @ so3_exp(omega * dt))
return delta_p, delta_v, j_p_ba, j_v_ba, float(max(t1 - t0, 0.0))
def _make_session_factors(
session: RotationSession,
rotation: RotationCalibrationResult,
*,
knot_step_s: float,
) -> _SessionFactors:
t_attitude, r_enu_rtk = _attitude_rows(session.rtk, rotation.convention)
position_valid = session.rtk.position_valid
t_position = session.rtk.t_s[position_valid]
position = session.rtk.position_enu_m[position_valid]
time_offset_s = rotation.applied_time_offset_s
start = max(float(t_attitude[0]), float(t_position[0]), float(session.imu.t_s[0] - time_offset_s))
end = min(float(t_attitude[-1]), float(t_position[-1]), float(session.imu.t_s[-1] - time_offset_s))
if end - start < 5.0:
raise ValueError(f"{session.session_id}: less than 5 s common RTK/IMU support")
knot_t = np.arange(start + 0.25, end - 0.25, knot_step_s)
if knot_t.size < 4:
raise ValueError(f"{session.session_id}: not enough translation knots")
position_knots = np.column_stack(
[np.interp(knot_t, t_position, position[:, axis]) for axis in range(3)]
)
r_enu_rtk_knots = Slerp(t_attitude, Rotation.from_matrix(r_enu_rtk))(knot_t).as_matrix()
r_enu_imu = r_enu_rtk_knots @ rotation.R_RTK_IMU
bg = rotation.gyro_bias_by_session_rad_s[session.session_id]
delta_p: list[np.ndarray] = []
delta_v: list[np.ndarray] = []
j_p_ba: list[np.ndarray] = []
j_v_ba: list[np.ndarray] = []
durations = []
for t0, t1 in zip(knot_t[:-1], knot_t[1:]):
dp, dv, jp, jv, duration = _preintegrate_translation_interval(
session.imu.t_s,
session.imu.gyro_rad_s,
session.imu.acc_m_s2,
float(t0 + time_offset_s),
float(t1 + time_offset_s),
bg,
)
delta_p.append(dp)
delta_v.append(dv)
j_p_ba.append(jp)
j_v_ba.append(jv)
durations.append(duration)
return _SessionFactors(
session=session,
knot_t_s=knot_t,
position_enu_m=position_knots,
R_ENU_IMU=r_enu_imu,
delta_p=tuple(delta_p),
delta_v=tuple(delta_v),
J_p_ba=tuple(j_p_ba),
J_v_ba=tuple(j_v_ba),
duration_s=np.asarray(durations),
)
def _append_block(
rows: list[int],
cols: list[int],
values: list[float],
rhs: list[float],
groups: list[int],
matrix_blocks: list[tuple[int, np.ndarray]],
vector: np.ndarray,
sigma: np.ndarray,
group: int,
) -> None:
row0 = len(rhs)
for axis in range(3):
rhs.append(float(vector[axis] / sigma[axis]))
groups.append(group)
for col0, block in matrix_blocks:
for local_col in range(block.shape[1]):
value = float(block[axis, local_col] / sigma[axis])
if value != 0.0:
rows.append(row0 + axis)
cols.append(col0 + local_col)
values.append(value)
def _build_system(
factors: list[_SessionFactors],
*,
position_sigma_xyz_m: np.ndarray,
velocity_sigma_xyz_m_s: np.ndarray,
) -> tuple[csr_matrix, np.ndarray, np.ndarray, dict[str, tuple[int, int]], list[tuple[str, int, str]]]:
# x = [shared lever(3), per-session ba(3), per-knot velocities(3*K)]
offsets: dict[str, tuple[int, int]] = {}
variable_count = 3
for item in factors:
ba_offset = variable_count
velocity_offset = ba_offset + 3
offsets[item.session.session_id] = (ba_offset, velocity_offset)
variable_count = velocity_offset + 3 * item.knot_t_s.size
rows: list[int] = []
cols: list[int] = []
values: list[float] = []
rhs: list[float] = []
groups: list[int] = []
factor_labels: list[tuple[str, int, str]] = []
gravity = np.array([0.0, 0.0, -9.80665])
group = 0
for item in factors:
ba_offset, velocity_offset = offsets[item.session.session_id]
for index, dt in enumerate(item.duration_s):
r_i = item.R_ENU_IMU[index]
r_j = item.R_ENU_IMU[index + 1]
dp_rtk = item.position_enu_m[index + 1] - item.position_enu_m[index]
constant_p = dp_rtk - 0.5 * gravity * dt**2 - r_i @ item.delta_p[index]
_append_block(
rows,
cols,
values,
rhs,
groups,
[
(0, r_i - r_j),
(ba_offset, -r_i @ item.J_p_ba[index]),
(velocity_offset + 3 * index, -dt * np.eye(3)),
],
-constant_p,
position_sigma_xyz_m,
group,
)
factor_labels.append((item.session.session_id, group, "position"))
group += 1
constant_v = -gravity * dt - r_i @ item.delta_v[index]
_append_block(
rows,
cols,
values,
rhs,
groups,
[
(ba_offset, -r_i @ item.J_v_ba[index]),
(velocity_offset + 3 * index, -np.eye(3)),
(velocity_offset + 3 * (index + 1), np.eye(3)),
],
-constant_v,
velocity_sigma_xyz_m_s,
group,
)
factor_labels.append((item.session.session_id, group, "velocity"))
group += 1
# Loose physical bias prior. It prevents an unobservable constant
# acceleration from masquerading as gravity while remaining data-led.
_append_block(
rows,
cols,
values,
rhs,
groups,
[(ba_offset, np.eye(3))],
np.zeros(3),
np.full(3, 0.5),
group,
)
factor_labels.append((item.session.session_id, group, "bias_prior"))
group += 1
matrix = coo_matrix((values, (rows, cols)), shape=(len(rhs), variable_count)).tocsr()
return matrix, np.asarray(rhs), np.asarray(groups), offsets, factor_labels
def _irls(matrix: csr_matrix, rhs: np.ndarray, groups: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
row_weights = np.ones(rhs.size)
solution = np.zeros(matrix.shape[1])
for _ in range(5):
weighted = matrix.multiply(row_weights[:, None])
solution = lsqr(weighted, rhs * row_weights, atol=1e-10, btol=1e-10, iter_lim=3000)[0]
residual = matrix @ solution - rhs
new_weights = np.ones_like(row_weights)
for group in np.unique(groups):
selection = groups == group
norm = float(np.linalg.norm(residual[selection]))
if norm > 3.0:
new_weights[selection] = np.sqrt(3.0 / norm)
if np.max(np.abs(new_weights - row_weights)) < 1e-3:
row_weights = new_weights
break
row_weights = new_weights
return solution, row_weights
def _solve_factors(
factors: list[_SessionFactors],
position_sigma: np.ndarray,
velocity_sigma: np.ndarray,
) -> tuple[np.ndarray, np.ndarray, csr_matrix, np.ndarray, dict[str, tuple[int, int]], np.ndarray, np.ndarray]:
matrix, rhs, groups, offsets, labels = _build_system(
factors,
position_sigma_xyz_m=position_sigma,
velocity_sigma_xyz_m_s=velocity_sigma,
)
solution, row_weights = _irls(matrix, rhs, groups)
weighted = matrix.multiply(row_weights[:, None]).tocsr()
residual = matrix @ solution - rhs
data_groups = {group for _, group, kind in labels if kind != "bias_prior"}
data_rows = np.isin(groups, list(data_groups))
variance = float(np.sum((residual[data_rows] * row_weights[data_rows]) ** 2) / max(np.count_nonzero(data_rows) - solution.size, 1))
information = (weighted.T @ weighted).tocsc() + eye(weighted.shape[1], format="csc") * 1e-10
h_ll = information[:3, :3].toarray()
h_ln = information[:3, 3:]
h_nn = information[3:, 3:]
nuisance_solve = splu(h_nn).solve(h_ln.T.toarray())
schur = h_ll - h_ln.toarray() @ nuisance_solve
covariance_lever = np.linalg.pinv(schur, rcond=1e-10) * variance
return solution, covariance_lever, matrix, rhs, offsets, groups, residual
def solve_rtk_imu_translation(
sessions: list[RotationSession] | tuple[RotationSession, ...],
rotation: RotationCalibrationResult,
*,
knot_step_s: float = 2.0,
compute_loo: bool = True,
) -> TranslationCalibrationResult:
"""Estimate the shared IMU-to-RTK lever arm and return ``T_RTK_IMU``."""
items = list(sessions)
factors = [_make_session_factors(session, rotation, knot_step_s=knot_step_s) for session in items]
position_sigma = np.array([0.025, 0.025, 0.060])
velocity_sigma = np.array([0.08, 0.08, 0.12])
solution, covariance_l, matrix, rhs, offsets, groups, residual = _solve_factors(
factors, position_sigma, velocity_sigma
)
lever = solution[:3]
t_rtk_imu = -rotation.R_RTK_IMU @ lever
covariance_t = rotation.R_RTK_IMU @ covariance_l @ rotation.R_RTK_IMU.T
std_t = np.sqrt(np.maximum(np.diag(covariance_t), 0.0))
schur_information = np.linalg.pinv(covariance_l, rcond=1e-12)
singular_values = np.linalg.svd(schur_information, compute_uv=False)
threshold = max(float(singular_values[0]) * 1e-4, 1e-9)
rank = int(np.count_nonzero(singular_values > threshold))
# Recover physical residuals: system rows are grouped in XYZ triples and
# alternate position/velocity, followed by one bias prior per session.
position_errors: list[np.ndarray] = []
velocity_errors: list[np.ndarray] = []
cursor = 0
for item in factors:
for _ in range(item.knot_t_s.size - 1):
position_errors.append(residual[cursor : cursor + 3] * position_sigma)
cursor += 3
velocity_errors.append(residual[cursor : cursor + 3] * velocity_sigma)
cursor += 3
cursor += 3
pos_rms = np.sqrt(np.mean(np.asarray(position_errors) ** 2, axis=0))
vel_rms = np.sqrt(np.mean(np.asarray(velocity_errors) ** 2, axis=0))
biases = {
item.session.session_id: solution[offsets[item.session.session_id][0] : offsets[item.session.session_id][0] + 3].copy()
for item in factors
}
loo: dict[str, np.ndarray] = {}
if compute_loo and len(factors) >= 3:
for omitted in factors:
kept = [item for item in factors if item.session.session_id != omitted.session.session_id]
loo_solution, *_ = _solve_factors(kept, position_sigma, velocity_sigma)
loo[omitted.session.session_id] = (-rotation.R_RTK_IMU @ loo_solution[:3]) - t_rtk_imu
max_loo_xy = max((float(np.linalg.norm(value[:2])) for value in loo.values()), default=0.0)
max_loo_z = max((abs(float(value[2])) for value in loo.values()), default=0.0)
ok = bool(
rank == 3
and float(np.max(std_t[:2])) <= 0.05
and float(std_t[2]) <= 0.10
and float(np.max(pos_rms[:2])) <= 0.10
and float(pos_rms[2]) <= 0.20
and max_loo_xy <= 0.10
and max_loo_z <= 0.20
)
notes = [
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
]
if not rotation.ok:
notes.append("upstream rotation is not accepted, so translation is diagnostic only")
ok = False
if not ok:
notes.append("translation failed one or more strict acceptance gates")
return TranslationCalibrationResult(
lever_IMU_to_RTK_in_IMU_m=lever,
t_RTK_IMU_m=t_rtk_imu,
T_RTK_IMU=make_transform(t_rtk_imu, rotation.R_RTK_IMU),
translation_std_m=std_t,
lever_information_singular_values=singular_values,
lever_precision_rank=rank,
position_residual_rms_xyz_m=pos_rms,
velocity_residual_rms_xyz_m_s=vel_rms,
accel_bias_by_session_m_s2=biases,
knot_count_by_session={item.session.session_id: int(item.knot_t_s.size) for item in factors},
loo_delta_m=loo,
ok=ok,
notes=tuple(notes),
)
+116
View File
@@ -0,0 +1,116 @@
"""RTK CSV loading for the independent RTK--IMU calibration path."""
from __future__ import annotations
from dataclasses import dataclass
from pathlib import Path
import numpy as np
from .geodesy import geodetic_to_enu
@dataclass(frozen=True)
class RtkSeries:
"""Normalized RTK observations on the IMU device clock."""
t_s: np.ndarray
attitude_t_s: np.ndarray
position_enu_m: np.ndarray
heading_deg: np.ndarray
pitch_deg: np.ndarray
roll_deg: np.ndarray
fix_quality: np.ndarray
heading_quality: np.ndarray
hdop: np.ndarray
origin_geodetic: tuple[float, float, float]
source: Path
@property
def attitude_valid(self) -> np.ndarray:
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))
)
@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)
)
def _column(data: np.ndarray, name: str, *, default: float = np.nan) -> np.ndarray:
names = set(data.dtype.names or ())
if name not in names:
return np.full(data.shape[0], default, dtype=float)
return np.asarray(data[name], dtype=float).reshape(-1)
def load_rtk_csv(path: Path | str) -> RtkSeries:
"""Load an exported G90 RTK CSV and convert its positions to local ENU.
The required ``t`` column must already be NMEA measurement UTC mapped onto
the IMU device clock. Host receive time is deliberately never accepted as
a fallback because it is delayed by several seconds in the recorded data.
"""
source = Path(path)
if not source.is_file():
raise FileNotFoundError(source)
data = np.genfromtxt(source, delimiter=",", names=True, dtype=float, encoding="utf-8")
if data.ndim == 0:
data = np.array([data], dtype=data.dtype)
names = set(data.dtype.names or ())
required = {"t", "lat_deg", "lon_deg", "altitude_m", "fix_quality"}
if not required.issubset(names):
raise ValueError(f"RTK CSV must contain {sorted(required)}, got {sorted(names)}")
t_s = _column(data, "t")
measurement_utc = _column(data, "t_measurement_utc_s")
hpr_measurement_utc = _column(data, "hpr_measurement_utc_s")
attitude_t = t_s.copy()
has_hpr_time = np.isfinite(measurement_utc) & np.isfinite(hpr_measurement_utc)
attitude_t[has_hpr_time] += hpr_measurement_utc[has_hpr_time] - measurement_utc[has_hpr_time]
order = np.argsort(t_s)
position, origin = geodetic_to_enu(
_column(data, "lat_deg")[order],
_column(data, "lon_deg")[order],
_column(data, "altitude_m")[order],
)
return RtkSeries(
t_s=t_s[order],
attitude_t_s=attitude_t[order],
position_enu_m=position,
heading_deg=_column(data, "heading_deg")[order],
pitch_deg=_column(data, "pitch_deg")[order],
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],
hdop=_column(data, "hdop")[order],
origin_geodetic=origin,
source=source,
)
def longest_valid_interval(t_s: np.ndarray, valid: np.ndarray, *, max_gap_s: float = 0.2) -> tuple[float, float]:
"""Return the longest contiguous valid time interval."""
times = np.asarray(t_s, dtype=float).reshape(-1)
mask = np.asarray(valid, dtype=bool).reshape(-1)
indices = np.flatnonzero(mask)
if indices.size == 0:
raise ValueError("no valid RTK samples")
best_start = best_end = int(indices[0])
start = previous = int(indices[0])
for index in indices[1:]:
index = int(index)
if index != previous + 1 or times[index] - times[previous] > max_gap_s:
if times[previous] - times[start] > times[best_end] - times[best_start]:
best_start, best_end = start, previous
start = index
previous = index
if times[previous] - times[start] > times[best_end] - times[best_start]:
best_start, best_end = start, previous
return float(times[best_start]), float(times[best_end])
+35
View File
@@ -0,0 +1,35 @@
from __future__ import annotations
from datetime import datetime, timezone
import numpy as np
from tools.export_g90_rtk_to_sessions import nmea_utc_to_unix_s
from tools.time_alignment import fit_affine_clock
def test_fit_affine_clock_large_epoch_and_receive_spike() -> None:
device = np.linspace(10_000.0, 10_300.0, 601)
host = 1_786_000_000.0 + 1.00002 * (device - device[0])
host[250] += 0.25
model = fit_affine_clock(device, host)
assert abs(model.scale - 1.00002) < 1e-7
assert abs(model.map(device[400]) - host[400]) < 1e-4
assert model.inlier_count < model.sample_count
assert abs(model.inverse(model.map(device[123])) - device[123]) < 1e-7
def test_nmea_utc_uses_measurement_time_not_receive_time() -> None:
receive = datetime(2026, 8, 14, 11, 34, 12, tzinfo=timezone.utc).timestamp()
measurement = nmea_utc_to_unix_s("113408.85", receive)
assert abs((receive - measurement) - 3.15) < 1e-6
def test_nmea_utc_resolves_midnight_rollover() -> None:
receive = datetime(2026, 8, 15, 0, 0, 1, tzinfo=timezone.utc).timestamp()
measurement = nmea_utc_to_unix_s("235959.50", receive)
assert abs((receive - measurement) - 1.5) < 1e-6
+67
View File
@@ -0,0 +1,67 @@
from __future__ import annotations
from pathlib import Path
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_imu_translation import _preintegrate_translation_interval
from imu_lidar.rtk_io import load_rtk_csv
def test_geodetic_to_enu_has_expected_axis_and_scale() -> None:
enu, origin = geodetic_to_enu(
np.array([0.0, 0.0, 1e-5]),
np.array([0.0, 1e-5, 0.0]),
np.array([10.0, 10.0, 10.0]),
)
assert origin == (0.0, 0.0, 10.0)
assert np.allclose(enu[0], 0.0, atol=1e-8)
assert np.allclose(enu[1], [1.1131949, 0.0, 0.0], atol=2e-4)
assert np.allclose(enu[2], [0.0, 1.1057428, 0.0], atol=2e-4)
def test_gnhpr_heading_maps_north_clockwise_into_enu() -> None:
rotations = gnhpr_to_rotation_enu_rtk(
np.array([0.0, 90.0]),
np.zeros(2),
np.zeros(2),
)
assert np.allclose(rotations[0][:, 0], [0.0, 1.0, 0.0], atol=1e-12)
assert np.allclose(rotations[1][:, 0], [1.0, 0.0, 0.0], 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(
"t,t_measurement_utc_s,hpr_measurement_utc_s,lat_deg,lon_deg,altitude_m,"
"fix_quality,heading_deg,pitch_deg,roll_deg,heading_quality,hdop\n"
"10.0,1000.0,1000.05,30.0,114.0,20.0,4,12.0,1.0,0.0,4,0.6\n"
"10.1,1000.1,1000.10,30.0,114.0,20.0,4,13.0,1.0,0.0,4,0.6\n",
encoding="utf-8",
)
loaded = load_rtk_csv(path)
assert np.allclose(loaded.t_s, [10.0, 10.1])
assert np.allclose(loaded.attitude_t_s, [10.05, 10.1])
assert np.all(loaded.attitude_valid)
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))
acc = np.tile(np.array([0.4, -0.2, 9.7]), (times.size, 1))
reference_rotation = preintegrate_gyro(times, gyro, 0.13, 0.87)
assert np.allclose(reference_rotation.delta_R, so3_exp(gyro[0] * 0.74), atol=1e-10)
reference = preintegrate_imu(times, gyro, acc, 0.13, 0.87)
dp, dv, jp, jv, duration = _preintegrate_translation_interval(
times, gyro, acc, 0.13, 0.87, np.zeros(3)
)
assert np.isclose(duration, 0.74)
assert np.allclose(dp, reference.delta_p, atol=1e-10)
assert np.allclose(dv, reference.delta_v, atol=1e-10)
assert np.allclose(jp, reference.J_ba[6:9], atol=1e-10)
assert np.allclose(jv, reference.J_ba[3:6], atol=1e-10)
+308
View File
@@ -0,0 +1,308 @@
#!/usr/bin/env python3
"""Cut G90 captures into per-window RTK CSV files.
NMEA GGA/GNHPR UTC is the measurement time. Host receive UTC is retained only
for diagnostics. The measurement UTC is mapped onto the IMU device clock with
one robust affine clock model per window.
"""
from __future__ import annotations
import argparse
import csv
import json
import math
import sys
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.time_alignment import AffineClockModel, fit_affine_clock
LOCAL_TZ = timezone(timedelta(hours=8))
HPR_MATCH_S = 0.08
CSV_FIELDS = [
"t",
"t_measurement_utc_s",
"t_host_utc_s",
"receive_delay_s",
"t_local",
"receive_utc_ticks",
"lat_deg",
"lon_deg",
"altitude_m",
"fix_quality",
"satellites",
"hdop",
"heading_deg",
"pitch_deg",
"roll_deg",
"heading_quality",
"heading_valid",
"gga_utc",
"hpr_utc",
"hpr_measurement_utc_s",
"checksum_valid",
]
def _fmt(value) -> str:
if value is None:
return ""
if isinstance(value, bool):
return "1" if value else "0"
if isinstance(value, float):
if math.isnan(value):
return ""
return f"{value:.12g}"
return str(value)
def load_imu_clock_model(
imu_csv: Path,
) -> tuple[float, float, float, float, AffineClockModel]:
"""Return device/host spans and a robust ``IMU device -> host UTC`` model."""
t_device: list[float] = []
t_host: list[float] = []
with imu_csv.open("r", encoding="utf-8", newline="") as handle:
reader = csv.DictReader(handle)
for row in reader:
t_device.append(float(row["t"]))
t_host.append(float(row["t_host_utc_s"]))
if not t_host:
raise ValueError(f"empty IMU csv: {imu_csv}")
model = fit_affine_clock(np.asarray(t_device), np.asarray(t_host))
return min(t_device), max(t_device), min(t_host), max(t_host), model
def sentence_host_s(row: RtkSentence) -> float:
return utc_dotnet_ticks_to_unix_s(row.receive_utc_ticks)
def nmea_utc_to_unix_s(value: str | None, receive_host_s: float) -> float:
"""Resolve NMEA ``hhmmss.s`` to the UTC day nearest host receive time."""
if value is None or not str(value).strip():
raise ValueError("missing NMEA UTC time")
packed = float(value)
hour = int(packed // 10000)
minute = int((packed - hour * 10000) // 100)
second = packed - hour * 10000 - minute * 100
if not (0 <= hour < 24 and 0 <= minute < 60 and 0.0 <= second < 60.0):
raise ValueError(f"invalid NMEA UTC time: {value!r}")
receive = datetime.fromtimestamp(receive_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 - receive_host_s),
)
def sentence_measurement_utc_s(row: RtkSentence) -> float:
return nmea_utc_to_unix_s(
row.fields.get("position_time_utc"),
sentence_host_s(row),
)
def nearest_hpr(gga: RtkSentence, hpr_rows: list[RtkSentence]) -> RtkSentence | None:
if not hpr_rows:
return None
lo, hi = 0, len(hpr_rows) - 1
target = sentence_measurement_utc_s(gga)
while lo < hi:
mid = (lo + hi) // 2
if sentence_measurement_utc_s(hpr_rows[mid]) < target:
lo = mid + 1
else:
hi = mid
best = hpr_rows[lo]
if lo > 0 and abs(sentence_measurement_utc_s(hpr_rows[lo - 1]) - target) < abs(
sentence_measurement_utc_s(best) - target
):
best = hpr_rows[lo - 1]
if abs(sentence_measurement_utc_s(best) - target) > HPR_MATCH_S:
return None
return best
def merged_row(
gga: RtkSentence,
hpr: RtkSentence | None,
imu_to_host: AffineClockModel,
) -> dict:
t_host = sentence_host_s(gga)
t_measurement = sentence_measurement_utc_s(gga)
t_hpr = None if hpr is None else sentence_measurement_utc_s(hpr)
local = datetime.fromtimestamp(t_measurement, tz=timezone.utc).astimezone(LOCAL_TZ)
fields = gga.fields
hpr_fields = hpr.fields if hpr is not None else {}
checksum = gga.checksum_valid and (hpr is None or hpr.checksum_valid)
return {
"t": imu_to_host.inverse(t_measurement),
"t_measurement_utc_s": t_measurement,
"t_host_utc_s": t_host,
"receive_delay_s": t_host - t_measurement,
"t_local": local.strftime("%Y-%m-%dT%H:%M:%S.%f")[:-3],
"receive_utc_ticks": gga.receive_utc_ticks,
"lat_deg": fields.get("lat_deg"),
"lon_deg": fields.get("lon_deg"),
"altitude_m": fields.get("altitude_m"),
"fix_quality": fields.get("fix_quality"),
"satellites": fields.get("satellites"),
"hdop": fields.get("hdop"),
"heading_deg": hpr_fields.get("heading_deg"),
"pitch_deg": hpr_fields.get("pitch_deg"),
"roll_deg": hpr_fields.get("roll_deg"),
"heading_quality": hpr_fields.get("heading_quality"),
"heading_valid": hpr_fields.get("heading_valid"),
"gga_utc": fields.get("position_time_utc"),
"hpr_utc": hpr_fields.get("position_time_utc"),
"hpr_measurement_utc_s": t_hpr,
"checksum_valid": checksum,
}
def write_rtk_csv(path: Path, rows: list[dict]) -> None:
path.parent.mkdir(parents=True, exist_ok=True)
with path.open("w", newline="", encoding="utf-8") as handle:
writer = csv.DictWriter(handle, fieldnames=CSV_FIELDS)
writer.writeheader()
for row in rows:
writer.writerow({key: _fmt(row[key]) for key in CSV_FIELDS})
def discover_windows(sessions_root: Path) -> list[Path]:
windows = sorted(
path
for path in sessions_root.iterdir()
if path.is_dir() and (path / "imu.csv").is_file()
)
if not windows:
raise SystemExit(f"no session dirs with imu.csv under {sessions_root}")
return windows
def find_default_rscap(sessions_root: Path) -> Path:
parents = [sessions_root, sessions_root.parent]
matches: list[Path] = []
for folder in parents:
matches.extend(sorted(folder.glob("wheeltec-g90*.rscap")))
matches.extend(sorted(folder.glob("*g90*.rscap")))
if not matches:
raise SystemExit(f"no G90 .rscap next to {sessions_root}")
return matches[0]
def _delay_summary(rows: list[dict]) -> dict[str, float] | None:
if not rows:
return None
values = np.asarray([row["receive_delay_s"] for row in rows], dtype=np.float64)
return {
"median_s": float(np.median(values)),
"p05_s": float(np.percentile(values, 5.0)),
"p95_s": float(np.percentile(values, 95.0)),
"std_s": float(np.std(values)),
}
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--sessions-root", type=Path, required=True)
parser.add_argument("--rtk-rscap", type=Path, action="append")
parser.add_argument("--overwrite", action="store_true")
args = parser.parse_args(argv)
sessions_root = args.sessions_root.resolve()
rscaps = [path.resolve() for path in (args.rtk_rscap or [find_default_rscap(sessions_root)])]
for rscap in rscaps:
if not rscap.is_file():
raise SystemExit(f"missing RTK capture: {rscap}")
sentences: list[RtkSentence] = []
captures_meta = []
for rscap in rscaps:
print(f"reading {rscap}", flush=True)
capture = read_capture(rscap)
print(f"chunks={len(capture.chunks)}", flush=True)
sentences.extend(iter_g90_sentences(capture))
captures_meta.append(file_summary(capture))
sentences.sort(key=lambda row: row.receive_utc_ticks)
gga_all = [row for row in sentences if row.sentence_type == "GGA"]
hpr_all = [row for row in sentences if row.sentence_type == "GNHPR"]
hpr_all.sort(key=sentence_measurement_utc_s)
print(
f"parsed sentences={len(sentences)} GGA={len(gga_all)} GNHPR={len(hpr_all)}",
flush=True,
)
summaries = {
"rtk_rscap": [str(path) for path in rscaps],
"captures": captures_meta,
"parsed_sentences": len(sentences),
"gga": len(gga_all),
"gnhpr": len(hpr_all),
"time_source": "NMEA measurement UTC mapped through IMU device->host affine clock",
"windows": [],
}
for window in discover_windows(sessions_root):
out_csv = window / "rtk.csv"
if out_csv.exists() and not args.overwrite:
raise SystemExit(f"{out_csv} exists; pass --overwrite")
dev_min, dev_max, host_min, host_max, imu_to_host = load_imu_clock_model(
window / "imu.csv"
)
gga = [
row
for row in gga_all
if dev_min <= imu_to_host.inverse(sentence_measurement_utc_s(row)) <= dev_max
]
hpr = [
row
for row in hpr_all
if dev_min - HPR_MATCH_S
<= imu_to_host.inverse(sentence_measurement_utc_s(row))
<= dev_max + HPR_MATCH_S
]
merged = [merged_row(row, nearest_hpr(row, hpr), imu_to_host) for row in gga]
write_rtk_csv(out_csv, merged)
brief = {
"window": window.name,
"imu_host_span_s": [host_min, host_max],
"imu_device_span_s": [dev_min, dev_max],
"imu_device_to_host_clock": imu_to_host.to_dict(),
"rtk_receive_delay": _delay_summary(merged),
"gga": len(gga),
"gnhpr_in_window": len(hpr),
"rows_written": len(merged),
"heading_matched": sum(1 for row in merged if row["heading_deg"] is not None),
"fix_quality_4_or_5": sum(
1 for row in merged if row["fix_quality"] in {4, 5}
),
"out": str(out_csv),
}
summaries["windows"].append(brief)
print(json.dumps(brief, ensure_ascii=False), flush=True)
manifest = sessions_root / "rtk_export_summary.json"
manifest.write_text(json.dumps(summaries, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
print(f"summary: {manifest}")
return 0
if __name__ == "__main__":
raise SystemExit(main())
+145
View File
@@ -0,0 +1,145 @@
"""Decode Wheeltec G90 NMEA (GGA / GNHPR) from a V2 .rscap capture."""
from __future__ import annotations
import bisect
import math
from dataclasses import dataclass
from .capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
def nmea_checksum_valid(line: str) -> bool:
star = line.rfind("*")
if star < 0:
return False
try:
expected = int(line[star + 1 : star + 3], 16)
except ValueError:
return False
value = 0
for char in line[1:star]:
value ^= ord(char)
return value == expected
def _safe_float(value: str):
try:
return float(value)
except (TypeError, ValueError):
return None
def _safe_int(value: str):
try:
return int(value)
except (TypeError, ValueError):
return None
def parse_nmea_latlon(value: str, hemisphere: str):
raw = _safe_float(value)
if raw is None:
return None
degrees = math.floor(raw / 100.0)
result = degrees + (raw - degrees * 100.0) / 60.0
if hemisphere.upper() in ("S", "W"):
result = -result
return result
def parse_gga(line: str) -> dict:
fields = line[: line.rfind("*")].split(",")
if len(fields) < 10:
raise ValueError("GGA has too few fields")
return {
"type": "GGA",
"position_time_utc": fields[1],
"lat_deg": parse_nmea_latlon(fields[2], fields[3]),
"lon_deg": parse_nmea_latlon(fields[4], fields[5]),
"fix_quality": _safe_int(fields[6]),
"satellites": _safe_int(fields[7]),
"hdop": _safe_float(fields[8]),
"altitude_m": _safe_float(fields[9]),
}
def parse_gnhpr(line: str) -> dict:
fields = line[: line.rfind("*")].split(",")
if len(fields) < 7:
raise ValueError("GNHPR has too few fields")
quality = _safe_int(fields[5])
return {
"type": "GNHPR",
"position_time_utc": fields[1],
"heading_deg": _safe_float(fields[2]),
"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},
}
def _chunk_starts(chunks: list[RawChunk]) -> list[int]:
starts = []
cursor = 0
for chunk in chunks:
starts.append(cursor)
cursor += len(chunk.raw)
return starts
def _host_ticks_for_span(chunks: list[RawChunk], starts: list[int], end: int) -> int:
end_index = max(0, min(len(chunks) - 1, bisect.bisect_left(starts, end) - 1))
return chunks[end_index].receive_utc_ticks
@dataclass(frozen=True)
class RtkSentence:
sentence_type: str
receive_utc_ticks: int
checksum_valid: bool
fields: dict
raw_line: str
def iter_g90_sentences(capture: CaptureFile) -> list[RtkSentence]:
"""Parse GGA/GNHPR lines; host time comes from the containing serial chunk."""
rows: list[RtkSentence] = []
for _segment_id, chunks in iter_contiguous_segments(capture.chunks):
stream = b"".join(chunk.raw for chunk in chunks)
starts = _chunk_starts(chunks)
cursor = 0
while cursor < len(stream):
newline = stream.find(b"\n", cursor)
if newline < 0:
break
end = newline + 1
raw_line = stream[cursor:end].rstrip(b"\r\n")
cursor = end
if not raw_line:
continue
line = raw_line.decode("ascii", "replace")
if not (line.startswith("$GNGGA") or line.startswith("$GPGGA") or line.startswith("$GNHPR")):
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)
except ValueError:
continue
rows.append(
RtkSentence(
sentence_type=str(fields["type"]),
receive_utc_ticks=int(ticks),
checksum_valid=nmea_checksum_valid(line),
fields=fields,
raw_line=line,
)
)
rows.sort(key=lambda row: row.receive_utc_ticks)
return rows
+93
View File
@@ -0,0 +1,93 @@
#!/usr/bin/env python3
"""Run the independent RTK--IMU calibration against the project inventory."""
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_replay import load_inventory, load_sessions, run_calibration
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--inventory",
type=Path,
default=ROOT / "artifacts" / "rtk_imu_inventory_v1" / "rtk_session_inventory.csv",
)
parser.add_argument(
"--output-dir",
type=Path,
default=ROOT / "artifacts" / "rtk_imu_calibration_v1",
)
parser.add_argument("--session", action="append", help="session id to include; repeatable")
parser.add_argument("--batch", action="append", help="batch id to include; repeatable")
parser.add_argument("--rotation-only", action="store_true")
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='')
args = parser.parse_args(argv)
entries = load_inventory(args.inventory)
if args.session:
selected = set(args.session)
entries = [entry for entry in entries if entry.session_id in selected]
if args.batch:
selected_batches = set(args.batch)
entries = [entry for entry in entries if entry.batch_id in selected_batches]
if not entries:
raise SystemExit("no inventory rows match the requested selection")
sessions = load_sessions(entries)
rotation, translation = run_calibration(
sessions,
args.output_dir,
rotation_only=args.rotation_only,
compute_loo=not args.no_loo,
knot_step_s=args.knot_step_s,
rtk_frame_definition=args.rtk_frame_definition,
rtk_reference_point=args.rtk_reference_point,
)
print(
json.dumps(
{
"output": str(args.output_dir.resolve()),
"sessions": [session.session_id for session in sessions],
"rotation_ok": rotation.ok,
"rotation_rpy_deg": rotation.rpy_deg.tolist(),
"rotation_rms_deg": rotation.residual_rms_deg,
"translation_ok": None if translation is None else translation.ok,
"translation_m": None if translation is None else translation.t_RTK_IMU_m.tolist(),
},
ensure_ascii=False,
indent=2,
)
)
if args.per_batch:
for batch in sorted({entry.batch_id for entry in entries}):
batch_entries = [entry for entry in entries if entry.batch_id == batch]
if len(batch_entries) < 2:
continue
run_calibration(
load_sessions(batch_entries),
args.output_dir / "per_batch" / batch,
rotation_only=args.rotation_only,
compute_loo=False,
knot_step_s=args.knot_step_s,
rtk_frame_definition=args.rtk_frame_definition,
rtk_reference_point=args.rtk_reference_point,
)
return 0
if __name__ == "__main__":
raise SystemExit(main())
+97
View File
@@ -0,0 +1,97 @@
"""Small, dependency-light helpers for mapping independent sensor clocks."""
from __future__ import annotations
from dataclasses import asdict, dataclass
import numpy as np
@dataclass(frozen=True)
class AffineClockModel:
"""Numerically stable affine map ``y = y_ref + scale * (x - x_ref)``."""
x_ref: float
y_ref: float
scale: float
sample_count: int
inlier_count: int
residual_std_s: float
residual_p95_s: float
def map(self, value: float | np.ndarray) -> float | np.ndarray:
array = np.asarray(value, dtype=np.float64)
mapped = self.y_ref + self.scale * (array - self.x_ref)
return float(mapped) if array.ndim == 0 else mapped
def inverse(self, value: float | np.ndarray) -> float | np.ndarray:
if abs(self.scale) < 1e-12:
raise ValueError("clock model scale is zero")
array = np.asarray(value, dtype=np.float64)
mapped = self.x_ref + (array - self.y_ref) / self.scale
return float(mapped) if array.ndim == 0 else mapped
def to_dict(self) -> dict[str, float | int]:
return asdict(self)
def fit_affine_clock(
x: np.ndarray,
y: np.ndarray,
*,
max_iterations: int = 4,
min_residual_gate_s: float = 5e-4,
) -> AffineClockModel:
"""Robustly fit an affine clock map while rejecting receive-time spikes.
``x`` and ``y`` may have large, unrelated epochs. Centering around their
medians avoids losing precision when host UTC is around 1e9 seconds.
"""
x_values = np.asarray(x, dtype=np.float64).reshape(-1)
y_values = np.asarray(y, dtype=np.float64).reshape(-1)
finite = np.isfinite(x_values) & np.isfinite(y_values)
x_values = x_values[finite]
y_values = y_values[finite]
if x_values.size < 2:
raise ValueError("need at least two finite clock samples")
x_ref = float(np.median(x_values))
y_ref = float(np.median(y_values))
dx = x_values - x_ref
dy = y_values - y_ref
inliers = np.ones(x_values.size, dtype=bool)
scale = 1.0
offset = 0.0
for _ in range(max_iterations):
local_x = dx[inliers]
local_y = dy[inliers]
denom = float(local_x @ local_x)
if denom < 1e-18:
raise ValueError("clock samples do not span enough time")
scale = float(local_x @ local_y / denom)
offset = float(np.median(local_y - scale * local_x))
residual = dy - (offset + scale * dx)
center = float(np.median(residual[inliers]))
mad = float(np.median(np.abs(residual[inliers] - center)))
sigma = 1.4826 * mad
gate = max(float(min_residual_gate_s), 6.0 * sigma)
updated = np.abs(residual - center) <= gate
if np.count_nonzero(updated) < 2 or np.array_equal(updated, inliers):
break
inliers = updated
# Fold the small centered intercept into y_ref so map/inverse stay simple.
y_ref += offset
residual = y_values - (y_ref + scale * (x_values - x_ref))
residual_inliers = residual[inliers]
return AffineClockModel(
x_ref=x_ref,
y_ref=y_ref,
scale=scale,
sample_count=int(x_values.size),
inlier_count=int(np.count_nonzero(inliers)),
residual_std_s=float(np.std(residual_inliers)),
residual_p95_s=float(np.percentile(np.abs(residual_inliers), 95.0)),
)