azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-21 08:51:12 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
db27e2563094962996f1120b1761c69db193d749
gps-denied-onboard/src/gps_denied_onboard/components/c5_state/eskf_baseline.py
T
Oleksandr Bezdieniezhnykh db27e25630 [AZ-355] C4 PoseEstimator Protocol + factory + DTOs + composition 
Land the foundational C4 surface AZ-358 (Marginals) and AZ-361
(Hybrid) build on top of:

- PoseEstimator Protocol (@runtime_checkable): estimate(...) +
  current_covariance_mode().
- Error hierarchy: PoseEstimatorError, PnpFailureError,
  PoseEstimatorConfigError; CovarianceDegradedWarning as a Warning
  subclass (warnings.warn path, not raised).
- ISam2GraphHandle Protocol stub (READ-ONLY view, get_pose_key only)
  decoupled from C5's concrete ISam2GraphHandleImpl.
- C4PoseConfig (frozen dataclass) + register on c4_pose import.
- runtime_root/pose_factory.build_pose_estimator with lazy-import
  fallback; INFO log c4.pose.strategy_loaded; shares ingest-thread
  binding with C5 per ADR-003.

DTO restructuring (cross-cutting): retire the legacy raw-4x4
PoseEstimate(int frame_id, datetime timestamp, pose_se3, ...) and
ship the contract shape PoseEstimate(UUID, LatLonAlt, Quat,
np.ndarray, CovarianceMode, PoseSourceLabel,
last_satellite_anchor_age_ms, emitted_at). C5 add_pose_anchor in
both gtsam_isam2 + eskf_baseline migrated in lockstep via
WGS84->ENU + Quat->R helpers; test fixtures updated. VIO output
stays on the raw shape until AZ-331 (C1 protocol) lands.

LatLonAlt upgraded to slots=True per AC-2. ThermalState stub added
to _types/thermal.py so the Protocol typechecks pre-AZ-302.

Tests: 25 new in tests/unit/c4_pose/test_az355_pose_protocol.py
covering AC-1..AC-10 + factory wiring + config validation; full
repo: 685 passed, 2 pre-existing CI-only skips.

Jira transition deferred: MCP "Not connected"; leftover entry in
_docs/_process_leftovers/2026-05-11_jira_transition_az355_deferred.md.

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-05-11 10:32:14 +03:00

948 lines
36 KiB
Python
Raw Blame History

"""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,
)
Reference in New Issue View Git Blame Copy Permalink