A six-state MEKF attitude filter

[bjornhbengtsson]

Description

Adds a six-state Multiplicative Extended Kalman Filter (MEKF) for attitude estimation using gyroscope prediction and accelerometer gravity-direction correction.

The nominal state contains a body-to-world attitude quaternion and a three-axis gyroscope-bias estimate. The Kalman error state contains three local attitude-error components and three gyro-bias-error components with a 6x6 covariance matrix.

Key changes

• Adds MEKF configuration and initialization
• Adds body-frame gyroscope prediction with gyro-bias subtraction
• Adds right-multiplicative quaternion propagation and normalization
• Adds six-state covariance prediction with attitude/bias coupling
• Adds gyroscope white-noise and gyro-bias random-walk process noise
• Adds accelerometer gravity-direction correction
• Adds accelerometer magnitude and normalized-innovation gating
• Adds Joseph-form covariance correction and MEKF covariance reset
• Adds covariance symmetry handling, input validation, and state-preserving failure paths
• Moves VECTOR_3F into math_sdr so it can be shared by Mahony and MEKF
• Adds comprehensive MEKF unit tests

Conventions

• Quaternion storage order: {w, x, y, z}
• Nominal attitude is body-to-world
• Gyroscope measurements are body-frame angular rates in radians per second
• Attitude propagation uses right multiplication
• Attitude error is a right-multiplicative local body-frame error
• Error state ordering is attitude error X/Y/Z followed by gyro-bias error X/Y/Z

Larger Documentation

• MEKF_DOCUMENTATION.html

References:

• https://www.anuncommonlab.com/articles/how-kalman-filters-work/
• https://youtu.be/VoO_T5GVW5c?is=Ef2esBiyFBNGZd7Z
• https://ntrs.nasa.gov/api/citations/20040037784/downloads/20040037784.pdf
• https://youtu.be/FgwVju8PcFw?is=NF1f2W4PJLnNyxim
• https://youtube.com/playlist?list=PLUeHTafWecAVRogm-cRiYfydRMFUpqJok&si=pWFGvYnePkf7kVMc

Made with assistance of GPT-5.6 Sol

Issue Link

Related to closing: #64

Parent PR: #142

Testing

• Passes existing unit tests
• Unit tests modified (link the test changes as a child PR)
• Integration test performed

MEKF unit tests are included directly in this PR.

MEKF unit-test result:

Passes: 722
Fails: 0
Result: PASS

Other

Magnetometer correction is intentionally deferred until hard-iron/soft-iron calibration and sensor-to-body orientation are verified.

Other simulation and validation will be completed separately.

Reviewer Checklist

Standards

• Follows FCF Architectural Standards
• Follows SDR Coding Standards
• Code complexity/function Size is minimized
• Code is testable
• Code is readable and commented properly
• License terms are respected

Error Handling

• Potentially unsafe functions return a status code
• Error returns properly handled

Memory

• Stack allocated memory is scoped correctly
• Heap allocated memory is avoided
• Globally allocated memory is minimized except when necessary
• Pointers are used correctly
• Concurrency has been considered

Performance

• Rate limiters are respected
• Busy waiting is avoided
• "Delay" calls are not used in performance sensitive code