azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-22 22:51:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

[AZ-383] C5 add_vio/add_pose_anchor/add_fc_imu factor adds

Browse Source 
Replaces AZ-382 NotImplementedError placeholders with real GTSAM factor
adds wired against the iSAM2 graph handle:

- add_vio  -> BetweenFactorPose3 between consecutive VIO pose keys
  (first call primes the chain; AZ-388 owns first-keyframe seeding).
- add_pose_anchor -> mode-dispatch per pose.covariance_mode:
  "marginals" -> PriorFactorPose3 + handle.update();
  "jacobian"  -> skip iSAM2 add per AZ-361 contract.
  Both paths bump _last_anchor_ns via time.monotonic_ns().
- add_fc_imu -> shared ImuPreintegrator.integrate_window +
  reset_for_new_keyframe; builds a CombinedImuFactor between the
  prev/curr (X, V, B) keyframe triple. Introduces new 'v' (velocity)
  and 'b' (bias) GTSAM key namespaces decoupled from the VIO/pose
  frame_id mapping.

Invariant 2 - non-decreasing timestamps - enforced per call with
EstimatorDegradedError + c5.state.out_of_order log. Every successful
add emits a structured DEBUG *_ok log; every failure emits a
structured ERROR *_failed log and raises through the C5 error
hierarchy (R05).

Contract-vs-reality fix-ups also landed:

- StateEstimator Protocol: add_fc_imu(ImuWindow) - was incorrectly
  annotated as ImuTelemetrySample by AZ-381.
- _last_anchor_ns semantics switched to monotonic_ns() to match
  last_anchor_age_ms.
- create() factory back-wires the ISam2GraphHandle to the estimator
  via the new attach_handle() method.

