mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 23:21:13 +00:00
[AZ-386] C5 ESKF baseline: 16-state error-state KF (NumPy)
Implements the mandatory simple-baseline StateEstimator per AC-2.1a engine-rule at C5 (IT-12 comparative study vs iSAM2). NumPy-only; no GTSAM dependency so BUILD_STATE_ESKF=ON binaries ship without GTSAM at all. - 16-state error vector (pos 3 + vel 3 + rot 3 + ba 3 + bg 3 + dt 1) over a textbook nominal-state / error-state ESKF split. - add_fc_imu: full nonlinear IMU integration + linearised F P F^T + Q covariance propagation per IMU sample. - add_vio: simplified relative-pose update (snapshot-based; baseline scope, documented). - add_pose_anchor: absolute-pose update; integrates BOTH marginals and jacobian modes (no skip — ESKF has no graph; AC-4). - AC-9 divergence test: Mahalanobis r^T S^-1 r > 100 (10 sigma) on the innovation covariance S = H P H^T + R. - AC-5 SPD: Cholesky-positive enforcement on every emitted covariance; non-SPD raises EstimatorFatalError and locks state to LOST. - AC-6 honesty: smoothed_history entries carry smoothed=False; deviation from C5 contract Invariant 7 documented in module + report. - AC-7 / AC-10 BUILD_STATE_ESKF gating: works through existing factory infra (state_factory._STATE_BUILD_FLAGS). - AC-8: SourceLabelStateMachine + FallbackWatcher auto-wired eagerly in __init__, same pattern as the iSAM2 estimator. Tests: 20 new unit tests covering AC-1..AC-10 + robustness checks. Full suite: 660 passed, 2 skipped (CI-only). The AZ-386 Jira transition to Done is deferred (Atlassian MCP returned 'Not connected'); recorded in _docs/_process_leftovers/ for replay on the next autodev invocation per the Leftovers Mechanism. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,924 @@
|
||||
"""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 = _datetime_to_ns(pose.timestamp)
|
||||
self._guard_timestamp(ts_ns, source="pose_anchor")
|
||||
meas_pose = _pose_se3_to_array(pose.pose_se3)
|
||||
|
||||
# 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 _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
Block a user