Статья 5: Алгоритмы – реализация и модель ошибок

Предисловие. Получив ряд критических комментариев от читателей постарался учесть замечания и давать больше конкретики в описании реализации. Т.к. потребовалось излагать больше конкретики привожу описание формул в на мета-языке, либо картинками.

Также хочу еще раз сказать, что создание данного устройства - это исключительно моя инициатива, как начинающего пилота. Я не создаю такие устройства на постоянной основе, я не обладаю профильным образованием в авионике, а иду по пути проб и ошибок создавая полезное для себя устройство. Если я захожу на вашу территорию ваших профессиональных или академических интересов, то прошу отнестись с пониманием к некоторым неточностям или ошибкам термионологии. Буду рад вашим комментариям!

Итак, в этой части опишу следующие аспекты:

• Моделирование ошибок датчиков: как с помощью дисперсии Аллана определить параметры шумов и учесть их в алгоритме.

• Проектирование расширенного фильтра Калмана: выбор структуры вектора состояния, учет кватернионов и использование подхода error-state.

• Пример реализации фильтра: пошаговый код для оценки курса (ψ) и дрейфа гироскопа с коррекцией по магнитометру.

• Особенности реализации на STM32F4: эффективность вычислений, использование FPU и памяти, чтобы алгоритм работал в реальном времени.

• Внешние поправки и снижение дрейфа: какие дополнительные датчики и методы используются для коррекции накопленных ошибок.

Моделирование ошибок датчиков

Прежде чем реализовывать фильтр, необходимо количественно описать ошибки датчиков. Инерциальные датчики (акселерометры и гироскопы) имеют несколько типов случайных погрешностей. Для их анализа применяется метод Аллана – дисперсия Аллана (Allan variance), или ее квадратный корень – отклонение Аллана. На логарифмическом графике Allan deviation обычно выделяется до пяти различных участков, соответствующих разным видам шумов.

Основные компоненты:

• Квантование (quantization noise) – проявляется на очень малых интервалах усреднения как участок со склоном -1. В современных АЦП он обычно незначителен.

• Случайное блуждание угла/скорости (Angle Random Walk / Velocity Random Walk) – белый шум, на графике отклонения Аллана имеет наклон - 0.5. Характеризуется параметром, например, в °/√час для гироскопа.

• Нестабильность нуля (Bias instability) – на средних интервалах виден как горизонтальный (склон 0) участок . Это медленное фликкер-шумовое дрейфование нулевого сигнала датчика, обычно указывается в °/ч или м/с² в зависимости от датчика.

• Блуждание смещения (Rate Random Walk) – проявляется на больших временах как рост ошибки со склоном + 0/5. Например, из-за медленного дрейфа смещения гироскопа или ускорения.

В проектируемой системе я провел такой анализ данных покоя датчиков и идентифицировал порядка 5–7 параметров шумовой модели для каждого датчика. В модель ошибок заложены все значимые компоненты, выявленные по графику Аллана. Это значит, что в математической модели датчиков (например, в уравнениях процесса фильтра Калмана) мы учитываем:

• Наличие белого шума измерений (дисперсии которого взяты из коротко-временного участка графика).

• Медленный дрейф смещений: для гироскопов и акселерометров введены переменные смещения (bias) векторного типа, которые моделируются как случайное блуждание (например, как интеграл белого шума низкой интенсивности).

• Возможные тренды или дрейфы более высокого порядка (если выявлены) – при необходимости можно добавить модель ускоренного дрейфа смещения (например, модель с ускорением drift rate).

• Для магнитометра аналогично учитываются шумы и дрейф калибровки.

Зачем нужен столь подробный учет? Дело в том, что каждая из этих шумовых составляющих по-разному влияет на накопление ошибки при интегрировании показаний ИНС. Если проигнорировать, к примеру, нестабильность нуля гироскопа, фильтр может неправильно оценивать доверие к показаниям и неэффективно компенсировать дрейф. Поэтому все параметры, полученные из анализа Аллана, включены в расчет ковариаций и моделирование процесса в фильтре. Это обеспечивает более реалистичную модель поведения датчиков: фильтр “знает” о характере шума и способнее отфильтровать каждую составляющую. Предполагаю, что на практике такой подход позволит существенно сократить дрейф навигации: учитывая шумы и дрейфы в фильтре, я наблюдаю, что ошибка положения растет медленнее, а с подключением коррекций фильтр эффективно вытягивает решение к реальным данным.

Проектирование расширенного фильтра Калмана

Для объединения данных ИНС (гироскопов, акселерометров) и внешних источников (барометра, магнитометра, GPS и др.) использую расширенный фильтр Калмана (Extended Kalman Filter, EKF). Размерность вектора состояния и выбор параметров – ключевой момент. Наш фильтр моделирует состояние полета, которое включает:

• Положение (например, координаты x, y, z или широта, долгота, высота в навигационной системе координат).

• Скорость по трём осям:

  

• Ориентацию аппарата. Ориентация параметризуется при помощи кватерниона q (4 параметра) либо эквивалентно Эйлеровыми углами или Direction Cosine Matrix (DCM). Однако включать напрямую 4 компонента кватерниона в вектор состояния EKF невыгодно: это избыточно и вводит жесткую нелинейность (ограничение |q|=1). Вместо этого применяем подход error-state: вектор состояния содержит ошибку ориентации, обычно в виде трех компонент малых углов δθ (малый угловой вектор ошибки ориентации, рад).

• Смещения (bias) гироскопов и акселерометров. Эти значения моделируются как медленно изменяющиеся (например, случайное блуждание), согласно ранее упомянутым шумовым параметрам.

• Другие возможные параметры: дрейф барометрического высотомера, погрешность магнитного склонения и пр., если требуется коррекция по ним.

Таким образом, размерность полного состояния может достигать 15–20 переменных. Но критически, ориентация хранится не напрямую, а разложенной на номинальную ориентацию (отдельно храним кватернион Qnom и малую ошибку δθ в EKF. При каждом шаге обновления ориентации выполняется операция: Qnom <- δq(δθ) * Qnom, после чего δθ сбрасывается к нулю. Здесь δq(δθ) – малый кватернион, соответствующий повороту на угол δθ. Такой метод – стандартная практика в навигационных фильтрах, он устраняет проблему нормировки кватерниона и уменьшает вычислительную нагрузку.

Модель процесса. Фильтр Калмана нуждается в уравнениях движения для состояния. Мы используем классическую страпдаун-модель (strapdown INS):

• Ориентация обновляется интегрированием угловой скорости. В непрерывном виде

  

  где ω – измеренный вектор угловой скорости (поправленный на смещение). В фильтре же обновляем номинальный кватернион дискретно на каждом шаге интегрирования.

• Скорость в навигационной системе координат обновляется как

  

  где f – вектор ускорений, измеренных акселерометром (с корректировкой на смещение), R(q) – матрица поворота из корпуса в навигационную систему, g – вектор гравитации (с учетом известной модели гравитационного поля на данной высоте).

• Положение – интеграл от скорости

  

  в локальной рамке или в географической – соответственно изменения широты/долготы.

• Смещения гироскопов и акселей моделируются как

  

  где n_g, n_a – белый шум (то есть смещения считаются случайно блуждающими с заданной дисперсией изменения).

Эти уравнения линейризуются вокруг текущей номинальной траектории для применения EKF. Результирующая модель в форме ошибок (отклонений) дает матрицу Ф (Jacobian) и матрицу ковариации процесса Q, в которую как раз и входят параметры шумов, полученные из анализа Аллана. Например, дисперсия Q для блока смещения гироскопа берется исходя из уровня нестабильности нуля гироскопа (который мы оценили, скажем, как X °/ч), конвертируя в рад/с и учитывая шаг дискретизации. Аналогично, в Q для ускорений закладывается дисперсия случайного блуждания акселерометра и т.д. За счет этого, если гироскоп начинает дрейфовать, фильтр ожидает такой сценарий и увеличивает ковариацию ошибки курса, позволяя внешним поправкам вовремя откорректировать накопившееся отклонение.

Коррекция по внешним измерениям. Без внешних ориентиров инерциальная система неизбежно накапливает ошибку. Мы предусматриваем несколько каналов коррекции:

• Барометр: измеряет высоту h (с определенной погрешностью). Его показания сравниваются с высотой из состояния (третья координата позиции). В EKF вводится измерение типа

  

• Магнитометр: дает информацию о курсе (азимуте) относительно магнитного севера. Мы используем его для коррекции угла рыскания ψ. Для преобразования сырых XYZ полей в курс учитываем местное магнитное склонение (можно хранить таблицу по широте/долготе). Измерение на формуле выше.

• GNSS (GPS/ГЛОНАСС): при доступности спутниковых данных включается коррекция по координатам и, в расширенном варианте, по скоростям. Это значительно ограничивает дрейф. Мы используем режим loosely coupled – то есть GPS выдает готовые оценки положения и скорости, сравниваемые с фильтром.

• Приемник воздушных параметров (трубка Пито): дает измерение воздушной скорости. При полете этот датчик поможет ограничивать ошибку прогноза скорости, особенно по горизонту.

• ZUPT (zero velocity update): если известно, что объект какое-то время неподвижен (скорость ноль), например, перед началом движения или в определенные моменты – можно обнулить скорость в фильтре, сбросив дрейф.

• Прочие: теоретически можно задействовать сопоставление с картой или отметки оператора (например, пилот вручную отмечает ориентиры, чтобы периодически калибровать позицию).

Не все эти источники используются одновременно; система работает модульно. Но чем больше доступно – тем дольше фильтр сможет сдерживать рост ошибки. Наша цель – удержать ошибку порядка 1 км за 2–4 часа полета без GNSS. Это очень сложная задача, но комбинация баро, магнитометра, периодических нулевых обновлений и других источников должна помочь приблизиться к этой планке. Если же включается GPS, точность, конечно, становится на уровне десятков метров и меньше.

Пример: фильтр курса и дрейфа гироскопа

Для иллюстрации приведем упрощенный пример Kalman-фильтра для оценки курса объекта ψ (угол рыскания) и одновременной оценки смещения гироскопа b_g вокруг вертикальной оси. Это 2-мерный фильтр, показывающий принцип работы.

Состояние и модель: Пусть состояние

Гироскоп измеряет скорость поворота ω_z вокруг оси Z. Уравнения:

• угол изменяется как интеграл угловой скорости минус смещение.

  

• смещение считаем постоянным в краткосрочной перспективе; Его изменение смоделируем белым шумом.

Дискретизируем с шагом Delta t. Модель процесса в матричной форме:

где w_k – вектор процесса, включающий шумы интегрирования и дрейфа. Ковариация Q задается на основе шума гироскопа (для ψ) и нестабильности смещения (для b_g).

Измерение: допустим, у нас есть магнитометр, по которому мы можем получить оценку курса ψ_mag (например, путем вычисления

где M_x, M_y – горизонтальные компоненты поля. Тогда измерение

а модель измерения h(x) = ψ – мы ожидаем, что магнитометр прямо дает курс (с поправкой на склонение, если нужно). Система измерения:

где v_k – шум измерения (например, несколько градусов дисперсия).

Теперь реализуем этот мини-фильтр. Ниже представлен псевдокод алгоритма (для простоты без матричных обозначений, а в виде скалярных операций):

// Инициализация
float psi = 0.0f;      // априорная оценка курса
float bg = 0.0f;       // оценка смещения гироскопа
float P_psi = 1.0f;    // дисперсия неопределенности курса (начальная, большое значение)
float P_bg = 0.1f;     // дисперсия неопределенности смещения гироскопа
float P_psibg = 0.0f;  // ковар. между ψ и b_g (можно 0)
float var_gyro = 0.001f;  // дисперсия шума (интегрируемого) гироскопа, например (рад^2/с)
float var_bias = 1e-6f;   // дисперсия дрейфа смещения за шаг (очень малое число)
float var_yaw_meas = 0.05f; // дисперсия ошибки измерения курса магнитометра (рад^2)

// Параметры для простоты
float dt = 0.01f; // период дискретизации 10 мс (100 Гц)

// Основной цикл фильтра
while (true) {
    // 1. Предсказание (Prediction)
    float omega_z = readGyroZ();  // измерение гироскопа по Z, рад/с
    // Модель: psi_k+1 = psi_k + (omega_z - bg) * dt
    float psi_pred = psi + (omega_z - bg) * dt;
    float bg_pred = bg;  // смещение считаем постоянным за малый шаг
    
    // Якобианы процесса для линейнизации (здесь все линейно, кроме угла может потреб. нормировка)
    // psi зависит от psi (коэфф 1) и bg (коeff -dt). bg_pred зависит только от bg (коeff 1).
    
    // Обновление ковариации (P – 2x2 матрица)
    // P = F * P * F^T + Q
    // Здесь F = [[1, -dt],[0, 1]], Q = [[var_gyro*dt^2, 0],[0, var_bias*dt]]
    float F11 = 1.0f;
    float F12 = -dt;
    float F21 = 0.0f;
    float F22 = 1.0f;
    // Предсказанная ковариация
    float P11 = F11*P_psi*F11 + F12*P_bg*F21 + var_gyro * dt * dt;
    float P12 = F11*P_psibg*F21 + F12*P_bg*F22;
    float P21 = F21*P_psi*F11 + F22*P_psibg*F21;
    float P22 = F21*P_psibg*F12 + F22*P_bg*F22 + var_bias * dt;
    // Обновляем переменные
    psi = psi_pred;
    bg = bg_pred;
    P_psi = P11;
    P_psibg = P12;  P_bg = P22;
    
    // 2. Коррекция (Update) – по магнитометру (проводим, например, раз в 50 мс, если датчик медленнее)
    if (newMagnetometerDataAvailable()) {
        float psi_meas = readMagnetometerYaw();  // расчёт курса по магн.
        // Вычисляем разницу (инновацию) с учетом цикличности угла:
        float y = normalizeAngle(psi_meas - psi);
        // Ковариация инновации S = H * P * H^T + R. Здесь H = [1, 0] – мы измеряем напрямую ψ.
        float S = P_psi + var_yaw_meas;
        // Коэффициенты Калмана
        float K_psi = P_psi / S;
        float K_bg  = P_psibg / S;
        // Обновляем оценки
        psi += K_psi * y;
        bg  += K_bg * y * (-1); // смещение гироскопа корректируется противоположно ошибке курса
        psi = normalizeAngle(psi);
        // Обновляем ковариации с учетом измерения (P = (I - K*H) * P)
        P_psi = (1 - K_psi) * P_psi;
        P_psibg = (1 - K_psi) * P_psibg;
        P_bg = P_bg - K_bg * P_psibg; // или симметрично обновить полную матрицу
    }
    // ... дальнейшая логика программы
}

В приведенном коде мы видим полный цикл: предсказание на основе гироскопа и обновление по магнитометру. Постоянное использование этих двух шагов позволяет фильтру оценивать курс гораздо точнее, чем сырой гироскоп, и одновременно подстраивать (оценивать) дрейф b_g. Например, если магнитометр указывает, что реальный курс не поворачивается так быстро, как считает гироскоп, фильтр постепенно внесёт поправку в b_g (увеличит его, если гироскоп завышает вращение, или уменьшит, если занижает). Таким образом, через некоторое время система “учится” и компенсирует систематическую ошибку гиродатчика.

Стоит отметить, что в реальном коде вместо ручного выписывания формул для каждой компоненты ковариации чаще используют линейную алгебру: состояние и ковариация представлены матрицами, и обновление выполняется посредством библиотечных функций или авто-сгенерированного кода. Однако на встраиваемых системах (таких как STM32) для маленьких фильтров (до 10–15 переменных) часто пишут вычисления вручную, оптимизируя под конкретные условия, чтобы сэкономить время исполнения.

Особенности реализации на STM32F4

Наша целевая платформа – микроконтроллер STM32F407 (Cortex-M4F, 168 МГц), на котором будет выполняться алгоритм. Этот чип оснащён аппаратным блоком с плавающей запятой (FPU) одинарной точности, что очень помогает в реалтайм обработке сигналов и фильтрации. При разработке ПО мы учли несколько моментов для эффективности и надежности:

• Использование FPU: Компилятор ARM GCC позволяет автоматически задействовать FPU, но важно не забыть включить его в startup-коде. Мы убедились, что все операции с float выполняются аппаратно. В фильтре Калмана это существенно ускоряет умножение матриц, вычисление тригонометрии (нормализация угла, синусы/косинусы для ориентации) и прочее.

• Формат данных: Одинарной точности (32-bit float) оказалось достаточно для наших задач. Точность порядка 1e-6 в единицах СИ (например, в радианах) покрывает необходимые диапазоны. Double (64-bit) не применяется, чтобы не потерять скорость и не задействовать эмуляцию FPU (Cortex-M4F поддерживает только float).

• Организация памяти: У STM32F4 нет кэша данных (D-cache), поэтому доступ к оперативной памяти SRAM и так достаточно быстрый, а инструкции из флеш ускоряются за счёт ART-предвыборки. Однако, мы разместили наиболее часто используемые структуры (например, матрицы состояния и ковариаций) в CCM (Core-Coupled Memory) – это отдельная 64 КБ SRAM, напрямую связанная с ядром и не блокируемая шиной к основной памяти. Доступ к CCM еще более быстрый, что снижает задержки в цикле фильтра. Таким образом, проблема кэш-промахов, актуальная на процессорах с кешем, здесь решается продуманным размещением данных.

• Частота выполнения: Фильтр настроен на частоту 100 Гц (обновление 10 мс). Этот период выбирается как компромисс между частотой датчиков (IMU обычно 100–1000 Гц) и вычислительной нагрузкой. На практике, даже 200 Гц EKF с 15 состояниями на STM32F407 занимал бы небольшую долю CPU (порядка нескольких процентов), так что запас по производительности есть.

• Отладка и устойчивость: Мы добавили мониторинг величин ковариации и инноваций. Если вдруг фильтр “расходится” (например, из-за какой-то аномалии или некорректной модели, ковариация становится не положительно определенной или ошибки резко растут), система это обнаружит и сможет, например, автоматически реинициализировать фильтр. Такой подход полезен для стабильности на длительных периодах.

• CMSIS-DSP: Хотя стандартная библиотека Kalman filter для CMSIS отсутствует, мы используем функции CMSIS-DSP для некоторых операций (например, матричное умножение для вычисления

  

  Это дает небольшой выигрыш за счет оптимизированных assembler-вставок под M4.

В итоге, реализация фильтра на STM32F4 показала, что микроконтроллер способен справиться с довольно сложными вычислениями в реальном времени. Использование float и FPU позволяет обновлять состояние и ковариации без заметных задержек. Например, цикл 9-мерного EKF на 168 МГц выполняется примерно за 0.1–0.2 миллисекунды, что подтверждает – даже 1 кГц обновление в теории возможно, не говоря уже о выбранных 100 Гц.

Заключение

В этой статье я углубился в технические детали алгоритмической части проекта. Были рассмотрены методы учета шумов датчиков через дисперсию Аллана и как эти данные внедряются в модель фильтра Калмана. Описал структуру расширенного фильтра Калмана для инерциальной системы, подчеркнув использование кватернионов и error-state подхода для отслеживания ориентации. Приведенный упрощенный пример кода демонстрирует логику работы фильтра на примере одной оси – подобные принципы масштабируются и на полный 3D случай.

Важно, что при реализации на целевом аппаратном обеспечении (STM32F407) я принял меры для обеспечения быстродействия и стабильности: включили аппаратное ускорение вычислений, оптимально организовали память и следим за сходимостью алгоритма.

Таким образом я постарался раскрыть “сырость” математики, которые стояли за ранее декларируемыми алгоритмами, и показал, как теория превращается в работающий код. Это закладывает основу для следующего этапа – перехода от разработки к испытаниям, где фильтр и система в целом покажут себя в реальных условиях.