Tests: +21 in tests/unit/c5_state/test_az383_factor_adds.py covering
all 8 ACs with mock ISam2GraphHandle instances. Three obsolete
AZ-382 tests (test_ac10_add_*_raises_named_az383) removed. Full
suite: 565 passed, 2 skipped.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-11 06:07:45 +03:00
parent 8b394a98c6
commit fd848266d1
7 changed files with 955 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py
+434 -48
View File
@@ -1,55 +1,57 @@
"""C5 ``GtsamIsam2StateEstimator`` — skeleton wiring (AZ-382 / E-C5).
"""C5 ``GtsamIsam2StateEstimator`` — graph + factor adds (AZ-382 + AZ-383).
Builds the GTSAM iSAM2 graph + ``IncrementalFixedLagSmoother`` substrate
on which AZ-383 / AZ-384 / AZ-385 will hang the actual factor adds,
marginals, and source-label logic. This task owns:
AZ-382 wired the GTSAM substrate (iSAM2 + ``IncrementalFixedLagSmoother``
+ ``NonlinearFactorGraph`` + ``Values``) and the four ``ISam2GraphHandleImpl``
real bodies. AZ-383 owns the three Protocol factor-add methods:
* ``__init__`` — constructs ``_isam2`` (``gtsam.ISAM2``), ``_smoother``
(``gtsam_unstable.IncrementalFixedLagSmoother`` with a
``K * frame_period_s`` window), ``_graph`` + ``_values`` containers,
and the ``_key_for_frame`` mapping used by AZ-383 when it converts
``UUID`` ``frame_id`` values into GTSAM ``Key`` integers via
``gtsam.symbol('x', counter)``.
* :func:`create` — module-level factory the composition root registers
as ``register_state_estimator("gtsam_isam2", create)``. Returns the
``(StateEstimator, ISam2GraphHandle)`` tuple per
:mod:`gps_denied_onboard.runtime_root.state_factory`.
* :func:`register` — convenience that calls ``register_state_estimator``
with this strategy. Test fixtures + the per-binary bootstrap module
call this; AZ-381 deliberately did not auto-register at import.
* ``add_vio(vio: VioOutput)`` — ``BetweenFactorPose3`` between
consecutive pose keys with a noise model derived from
``vio.covariance_6x6``.
* ``add_pose_anchor(pose: PoseEstimate)`` — mode-dispatched per
``pose.covariance_mode``: ``"marginals"`` → ``PriorFactorPose3`` +
``update``; ``"jacobian"`` → skip iSAM2 add (per the AZ-361 cross-task
contract) but still bump ``_last_anchor_ns``.
* ``add_fc_imu(imu_window: ImuWindow)`` — feeds the shared
``ImuPreintegrator``, builds a ``CombinedImuFactor`` between the
prev/curr (X, V, B) keyframe triple, and inserts initial values for
the new keys.
Every ``StateEstimator`` Protocol method intentionally raises
``NotImplementedError`` with the **next** task's tracker ID in the
message — the bodies are owned by:
Invariant 2 — every ``add_*`` call enforces non-decreasing timestamps;
out-of-order arrivals raise :class:`EstimatorDegradedError` and emit an
ERROR log. R05 — every successful factor add emits a DEBUG SUCCESS
log; every failure emits an ERROR FAILURE log AND raises through the
C5 error hierarchy.
* ``add_vio`` / ``add_pose_anchor`` / ``add_fc_imu`` → AZ-383
* ``current_estimate`` / ``smoothed_history`` / ``health_snapshot`` → AZ-384
The ``ISam2GraphHandleImpl`` bodies in
:mod:`gps_denied_onboard.components.c5_state._isam2_handle`, however,
ARE owned by this task and are populated against the estimator
constructed here.
The remaining three Protocol methods (``current_estimate`` /
``smoothed_history`` / ``health_snapshot``) still raise
``NotImplementedError`` naming AZ-384 — marginals + outputs land
there.
"""
from __future__ import annotations
import time
from typing import TYPE_CHECKING, Any, Final
from uuid import UUID
import gtsam
import gtsam_unstable
import numpy as np
from gps_denied_onboard.components.c5_state._isam2_handle import (
ISam2GraphHandle,
ISam2GraphHandleImpl,
)
from gps_denied_onboard.components.c5_state.config import C5StateConfig
from gps_denied_onboard.components.c5_state.errors import StateEstimatorConfigError
from gps_denied_onboard.components.c5_state.errors import (
EstimatorDegradedError,
StateEstimatorConfigError,
)
from gps_denied_onboard.components.c5_state.interface import StateEstimator
from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
from gps_denied_onboard._types.fc import ImuTelemetrySample
from gps_denied_onboard._types.nav import ImuWindow
from gps_denied_onboard._types.pose import PoseEstimate
from gps_denied_onboard._types.state import EstimatorHealth, EstimatorOutput
from gps_denied_onboard._types.vio import VioOutput
@@ -74,6 +76,12 @@ _FRAME_PERIOD_S: Final[float] = 1.0 / 3.0
# ``register_state_estimator`` registry.
_STRATEGY: Final[str] = "gtsam_isam2"
# Default isotropic noise covariance used when an input DTO arrives
# without an explicit ``covariance_6x6``. 0.1 m / 0.1 rad sigmas match
# the documented BMI088-class defaults and let the iSAM2 solver
# converge without exploding.
_DEFAULT_POSE_SIGMA: Final[float] = 0.1
class GtsamIsam2StateEstimator(StateEstimator):
"""Production-default C5 estimator — iSAM2 + ``IncrementalFixedLagSmoother``.
@@ -109,7 +117,10 @@ class GtsamIsam2StateEstimator(StateEstimator):
self._graph = gtsam.NonlinearFactorGraph()
self._values = gtsam.Values()
self._key_for_frame: dict[UUID, int] = {}
# ``_key_for_frame`` maps the input DTO's ``frame_id`` (int on
# ``VioOutput`` + ``PoseEstimate``, UUID on the C5 output) to a
# GTSAM ``Key``. The dict accepts any hashable.
self._key_for_frame: dict[Any, int] = {}
self._next_key_counter: int = 0
# Initialised to 0 ⇒ ``last_anchor_age_ms`` returns
# ``monotonic_ns() / 1e6`` (a very large number) until AZ-383
@@ -117,7 +128,33 @@ class GtsamIsam2StateEstimator(StateEstimator):
# documents this as the "no anchor yet" sentinel.
self._last_anchor_ns: int = 0
get_logger("c5_state.gtsam_isam2").debug(
# AZ-383 state -----------------------------------------------------
self._log = get_logger("c5_state.gtsam_isam2")
# The handle is constructed by ``create(...)`` and assigned
# post-init so the estimator can call back into it during
# ``add_*``. AZ-383 (not AZ-382) introduced this back-reference;
# before then the handle was held only by the composition root.
self._isam2_handle: ISam2GraphHandle | None = None
# Invariant 2 — strictly non-decreasing per-stream timestamps.
# Stored as int ns on the wall-clock (datetime → ns for
# VioOutput/PoseEstimate; ts_end_ns for ImuWindow). The merge
# rule across the three streams is the composition root's job;
# C5 only enforces the per-call monotonicity guard.
self._last_added_ts_ns: int = 0
# ``add_vio`` between-factor wiring.
self._prev_vio: VioOutput | None = None
# ``add_fc_imu`` uses an INDEPENDENT (X, V, B) keyframe chain
# decoupled from the VIO/pose frame_id mapping — see the
# implementation note on add_fc_imu below.
self._imu_keyframe_counter: int = 0
self._prev_imu_x_key: int | None = None
self._prev_imu_v_key: int | None = None
self._prev_imu_b_key: int | None = None
self._log.debug(
"c5.state.isam2_initialised",
extra={
"kind": "c5.state.isam2_initialised",
@@ -141,14 +178,25 @@ class GtsamIsam2StateEstimator(StateEstimator):
f"config.components['c5_state'] must be a C5StateConfig; got {type(block).__name__}"
)
def key_for_frame(self, frame_id: UUID) -> int:
def attach_handle(self, handle: ISam2GraphHandle) -> None:
"""Wire the iSAM2 graph handle.
Called once by :func:`create` after the handle is constructed
against this estimator instance. The handle is required by
every ``add_*`` method, so any ``add_*`` call against an
estimator with no handle raises ``StateEstimatorConfigError``.
"""
self._isam2_handle = handle
def key_for_frame(self, frame_id: UUID | int) -> int:
"""Return the GTSAM ``Key`` for ``frame_id``, assigning on first use.
AZ-383 factor adds call this to translate ``UUID`` frame
identifiers into the integer keys GTSAM uses for symbols. Per
the C5 contract §"Key management policy" we reserve the
``'x'`` (pose) namespace; AZ-383 will additionally use
``'b'`` (bias) and ``'v'`` (velocity).
AZ-383 calls this from ``add_vio`` and ``add_pose_anchor`` to
translate the input DTO's frame identifier into the integer
key GTSAM uses. Per the C5 contract §"Key management policy"
we reserve the ``'x'`` (pose) namespace; ``add_fc_imu``
additionally uses ``'v'`` (velocity) and ``'b'`` (bias) via
its own internal counter.
"""
existing = self._key_for_frame.get(frame_id)
if existing is not None:
@@ -158,21 +206,268 @@ class GtsamIsam2StateEstimator(StateEstimator):
self._next_key_counter += 1
return new_key
# ------------------------------------------------------------------
# AZ-383: factor-add bodies.
def add_vio(self, vio: VioOutput) -> None:
raise NotImplementedError(
"Factor adds owned by AZ-383 — VIO/Pose/IMU factor wiring lands there."
"""Append a VIO ``BetweenFactorPose3`` to the running graph.
First call after construction records the initial pose without
emitting a factor (there's no previous frame to relate to);
every subsequent call builds a between-factor between the
previous and the current VIO frame's pose keys and triggers a
single ``handle.update()`` per AC-7.
"""
handle = self._require_handle()
ts_ns = _datetime_to_ns(vio.timestamp)
self._guard_timestamp(ts_ns, source="vio")
curr_pose = _pose_se3_to_gtsam(vio.pose_se3)
curr_key = self.key_for_frame(vio.frame_id)
if self._prev_vio is None:
# First VIO frame — record its absolute pose as an initial
# value but DO NOT emit a between-factor. AZ-388 (AC-5.2)
# owns the first-keyframe seeding once the full chain lands.
self._prev_vio = vio
self._last_added_ts_ns = ts_ns
self._log.debug(
"c5.state.add_vio_first_frame",
extra={
"kind": "c5.state.add_vio_first_frame",
"kv": {"frame_id": str(vio.frame_id), "ts_ns": ts_ns},
},
)
return
prev_pose = _pose_se3_to_gtsam(self._prev_vio.pose_se3)
prev_key = self.key_for_frame(self._prev_vio.frame_id)
relative_pose = prev_pose.between(curr_pose)
noise = _build_pose_noise(vio.covariance_6x6)
factor = gtsam.BetweenFactorPose3(prev_key, curr_key, relative_pose, noise)
try:
handle.add_factor(factor)
self._values.insert(curr_key, curr_pose)
timestamps = _make_timestamp_map([curr_key], ts_ns)
handle.update(self._graph, self._values, timestamps)
except EstimatorDegradedError:
raise
except Exception as exc:
self._log.error(
"c5.state.add_vio_failed",
extra={
"kind": "c5.state.add_vio_failed",
"kv": {"frame_id": str(vio.frame_id), "error": str(exc)},
},
)
raise EstimatorDegradedError(f"add_vio failed: {exc}") from exc
self._reset_staging()
self._prev_vio = vio
self._last_added_ts_ns = ts_ns
self._log.debug(
"c5.state.add_vio_ok",
extra={
"kind": "c5.state.add_vio_ok",
"kv": {
"frame_id": str(vio.frame_id),
"prev_key": prev_key,
"curr_key": curr_key,
},
},
)
def add_pose_anchor(self, pose: PoseEstimate) -> None:
raise NotImplementedError(
"Factor adds owned by AZ-383 — VIO/Pose/IMU factor wiring lands there."
"""Add a C4 pose anchor; mode-dispatched per ``pose.covariance_mode``.
``"marginals"`` path builds a ``PriorFactorPose3`` on the pose
key and triggers ``handle.update()``. ``"jacobian"`` path
SKIPS the iSAM2 add per the AZ-361 contract — the running
estimate is fed but the graph stops growing under throttle.
Both paths bump ``_last_anchor_ns`` so the spoof-promotion
gate (AZ-385) and ``last_anchor_age_ms`` see a recent anchor.
"""
handle = self._require_handle()
ts_ns = _datetime_to_ns(pose.timestamp)
self._guard_timestamp(ts_ns, source="pose_anchor")
pose_key = self.key_for_frame(pose.frame_id)
mode = (pose.covariance_mode or "marginals").lower()
# Both paths update the anchor freshness sentinel. The C5
# contract documents this — even the throttled JACOBIAN path
# counts as a recent anchor for AC-1.3 binning.
self._last_anchor_ns = time.monotonic_ns()
if mode == "marginals":
gtsam_pose = _pose_se3_to_gtsam(pose.pose_se3)
noise = _build_pose_noise(pose.covariance_6x6)
factor = gtsam.PriorFactorPose3(pose_key, gtsam_pose, noise)
try:
handle.add_factor(factor)
self._values.insert(pose_key, gtsam_pose)
timestamps = _make_timestamp_map([pose_key], ts_ns)
handle.update(self._graph, self._values, timestamps)
except EstimatorDegradedError:
raise
except Exception as exc:
self._log.error(
"c5.state.add_pose_anchor_failed",
extra={
"kind": "c5.state.add_pose_anchor_failed",
"kv": {"frame_id": str(pose.frame_id), "error": str(exc)},
},
)
raise EstimatorDegradedError(f"add_pose_anchor failed: {exc}") from exc
self._reset_staging()
self._log.debug(
"c5.state.add_pose_anchor_ok",
extra={
"kind": "c5.state.add_pose_anchor_ok",
"kv": {
"frame_id": str(pose.frame_id),
"pose_key": pose_key,
"covariance_mode": mode,
},
},
)
else:
# JACOBIAN path — AZ-361 contract: skip the iSAM2 add to
# keep the graph from growing under throttle.
self._log.info(
"c5.state.skip_isam2_jacobian_path",
extra={
"kind": "c5.state.skip_isam2_jacobian_path",
"kv": {
"frame_id": str(pose.frame_id),
"pose_key": pose_key,
"covariance_mode": mode,
},
},
)
self._last_added_ts_ns = ts_ns
def add_fc_imu(self, imu_window: ImuWindow) -> None:
"""Build a ``CombinedImuFactor`` from a window of FC IMU samples.
The window's samples are fed into the shared
:class:`ImuPreintegrator`; ``reset_for_new_keyframe()``
closes the PIM and yields the
``PreintegratedCombinedMeasurements`` used by the factor.
The (X, V, B) keyframe chain advances per call:
- First call primes the chain (no factor — no previous
keyframe to relate to).
- Subsequent calls add a factor between the previous and the
current (X, V, B) triple and insert initial values for the
new keys (identity Pose3, zero Velocity, zero Bias).
This chain is intentionally independent of the VIO/pose
frame_id mapping. The composition root will arrange the
higher-level scheduling that fuses the chains; AZ-383 only
owns the per-window factor add.
"""
handle = self._require_handle()
self._guard_timestamp(imu_window.ts_end_ns, source="imu_window")
try:
self._imu_preintegrator.integrate_window(imu_window)
closed_pim = self._imu_preintegrator.reset_for_new_keyframe()
except Exception as exc:
self._log.error(
"c5.state.add_fc_imu_preintegrate_failed",
extra={
"kind": "c5.state.add_fc_imu_preintegrate_failed",
"kv": {"ts_end_ns": imu_window.ts_end_ns, "error": str(exc)},
},
)
raise EstimatorDegradedError(f"add_fc_imu preintegrate failed: {exc}") from exc
curr_x_key = gtsam.symbol("x", self._next_key_counter)
curr_v_key = gtsam.symbol("v", self._imu_keyframe_counter)
curr_b_key = gtsam.symbol("b", self._imu_keyframe_counter)
self._next_key_counter += 1
self._imu_keyframe_counter += 1
if self._prev_imu_x_key is None:
# First IMU window — prime the chain. AZ-388 owns the
# first-keyframe seeding (prior factor + initial values)
# once the AC-5.2 fallback wiring lands.
self._prev_imu_x_key = curr_x_key
self._prev_imu_v_key = curr_v_key
self._prev_imu_b_key = curr_b_key
self._last_added_ts_ns = imu_window.ts_end_ns
self._log.debug(
"c5.state.add_fc_imu_first_window",
extra={
"kind": "c5.state.add_fc_imu_first_window",
"kv": {
"ts_end_ns": imu_window.ts_end_ns,
"x_key": curr_x_key,
"v_key": curr_v_key,
"b_key": curr_b_key,
},
},
)
return
factor = gtsam.CombinedImuFactor(
self._prev_imu_x_key,
self._prev_imu_v_key,
curr_x_key,
curr_v_key,
self._prev_imu_b_key,
curr_b_key,
closed_pim,
)
def add_fc_imu(self, imu_window: ImuTelemetrySample) -> None:
raise NotImplementedError(
"Factor adds owned by AZ-383 — VIO/Pose/IMU factor wiring lands there."
try:
handle.add_factor(factor)
# Initial values for the new triple — identity pose,
# zero velocity, zero bias. The smoother will refine.
self._values.insert(curr_x_key, gtsam.Pose3())
self._values.insert(curr_v_key, np.zeros(3, dtype=np.float64))
self._values.insert(curr_b_key, gtsam.imuBias.ConstantBias())
timestamps = _make_timestamp_map(
[curr_x_key, curr_v_key, curr_b_key], imu_window.ts_end_ns
)
handle.update(self._graph, self._values, timestamps)
except EstimatorDegradedError:
raise
except Exception as exc:
self._log.error(
"c5.state.add_fc_imu_failed",
extra={
"kind": "c5.state.add_fc_imu_failed",
"kv": {"ts_end_ns": imu_window.ts_end_ns, "error": str(exc)},
},
)
raise EstimatorDegradedError(f"add_fc_imu failed: {exc}") from exc
self._reset_staging()
self._prev_imu_x_key = curr_x_key
self._prev_imu_v_key = curr_v_key
self._prev_imu_b_key = curr_b_key
self._last_added_ts_ns = imu_window.ts_end_ns
self._log.debug(
"c5.state.add_fc_imu_ok",
extra={
"kind": "c5.state.add_fc_imu_ok",
"kv": {
"ts_end_ns": imu_window.ts_end_ns,
"x_key": curr_x_key,
"v_key": curr_v_key,
"b_key": curr_b_key,
},
},
)
# ------------------------------------------------------------------
# AZ-384 / AZ-385 / AZ-386 — body still owned by those tasks.
def current_estimate(self) -> EstimatorOutput:
raise NotImplementedError(
"Marginals + outputs owned by AZ-384 — current_estimate body lands there."
@@ -188,6 +483,42 @@ class GtsamIsam2StateEstimator(StateEstimator):
"Marginals + outputs owned by AZ-384 — health_snapshot body lands there."
)
# ------------------------------------------------------------------
# Internals.
def _require_handle(self) -> ISam2GraphHandle:
if self._isam2_handle is None:
raise StateEstimatorConfigError(
"ISam2GraphHandle not attached; call attach_handle(...) before any add_*"
)
return self._isam2_handle
def _guard_timestamp(self, ts_ns: int, *, source: str) -> None:
if ts_ns < self._last_added_ts_ns:
self._log.error(
"c5.state.out_of_order",
extra={
"kind": "c5.state.out_of_order",
"kv": {
"source": source,
"ts_ns": ts_ns,
"last_added_ts_ns": self._last_added_ts_ns,
},
},
)
raise EstimatorDegradedError(
f"out-of-order {source}: ts_ns={ts_ns} < last_added_ts_ns={self._last_added_ts_ns}"
)
def _reset_staging(self) -> None:
"""Clear the staging ``_graph`` + ``_values`` after a successful flush."""
self._graph = gtsam.NonlinearFactorGraph()
self._values = gtsam.Values()
# ----------------------------------------------------------------------
# Module-level helpers.
def create(
*,
@@ -199,10 +530,10 @@ def create(
) -> tuple[StateEstimator, ISam2GraphHandle]:
"""Composition-root factory — returns ``(estimator, handle)`` tuple.
The handle holds a reference to the estimator so the four
``ISam2GraphHandleImpl`` method bodies (defined in
:mod:`...components.c5_state._isam2_handle`) can drive the same
graph the estimator owns.
Construction order matters: the estimator is built first, the
handle is wired against it (the handle holds the estimator
reference for graph access), and the estimator is back-wired with
the handle so its ``add_*`` methods can call into it.
"""
estimator = GtsamIsam2StateEstimator(
config,
@@ -212,6 +543,7 @@ def create(
fdr_client=fdr_client,
)
handle = ISam2GraphHandleImpl(estimator)
estimator.attach_handle(handle)
return estimator, handle
@@ -228,3 +560,57 @@ def register() -> None:
from gps_denied_onboard.runtime_root.state_factory import register_state_estimator
register_state_estimator(_STRATEGY, create)
# ----------------------------------------------------------------------
# Module-level pure helpers (AZ-383).
def _datetime_to_ns(dt: Any) -> int:
"""Convert ``dt.timestamp() * 1e9`` to int ns.
Accepts any object exposing a ``timestamp()`` method (e.g.
:class:`datetime.datetime`); avoids importing ``datetime`` here
so test stubs can pass synthetic time objects.
"""
return int(dt.timestamp() * 1_000_000_000)
def _pose_se3_to_gtsam(pose_se3: Any) -> gtsam.Pose3:
"""Wrap a 4x4 numpy SE(3) matrix into ``gtsam.Pose3``."""
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 gtsam.Pose3(arr)
def _build_pose_noise(covariance: Any | None) -> gtsam.noiseModel.Base:
"""Build a 6-DoF Gaussian noise model from a 6x6 covariance.
Falls back to an isotropic default when the input is ``None`` —
the C5 contract permits this for early-flight VIO frames where
the covariance is not yet computed; the iSAM2 solver penalises
over-tight priors so the default is conservative.
"""
if covariance is None:
return gtsam.noiseModel.Isotropic.Sigma(6, _DEFAULT_POSE_SIGMA)
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 gtsam.noiseModel.Gaussian.Covariance(cov)
def _make_timestamp_map(
keys: list[int], ts_ns: int
) -> gtsam_unstable.FixedLagSmootherKeyTimestampMap:
"""Build a ``FixedLagSmootherKeyTimestampMap`` for the smoother.
The smoother needs per-key arrival timestamps in seconds (its
sliding-window evict logic uses them); we feed every newly
inserted key the same window-end timestamp.
"""
ts_map = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
ts_seconds = ts_ns * 1e-9
for key in keys:
ts_map.insert((key, ts_seconds))
return ts_map
src/gps_denied_onboard/components/c5_state/interface.py
+9 -3
View File
@@ -20,7 +20,7 @@ from __future__ import annotations
from typing import TYPE_CHECKING, Protocol, runtime_checkable
if TYPE_CHECKING:
from gps_denied_onboard._types.fc import ImuTelemetrySample
from gps_denied_onboard._types.nav import ImuWindow
from gps_denied_onboard._types.pose import PoseEstimate
from gps_denied_onboard._types.state import (
EstimatorHealth,
@@ -52,8 +52,14 @@ class StateEstimator(Protocol):
factor + update cycle.
"""
def add_fc_imu(self, imu_window: ImuTelemetrySample) -> None:
"""Add an FC IMU sample / window to the iSAM2 preintegrator."""
def add_fc_imu(self, imu_window: ImuWindow) -> None:
"""Add an FC IMU window to the iSAM2 preintegrator (CombinedImuFactor).
AZ-383 fix-up: AZ-381 incorrectly annotated this as
``ImuTelemetrySample`` (a single sample); the C5 contract has
always specified ``ImuWindow`` because the shared
``ImuPreintegrator`` operates on batches.
"""
def current_estimate(self) -> EstimatorOutput:
"""Return the latest (non-smoothed) estimate. Never returns ``None``."""