305 lines
17 KiB
Python
305 lines
17 KiB
Python
#!/usr/bin/env python3
|
|
'''Root-cause audit for paired BEST/Doppler propagation innovations.'''
|
|
from __future__ import annotations
|
|
import argparse,json,sys
|
|
from pathlib import Path
|
|
import numpy as np
|
|
from scipy.spatial.transform import Rotation
|
|
ROOT=Path(__file__).resolve().parents[1]
|
|
if str(ROOT) not in sys.path: sys.path.insert(0,str(ROOT))
|
|
from rtk_imu.rtk_imu_engineering import G0,G_ENU,_height_reference,_nodes,_world_rtk
|
|
from rtk_imu.rtk_imu_multisource import load_unified_sessions
|
|
from rtk_imu.rtk_imu_node_graph import build_problem
|
|
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
|
from tools.audit_rtk_imu_heldout_innovation import _calibration_biases
|
|
from tools.audit_rtk_imu_innovation_noise import _factor_report,_interval_innovations
|
|
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
|
|
|
def _vector_summary(values):
|
|
a=np.asarray(values,dtype=float).reshape(-1,3)
|
|
if not len(a): return {'count':0}
|
|
norm=np.linalg.norm(a,axis=1)
|
|
return {'count':len(a),'bias':np.mean(a,axis=0),
|
|
'axis_rms':np.sqrt(np.mean(a*a,axis=0)),
|
|
'axis_p95_abs':np.percentile(np.abs(a),95,axis=0),
|
|
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
|
|
'vector_p95':float(np.percentile(norm,95)),
|
|
'empirical_covariance':np.cov(a,rowvar=False)}
|
|
|
|
def _correlation(left,right):
|
|
a,b=np.asarray(left),np.asarray(right); result=np.full(3,np.nan)
|
|
for axis in range(3):
|
|
if len(a)>2 and np.std(a[:,axis])>1e-12 and np.std(b[:,axis])>1e-12:
|
|
result[axis]=np.corrcoef(a[:,axis],b[:,axis])[0,1]
|
|
return result
|
|
|
|
def _root_report(position,velocity):
|
|
p={x['interval_id']:x for x in position}; v={x['interval_id']:x for x in velocity}
|
|
ids=sorted(set(p)&set(v)); records=[]
|
|
for key in ids:
|
|
left,right=p[key],v[key]; dt=float(left['dt_s'])
|
|
a_p=2.*np.asarray(left['residual'])/(dt*dt)
|
|
a_v=np.asarray(right['residual'])/dt
|
|
R=np.asarray(left['R0_WI'])
|
|
records.append({**{k:left[k] for k in (
|
|
'interval_id','session','motion','t_s','speed_bin','gyro_bin')},
|
|
'dt_s':dt,'a_position':a_p,'a_velocity':a_v,
|
|
'difference':a_v-a_p,'a_common':.5*(a_p+a_v),
|
|
'a_common_body':R.T@(.5*(a_p+a_v))})
|
|
def summarize(items):
|
|
if not items:
|
|
return {'interval_count':0,
|
|
'a_err_from_position_m_s2':{'count':0},
|
|
'a_err_from_velocity_m_s2':{'count':0},
|
|
'per_axis_correlation':[np.nan]*3,
|
|
'mean_direction_cosine':np.nan,
|
|
'mean_magnitude_ratio_velocity_over_position':np.nan,
|
|
'difference_velocity_minus_position_m_s2':{'count':0},
|
|
'common_acceleration_world_m_s2':{'count':0},
|
|
'common_acceleration_body_m_s2':{'count':0},
|
|
'equivalent_horizontal_tilt_rad':np.nan,
|
|
'equivalent_horizontal_tilt_deg':np.nan}
|
|
ap=[x['a_position'] for x in items]; av=[x['a_velocity'] for x in items]
|
|
diff=[x['difference'] for x in items]; common=[x['a_common'] for x in items]
|
|
body=[x['a_common_body'] for x in items]
|
|
mean_p=np.mean(ap,axis=0); mean_v=np.mean(av,axis=0)
|
|
denom=np.linalg.norm(mean_p)*np.linalg.norm(mean_v)
|
|
cosine=float(mean_p@mean_v/denom) if denom>1e-12 else np.nan
|
|
ratio=float(np.linalg.norm(mean_v)/max(np.linalg.norm(mean_p),1e-12))
|
|
body_bias=np.mean(body,axis=0)
|
|
body_std=np.std(body,axis=0)
|
|
horizontal=float(np.linalg.norm(body_bias[:2]))
|
|
return {'interval_count':len(items),
|
|
'a_err_from_position_m_s2':_vector_summary(ap),
|
|
'a_err_from_velocity_m_s2':_vector_summary(av),
|
|
'per_axis_correlation':_correlation(ap,av),
|
|
'mean_direction_cosine':cosine,'mean_magnitude_ratio_velocity_over_position':ratio,
|
|
'difference_velocity_minus_position_m_s2':_vector_summary(diff),
|
|
'common_acceleration_world_m_s2':_vector_summary(common),
|
|
'common_acceleration_body_m_s2':_vector_summary(body),
|
|
'body_bias_stability_std_over_bias_norm':float(
|
|
np.linalg.norm(body_std)/max(np.linalg.norm(body_bias),1e-12)),
|
|
'constant_body_accelerometer_bias_direction_stable':bool(
|
|
np.linalg.norm(body_std)<=np.linalg.norm(body_bias)),
|
|
'equivalent_horizontal_tilt_rad':horizontal/G0,
|
|
'equivalent_horizontal_tilt_deg':float(np.degrees(horizontal/G0)),
|
|
'gravity_tilt_leakage_magnitude_le_0p5deg':bool(
|
|
np.degrees(horizontal/G0)<=.5)}
|
|
def grouped(key):
|
|
groups={}
|
|
for record in records: groups.setdefault(record[key],[]).append(record)
|
|
return {name:summarize(items) for name,items in groups.items()}
|
|
overall=summarize(records)
|
|
detection_checks={}
|
|
if records:
|
|
mp=np.asarray(overall['a_err_from_position_m_s2']['bias'])
|
|
mv=np.asarray(overall['a_err_from_velocity_m_s2']['bias'])
|
|
detection_checks={'mean_direction_cosine_ge_0p95':
|
|
overall['mean_direction_cosine']>=.95,
|
|
'mean_magnitude_ratio_in_0p75_1p25':
|
|
.75<=overall['mean_magnitude_ratio_velocity_over_position']<=1.25,
|
|
'mean_acceleration_difference_norm_le_0p05_m_s2':
|
|
np.linalg.norm(mv-mp)<=.05}
|
|
detected=bool(all(detection_checks.values()))
|
|
else:
|
|
detected=False
|
|
return {'overall':overall,'per_session':grouped('session'),
|
|
'by_motion_class':grouped('motion'),'by_speed':grouped('speed_bin'),
|
|
'by_gyro_norm':grouped('gyro_bin'),
|
|
'common_acceleration_detection_gate':{
|
|
'thresholds':{'direction_cosine_min':.95,
|
|
'magnitude_ratio_range':[.75,1.25],
|
|
'mean_difference_norm_max_m_s2':.05},
|
|
'checks':detection_checks,'passed':detected},
|
|
'common_constant_acceleration_error_detected':detected},records
|
|
|
|
def _outside_targets(t,intervals):
|
|
return all(not (start-1.<=t<=end+1.) for start,end in intervals)
|
|
|
|
def _physical_static_biases(sessions,reference,R,heldout):
|
|
ranges={}
|
|
for item in heldout:
|
|
ranges.setdefault(item['session_id'],[]).append((item['start_s'],item['end_s']))
|
|
result={}
|
|
for session in sessions:
|
|
estimates=[]; times=[]
|
|
for node in _nodes(session,reference,1.):
|
|
if (node.zupt_static and node.hpr_factor_valid and
|
|
_outside_targets(node.t_s,ranges.get(session.session_id,[]))):
|
|
R_WI=_world_rtk(node.baseline_enu)@R
|
|
estimates.append(node.accel_m_s2-R_WI.T@(-G_ENU)); times.append(node.t_s)
|
|
if len(estimates)>=3:
|
|
a=np.asarray(estimates)
|
|
result[session.session_id]={'available':True,'sample_count':len(a),
|
|
'time_min_s':min(times),'time_max_s':max(times),
|
|
'physical_accel_bias_m_s2':np.median(a,axis=0),
|
|
'sample_axis_std_m_s2':np.std(a,axis=0),
|
|
'method':('target-excluded zupt_static + fixed R2G/HPR gravity; '
|
|
'horizontal components remain gravity-tilt confounded')}
|
|
else:
|
|
result[session.session_id]={'available':False,'sample_count':len(estimates),
|
|
'reason':'fewer than 3 target-excluded independent static nodes'}
|
|
return result
|
|
|
|
def _bias_distribution(values,key):
|
|
a=np.asarray([x[key] for x in values.values()],dtype=float)
|
|
return {'session_count':len(a),'mean':np.mean(a,axis=0),'std':np.std(a,axis=0),
|
|
'min':np.min(a,axis=0),'max':np.max(a,axis=0),
|
|
'peak_to_peak':np.ptp(a,axis=0)}
|
|
|
|
def main():
|
|
p=argparse.ArgumentParser(description=__doc__)
|
|
p.add_argument('--manifest',type=Path,required=True)
|
|
p.add_argument('--calibration-selection',type=Path,required=True)
|
|
p.add_argument('--all-selection',type=Path,required=True)
|
|
p.add_argument('--engineering-result',type=Path,required=True)
|
|
p.add_argument('--output',type=Path,required=True)
|
|
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
|
default=[.4543066225,-.0026392019,.0122384129])
|
|
args=p.parse_args()
|
|
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
|
|
calibration=json.loads(args.calibration_selection.read_text(encoding='utf-8'))
|
|
selected=json.loads(args.all_selection.read_text(encoding='utf-8'))
|
|
calibration_ids={x['candidate_id'] for x in calibration['selected_windows']}
|
|
heldout=[x for x in selected['selected_windows']
|
|
if x['candidate_id'] not in calibration_ids]
|
|
if len(calibration_ids)!=47 or len(heldout)!=267:
|
|
raise RuntimeError(f'expected 47+267 windows, got {len(calibration_ids)}+{len(heldout)}')
|
|
lever=np.asarray(engineering['prior_constrained_solution']['result']['final_l_I_m'])
|
|
frozen=np.array([-.4518015159,-.2644749820,.7314656115])
|
|
if not np.allclose(lever,frozen,atol=1e-10):
|
|
raise RuntimeError('candidate lever differs from frozen root-cause value')
|
|
sessions=load_unified_sessions(
|
|
args.manifest,selected_session_ids={x['session_id'] for x in heldout})
|
|
reference=_height_reference(sessions)
|
|
segments=_restore_segments(sessions,reference,heldout,1.)
|
|
R=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
|
problems=[build_problem(segment,R,lever,.006) for segment in segments]
|
|
calibration_bias=_calibration_biases(engineering,problems)
|
|
physical_bias=_physical_static_biases(sessions,reference,R,heldout)
|
|
motion_map={'0808_20260808_092827':'circle',
|
|
'0808_20260808_082148':'left_right',
|
|
'0815_20260812_123424':'slope'}
|
|
|
|
def evaluate(mode):
|
|
output={'best_position':[],'doppler':[],'imu_preintegration':[]}
|
|
for problem in problems:
|
|
session_id=problem.segment.session_id
|
|
if mode=='calibration_frozen':
|
|
bg=calibration_bias[session_id]['gyro_bias_rad_s']
|
|
ba=calibration_bias[session_id]['accel_bias_m_s2']
|
|
elif mode=='zero':
|
|
bg=np.zeros(3); ba=np.zeros(3)
|
|
else:
|
|
source=physical_bias[session_id]
|
|
if not source['available']: continue
|
|
bg=np.zeros(3); ba=source['physical_accel_bias_m_s2']
|
|
motion=motion_map.get(session_id,'other_recovered_dynamic')
|
|
records=_interval_innovations(problem,motion,bg,ba)
|
|
for key,value in records.items(): output[key].extend(value)
|
|
root,pairs=_root_report(output['best_position'],output['doppler'])
|
|
return {'bias_source':mode,
|
|
'BEST_position_innovation':_factor_report(output['best_position']),
|
|
'Doppler_innovation':_factor_report(output['doppler']),
|
|
'IMU_preintegration_innovation':_factor_report(output['imu_preintegration']),
|
|
'propagation_acceleration_consistency':root},pairs,output
|
|
|
|
modes={}; pairs={}; raw={}
|
|
for mode in ('calibration_frozen','zero','session_static_physical'):
|
|
modes[mode],pairs[mode],raw[mode]=evaluate(mode)
|
|
static_ids={x['interval_id'] for x in pairs['session_static_physical']}
|
|
common_subset={}
|
|
for mode in ('calibration_frozen','zero','session_static_physical'):
|
|
pos=[x for x in raw[mode]['best_position'] if x['interval_id'] in static_ids]
|
|
vel=[x for x in raw[mode]['doppler'] if x['interval_id'] in static_ids]
|
|
root,_=_root_report(pos,vel)
|
|
common_subset[mode]={'BEST_position_innovation':_factor_report(pos),
|
|
'Doppler_innovation':_factor_report(vel),
|
|
'propagation_acceleration_consistency':root}
|
|
def bias_norm(report,factor):
|
|
return float(np.linalg.norm(report[factor]['overall']['innovation_bias']))
|
|
A,C=common_subset['calibration_frozen'],common_subset['session_static_physical']
|
|
static_available=bool(static_ids)
|
|
best_reduction=(bias_norm(C,'BEST_position_innovation')/
|
|
max(bias_norm(A,'BEST_position_innovation'),1e-12)
|
|
if static_available else np.inf)
|
|
doppler_reduction=(bias_norm(C,'Doppler_innovation')/
|
|
max(bias_norm(A,'Doppler_innovation'),1e-12)
|
|
if static_available else np.inf)
|
|
nuisance_transfer=bool(static_available and best_reduction<=.5 and doppler_reduction<=.5)
|
|
high_gyro={}
|
|
for factor in ('BEST_position_innovation','Doppler_innovation'):
|
|
summary=modes['calibration_frozen'][factor]['by_gyro_norm'].get('gyro_ge_0p10',{})
|
|
limit=.5
|
|
high_gyro[factor]={'sample_count':summary.get('sample_count',0),
|
|
'bias_norm':float(np.linalg.norm(summary.get('innovation_bias',[np.inf]*3))),
|
|
'vector_p95':summary.get('vector_p95',np.inf),
|
|
'passed':bool(summary.get('sample_count',0)>=20 and
|
|
np.linalg.norm(summary['innovation_bias'])<=.10 and
|
|
summary['vector_p95']<=limit)}
|
|
common_detected=modes['calibration_frozen'][
|
|
'propagation_acceleration_consistency'][
|
|
'common_constant_acceleration_error_detected']
|
|
extrinsic_sensitive=bool(common_detected and
|
|
all(x['passed'] for x in high_gyro.values()))
|
|
propagation_passed=bool(nuisance_transfer)
|
|
graph_bias=engineering['prior_constrained_solution'][
|
|
'calibration_only_frozen_bias_by_session']
|
|
graph_distribution=_bias_distribution(graph_bias,'accel_bias_m_s2')
|
|
physical_available={k:v for k,v in physical_bias.items() if v['available']}
|
|
physical_values={k:{'accel_bias_m_s2':v['physical_accel_bias_m_s2']}
|
|
for k,v in physical_available.items()}
|
|
payload={'scope':'propagation bias root-cause only; frozen extrinsic and covariance',
|
|
'lever_reoptimized':False,'R2G_refit':False,'covariance_retuned':False,
|
|
'R0_parser_modified':False,'new_window_selection':False,
|
|
'data_only_free_bootstrap_LOO_called':False,
|
|
'fixed_l_I_m':lever,'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
|
|
'calibration_window_count':47,'heldout_window_count':267,
|
|
'target_GNSS_observation_used_for_bias_estimation':False,
|
|
'bias_semantics':{
|
|
'graph_nuisance_accel_bias':(
|
|
'node-graph nuisance absorbing IMU/model/attitude effects; '
|
|
'not assumed transferable physical sensor zero bias'),
|
|
'physical_IMU_accel_bias':(
|
|
'target-excluded session static estimate; horizontal components '
|
|
'remain gravity-tilt confounded')},
|
|
'graph_nuisance_accel_bias_distribution_m_s2':graph_distribution,
|
|
'per_session_graph_nuisance_bias':graph_bias,
|
|
'per_session_static_physical_bias':physical_bias,
|
|
'static_physical_bias_distribution_m_s2':(
|
|
_bias_distribution(physical_values,'accel_bias_m_s2')
|
|
if physical_values else {'session_count':0}),
|
|
'bias_source_ablation':modes,
|
|
'common_static_interval_subset_ablation':common_subset,
|
|
'static_common_subset_interval_count':len(static_ids),
|
|
'physical_over_calibration_bias_norm_ratio':{
|
|
'BEST_position':best_reduction,'Doppler':doppler_reduction},
|
|
'common_constant_acceleration_error_detected':common_detected,
|
|
'nuisance_bias_transfer_failure_detected':nuisance_transfer,
|
|
'physical_ba_ablation_available':static_available,
|
|
'nuisance_bias_transfer_assessment':(
|
|
'confirmed' if nuisance_transfer else
|
|
'not_testable_no_independent_static_segments' if not static_available
|
|
else 'not_confirmed_by_static_ablation'),
|
|
'lever_sensitive_high_gyro_validation':high_gyro,
|
|
'independent_propagation_validation_passed':propagation_passed,
|
|
'independent_extrinsic_sensitive_validation_passed':extrinsic_sensitive,
|
|
'engineering_translation_accepted':False,
|
|
'acceptance_modified_by_this_audit':False}
|
|
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
|
allow_nan=False)+'\n',encoding='utf-8')
|
|
compact={'common_constant_acceleration_error_detected':common_detected,
|
|
'static_session_count':len(physical_available),
|
|
'static_common_subset_interval_count':len(static_ids),
|
|
'bias_norm_ratio':payload['physical_over_calibration_bias_norm_ratio'],
|
|
'nuisance_bias_transfer_failure_detected':nuisance_transfer,
|
|
'high_gyro':high_gyro,
|
|
'independent_propagation_validation_passed':propagation_passed,
|
|
'independent_extrinsic_sensitive_validation_passed':extrinsic_sensitive,
|
|
'engineering_translation_accepted':False,
|
|
'calibration_frozen_acceleration':
|
|
modes['calibration_frozen']['propagation_acceleration_consistency']['overall']}
|
|
print(json.dumps(_jsonable(compact),ensure_ascii=False,indent=2))
|
|
return 0
|
|
if __name__=='__main__': raise SystemExit(main())
|