Magnetometer Heading Fusion on MCU: Calibration and Gating

Key Takeaways

• The design keeps a gyro-propagated heading from drifting with a magnetometer observation corrected through a clamped PI loop, at O(1) time and static memory only.
• Calibration uses per-axis min/max spans rather than a least-squares ellipsoid fit: at least two valid axes are required, and each valid axis is rescaled to a common half-span R.
• Four series gates run before the expensive math — field magnitude, reference consistency, body rate and horizontal ratio — so rejected samples cost one square root and a few compares.
• The heading observation is built by rotating the filtered field into the navigation frame and taking atan2 of the horizontal components, so the output depends only on the magnetic measurement and declination.
• The code has real defects: calibration writes the instance before validating, invalid axes still get an offset written, and a rejected cycle leaves a stale confidence value for the caller.

On an MCU, a full multi-sensor Kalman filter is often too expensive in both cycles and RAM, so the practical pattern is a cascade: an attitude layer such as a Mahony complementary filter handles roll, pitch and high-rate yaw, while a magnetometer layer corrects low-frequency yaw drift. This article formalizes one such magnetometer layer from working production code and walks its implementation section by section. It answers four questions — how calibration estimates hard-iron offset and soft-iron scale from limited samples, how the heading observation is constructed and injected safely, how multi-stage gating with a low-pass filter and online learning cooperate, and how the code organizes state and error codes to stay real-time safe. The claim is engineering suitability under MCU constraints, not optimality over a Kalman filter.

Problem Statement and Notation

Frames and variables

The body frame b uses X forward, Y left, Z up. The navigation frame n has +X pointing to magnetic north, and heading ψ is positive counter-clockwise about Z_n.

• Raw magnetometer observation: m_r ∈ ℝ³, in mGauss
• Attitude quaternion: q = [q_w, q_x, q_y, q_z]^T
• Body angular rate: ω_b ∈ ℝ³, in degrees per second
• Fusion period: Δt, with the caller-supplied value written Δt_s

Correction model

The algorithm adopts a diagonal simplification of the ellipsoid model:

m_c = S(m_r − b)    (1)

Here b ∈ ℝ³ is the hard-iron offset and S = diag(s_x, s_y, s_z) is the soft-iron normalization scale. Equation 1 ignores inter-axis cross-coupling, which suits cases where soft-iron distortion is dominated by per-axis gain differences. Before calibration, b = 0 and S = I, so the correction is a pass-through.

Frame transformation

A unit quaternion rotates a body-frame vector into the navigation frame:

v_n = q ⊗ v_b ⊗ q*    (2)

The code uses the equivalent expansion. Let q_v = [q_x, q_y, q_z]^T, then:

t = 2q_v × v_b,    v_n = v_b + q_wt + q_v × t    (3)

Equation 3 needs only cross products and scalar multiply-adds, avoiding construction of a rotation matrix — exactly what an MCU wants.

Algorithm Design

Ellipsoid calibration

Calibration uses per-axis min/max extremes. Let the i-th axis have sample minimum m_i^min and maximum m_i^max, with span σ = m_i^max − m_i^min.

Validity criterion: an axis is valid when σ_i ≥ σ_min (100 mGauss by default). Submission requires at least two valid axes, which prevents “sampling while standing still” from being accepted as a calibration.

Reference field strength: the mean half-span of the valid axes.

R = (1/n_v) Σ_i∈V σ_i/2    (4)

Hard-iron offset:

b_i = (m_i^max + m_i^min) / 2    (5)

Soft-iron scale: for each valid axis, set the corrected axis radius to R, that is:

s_i = 2R/σ_i for i ∈ V;    s_i = 1 for i ∉ V    (6)

Equation 6 gives every valid axis the same half-span after correction, which approximately normalizes the ellipsoid to a sphere. Before submission the code must verify σ_i ≤ σ_max, ‖b‖ ≤ b_max and R ∈ [n_min, n_max].

Algorithm 1 gives the calibration submission flow, including the transactional improvement.

Algorithm 1  Ellipsoid calibration submission (transactional version)
Input:  min[3], max[3], count, min_samples
Output: SUCCESS or an error code
 1: if count < min_samples: return BUSY
 2: for i in 0..2:
 3:     span[i] ← max[i] - min[i]
 4:     if not finite(span[i]) or span[i] > SPAN_MAX: return RANGE
 5:     valid[i] ← span[i] ≥ SPAN_MIN
 6: if sum(valid) < 2: return QUALITY
 7: R ← mean(0.5*span[i] for i where valid[i])
 8: if R < NORM_MIN or R > NORM_MAX: return RANGE
 9: for i in 0..2:
10:     b_tmp[i] ← 0.5*(max[i]+min[i])
11:     s_tmp[i] ← valid[i] ? 2R/span[i] : 1.0
12: if norm(b_tmp) > OFFSET_MAX: return RANGE
13: commit b←b_tmp, S←diag(s_tmp), R_ref←R
14: return SUCCESS

Note: the original code already writes the instance offset at line 10, so a failure at line 12 leaves a “new offset with old scale” inconsistency. Algorithm 1 computes into locals first and commits once, removing that defect. Line 11 keeps s_i = 1.0 for invalid axes, but the original code still writes b_i for them, which can drive that axis toward zero after correction; an invalid axis should either keep its previous offset or be zeroed.

Constructing the heading observation

The corrected and low-pass filtered field m_f is rotated into the navigation frame by Equation 2, giving e = [e_x, e_y, e_z]^T. The horizontal projection azimuth is:

θ = atan2(e_y, e_x)    (7)

By the frame convention, θ equals “current heading estimate minus magnetic heading”. The expected correction angle is:

ε = wrap₁₈₀(δ − θ)    (8)

where δ is the magnetic declination, positive to the east, and ε > 0 means heading must increase. The resulting true heading is:

ψ = wrap₁₈₀(ψ_q + ε) = wrap₁₈₀(ψ_q + δ − (ψ_q − ψ_mag)) = wrap₁₈₀(ψ_mag + δ)    (9)

where ψ_q = atan2(2(q_wq_z + q_xq_y), 1 − 2(q_y² + q_z²)). Equation 9 shows the output heading is independent of the current attitude estimate and depends only on the magnetic observation and declination, which is why it serves as an absolute reference.

Multi-stage gating

Gates are chained cheapest-first, most expensive-last, and any failure rejects the sample for that cycle:

Gate	Test	Rejects
G1 field magnitude	‖mc‖ ∈ [nmin, nmax]	Sensor saturation or a broken lead
G2 reference consistency	|‖mc‖ − R| ≤ ρR, ρ = 0.25 default	External magnetic interference
G3 body rate	maxi|ωb,i| ≤ ωmax, 150 dps default	Maneuvering, where the magnetometer lags
G4 horizontal ratio	√(ex2 + ey2) ≥ η‖mc‖, η = 0.30 default	Near the dip-angle limit, or unreliable roll/pitch

Placing G1 (one square root), G2 (one compare) and G3 (three compares) before the quaternion rotation and atan2 lowers the average cost of the high-frequency path.

First-order low-pass filter

Each corrected field axis goes through a first-order IIR:

m_f[k] = m_f[k−1] + α(m_c[k] − m_f[k−1]),   α = Δt/(τ + Δt)    (10)

τ is the time constant, 20 ms by default. Equation 10 degenerates to a pass-through when τ = 0. The filter suppresses magnetometer noise and residual hard-iron jitter, but introduces a phase lag of arctan(ωτ), which is why G3 suspends fusion during maneuvers to avoid contaminating heading with that lag.

PI heading correction

The error ε in radians drives a PI controller:

I[k] = clamp(I[k−1] + εΔt, ±I_max)    (11)

ω_c = clamp(K_pε + K_iI, ±ω_c,max)    (12)

Δψ = ω_cΔt    (13)

Finally only the heading component of the attitude is rotated, about the navigation-frame Z axis:

q ← q_Δψ ⊗ q,   q_Δψ = [cos(Δψ/2), 0, 0, sin(Δψ/2)]^T    (14)

Equation 14 preserves roll and pitch, consistent with the gravity observation. K_p provides dynamic tracking and K_i absorbs residual gyro bias; I_max and ω_c,max prevent an abnormal observation from stepping the heading.

Online learning of the reference field strength

When R is not supplied explicitly, the first sample to pass every gate establishes the baseline, after which:

R ← R + γ(‖m_c‖ − R),   γ ∈ (0, 1]    (15)

