lichun.qu/calibration
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
lichun.qu
2026-08-25 09:56:25 +08:00
parent c2da6dd192
commit d14ae74117
56 changed files with 118382 additions and 159 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/audit_rtk_imu_innovation_noise.py
+286
View File
@@ -0,0 +1,286 @@
#!/usr/bin/env python3
'''Independent prediction/innovation noise audit for node-state RTK/IMU factors.'''
from __future__ import annotations
import argparse, json, math, sys
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
from imu_lidar.geometry import so3_log
from imu_lidar.imu_preintegration import apply_bias_correction_imu,preintegrate_gyro
from imu_lidar.rtk_imu_engineering import (
G_ENU,HPR_DIRECT_ANGULAR_SIGMA_RAD,_all_hpr,_height_reference,
_world_rtk)
from imu_lidar.rtk_imu_multisource import load_unified_sessions
from imu_lidar.rtk_imu_node_graph import build_problem
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
BEST_SIGMA = np.array([.06,.06,.12])
DOPPLER_SIGMA = np.array([.15,.15,.30])
def _whiten(value,covariance):
covariance = .5*(covariance+covariance.T)+np.eye(len(value))*1e-12
return np.linalg.solve(np.linalg.cholesky(covariance),value)
def _bin_speed(value):
if value < .2: return 'speed_lt_0p2'
if value < 1.: return 'speed_0p2_to_1'
return 'speed_ge_1'
def _bin_gyro(value):
if value < .02: return 'gyro_lt_0p02'
if value < .10: return 'gyro_0p02_to_0p10'
return 'gyro_ge_0p10'
def _bin_gap(value):
if not np.isfinite(value): return 'gap_unavailable'
if value <= .12: return 'gap_direct'
if value <= .3: return 'gap_0p12_to_0p3'
if value <= .5: return 'gap_0p3_to_0p5'
return 'gap_gt_0p5'
def _distribution(values):
a = np.asarray(values,dtype=float).reshape(-1)
if not a.size:
return {'count':0,'rms':np.nan,'p50_abs':np.nan,'p95_abs':np.nan,'p99_abs':np.nan}
return {'count':len(a),'rms':float(np.sqrt(np.mean(a*a))),
'p50_abs':float(np.percentile(np.abs(a),50.)),
'p95_abs':float(np.percentile(np.abs(a),95.)),
'p99_abs':float(np.percentile(np.abs(a),99.))}
def _autocorrelation(vectors):
a = np.asarray(vectors,dtype=float)
result = np.full(a.shape[1] if a.ndim == 2 else 0,np.nan)
if a.ndim != 2 or len(a) < 3: return result
for axis in range(a.shape[1]):
left,right = a[:-1,axis],a[1:,axis]
if np.std(left)>1e-12 and np.std(right)>1e-12:
result[axis] = np.corrcoef(left,right)[0,1]
return result
def _summarize(records):
if not records: return {'sample_count':0}
residual = np.asarray([item['residual'] for item in records])
normalized = np.asarray([item['normalized'] for item in records])
factor_only = np.asarray([item['normalized_factor_only'] for item in records])
nis = np.sum(normalized*normalized,axis=1)
dimension = residual.shape[1]
effective_dimension = int(records[0].get('effective_dimension',dimension))
total_dof = len(records)*effective_dimension
norm = np.linalg.norm(residual,axis=1)
centered_t = np.asarray([item['t_s'] for item in records],dtype=float)
centered_t -= np.mean(centered_t)
temporal_drift = np.zeros(dimension)
if len(records)>=3 and np.ptp(centered_t)>1e-9:
temporal_drift = np.asarray([
np.polyfit(centered_t,residual[:,axis],1)[0]
for axis in range(dimension)])
return {'sample_count':len(records),'dimension':dimension,
'effective_dof_per_sample':effective_dimension,
'innovation_bias':np.mean(residual,axis=0),
'innovation_distribution':_distribution(residual),
'axis_rms':np.sqrt(np.mean(residual*residual,axis=0)),
'axis_p50_abs':np.percentile(np.abs(residual),50.,axis=0),
'axis_p95_abs':np.percentile(np.abs(residual),95.,axis=0),
'axis_p99_abs':np.percentile(np.abs(residual),99.,axis=0),
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
'vector_p50':float(np.percentile(norm,50.)),
'vector_p95':float(np.percentile(norm,95.)),
'vector_p99':float(np.percentile(norm,99.)),
'normalized_distribution':_distribution(normalized),
'empirical_covariance':np.cov(residual,rowvar=False),
'nis_distribution':_distribution(nis),
'nis_total':float(np.sum(nis)),'nis_per_dof':float(np.sum(nis)/total_dof),
'alpha_factor':float(np.sum(nis)/total_dof),
'alpha_composite_prediction':float(np.sum(nis)/total_dof),
'alpha_factor_only_upper_bound':float(np.sum(factor_only*factor_only)/total_dof),
'temporal_autocorrelation_lag1':_autocorrelation(residual),
'temporal_linear_drift_per_s':temporal_drift}
def _group(records,key):
values = {}
for item in records: values.setdefault(str(item[key]),[]).append(item)
return {name:_summarize(items) for name,items in values.items()}
def _base_record(session_id,motion,t,speed,gyro_norm,gap):
return {'session':session_id,'motion':motion,'t_s':float(t),
'speed_bin':_bin_speed(speed),'gyro_bin':_bin_gyro(gyro_norm),
'hpr_gap_bin':_bin_gap(gap)}
def _interval_innovations(problem,motion,bg=None,ba=None):
records = {'best_position':[],'doppler':[],'imu_preintegration':[]}
lever = problem.fixed_l_I_m
bg=np.zeros(3) if bg is None else np.asarray(bg,dtype=float)
ba=np.zeros(3) if ba is None else np.asarray(ba,dtype=float)
nodes = problem.segment.nodes
for index,(left,right,pre) in enumerate(zip(nodes[:-1],nodes[1:],problem.segment.preintegrations)):
if not (left.hpr_factor_valid and right.hpr_factor_valid): continue
if left.velocity_enu_m_s is None or right.velocity_enu_m_s is None: continue
R0 = _world_rtk(left.baseline_enu)@problem.R_RTK_IMU
R1 = _world_rtk(right.baseline_enu)@problem.R_RTK_IMU
p_i0 = left.p_enu_m-R0@lever
p_i1 = right.p_enu_m-R1@lever
v_i0 = left.velocity_enu_m_s-R0@np.cross(left.gyro_rad_s-bg,lever)
v_i1 = right.velocity_enu_m_s-R1@np.cross(right.gyro_rad_s-bg,lever)
dt = pre.duration_s
delta_R,delta_v,delta_p=apply_bias_correction_imu(pre,bg,ba)
pred_p_i1 = p_i0+v_i0*dt+.5*G_ENU*dt*dt+R0@delta_p
pred_v_i1 = v_i0+G_ENU*dt+R0@delta_v
pred_R1 = R0@delta_R
p_innovation = pred_p_i1+pred_R1@lever-right.p_enu_m
v_innovation = pred_v_i1+pred_R1@np.cross(
right.gyro_rad_s-bg,lever)-right.velocity_enu_m_s
speed = float(np.linalg.norm(left.velocity_enu_m_s))
gyro_norm = float(pre.mean_gyro_norm)
gap = max(left.hpr_support_gap_s,right.hpr_support_gap_s)
base = _base_record(problem.segment.session_id,motion,right.t_s,speed,gyro_norm,gap)
base.update({'interval_id':f'{problem.segment.segment_id}:{index}',
'dt_s':float(dt),'R0_WI':R0})
p_cov = (np.diag(BEST_SIGMA**2)+dt*dt*np.diag(DOPPLER_SIGMA**2)
+R0@pre.cov[6:9,6:9]@R0.T+np.diag(BEST_SIGMA**2))
v_cov = (np.diag(DOPPLER_SIGMA**2)+R0@pre.cov[3:6,3:6]@R0.T
+np.diag(DOPPLER_SIGMA**2))
records['best_position'].append({**base,'residual':p_innovation,
'normalized':_whiten(p_innovation,p_cov),
'normalized_factor_only':p_innovation/BEST_SIGMA})
records['doppler'].append({**base,'residual':v_innovation,
'normalized':_whiten(v_innovation,v_cov),
'normalized_factor_only':v_innovation/DOPPLER_SIGMA})
imu_error = np.concatenate([
so3_log(delta_R.T@R0.T@R1),
R0.T@(v_i1-v_i0-G_ENU*dt)-delta_v,
R0.T@(p_i1-p_i0-v_i0*dt-.5*G_ENU*dt*dt)-delta_p])
anchored_cov = pre.cov.copy()
hpr_var = left.hpr_angular_sigma_rad**2+right.hpr_angular_sigma_rad**2
anchored_cov[:3,:3] += np.eye(3)*hpr_var
anchored_cov[3:6,3:6] += R0.T@np.diag(2.*DOPPLER_SIGMA**2)@R0
anchored_cov[6:9,6:9] += R0.T@np.diag(
2.*BEST_SIGMA**2+dt*dt*DOPPLER_SIGMA**2)@R0
records['imu_preintegration'].append({**base,'residual':imu_error,
'normalized':_whiten(imu_error,anchored_cov),
'normalized_factor_only':_whiten(imu_error,pre.cov)})
return records
def _hpr_innovations(session,problem,motion):
hpr = _all_hpr(session)
indices = [int(i) for i in hpr.valid_indices
if problem.segment.nodes[0].t_s <= hpr.t_s[i] <= problem.segment.nodes[-1].t_s]
records = []
baseline_I = problem.R_RTK_IMU.T[:,0]
node_times = np.asarray([node.t_s for node in problem.segment.nodes])
for position in range(1,len(indices)-1):
left,index,right = indices[position-1],indices[position],indices[position+1]
t0,t,t1 = hpr.t_s[left],hpr.t_s[index],hpr.t_s[right]
total_gap = float(t1-t0)
if not 0. < total_gap <= .5: continue
fraction = float((t-t0)/total_gap)
predicted = (1.-fraction)*hpr.baseline_enu[left]+fraction*hpr.baseline_enu[right]
predicted /= np.linalg.norm(predicted)
innovation = np.cross(predicted,hpr.baseline_enu[index])
sigma = HPR_DIRECT_ANGULAR_SIGMA_RAD*np.sqrt(
1.+(1.-fraction)**2+fraction**2)
nearest = int(np.argmin(np.abs(node_times-t)))
node = problem.segment.nodes[nearest]
speed = float(np.linalg.norm(node.velocity_enu_m_s)) if node.velocity_enu_m_s is not None else 0.
gyro_norm = float(np.linalg.norm(node.gyro_rad_s))
base = _base_record(session.session_id,motion,t,speed,gyro_norm,total_gap)
records.append({**base,'method':'leave_one_out_interpolation',
'effective_dimension':2,'residual':innovation,
'normalized':innovation/sigma,
'normalized_factor_only':innovation/HPR_DIRECT_ANGULAR_SIGMA_RAD})
gyro_pre = preintegrate_gyro(session.imu.t_s,session.imu.gyro_rad_s,float(t0),float(t))
R0 = _world_rtk(hpr.baseline_enu[left])@problem.R_RTK_IMU
propagated = R0@gyro_pre.delta_R@baseline_I
gyro_innovation = np.cross(propagated,hpr.baseline_enu[index])
gyro_sigma = np.sqrt(2.*HPR_DIRECT_ANGULAR_SIGMA_RAD**2
+float(np.trace(gyro_pre.cov))/3.)
records.append({**base,'method':'gyro_propagation',
'effective_dimension':2,'residual':gyro_innovation,
'normalized':gyro_innovation/gyro_sigma,
'normalized_factor_only':gyro_innovation/HPR_DIRECT_ANGULAR_SIGMA_RAD})
return records
def _factor_report(records):
return {'overall':_summarize(records),
'by_session':_group(records,'session'),'by_motion':_group(records,'motion'),
'by_speed':_group(records,'speed_bin'),'by_gyro_norm':_group(records,'gyro_bin'),
'by_hpr_bridge_gap':_group(records,'hpr_gap_bin'),
'by_method':_group(records,'method') if records and 'method' in records[0] else {}}
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--manifest',type=Path,required=True)
parser.add_argument('--output',type=Path,required=True)
parser.add_argument('--circle-session',required=True)
parser.add_argument('--left-right-session',required=True)
parser.add_argument('--slope-session',required=True)
parser.add_argument('--sample-period-s',type=float,default=1.)
parser.add_argument('--target-duration-s',type=float,default=15.)
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
default=[.4543066225,-.0026392019,.0122384129])
parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
default=MECHANICAL_L_I_M.tolist())
args = parser.parse_args()
categories = {'circle':args.circle_session,'left_right':args.left_right_session,
'slope':args.slope_session}
sessions = load_unified_sessions(args.manifest,selected_session_ids=set(categories.values()))
reference = _height_reference(sessions)
if reference is None: raise RuntimeError('no BEST reference')
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
lever = np.asarray(args.mechanical_l_I_m)
selections, all_records = {}, {
'best_position':[],'doppler':[],'hpr':[],'imu_preintegration':[]}
for motion,session_id in categories.items():
session = next(item for item in sessions if item.session_id == session_id)
segment,selection = _select_window(
[session],reference,args.sample_period_s,args.target_duration_s)
selections[motion] = selection
problem = build_problem(segment,rotation,lever)
interval = _interval_innovations(problem,motion)
for key,records in interval.items(): all_records[key].extend(records)
all_records['hpr'].extend(_hpr_innovations(session,problem,motion))
factors = {key:_factor_report(records) for key,records in all_records.items()}
payload = {
'scope':'independent prediction innovations; no node optimization/covariance writeback',
'least_squares_called':False,'covariance_parameters_modified':False,
'fixed_l_I_m':lever,'selections':selections,
'current_physical_sigma':{
'best_position_xyz_m':BEST_SIGMA,
'doppler_xyz_m_s':DOPPLER_SIGMA,
'hpr_direct_angular_rad':HPR_DIRECT_ANGULAR_SIGMA_RAD,
'bias_random_walk_source':'unchanged device/static/Allan noise model'},
'alpha_semantics':{
'alpha_factor':'innovation NIS/dof using composite prediction covariance',
'alpha_factor_only_upper_bound':(
'innovation divided only by current factor covariance; includes predictor and '
'endpoint-anchor noise and must not be written back directly')},
'factors':factors,
'recommended_covariance_writeback':False,
'covariance_freeze_assessment':{
'best_position':'predictor-confounded; unchanged',
'doppler':'predictor-confounded; unchanged',
'imu_preintegration':'endpoint-anchor dominated; unchanged',
'bias_random_walk':'static/Allan/device-model based; unchanged',
'hpr':{
'identifiable':True,
'evidence':'264 withheld samples; LOO and gyro predictions agree',
'frozen_direct_sigma_rad':.006,
'writeback_policy':'explicit audit decision, not automatic alpha'}}}
args.output.parent.mkdir(parents=True,exist_ok=True)
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
allow_nan=False)+'\n',encoding='utf-8')
compact = {key:{'count':value['overall'].get('sample_count',0),
'bias':value['overall'].get('innovation_bias'),
'rms':value['overall'].get('innovation_distribution',{}).get('rms'),
'p95':value['overall'].get('innovation_distribution',{}).get('p95_abs'),
'alpha_composite':value['overall'].get('alpha_composite_prediction'),
'alpha_factor_only_upper_bound':value['overall'].get('alpha_factor_only_upper_bound'),
'autocorrelation':value['overall'].get('temporal_autocorrelation_lag1')}
for key,value in factors.items()}
print(json.dumps(_jsonable(compact),ensure_ascii=False,indent=2))
return 0
if __name__ == '__main__':
raise SystemExit(main())