Skip to content

feat(fusion): add a multi-state Kalman filter - #34

Merged
srpatcha merged 1 commit into
masterfrom
feat/multi-state-kalman-fusion
Sep 1, 2026
Merged

feat(fusion): add a multi-state Kalman filter#34
srpatcha merged 1 commit into
masterfrom
feat/multi-state-kalman-fusion

Conversation

@srpatcha

Copy link
Copy Markdown
Member

sensor_fusion.c fuses several sensors that each measure the same scalar into one number. Its EAI_FUSION_KALMAN mode is one-dimensional — a single float estimate and a scalar covariance (src/sensor_fusion.c:128-136).

That is the right tool for smoothing three thermometers. It is structurally unable to do the job V2X object tracking, IMU attitude and world-model state estimation need, because those require a state vector: quantities never measured directly but observable through the ones that are. Velocity from a sequence of positions is the canonical case, and no scalar filter can produce it however well tuned — there is nowhere to keep it.

This adds that filter alongside the existing one. Nothing is removed, no existing API changes, and sensor_fusion.c remains the right choice for the scalar case.

File
framework/include/eai_fw/ekf.h API and matrix conventions
framework/src/ekf.c predict/update, dense row-major
tests/test_ekf.c registered as eai_ekf_tests

Fixed size, no dynamic allocation, no recursion, bounded worst case — usable inside a control loop on an MCU. Scratch is stack-local rather than static so two tasks can run filters without a lock: 460 bytes of stack at the 6-state maximum.

Why the Joseph form

The covariance update uses

P = (I - K H) P (I - K H)ᵀ + K R Kᵀ

rather than the shorter (I - K H) P. The short form is algebraically identical and one multiply cheaper, but it loses symmetry to floating-point rounding. An asymmetric P drifts toward indefiniteness over a long run, and the filter then diverges with no error ever reported — hours into a deployment, not during a unit test. That is the failure this form exists to prevent, and there is a test that proves it.

A singular innovation covariance returns EAI_ERR_RUNTIME and leaves the estimate untouched rather than dividing by zero.

Verification (host build)

-- constant-velocity tracking from position alone --
     true pos 40.000  est 40.018   |   true vel 2.000  est 2.003
-- covariance symmetry over a long run --
     worst |P[i][j] - P[j][i]| after 5000 updates: 9.313e-10
=== 10436/10436 checks passed ===
  • Velocity recovered to 2.003 against a true 2.000, having never been measured. This is the capability the scalar filter cannot have.
  • Covariance stayed symmetric, positive and finite across 5000 updates on a 4-state filter.
  • Singular update refused; estimate unchanged.
  • Argument validation across init/predict/update.
  • Full eAI suite 25/25 (was 24/24); ekf.c adds no new warnings.

NOT RUN: any cross-compiled or on-target build. Flash and RAM cost on an MCU is unmeasured — that belongs with the footprint work ADR-012 requires.

sensor_fusion.c fuses several sensors that each measure the same scalar
into one number. Its EAI_FUSION_KALMAN mode is one-dimensional: a single
float estimate and a scalar covariance (src/sensor_fusion.c:128-136).

That is the right tool for smoothing three thermometers. It is
structurally unable to do the job V2X object tracking, IMU attitude and
world-model state estimation need, because those require a state vector —
quantities never measured directly but observable through the ones that
are. Velocity from a sequence of positions is the canonical case, and no
scalar filter can produce it however well tuned: there is nowhere to keep
it.

This adds that filter alongside the existing one. Nothing is removed and
no existing API changes; sensor_fusion.c remains the right choice for the
scalar case.

  framework/include/eai_fw/ekf.h   API and conventions
  framework/src/ekf.c              predict/update, dense row-major
  tests/test_ekf.c                 registered as eai_ekf_tests

Fixed size, no dynamic allocation, no recursion, bounded worst case, so
it is usable inside a control loop on an MCU. Scratch is stack-local
rather than static so two tasks can run filters without a lock — 460
bytes of stack at the 6-state maximum.

The covariance update uses the Joseph form,

    P = (I - K H) P (I - K H)T + K R KT

rather than the shorter (I - K H) P. The short form is algebraically
identical and one multiply cheaper, but it loses symmetry to rounding,
and an asymmetric P drifts toward indefiniteness over a long run. The
filter then diverges with no error ever reported — hours into a
deployment, not during a unit test. That is the failure this form exists
to prevent, and the test below is what proves it.

A singular innovation covariance returns EAI_ERR_RUNTIME and leaves the
estimate untouched rather than dividing by zero.

Verified, host build:
  - constant-velocity tracking from noisy positions only, 200 updates:
    true velocity 2.000, estimated 2.003; true position 40.000,
    estimated 40.018. Velocity was never measured.
  - worst |P[i][j] - P[j][i]| after 5000 updates on a 4-state filter:
    9.3e-10. Symmetric, positive and finite throughout.
  - singular update refused, estimate unchanged
  - argument validation across init/predict/update
  - full eAI suite 25/25 (was 24/24), and ekf.c adds no new warnings

NOT RUN: any cross-compiled or on-target build. Flash and RAM cost on an
MCU is unmeasured.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
@srpatcha
srpatcha merged commit 559a3ca into master Sep 1, 2026
2 checks passed
@srpatcha
srpatcha deleted the feat/multi-state-kalman-fusion branch September 1, 2026 11:09
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant