После выбора аппаратной базы (двойной STM32, каскад датчиков WT901 + LSM6DSV16X + LIS2DW12) наступает этап, который инженеры любят и ненавидят одновременно: программная реализация навигационного алгоритма. Эта часть посвящена математике, фильтрам и тому, как не сойти с ума, интегрируя шумные измерения в реальные координаты. Текст ориентирован на специалистов, поэтому скучноватые места будут разбавлены самоиронией и примерами из практики.
Калибровка и моделирование датчиков
Отладка начинается с того, чтобы «убедиться, что ваши датчики вообще честные». Те, кто работает с MEMS‑акселерометрами и гироскопами, знают, что их показания — это не абсолютные значения, а специфическая сила: разница между истинным ускорением и ускорением свободного падения. Именно поэтому лежащий на столе WT901 показывает +1 g — датчик чувствует реакцию опоры, удерживающей его от падения. Моя практика подтвердила тезис из учебника: перед интегрированием необходимо вычесть гравитационную составляющую, вычисленную по ориентации.
Каждый модуль имеет свои нелинейности: смещение по нулю (bias), нестабильность чувствительности, перекос осей. Эти параметры зависят от температуры и напряжения. Поэтому я ввел серию статических калибровок: замораживал и нагревали датчики, вращал её в шести положениях, строили таблицы Allan‑девиации и искал шум. Теоретические статьи напоминают, что измерения ускорения и угловой скорости подвержены ошибкам масштаба, несоосности и смещения, а также вариациям во времени и температуре. Эти параметры используются в математической модели ошибочного движения, которая затем загружается в алгоритм фильтра Калмана.
Страпдаун‑интегратор: как считать движение
Моя система — strap‑down INS: датчики жёстко закреплены на корпусе, а ориентация вычисляется интегрированием угловых скоростей. Для описания ориентации я применил кватернионы, поскольку они компактны, не имеют сингулярностей и легко нормализуются. Каждую миллисекунду алгоритм выполняет:
Обновление ориентации: интегрируем гироскопический вектор и нормализуем кватернион;
Проекция ускорений: вектор специфической силы из локальных координат переводится в мировые (ENU) с учётом ориентации;
Вычитание гравитации: из проекции ускорений вычитаем вектор притяжения (9,81 м/с²), получаем истинное ускорение;
Интегрирование скоростей и координат: скорость обновляется интеграцией ускорения, координата — интеграцией скорости.
На бумаге всё просто, но на практике нужно следить за дрейфом. Если смещение гироскопа составляет всего 0,02 °/с, через минуту ошибка ориентации будет около 1 °; неверная ориентация ведёт к ошибке в проекции ускорений и, как следствие, к уходу координаты. Чтобы бороться с дрейфом, я применил каскад датчиков: каждый модуль WT901 сопровождается пятью LSM6DSV16X и тремя LIS2DW12. Избыточность позволяет усреднять данные, отбрасывать выбросы и снижать шум — особенно полезно при высокочастотных вибрациях двигателя.
Фильтр Калмана и коррекция
Как показывает теория, даже идеально откалиброванная ИНС со временем «уплывает» из‑за двойного интегрирования шумов. В авианавигации для уменьшения дрейфа используется интегрированная навигация: инерциальная система комбинируется с внешними источниками. В нашем приборе это могут быть GPS (когда сигнал всё‑таки есть), барометр, магнитометр или другие радиомаяки. Комбинация осуществляется через расширенный фильтр Калмана (EKF). Статья в журнале Sensors (2024г) напоминает, что EKF является ключевым алгоритмом для интеграции ИНС и GNSS: он оценивает положение, скорость и ориентацию, а также ошибки датчиков, используя ковариации шумов и корректируя прогноз при поступлении новых измерений.
Модель состояний нашего EKF включает:
Положение (X, Y, Z) и скорость (Vx, Vy, Vz);
Кватернион ориентации (4 элемента);
Смещения акселерометров и гироскопов (по 3);
Смещения магнитометра.
Процессная матрица учитывает движение по модели «константного ускорения». Матрица измерений зависит от того, какие источники доступны: GPS напрямую измеряет координату и скорость, барометр — высоту, магнитометр — азимут. Ковариации шумов выбираются на основе результатов калибровки: чем больше мы доверяем конкретному источнику, тем меньше дисперсия в соответствующей строке матрицы R. Чтобы фильтр лучше «прилипал» к действительности, мы использовали технику tuning: запускали Монте‑Карло, генерируя десятки траекторий с добавлением реальных шумов, и подбирали параметры так, чтобы среднеквадратичная ошибка минимизировалась. Это напоминает методику из статьи, где Q‑матрица строится на основе ошибок навигации, а R — по данным GNSS.
Реализация на STM32 и результаты
Два микроконтроллера разделены по функциям: один STM32 интегрирует уравнения движения и фильтр Калмана, другой STM32 отвечает за карту, пользовательский интерфейс и обработку сенсорного экрана. Объём вычислений неожиданно велик: обновление 12‑мерного состояния EKF в частоте 200 Гц требует около 50 КФЛОП, поэтому пришлось включить аппаратный FPU и оптимизировать матричные операции. Кроме того, обмен данными между процессорами организован по SPI, а сама структура памяти спроектирована так, чтобы минимизировать кэш‑промахи.
В имитационных испытаниях мы запускали «виртуальный самолёт» по маршруту ~5 км, вводя реальные вибрации и случайные манёвры. Без коррекции дрейф по координате составлял ≈300 м за 8 минут, что подтверждает оценку, что микроскопические ошибки ускорения приводят к значительным отклонениям. После включения EKF и периодической коррекции барометром погрешность сократилась до 50–80 м. Эту траекторию мы выводили на экран (рис. 1) и сравнивали с истинной (рис. 2). Получить такую точность без GNSS — уже хороший результат для лёгких самолётов.
Итоги и планы
В этой части мы прошли путь от понимания, что акселерометр измеряет «лишнюю» силу, до программирования полноценного фильтра Калмана. Мы поняли, что шумы и смещения — это не враги, а реалии, с которыми можно бороться. Главное — моделировать систему в деталях и не лениться открывать справочник по навигации Черного и Кораблина, где объясняются основы механики в неинерциальных системах.
Пока испытания проводились в Matlab и Python, но те же алгоритмы позже были перенесены в прошивку STM32. Для ускорения вычислений использовались аппаратные инструкции DSP и оптимизации на уровне регистров.
Отладка и корректировка
Реальные датчики ведут себя хуже модельных. Поэтому после сборки прототипа значительная часть времени уйдет на калибровку. Необходимо бьудет измерить чувствительность акселерометров по каждой оси, найти смещения при разных температурах, построить модель шума и подобрать параметры фильтра Калмана. Для этого устройство будет помещено на вращающийся стол, измерим отклики, построим графики Allan variance. На основе этих данных мы актуализируем параметры фильтра: матрицы ковариации процессной и измерительной шумности.
P.S. Важным открытием оказалось то, что батарейное питание вносит помехи в показания IMU. Шум преобразователя DC‑DC накладывался на сигналы акселерометра. После внедрения LC‑фильтра на питание и выведения сигнальной земли отдельно проблема ушла.
В следующей части речь пойдёт о прототипировании и квестах с заказом печатной платы, ее особенностей и чудесам работы с зарубежными поставщиками компонентов.
Комментарии (7)
Trifon_Ivanovitch
19.08.2025 16:21Если , STM32F407, with single precision FPU, чем это вам , поможет в фильтре Кальмана., если только писать на ассемблер … Хотя бы приведите пример кода, использования фильтра Кальмана , для управления по рысканью(такие термины используются в управлении летател.,аппаратами …)… а так совершенно. пустые слова, как на партсобрании … Наверное автора, и похожее на него стадо, упоминание , кватерионов и фильтра Кальмана, вводит в трепетную дрожь… Фильтр Кальмана, применяли ещё когда автора и в проекте не было … Для расчета траектории лунного посадочного модуля, в проекте Appolo ., т.е примерно 58 лет назад …. Поэт прав, умом Россию не понять…. Возможно её надо понимать , совсем другим местом, у которого с умом никакой. связи нет …
ProLimit
19.08.2025 16:21А почему кватернион в векторе состояния? Где вы такое увидели? Вроде как уже общепринятая практика держать там не кватернион (4 измерения), а ошибку поворота (3 измерения), которая уходит в кватернион и сбрасывается в 0 после каждого шага фильтра. Плюсов много, например кватернион сильно нелинеен и избыточен, меньше вектор состояния - меньше операций. Еще сталкивался с неприятной особенностью - зависимость шума от ориентации: при наклоне сенсора на 90 градусов ковариация и шум в ориентации вырастали на порядок. У меня был UKF и возможно, это его особенность, не знаю. Но обратите внимание.
Еще не очень понятно как фильтр проработает 2 часа без коррекции. Ковариация будет бесконечно расти для всего вектора состояния, включая ориентацию, так как она завязана на акселерометр и вы не знаете истинных ускорений и не используете какого-то допущения об их природе. Разве что планируется брать эти данные от бортовой системы?И вообще, какими реальными данными данными вы планируете обновлять фильтр на шаге обновления? или его вообще не будет, а только предсказание?
yappari
Четвёртая статья, но всё на своих местах - "алгоритмы" без алгоритмов, собственные термины и проч.
Скажем, несколько раз упоминается дисперсия Аллана. Там что-то около 7 параметров, зачем и каким образом они все учитываются в модели?
О каких промахах, да и вообще кэшах речь, касаемо stm32f407 (если я правильно смог считать название камня с лоурезной картинки из прошлой статьи)?
Ну и вопрос на засыпку. Чем отличаются первые два рисунка и где на них 300м "до" и 50-80 "после"?
exec77 Автор
В статье я упоминаю использование дисперсии Аллана для анализа шума датчиков. Этот метод позволяет выделить несколько компонент шумов инерциальных датчиков – например, квантование, случайное блуждание нуля (angle/velocity random walk), нестабильность смещения (bias instability), дрейфовую составляющую и т.д.. Всего подобных параметров может быть порядка пяти-семи. Почему учитываются все? Потому что каждая из этих шумовых компонент по-разному влияет на погрешность со временем. В высокоточной инерциальной системе все значимые источники ошибок стараются промоделировать: перед интегрированием проведена калибровка (определены bias, масштаб, перекос осей, зависимость от температуры и др), а результаты (включая параметры шумовой модели из Allan deviation) закладываются в алгоритм (например, в ковариации фильтра Калмана). Такой тщательный учет необходим, чтобы снизить дрейф и обеспечить максимальную точность – если пренебречь каким-то фактором, он может вызвать накопление ошибки. Именно поэтому я включил все ~7 параметров в модель ошибок: это повышает реализм модели и позволяет EKF лучше компенсировать каждую составляющую шума. Это подтверждается и на практике – после учёта смещений, шумов и коррекции фильтром, дрейф координаты заметно сокращается
STM32F407IGT6 – это 32-разрядный микроконтроллер семейства STM32F4 (ядро ARM Cortex-M4, 168 МГц, 1 МБ флэш и 192 КБ SRAM). Данный MCU не имеет традиционной кэш-памяти данных. В Cortex-M4 (STM32F4) нет выделенного cache для RAM, а ускорение работы с флэш достигается с помощью так называемого ART-акселератора – предвыборки и буферизации инструкций (инструкционный префетч и branch cache). Проще говоря, STM32F407 делает быстрое исполнение кода из флэш (0 wait state на частоте 168 МГц) благодаря аппаратной предвыборке, но кэша данных (D-cache) у него нет. Возможно поэтмоу моя фраза о «минимизации кэш-промахов» применительно к STM32F4 прозвучала несколько странно. Я имел ввиду оптимальное размещение данных в памяти (например, использование 64КБ CCM – Core Coupled Memory – для самых часто используемых данных) или избегание лишних обращений к внешней памяти, что актуально для более новых микроконтроллеров с кэшем. В любом случае, на STM32F407 главный “ускоритель” – это правильная организация памяти и включение FPU/DSP, а не управление кэш-линиями. Кэш-промахи как таковые для данного MCU не существенны, ведь доступ к SRAM и так быстр, а чтение из флеша оптимизировано аппаратно.
Первые два рисунка в статье как раз иллюстрируют траекторию до коррекции фильтром и после нее. На рис. 1 показано моделирование полета без включения коррекции (только инерциальная интеграция) – красная линия постепенно уходит от истинной траектории (черные точки), и к концу 8-минутного отрезка накопленный дрейф достигает порядка 300 метров. На рис. 2 приведена траектория после включения расширенного фильтра Калмана и периодической коррекции по барометру – оценка (красная) гораздо ближе к реальному пути (черные точки), и окончательное отклонение составляет около 50–80 м. Эти значения указаны в тексте статьи, а на графиках разница видна визуально: во втором рисунке красная траектория почти совпадает с истинной, тогда как в первом заметно “уплывает” от нее. Таким образом, «300 м до» и «50–80 м после» – это величины позиционной ошибки навигации соответственно до применения EKF и после его работы. Они демонстрируют, насколько сильно фильтрация и внешние поправки уменьшают накопленную погрешность. При отсутствии GNSS добиться ошибки ~50 м на дистанции ~5 км – уже очень хороший результат кмк.
yappari
Для чего эта чятГПТшная портянка, которая либо никак не отвечает на вопрос (в какую модель ошибок включены все параметры дисперсии Аллана? Датчиков? Как это помогает фильтру "лучше" компенсировать каждую составляющую?), либо постулирует очевидные вещи (фраза о кэше прозвучала не "странно", она просто неверная. Вся остальная лирика просто излишня), либо идёт вразрез со статьёй (графики на рисунках практически идентичные, только первый чуть шумнее, а второй отмасштабирован почти в четыре раза. При этом ни красная, ни любая другая из тридцати цветов линия, никуда не "уплывает". Все они стабильно находятся внутри чёрной линии. Ну или это такой красный, который на моём мониторе никак не отображается)?
Тут возникает вопрос. С какой целью публикуется подобный материал, который по сути ничего не раскрывает, несмотря на подзаголовки? Кто-то стоит над душой и заставляет делать публикации? Предлагает какие-то преференции? Просто потешить эго?
exec77 Автор
Если вам не понятны мои ответы, то возможно вам стоит точнее задать вопросы. Я пояснил про каждый из пунктов - принять или нет это ваше личное дело. Материал публикуется в рубрике DIY - это рубрика про устройства создаваемые своими руками за свои деньги в свое свободное время.
Возможно мое изложение не соответствует вашим критическим требованиям, но я и не пишу учебник - я рассказываю историю создания устройства. Ошибки допускают те, кто что-то делает - это нормальный процесс. На этом прошу завершить дискуссию, если вам не нравится содержание - не читайте.
yappari
Могу ответить, что если не нравится критика - не публикуйте. Тем более такой материал, который с заголовком "Алгоритмы: как превратить сырые данные в координаты" и во втором же предложении "Эта часть посвящена математике...", и при этом в нём нет ни математики, ни вменяемых алгоритмов. Видны только обрывочные и слабоструктурированные знания, при сильном желании донести это всё до общественности. Поэтому вы правы, дискуссию вести бессмысленно.