"""Conditional engineering RTK--IMU 6DoF calibration. The data-only V3 gates remain unchanged. This branch estimates the ANT1 lever arm only after an R2G gravity/level rotation is explicitly fixed. HI13 absolute attitude and quaternion yaw are never used as mechanical constraints. """ from __future__ import annotations from dataclasses import dataclass from typing import Iterable, Sequence import numpy as np from scipy.optimize import least_squares from scipy.spatial.transform import Rotation from imu_lidar.geometry import make_transform, so3_exp from imu_lidar.imu_preintegration import apply_bias_correction_imu, preintegrate_imu from .rtk_attitude import gnhpr_to_baseline_enu from .rtk_imu_multisource import UnifiedSession, _f, _nearest_index, _truth G0 = 9.80665 G_ENU = np.array([0.0, 0.0, -G0]) EARTH_RADIUS_M = 6_378_137.0 NUISANCE_DOF_PER_SEGMENT = 15 LEGACY_DETERMINISTIC_SEGMENT_MODEL_DEPRECATED = True LEGACY_DEPRECATION_REASON = ( 'multi-horizon audit: 20 s position/velocity P95 grew 259.8x/13.4x ' 'relative to 1 s re-anchored propagation' ) MIN_SEGMENT_DURATION_S = 5.0 MIN_SEGMENT_NODE_COUNT = 6 DEFAULT_MANUAL_L_I_M = np.array([-0.45072, -0.25682, 0.73208]) DEFAULT_MANUAL_L_I_COVARIANCE_M2 = np.diag([4e-4, 4e-4, 9e-4]) FREE_MANUAL_MAHALANOBIS_99_DF3 = 11.345 @dataclass(frozen=True) class ResidualAudit: count: int axis_rms: np.ndarray axis_p95_abs: np.ndarray vector_rms: float vector_p95: float @dataclass(frozen=True) class LeverFitSummary: l_I_m: np.ndarray l_I_marginal_covariance_m2: np.ndarray l_I_std_m: np.ndarray lever_marginal_information: np.ndarray lever_information_singular_values: np.ndarray lever_information_condition_number: float lever_precision_rank: int weakest_lever_direction_I: np.ndarray gga_xy_residual: ResidualAudit bestnava_xyz_residual: ResidualAudit doppler_velocity_residual: ResidualAudit optimizer_converged: bool @dataclass(frozen=True) class FixedLeverResidualSummary: l_I_m: np.ndarray gga_xy_residual: ResidualAudit bestnava_xyz_residual: ResidualAudit doppler_velocity_residual: ResidualAudit optimizer_converged: bool @dataclass(frozen=True) class Engineering6DofResult: rotation_source: str translation_conditional_on_rotation: bool R_RTK_IMU: np.ndarray l_I_m: np.ndarray | None l_I_marginal_covariance_m2: np.ndarray l_I_std_m: np.ndarray lever_marginal_information: np.ndarray lever_information_singular_values: np.ndarray lever_information_condition_number: float lever_precision_rank: int weakest_lever_direction_I: np.ndarray gga_xy_residual: ResidualAudit bestnava_xyz_residual: ResidualAudit doppler_velocity_residual: ResidualAudit session_count: int segment_count: int gga_factor_count: int bestnava_factor_count: int velocity_factor_count: int gravity_candidate_factor_count: int zupt_static_factor_count: int leave_one_session_out_l_I_m: dict[str, np.ndarray] leave_one_session_out_delta_m: dict[str, float] bootstrap_l_I_mean_m: np.ndarray bootstrap_l_I_std_m: np.ndarray bootstrap_success_count: int bootstrap_requested_count: int rotation_sensitivity_delta_l_I_m: dict[str, np.ndarray] rotation_sensitivity_max_delta_m: float manual_l_I_m: np.ndarray | None manual_l_I_delta_m: np.ndarray | None solver_health_gates: dict[str, bool] solver_health_passed: bool engineering_acceptance_gates: dict[str, bool] T_RTK_IMU: np.ndarray | None T_IMU_RTK: np.ndarray | None engineering_6dof_accepted: bool blockers: tuple[str, ...] notes: tuple[str, ...] translation_prior_applied: bool lever_prior_covariance_m2: np.ndarray | None mechanical_reference_l_I_m: np.ndarray | None free_solution: LeverFitSummary | None prior_constrained_solution: LeverFitSummary | None free_to_mechanical_delta_m: np.ndarray | None prior_to_mechanical_delta_m: np.ndarray | None free_translation_observable: bool data_only_translation_accepted: bool free_manual_mahalanobis_d2: float | None free_translation_observability_gates: dict[str, bool] posterior_to_prior_covariance_ratio: np.ndarray | None free_to_prior_residual_delta: dict[str, float] | None mechanical_reference_solution: FixedLeverResidualSummary | None residual_comparison: dict[str, float] | None @dataclass(frozen=True) class _HprSeries: t_s: np.ndarray baseline_enu: np.ndarray valid: np.ndarray valid_indices: np.ndarray @dataclass(frozen=True) class _Node: t_s: float source: str p_enu_m: np.ndarray position_mask: np.ndarray baseline_enu: np.ndarray hpr_index: int hpr_factor_valid: bool hpr_factor_method: str hpr_support_gap_s: float hpr_angular_sigma_rad: float position_time_discontinuity_before: bool gyro_rad_s: np.ndarray accel_m_s2: np.ndarray velocity_enu_m_s: np.ndarray | None gravity_candidate: bool zupt_static: bool continuity_id: int @dataclass(frozen=True) class _Segment: segment_id: str session_id: str nodes: tuple[_Node, ...] preintegrations: tuple[object, ...] R_WRTK_initial: np.ndarray def _empty_audit(dimension: int) -> ResidualAudit: return ResidualAudit(0, np.full(dimension, np.nan), np.full(dimension, np.nan), np.nan, np.nan) def _audit(values: Sequence[np.ndarray], dimension: int) -> ResidualAudit: if not values: return _empty_audit(dimension) array = np.asarray(values, dtype=float).reshape(-1, dimension) norm = np.linalg.norm(array, axis=1) return ResidualAudit( count=array.shape[0], axis_rms=np.sqrt(np.mean(array**2, axis=0)), axis_p95_abs=np.percentile(np.abs(array), 95.0, axis=0), vector_rms=float(np.sqrt(np.mean(norm**2))), vector_p95=float(np.percentile(norm, 95.0)), ) def _world_rtk(right_enu: np.ndarray) -> np.ndarray: right = np.asarray(right_enu, dtype=float).reshape(3).copy() right /= np.linalg.norm(right) up = np.array([0.0, 0.0, 1.0]) up -= right * np.dot(up, right) up /= np.linalg.norm(up) forward = np.cross(up, right) forward /= np.linalg.norm(forward) return np.column_stack([right, forward, up]) def _all_hpr(session: UnifiedSession) -> _HprSeries: annotated: list[tuple[dict[str, str], bool]] = [] last_device_t = -np.inf for row in session.rtk_by_type.get("GNHPR", []): value_t = _f(row, "t_device_s") monotonic = bool(np.isfinite(value_t) and value_t > last_device_t) if not monotonic and annotated: previous_row, _ = annotated[-1] annotated[-1] = (previous_row, False) if np.isfinite(value_t): last_device_t = value_t annotated.append((row, monotonic)) annotated.sort(key=lambda item: _f(item[0], "t_device_s")) t, baseline, valid = [], [], [] for row, monotonic in annotated: value_t = _f(row, "t_device_s") heading, pitch = _f(row, "heading_deg"), _f(row, "pitch_deg") ok = bool( monotonic and np.isfinite(value_t) and np.isfinite(heading) and np.isfinite(pitch) and _truth(row, "checksum_valid") and int(_f(row, "heading_quality", -1)) == 4 ) t.append(value_t) if np.isfinite(heading) and np.isfinite(pitch): vector = gnhpr_to_baseline_enu(np.array([heading]), np.array([pitch]))[0] else: vector = np.full(3, np.nan) baseline.append(vector) valid.append(ok) valid_array = np.asarray(valid, dtype=bool) return _HprSeries(np.asarray(t), np.asarray(baseline).reshape(-1, 3), valid_array, np.flatnonzero(valid_array)) # HPR is an optional baseline-direction observation. These limits decide only # whether an HPR factor is usable; they never define IMU trajectory continuity. HPR_NEAREST_Q4_TOLERANCE_S = 0.12 HPR_BRIDGE_MAX_GAP_S = 0.50 # data-backed: 0.5 s dropout max error < 0.035 rad; 0.6 s does not HPR_ISOLATED_OUTLIER_DEG = 30.0 HPR_DIRECT_ANGULAR_SIGMA_RAD = 0.035 # Conservative envelope of the three-session artificial-dropout interpolation # P95 errors (deg), forced non-decreasing before covariance composition. HPR_BRIDGE_ERROR_P95_DEG = np.array([ [0.00, 0.000], [0.20, 0.899], [0.30, 0.903], [0.40, 0.928], [0.50, 0.928], ], dtype=float) def _hpr_angular_sigma_rad(method: str, support_gap_s: float) -> float: """Per-factor angular sigma; bridge uncertainty is never treated as direct HPR.""" if method == "nearest_q4": return HPR_DIRECT_ANGULAR_SIGMA_RAD if method != "bracket_interpolation" or not np.isfinite(support_gap_s): return np.inf if support_gap_s <= 0.0 or support_gap_s > HPR_BRIDGE_MAX_GAP_S: return np.inf bridge_error_deg = float(np.interp(support_gap_s, HPR_BRIDGE_ERROR_P95_DEG[:, 0], HPR_BRIDGE_ERROR_P95_DEG[:, 1])) # Treat the empirical P95 envelope itself as an additional conservative # 1-sigma-equivalent term rather than granting bridge direct-HPR weight. return float(np.hypot(HPR_DIRECT_ANGULAR_SIGMA_RAD, np.deg2rad(bridge_error_deg))) def _unit_vector(vector: np.ndarray) -> np.ndarray: value = np.asarray(vector, dtype=float).reshape(3) norm = float(np.linalg.norm(value)) return value / norm if np.isfinite(norm) and norm > 0.0 else np.full(3, np.nan) def _baseline_angle_deg(left: np.ndarray, right: np.ndarray) -> float: a, b = _unit_vector(left), _unit_vector(right) if not (np.all(np.isfinite(a)) and np.all(np.isfinite(b))): return np.nan return float(np.degrees(np.arccos(np.clip(np.dot(a, b), -1.0, 1.0)))) def _interpolate_baseline(left: np.ndarray, right: np.ndarray, fraction: float) -> np.ndarray: # Normalized linear interpolation is stable for the sub-second bridge range. return _unit_vector((1.0 - fraction) * _unit_vector(left) + fraction * _unit_vector(right)) def _hpr_factor_observation(series: _HprSeries, t_s: float) -> tuple[np.ndarray, int, bool, str, float]: """Return optional Q4 baseline support without changing trajectory continuity.""" valid_indices = series.valid_indices if valid_indices.size == 0 or not np.isfinite(t_s): return np.full(3, np.nan), -1, False, "unavailable", np.nan nearest_local = int(np.argmin(np.abs(series.t_s[valid_indices] - t_s))) nearest = int(valid_indices[nearest_local]) nearest_dt = float(abs(series.t_s[nearest] - t_s)) if nearest_dt <= HPR_NEAREST_Q4_TOLERANCE_S: # A single gross direction spike is a rejected factor, not a trajectory cut. nearest_position = int(np.searchsorted(valid_indices, nearest)) if 0 < nearest_position < valid_indices.size - 1: left, right = int(valid_indices[nearest_position - 1]), int(valid_indices[nearest_position + 1]) gap = float(series.t_s[right] - series.t_s[left]) if 0.0 < gap <= 2.0 * HPR_BRIDGE_MAX_GAP_S: expected = _interpolate_baseline( series.baseline_enu[left], series.baseline_enu[right], float((series.t_s[nearest] - series.t_s[left]) / gap), ) if _baseline_angle_deg(series.baseline_enu[nearest], expected) > HPR_ISOLATED_OUTLIER_DEG: return np.full(3, np.nan), nearest, False, "isolated_outlier", gap return series.baseline_enu[nearest], nearest, True, "nearest_q4", nearest_dt right_local = int(np.searchsorted(series.t_s[valid_indices], t_s, side="right")) if 0 < right_local < valid_indices.size: left, right = int(valid_indices[right_local - 1]), int(valid_indices[right_local]) gap = float(series.t_s[right] - series.t_s[left]) if 0.0 < gap <= HPR_BRIDGE_MAX_GAP_S: fraction = float((t_s - series.t_s[left]) / gap) return (_interpolate_baseline(series.baseline_enu[left], series.baseline_enu[right], fraction), -1, True, "bracket_interpolation", gap) return np.full(3, np.nan), -1, False, "unavailable", np.nan def _node_interval_threshold_s(period_s: float) -> float: return max(1.5, 1.75 * float(period_s)) def _trajectory_continuity_reasons(session: UnifiedSession, t0: float, t1: float, period_s: float) -> tuple[str, ...]: """Structural R0 continuity: device time and IMU preintegration coverage only.""" dt = float(t1 - t0) threshold = _node_interval_threshold_s(period_s) reasons: list[str] = [] if not np.isfinite(dt) or dt <= 0.0: reasons.append("measurement_time_nonmonotonic") elif dt > threshold: reasons.append("measurement_gap") imu_t = session.imu.t_s if (imu_t.size < 2 or t0 < imu_t[0] or t1 > imu_t[-1] or not _imu_continuous(session, t0, t1)): reasons.append("imu_device_time_gap_or_preintegration_uncovered") return tuple(reasons) def _hpr_chain_continuous(series: _HprSeries, left: int, right: int) -> bool: """Legacy diagnostic only; never use this to split R0 trajectories.""" if left < 0 or right < left or right >= series.t_s.size: return False if not np.all(series.valid[left:right + 1]): return False if left == right: return True dt = np.diff(series.t_s[left:right + 1]) return bool(np.all(np.isfinite(dt)) and np.all(dt >= 0.03) and np.all(dt <= 0.25)) def _all_position_rows(session: UnifiedSession) -> list[tuple[dict[str, str], str]]: rows = [(row, "BESTNAVA") for row in session.rtk_by_type.get("BESTNAVA", [])] rows.extend((row, "GGA") for row in session.rtk_by_type.get("GGA", [])) return sorted(rows, key=lambda item: (_f(item[0], "t_device_s"), 0 if item[1] == "BESTNAVA" else 1)) def _position_valid(row: dict[str, str], source: str) -> bool: fixed = _truth(row, "position_fixed") if source == "BESTNAVA" else int(_f(row, "fix_quality", -1)) == 4 required = (_f(row, "t_device_s"), _f(row, "lat_deg"), _f(row, "lon_deg")) if source == "BESTNAVA": required += (_f(row, "altitude_m"),) return bool(_truth(row, "checksum_valid") and fixed and all(np.isfinite(value) for value in required)) def _height_reference(sessions: Sequence[UnifiedSession]) -> tuple[float, float, float, bool] | None: for session in sessions: for row in session.rtk_by_type.get("BESTNAVA", []): if _position_valid(row, "BESTNAVA"): return _f(row, "lat_deg"), _f(row, "lon_deg"), _f(row, "altitude_m"), True for session in sessions: for row in session.rtk_by_type.get("GGA", []): if _position_valid(row, "GGA"): return _f(row, "lat_deg"), _f(row, "lon_deg"), 0.0, False return None def _enu(row: dict[str, str], source: str, reference: tuple[float, float, float, bool]) -> tuple[np.ndarray, np.ndarray]: lat0, lon0, alt0, has_best_height = reference lat, lon = _f(row, "lat_deg"), _f(row, "lon_deg") north = np.deg2rad(lat - lat0) * EARTH_RADIUS_M east = np.deg2rad(lon - lon0) * EARTH_RADIUS_M * np.cos(np.deg2rad(lat0)) if source == "BESTNAVA" and has_best_height: return np.array([east, north, _f(row, "altitude_m") - alt0]), np.ones(3, dtype=bool) # Never let a GGA MSL altitude define or constrain the BESTNAVA Z datum. return np.array([east, north, 0.0]), np.array([True, True, False]) def _best_velocity_series(session: UnifiedSession) -> tuple[np.ndarray, np.ndarray]: times, vectors = [], [] for row in session.rtk_by_type.get("BESTNAVA", []): value = np.array([_f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"), _f(row, "vertical_speed_m_s")]) if (_truth(row, "checksum_valid") and _truth(row, "position_fixed") and _truth(row, "doppler_velocity_valid") and np.all(np.isfinite(value))): times.append(_f(row, "t_device_s")) vectors.append(value) order = np.argsort(times) return np.asarray(times)[order], np.asarray(vectors).reshape(-1, 3)[order] def _motion_flags(session: UnifiedSession, t_s: float, velocity_t: np.ndarray, velocity: np.ndarray, window_s: float = 1.5) -> tuple[bool, bool]: """Classify gravity/ZUPT in a local device-time slice, never a host-time window.""" half = 0.5 * window_s imu_left = int(np.searchsorted(session.imu.t_s, t_s - half, side="left")) imu_right = int(np.searchsorted(session.imu.t_s, t_s + half, side="right")) imu_t = session.imu.t_s[imu_left:imu_right] if imu_t.size < 20 or imu_t[-1] - imu_t[0] < 1.2: return False, False gyro = session.imu.gyro_rad_s[imu_left:imu_right] accel = session.imu.acc_m_s2[imu_left:imu_right] gravity_candidate = bool( np.linalg.norm(np.mean(gyro, axis=0)) <= np.deg2rad(0.35) and np.max(np.std(gyro, axis=0)) <= np.deg2rad(0.10) and abs(float(np.mean(np.linalg.norm(accel, axis=1))) - G0) <= 0.15 and float(np.std(np.linalg.norm(accel, axis=1))) <= 0.06 and np.max(np.std(accel, axis=0)) <= 0.10 ) if not gravity_candidate or velocity_t.size == 0: return gravity_candidate, False velocity_left = int(np.searchsorted(velocity_t, t_s - half, side="left")) velocity_right = int(np.searchsorted(velocity_t, t_s + half, side="right")) local_t, local_v = velocity_t[velocity_left:velocity_right], velocity[velocity_left:velocity_right] if local_t.size < 3 or local_t[-1] - local_t[0] < 1.0 or np.any(np.diff(local_t) > 0.75): return gravity_candidate, False speed = np.linalg.norm(local_v, axis=1) zupt = bool(np.max(speed) <= 0.12 and np.median(speed) <= 0.05) return gravity_candidate, zupt def _imu_continuous(session: UnifiedSession, t0: float, t1: float) -> bool: left = max(int(np.searchsorted(session.imu.t_s, t0, side="right")) - 1, 0) right = min(int(np.searchsorted(session.imu.t_s, t1, side="left")) + 1, session.imu.t_s.size) local = session.imu.t_s[left:right] return bool(local.size >= 2 and np.all(np.diff(local) > 0.0) and np.all(np.diff(local) <= 0.05)) def _source_nodes(session: UnifiedSession, reference: tuple[float, float, float, bool], period_s: float, source: str) -> list[_Node]: """Build one position source's nodes; HPR only annotates optional factors.""" hpr = _all_hpr(session) velocity_t, velocity_values = _best_velocity_series(session) bad_position_rows: set[int] = set() last_device_t = -np.inf previous_row: dict[str, str] | None = None for raw_row in session.rtk_by_type.get(source, []): raw_t = _f(raw_row, "t_device_s") if not np.isfinite(raw_t) or raw_t <= last_device_t: bad_position_rows.add(id(raw_row)) if previous_row is not None: bad_position_rows.add(id(previous_row)) if np.isfinite(raw_t): last_device_t = raw_t previous_row = raw_row result: list[_Node] = [] continuity_id = 0 last_selected: _Node | None = None pending_position_time_break = False # Tolerate normal timestamp jitter: the old `next_t=t+period` skipped a # nominal 1 Hz sample whenever it arrived microscopically early. min_selected_spacing_s = 0.75 * float(period_s) rows = sorted(session.rtk_by_type.get(source, []), key=lambda row: _f(row, "t_device_s")) for row in rows: t = _f(row, "t_device_s") if id(row) in bad_position_rows: # Preserve a raw device-time discontinuity after sorting/dropping # the malformed rows: it is a structural trajectory break. pending_position_time_break = True continuity_id += 1 last_selected = None continue if not _position_valid(row, source): # A rejected position factor alone is not allowed to create an # HPR-like artificial trajectory split. continue if last_selected is not None and t - last_selected.t_s < min_selected_spacing_s: continue imu_index = _nearest_index(session.imu.t_s, t, 0.03) if imu_index is None: continuity_id += 1 last_selected = None continue p_enu, mask = _enu(row, source, reference) row_velocity = None if source == "BESTNAVA" and _truth(row, "doppler_velocity_valid"): candidate = np.array([_f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"), _f(row, "vertical_speed_m_s")]) if np.all(np.isfinite(candidate)): row_velocity = candidate gravity_candidate, zupt_static = _motion_flags(session, t, velocity_t, velocity_values) baseline, hpr_index, hpr_valid, hpr_method, hpr_gap = _hpr_factor_observation(hpr, t) node = _Node( t_s=float(t), source=source, p_enu_m=p_enu, position_mask=mask, baseline_enu=baseline, hpr_index=hpr_index, hpr_factor_valid=hpr_valid, hpr_factor_method=hpr_method, hpr_support_gap_s=hpr_gap, hpr_angular_sigma_rad=_hpr_angular_sigma_rad(hpr_method, hpr_gap) if hpr_valid else np.inf, position_time_discontinuity_before=pending_position_time_break, gyro_rad_s=session.imu.gyro_rad_s[imu_index], accel_m_s2=session.imu.acc_m_s2[imu_index], velocity_enu_m_s=row_velocity, gravity_candidate=gravity_candidate, zupt_static=zupt_static, continuity_id=continuity_id, ) if last_selected is not None: if _trajectory_continuity_reasons(session, last_selected.t_s, node.t_s, period_s): continuity_id += 1 node = _Node(**{**node.__dict__, "continuity_id": continuity_id}) result.append(node) last_selected = node pending_position_time_break = False return result def _nodes(session: UnifiedSession, reference: tuple[float, float, float, bool], period_s: float) -> list[_Node]: """Prefer BESTNAVA; let GGA fill only intervals absent from BESTNAVA.""" best = _source_nodes(session, reference, period_s, "BESTNAVA") gga = _source_nodes(session, reference, period_s, "GGA") if best: best_t = np.asarray([node.t_s for node in best]) keep_gga: list[_Node] = [] for node in gga: right = int(np.searchsorted(best_t, node.t_s)) candidates = best_t[max(right - 1, 0):min(right + 1, best_t.size)] if candidates.size == 0 or np.min(np.abs(candidates - node.t_s)) > period_s: keep_gga.append(node) else: keep_gga = gga merged = sorted([*best, *keep_gga], key=lambda node: (node.t_s, 0 if node.source == "BESTNAVA" else 1)) result: list[_Node] = [] continuity_id = 0 previous: _Node | None = None for node in merged: if previous is not None: # Per-source continuity ids cannot be compared across BEST/GGA: # an alternate source may legitimately bridge a missing position row. # The merged trajectory is continuous exactly when its device-time # interval has usable IMU preintegration coverage. if (node.position_time_discontinuity_before or _trajectory_continuity_reasons(session, previous.t_s, node.t_s, period_s)): continuity_id += 1 node = _Node(**{**node.__dict__, "continuity_id": continuity_id}) result.append(node) previous = node return result def _segments(sessions: Iterable[UnifiedSession], period_s: float) -> list[_Segment]: sessions = list(sessions) reference = _height_reference(sessions) if reference is None: return [] result: list[_Segment] = [] for session in sessions: nodes = _nodes(session, reference, period_s) if not nodes: continue start = 0 qualifying_index = 0 for end in range(1, len(nodes) + 1): split = end == len(nodes) or nodes[end].continuity_id != nodes[end - 1].continuity_id if not split: continue run, start = nodes[start:end], end if (len(run) < MIN_SEGMENT_NODE_COUNT or run[-1].t_s - run[0].t_s < MIN_SEGMENT_DURATION_S): continue pre = tuple(preintegrate_imu( session.imu.t_s, session.imu.gyro_rad_s, session.imu.acc_m_s2, left.t_s, right.t_s, ) for left, right in zip(run[:-1], run[1:])) if any(item.duration_s <= 0.0 for item in pre): continue initial_hpr = next((node for node in run if node.hpr_factor_valid), None) if initial_hpr is None: continue result.append(_Segment( f"{session.session_id}:{qualifying_index:02d}", session.session_id, tuple(run), pre, _world_rtk(initial_hpr.baseline_enu) )) qualifying_index += 1 return result def _initial_parameters(segments: Sequence[_Segment]) -> np.ndarray: x = np.zeros(3 + NUISANCE_DOF_PER_SEGMENT * len(segments)) for index, segment in enumerate(segments): offset = 3 + NUISANCE_DOF_PER_SEGMENT * index x[offset + 3:offset + 6] = segment.nodes[0].p_enu_m return x def _residual(x: np.ndarray, segments: Sequence[_Segment], R_RTK_IMU: np.ndarray, details: dict[str, list[np.ndarray]] | None = None, lever_prior_l_I_m: np.ndarray | None = None, lever_prior_cholesky: np.ndarray | None = None) -> np.ndarray: l_I, values = x[:3], [] baseline_I = R_RTK_IMU.T[:, 0] for index, segment in enumerate(segments): offset = 3 + NUISANCE_DOF_PER_SEGMENT * index dtheta = x[offset:offset + 3] p_I = x[offset + 3:offset + 6].copy() velocity = x[offset + 6:offset + 9].copy() bg = x[offset + 9:offset + 12] ba = x[offset + 12:offset + 15] R_WI = segment.R_WRTK_initial @ R_RTK_IMU @ so3_exp(dtheta) for node_index, node in enumerate(segment.nodes): p_error = p_I + R_WI @ l_I - node.p_enu_m if node.source == "GGA": values.extend(p_error[:2] / 0.06) if details is not None: details["gga_xy"].append(p_error[:2]) else: values.extend(p_error / np.array([0.06, 0.06, 0.12])) if details is not None: details["bestnava_xyz"].append(p_error) if node.hpr_factor_valid: values.extend(np.cross(R_WI @ baseline_I, node.baseline_enu) / node.hpr_angular_sigma_rad) if node.velocity_enu_m_s is not None: v_error = velocity + R_WI @ np.cross(node.gyro_rad_s - bg, l_I) - node.velocity_enu_m_s values.extend(v_error / np.array([0.15, 0.15, 0.30])) if details is not None: details["doppler_velocity"].append(v_error) if node.gravity_candidate: gravity_error = node.accel_m_s2 - ba - R_WI.T @ (-G_ENU) values.extend(gravity_error / 0.12) if details is not None: details["gravity"].append(gravity_error) if node.zupt_static: values.extend(velocity / 0.10) if details is not None: details["zupt"].append(velocity.copy()) if node_index == len(segment.preintegrations): continue pre = segment.preintegrations[node_index] dR, dv, dp = apply_bias_correction_imu(pre, bg, ba) dt = pre.duration_s p_I = p_I + velocity * dt + 0.5 * G_ENU * dt * dt + R_WI @ dp velocity = velocity + G_ENU * dt + R_WI @ dv R_WI = R_WI @ dR values.extend(dtheta / np.deg2rad(5.0)) values.extend(bg / 0.02) values.extend(ba / 0.50) if lever_prior_l_I_m is not None and lever_prior_cholesky is not None: values.extend(np.linalg.solve(lever_prior_cholesky, l_I - lever_prior_l_I_m)) return np.asarray(values, dtype=float) def _segment_residual_size(segment: _Segment) -> int: size = 9 # dtheta, gyro-bias, and accel-bias priors for node in segment.nodes: size += 2 if node.source == "GGA" else 3 size += 3 if node.hpr_factor_valid else 0 # optional ANT1-to-ANT2 direction size += 3 if node.velocity_enu_m_s is not None else 0 size += 3 if node.gravity_candidate else 0 size += 3 if node.zupt_static else 0 return size def _dense_colored_jacobian(x: np.ndarray, segments: Sequence[_Segment], R_RTK_IMU: np.ndarray, lever_prior_l_I_m: np.ndarray | None = None, lever_prior_cholesky: np.ndarray | None = None) -> np.ndarray: """Finite-difference the block-separable Jacobian using 18 color groups.""" base = _residual(x, segments, R_RTK_IMU, lever_prior_l_I_m=lever_prior_l_I_m, lever_prior_cholesky=lever_prior_cholesky) jacobian = np.zeros((base.size, x.size), dtype=float) step_scale = np.sqrt(np.finfo(float).eps) row_slices: list[slice] = [] row = 0 for segment in segments: count = _segment_residual_size(segment) row_slices.append(slice(row, row + count)) row += count for column in range(3): step = step_scale * max(1.0, abs(float(x[column]))) shifted = x.copy() shifted[column] += step jacobian[:, column] = (_residual( shifted, segments, R_RTK_IMU, lever_prior_l_I_m=lever_prior_l_I_m, lever_prior_cholesky=lever_prior_cholesky, ) - base) / step for local_column in range(NUISANCE_DOF_PER_SEGMENT): shifted = x.copy() steps = [] for segment_index in range(len(segments)): column = 3 + NUISANCE_DOF_PER_SEGMENT * segment_index + local_column step = step_scale * max(1.0, abs(float(x[column]))) shifted[column] += step steps.append(step) difference = _residual( shifted, segments, R_RTK_IMU, lever_prior_l_I_m=lever_prior_l_I_m, lever_prior_cholesky=lever_prior_cholesky, ) - base for segment_index, rows in enumerate(row_slices): column = 3 + NUISANCE_DOF_PER_SEGMENT * segment_index + local_column jacobian[rows, column] = difference[rows] / steps[segment_index] return jacobian def _fit_segments(segments: Sequence[_Segment], R_RTK_IMU: np.ndarray, *, lever_prior_l_I_m: np.ndarray | None = None, lever_prior_covariance_m2: np.ndarray | None = None, max_nfev: int = 80): details = {"gga_xy": [], "bestnava_xyz": [], "doppler_velocity": [], "gravity": [], "zupt": []} if not segments: return None, np.zeros(0), details if (lever_prior_l_I_m is None) != (lever_prior_covariance_m2 is None): raise ValueError("lever prior mean and covariance must be provided together") prior_l_I = None if lever_prior_l_I_m is None else np.asarray(lever_prior_l_I_m, dtype=float).reshape(3) prior_cholesky = None if lever_prior_covariance_m2 is not None: covariance = np.asarray(lever_prior_covariance_m2, dtype=float).reshape(3, 3) prior_cholesky = np.linalg.cholesky(covariance) x0 = _initial_parameters(segments) fit = least_squares( lambda value: _residual(value, segments, R_RTK_IMU, lever_prior_l_I_m=prior_l_I, lever_prior_cholesky=prior_cholesky), x0, jac=lambda value: _dense_colored_jacobian( value, segments, R_RTK_IMU, prior_l_I, prior_cholesky, ), loss="huber", f_scale=1.5, max_nfev=max_nfev, ) residual = _residual(fit.x, segments, R_RTK_IMU, details, prior_l_I, prior_cholesky) return fit, residual, details def _marginal_lever_information(jacobian: np.ndarray, residual: np.ndarray): jacobian = np.asarray(jacobian, dtype=float) hessian = jacobian.T @ jacobian H_ll = hessian[:3, :3] H_ln = hessian[:3, 3:] H_nn = hessian[3:, 3:] marginal = H_ll.copy() nuisance_size = H_nn.shape[0] if nuisance_size: # Every engineering segment owns an independent 15-DoF nuisance block. # Marginalize those blocks separately: one global relative pinv cutoff # can otherwise discard valid modes from smaller-motion blocks and make # full information disagree with the sum of category information. if nuisance_size % NUISANCE_DOF_PER_SEGMENT == 0: for start in range(0, nuisance_size, NUISANCE_DOF_PER_SEGMENT): stop = start + NUISANCE_DOF_PER_SEGMENT H_l_block = H_ln[:, start:stop] H_block = H_nn[start:stop, start:stop] marginal -= H_l_block @ np.linalg.pinv(H_block, rcond=1e-10) @ H_l_block.T else: marginal -= H_ln @ np.linalg.pinv(H_nn, rcond=1e-10) @ H_ln.T marginal = 0.5 * (marginal + marginal.T) _, singular, vh = np.linalg.svd(marginal) threshold = max(float(singular[0]) * 1e-6, 1e-8) if singular.size else np.inf rank = int(np.count_nonzero(singular > threshold)) condition = float(singular[0] / singular[-1]) if singular[-1] > threshold else np.inf weakest = vh[-1] if vh.size else np.full(3, np.nan) dof = max(jacobian.shape[0] - jacobian.shape[1], 1) variance = float(np.dot(residual, residual) / dof) covariance = variance * np.linalg.pinv(marginal, rcond=1e-10) return marginal, singular, condition, rank, weakest, covariance def _fit_summary(fit, residual: np.ndarray, detail: dict[str, list[np.ndarray]]) -> LeverFitSummary | None: if fit is None: return None marginal, singular, condition, rank, weak, covariance = _marginal_lever_information(fit.jac, residual) std = np.sqrt(np.maximum(np.diag(covariance), 0.0)) return LeverFitSummary( l_I_m=fit.x[:3].copy(), l_I_marginal_covariance_m2=covariance, l_I_std_m=std, lever_marginal_information=marginal, lever_information_singular_values=singular, lever_information_condition_number=condition, lever_precision_rank=rank, weakest_lever_direction_I=weak, gga_xy_residual=_audit(detail["gga_xy"], 2), bestnava_xyz_residual=_audit(detail["bestnava_xyz"], 3), doppler_velocity_residual=_audit(detail["doppler_velocity"], 3), optimizer_converged=bool(fit.success and np.all(np.isfinite(fit.x))), ) def _fit_fixed_lever_residual( segments: Sequence[_Segment], R_RTK_IMU: np.ndarray, l_I_m: np.ndarray, *, max_nfev: int = 80, ) -> FixedLeverResidualSummary | None: """Profile nuisance states at a fixed mechanical lever arm for residual comparison.""" if not segments: return None lever = np.asarray(l_I_m, dtype=float).reshape(3) initial = _initial_parameters(segments)[3:] def compose(nuisance: np.ndarray) -> np.ndarray: return np.concatenate([lever, nuisance]) fit = least_squares( lambda nuisance: _residual(compose(nuisance), segments, R_RTK_IMU), initial, loss="huber", f_scale=1.5, max_nfev=max_nfev, ) detail = {"gga_xy": [], "bestnava_xyz": [], "doppler_velocity": [], "gravity": [], "zupt": []} _residual(compose(fit.x), segments, R_RTK_IMU, detail) return FixedLeverResidualSummary( l_I_m=lever.copy(), gga_xy_residual=_audit(detail["gga_xy"], 2), bestnava_xyz_residual=_audit(detail["bestnava_xyz"], 3), doppler_velocity_residual=_audit(detail["doppler_velocity"], 3), optimizer_converged=bool(fit.success and np.all(np.isfinite(fit.x))), ) def _resample_segments_with_multiplicity(segments: Sequence[_Segment], drawn_session_ids: Sequence[str]) -> list[_Segment]: by_session = { session_id: [segment for segment in segments if segment.session_id == session_id] for session_id in set(drawn_session_ids) } return [segment for session_id in drawn_session_ids for segment in by_session.get(session_id, [])] def _subset_value(segments: Sequence[_Segment], R_RTK_IMU: np.ndarray, *, lever_prior_l_I_m: np.ndarray | None = None, lever_prior_covariance_m2: np.ndarray | None = None, max_nfev: int = 80) -> np.ndarray | None: fit, _, _ = _fit_segments( segments, R_RTK_IMU, lever_prior_l_I_m=lever_prior_l_I_m, lever_prior_covariance_m2=lever_prior_covariance_m2, max_nfev=max_nfev, ) return None if fit is None else fit.x[:3] def _solver_health_gates(fit, std: np.ndarray, rank: int, condition: float, gga: ResidualAudit, best: ResidualAudit, velocity: ResidualAudit) -> dict[str, bool]: return { "optimizer_converged_and_finite": bool( fit is not None and fit.success and np.all(np.isfinite(fit.x)) ), "lever_std_below_0p5m": bool(np.all(np.isfinite(std)) and np.max(std) <= 0.50), "lever_precision_rank_at_least_2": rank >= 2, "lever_condition_below_1e10": bool(np.isfinite(condition) and condition <= 1e10), "position_residual_not_diverged": bool( max(gga.vector_p95 if gga.count else 0.0, best.vector_p95 if best.count else 0.0) <= 2.0 ), "velocity_residual_not_diverged": bool(velocity.count and velocity.vector_p95 <= 3.0), } def _engineering_gates(std: np.ndarray, singular: np.ndarray, rank: int, condition: float, gga: ResidualAudit, best: ResidualAudit, velocity: ResidualAudit, loo: dict[str, float], bootstrap_std: np.ndarray, bootstrap_complete: bool, sensitivity_max: float, sensitivity_complete: bool, manual_delta: np.ndarray | None, free_translation_observable: bool, free_manual_mahalanobis_d2: float | None) -> dict[str, bool]: return { "lever_marginal_std": bool(np.all(np.isfinite(std)) and np.all(std <= np.array([0.15, 0.15, 0.20]))), "lever_information_rank": rank == 3, "lever_information_condition": bool(np.isfinite(condition) and condition <= 1e6), "lever_min_information": bool(singular.size == 3 and singular[-1] >= 1e-3), "bestnava_xyz_rms_p95": bool( best.count >= 10 and np.all(best.axis_rms <= np.array([0.20, 0.20, 0.35])) and np.all(best.axis_p95_abs <= np.array([0.40, 0.40, 0.70])) ), "gga_xy_rms_p95": bool( gga.count == 0 or (np.all(gga.axis_rms <= 0.20) and np.all(gga.axis_p95_abs <= 0.40)) ), "doppler_velocity_rms_p95": bool( velocity.count >= 10 and np.all(velocity.axis_rms <= np.array([0.30, 0.30, 0.50])) and np.all(velocity.axis_p95_abs <= np.array([0.60, 0.60, 1.00])) ), "leave_one_session_out": bool(len(loo) >= 3 and max(loo.values(), default=np.inf) <= 0.20), "bootstrap_complete_and_stable": bool( bootstrap_complete and np.all(np.isfinite(bootstrap_std)) and np.max(bootstrap_std) <= 0.20 ), "rotation_sensitivity_complete_and_stable": bool( sensitivity_complete and np.isfinite(sensitivity_max) and sensitivity_max <= 0.10 ), "manual_lever_consistency": bool( manual_delta is None or np.linalg.norm(manual_delta) <= 0.30 ), "free_manual_mahalanobis_consistency": bool( not free_translation_observable or free_manual_mahalanobis_d2 is None or free_manual_mahalanobis_d2 <= FREE_MANUAL_MAHALANOBIS_99_DF3 ), } def _free_translation_observability(summary: LeverFitSummary) -> dict[str, bool]: return { "free_optimizer_converged": summary.optimizer_converged, "free_lever_marginal_std": bool( np.all(np.isfinite(summary.l_I_std_m)) and np.all(summary.l_I_std_m <= np.array([0.15, 0.15, 0.20])) ), "free_lever_information_rank": summary.lever_precision_rank == 3, "free_lever_information_condition": bool( np.isfinite(summary.lever_information_condition_number) and summary.lever_information_condition_number <= 1e6 ), "free_min_information": bool( summary.lever_information_singular_values.size == 3 and summary.lever_information_singular_values[-1] >= 1e-3 ), } def solve_engineering_6dof( sessions: list[UnifiedSession], *, R_RTK_IMU: np.ndarray, manual_l_I_m: np.ndarray | None = None, manual_l_I_covariance_m2: np.ndarray | None = None, sample_period_s: float = 0.5, run_loo: bool = True, run_bootstrap: bool = False, bootstrap_repetitions: int = 40, bootstrap_seed: int = 0, run_rotation_sensitivity: bool = False, selected_segment_ids: set[str] | None = None, ) -> Engineering6DofResult: """Fit free and optionally mechanically-prior-constrained ``l_I=p_ANT1^I``.""" R = np.asarray(R_RTK_IMU, dtype=float).reshape(3, 3) manual = None if manual_l_I_m is None else np.asarray(manual_l_I_m, dtype=float).reshape(3) prior_covariance = ( None if manual_l_I_covariance_m2 is None else np.asarray(manual_l_I_covariance_m2, dtype=float).reshape(3, 3) ) if (manual is None) != (prior_covariance is None): raise ValueError("manual_l_I_m and manual_l_I_covariance_m2 must be provided together") if prior_covariance is not None: if not np.allclose(prior_covariance, prior_covariance.T, atol=1e-12): raise ValueError("manual_l_I_covariance_m2 must be symmetric") np.linalg.cholesky(prior_covariance) segments = _segments(sessions, sample_period_s) if selected_segment_ids is not None: selected = set(selected_segment_ids) unknown = selected.difference(segment.segment_id for segment in segments) if unknown: raise ValueError(f"unknown selected_segment_ids: {sorted(unknown)}") segments = [segment for segment in segments if segment.segment_id in selected] free_fit, free_residual, free_detail = _fit_segments(segments, R) free_solution = _fit_summary(free_fit, free_residual, free_detail) nan3, nan33 = np.full(3, np.nan), np.full((3, 3), np.nan) empty2, empty3 = _empty_audit(2), _empty_audit(3) if free_solution is None: return Engineering6DofResult( rotation_source="R2G_gravity_level_prior", translation_conditional_on_rotation=True, R_RTK_IMU=R, l_I_m=None, l_I_marginal_covariance_m2=nan33, l_I_std_m=nan3, lever_marginal_information=nan33, lever_information_singular_values=nan3, lever_information_condition_number=np.inf, lever_precision_rank=0, weakest_lever_direction_I=nan3, gga_xy_residual=empty2, bestnava_xyz_residual=empty3, doppler_velocity_residual=empty3, session_count=0, segment_count=0, gga_factor_count=0, bestnava_factor_count=0, velocity_factor_count=0, gravity_candidate_factor_count=0, zupt_static_factor_count=0, leave_one_session_out_l_I_m={}, leave_one_session_out_delta_m={}, bootstrap_l_I_mean_m=nan3, bootstrap_l_I_std_m=nan3, bootstrap_success_count=0, bootstrap_requested_count=bootstrap_repetitions, rotation_sensitivity_delta_l_I_m={}, rotation_sensitivity_max_delta_m=np.inf, manual_l_I_m=manual, manual_l_I_delta_m=None, solver_health_gates={"optimizer_converged_and_finite": False}, solver_health_passed=False, engineering_acceptance_gates={"base_fit_available": False}, T_RTK_IMU=None, T_IMU_RTK=None, engineering_6dof_accepted=False, blockers=("no continuous Fixed GNSS + Q4 GNHPR segment",), notes=("data_only_6dof_accepted remains false",), translation_prior_applied=False, lever_prior_covariance_m2=prior_covariance, mechanical_reference_l_I_m=manual, free_solution=None, prior_constrained_solution=None, free_to_mechanical_delta_m=None, prior_to_mechanical_delta_m=None, free_translation_observable=False, data_only_translation_accepted=False, free_manual_mahalanobis_d2=None, free_translation_observability_gates={"free_fit_available": False}, posterior_to_prior_covariance_ratio=None, free_to_prior_residual_delta=None, mechanical_reference_solution=None, residual_comparison=None, ) prior_fit = prior_residual = prior_detail = None prior_solution: LeverFitSummary | None = None if manual is not None: prior_fit, prior_residual, prior_detail = _fit_segments( segments, R, lever_prior_l_I_m=manual, lever_prior_covariance_m2=prior_covariance, ) prior_solution = _fit_summary(prior_fit, prior_residual, prior_detail) mechanical_solution = ( None if manual is None else _fit_fixed_lever_residual(segments, R, manual) ) active_fit = prior_fit if prior_solution is not None else free_fit active_detail = prior_detail if prior_solution is not None else free_detail active_solution = prior_solution if prior_solution is not None else free_solution assert active_fit is not None and active_detail is not None and active_solution is not None l_I = active_solution.l_I_m ids = sorted({segment.session_id for segment in segments}) loo_l: dict[str, np.ndarray] = {} loo_delta: dict[str, float] = {} if run_loo: for session_id in ids: kept = [segment for segment in segments if segment.session_id != session_id] value = _subset_value( kept, R, lever_prior_l_I_m=manual, lever_prior_covariance_m2=prior_covariance, ) if value is not None: loo_l[session_id] = value loo_delta[session_id] = float(np.linalg.norm(value - l_I)) rng = np.random.default_rng(bootstrap_seed) bootstrap: list[np.ndarray] = [] if run_bootstrap and len(ids) >= 2: for _ in range(bootstrap_repetitions): drawn = list(rng.choice(ids, size=len(ids), replace=True)) sampled = _resample_segments_with_multiplicity(segments, drawn) value = _subset_value( sampled, R, lever_prior_l_I_m=manual, lever_prior_covariance_m2=prior_covariance, ) if value is not None: bootstrap.append(value) bootstrap_array = np.asarray(bootstrap) bootstrap_mean = np.mean(bootstrap_array, axis=0) if bootstrap else nan3 bootstrap_std = np.std(bootstrap_array, axis=0, ddof=1) if len(bootstrap) > 1 else nan3 bootstrap_complete = bool(run_bootstrap and len(bootstrap) == bootstrap_repetitions and bootstrap_repetitions >= 10) sensitivity: dict[str, np.ndarray] = {} if run_rotation_sensitivity: for axis, name in enumerate("xyz"): for angle in (0.2, 0.3, 0.5): for sign in (-1.0, 1.0): vector = np.zeros(3) vector[axis] = np.deg2rad(sign * angle) perturbed = Rotation.from_rotvec(vector).as_matrix() @ R value = _subset_value( segments, perturbed, lever_prior_l_I_m=manual, lever_prior_covariance_m2=prior_covariance, ) if value is not None: sensitivity[f"RTK_{name}_{sign * angle:+.1f}deg"] = value - l_I sensitivity_max = max((float(np.linalg.norm(value)) for value in sensitivity.values()), default=np.inf) sensitivity_complete = bool(run_rotation_sensitivity and len(sensitivity) == 18) active_manual_delta = None if manual is None else l_I - manual free_manual_delta = None if manual is None else free_solution.l_I_m - manual prior_manual_delta = None if prior_solution is None or manual is None else prior_solution.l_I_m - manual free_observability_gates = _free_translation_observability(free_solution) free_translation_observable = all(free_observability_gates.values()) free_manual_mahalanobis_d2: float | None = None if manual is not None and free_translation_observable: combined_covariance = free_solution.l_I_marginal_covariance_m2 + prior_covariance try: free_manual_mahalanobis_d2 = float( free_manual_delta @ np.linalg.solve(combined_covariance, free_manual_delta) ) except np.linalg.LinAlgError: free_manual_mahalanobis_d2 = np.inf posterior_to_prior_covariance_ratio: np.ndarray | None = None free_to_prior_residual_delta: dict[str, float] | None = None residual_comparison: dict[str, float] | None = None if prior_solution is not None and prior_covariance is not None: # This ratio uses information matrices, not residual-scale covariance. # Known factor sigmas already define the statistical scale of the prior. posterior_covariance_by_information = np.linalg.pinv( prior_solution.lever_marginal_information, rcond=1e-10 ) posterior_to_prior_covariance_ratio = np.diag( posterior_covariance_by_information ) / np.diag(prior_covariance) free_to_prior_residual_delta = { "bestnava_xyz_vector_rms_m": float( prior_solution.bestnava_xyz_residual.vector_rms - free_solution.bestnava_xyz_residual.vector_rms ), "bestnava_xyz_vector_p95_m": float( prior_solution.bestnava_xyz_residual.vector_p95 - free_solution.bestnava_xyz_residual.vector_p95 ), "doppler_velocity_vector_rms_m_s": float( prior_solution.doppler_velocity_residual.vector_rms - free_solution.doppler_velocity_residual.vector_rms ), "doppler_velocity_vector_p95_m_s": float( prior_solution.doppler_velocity_residual.vector_p95 - free_solution.doppler_velocity_residual.vector_p95 ), } residual_comparison = dict(free_to_prior_residual_delta) if mechanical_solution is not None: for name, summary in (("mechanical", mechanical_solution), ("prior", prior_solution)): residual_comparison[f"{name}_minus_free_bestnava_xyz_vector_rms_m"] = float( summary.bestnava_xyz_residual.vector_rms - free_solution.bestnava_xyz_residual.vector_rms ) residual_comparison[f"{name}_minus_free_bestnava_xyz_vector_p95_m"] = float( summary.bestnava_xyz_residual.vector_p95 - free_solution.bestnava_xyz_residual.vector_p95 ) residual_comparison[f"{name}_minus_free_doppler_velocity_vector_rms_m_s"] = float( summary.doppler_velocity_residual.vector_rms - free_solution.doppler_velocity_residual.vector_rms ) residual_comparison[f"{name}_minus_free_doppler_velocity_vector_p95_m_s"] = float( summary.doppler_velocity_residual.vector_p95 - free_solution.doppler_velocity_residual.vector_p95 ) residual_comparison["prior_minus_mechanical_bestnava_xyz_vector_rms_m"] = float( prior_solution.bestnava_xyz_residual.vector_rms - mechanical_solution.bestnava_xyz_residual.vector_rms ) residual_comparison["prior_minus_mechanical_doppler_velocity_vector_rms_m_s"] = float( prior_solution.doppler_velocity_residual.vector_rms - mechanical_solution.doppler_velocity_residual.vector_rms ) solver_gates = _solver_health_gates( active_fit, active_solution.l_I_std_m, active_solution.lever_precision_rank, active_solution.lever_information_condition_number, active_solution.gga_xy_residual, active_solution.bestnava_xyz_residual, active_solution.doppler_velocity_residual, ) engineering_gates = _engineering_gates( active_solution.l_I_std_m, active_solution.lever_information_singular_values, active_solution.lever_precision_rank, active_solution.lever_information_condition_number, active_solution.gga_xy_residual, active_solution.bestnava_xyz_residual, active_solution.doppler_velocity_residual, loo_delta, bootstrap_std, bootstrap_complete, sensitivity_max, sensitivity_complete, active_manual_delta, free_translation_observable, free_manual_mahalanobis_d2, ) if manual is not None and prior_solution is None: engineering_gates["prior_constrained_fit_available"] = False engineering_gates["legacy_deterministic_segment_model_not_deprecated"] = ( not LEGACY_DETERMINISTIC_SEGMENT_MODEL_DEPRECATED ) solver_health = all(solver_gates.values()) accepted = bool(solver_health and all(engineering_gates.values())) blockers = tuple( [f"solver_health:{name}" for name, passed in solver_gates.items() if not passed] + [f"engineering_acceptance:{name}" for name, passed in engineering_gates.items() if not passed] ) T_RTK_IMU = make_transform(-R @ l_I, R) T_IMU_RTK = make_transform(l_I, R.T) return Engineering6DofResult( rotation_source="R2G_gravity_level_prior", translation_conditional_on_rotation=True, R_RTK_IMU=R, l_I_m=l_I, l_I_marginal_covariance_m2=active_solution.l_I_marginal_covariance_m2, l_I_std_m=active_solution.l_I_std_m, lever_marginal_information=active_solution.lever_marginal_information, lever_information_singular_values=active_solution.lever_information_singular_values, lever_information_condition_number=active_solution.lever_information_condition_number, lever_precision_rank=active_solution.lever_precision_rank, weakest_lever_direction_I=active_solution.weakest_lever_direction_I, gga_xy_residual=active_solution.gga_xy_residual, bestnava_xyz_residual=active_solution.bestnava_xyz_residual, doppler_velocity_residual=active_solution.doppler_velocity_residual, session_count=len(ids), segment_count=len(segments), gga_factor_count=active_solution.gga_xy_residual.count, bestnava_factor_count=active_solution.bestnava_xyz_residual.count, velocity_factor_count=active_solution.doppler_velocity_residual.count, gravity_candidate_factor_count=len(active_detail["gravity"]), zupt_static_factor_count=len(active_detail["zupt"]), leave_one_session_out_l_I_m=loo_l, leave_one_session_out_delta_m=loo_delta, bootstrap_l_I_mean_m=bootstrap_mean, bootstrap_l_I_std_m=bootstrap_std, bootstrap_success_count=len(bootstrap), bootstrap_requested_count=bootstrap_repetitions, rotation_sensitivity_delta_l_I_m=sensitivity, rotation_sensitivity_max_delta_m=sensitivity_max, manual_l_I_m=manual, manual_l_I_delta_m=active_manual_delta, solver_health_gates=solver_gates, solver_health_passed=solver_health, engineering_acceptance_gates=engineering_gates, T_RTK_IMU=T_RTK_IMU, T_IMU_RTK=T_IMU_RTK, engineering_6dof_accepted=accepted, blockers=blockers, notes=( "data_only_6dof_accepted=false is intentionally unchanged", "R2V and HI13 absolute quaternion yaw are diagnostics only", "GGA contributes XY only; BESTNAVA defines and constrains the ENU Z datum", "BESTNAVA is selected before GGA; GGA fills only BESTNAVA gaps", "gravity_candidate is not ZUPT; ZUPT additionally requires continuous near-zero Doppler", "bootstrap preserves repeated-session multiplicity", "engineering acceptance remains false until bootstrap and all 18 rotation perturbations complete", "T_RTK_IMU origin is ANT1; l_I=p_ANT1^I and T_IMU_RTK translation equals l_I", ), translation_prior_applied=prior_solution is not None, lever_prior_covariance_m2=prior_covariance, mechanical_reference_l_I_m=manual, free_solution=free_solution, prior_constrained_solution=prior_solution, free_to_mechanical_delta_m=free_manual_delta, prior_to_mechanical_delta_m=prior_manual_delta, free_translation_observable=free_translation_observable, data_only_translation_accepted=free_translation_observable, free_manual_mahalanobis_d2=free_manual_mahalanobis_d2, free_translation_observability_gates=free_observability_gates, posterior_to_prior_covariance_ratio=posterior_to_prior_covariance_ratio, free_to_prior_residual_delta=free_to_prior_residual_delta, mechanical_reference_solution=mechanical_solution, residual_comparison=residual_comparison, )