99б. Ориентация по IMU: кватернион, фильтр Маджвика и калибровка магнитометра

В разделе 99 наклон собирали комплементарным фильтром: один угол, гироскоп плюс акселерометр. Курса там нет — поворот вокруг вертикали тяжесть не меняет, нужен магнитометр. Здесь — полная ориентация, все три угла сразу: система ориентации и курса, по-английски AHRS. Хранится она в кватернионе, гироскоп его поворачивает, а тяжесть и магнитное поле понемногу подтягивают к правде.

По шагам: кватернион и интегрирование гироскопа; почему не углы Эйлера (опыт с «замком»); фильтр Маджвика — шаг гироскопа плюс шаг против градиента ошибки; смещение нуля гироскопа, найденное на паузах, и отбрасывание акселерометра при толчках; калибровка магнитометра — жёсткое железо сдвигает центр, мягкое делает из сферы эллипсоид, метод наименьших квадратов возвращает сферу. Весь код — на float; из математической библиотеки фильтру нужны только sqrtf и fabsf, а углам Эйлера для показа — ещё atan2f, asinf, sinf и cosf.

Проверено всё на модели, где истинное движение известно: две минуты вращений, тряска, ползущее смещение гироскопа, искажённое поле. Тот же код на C собран под Cortex-M3 и Cortex-M4F, команды на шаг посчитаны в QEMU. Истину, фильтр и калибровку независимо пересчитали numpy и scipy.

Опоры: идея — библиотека Fusion (github.com/xioTechnologies/Fusion, лицензия MIT): пересмотренный фильтр Маджвика с оценкой смещения нуля по паузам и отбрасыванием ускорений, — и заметка NXP AN4246 о калибровке магнитометра. Ни код Fusion, ни текст AN4246 в разделе не использованы и с ними раздел не сверялся: формулы выведены заново и проверены численно. Для сравнения — src/main/flight/imu.c из Betaflight 2026.12.0-alpha (коммит 744f95f, лицензия GPL-3.0), тот же, что в разделе 99а; из него — только пересказ. Код раздела свой: quat99b.c, ahrs99b.c, magcal99b.c; модель — sim99b.c, сверка — ref99b.py, замер на Cortex-M — bench99b.c.

Как это называется в документации

Оригинал	Перевод
AHRS (attitude and heading reference system)	система ориентации и курса
quaternion	кватернион
Euler angles: roll, pitch, yaw	углы Эйлера: крен, тангаж, курс (рыскание)
gimbal lock	«замок»: два угла Эйлера сливаются в один
gradient descent, step size β	градиентный спуск, шаг коррекции β
gyroscope bias, rest detection	смещение нуля гироскопа, обнаружение покоя
acceleration rejection	отбрасывание ускорений
hard iron, soft iron	жёсткое железо, мягкое железо
ellipsoid fit, least squares	подгонка эллипсоида, метод наименьших квадратов
magnetic inclination	магнитное наклонение
FPU, soft-float	блок плавающей точки, программный float

99б.1 Что строим и как проверяем

Ориентация — это поворот из осей датчика в оси Земли. Оси Земли здесь: x — на магнитный север, y — на запад, z — вверх. Кватернион — четыре числа (w, x, y, z) длиной 1; поворот на угол φ вокруг единичной оси n записывается как (cos φ/2, n·sin φ/2). Матрица 3×3 — это девять чисел, которые надо держать взаимно перпендикулярными; кватернион — четыре, и держать надо только длину. Почему не три угла — в 99б.2.

Цикл фильтра на каждом отсчёте: гироскоп поворачивает кватернион на ω·dt; акселерометр говорит, где верх; магнитометр — где север; разница между тем, что датчики видят, и тем, что они должны видеть при текущем кватернионе, даёт небольшую поправку.

Модель: 12 000 отсчётов по 100 Гц, 120 с. Прибор лежит 10 с, потом три отрезка вращения (10–40, 45–70 и 75–100 с) с синусоидами до 60, 10 и 45 °/с по осям, между ними паузы по 5 с, в конце 20 с покоя. На 85–95 с прибор ещё и трясут вдоль севера: до 0,5 g на 2 Гц. Наибольший тангаж — 82,1°. Истина интегрируется в double по 20 мелких шагов на отсчёт; scipy пересчитал её своим способом, расхождение — 3,30·10⁻¹² градуса.

Датчики. Гироскоп: шум 0,05 °/с и смещение, которое ползёт, как при прогреве, с 0,50; −0,30; 0,40 до 0,98; 0,06; −0,20 °/с. Акселерометр: шум 0,005 g. Магнитометр: поле 50 мкТл с наклонением 70°, шум 0,3 мкТл, жёсткое железо V = (25; −12; 40) мкТл и мягкое — симметричная матрица A с диагональю 1,15; 0,92; 1,03. numpy по остаткам «показание минус ожидаемое по истине» нашёл шумы 0,0501 °/с, 0,00500 g и 0,2983 мкТл — датчики такие, как заявлены.

Ошибки считаются после первых 10 с: наклон — угол между истинным и оценённым направлением «вверх», курс — разница углов курса. СКО — средний квадратичный, «макс» — худший отсчёт.

99б.2 Кватернион на C и «замок» углов Эйлера

Гироскоп даёт скорость в осях датчика, и кватернион меняется так: dq/dt = ½·q ⊗ (0, ω). Шаг — по первой производной, после него — нормировка: ошибки округления копятся, и длина уходит от единицы.

