feat(fusion): add a multi-state Kalman filter - #34
Merged
Conversation
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>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
sensor_fusion.cfuses several sensors that each measure the same scalar into one number. ItsEAI_FUSION_KALMANmode is one-dimensional — a singlefloatestimate 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.cremains the right choice for the scalar case.framework/include/eai_fw/ekf.hframework/src/ekf.ctests/test_ekf.ceai_ekf_testsFixed 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
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 asymmetricPdrifts 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_RUNTIMEand leaves the estimate untouched rather than dividing by zero.Verification (host build)
init/predict/update.ekf.cadds 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.