γ is 0.005 by default. Equation 15 is an exponential moving average updated only on samples that passed the gates, so interference never pollutes the baseline.

Algorithm 2 gives the single-cycle fusion pseudo-code.

Algorithm 2  Single-cycle heading fusion
Input:  m_r, q, w_b, dt_s
Output: updated q, heading, confidence, status
 1: dt ← clamp(dt_s, dt_min, dt_max)
 2: m_c ← S(m_r - b)
 3: n ← ||m_c||;  if n not in [n_min,n_max]: return REJECTED
 4: if R ≤ 0: R ← n
 5: if |n - R| > rho*R: return REJECTED
 6: if max|w_b| > w_max: return REJECTED
 7: m_f ← LPF(m_c, dt)
 8: e ← Rot(q, m_f)
 9: h ← sqrt(e_x^2+e_y^2);  if h < eta*n: return REJECTED
10: theta ← atan2(e_y,e_x);  eps ← wrap180(delta - theta)
11: conf ← clamp(1-|n-R|/(rho*R),0,1) * clamp((h/n-eta)/(1-eta),0,1)
12: heading ← wrap180(yaw(q) + eps)
13: if auto: R ← R + gamma*(n-R)
14: if not enable: return OK
15: I ← clamp(I + eps*dt, +/-I_max)
16: w_c ← clamp(Kp*eps + Ki*I, +/-w_c_max)
17: RotateYaw(q, w_c*dt)
18: return OK

Implementation

This section follows the module layout, state organization, initialization, periodic fusion, calibration, auxiliary interfaces and private math helpers, explaining the code section by section. Code is reproduced as published; where identifiers were renamed for readability, a comment says so.

Module layout and build configuration

The module consists of mag_fusion.h and mag_fusion.c. The header exposes status codes, configuration, the calibration accumulator, the fusion instance and the API declarations; the source file implements all logic. The whole module is compiled out by a single switch:

#ifndef MAG_FUSION_ENABLE
#define MAG_FUSION_ENABLE 1U
#endif

The implementation body is wrapped in #if (MAG_FUSION_ENABLE == 1). It depends only on stdbool.h, stdint.h, math.h, string.h and the attitude module attitude.h, with no dynamic memory and no platform-specific calls. Every private function is static; only the header API is exported.

Data types and instance state

mag_fusion_t groups state into five sets: (1) configuration, cfg; (2) calibration results, offset_mgauss[3], scale[3], field_norm_mgauss, calibrated, auto_norm; (3) filter and PI state, lpf[3], lpf_valid, integral; (4) outputs and diagnostics, heading_deg, confidence, last_error_deg, last_correction_dps, update_count, rejected_norm/tilt/rate; (5) the calibration accumulator, calib. All state is statically allocated by the caller; the module never touches the heap.

mag_fusion_calib_t holds only a sample count, the minimum sample count, three-axis min/max and an active flag. Calibration caches no raw samples, so memory stays constant.

Initialization and configuration validation

mag_fusion_init() handles null pointers first, copies the configuration or fills defaults, then validates through mag_config_valid(), and finally memsets the whole instance and writes the pass-through calibration and reference field strength:

(void)memset(p_mag, 0, sizeof(*p_mag));
p_mag->cfg = cfg;
p_mag->offset_mgauss[0] = 0.0f; /* same for all three axes */
p_mag->scale[0] = 1.0f;         /* same for all three axes */
p_mag->field_norm_mgauss = cfg.field_norm_mgauss;
p_mag->auto_norm = (cfg.field_norm_mgauss <= 0.0f);
p_mag->initialized = true;

Implementation notes:

• Pass-through semantics: before calibration b = 0 and S = I, so fusion still works without calibration.
• Auto-learn decision: field_norm_mgauss <= 0 sets auto_norm=true, and Equation 15 establishes the baseline online later.
• Idempotence and side effects: memset wipes any previous calibration and statistics, so calling init twice loses calibration results. The header’s “idempotent” claim should be read as “safe to call repeatedly without crashing,” not “preserves state.”
• Configuration validation: mag_config_valid() checks finiteness and range on every float parameter, for example norm_tol_ratio ∈ (0,1], horiz_min_ratio ∈ (0,1), dt_max_s > dt_min_s and online_norm_gain ∈ [0,1]. It does not check that a positive field_norm_mgauss falls inside [norm_min, norm_max], which is a gap worth closing.

Periodic fusion in mag_fusion_update()

update() is the high-frequency path, ordered as entry checks, correction, gating, low-pass, rotation, observation, confidence, online learning, then PI correction. Each stage follows.

Entry checks and step clamping

if ((p_mag == NULL) || (p_att == NULL) || (mag_mgauss == NULL)) {
    return MAG_FUSION_ERR_PARAM;
}
if (!p_mag->initialized) return MAG_FUSION_ERR_NOT_INIT;
if (!p_att->initialized)  return MAG_FUSION_ERR_NOT_INIT;
if ((!mag_finite3(mag_mgauss)) || (!mag_finite(dt_s)) || (dt_s <= 0.0f)) {
    return MAG_FUSION_ERR_PARAM;
}
dt = mag_clampf(dt_s, p_mag->cfg.dt_min_s, p_mag->cfg.dt_max_s);

Null pointers, an uninitialized instance and non-finite input each return a distinguishable error code, and dt is clamped to [2, 200] ms to suppress sampling jitter. Note that the finiteness of p_att->q and p_att->rate is not checked here: if upstream attitude is polluted with NaN, atan2 and the PI loop can be contaminated, and every gate comparison against NaN evaluates false, so the sample cannot be rejected.

Correction and the G1/G2 gates

for (i = 0U; i < 3U; i++) {
    corrected[i] = (mag_mgauss[i] - p_mag->offset_mgauss[i]) * p_mag->scale[i];
}
norm = sqrtf(corrected[0]*corrected[0] + corrected[1]*corrected[1] + corrected[2]*corrected[2]);

if ((!mag_finite(norm)) ||
    (norm < p_mag->cfg.norm_min_mgauss) ||
    (norm > p_mag->cfg.norm_max_mgauss)) {
    p_mag->rejected_norm++;
    return MAG_FUSION_REJECTED;
}

if (p_mag->field_norm_mgauss <= 0.0f) {
    p_mag->field_norm_mgauss = norm;
}
field_norm = p_mag->field_norm_mgauss;
if (fabsf(norm - field_norm) > (p_mag->cfg.norm_tol_ratio * field_norm)) {
    p_mag->rejected_norm++;
    return MAG_FUSION_REJECTED;
}

Implementation notes:

• Correction applies Equation 1 axis by axis and is a pass-through before calibration.
• G1 also checks that norm is finite, so NaN cannot slip through a comparison.
• G2 uses the current norm to establish the baseline when the reference is zero, implementing “the first valid sample learns automatically,” then judges interference by relative tolerance.
• A physical range violation and a reference mismatch share the rejected_norm counter, so diagnostics cannot separate a sensor fault from magnetic interference.

G3 rate gate and the first-order low-pass

if ((fabsf(p_att->rate[0]) > p_mag->cfg.max_rate_dps) ||
    (fabsf(p_att->rate[1]) > p_mag->cfg.max_rate_dps) ||
    (fabsf(p_att->rate[2]) > p_mag->cfg.max_rate_dps)) {
    p_mag->rejected_rate++;
    return MAG_FUSION_REJECTED;
}

if (!p_mag->lpf_valid) {
    p_mag->lpf[0] = corrected[0]; /* same for all three axes */
    p_mag->lpf_valid = true;
} else {
    alpha = (p_mag->cfg.lpf_tau_s > 0.0f)
            ? (dt / (p_mag->cfg.lpf_tau_s + dt))
            : 1.0f;
    p_mag->lpf[0] += alpha * (corrected[0] - p_mag->lpf[0]); /* same for all three axes */
}
field[0] = p_mag->lpf[0]; /* same for all three axes */

Implementation notes:

• G3 sits before the low-pass, so maneuvers are rejected outright and low-pass phase lag never contaminates heading.
• The first valid sample seeds the filter directly through lpf_valid, avoiding a start-from-zero transient. lpf_valid is cleared in reset() and on a successful calibration commit.
• With tau=0, alpha=1 and the filter degenerates to a pass-through.
• Filter state does not update on a gated rejection, so a rejected cycle introduces no anomalous sample.

Frame rotation and the G4 horizontal gate

