99б. IMU orientation: the quaternion, the Madgwick filter and magnetometer calibration

In section 99 you built tilt with a complementary filter: one angle, gyroscope plus accelerometer. There was no heading there — gravity does not change with a turn about the vertical, so you need a magnetometer. Here you get full orientation, all three angles at once: an attitude and heading reference system, AHRS. It is stored in a quaternion, the gyroscope rotates it, and gravity and the magnetic field pull it little by little towards the truth.

Step by step: the quaternion and gyroscope integration; why not Euler angles (the gimbal lock experiment); the Madgwick filter — a gyroscope step plus a step against the error gradient; gyroscope bias found during pauses, and rejection of the accelerometer during jolts; magnetometer calibration — hard iron shifts the centre, soft iron turns the sphere into an ellipsoid, and least squares turns it back into a sphere. All the code uses float; from the math library the filter needs only sqrtf and fabsf, and the Euler angles for display also need atan2f, asinf, sinf and cosf.

Everything was tested on a model where the true motion is known: two minutes of rotation, shaking, a drifting gyroscope bias, a distorted field. The same C code was built for Cortex-M3 and Cortex-M4F, and the instructions per step were counted in QEMU. numpy and scipy recomputed the truth, the filter and the calibration independently.

References: the idea comes from the Fusion library (github.com/xioTechnologies/Fusion, MIT license): a revised Madgwick filter with bias estimation during pauses and acceleration rejection, — and NXP application note AN4246 on magnetometer calibration. Neither the Fusion code nor the text of AN4246 is used in this section, and the section was not checked against them: the formulas were derived anew and verified numerically. For comparison — src/main/flight/imu.c from Betaflight 2026.12.0-alpha (commit 744f95f, license GPL-3.0), the same one as in section 99а; from it there is only a retelling. The section's code is our own: quat99b.c, ahrs99b.c, magcal99b.c; the model is sim99b.c, the cross-check is ref99b.py, the measurement on Cortex-M is bench99b.c.

Terms used in the documentation

Term	Meaning
AHRS (attitude and heading reference system)	a system that estimates orientation and heading
quaternion	four numbers that store a rotation
Euler angles: roll, pitch, yaw	three angles: roll, pitch, heading (yaw)
gimbal lock	two Euler angles merge into one
gradient descent, step size β	gradient descent, correction step β
gyroscope bias, rest detection	gyroscope zero offset, detecting that the device is still
acceleration rejection	ignoring the accelerometer during accelerations
hard iron, soft iron	a constant offset of the field, a distortion of the field by nearby steel
ellipsoid fit, least squares	fitting an ellipsoid, the least-squares method
magnetic inclination	the angle of the field below the horizontal
FPU, soft-float	hardware floating-point unit, floating point done in software

99б.1 What we build and how we test it

Orientation is the rotation from the sensor axes to the Earth axes. The Earth axes here are: x — towards magnetic north, y — west, z — up. A quaternion is four numbers (w, x, y, z) of length 1; a rotation by angle φ about a unit axis n is written as (cos φ/2, n·sin φ/2). A 3×3 matrix is nine numbers that must be kept mutually perpendicular; a quaternion is four, and only the length has to be kept. Why not three angles — see 99б.2.

The filter loop on every sample: the gyroscope rotates the quaternion by ω·dt; the accelerometer says where up is; the magnetometer says where north is; the difference between what the sensors see and what they should see with the current quaternion gives a small correction.

The model: 12000 samples at 100 Hz, 120 s. The device lies still for 10 s, then come three rotation segments (10–40, 45–70 and 75–100 s) with sine waves up to 60, 10 and 45 °/s on the axes, with 5 s pauses between them, and 20 s of rest at the end. From 85 to 95 s the device is also shaken along north: up to 0.5 g at 2 Hz. The largest pitch is 82.1°. The truth is integrated in double in 20 small steps per sample; scipy recomputed it its own way, and the difference is 3.30·10⁻¹² degrees.

Sensors. Gyroscope: noise 0.05 °/s and a bias that drifts, as during warm-up, from 0.50; −0.30; 0.40 to 0.98; 0.06; −0.20 °/s. Accelerometer: noise 0.005 g. Magnetometer: a 50 µT field with 70° inclination, noise 0.3 µT, hard iron V = (25; −12; 40) µT and soft iron — a symmetric matrix A with diagonal 1.15; 0.92; 1.03. From the residuals "reading minus what the truth predicts" numpy found noise of 0.0501 °/s, 0.00500 g and 0.2983 µT — the sensors are as specified.

Errors are counted after the first 10 s: tilt is the angle between the true and the estimated "up" direction, heading is the difference of the heading angles. RMS is the root mean square, "max" is the worst sample.

99б.2 The quaternion in C and the gimbal lock of Euler angles

The gyroscope gives the rate in the sensor axes, and the quaternion changes as dq/dt = ½·q ⊗ (0, ω). The step uses the first derivative, followed by normalization: rounding errors accumulate, and the length drifts away from one.

/* quat99b.h — раздел 99б: кватернион ориентации (float, без библиотек, кроме sqrtf/atan2f/asinf).
 * q = (w, x, y, z) поворачивает вектор из осей датчика в оси Земли: v_земля = q ⊗ v ⊗ q*.
 * Оси Земли: x — на магнитный север, y — на запад, z — вверх. */
#ifndef QUAT99B_H
#define QUAT99B_H

typedef struct {
    float w, x, y, z;
} quat_t;

quat_t quat_mul(quat_t a, quat_t b);
quat_t quat_norm(quat_t q);
void quat_rotate(quat_t q, const float v[3], float out[3]);
void quat_rotate_inv(quat_t q, const float v[3], float out[3]);
quat_t quat_gyro_step(quat_t q, const float g[3], float dt);
void quat_to_euler(quat_t q, float *roll, float *pitch, float *yaw);
quat_t quat_from_euler(float roll, float pitch, float yaw);

#endif

/* Произведение a ⊗ b: сначала поворот b, потом a. */
quat_t quat_mul(quat_t a, quat_t b)
{
    quat_t r;
    r.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
    r.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
    r.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
    r.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
    return r;
}

/* Привести к длине 1: ошибки округления копятся, и без этого кватернион «распухает». */
quat_t quat_norm(quat_t q)
{
    float n = 1.0f / sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
    q.w *= n;
    q.x *= n;
    q.y *= n;
    q.z *= n;
    return q;
}

/* Шаг по гироскопу: dq/dt = 0,5 · q ⊗ (0, ω), ω в рад/с в осях датчика. */
quat_t quat_gyro_step(quat_t q, const float g[3], float dt)
{
    quat_t w = { 0.0f, g[0], g[1], g[2] };
    quat_t d = quat_mul(q, w);
    float h = 0.5f * dt;
    q.w += d.w * h;
    q.x += d.x * h;
    q.y += d.y * h;
    q.z += d.z * h;
    return quat_norm(q);
}

