重构RTK-IMU标定链路并完成机械先验工程验证

2026-08-25 09:56:25 +08:00
parent c2da6dd192
commit d14ae74117
56 changed files with 118382 additions and 159 deletions
@@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+'''Three-start, no-prior free-lever solve on the frozen P0.5 circle window.'''
+from __future__ import annotations
+import argparse,json,sys
+from dataclasses import asdict
+from pathlib import Path
+import numpy as np
+from scipy.spatial.transform import Rotation
+ROOT=Path(__file__).resolve().parents[1]
+if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
+from imu_lidar.rtk_imu_engineering import _height_reference
+from imu_lidar.rtk_imu_multisource import load_unified_sessions
+from imu_lidar.rtk_imu_node_graph import build_problem,solve_free_lever
+from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
+from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
+
+
+def main():
+    parser=argparse.ArgumentParser(description=__doc__)
+    parser.add_argument('--manifest',type=Path,required=True)
+    parser.add_argument('--output',type=Path,required=True)
+    parser.add_argument('--circle-session',required=True)
+    parser.add_argument('--sample-period-s',type=float,default=1.)
+    parser.add_argument('--target-duration-s',type=float,default=15.)
+    parser.add_argument('--max-nfev',type=int,default=120)
+    parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
+    parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
+                        default=[.4543066225,-.0026392019,.0122384129])
+    parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
+                        default=MECHANICAL_L_I_M.tolist())
+    parser.add_argument('--large-perturbation-m',nargs=3,type=float,
+                        default=[.5,-.5,.5])
+    args=parser.parse_args()
+    sessions=load_unified_sessions(
+        args.manifest,selected_session_ids={args.circle_session})
+    reference=_height_reference(sessions)
+    segment,selection=_select_window(
+        sessions,reference,args.sample_period_s,args.target_duration_s)
+    rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
+    mechanical=np.asarray(args.mechanical_l_I_m,dtype=float)
+    problem=build_problem(segment,rotation,mechanical,args.hpr_direct_sigma_rad)
+    starts={'zero':np.zeros(3),'mechanical':mechanical,
+            'mechanical_large_perturbation':mechanical+args.large_perturbation_m}
+    results={name:asdict(solve_free_lever(problem,value,args.max_nfev))
+             for name,value in starts.items()}
+    for result in results.values():
+        covariance=np.asarray(result['lever_covariance_m2'])
+        result['lever_std_m']=np.sqrt(np.maximum(np.diag(covariance),0.))
+        result['delta_to_mechanical_m']=np.asarray(result['final_l_I_m'])-mechanical
+        result['xy_marginal_covariance_m2']=covariance[:2,:2]
+        result['xy_information_singular_values']=np.linalg.svd(
+            np.linalg.pinv(covariance[:2,:2],rcond=1e-9),compute_uv=False)
+    solutions=np.asarray([value['final_l_I_m'] for value in results.values()])
+    spread=float(max(np.linalg.norm(a-b) for a in solutions for b in solutions))
+    xy_spread=float(max(np.linalg.norm(a[:2]-b[:2]) for a in solutions for b in solutions))
+    circle_xy_observable=bool(all(value['success'] and
+        np.all(np.asarray(value['lever_std_m'])[:2]<=.15) for value in results.values()))
+    payload={'scope':'circle single-segment no-prior three-start free lever',
+        'translation_variable_enabled':True,'manual_prior_used':False,
+        'loo_bootstrap_sensitivity_called':False,'selection':selection,
+        'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
+        'hpr_direct_sigma_rad':args.hpr_direct_sigma_rad,
+        'mechanical_reference_m':mechanical,'large_perturbation_m':args.large_perturbation_m,
+        'solutions':results,'maximum_solution_spread_m':spread,
+        'maximum_xy_solution_spread_m':xy_spread,
+        'circle_only_observability_gate':{
+            'xy_marginal_std_max_m':.15,'passed':circle_xy_observable},
+        'circle_only_data_only_accepted':circle_xy_observable,
+        'joint_free_blocked_by_circle_gate':False,
+        'covariance_postfit_scaled':False}
+    args.output.parent.mkdir(parents=True,exist_ok=True)
+    args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
+                                      allow_nan=False)+'\n',encoding='utf-8')
+    print(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2))
+    return 0
+
+
+if __name__=='__main__':
+    raise SystemExit(main())