1209 lines
57 KiB
Python
1209 lines
57 KiB
Python
"""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,
|
|
)
|