/* Углы Эйлера в порядке «курс, тангаж, крен» (Z, Y, X), радианы.
 * При тангаже ±90° крен и курс слиты в один поворот — это и есть «замок». */
void quat_to_euler(quat_t q, float *roll, float *pitch, float *yaw)
{
    float s = 2.0f * (q.w * q.y - q.z * q.x);
    if (s > 1.0f)
        s = 1.0f;
    if (s < -1.0f)
        s = -1.0f;
    *roll = atan2f(2.0f * (q.w * q.x + q.y * q.z), 1.0f - 2.0f * (q.x * q.x + q.y * q.y));
    *pitch = asinf(s);
    *yaw = atan2f(2.0f * (q.w * q.z + q.x * q.y), 1.0f - 2.0f * (q.y * q.y + q.z * q.z));
}

The gyroscope alone, starting from the truth, drifted in 120 s by 36.35° in tilt and 17.77° in heading — the bias is integrated, as in 99.6, only now on all three axes.

The Euler angles here are in "heading, pitch, roll" order: first a rotation about z, then y, then x. The sine of pitch is 2(wy − xz); at a pitch of ±90° roll and heading become rotations about the same axis and are not defined separately. Experiment: the device raises its nose at 0.6 rad/s about y, plus 0.01 rad/s about x and z each, 34.4 °/s in total, for 5 s. The true angles (quat_to_euler; scipy via as_euler gave the same numbers):

In half a second the device turned by 17°, while roll and heading turned by 146°. If you integrate the angles themselves, the derivatives contain tan and 1/cos of pitch: the rate of roll or heading reaches 1303 °/s with a rotation of 34.4 °/s, and the error over the experiment is 0.79°. The quaternion went the same way with an error of 0.0438° — that is float rounding. Conclusion: compute and store the quaternion, and use the angles for display only.

99б.3 The Madgwick filter: a step against the gradient

At rest the accelerometer sees the Earth's "up" (0, 0, 1) in the sensor axes. With the current q it should see Rᵀ(q)·(0, 0, 1); the difference from the normalized measurement is the error f. The same goes for the magnetometer: the field in the Earth axes is (bx, 0, bz), with the horizontal part pointing strictly north. The filter takes the gradient ∇ = Jᵀ·f (J is the derivatives of f with respect to w, x, y, z) and adds to the gyroscope step a step against it of length β:

q̇ = ½·q ⊗ (0, ω) − β·∇/|∇|, then q ← norm(q + q̇·dt).

β is in radians per second: how fast the correction can rotate the estimate. The reference field (bx, bz) is taken from the measurement itself: the measured vector is moved into the Earth axes, its horizontal part is folded onto north, and the vertical part stays. So you do not need to know the inclination.

/* Вектор из осей датчика в оси Земли: out = R(q) v. */
void quat_rotate(quat_t q, const float v[3], float out[3])
{
    float xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z;
    float xy = q.x * q.y, xz = q.x * q.z, yz = q.y * q.z;
    float wx = q.w * q.x, wy = q.w * q.y, wz = q.w * q.z;
    out[0] = 2.0f * ((0.5f - yy - zz) * v[0] + (xy - wz) * v[1] + (xz + wy) * v[2]);
    out[1] = 2.0f * ((xy + wz) * v[0] + (0.5f - xx - zz) * v[1] + (yz - wx) * v[2]);
    out[2] = 2.0f * ((xz - wy) * v[0] + (yz + wx) * v[1] + (0.5f - xx - yy) * v[2]);
}

/* Обратно: из осей Земли в оси датчика, out = R(q)^T v. */
void quat_rotate_inv(quat_t q, const float v[3], float out[3])
{
    float xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z;
    float xy = q.x * q.y, xz = q.x * q.z, yz = q.y * q.z;
    float wx = q.w * q.x, wy = q.w * q.y, wz = q.w * q.z;
    out[0] = 2.0f * ((0.5f - yy - zz) * v[0] + (xy + wz) * v[1] + (xz - wy) * v[2]);
    out[1] = 2.0f * ((xy - wz) * v[0] + (0.5f - xx - zz) * v[1] + (yz + wx) * v[2]);
    out[2] = 2.0f * ((xz + wy) * v[0] + (yz - wx) * v[1] + (0.5f - xx - yy) * v[2]);
}

quat_t madgwick_step(quat_t q, const float g[3], const float a[3], const float m[3], float beta, float dt)
{
    quat_t w = { 0.0f, g[0], g[1], g[2] };
    quat_t d = quat_mul(q, w); /* 2 · dq/dt по гироскопу */
    float s0 = 0.0f, s1 = 0.0f, s2 = 0.0f, s3 = 0.0f; /* градиент по w, x, y, z */
    float an = a ? a[0] * a[0] + a[1] * a[1] + a[2] * a[2] : 0.0f;

    if (an > 0.0f) {
        float k = 1.0f / sqrtf(an);
        /* где «верх» должен быть в осях датчика при этом q, минус где его видит акселерометр */
        float f0 = 2.0f * (q.x * q.z - q.w * q.y) - a[0] * k;
        float f1 = 2.0f * (q.w * q.x + q.y * q.z) - a[1] * k;
        float f2 = 1.0f - 2.0f * (q.x * q.x + q.y * q.y) - a[2] * k;
        s0 += -2.0f * q.y * f0 + 2.0f * q.x * f1;
        s1 += 2.0f * q.z * f0 + 2.0f * q.w * f1 - 4.0f * q.x * f2;
        s2 += -2.0f * q.w * f0 + 2.0f * q.z * f1 - 4.0f * q.y * f2;
        s3 += 2.0f * q.x * f0 + 2.0f * q.y * f1;

        float mn = m ? m[0] * m[0] + m[1] * m[1] + m[2] * m[2] : 0.0f;
        if (mn > 0.0f) {
            float mv[3], h[3], r[3], b[3];
            k = 1.0f / sqrtf(mn);
            mv[0] = m[0] * k;
            mv[1] = m[1] * k;
            mv[2] = m[2] * k;
            quat_rotate(q, mv, h); /* измеренное поле в осях Земли */
            b[0] = sqrtf(h[0] * h[0] + h[1] * h[1]); /* горизонталь — строго на север */
            b[1] = 0.0f;
            b[2] = h[2]; /* наклонение берётся из самого измерения */
            quat_rotate_inv(q, b, r); /* где поле должно быть в осях датчика */
            float e0 = r[0] - mv[0], e1 = r[1] - mv[1], e2 = r[2] - mv[2];
            float bx = 2.0f * b[0], bz = 2.0f * b[2];
            s0 += -bz * q.y * e0 + (-bx * q.z + bz * q.x) * e1 + bx * q.y * e2;
            s1 += bz * q.z * e0 + (bx * q.y + bz * q.w) * e1 + (bx * q.z - 2.0f * bz * q.x) * e2;
            s2 += (-2.0f * bx * q.y - bz * q.w) * e0 + (bx * q.x + bz * q.z) * e1 + (bx * q.w - 2.0f * bz * q.y) * e2;
            s3 += (-2.0f * bx * q.z + bz * q.x) * e0 + (-bx * q.w + bz * q.y) * e1 + bx * q.x * e2;
        }
    }

    float sn = s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3;
    float k = sn > 0.0f ? beta / sqrtf(sn) : 0.0f; /* шаг длиной beta против градиента */
    q.w += (0.5f * d.w - k * s0) * dt;
    q.x += (0.5f * d.x - k * s1) * dt;
    q.y += (0.5f * d.y - k * s2) * dt;
    q.z += (0.5f * d.z - k * s3) * dt;
    return quat_norm(q);
}

The derivatives were written out by hand. numpy checked them a different way: the gradient was taken numerically, as a central difference of ½·|f|², and the whole filter ran in float64 on the same samples. Over 12000 steps the estimates differed by no more than 0.002°, and the tilt and heading errors matched to the hundredths.

Why Madgwick and not Mahony. Betaflight (imu.c) computes orientation with the Mahony filter (imuMahonyAHRSupdate): the error is the cross product of the measured and expected "up", and the correction is added to the gyroscope proportionally (by default Kp = 0.25: imu_dcm_kp = 2500 divided by 10000) and integrally (Ki = 0 — disabled). The integral part of Mahony finds the bias by itself. Madgwick has one parameter with a clear meaning and gravity and field in one function; here the bias is estimated separately, during pauses (99б.4), — this is easy to check. The price is in 99б.5: with Madgwick a magnetometer error also pulls the tilt. Betaflight applies the magnetometer correction only about the Earth's vertical.

Choosing β (with the magnetometer, ellipsoid, pauses and rejection enabled). Convergence — the device lies with 20° roll, −10° pitch and 60° heading, the filter starts from zero rotation and without a boost; motion — the model from 99б.1 with a boost of β = 1 for the first 3 s:

A small β is quiet but slow, and without bias estimation it drifts away; a large one converges fast but lets jolts and noise into the estimate. The working point is β = 0.033 with a boost of β = 1 for the first 3 s: from zero rotation to 2° in 0.49 s.

99б.4 Pauses and jolts: gyroscope bias and acceleration rejection

In section 99.6 the bias was measured once at power-up. But it drifts with temperature, and the device often lies still: every pause — 100 samples in a row where |a| differs from 1 g by less than 0.03 g and the raw gyroscope reads less than 2 °/s — gives a fresh estimate. Rejection: if |a| differs from 1 g by more than 0.1 g, the accelerometer sees more than just gravity, and the step uses the gyroscope alone.

/* ahrs99b.h — раздел 99б: фильтр Маджвика (градиентный шаг) с отбрасыванием ускорений
 * и оценкой смещения нуля гироскопа по паузам. Гироскоп — рад/с, акселерометр — в g,
 * магнитометр — в любых единицах (нормируется), после калибровки (magcal99b.h). */
#ifndef AHRS99B_H
#define AHRS99B_H

#include "quat99b.h"

#define REST_SAMPLES 100 /* пауза: столько отсчётов подряд без движения */

typedef struct {
    quat_t q;
    float beta;          /* шаг коррекции, рад/с */
    float acc_tol;       /* допуск |a| около 1 g для коррекции; 0 — не отбрасывать */
    int use_rest;        /* 1 — оценивать смещение нуля гироскопа по паузам */
    float bias[3];       /* оценка смещения, рад/с */
    float sum[3];
    int n_rest;
    unsigned rejected;   /* сколько шагов прошло без коррекции */
    unsigned rest_updates;
} ahrs_t;

quat_t madgwick_step(quat_t q, const float g[3], const float a[3], const float m[3], float beta, float dt);
void ahrs_init(ahrs_t *s, float beta, float acc_tol, int use_rest);
void ahrs_update(ahrs_t *s, const float g[3], const float a[3], const float m[3], float dt);

#endif

#define REST_ACC 0.03f   /* пауза: |a| отличается от 1 g меньше чем на 0,03 g */
#define REST_GYRO 0.035f /* и сырой гироскоп меньше 0,035 рад/с (2 °/с) */

void ahrs_init(ahrs_t *s, float beta, float acc_tol, int use_rest)
{
    quat_t one = { 1.0f, 0.0f, 0.0f, 0.0f };
    s->q = one;
    s->beta = beta;
    s->acc_tol = acc_tol;
    s->use_rest = use_rest;
    for (int i = 0; i < 3; i++)
        s->bias[i] = s->sum[i] = 0.0f;
    s->n_rest = 0;
    s->rejected = 0u;
    s->rest_updates = 0u;
}

void ahrs_update(ahrs_t *s, const float g[3], const float a[3], const float m[3], float dt)
{
    float gc[3];
    float an = sqrtf(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);

    if (s->use_rest) {
        /* пауза: прибор лежит — среднее сырого гироскопа за REST_SAMPLES отсчётов и есть смещение */
        float gn = g[0] * g[0] + g[1] * g[1] + g[2] * g[2];
        if (fabsf(an - 1.0f) < REST_ACC && gn < REST_GYRO * REST_GYRO) {
            for (int i = 0; i < 3; i++)
                s->sum[i] += g[i];
            if (++s->n_rest == REST_SAMPLES) {
                for (int i = 0; i < 3; i++) {
                    s->bias[i] = s->sum[i] / REST_SAMPLES;
                    s->sum[i] = 0.0f;
                }
                s->n_rest = 0;
                s->rest_updates++;
            }
        } else {
            s->n_rest = 0;
            s->sum[0] = s->sum[1] = s->sum[2] = 0.0f;
        }
    }
    for (int i = 0; i < 3; i++)
        gc[i] = g[i] - s->bias[i];

    if (s->acc_tol > 0.0f && fabsf(an - 1.0f) > s->acc_tol) {
        /* ускорение: акселерометр показывает не только тяжесть — шаг только по гироскопу */
        s->rejected++;
        s->q = madgwick_step(s->q, gc, 0, 0, s->beta, dt);
    } else {
        s->q = madgwick_step(s->q, gc, a, m, s->beta, dt);
    }
}

Over the run the bias was updated 40 times; at the end the estimate is 0.974; 0.056; −0.207 °/s against the true 0.98; 0.06; −0.20. During shaking 212 steps went without correction. What each part gives (β = 0.033, magnetometer calibrated with the ellipsoid, 99б.5):

Without the magnetometer the filter started from the truth (it has nowhere to get heading from), and after the first 10 s the heading differed from the truth by no more than 3.21°: the estimation during pauses also removes the bias on the z axis. But without a magnetometer there is nothing to bring the heading back. Betaflight has the same rejection rule: the accelerometer is accepted only if |a| is between 0.9 and 1.1 g (imuIsAccelerometerHealthy).

99б.5 The magnetometer: hard iron, soft iron, ellipsoid to sphere

Without interference, the tip of the field vector lies on a sphere for any rotation. Near magnets and board currents — hard iron — the sphere shifts by V. Steel nearby — soft iron — stretches and tilts it into an ellipsoid: m = A·m_true + V. Calibration finds V and W ≈ A⁻¹ and computes m_cal = W·(m − V). The rotational part of A cannot be seen from the points, nor can the scale — but the filter needs only directions.

The simplest way is the middle of the range on each axis: it removes V but not A. The full way is to fit the surface a·x² + b·y² + c·z² + 2f·yz + 2g·xz + 2h·xy + 2p·x + 2q·y + 2r·z = 1 by least squares (nine unknowns), then from the matrix M = [[a, h, g], [h, b, f], [g, f, c]] find the centre u0 = −M⁻¹·(p, q, r) and W = E·diag(√(λ/k))·Eᵀ, where λ and E are the eigenvalues and eigenvectors of M, and k = 1 + u0ᵀ·M·u0. So that float is enough, the points are first shifted by the range centre and divided by the radius: the numbers are near one. The normal equations are accumulated one point at a time — 360 bytes for the whole calibration.

/* magcal99b.h — раздел 99б: калибровка магнитометра.
 * Модель искажения: m = A · m_истинное + V. V — жёсткое железо (сдвиг центра), A — мягкое
 * железо (сфера становится эллипсоидом). Калибровка находит V и W ≈ A^-1 с точностью до
 * масштаба и поворота: m_кал = W · (m − V) лежит на сфере радиуса 1. */
#ifndef MAGCAL99B_H
#define MAGCAL99B_H

typedef struct {
    float c[3];      /* предварительный центр (по минимуму и максимуму) */
    float scale;     /* 1 / предварительный радиус: числа около единицы, float хватает */
    float n[9][10];  /* нормальные уравнения D^T D | D^T 1, копятся по одному отсчёту */
    unsigned count;
} magfit_t;

void mag_minmax(const float (*m)[3], unsigned n, float v[3], float *radius);
void magfit_init(magfit_t *f, const float c[3], float radius);
void magfit_add(magfit_t *f, const float m[3]);
int magfit_solve(magfit_t *f, float v[3], float w[3][3], float *pivot_ratio);
void sym3_eig(float a[3][3], float vec[3][3]);
void magcal_apply(const float v[3], const float w[3][3], const float m[3], float out[3]);

#endif

void mag_minmax(const float (*m)[3], unsigned n, float v[3], float *radius)
{
    float lo[3], hi[3], r = 0.0f;
    for (int k = 0; k < 3; k++)
        lo[k] = hi[k] = m[0][k];
    for (unsigned i = 1; i < n; i++)
        for (int k = 0; k < 3; k++) {
            if (m[i][k] < lo[k])
                lo[k] = m[i][k];
            if (m[i][k] > hi[k])
                hi[k] = m[i][k];
        }
    for (int k = 0; k < 3; k++) {
        v[k] = 0.5f * (lo[k] + hi[k]); /* жёсткое железо: середина размаха по каждой оси */
        r += 0.5f * (hi[k] - lo[k]);
    }
    *radius = r / 3.0f;
}

void magfit_init(magfit_t *f, const float c[3], float radius)
{
    for (int k = 0; k < 3; k++)
        f->c[k] = c[k];
    f->scale = 1.0f / radius;
    for (int i = 0; i < 9; i++)
        for (int j = 0; j < 10; j++)
            f->n[i][j] = 0.0f;
    f->count = 0u;
}

/* Один отсчёт: строка d плана и прибавка d·d^T и d к нормальным уравнениям. Память — 360 байт
 * на всю калибровку, сколько бы отсчётов ни пришло. */
void magfit_add(magfit_t *f, const float m[3])
{
    float x = (m[0] - f->c[0]) * f->scale;
    float y = (m[1] - f->c[1]) * f->scale;
    float z = (m[2] - f->c[2]) * f->scale;
    float d[9] = { x * x, y * y, z * z, 2.0f * y * z, 2.0f * x * z, 2.0f * x * y, 2.0f * x, 2.0f * y, 2.0f * z };
    for (int i = 0; i < 9; i++) {
        for (int j = i; j < 9; j++)
            f->n[i][j] += d[i] * d[j];
        f->n[i][9] += d[i];
    }
    f->count++;
}

/* Собственные числа и векторы симметричной 3×3 вращениями Якоби: на выходе a — диагональ
 * собственных чисел, столбцы vec — собственные векторы. */
void sym3_eig(float a[3][3], float vec[3][3])
{
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            vec[i][j] = i == j ? 1.0f : 0.0f;
    for (int sweep = 0; sweep < 8; sweep++)
        for (int p = 0; p < 2; p++)
            for (int q = p + 1; q < 3; q++) {
                float apq = a[p][q];
                if (fabsf(apq) <= 1e-9f * (fabsf(a[p][p]) + fabsf(a[q][q]))) {
                    a[p][q] = a[q][p] = 0.0f;
                    continue;
                }
                float th = (a[q][q] - a[p][p]) / (2.0f * apq);
                float t = (th >= 0.0f ? 1.0f : -1.0f) / (fabsf(th) + sqrtf(th * th + 1.0f));
                float c = 1.0f / sqrtf(t * t + 1.0f), s = t * c;
                for (int k = 0; k < 3; k++) {
                    float kp = a[k][p], kq = a[k][q];
                    a[k][p] = c * kp - s * kq;
                    a[k][q] = s * kp + c * kq;
                }
                for (int k = 0; k < 3; k++) {
                    float pk = a[p][k], qk = a[q][k];
                    a[p][k] = c * pk - s * qk;
                    a[q][k] = s * pk + c * qk;
                }
                for (int k = 0; k < 3; k++) {
                    float kp = vec[k][p], kq = vec[k][q];
                    vec[k][p] = c * kp - s * kq;
                    vec[k][q] = s * kp + c * kq;
                }
            }
}

/* Решить нормальные уравнения и превратить эллипсоид в центр v и матрицу w.
 * 0 — готово; -1 — система вырождена (точки не покрывают все стороны); -2 — это не эллипсоид. */
int magfit_solve(magfit_t *f, float v[3], float w[3][3], float *pivot_ratio)
{
    float a[9][10], sol[9], pmin = INFINITY, pmax = 0.0f;
    for (int i = 0; i < 9; i++)
        for (int j = 0; j < 10; j++)
            a[i][j] = j >= i ? f->n[i][j] : f->n[j][i]; /* копилась только верхняя половина */

    for (int col = 0; col < 9; col++) { /* метод Гаусса с выбором главного элемента */
        int best = col;
        for (int i = col + 1; i < 9; i++)
            if (fabsf(a[i][col]) > fabsf(a[best][col]))
                best = i;
        for (int j = 0; j < 10; j++) {
            float t = a[col][j];
            a[col][j] = a[best][j];
            a[best][j] = t;
        }
        float piv = fabsf(a[col][col]);
        if (piv < pmin)
            pmin = piv;
        if (piv > pmax)
            pmax = piv;
        if (piv == 0.0f)
            return -1;
        for (int i = col + 1; i < 9; i++) {
            float k = a[i][col] / a[col][col];
            for (int j = col; j < 10; j++)
                a[i][j] -= k * a[col][j];
        }
    }
    for (int i = 8; i >= 0; i--) {
        float s = a[i][9];
        for (int j = i + 1; j < 9; j++)
            s -= a[i][j] * sol[j];
        sol[i] = s / a[i][i];
    }
    *pivot_ratio = pmin / pmax;
    if (*pivot_ratio < 1e-3f)
        return -1;

    float mq[3][3] = { { sol[0], sol[5], sol[4] }, { sol[5], sol[1], sol[3] }, { sol[4], sol[3], sol[2] } };
    float m0[3][3], e[3][3], u0[3], pr[3], kk;
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            m0[i][j] = mq[i][j];
    sym3_eig(mq, e); /* M = E · diag(l) · E^T */
    for (int i = 0; i < 3; i++)
        if (!(mq[i][i] > 0.0f))
            return -2;
    for (int i = 0; i < 3; i++) /* центр: u0 = −M^-1 · (p, q, r) */
        pr[i] = (e[0][i] * sol[6] + e[1][i] * sol[7] + e[2][i] * sol[8]) / mq[i][i];
    for (int i = 0; i < 3; i++)
        u0[i] = -(e[i][0] * pr[0] + e[i][1] * pr[1] + e[i][2] * pr[2]);
    kk = 1.0f; /* (u−u0)^T M (u−u0) = 1 + u0^T M u0 */
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            kk += u0[i] * m0[i][j] * u0[j];
    if (!(kk > 0.0f))
        return -2;
    for (int i = 0; i < 3; i++) {
        v[i] = f->c[i] + u0[i] / f->scale;
        for (int j = 0; j < 3; j++) { /* w = E · diag(sqrt(l / kk)) · E^T, в единицах датчика */
            float s = 0.0f;
            for (int k = 0; k < 3; k++)
                s += e[i][k] * sqrtf(mq[k][k] / kk) * e[j][k];
            w[i][j] = s * f->scale;
        }
    }
    return 0;
}

void magcal_apply(const float v[3], const float w[3][3], const float m[3], float out[3])
{
    float d[3] = { m[0] - v[0], m[1] - v[1], m[2] - v[2] };
    for (int i = 0; i < 3; i++)
        out[i] = w[i][0] * d[0] + w[i][1] * d[1] + w[i][2] * d[2];
}

Experiment: three sets of 600 points — the device is turned in all directions; tilted by no more than 30°; rotated on a table (tilt with a spread of 1°). The spread of the field length (largest minus smallest, relative to the mean) is on the first set; compass is the heading from the calibrated field against the true one on a level device, 360 positions, no noise:

On the first set W·A = 0.01999·(I + 0.0008): the inverse matrix was found to within 0.08 %. numpy (lstsq and eigh in float64 on the raw points) gave the same centre to within 0.0001 µT and W to within 8.21·10⁻⁷; the rest is noise: the length spread after calibration is 0.566 % RMS, and the noise of 0.3 µT out of 50 is 0.6 %. On the "up to 30°" set, float and double already differ by 0.26 µT in z (38.57 against 38.31). On the table, the ratio of the smallest Gaussian elimination pivot to the largest is 6.15·10⁻⁴ against 6.14·10⁻² on the full set, and magfit_solve refuses (threshold 10⁻³). numpy without the threshold gave a z centre of −4.64 µT instead of 40: on a table z barely changes, and its centre cannot be found from such points.

With the filter (β = 0.033, pauses and rejection enabled):

Two things. An uncalibrated magnetometer spoils not only the heading but also the tilt: 8.60° against 0.37° — the Madgwick gradient rotates the estimate about any axis, not only the vertical. And hard iron alone is not enough: on a level device the compass is off by 18.51°, and in motion the heading drifts by tens of degrees. With 70° inclination the horizontal field is only 17.1 µT out of 50, and a small error in the vector direction becomes a large heading error.

99б.6 On the microcontroller: size, instructions per step, M3 without an FPU and M4F

The code is built with arm-none-eabi-gcc 12.2.1 with -Os for Cortex-M3 (like the F103: no FPU, float is computed by libgcc) and Cortex-M4F (like the F407: -mfpu=fpv4-sp-d16 -mfloat-abi=hard). Code size, bytes:

In ahrs99b.o on the M3 there are 175 calls to the float library; on the M4F there are 90 FPU instructions instead. The firmware "sensors → calibration → filter" (size99b.c, unused code removed by the linker): on the M3 — 5236 bytes of code, of which 1752 are libraries (float addition, multiplication, division and comparison from libgcc, sqrtf from libm), plus 104 bytes of data: the library sqrtf writes errno, and that brings in a newlib housekeeping structure. On the M4F — 2032 bytes, zero library code: the square root is a single FPU instruction.

Instructions per call — QEMU (netduino2 with a Cortex-M3 core and netduinoplus2 with a Cortex-M4F) in -icount mode, SysTick counts, calibrated with a loop of two instructions:

These are instructions, not cycles: there are more cycles on the chip, so the times below are a lower bound. An ahrs_update step on the M3 at 72 MHz takes at least 193 µs: at 100 Hz that is at least 1.9 % of the CPU, at 1 kHz — 19 %. On the M4F at 168 MHz — at least 2.5 µs, 33 times fewer instructions. Calibration on 600 points on the M3 is about 3.33 million instructions, at least 46 ms: it is done once. The fit on the Cortex-M itself, on points of an exact ellipsoid, gave the centre 25.00; −12.00; 40.00 and semi-axes 57; 46; 52 — the same on the M3 and the M4F, and the resulting quaternions matched to the last printed digit.

99б.7 Pitfalls

The heading jumps by 180° although the device turned by a few degrees. Pitch near ±90°: gimbal lock. Roll and heading are not defined separately there (99б.2). Store the quaternion, use the angles for display only; for control use the quaternion itself or the "up" and "north" vectors.

Without the magnetometer the horizon holds, with it the horizon tips over. The magnetometer is not calibrated or there is iron nearby: with Madgwick a field error also pulls the tilt — 8.60° RMS against 0.37°. Calibrate; if the interference cannot be removed, apply the magnetometer correction only about the vertical, as in Betaflight.

Calibrated on a table — on the table the compass is accurate, in the hand it lies. On a table z barely changes, and the z centre cannot be found: numpy without the check gave −4.64 µT instead of 40. magfit_solve refuses in this case (pivot ratio 6.15·10⁻⁴). Turn the device in all directions.

Took only hard iron into account — the heading still drifts. The middle of the range removes the offset but not the ellipsoid: 18.51° on a level device, in motion — tens of degrees. You need the full fit.

The filter takes half a minute to "wake up". β is small: from zero rotation to 2° in 25.41 s with β = 0.033. A boost at start: β = 1 for the first 3 s — 0.49 s. Betaflight does the same: while the motors are not running, Kp is 10 times larger, and after the motors are disarmed, having waited for 250 ms of rest, it is 100 times larger for 0.5 s.

The horizon twitches on jolts. β is large or there is no rejection: with β = 1 the tilt goes up to 14.23°, without rejection during shaking — 1.78° against 1.03°.

With a small β the heading slowly drifts. Gyroscope bias: with β = 0.01 without estimation during pauses the heading is 4.25° RMS against 0.46°. If there are no pauses in operation, you have to take a larger β.

The quaternion "swelled", the angles went off. The length is not 1. Normalize after every step (quat_norm).

On a Cortex-M3 the filter eats a noticeable share of the CPU. Software float: 13900 instructions per step against 420 on the M4F. On the M3 — use a lower update rate or a core with an FPU.

99б.8 Summary

• Store orientation in a quaternion: in Euler angles at a pitch of ±90° roll and heading merge — heading rate up to 1303 °/s with a rotation of 34.4 °/s, roll jumps by 146° in 0.5 s.
• The gyroscope alone drifted by 36.35° in tilt over 120 s: you need a correction from gravity and the field.
• The Madgwick filter: a gyroscope step plus a step of length β against the error gradient; with β = 0.033 and a boost of β = 1 for 3 s — convergence in 0.49 s, tilt 0.37° RMS, heading 0.41° RMS on a model with shaking and a drifting bias. numpy in float64 with a numerical gradient matched to within 0.002°.
• Bias estimated during pauses (100 samples at rest) — 40 updates, the error at the end is less than 0.01 °/s; without it the heading is 0.96° instead of 0.41°.
• Rejection when | |a| − 1 g | > 0.1 g: tilt during shaking 1.03° instead of 1.78°.
• Magnetometer calibration with an ellipsoid: centre 24.98; −11.97; 39.99 µT against the true 25; −12; 40, W·A = c·(I + 0.0008), compass on a level device 0.12° against 18.51° with hard iron alone and 179.17° without calibration. Points taken on a table are no good.
• Without calibration the magnetometer spoils the tilt as well: 8.60° RMS.
• Size with -Os: 3980 bytes of code in total on the M4F, 6064 on the M3; a filter step is 420 instructions on the M4F against 13900 on the M3.

Full source code

quat99b.c — quaternion operations and gyroscope integration.

/* quat99b.c — раздел 99б: действия с кватернионом и интегрирование гироскопа. */
#include <math.h>
#include "quat99b.h"

/* Произведение a ⊗ b: сначала поворот b, потом a. */
quat_t quat_mul(quat_t a, quat_t b)
{
    quat_t r;
    r.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
    r.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
    r.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
    r.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
    return r;
}

/* Привести к длине 1: ошибки округления копятся, и без этого кватернион «распухает». */
quat_t quat_norm(quat_t q)
{
    float n = 1.0f / sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
    q.w *= n;
    q.x *= n;
    q.y *= n;
    q.z *= n;
    return q;
}

/* Вектор из осей датчика в оси Земли: out = R(q) v. */
void quat_rotate(quat_t q, const float v[3], float out[3])
{
    float xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z;
    float xy = q.x * q.y, xz = q.x * q.z, yz = q.y * q.z;
    float wx = q.w * q.x, wy = q.w * q.y, wz = q.w * q.z;
    out[0] = 2.0f * ((0.5f - yy - zz) * v[0] + (xy - wz) * v[1] + (xz + wy) * v[2]);
    out[1] = 2.0f * ((xy + wz) * v[0] + (0.5f - xx - zz) * v[1] + (yz - wx) * v[2]);
    out[2] = 2.0f * ((xz - wy) * v[0] + (yz + wx) * v[1] + (0.5f - xx - yy) * v[2]);
}

/* Обратно: из осей Земли в оси датчика, out = R(q)^T v. */
void quat_rotate_inv(quat_t q, const float v[3], float out[3])
{
    float xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z;
    float xy = q.x * q.y, xz = q.x * q.z, yz = q.y * q.z;
    float wx = q.w * q.x, wy = q.w * q.y, wz = q.w * q.z;
    out[0] = 2.0f * ((0.5f - yy - zz) * v[0] + (xy + wz) * v[1] + (xz - wy) * v[2]);
    out[1] = 2.0f * ((xy - wz) * v[0] + (0.5f - xx - zz) * v[1] + (yz + wx) * v[2]);
    out[2] = 2.0f * ((xz + wy) * v[0] + (yz - wx) * v[1] + (0.5f - xx - yy) * v[2]);
}

/* Шаг по гироскопу: dq/dt = 0,5 · q ⊗ (0, ω), ω в рад/с в осях датчика. */
quat_t quat_gyro_step(quat_t q, const float g[3], float dt)
{
    quat_t w = { 0.0f, g[0], g[1], g[2] };
    quat_t d = quat_mul(q, w);
    float h = 0.5f * dt;
    q.w += d.w * h;
    q.x += d.x * h;
    q.y += d.y * h;
    q.z += d.z * h;
    return quat_norm(q);
}

/* Углы Эйлера в порядке «курс, тангаж, крен» (Z, Y, X), радианы.
 * При тангаже ±90° крен и курс слиты в один поворот — это и есть «замок». */
void quat_to_euler(quat_t q, float *roll, float *pitch, float *yaw)
{
    float s = 2.0f * (q.w * q.y - q.z * q.x);
    if (s > 1.0f)
        s = 1.0f;
    if (s < -1.0f)
        s = -1.0f;
    *roll = atan2f(2.0f * (q.w * q.x + q.y * q.z), 1.0f - 2.0f * (q.x * q.x + q.y * q.y));
    *pitch = asinf(s);
    *yaw = atan2f(2.0f * (q.w * q.z + q.x * q.y), 1.0f - 2.0f * (q.y * q.y + q.z * q.z));
}

quat_t quat_from_euler(float roll, float pitch, float yaw)
{
    float cr = cosf(0.5f * roll), sr = sinf(0.5f * roll);
    float cp = cosf(0.5f * pitch), sp = sinf(0.5f * pitch);
    float cy = cosf(0.5f * yaw), sy = sinf(0.5f * yaw);
    quat_t q;
    q.w = cr * cp * cy + sr * sp * sy;
    q.x = sr * cp * cy - cr * sp * sy;
    q.y = cr * sp * cy + sr * cp * sy;
    q.z = cr * cp * sy - sr * sp * cy;
    return q;
}

ahrs99b.c — the Madgwick filter.

/* ahrs99b.c — раздел 99б: фильтр Маджвика. Шаг гироскопа плюс шаг против градиента ошибки:
 * ошибка — разница между тем, где тяжесть и магнитное поле должны быть при текущем q, и где их
 * показывают датчики. Длина шага коррекции — beta (рад/с), направление — по градиенту. */
#include <math.h>
#include "ahrs99b.h"

#define REST_ACC 0.03f   /* пауза: |a| отличается от 1 g меньше чем на 0,03 g */
#define REST_GYRO 0.035f /* и сырой гироскоп меньше 0,035 рад/с (2 °/с) */

quat_t madgwick_step(quat_t q, const float g[3], const float a[3], const float m[3], float beta, float dt)
{
    quat_t w = { 0.0f, g[0], g[1], g[2] };
    quat_t d = quat_mul(q, w); /* 2 · dq/dt по гироскопу */
    float s0 = 0.0f, s1 = 0.0f, s2 = 0.0f, s3 = 0.0f; /* градиент по w, x, y, z */
    float an = a ? a[0] * a[0] + a[1] * a[1] + a[2] * a[2] : 0.0f;

    if (an > 0.0f) {
        float k = 1.0f / sqrtf(an);
        /* где «верх» должен быть в осях датчика при этом q, минус где его видит акселерометр */
        float f0 = 2.0f * (q.x * q.z - q.w * q.y) - a[0] * k;
        float f1 = 2.0f * (q.w * q.x + q.y * q.z) - a[1] * k;
        float f2 = 1.0f - 2.0f * (q.x * q.x + q.y * q.y) - a[2] * k;
        s0 += -2.0f * q.y * f0 + 2.0f * q.x * f1;
        s1 += 2.0f * q.z * f0 + 2.0f * q.w * f1 - 4.0f * q.x * f2;
        s2 += -2.0f * q.w * f0 + 2.0f * q.z * f1 - 4.0f * q.y * f2;
        s3 += 2.0f * q.x * f0 + 2.0f * q.y * f1;

        float mn = m ? m[0] * m[0] + m[1] * m[1] + m[2] * m[2] : 0.0f;
        if (mn > 0.0f) {
            float mv[3], h[3], r[3], b[3];
            k = 1.0f / sqrtf(mn);
            mv[0] = m[0] * k;
            mv[1] = m[1] * k;
            mv[2] = m[2] * k;
            quat_rotate(q, mv, h); /* измеренное поле в осях Земли */
            b[0] = sqrtf(h[0] * h[0] + h[1] * h[1]); /* горизонталь — строго на север */
            b[1] = 0.0f;
            b[2] = h[2]; /* наклонение берётся из самого измерения */
            quat_rotate_inv(q, b, r); /* где поле должно быть в осях датчика */
            float e0 = r[0] - mv[0], e1 = r[1] - mv[1], e2 = r[2] - mv[2];
            float bx = 2.0f * b[0], bz = 2.0f * b[2];
            s0 += -bz * q.y * e0 + (-bx * q.z + bz * q.x) * e1 + bx * q.y * e2;
            s1 += bz * q.z * e0 + (bx * q.y + bz * q.w) * e1 + (bx * q.z - 2.0f * bz * q.x) * e2;
            s2 += (-2.0f * bx * q.y - bz * q.w) * e0 + (bx * q.x + bz * q.z) * e1 + (bx * q.w - 2.0f * bz * q.y) * e2;
            s3 += (-2.0f * bx * q.z + bz * q.x) * e0 + (-bx * q.w + bz * q.y) * e1 + bx * q.x * e2;
        }
    }

    float sn = s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3;
    float k = sn > 0.0f ? beta / sqrtf(sn) : 0.0f; /* шаг длиной beta против градиента */
    q.w += (0.5f * d.w - k * s0) * dt;
    q.x += (0.5f * d.x - k * s1) * dt;
    q.y += (0.5f * d.y - k * s2) * dt;
    q.z += (0.5f * d.z - k * s3) * dt;
    return quat_norm(q);
}

void ahrs_init(ahrs_t *s, float beta, float acc_tol, int use_rest)
{
    quat_t one = { 1.0f, 0.0f, 0.0f, 0.0f };
    s->q = one;
    s->beta = beta;
    s->acc_tol = acc_tol;
    s->use_rest = use_rest;
    for (int i = 0; i < 3; i++)
        s->bias[i] = s->sum[i] = 0.0f;
    s->n_rest = 0;
    s->rejected = 0u;
    s->rest_updates = 0u;
}

void ahrs_update(ahrs_t *s, const float g[3], const float a[3], const float m[3], float dt)
{
    float gc[3];
    float an = sqrtf(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);

    if (s->use_rest) {
        /* пауза: прибор лежит — среднее сырого гироскопа за REST_SAMPLES отсчётов и есть смещение */
        float gn = g[0] * g[0] + g[1] * g[1] + g[2] * g[2];
        if (fabsf(an - 1.0f) < REST_ACC && gn < REST_GYRO * REST_GYRO) {
            for (int i = 0; i < 3; i++)
                s->sum[i] += g[i];
            if (++s->n_rest == REST_SAMPLES) {
                for (int i = 0; i < 3; i++) {
                    s->bias[i] = s->sum[i] / REST_SAMPLES;
                    s->sum[i] = 0.0f;
                }
                s->n_rest = 0;
                s->rest_updates++;
            }
        } else {
            s->n_rest = 0;
            s->sum[0] = s->sum[1] = s->sum[2] = 0.0f;
        }
    }
    for (int i = 0; i < 3; i++)
        gc[i] = g[i] - s->bias[i];

    if (s->acc_tol > 0.0f && fabsf(an - 1.0f) > s->acc_tol) {
        /* ускорение: акселерометр показывает не только тяжесть — шаг только по гироскопу */
        s->rejected++;
        s->q = madgwick_step(s->q, gc, 0, 0, s->beta, dt);
    } else {
        s->q = madgwick_step(s->q, gc, a, m, s->beta, dt);
    }
}

magcal99b.c — magnetometer calibration: hard iron and ellipsoid.

/* magcal99b.c — раздел 99б: жёсткое железо по минимуму и максимуму и эллипсоид методом
 * наименьших квадратов. Поверхность второго порядка
 *   a·x² + b·y² + c·z² + 2f·yz + 2g·xz + 2h·xy + 2p·x + 2q·y + 2r·z = 1
 * подгоняется по всем отсчётам (9 неизвестных), потом из неё — центр и матрица в сферу. */
#include <math.h>
#include "magcal99b.h"

void mag_minmax(const float (*m)[3], unsigned n, float v[3], float *radius)
{
    float lo[3], hi[3], r = 0.0f;
    for (int k = 0; k < 3; k++)
        lo[k] = hi[k] = m[0][k];
    for (unsigned i = 1; i < n; i++)
        for (int k = 0; k < 3; k++) {
            if (m[i][k] < lo[k])
                lo[k] = m[i][k];
            if (m[i][k] > hi[k])
                hi[k] = m[i][k];
        }
    for (int k = 0; k < 3; k++) {
        v[k] = 0.5f * (lo[k] + hi[k]); /* жёсткое железо: середина размаха по каждой оси */
        r += 0.5f * (hi[k] - lo[k]);
    }
    *radius = r / 3.0f;
}

void magfit_init(magfit_t *f, const float c[3], float radius)
{
    for (int k = 0; k < 3; k++)
        f->c[k] = c[k];
    f->scale = 1.0f / radius;
    for (int i = 0; i < 9; i++)
        for (int j = 0; j < 10; j++)
            f->n[i][j] = 0.0f;
    f->count = 0u;
}

/* Один отсчёт: строка d плана и прибавка d·d^T и d к нормальным уравнениям. Память — 360 байт
 * на всю калибровку, сколько бы отсчётов ни пришло. */
void magfit_add(magfit_t *f, const float m[3])
{
    float x = (m[0] - f->c[0]) * f->scale;
    float y = (m[1] - f->c[1]) * f->scale;
    float z = (m[2] - f->c[2]) * f->scale;
    float d[9] = { x * x, y * y, z * z, 2.0f * y * z, 2.0f * x * z, 2.0f * x * y, 2.0f * x, 2.0f * y, 2.0f * z };
    for (int i = 0; i < 9; i++) {
        for (int j = i; j < 9; j++)
            f->n[i][j] += d[i] * d[j];
        f->n[i][9] += d[i];
    }
    f->count++;
}

/* Собственные числа и векторы симметричной 3×3 вращениями Якоби: на выходе a — диагональ
 * собственных чисел, столбцы vec — собственные векторы. */
void sym3_eig(float a[3][3], float vec[3][3])
{
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            vec[i][j] = i == j ? 1.0f : 0.0f;
    for (int sweep = 0; sweep < 8; sweep++)
        for (int p = 0; p < 2; p++)
            for (int q = p + 1; q < 3; q++) {
                float apq = a[p][q];
                if (fabsf(apq) <= 1e-9f * (fabsf(a[p][p]) + fabsf(a[q][q]))) {
                    a[p][q] = a[q][p] = 0.0f;
                    continue;
                }
                float th = (a[q][q] - a[p][p]) / (2.0f * apq);
                float t = (th >= 0.0f ? 1.0f : -1.0f) / (fabsf(th) + sqrtf(th * th + 1.0f));
                float c = 1.0f / sqrtf(t * t + 1.0f), s = t * c;
                for (int k = 0; k < 3; k++) {
                    float kp = a[k][p], kq = a[k][q];
                    a[k][p] = c * kp - s * kq;
                    a[k][q] = s * kp + c * kq;
                }
                for (int k = 0; k < 3; k++) {
                    float pk = a[p][k], qk = a[q][k];
                    a[p][k] = c * pk - s * qk;
                    a[q][k] = s * pk + c * qk;
                }
                for (int k = 0; k < 3; k++) {
                    float kp = vec[k][p], kq = vec[k][q];
                    vec[k][p] = c * kp - s * kq;
                    vec[k][q] = s * kp + c * kq;
                }
            }
}

/* Решить нормальные уравнения и превратить эллипсоид в центр v и матрицу w.
 * 0 — готово; -1 — система вырождена (точки не покрывают все стороны); -2 — это не эллипсоид. */
int magfit_solve(magfit_t *f, float v[3], float w[3][3], float *pivot_ratio)
{
    float a[9][10], sol[9], pmin = INFINITY, pmax = 0.0f;
    for (int i = 0; i < 9; i++)
        for (int j = 0; j < 10; j++)
            a[i][j] = j >= i ? f->n[i][j] : f->n[j][i]; /* копилась только верхняя половина */

    for (int col = 0; col < 9; col++) { /* метод Гаусса с выбором главного элемента */
        int best = col;
        for (int i = col + 1; i < 9; i++)
            if (fabsf(a[i][col]) > fabsf(a[best][col]))
                best = i;
        for (int j = 0; j < 10; j++) {
            float t = a[col][j];
            a[col][j] = a[best][j];
            a[best][j] = t;
        }
        float piv = fabsf(a[col][col]);
        if (piv < pmin)
            pmin = piv;
        if (piv > pmax)
            pmax = piv;
        if (piv == 0.0f)
            return -1;
        for (int i = col + 1; i < 9; i++) {
            float k = a[i][col] / a[col][col];
            for (int j = col; j < 10; j++)
                a[i][j] -= k * a[col][j];
        }
    }
    for (int i = 8; i >= 0; i--) {
        float s = a[i][9];
        for (int j = i + 1; j < 9; j++)
            s -= a[i][j] * sol[j];
        sol[i] = s / a[i][i];
    }
    *pivot_ratio = pmin / pmax;
    if (*pivot_ratio < 1e-3f)
        return -1;

    float mq[3][3] = { { sol[0], sol[5], sol[4] }, { sol[5], sol[1], sol[3] }, { sol[4], sol[3], sol[2] } };
    float m0[3][3], e[3][3], u0[3], pr[3], kk;
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            m0[i][j] = mq[i][j];
    sym3_eig(mq, e); /* M = E · diag(l) · E^T */
    for (int i = 0; i < 3; i++)
        if (!(mq[i][i] > 0.0f))
            return -2;
    for (int i = 0; i < 3; i++) /* центр: u0 = −M^-1 · (p, q, r) */
        pr[i] = (e[0][i] * sol[6] + e[1][i] * sol[7] + e[2][i] * sol[8]) / mq[i][i];
    for (int i = 0; i < 3; i++)
        u0[i] = -(e[i][0] * pr[0] + e[i][1] * pr[1] + e[i][2] * pr[2]);
    kk = 1.0f; /* (u−u0)^T M (u−u0) = 1 + u0^T M u0 */
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            kk += u0[i] * m0[i][j] * u0[j];
    if (!(kk > 0.0f))
        return -2;
    for (int i = 0; i < 3; i++) {
        v[i] = f->c[i] + u0[i] / f->scale;
        for (int j = 0; j < 3; j++) { /* w = E · diag(sqrt(l / kk)) · E^T, в единицах датчика */
            float s = 0.0f;
            for (int k = 0; k < 3; k++)
                s += e[i][k] * sqrtf(mq[k][k] / kk) * e[j][k];
            w[i][j] = s * f->scale;
        }
    }
    return 0;
}

void magcal_apply(const float v[3], const float w[3][3], const float m[3], float out[3])
{
    float d[3] = { m[0] - v[0], m[1] - v[1], m[2] - v[2] };
    for (int i = 0; i < 3; i++)
        out[i] = w[i][0] * d[0] + w[i][1] * d[1] + w[i][2] * d[2];
}

Next: section 99в "NEO-6M GPS receiver: NMEA over UART, sentence parsing, checksum, time and coordinates, PPS" — NMEA sentences and their checksum, coordinates and time without float, the distance between points, PPS on a TIM2 capture and UBX commands.