mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 02:11:14 +00:00
Oleksandr Bezdieniezhnykh
fd848266d1
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>
15 lines
352 B
Markdown
15 lines
352 B
Markdown
# Autodev State
|
|
|
|
## Current Step
|
|
flow: greenfield
|
|
step: 7
|
|
name: Implement
|
|
status: in_progress
|
|
sub_step:
|
|
phase: 6
|
|
name: implement-tasks
|
|
detail: "batch 14 of N committed (AZ-383 c5 factor adds: add_vio BetweenFactorPose3 + add_pose_anchor mode-dispatch + add_fc_imu CombinedImuFactor with v/b key namespaces)"
|
|
retry_count: 0
|
|
cycle: 1
|
|
tracker: jira
|