#!/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())