mag_rotate_by_quat(p_att->q, field, earth);

horizontal = sqrtf((earth[0]*earth[0]) + (earth[1]*earth[1]));
if (horizontal < (p_mag->cfg.horiz_min_ratio * norm)) {
    p_mag->rejected_tilt++;
    return MAG_FUSION_REJECTED;
}

mag_rotate_by_quat() implements Equation 3 with quaternion order q[0]=w, q[1]=x, q[2]=y, q[3]=z. G4 compares the horizontal share against the corrected magnitude norm rather than the filtered magnitude, approximating total field strength with the corrected value to save a square root.

Heading error, confidence and heading output

error_deg = atan2f(earth[1], earth[0]) * MAG_FUSION_RAD_TO_DEG;
error_deg = mag_wrap180(p_mag->cfg.declination_deg - error_deg);
error_rad = error_deg * MAG_FUSION_DEG_TO_RAD;

ratio = horizontal / norm;
confidence = mag_clampf(1.0f - (fabsf(norm - field_norm) /
                               (p_mag->cfg.norm_tol_ratio * field_norm)), 0.0f, 1.0f);
if (ratio <= p_mag->cfg.horiz_min_ratio) {
    confidence = 0.0f;
} else {
    confidence *= mag_clampf((ratio - p_mag->cfg.horiz_min_ratio) /
                             (1.0f - p_mag->cfg.horiz_min_ratio), 0.0f, 1.0f);
}

yaw_est_deg = mag_quat_yaw_deg(p_att->q);
p_mag->heading_deg = mag_wrap180(yaw_est_deg + error_deg);
if (p_mag->heading_deg > 180.0f) { p_mag->heading_deg -= 360.0f; }
else if (p_mag->heading_deg < -180.0f) { p_mag->heading_deg += 360.0f; }
p_mag->confidence = confidence;

Implementation notes:

• The error follows Equation 8 as declination minus observed azimuth, giving the desired correction angle; wrap180 keeps it continuous across ±180°.
• Confidence is the product of field consistency and horizontal share. It does not affect the output after Equation 9 and serves only diagnostics and higher-level decisions.
• Heading is computed per Equation 9. Re-checking >180 / <-180 after wrap180 is redundant because wrap180 already guarantees the range, but it does not affect correctness.
• A gated rejection does not update confidence, so a caller judging availability by confidence alone reads the previous cycle’s value.

Online learning and PI correction

if (p_mag->auto_norm) {
    p_mag->field_norm_mgauss += p_mag->cfg.online_norm_gain * (norm - field_norm);
}
p_mag->last_error_deg = error_deg;

if (!p_mag->cfg.enable) {
    return MAG_FUSION_OK;
}

if (p_mag->cfg.ki > 0.0f) {
    p_mag->integral += error_rad * dt;
    if (p_mag->integral > p_mag->cfg.integral_limit) { p_mag->integral = p_mag->cfg.integral_limit; }
    else if (p_mag->integral < -p_mag->cfg.integral_limit) { p_mag->integral = -p_mag->cfg.integral_limit; }
}

omega = (p_mag->cfg.kp * error_rad) + (p_mag->cfg.ki * p_mag->integral);
omega = mag_clampf(omega,
                   -p_mag->cfg.max_correction_dps * MAG_FUSION_DEG_TO_RAD,
                    p_mag->cfg.max_correction_dps * MAG_FUSION_DEG_TO_RAD);

delta_deg = omega * dt * MAG_FUSION_RAD_TO_DEG;
p_mag->last_correction_dps = omega * MAG_FUSION_RAD_TO_DEG;

if (attitude_rotate_yaw(p_att, delta_deg) != ATTITUDE_OK) {
    return MAG_FUSION_ERR_PARAM;
}
p_mag->update_count++;
return MAG_FUSION_OK;

Implementation notes:

• Online learning sits after every gate and before the enable check, so only samples that passed the gates contribute; with enable=false the reference strength and observation statistics still update.
• The enable check sits after the observation and confidence update but before PI, implementing a “observe without correcting” diagnostic mode. In that mode update_count does not increment.
• The integral term updates and clamps only when ki>0; omega is clamped per Equation 12 and delta_deg converted per Equation 13.
• Only attitude_rotate_yaw() changes heading, preserving roll and pitch. On failure it returns a generic ERR_PARAM, losing the specific error code.
• update_count increments only after a successful correction, so its meaning is “cycles that actually participated in fusion,” excluding observation-only cycles with enable=false.

