Quaternion-Based Multiplicative Extended Kalman Filter (MEKF)

TL;DR: I built a quaternion-based MEKF for real-time attitude estimation: gyro-driven prediction + two sequential 3D vector updates (accelerometer then magnetometer). The filter was prototyped in MATLAB/Simulink and deployed on STM32G431KB in C.

Problem

Reliable attitude estimation on resource-constrained embedded hardware requires:

• stable 3D orientation representation (avoid gimbal lock),
• robustness to vibration / acceleration bursts and magnetic disturbances,
• predictable compute and memory usage for real-time execution.

Gyro integration alone drifts over time; accelerometer and magnetometer provide long-term references but are often corrupted during maneuvers or in disturbed environments.

Approach: Quaternion MEKF (Right-Multiplicative)

This implementation follows a multiplicative error-state formulation:

• Nominal state uses a unit quaternion q.
• The error state is a small-angle rotation δθ and gyro bias error δb (total size 6).
• Corrections are applied right-multiplicatively:

q ← q ⊗ δq(δθ)
δq(δθ) ≈ [1, 0.5*δθ]  (then normalized)

Sensor usage

• Prediction: gyro (bias-corrected)

• Update: two sequential direction measurements

  1. Accelerometer (gravity direction → roll/pitch stabilization)
  2. Magnetometer (magnetic field direction → yaw stabilization)

System Block Diagram (Simulink)

Estimator Pipeline

1. Bias correction

ω = ω_meas − b_g

2. Quaternion propagation

q ← q ⊗ dq(ω, dt)

3. Covariance propagation

P ← Φ P Φᵀ + Qd

4. Accelerometer update (gating + adaptive R)

5. Magnetometer update (gating + adaptive R)

6. Outputs: quaternion + Euler angles.

Embedded Deployment (STM32G431KB)

Implementation priorities:

• small fixed-size math (3×3, 6×6) without external dependencies,
• numerically stable covariance update (Joseph form),
• measurement gating to reject outliers,
• adaptive tuning to keep the filter responsive during fast rotation.

Core Implementation Notes

Adaptive process noise (Static vs Dynamic)

A simple motion detector switches noise parameters based on accelerometer and gyro magnitudes:

• Static mode: bias is allowed to evolve faster (learn bias);
• Dynamic mode: bias is nearly locked (avoid chasing motion).

Additionally, the gyro process noise is scaled with angular-rate magnitude:

adaptive_scale = 1 + 2*|ω|

Code Snapshots

The following excerpts show structure and key design choices (not full derivations).

Prediction (Gyro integration + P propagation)

void mekf_predict(MEKF* f, const vec3f_t omega_meas, const vec3f_t acc_meas, float dt)
{
    // omega_corrected = omega_meas - b_g
    vec3f_t omega_corrected = {
        omega_meas[0] - f->state.b_g[0],
        omega_meas[1] - f->state.b_g[1],
        omega_meas[2] - f->state.b_g[2]
    };
 
    // Static/Dynamic detection using |acc|-1 and |gyro|
    // -> choose gyro/bias noise levels
 
    // Adaptive process scaling: 1 + 2*|omega_corrected|
 
    // Quaternion propagation (body-frame): q <- q ⊗ dq
 
    // Covariance propagation: P <- Phi * P * Phi^T + Qd
}

Shared 3D direction update (Accel/Mag)

static void mekf_update_dir(MEKF* f,
                           const vec3f_t z_body,
                           const vec3f_t ref_dir,
                           const mat3f_t R_meas)
{
    // Normalize z
 
    // a_hat = R(q)^T * ref_dir   (R: nav->body)
    // r = z - a_hat
 
    // H = [skew(a_hat), 0]
    // S = HPH^T + R
 
    // Gate using gamma = r^T S^-1 r (df=3)
 
    // K = P H^T S^-1
    // dx = K r -> dtheta, dbias
 
    // Right-multiplicative correction:
    // q <- q ⊗ dq(dtheta)
    // b_g <- b_g + dbias
 
    // Joseph form covariance update
}

Sequential updates (Accel then Mag)

void mekf_update_accel(MEKF* f, const vec3f_t accel_body)
{
    // Norm gate: 0.6g - 1.4g
    // Adaptive R: exp(|norm-1| * 8), clamped
    mekf_update_dir(f, accel_body, f->g_ref, R_acc);
}
 
void mekf_update_mag(MEKF* f, const vec3f_t mag_body)
{
    // Norm gate: 0.5 - 1.5
    // Adaptive R: exp(|norm-1| * 5)
    mekf_update_dir(f, mag_body, f->m_ref, R_mag);
}

Quaternion utilities (minimal)

// Convention: [w, x, y, z]
// Hamilton product: q = q1 ⊗ q2
quat_t q_mul(quat_t q1, quat_t q2);
 
// δq(δθ) ≈ [1, 0.5*δθ] then normalize
quat_t q_from_small_angle(const vec3f_t dtheta);
 
void q_to_rotmat(quat_t q, mat3f_t R);
void q_to_euler(quat_t q, vec3f_t euler_rad);