"""AZ-386 ``EskfStateEstimator`` — mandatory simple-baseline (IT-12). Error-State Kalman Filter (ESKF) over a 16-state error vector: ============ ================================= Indices Component ------------ --------------------------------- ``[0:3]`` position error (ENU world, m) ``[3:6]`` velocity error (ENU world, m/s) ``[6:9]`` orientation error (body-axis, rad) ``[9:12]`` accel bias error (body, m/s²) ``[12:15]`` gyro bias error (body, rad/s) ``[15]`` IMU dt offset (s) ============ ================================= The nominal state is kept separately (position, velocity, body→world quaternion, biases, dt offset). Per the ESKF formulation: the nominal state is updated by full nonlinear IMU integration; the error covariance is propagated via a linearised transition matrix; measurement updates correct the nominal state through ``inject_error`` after the standard Joseph-form Kalman update. AC-2.1a engine-rule application: ESKF is the mandatory simple baseline against which the iSAM2 estimator is compared (IT-12). It uses ONLY NumPy — no GTSAM dependency in this module — so the ``BUILD_STATE_ESKF=ON`` binary can ship without GTSAM at all. Per the C5 contract surface this estimator is composition- selectable via ``config.components["c5_state"].strategy = "eskf"`` plus the ``BUILD_STATE_ESKF`` flag (ADR-002 build-time exclusion). Honesty caveat — Invariant 7 deviation ===================================== The C5 contract Invariant 7 says ``smoothed_history`` entries carry ``smoothed=True``. The AZ-386 task spec (AC-6) overrides this for ESKF specifically: the simple-baseline cannot smooth, so its ``smoothed_history`` entries report ``smoothed=False``. This is intentionally honest — and forces C8 outbound to use a routing rule that doesn't conflate "smoothed=True" with "do not emit to FC". The cross-task invariant lives in AZ-261's C8 outbound filter; this module documents the deviation in the ``smoothed_history`` method docstring. """ from __future__ import annotations import time from collections import deque from typing import TYPE_CHECKING, Any, Final from uuid import uuid4 import numpy as np from numpy.linalg import LinAlgError from gps_denied_onboard._types.geo import LatLonAlt from gps_denied_onboard._types.state import ( EstimatorHealth, EstimatorOutput, IsamState, PoseSourceLabel, Quat, ) from gps_denied_onboard.components.c5_state._fallback_watcher import ( FallbackEngagementCallback, FallbackRecoveryCallback, FallbackSubscription, FallbackWatcher, ) from gps_denied_onboard.components.c5_state._source_label_sm import ( RejectionCallback, RejectionSubscription, SourceLabelStateMachine, ) from gps_denied_onboard.components.c5_state.config import C5StateConfig from gps_denied_onboard.components.c5_state.errors import ( EstimatorDegradedError, EstimatorFatalError, StateEstimatorConfigError, ) from gps_denied_onboard.components.c5_state.interface import StateEstimator from gps_denied_onboard.helpers.wgs_converter import WgsConverter from gps_denied_onboard.logging import get_logger if TYPE_CHECKING: from gps_denied_onboard._types.fc import GpsHealth from gps_denied_onboard._types.nav import ImuWindow from gps_denied_onboard._types.pose import PoseEstimate from gps_denied_onboard._types.vio import VioOutput from gps_denied_onboard.config import Config __all__ = [ "EskfStateEstimator", "create", "register", ] _STRATEGY: Final[str] = "eskf" # ENU world gravity vector — positive Z is up, gravity points down. _GRAVITY_ENU: Final[np.ndarray] = np.array([0.0, 0.0, -9.80665], dtype=np.float64) # State dimensions. _N_STATE: Final[int] = 16 _IDX_POS: Final[slice] = slice(0, 3) _IDX_VEL: Final[slice] = slice(3, 6) _IDX_ROT: Final[slice] = slice(6, 9) _IDX_BA: Final[slice] = slice(9, 12) _IDX_BG: Final[slice] = slice(12, 15) _IDX_DT: Final[int] = 15 # Initial covariance diagonals. Conservative — the contract permits # defaults so a freshly-built estimator can run on synthetic # fixtures without explicit operator priors. _INIT_SIGMA_POS_M: Final[float] = 1.0 _INIT_SIGMA_VEL_MPS: Final[float] = 0.5 _INIT_SIGMA_ROT_RAD: Final[float] = 0.1 _INIT_SIGMA_BA: Final[float] = 0.05 _INIT_SIGMA_BG: Final[float] = 0.005 _INIT_SIGMA_DT: Final[float] = 0.001 # Process noise (per second, applied as dt * sigma² in the propagation). _PROC_SIGMA_ACCEL: Final[float] = 0.2 _PROC_SIGMA_GYRO: Final[float] = 0.01 _PROC_SIGMA_BA: Final[float] = 0.001 _PROC_SIGMA_BG: Final[float] = 0.0001 # Fall-back measurement noise when the input DTO has no covariance. _DEFAULT_MEAS_SIGMA: Final[float] = 0.1 # Filter divergence threshold (AC-9): Mahalanobis ``r^T S^{-1} r`` > # (10 sigma)^2 = 100 triggers EstimatorFatalError + AC-5.2 fallback. # The spec wording is "innovation exceeds 10x the measurement-covariance # norm" — we read this as "10 standard deviations in the innovation # covariance S = H P H^T + R", which is the standard Kalman-filter # divergence gate. _INNOV_DIVERGENCE_SIGMA: Final[float] = 10.0 _INNOV_DIVERGENCE_MAHAL_SQ: Final[float] = _INNOV_DIVERGENCE_SIGMA**2 # Smoothed-history circular buffer length (entries are forward-time # estimates; per AC-6 they carry smoothed=False). _HISTORY_DEPTH: Final[int] = 30 # Default ENU origin for WGS84 conversion before the composition # root injects a real origin. Mirrors the iSAM2 default. _DEFAULT_ENU_ORIGIN: Final[LatLonAlt] = LatLonAlt(lat_deg=0.0, lon_deg=0.0, alt_m=0.0) class EskfStateEstimator(StateEstimator): """ESKF mandatory simple-baseline. NumPy-only; no GTSAM.""" def __init__( self, config: Config, *, imu_preintegrator: Any, se3_utils: Any, wgs_converter: Any, fdr_client: Any, ) -> None: block = self._extract_block(config) self._config: Config = config self._block: C5StateConfig = block self._imu_preintegrator: Any = imu_preintegrator self._se3_utils: Any = se3_utils self._wgs_converter: Any = wgs_converter self._fdr_client: Any = fdr_client self._log = get_logger("c5_state.eskf_baseline") self._nominal_pos: np.ndarray = np.zeros(3, dtype=np.float64) self._nominal_vel: np.ndarray = np.zeros(3, dtype=np.float64) self._nominal_q: np.ndarray = np.array( [1.0, 0.0, 0.0, 0.0], dtype=np.float64 ) # (w, x, y, z) self._nominal_ba: np.ndarray = np.zeros(3, dtype=np.float64) self._nominal_bg: np.ndarray = np.zeros(3, dtype=np.float64) self._nominal_dt: float = 0.0 self._P: np.ndarray = self._initial_covariance() # The previous VIO frame's world-frame pose; we treat each # VIO arrival as a relative-pose measurement against the # nominal motion since the last VIO frame. self._prev_vio_pose: np.ndarray | None = None self._prev_vio_nominal_pos: np.ndarray | None = None self._prev_vio_nominal_q: np.ndarray | None = None self._last_added_ts_ns: int = 0 # Wall-clock-equivalent monotonic timestamp of the most-recent # successful satellite anchor; used by ``last_satellite_anchor_age_ms``. # 0 means "no anchor yet"; ``current_estimate`` reports a very # large age in that case. self._last_anchor_ns: int = 0 self._isam2_state: IsamState = IsamState.INIT self._enu_origin: LatLonAlt | None = None self._history: deque[EstimatorOutput] = deque(maxlen=_HISTORY_DEPTH) # AZ-385: source-label SM. Eagerly constructed; composition # root drives notify_gps_health + subscribe_spoof_rejection. self._source_label_machine: SourceLabelStateMachine = SourceLabelStateMachine( spoof_promotion_min_stable_s=block.spoof_promotion_min_stable_s, spoof_promotion_visual_consistency_tol_m=block.spoof_promotion_visual_consistency_tol_m, fdr_client=fdr_client, producer_id="c5_state", ) # AZ-388: AC-5.2 fallback watcher. self._fallback = FallbackWatcher( threshold_s=block.no_estimate_fallback_s, fdr_client=fdr_client, producer_id="c5_state", ) self._log.debug( "c5.state.eskf_initialised", extra={ "kind": "c5.state.eskf_initialised", "kv": {"strategy": "eskf"}, }, ) @staticmethod def _extract_block(config: Config) -> C5StateConfig: components = getattr(config, "components", None) or {} block = components.get("c5_state") if isinstance(components, dict) else None if block is None: return C5StateConfig(strategy="eskf") if isinstance(block, C5StateConfig): return block raise StateEstimatorConfigError( f"config.components['c5_state'] must be a C5StateConfig; got {type(block).__name__}" ) @staticmethod def _initial_covariance() -> np.ndarray: diag = np.empty(_N_STATE, dtype=np.float64) diag[_IDX_POS] = _INIT_SIGMA_POS_M**2 diag[_IDX_VEL] = _INIT_SIGMA_VEL_MPS**2 diag[_IDX_ROT] = _INIT_SIGMA_ROT_RAD**2 diag[_IDX_BA] = _INIT_SIGMA_BA**2 diag[_IDX_BG] = _INIT_SIGMA_BG**2 diag[_IDX_DT] = _INIT_SIGMA_DT**2 return np.diag(diag) # ------------------------------------------------------------------ # Public injection points (mirror the iSAM2 estimator surface). def set_enu_origin(self, origin: LatLonAlt) -> None: self._enu_origin = origin def attach_source_label_state_machine(self, machine: Any) -> None: self._source_label_machine = machine def notify_gps_health(self, gps_health: GpsHealth, now_ns: int | None = None) -> None: machine = self._source_label_machine if isinstance(machine, SourceLabelStateMachine): machine.notify_gps_health(gps_health, now_ns=now_ns) def subscribe_spoof_rejection(self, callback: RejectionCallback) -> RejectionSubscription: machine = self._source_label_machine if not isinstance(machine, SourceLabelStateMachine): raise StateEstimatorConfigError( "subscribe_spoof_rejection requires the built-in SourceLabelStateMachine" ) return machine.subscribe_rejection(callback) def check_fallback_state(self, now_ns: int) -> bool: return self._fallback.check_and_engage(now_ns) def subscribe_fallback_engaged( self, callback: FallbackEngagementCallback ) -> FallbackSubscription: return self._fallback.subscribe_engaged(callback) def subscribe_fallback_recovered( self, callback: FallbackRecoveryCallback ) -> FallbackSubscription: return self._fallback.subscribe_recovered(callback) # ------------------------------------------------------------------ # Protocol: factor adds. def add_vio(self, vio: VioOutput) -> None: """Relative-pose measurement from C1 VIO. First VIO frame seeds ``_prev_vio_*``; subsequent frames feed the relative-pose update. The measurement is the SE(3) delta ``inv(prev_vio) @ curr_vio`` and the predicted measurement is the analogous nominal delta — both are projected to a 6-vector residual in the previous body frame. """ ts_ns = _datetime_to_ns(vio.timestamp) self._guard_timestamp(ts_ns, source="vio") curr_pose = _pose_se3_to_array(vio.pose_se3) if self._prev_vio_pose is None: self._prev_vio_pose = curr_pose self._prev_vio_nominal_pos = self._nominal_pos.copy() self._prev_vio_nominal_q = self._nominal_q.copy() self._last_added_ts_ns = ts_ns self._log.debug( "c5.state.eskf_add_vio_first_frame", extra={ "kind": "c5.state.eskf_add_vio_first_frame", "kv": {"frame_id": str(vio.frame_id), "ts_ns": ts_ns}, }, ) return prev_pose = self._prev_vio_pose measured_delta_world = curr_pose[:3, 3] - prev_pose[:3, 3] prev_nominal_pos = ( self._prev_vio_nominal_pos if self._prev_vio_nominal_pos is not None else self._nominal_pos.copy() ) predicted_delta_world = self._nominal_pos - prev_nominal_pos residual_pos = measured_delta_world - predicted_delta_world prev_R = prev_pose[:3, :3] curr_R_meas = prev_pose[:3, :3].T @ curr_pose[:3, :3] prev_q_nominal = ( self._prev_vio_nominal_q if self._prev_vio_nominal_q is not None else self._nominal_q.copy() ) curr_R_nominal = _quat_to_rot(prev_q_nominal).T @ _quat_to_rot(self._nominal_q) residual_rot = _rot_to_axis_angle(curr_R_meas @ curr_R_nominal.T) residual = np.concatenate([residual_pos, residual_rot]) # Measurement Jacobian: the residual depends on the CURRENT # state's position + orientation only (the snapshot at the # previous frame is treated as a fixed reference). This is a # SIMPLIFICATION of the full ESKF relative-pose Jacobian — # honest about being a baseline. H = np.zeros((6, _N_STATE), dtype=np.float64) H[0:3, _IDX_POS] = np.eye(3) H[3:6, _IDX_ROT] = prev_R # rotate body-frame perturbation back to world R_meas = _measurement_noise(vio.covariance_6x6) try: self._kalman_update(H, residual, R_meas, source="vio") except EstimatorFatalError: self._isam2_state = IsamState.LOST raise except EstimatorDegradedError: raise except Exception as exc: self._log.error( "c5.state.eskf_add_vio_failed", extra={ "kind": "c5.state.eskf_add_vio_failed", "kv": {"frame_id": str(vio.frame_id), "error": str(exc)}, }, ) raise EstimatorDegradedError(f"eskf add_vio failed: {exc}") from exc self._prev_vio_pose = curr_pose self._prev_vio_nominal_pos = self._nominal_pos.copy() self._prev_vio_nominal_q = self._nominal_q.copy() self._last_added_ts_ns = ts_ns if self._isam2_state == IsamState.INIT: self._isam2_state = IsamState.TRACKING def add_pose_anchor(self, pose: PoseEstimate) -> None: """Absolute-pose measurement from C4. Per AC-4 the ESKF does NOT skip the JACOBIAN-mode anchor (it has no graph to throttle); it integrates every anchor as a regular measurement. """ ts_ns = int(pose.emitted_at) self._guard_timestamp(ts_ns, source="pose_anchor") meas_pose = self._pose_estimate_to_matrix(pose) # Both modes are treated identically by the ESKF — the # JACOBIAN exclusion is iSAM2-graph-specific. AC-4. self._last_anchor_ns = time.monotonic_ns() residual_pos = meas_pose[:3, 3] - self._nominal_pos meas_R = meas_pose[:3, :3] nominal_R = _quat_to_rot(self._nominal_q) residual_rot = _rot_to_axis_angle(meas_R @ nominal_R.T) residual = np.concatenate([residual_pos, residual_rot]) H = np.zeros((6, _N_STATE), dtype=np.float64) H[0:3, _IDX_POS] = np.eye(3) H[3:6, _IDX_ROT] = np.eye(3) R_meas = _measurement_noise(pose.covariance_6x6) try: self._kalman_update(H, residual, R_meas, source="pose_anchor") except EstimatorFatalError: self._isam2_state = IsamState.LOST raise except EstimatorDegradedError: raise except Exception as exc: self._log.error( "c5.state.eskf_add_pose_anchor_failed", extra={ "kind": "c5.state.eskf_add_pose_anchor_failed", "kv": {"frame_id": str(pose.frame_id), "error": str(exc)}, }, ) raise EstimatorDegradedError(f"eskf add_pose_anchor failed: {exc}") from exc self._notify_source_label_anchor(pose) self._last_added_ts_ns = ts_ns if self._isam2_state == IsamState.INIT: self._isam2_state = IsamState.TRACKING def add_fc_imu(self, imu_window: ImuWindow) -> None: """Predict nominal state + propagate covariance over the IMU window.""" self._guard_timestamp(imu_window.ts_end_ns, source="imu_window") samples = imu_window.samples if not samples: self._last_added_ts_ns = imu_window.ts_end_ns return prev_ts_ns = samples[0].ts_ns for sample in samples: dt = max(0.0, (sample.ts_ns - prev_ts_ns) / 1_000_000_000) if dt > 0.0: try: self._predict( dt=dt, accel_meas=np.asarray(sample.accel_xyz, dtype=np.float64), gyro_meas=np.asarray(sample.gyro_xyz, dtype=np.float64), ) except Exception as exc: self._log.error( "c5.state.eskf_predict_failed", extra={ "kind": "c5.state.eskf_predict_failed", "kv": {"ts_ns": sample.ts_ns, "error": str(exc)}, }, ) raise EstimatorDegradedError(f"eskf predict failed: {exc}") from exc prev_ts_ns = sample.ts_ns self._last_added_ts_ns = imu_window.ts_end_ns if self._isam2_state == IsamState.INIT: self._isam2_state = IsamState.TRACKING # ------------------------------------------------------------------ # Protocol: outputs. def current_estimate(self) -> EstimatorOutput: """Forward-time estimate. ``smoothed=False`` (Invariant 7).""" now_ns = time.monotonic_ns() self._fallback.check_and_engage(now_ns) cov6 = self._pose_covariance_6x6() try: _enforce_spd(cov6) except EstimatorFatalError: self._isam2_state = IsamState.LOST self._log.error( "c5.state.eskf_current_estimate_spd_failed", extra={ "kind": "c5.state.eskf_current_estimate_spd_failed", "kv": {"cov_fro_norm": float(np.linalg.norm(cov6, ord="fro"))}, }, ) raise emitted_at = time.monotonic_ns() position_wgs84 = self._enu_pose_to_wgs84() orientation = _quat_to_quat_dto(self._nominal_q) velocity_world = ( float(self._nominal_vel[0]), float(self._nominal_vel[1]), float(self._nominal_vel[2]), ) last_anchor_age_ms = self._compute_last_anchor_age_ms(now_ns) source_label = self._derive_source_label() out = EstimatorOutput( frame_id=uuid4(), position_wgs84=position_wgs84, orientation_world_T_body=orientation, velocity_world_mps=velocity_world, covariance_6x6=cov6, source_label=source_label, last_satellite_anchor_age_ms=last_anchor_age_ms, smoothed=False, emitted_at=emitted_at, ) self._history.append(out) self._fallback.mark_successful_estimate(emitted_at) return out def smoothed_history(self, n_keyframes: int) -> list[EstimatorOutput]: """Return up to ``n_keyframes`` recent forward-time entries. ESKF cannot smooth (it is forward-only). Per AZ-386 AC-6 and the "honest baseline" principle we surface the circular buffer of past forward-time estimates AS-IS, with ``smoothed=False`` — deviating from C5 contract Invariant 7 which generally assumes ``smoothed_history`` is truly smoothed. The cross-task invariant (AZ-261 C8 outbound filter) MUST therefore not equate ``smoothed=True`` with "do not emit to FC"; it MUST use the dedicated routing rule documented in the C5 contract. """ if n_keyframes <= 0: return [] if not self._history: return [] out: list[EstimatorOutput] = list(self._history)[-n_keyframes:] return [_with_smoothed_false(entry) for entry in out] def health_snapshot(self) -> EstimatorHealth: """O(1) accumulator read. Reports the simplified ESKF lifecycle: INIT until the first measurement, TRACKING after, DEGRADED on innovation-threshold breaches, LOST on SPD failure or filter divergence. ``keyframe_count`` mirrors the history buffer length; ``cov_norm_growing_for_s`` is set to 0.0 — the ESKF has no sliding window with which to estimate a covariance trend. """ return EstimatorHealth( isam2_state=self._isam2_state, keyframe_count=len(self._history), cov_norm_growing_for_s=0.0, spoof_promotion_blocked=self._spoof_promotion_blocked(), ) # ------------------------------------------------------------------ # Internals: KF math. def _predict(self, *, dt: float, accel_meas: np.ndarray, gyro_meas: np.ndarray) -> None: # Compensate biases, rotate accel to world, integrate. accel_body = accel_meas - self._nominal_ba gyro_body = gyro_meas - self._nominal_bg R = _quat_to_rot(self._nominal_q) accel_world = R @ accel_body + _GRAVITY_ENU new_pos = self._nominal_pos + self._nominal_vel * dt + 0.5 * accel_world * dt * dt new_vel = self._nominal_vel + accel_world * dt delta_q = _quat_from_axis_angle(gyro_body * dt) new_q = _quat_normalise(_quat_mul(self._nominal_q, delta_q)) self._nominal_pos = new_pos self._nominal_vel = new_vel self._nominal_q = new_q # Error covariance propagation. F is approximately: # d(pos_err)/dt = vel_err # d(vel_err)/dt = -R @ skew(accel_body) @ rot_err - R @ ba_err # d(rot_err)/dt = -skew(gyro_body) @ rot_err - bg_err # The dt and bias-dt couplings are captured as identity blocks. F = np.eye(_N_STATE, dtype=np.float64) F[_IDX_POS, _IDX_VEL] = np.eye(3) * dt F[_IDX_VEL, _IDX_ROT] = -R @ _skew(accel_body) * dt F[_IDX_VEL, _IDX_BA] = -R * dt F[_IDX_ROT, _IDX_ROT] = np.eye(3) - _skew(gyro_body) * dt F[_IDX_ROT, _IDX_BG] = -np.eye(3) * dt Q = np.zeros((_N_STATE, _N_STATE), dtype=np.float64) Q[_IDX_VEL, _IDX_VEL] = np.eye(3) * (_PROC_SIGMA_ACCEL**2) * dt Q[_IDX_ROT, _IDX_ROT] = np.eye(3) * (_PROC_SIGMA_GYRO**2) * dt Q[_IDX_BA, _IDX_BA] = np.eye(3) * (_PROC_SIGMA_BA**2) * dt Q[_IDX_BG, _IDX_BG] = np.eye(3) * (_PROC_SIGMA_BG**2) * dt self._P = F @ self._P @ F.T + Q # Cholesky-positive enforcement happens on the read path; we # keep the symmetrisation here to defend against accumulating # numerical asymmetry. self._P = 0.5 * (self._P + self._P.T) def _kalman_update( self, H: np.ndarray, residual: np.ndarray, R_meas: np.ndarray, *, source: str, ) -> None: """Joseph-form Kalman update with AC-9 divergence detection.""" S = H @ self._P @ H.T + R_meas try: S_inv = np.linalg.inv(S) except LinAlgError as exc: raise EstimatorFatalError(f"eskf {source} S not invertible: {exc}") from exc mahal_sq = float(residual @ S_inv @ residual) if mahal_sq > _INNOV_DIVERGENCE_MAHAL_SQ: self._log.error( "c5.state.eskf_filter_divergence", extra={ "kind": "c5.state.eskf_filter_divergence", "kv": { "source": source, "mahalanobis_sq": mahal_sq, "threshold_sq": _INNOV_DIVERGENCE_MAHAL_SQ, }, }, ) raise EstimatorFatalError( f"eskf filter divergence on {source}: mahalanobis²={mahal_sq:.3f} > " f"{_INNOV_DIVERGENCE_MAHAL_SQ:.1f}" ) K = self._P @ H.T @ S_inv delta_x = K @ residual self._inject_error(delta_x) I = np.eye(_N_STATE, dtype=np.float64) # noqa: E741 — standard KF symbol I_minus_KH = I - K @ H self._P = I_minus_KH @ self._P @ I_minus_KH.T + K @ R_meas @ K.T self._P = 0.5 * (self._P + self._P.T) def _inject_error(self, delta_x: np.ndarray) -> None: self._nominal_pos = self._nominal_pos + delta_x[_IDX_POS] self._nominal_vel = self._nominal_vel + delta_x[_IDX_VEL] delta_q = _quat_from_axis_angle(delta_x[_IDX_ROT]) self._nominal_q = _quat_normalise(_quat_mul(self._nominal_q, delta_q)) self._nominal_ba = self._nominal_ba + delta_x[_IDX_BA] self._nominal_bg = self._nominal_bg + delta_x[_IDX_BG] self._nominal_dt = self._nominal_dt + float(delta_x[_IDX_DT]) def _pose_covariance_6x6(self) -> np.ndarray: """Project the 16x16 error covariance onto pose (pos + rot).""" pose_idx = np.array([0, 1, 2, 6, 7, 8]) return np.ascontiguousarray(self._P[np.ix_(pose_idx, pose_idx)], dtype=np.float64) def _enu_pose_to_wgs84(self) -> LatLonAlt: origin = self._enu_origin if self._enu_origin is not None else _DEFAULT_ENU_ORIGIN enu = np.array( [ float(self._nominal_pos[0]), float(self._nominal_pos[1]), float(self._nominal_pos[2]), ], dtype=np.float64, ) return WgsConverter.local_enu_to_latlonalt(origin, enu) def _pose_estimate_to_matrix(self, pose: PoseEstimate) -> np.ndarray: """Convert a C4 :class:`PoseEstimate` to a 4x4 homogeneous matrix. WGS84 → ENU via the injected origin (matches the iSAM2 estimator's helper); ``Quat`` → 3x3 rotation via the local ``_quat_to_rot`` helper applied to a freshly-built quaternion. """ origin = self._enu_origin if self._enu_origin is not None else _DEFAULT_ENU_ORIGIN try: enu = WgsConverter.latlonalt_to_local_enu(origin, pose.position_wgs84) except Exception as exc: raise EstimatorDegradedError( f"eskf add_pose_anchor: WGS84→ENU failed for frame {pose.frame_id}: {exc}" ) from exc q = pose.orientation_world_T_body rotation = _quat_to_rot( np.array([float(q.w), float(q.x), float(q.y), float(q.z)], dtype=np.float64) ) matrix = np.eye(4, dtype=np.float64) matrix[:3, :3] = rotation matrix[:3, 3] = enu return matrix def _compute_last_anchor_age_ms(self, now_ns: int) -> int: if self._last_anchor_ns == 0: return now_ns // 1_000_000 return max(0, (now_ns - self._last_anchor_ns) // 1_000_000) def _derive_source_label(self) -> PoseSourceLabel: machine = self._source_label_machine if machine is None: return PoseSourceLabel.VISUAL_PROPAGATED try: label = machine.current_label() except Exception as exc: self._log.error( "c5.state.source_label_machine_failed", extra={ "kind": "c5.state.source_label_machine_failed", "kv": {"error": str(exc)}, }, ) return PoseSourceLabel.VISUAL_PROPAGATED if not isinstance(label, PoseSourceLabel): return PoseSourceLabel.VISUAL_PROPAGATED return label def _spoof_promotion_blocked(self) -> bool: machine = self._source_label_machine if machine is None: return False try: return bool(machine.is_spoof_promotion_blocked()) except Exception: return False def _notify_source_label_anchor(self, pose: PoseEstimate) -> None: machine = self._source_label_machine if not isinstance(machine, SourceLabelStateMachine): return try: machine.notify_satellite_anchor( now_ns=time.monotonic_ns(), gps_consistency_delta_m=None, ) except Exception as exc: self._log.error( "c5.state.eskf_source_label_anchor_notify_failed", extra={ "kind": "c5.state.eskf_source_label_anchor_notify_failed", "kv": {"frame_id": str(pose.frame_id), "error": str(exc)}, }, ) def _guard_timestamp(self, ts_ns: int, *, source: str) -> None: if ts_ns < self._last_added_ts_ns: self._log.error( "c5.state.eskf_out_of_order", extra={ "kind": "c5.state.eskf_out_of_order", "kv": { "source": source, "ts_ns": ts_ns, "last_added_ts_ns": self._last_added_ts_ns, }, }, ) raise EstimatorDegradedError( f"eskf out-of-order {source}: ts_ns={ts_ns} < " f"last_added_ts_ns={self._last_added_ts_ns}" ) # ---------------------------------------------------------------------- # Module-level factory + register helpers. def create( *, config: Config, imu_preintegrator: Any, se3_utils: Any, wgs_converter: Any, fdr_client: Any, ) -> tuple[StateEstimator, Any]: """Composition-root factory for the ESKF estimator. Returns ``(estimator, handle)`` to match the iSAM2 factory shape; the ESKF returns ``handle=None`` because it has no GTSAM graph. """ estimator = EskfStateEstimator( config, imu_preintegrator=imu_preintegrator, se3_utils=se3_utils, wgs_converter=wgs_converter, fdr_client=fdr_client, ) return estimator, None def register() -> None: """Register :func:`create` under the ``"eskf"`` strategy slug.""" from gps_denied_onboard.runtime_root.state_factory import register_state_estimator register_state_estimator(_STRATEGY, create) # ---------------------------------------------------------------------- # Module-level math + DTO helpers (kept local; small and self-contained). def _datetime_to_ns(dt: Any) -> int: return int(dt.timestamp() * 1_000_000_000) def _pose_se3_to_array(pose_se3: Any) -> np.ndarray: arr = np.asarray(pose_se3, dtype=np.float64) if arr.shape != (4, 4): raise ValueError(f"pose_se3 must be 4x4; got shape {arr.shape}") return arr def _measurement_noise(covariance: Any | None) -> np.ndarray: if covariance is None: return np.eye(6, dtype=np.float64) * (_DEFAULT_MEAS_SIGMA**2) cov = np.asarray(covariance, dtype=np.float64) if cov.shape != (6, 6): raise ValueError(f"covariance_6x6 must be 6x6; got shape {cov.shape}") return cov def _skew(v: np.ndarray) -> np.ndarray: return np.array( [ [0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0], ], dtype=np.float64, ) def _quat_to_rot(q: np.ndarray) -> np.ndarray: w, x, y, z = float(q[0]), float(q[1]), float(q[2]), float(q[3]) return np.array( [ [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], ], dtype=np.float64, ) def _quat_mul(a: np.ndarray, b: np.ndarray) -> np.ndarray: aw, ax, ay, az = a bw, bx, by, bz = b return np.array( [ aw * bw - ax * bx - ay * by - az * bz, aw * bx + ax * bw + ay * bz - az * by, aw * by - ax * bz + ay * bw + az * bx, aw * bz + ax * by - ay * bx + az * bw, ], dtype=np.float64, ) def _quat_normalise(q: np.ndarray) -> np.ndarray: n = float(np.linalg.norm(q)) if n == 0.0: return np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64) return q / n def _quat_from_axis_angle(axis_angle: np.ndarray) -> np.ndarray: """Build a unit quaternion from a 3-vector axis-angle (rad).""" theta = float(np.linalg.norm(axis_angle)) if theta < 1e-12: return np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64) half = theta * 0.5 s = float(np.sin(half)) / theta return np.array( [ float(np.cos(half)), float(axis_angle[0]) * s, float(axis_angle[1]) * s, float(axis_angle[2]) * s, ], dtype=np.float64, ) def _rot_to_axis_angle(R: np.ndarray) -> np.ndarray: """Inverse of ``_quat_from_axis_angle ∘ _quat_to_rot`` for small rotations.""" trace = float(np.trace(R)) cos_theta = max(-1.0, min(1.0, (trace - 1.0) * 0.5)) theta = float(np.arccos(cos_theta)) if theta < 1e-12: return np.zeros(3, dtype=np.float64) sin_theta = float(np.sin(theta)) if abs(sin_theta) < 1e-12: # Pi rotation — pick the axis from the largest diagonal entry. diag = np.diag(R) i = int(np.argmax(diag)) axis = np.zeros(3, dtype=np.float64) axis[i] = float(np.sqrt((diag[i] + 1.0) * 0.5)) return axis * theta return ( np.array( [ (R[2, 1] - R[1, 2]) / (2.0 * sin_theta), (R[0, 2] - R[2, 0]) / (2.0 * sin_theta), (R[1, 0] - R[0, 1]) / (2.0 * sin_theta), ], dtype=np.float64, ) * theta ) def _rot_to_quat(R: np.ndarray) -> np.ndarray: """Convert a 3x3 rotation matrix to a unit quaternion (w, x, y, z).""" trace = R[0, 0] + R[1, 1] + R[2, 2] if trace > 0.0: s = 0.5 / float(np.sqrt(trace + 1.0)) w = 0.25 / s x = (R[2, 1] - R[1, 2]) * s y = (R[0, 2] - R[2, 0]) * s z = (R[1, 0] - R[0, 1]) * s else: if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: s = 2.0 * float(np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2])) w = (R[2, 1] - R[1, 2]) / s x = 0.25 * s y = (R[0, 1] + R[1, 0]) / s z = (R[0, 2] + R[2, 0]) / s elif R[1, 1] > R[2, 2]: s = 2.0 * float(np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2])) w = (R[0, 2] - R[2, 0]) / s x = (R[0, 1] + R[1, 0]) / s y = 0.25 * s z = (R[1, 2] + R[2, 1]) / s else: s = 2.0 * float(np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1])) w = (R[1, 0] - R[0, 1]) / s x = (R[0, 2] + R[2, 0]) / s y = (R[1, 2] + R[2, 1]) / s z = 0.25 * s return _quat_normalise(np.array([w, x, y, z], dtype=np.float64)) def _quat_to_quat_dto(q: np.ndarray) -> Quat: return Quat(w=float(q[0]), x=float(q[1]), y=float(q[2]), z=float(q[3])) def _enforce_spd(cov: np.ndarray) -> None: """Run a Cholesky decomposition; raise :class:`EstimatorFatalError` on failure.""" try: np.linalg.cholesky(cov) except LinAlgError as exc: raise EstimatorFatalError(f"covariance not SPD: {exc}") from exc def _with_smoothed_false(out: EstimatorOutput) -> EstimatorOutput: """Return a copy of ``out`` with ``smoothed=False``. The buffer entries are appended with ``smoothed=False`` already (forward-time path); this helper is a defensive belt-and-suspenders in case a future refactor flips the flag at append time. """ if out.smoothed is False: return out return EstimatorOutput( frame_id=out.frame_id, position_wgs84=out.position_wgs84, orientation_world_T_body=out.orientation_world_T_body, velocity_world_mps=out.velocity_world_mps, covariance_6x6=out.covariance_6x6, source_label=out.source_label, last_satellite_anchor_age_ms=out.last_satellite_anchor_age_ms, smoothed=False, emitted_at=out.emitted_at, )