Calibration state machine

The calibration API forms a begin → feed* → end/abort state machine. begin() clamps min_samples to [32, 10000], clears the count, sets active=true and initializes the three-axis min/max to sentinel values of ±10⁹.

feed() is implemented as:

if (!p_mag->calib.active) return MAG_FUSION_ERR_CALIB_NOT_ACTIVE;
if (!mag_finite3(mag_mgauss)) return MAG_FUSION_ERR_PARAM;
if (p_mag->calib.count >= MAG_FUSION_CALIB_MAX_SAMPLES) return MAG_FUSION_OK;

for (i = 0U; i < 3U; i++) {
    if (mag_mgauss[i] < p_mag->calib.min_mgauss[i]) p_mag->calib.min_mgauss[i] = mag_mgauss[i];
    if (mag_mgauss[i] > p_mag->calib.max_mgauss[i]) p_mag->calib.max_mgauss[i] = mag_mgauss[i];
}
p_mag->calib.count++;
return (p_mag->calib.count >= p_mag->calib.min_samples)
       ? MAG_FUSION_OK : MAG_FUSION_ERR_CALIB_BUSY;

Only comparisons and assignments run, so the cost is O(1). Once the maximum sample count is reached, new samples are ignored and OK is returned, avoiding an unbounded block; the return value distinguishes “enough samples” from “keep sampling.”

end() follows the steps of Algorithm 1, but the original code has a write-ordering problem: it writes offset_mgauss and accumulates offset_norm_sq first, then checks the magnitude, so a failure return leaves the instance partially modified. In addition, an invalid axis still receives an offset per Equation 5 while its scale stays 1, which can drive that axis toward zero after correction. On a successful commit:

p_mag->field_norm_mgauss = reference;
p_mag->calibrated = true;
p_mag->calib.active = false;
p_mag->lpf_valid = false;
p_mag->integral = 0.0f;

Clearing the low-pass and integral after commit avoids a mismatch between stale filter state and the new calibration; the rejection counters and update_count are preserved for diagnostics.

abort() only sets calib.active=false without clearing the accumulator, since the next begin() reinitializes it.

Correction and read-back interfaces

mag_fusion_correct() applies Equation 1 to a raw field and checks null pointers, initialization and finiteness, for diagnostics or higher-level reuse. mag_fusion_get_heading(), get_offset() and get_scale() are read-only copies that allow null output pointers (get_heading needs at least one non-null) and verify initialization. There is no locking; the caller must guarantee single-threaded access to a single instance.

Private math helpers

Private functions cover finiteness testing, clamping, angle unwrapping, quaternion rotation and yaw extraction:

static bool mag_finite(float value)
{
    return (value > -MAG_MACRO_FINITE_LIMIT) && (value < MAG_MACRO_FINITE_LIMIT);
}

static float mag_wrap180(float degree)
{
    while (degree > 180.0f)  degree -= 360.0f;
    while (degree < -180.0f) degree += 360.0f;
    return degree;
}

mag_finite() uses ±10⁶ as the bound and relies on NaN comparisons always being false to detect non-finite values. wrap180() uses while rather than fmodf so that iteration count is bounded for bounded input, avoiding library call overhead. mag_rotate_by_quat() expands Equation 3 using only temporary cross products tx/ty/tz plus multiply-adds; mag_quat_yaw_deg() extracts yaw with atan2f from the expression under Equation 9 without building a rotation matrix. mag_clampf() is a branch clamp reused by PI, dt and confidence.

Status codes and error handling strategy

Errors fall into three classes: parameter or state errors (ERR_PARAM, ERR_NOT_INIT), gate rejections (REJECTED) and calibration errors (CALIB_*). A gate rejection is not a fault — attitude stays unchanged and the caller should keep running. Parameter and state errors indicate programming mistakes. Calibration errors prompt the user in the field to rotate again or check the mounting. Every public API checks null pointers and initialization at entry, so misuse returns a locatable error code instead of crashing.

Engineering details visible in the implementation

1. Gate order and cost: G1 (one square root), G2 (one compare) and G3 (three compares) all precede the quaternion rotation and atan2, so the average cost of an invalid sample is far below that of a valid one.
2. Low-pass initialization: lpf_valid makes the first sample pass through, avoiding a start-from-zero transient; it clears after a successful calibration so the new calibration begins from a clean state.
3. enable semantics: with correction disabled, the code still performs correction, gating, low-pass, observation, confidence and online learning, skipping only PI and the attitude update, which supports live diagnostics.
4. update_count semantics: it increments only after a successful attitude_rotate_yaw() call, counting cycles that actually corrected.
5. State preservation on rejection: a gated rejection leaves the low-pass, integral, confidence and heading untouched and attitude unchanged; the caller must use the return code to know whether the cycle’s observation was valid.
6. Calibration and fusion share one instance: update() does not check calib.active, so whether fusion pauses during calibration is the caller’s decision.
7. No locking, static allocation: all state lives in the instance with no dynamic memory, and the same instance must not be called concurrently.

Algorithm Analysis

Complexity

Let the gate pass rate be P. An invalid sample runs only Equation 1, one square root and a constant number of compares, at O(1). A valid sample additionally runs one low-pass (nine multiply-adds), one quaternion rotation (about eighteen multiply-adds), one atan2, one yaw extraction atan2 and the PI loop (a constant number of operations). The worst case remains O(1), with no loop depending on the sample count — the while in wrap180 has a bounded iteration count for bounded input. In space, the instance is a fixed-size struct with no dynamic allocation, satisfying MCU static memory constraints.

Stability

Treating Equations 11 to 13 as a discrete PI controller on the attitude heading channel and ignoring gating and the low-pass, the error dynamics approximate:

ε[k+1] ≈ ε[k] − Δt(K_pε[k] + K_iI[k])    (16)

With 0 < K_pΔt < 2 the proportional term is locally stable, and the integral term cannot grow without bound while I_max clamps it. The defaults K_p = 1.2 and Δt ≤ 0.2 s give K_pΔt ≤ 0.24 << 2, so the proportional channel is over-damped and does not oscillate. The output clamp ω_c,max bounds the single-cycle correction to |Δψ| ≤ ω_c,maxΔt, which with the default 30 dps × 0.2 s = 6° prevents an abnormal observation from stepping the heading.

Note that Equation 11 keeps integrating while the output is saturated, so a large error held for a long time can cause integral windup and a slow recovery. With the defaults I_max = 0.25 and K_i = 0.03 the effect is limited, but if those parameters grow, conditional integration should be used — stop integrating while saturated and the error still points the same way.

Effect of gating on observation availability

G2 judges interference by relative tolerance ρ, and its rejection probability rises monotonically with interference strength. G3 switches to pure gyro during maneuvers, avoiding contamination by the low-pass lag of Equation 10. G4 uses the horizontal share η to exclude dip-angle extremes and large-attitude cases. Chaining four gates gives a pass rate p = p₁p₂p₃p₄ when they are approximately independent, close to 1 while stationary and interference-free and falling sharply under maneuver or interference — which is exactly the intent of “use it when usable, hand it to the gyro when not.”

Unwrapping and sampling jitter

Equations 8 and 9 both map angles to [−180°, 180°] with wrap₁₈₀, so the error never jumps by 360° across ±180°. Equation 11 clamps Δt to [2, 200] ms, suppressing the effect of sampling jitter on both the integral and the correction angle.

Calibration consistency

Equations 4 to 6 give every valid axis a corrected half-span of R when σ_i ≥ σ_min. But the original code still writes an offset for invalid axes per Equation 5, which can drive that axis toward zero after correction, break ‖m_c‖ and trigger continuous G2 rejection. The transactional commit and invalid-axis retention in Algorithm 1 remove that risk.

There is a second failure mode: if R deviates from the actual corrected magnitude by more than ρR, G2 rejects the samples, and Equation 15 updates only on samples that passed the gates, so the reference can lock itself out. The fix is to re-estimate R from several frames of corrected magnitude after calibration, or to add a recapture window.

Confidence semantics

The confidence computed before Equation 11 is the product of field consistency and horizontal share, but a gated rejection does not update confidence, so a caller judging availability by confidence alone can read a stale high value during a rejected cycle. Either zero the confidence on rejection, or require the caller to combine it with the REJECTED return code.