/* 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));
}

Один гироскоп, стартуя с истины, за 120 с ушёл на 36,35° по наклону и 17,77° по курсу — смещение нуля интегрируется, как в 99.6, только теперь по всем трём осям.

Углы Эйлера здесь в порядке «курс, тангаж, крен»: сначала поворот вокруг z, потом y, потом x. Синус тангажа — 2(wy − xz); при тангаже ±90° крен и курс становятся поворотами вокруг одной и той же оси, и по отдельности не определены. Опыт: прибор задирает нос со скоростью 0,6 рад/с вокруг y, плюс по 0,01 рад/с вокруг x и z, всего 34,4 °/с, 5 с. Истинные углы (quat_to_euler; scipy через as_euler дал те же числа):

За полсекунды прибор повернулся на 17°, а крен и курс — на 146°. Если интегрировать сами углы, в производных стоят tg и 1/cos тангажа: скорость крена или курса доходит до 1303 °/с при вращении 34,4 °/с, и ошибка за опыт — 0,79°. Кватернион тот же путь прошёл с ошибкой 0,0438° — это округление float. Вывод: считать и хранить кватернион, углы — только для показа.

99б.3 Фильтр Маджвика: шаг против градиента

Акселерометр в покое видит «верх» Земли (0, 0, 1) в осях датчика. При текущем q он должен видеть Rᵀ(q)·(0, 0, 1); разница с нормированным измерением — ошибка f. Так же для магнитометра: поле в осях Земли — (bx, 0, bz), горизонталь строго на север. Фильтр берёт градиент ∇ = Jᵀ·f (J — производные f по w, x, y, z) и к шагу гироскопа добавляет шаг против него длиной β:

q̇ = ½·q ⊗ (0, ω) − β·∇/|∇|, потом q ← норм(q + q̇·dt).

β — в радианах в секунду: с какой скоростью поправка может разворачивать оценку. Опорное поле (bx, bz) берётся из самого измерения: измеренный вектор переводится в оси Земли, горизонталь сворачивается на север, вертикаль остаётся. Поэтому наклонение знать не нужно.

/* Вектор из осей датчика в оси Земли: 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);
}

Производные выписаны руками. numpy проверил их по-другому: градиент взят численно, центральной разностью от ½·|f|², весь фильтр — в float64 на тех же отсчётах. За 12 000 шагов оценки разошлись не больше чем на 0,002°, ошибки наклона и курса совпали до сотых.

Почему Маджвик, а не Махони. Betaflight (imu.c) считает ориентацию фильтром Махони (imuMahonyAHRSupdate): ошибка — векторное произведение измеренного и ожидаемого «верха», поправка добавляется к гироскопу пропорционально (по умолчанию Kp = 0,25: imu_dcm_kp = 2500, делённое на 10 000) и интегрально (Ki = 0 — выключено). Интегральная часть Махони сама находит смещение нуля. У Маджвика один параметр с понятным смыслом и тяжесть с полем в одной функции; смещение здесь оценивается отдельно, на паузах (99б.4), — это легко проверить. Цена — в 99б.5: ошибка магнитометра у Маджвика тянет и наклон. Betaflight подаёт поправку от магнитометра только вокруг вертикали Земли.

Подбор β (с магнитометром, эллипсоид, паузы и отбрасывание включены). Сходимость — прибор лежит с креном 20°, тангажом −10° и курсом 60°, фильтр стартует с нулевого поворота и без разгона; движение — модель из 99б.1 с разгоном β = 1 первые 3 с:

Малый β тихий, но медленный и без оценки смещения уплывает; большой быстро сходится, но пускает в оценку толчки и шум. Рабочая точка — β = 0,033 с разгоном β = 1 на первые 3 с: с нулевого поворота до 2° за 0,49 с.

99б.4 Паузы и толчки: смещение нуля и отбрасывание ускорений

Смещение нуля в разделе 99.6 мерили один раз при включении. Но оно ползёт с температурой, а прибор то и дело лежит: каждая пауза — 100 отсчётов подряд, где |a| отличается от 1 g меньше чем на 0,03 g, а сырой гироскоп меньше 2 °/с, — даёт свежую оценку. Отбрасывание: если |a| отличается от 1 g больше чем на 0,1 g, акселерометр видит не только тяжесть, и шаг делается по одному гироскопу.

/* 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);
    }
}

За прогон смещение обновилось 40 раз, в конце оценка — 0,974; 0,056; −0,207 °/с при истинных 0,98; 0,06; −0,20. На тряске 212 шагов прошли без коррекции. Что даёт каждая часть (β = 0,033, магнитометр откалиброван эллипсоидом, 99б.5):

Без магнитометра фильтр стартовал с истины (курс ему взять неоткуда), и после первых 10 с курс отличался от истины не больше чем на 3,21°: оценка на паузах убирает и смещение по оси z. Но вернуть курс без магнитометра нечем. В Betaflight то же правило отбрасывания: акселерометр принимается, только если |a| от 0,9 до 1,1 g (imuIsAccelerometerHealthy).

99б.5 Магнитометр: жёсткое железо, мягкое железо, эллипсоид в сферу

Без помех конец вектора поля при любых поворотах лежит на сфере. Рядом с магнитами и токами платы — жёсткое железо — сфера сдвигается на V. Сталь рядом — мягкое железо — растягивает и наклоняет её в эллипсоид: m = A·m_ист + V. Калибровка находит V и W ≈ A⁻¹ и делает m_кал = W·(m − V). Поворотная часть A по точкам не видна, масштаб тоже — но фильтру нужны только направления.

Простейшее — середина размаха по каждой оси: убирает V, но не A. Полное — подогнать поверхность a·x² + b·y² + c·z² + 2f·yz + 2g·xz + 2h·xy + 2p·x + 2q·y + 2r·z = 1 методом наименьших квадратов (девять неизвестных), потом из матрицы M = [[a, h, g], [h, b, f], [g, f, c]] найти центр u0 = −M⁻¹·(p, q, r) и W = E·diag(√(λ/k))·Eᵀ, где λ и E — собственные числа и векторы M, а k = 1 + u0ᵀ·M·u0. Чтобы хватило float, точки сначала сдвигаются на центр по размаху и делятся на радиус: числа около единицы. Нормальные уравнения копятся по одной точке — 360 байт на всю калибровку.

/* 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];
}

Опыт: три набора по 600 точек — прибор крутят во все стороны; качают не больше чем на 30°; вращают на столе (наклон — с разбросом 1°). Разброс длины поля (наибольшая минус наименьшая, к средней) — на первом наборе; компас — курс по калиброванному полю против истинного на горизонтальном приборе, 360 положений, без шума:

На первом наборе W·A = 0,01999·(I + 0,0008): обратная матрица найдена с точностью 0,08 %. numpy (lstsq и eigh в float64 по сырым точкам) дал тот же центр с точностью 0,0001 мкТл и W с точностью 8,21·10⁻⁷; остаток — шум: разброс длины после калибровки — 0,566 % по СКО, а шум 0,3 мкТл от 50 — 0,6 %. На наборе «до 30°» float и double расходятся уже на 0,26 мкТл по z (38,57 против 38,31). На столе отношение наименьшего ведущего элемента метода Гаусса к наибольшему — 6,15·10⁻⁴ против 6,14·10⁻² на полном наборе, и magfit_solve отказывается (порог 10⁻³). numpy без порога дал центр по z −4,64 мкТл вместо 40: z на столе почти не меняется, и его центр из таких точек не найти.

С фильтром (β = 0,033, паузы и отбрасывание включены):

Две вещи. Неоткалиброванный магнитометр портит не только курс, но и наклон: 8,60° против 0,37° — градиент Маджвика разворачивает оценку вокруг любой оси, а не только вертикали. И одного жёсткого железа мало: на горизонтали компас ошибается на 18,51°, а в движении курс уходит на десятки градусов. При наклонении 70° горизонталь поля — всего 17,1 мкТл из 50, и небольшая ошибка направления вектора становится большой ошибкой курса.

99б.6 На микроконтроллере: размер, команды на шаг, M3 без FPU и M4F

Код собран arm-none-eabi-gcc 12.2.1 с -Os для Cortex-M3 (как F103: FPU нет, float считает libgcc) и Cortex-M4F (как F407: -mfpu=fpv4-sp-d16 -mfloat-abi=hard). Размер кода, байт:

В ahrs99b.o на M3 — 175 вызовов библиотеки float, на M4F вместо них 90 команд FPU. Прошивка «датчики → калибровка → фильтр» (size99b.c, лишнее выброшено компоновщиком): на M3 — 5236 байт кода, из них 1752 — библиотеки (сложение, умножение, деление и сравнение float из libgcc, sqrtf из libm), плюс 104 байта данных: библиотечный sqrtf пишет errno, а с ним приходит служебная структура newlib. На M4F — 2032 байта, библиотек ноль: корень — одна команда FPU.

Команды на вызов — QEMU (netduino2 с ядром Cortex-M3 и netduinoplus2 с Cortex-M4F) в режиме -icount, отсчёты SysTick, калибровка циклом из двух команд:

Это команды, не такты: тактов на кристалле больше, так что время ниже — нижняя граница. Шаг ahrs_update на M3 при 72 МГц — не меньше 193 мкс: при 100 Гц это не меньше 1,9 % процессора, при 1 кГц — 19 %. На M4F при 168 МГц — не меньше 2,5 мкс, в 33 раза меньше команд. Калибровка по 600 точкам на M3 — около 3,33 млн команд, не меньше 46 мс: её делают один раз. Подгонка на самом Cortex-M по точкам точного эллипсоида дала центр 25,00; −12,00; 40,00 и полуоси 57; 46; 52 — на M3 и M4F одинаково, и итоговые кватернионы совпали до последней напечатанной цифры.

99б.7 Грабли

Курс прыгает на 180°, хотя прибор повернулся на несколько градусов. Тангаж около ±90°: «замок». Крен и курс там не определены по отдельности (99б.2). Хранить кватернион, углы — только для показа; для управления брать сам кватернион или векторы «верх» и «север».

Без магнитометра горизонт держится, а с ним заваливается. Магнитометр не откалиброван или рядом железо: у Маджвика ошибка поля тянет и наклон — 8,60° СКО против 0,37°. Калибровать; если помехи неустранимы — подавать поправку от магнитометра только вокруг вертикали, как в Betaflight.

Откалибровал на столе — на столе компас точный, в руке врёт. На столе z почти не меняется, и центр по z не найти: numpy без проверки дал −4,64 мкТл вместо 40. magfit_solve в этом случае отказывается (отношение ведущих элементов 6,15·10⁻⁴). Крутить прибор во все стороны.

Учёл только жёсткое железо — курс всё равно уходит. Середина размаха убирает сдвиг, но не эллипсоид: на горизонтали 18,51°, в движении — десятки градусов. Нужна полная подгонка.

Фильтр полминуты «просыпается». Малый β: с нулевого поворота до 2° за 25,41 с при β = 0,033. Разгон на старте: β = 1 первые 3 с — 0,49 с. Betaflight делает так же: пока моторы не запущены, Kp в 10 раз больше, а после выключения моторов, дождавшись 250 мс покоя, — в 100 раз на 0,5 с.

При толчках горизонт дёргается. Большой β или нет отбрасывания: при β = 1 наклон до 14,23°, без отбрасывания на тряске — 1,78° против 1,03°.

При малом β курс медленно уплывает. Смещение нуля: при β = 0,01 без оценки на паузах курс 4,25° СКО против 0,46°. Если пауз в работе нет, β приходится брать больше.

Кватернион «распух», углы поехали. Длина не 1. Нормировать после каждого шага (quat_norm).

На Cortex-M3 фильтр съедает заметную часть процессора. Программный float: 13 900 команд на шаг против 420 на M4F. На M3 — меньше частота обновления или ядро с FPU.

99б.8 Итоги

• Ориентацию хранить в кватернионе: в углах Эйлера у тангажа ±90° крен и курс сливаются — скорость курса до 1303 °/с при вращении 34,4 °/с, крен за 0,5 с скачет на 146°.
• Один гироскоп за 120 с ушёл на 36,35° по наклону: нужна поправка по тяжести и полю.
• Фильтр Маджвика: шаг гироскопа плюс шаг длиной β против градиента ошибки; при β = 0,033 и разгоне β = 1 на 3 с — сходимость за 0,49 с, наклон 0,37° СКО, курс 0,41° СКО на модели с тряской и ползущим смещением. numpy в float64 с численным градиентом совпал до 0,002°.
• Смещение нуля на паузах (100 отсчётов покоя) — 40 обновлений, ошибка в конце меньше 0,01 °/с; без него курс 0,96° вместо 0,41°.
• Отбрасывание при | |a| − 1 g | > 0,1 g: наклон на тряске 1,03° вместо 1,78°.
• Калибровка магнитометра эллипсоидом: центр 24,98; −11,97; 39,99 мкТл при истинном 25; −12; 40, W·A = c·(I + 0,0008), компас на горизонтали 0,12° против 18,51° у одного жёсткого железа и 179,17° без калибровки. Точки на столе не годятся.
• Без калибровки с магнитометром портится и наклон: 8,60° СКО.
• Размер -Os: на M4F 3980 байт всего кода, на M3 6064; шаг фильтра — 420 команд на M4F против 13 900 на M3.

Полные исходники

quat99b.c — действия с кватернионом и интегрирование гироскопа.

/* 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 — фильтр Маджвика.

/* 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 — калибровка магнитометра: жёсткое железо и эллипсоид.

/* 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];
}

Дальше: раздел 99в «GPS-приёмник NEO-6M: NMEA по UART, разбор строк, контрольная сумма, время и координаты, PPS» — строки NMEA и их сумма, координаты и время без float, расстояние между точками, PPS на захвате TIM2 и команды UBX.