Discussion

Relation to a Kalman filter. This algorithm is a simplified observer made of observation gating plus a scalar PI loop. It does not model magnetometer noise covariance or gyro bias as a state, so it is not optimal in the EKF/UKF sense. What it buys is O(1) cost, no matrix math and no dynamic memory, which suits resource-constrained MCUs and high-rate control loops. The PI integral term functions approximately as a one-dimensional bias estimator, but without covariance adaptation.

Choosing gate thresholds. The defaults ρ = 0.25, η = 0.30 and ω_max = 150 dps target a geomagnetic field of 250 to 650 mGauss, a 15 to 75 Hz magnetometer and roughly 100 Hz attitude updates. Too loose and interference passes; too tight and availability drops. In practice, tune against measured interference spectra and maneuver rate distributions.

Limits of the calibration model. Equation 1 ignores inter-axis coupling, and the min/max extreme method is sensitive to spikes. A full ellipsoid least-squares fit is more accurate but demands more computation and more samples. This algorithm targets “workable on a production line in the field,” which is a deliberate trade between accuracy and cost.

Where the implementation should improve. Calibration submission should be transactional (compute in locals, validate everything, commit once); an invalid axis should keep its previous offset or be zeroed; confidence should clear on a gated rejection; entry checks should cover the attitude quaternion and angular rate for finiteness; a reference recapture or a wider window should follow calibration; and fusion should either be paused by the caller during calibration or check calib.active inside update().

Conclusion

This article formalized and analyzed an MCU magnetometer heading fusion algorithm and walked its code implementation: ellipsoid approximation for hard- and soft-iron calibration, a heading observation built from quaternion rotation and horizontal projection, a clamped PI loop that corrects only the heading component, and robustness from four-stage gating, a first-order low-pass and online learning of the reference field strength. In implementation, the module organizes state in a static instance and executes entry checks, correction, gating, low-pass, rotation, observation, online learning and PI in that order, rejecting invalid samples before the expensive math and completing observation and correction for valid samples at O(1) cost.

The analysis shows that the algorithm gives gyro-integrated heading a long-term absolute reference under O(1) time and static memory, with correction and integral clamps keeping the control loop safe and unwrapping plus Δt clamping suppressing zero-crossing and sampling-jitter effects. The main improvements are transactional calibration submission, invalid-axis offset handling, zeroing confidence on rejection, finiteness checks on the attitude quaternion and angular rate, and reference recapture after calibration. None of these adds asymptotic complexity, and together they would materially improve state consistency and robustness.

Have questions about this article? Feel free to contact us at [email protected] — we’re happy to help!

Frequently Asked Questions

Why fuse a magnetometer with a gyro at all?

Gyro-integrated heading drifts over time because of bias, while a three-axis magnetometer provides an absolute heading reference but is corrupted by hard-iron offset, soft-iron distortion and external interference. Fusing them keeps short-term gyro smoothness and long-term magnetic accuracy.

Why use min/max spans instead of a least-squares ellipsoid fit?

Because it runs in constant memory and constant time on an MCU with no matrix math, which matters when calibration must be repeatable in the field. The trade-off is sensitivity to spikes and no inter-axis coupling estimate.

What do the four gates actually reject?

G1 rejects out-of-range field magnitude from sensor saturation or a broken lead; G2 rejects samples inconsistent with the reference strength, meaning external interference; G3 rejects maneuvers where the magnetometer lags; G4 rejects near-vertical field or unreliable roll and pitch.

Why correct only the heading component of the quaternion?

Because roll and pitch are already observed by gravity, so rotating about the navigation Z axis injects the magnetic correction without disturbing the tilt estimate. Equation 14 constructs that rotation as a pure yaw quaternion.

What breaks if calibration writes a bad offset?

The corrected field magnitude stops matching the reference R, so G2 rejects every subsequent sample and heading silently falls back to pure gyro. On an invalid axis the offset can also drive that axis toward zero, which is why the offset should be retained or zeroed.

About Aomway

Aomway is a drone technology company that designs and manufactures FPV video transmitters, antennas, cameras and flight electronics, and publishes engineering deep dives on autonomous navigation and RF systems. Explore the full hardware lineup at aomway.com to see the components behind these builds.