Беспилотные надводные аппараты (БНА) находят всё более широкое применение в задачах экологического мониторинга водоёмов, поисково-спасательных операциях, картировании дна и других прикладных областях. Ключевым требованием к таким системам является способность автономно определять собственное пространственное положение и ориентацию. Особую сложность эта задача приобретает в ситуациях, когда сигналы глобальных навигационных спутниковых систем (GNSS) недоступны или подвержены сильным помехам. В подобных условиях основным источником данных для оценивания ориентации методом счисления пути становятся инерциальные измерительные блоки (IMU), включающие гироскопы, акселерометры и магнитометры [Капирусов]; [Farrell].
Однако инерциальные датчики обладают рядом существенных недостатков: интегральный дрейф гироскопов, высокий уровень шумов акселерометров, а также восприимчивость магнитометров к внешним магнитным полям. Для компенсации этих погрешностей применяются алгоритмы слияния данных, объединяющие показания различных датчиков и использующие их взаимодополняющие свойства [Alonso]. Наиболее распространёнными подходами являются расширенный фильтр Калмана (EKF), фильтр Махони (PI-наблюдатель) и фильтр Маджвика (градиентный спуск). Каждый из них имеет свои математические и вычислительные особенности, а выбор конкретного алгоритма определяется требуемой точностью, доступными ресурсами и условиями эксплуатации [Ludwig]; [Munarto].
Несмотря на то, что перечисленные алгоритмы хорошо изучены, их сравнительный анализ применительно к беспилотным надводным комплексам остаётся недостаточно освещённым в литературе [Simon]. Большинство исследований выполнено для беспилотных летательных аппаратов, наземных или подводных роботов, либо базируется на синтетических данных. Специфика надводных аппаратов — волновые возмущения, вибрации от двигателей, магнитные аномалии от корпуса и бортового оборудования [Almeida]; [Bryne] — в известных работах практически не учитывается. Кроме того, фильтры часто исследуются на мощных вычислительных платформах, тогда как для малых БНА характерно применение микроконтроллеров с ограниченными ресурсами, таких как ESP32. Этим определяется актуальность настоящего исследования, новизна которого заключается в комплексном экспериментальном сравнении трёх фильтров на реальных данных, полученных с борта БНА, в условиях водной поверхности и нестабильного GPS-сигнала.
Целью данной работы является повышение точности и надёжности навигационных определений БНА в условиях деградации GNSS-сигнала путём экспериментального сравнения и адаптации алгоритмов сенсорной фильтрации для маломощных бортовых вычислителей. Для достижения поставленной цели необходимо решение следующих задач:
- разработка методики и проведение натурных экспериментов для формирования репрезентативной базы данных инерциальных измерений. Решение данной задачи необходимо для верификации алгоритмов в условиях, максимально приближённых к реальной эксплуатации (с учётом волнения и вибраций), а не на синтетических моделях;
- математическое моделирование и адаптация алгоритмов EKF, Махони и Маджвика с учётом стохастических характеристик шумов конкретной связки IMU и вычислительных ограничений микроконтроллера. Это позволяет выявить влияние аппаратных погрешностей и структурных особенностей самих фильтров на итоговую точность навигационных определений, что является ключевым научным аспектом работы;
- количественный анализ точности, стабильности и вычислительной сложности алгоритмов для выработки критериев их оптимального выбора. Решение этой задачи обеспечивает переход от эмпирических данных к научно обоснованным практическим рекомендациям по построению навигационных подсистем БНА.
Последовательное решение поставленных задач обеспечивает достижение главной цели работы. Структура статьи выстроена в соответствии с логикой исследования: в разделе «Материалы и методы» описывается экспериментальная платформа и математические модели исследуемых фильтров; основная часть посвящена количественной оценке и сравнению алгоритмов в статическом и динамическом режимах; в заключении формулируются выводы и практические рекомендации по применению полученных результатов.
Материалы и методы. Алгоритмы фильтрации. В работе проводится сравнительный анализ трёх алгоритмов сенсорной фильтрации, реализованных в программной библиотеке ahrs: фильтр Маджвика (Madgwick), фильтр Махони (Mahony) и расширенный фильтр Калмана (EKF). Выбор этих алгоритмов обусловлен их широкой распространённостью в задачах оценки ориентации и принципиально различными математическими подходами к компенсации погрешностей инерциальных измерительных блоков (IMU).
Фильтр Маджвика [Madgwick] базируется на методе градиентного спуска. Алгоритм минимизирует целевую функцию, представляющую собой рассогласование между измеренными и эталонными векторами гравитации и магнитного поля. Обновление кватерниона ориентации `q` описывается дифференциальным уравнением:
, где `omega_m` — вектор угловой скорости гироскопа; `grad_f` — градиент целевой функции, определяющий направление корректировки; `beta` — коэффициент шага градиентного спуска. Физический смысл параметра `beta` заключается в балансировке между доверием к показаниям гироскопа и акселерометра/магнитометра. В экспериментах принято `beta`=0.05. Данное значение выбрано эмпирически для частоты дискретизации `f_s`=50 Гц (период `T_s`=0.02 с), что обеспечивает полосу пропускания фильтра ниже частоты механических вибраций корпуса, но достаточную для отработки динамических маневров. Использовалась модификация MARG (Magnetic, Angular Rate, and Gravity) с учётом данных магнитометра.
Фильтр Махони [Wang] относится к классу нелинейных комплементарных фильтров с пропорционально-интегральным (ПИ) регулятором на сфере единичных кватернионов. Ошибка ориентации `e` вычисляется как векторное произведение измеренных `v_m` и оценённых `v_e` единичных векторов гравитации и магнитного поля:
`e = v_m xx v_e`
Поправка к угловой скорости гироскопа формируется по закону ПИ-регулирования:
`omega_c = K_p * e + K_i * int(e * d_tau, 0, t)`
где `K_p` — пропорциональный коэффициент, определяющий скорость сходимости; `K_i` — интегральный коэффициент, компенсирующий интегральный дрейф гироскопов. Приняты значения `K_p`=2.0 и `K_i`=0.1. Такой выбор обеспечивает собственную частоту фильтра `omega_n`=0.7 рад/с, что гарантирует быструю отработку креновых и тангажных колебаний на волне без усиления высокочастотных шумов акселерометра.
Расширенный фильтр Калмана (EKF)
`x = [x, y, z, v_x, v_y, v_z, b_x, b_y, b_z]^T`
где компоненты `b` описывают смещение (bias) акселерометра. Ускорения, спроецированные в глобальную систему координат, выступали в роли управляющих воздействий, а координаты GPS — в роли измерений [Zhang].
Экспериментальная платформа. В качестве носителя сенсоров выступил беспилотный катамаран (рисунок 1), разработанный в Центре цифрового мониторинга северных и арктических экосистем Петрозаводского государственного университета. Конструкция включает два параллельных корпуса, соединённых мостом, что обеспечивает высокую остойчивость. Движение обеспечивают два бесколлекторных мотора, установленных на корме, что позволяет выполнять повороты за счёт дифференциальной тяги.
Бортовое электронное оснащение включает:
- Микроконтроллер ESP32 DOIT DEVKIT V1 (двухъядерный Xtensa LX6, 240 МГц, 520 КБ SRAM, 4 МБ Flash, Wi-Fi/Bluetooth);
- Инерциальный модуль MPU6050 (3-осевой гироскоп и акселерометр, интерфейс I²C, частота обновления до 1 кГц);
- Магнитометр QMC5883L (3-осевой, чувствительность до 2 мГс, I²C, частота до 200 Гц);
- GPS-приёмник Radiolink SE100 (протокол NMEA-0183, интерфейс UART) [Макаренко: 22-24].
Рис. 1. 3D-модель катамарана1
Сбор данных производился с частотой 50 Гц и записывался в Excel-файлы для последующей офлайн-обработки. Для синхронизации использовалась временная метка TimeUS (микросекунды).
Методика эксперимента включала калибровочный и основной этапы. Калибровка магнитометра выполнялась методом вращения в трёх плоскостях для компенсации жёстких и мягких искажений магнитного поля. Калибровка акселерометра проводилась в статике для определения нулевых смещений. Основной этап включал два режима:
- Статический режим. Аппарат был жёстко зафиксирован на горизонтальной поверхности. Длительность записи составила `t` =12.6 с (630 отсчётов). Данный объём выборки статистически достаточен для сходимости оценок математического ожидания и дисперсии шумов фильтров при нулевых углах крена (`phi` ), тангажа (`theta` ) и рыскания (`psi` ).
- Динамический режим. Движение по акватории Онежского озера по траектории, включающей прямолинейные участки, циркуляции и движение с переменной скоростью. Записано `N`=5 независимых сеансов длительностью от 254 до 327 с. Частота обновления GPS-координат составляла 5 Гц. Многократное повторение экспериментов с разнообразной динамикой необходимо для верификации устойчивости алгоритмов к внешним возмущениям (волнению, вибрациям).
Во всех экспериментах данные записывались с частотой 50 Гц в Excel-файлы для последующей офлайн-обработки. Для синхронизации использовалась временная метка TimeUS, представляющая собой время в микросекундах от начала записи. Такая схема обеспечивала единую временную шкалу для всех датчиков и позволяла сопоставлять показания гироскопа, акселерометра, магнитометра и GPS с точностью до миллисекунд.
Программная реализация. Обработка осуществлялась в среде Python 3.11 с использованием следующих библиотек:
- pandas — для чтения и предварительной обработки Excel-данных;
- numpy — для матричных операций;
- scipy — для интерполяции на равномерную временную сетку;
- matplotlib — для построения графиков;
- ahrs — реализации фильтров Madgwick, Mahony, EKF2.
Точность ориентации оценивалась по среднему значению `mu` и стандартному отклонению `sigma` углов Эйлера в статике. Точность позиционирования в динамике оценивалась средней ошибкой `delta_r_(mean) ` и минимальной ошибкой `delta_r_min`, вычисляемой как евклидово расстояние между оценкой фильтра и эталонными GPS-координатами в моменты достоверных спутниковых измерений:
`delta_r = sqrt((x_{est} - x_{GPS})^2 + (y_{est} - y_{GPS})^2)`
* * * * *
Оценка ориентации в статическом режиме. На рисунке 2 представлены зависимости углов крена (roll), тангажа (pitch) и рыскания (yaw) от времени для трёх фильтров в стационарных условиях.
Рис. 2. Сравнение оценки углов (крен, тангаж, рыскание) для фильтров Madgwick (синий), Mahony (оранжевый) и EKF (зелёный) в статическом режиме. Составлено автором
Анализ графиков показывает, что для углов крена и тангажа все алгоритмы демонстрируют высокую степень сходимости. Средние значения углов не превышают 0.03 градуса, что находится в пределах инструментальной погрешности MEMS-акселерометра и свидетельствует об отсутствии систематического смещения после калибровки. Стандартное отклонение ` sigma ` для крена и тангажа не превышает 0.18 градуса, что соответствует паспортному уровню собственных шумов сенсоров MPU6050. При этом траектории Madgwick и Mahony практически идентичны, тогда как EKF демонстрирует единичные кратковременные выбросы (например, на отметке t=2 с для тангажа), обусловленные реакцией фильтра на высокочастотные компоненты шума при неоптимальных матрицах ковариаций.
Наибольшие различия наблюдаются в канале рыскания. Теоретически, в статике угол psi должен быть константой. Однако EKF демонстрирует низкочастотный дрейф с размахом, значительно превышающим показатели комплементарных фильтров. Количественные характеристики приведены в таблице 1.
Таблица 1
Средние значения и стандартные отклонения углов (в градусах) для неподвижного БНА. Составлено автором
| Фильтр | Угол | Среднее | Стд. отклонение |
| Roll | -0.002 | 0.129 | |
| Фильтр Маджвика | Pitch | 0.010 | 0.124 |
| Yaw | 0.229 | 0.178 | |
| Roll | 0.018 | 0.107 | |
| Фильтр Махони | Pitch | 0.003 | 0.180 |
| Yaw | 0.234 | 0.178 | |
| Расширенный | Roll | 0.026 | 0.117 |
| Фильтр Калмана | Pitch | 0.031 | 0.174 |
| Yaw | 0.512 | 0.325 |
Данные таблицы 1 подтверждают, что дисперсия рыскания у EKF (`sigma`=0.325 градуса) почти вдвое выше, чем у Madgwick и Mahony (`sigma `=0.178 градуса). Среднее значение рыскания у EKF (0.512 градуса) выходит за пределы доверительного интервала +/- `3*sigma`, характерного для комплементарных фильтров. Деградация точности EKF в канале рыскания объясняется использованием стандартных матриц `Q` и `R` : фильтр переоценивает достоверность прогноза по гироскопу, что приводит к накоплению ошибки интегрирования при отсутствии явной компенсации дрейфа нуля гироскопа в базовой реализации.
Для оценки навигационной точности в динамике была рассчитана ошибка позиционирования `delta_r` по пяти экспериментальным сеансам (таблица 2).
Таблица 2
Сравнение средних и наилучших показателей фильтров
| Фильтр | Ср. ошибка коррекции, м | Лучшая ошибка коррекции, м | Ср. дрейф рыск., ° | Лучший дрейф рыск., ° |
| Маджвик | 45.24 | 29.79 | 6.89 | 4.77 |
| Махони | 40.07 | 29.78 | 25.73 | 9.50 |
| Расширенный фильтр Калмана | 42.65 | 30.16 | 18.70 | 13.46 |
На рисунке 3 приведён график ошибок позиционирования относительно GPS-измерений для одного из экспериментов.
Рис. 3. Сравнение ошибок позиционирования для фильтров Madgwick, Mahony и EKF в динамическом режиме. Составлено автором
В начальной фазе движения наблюдается расхождение кривых ошибок, однако по мере накопления GPS-измерений (при частоте 5 Гц) значения сближаются. Это указывает на то, что при высокой частоте внешних коррекций доминирующим фактором точности становится качество GPS-сигнала, а математическая структура фильтра нивелирует различия. Тем не менее, фильтр Махони обеспечивает наименьшую среднюю ошибку (`delta_r_(mean)`=40.07 м). Это обусловлено наличием интегральной составляющей `K_i` в ПИ-регуляторе, которая эффективно подавляет накопленную ошибку гироскопа на длинных интервалах.
Фильтр Маджвика демонстрирует сопоставимый результат (45.24 м), однако параметр `beta` в условиях волнения делает алгоритм более чувствительным к высокочастотным флуктуациям ускорений. EKF показал среднюю ошибку 42.65 м, но его дрейф рыскания остаётся стабильно высоким (среднее 18.70 градуса), что подтверждает выводы, сделанные по результатам статического теста: без предварительной тонкой настройки ковариационных матриц под конкретный IMU, EKF уступает комплементарным фильтрам в устойчивости канала рыскания.
Наилучшие показатели минимальной ошибки (`delta_r_min`=29.78 м) достигаются всеми алгоритмами на прямолинейных участках с равномерным движением. В условиях же длительных интервалов между GPS-коррекциями (например, при падении частоты до 1 Гц и ниже) инерциальная составляющая выходит на первый план: здесь ПИ-регулятор Махони обеспечивает более гладкую экстраполяцию траектории, а градиентный спуск Маджвика — более быструю адаптацию к резким изменениям курса.
Обобщение полученных количественных результатов и анализ вычислительной сложности алгоритмов позволяют сформулировать практические рекомендации по выбору фильтра для бортовых вычислителей с ограниченными ресурсами, таких как ESP32. Выбор оптимального алгоритма в данном случае представляет собой поиск компромисса между требуемой точностью инерциального счисления, вычислительной нагрузкой на микроконтроллер и сложностью предварительной настройки.
Как было показано в динамических испытаниях, при высокой частоте GPS-коррекций различия в точности позиционирования между алгоритмами нивелируются. Однако в условиях деградации спутникового сигнала на первый план выходят внутренние механизмы компенсации дрейфа. Фильтр Махони демонстрирует наилучшее соотношение точности и быстродействия благодаря эффективной структуре ПИ-регулятора. Фильтр Маджвика предлагает альтернативу с минимальным количеством настраиваемых параметров, что упрощает его внедрение. Расширенный фильтр Калмана, несмотря на математическую строгость и возможность оценки ковариации ошибок, требует значительных вычислительных затрат и обязательной процедуры идентификации шумовых характеристик конкретного IMU, что не всегда целесообразно в проектах с жёсткими ограничениями по времени и ресурсам.
Для систематизации выбора алгоритма в зависимости от приоритетов конкретного инженерного проекта в таблице 3 представлены итоговые рекомендации.
Таблица 3
Рекомендации по выбору фильтра в зависимости от приоритетов проекта
| Приоритет проекта | Рекомендуемый фильтр | Обоснование |
| Максимальная точность позиционирования | Фильтр Махони | Наименьшая средняя ошибка, лучшая стабильность рыскания |
| Минимальные вычислительные затраты | Фильтр Махони | Наибольшее быстродействие среди трёх фильтров |
| Простота настройки и адаптации | Фильтр Маджвика | Единственный параметр β, прост в подстройке под условия |
| Оценка ковариации ошибок | Расширенный фильтр Калмана | Требует идентификации шумовых характеристик IMU, после чего даёт ковариационную оценку |
Таким образом, для большинства типовых задач автономной навигации малых БНА на базе микроконтроллеров ESP32 предпочтительным является использование комплементарных фильтров (Махони или Маджвика). Применение EKF рекомендуется резервировать для случаев, когда архитектура системы требует явной передачи информации о неопределённости измерений в контур управления или контур принятия решений.
* * * * *
В данной статье представлено комплексное экспериментальное сравнение алгоритмов EKF, Махони и Маджвика для оценки ориентации беспилотного надводного аппарата в условиях деградации GPS-сигнала.
Разработанная методика натурных испытаний, включающая статические тесты и динамические сеансы на акватории Онежского озера, позволила сформировать репрезентативную базу данных. Это обеспечило верификацию алгоритмов в условиях реальных гидроаэродинамических возмущений, что отличает данное исследование от работ, базирующихся исключительно на синтетических моделях.
Количественный анализ точности и стабильности показал, что при использовании настроек по умолчанию комплементарные фильтры (Махони и Маджвика) превосходят EKF по стабильности канала рыскания. В статике их стандартное отклонение угла рыскания составило 0,178° против 0,325° у EKF, что обусловлено наличием эффективной коррекции дрейфа гироскопа. В динамическом режиме при частоте GPS-коррекции 5 Гц доминирующим фактором точности позиционирования становится качество спутникового сигнала, нивелируя структурные различия алгоритмов (средняя ошибка составила 40,07–45,24 м). Однако при снижении частоты внешних коррекций структура ПИ-регулятора фильтра Махони обеспечивает наиболее гладкую инерциальную экстраполяцию траектории. Оценка быстродействия подтвердила, что фильтр Махони является наиболее вычислительно эффективным для микроконтроллерных платформ.
На основе полученных количественных метрик и оценки вычислительной сложности в основной части статьи были формализованы критерии и сформулированы практические рекомендации по выбору алгоритма. Установлено, что для микроконтроллерных платформ с ограниченными ресурсами (ESP32) оптимальным является фильтр Махони, обеспечивающий баланс между точностью инерциального счисления и быстродействием. Применение EKF рекомендуется резервировать для случаев, требующих обязательной предварительной стохастической идентификации шумов IMU и явной оценки ковариации ошибок.
Примечания
1 Макаренко М.С. Разработка системы автопилота для стабилизации движения беспилотного надводного комплекса по траектории: дипломная работа. Петрозаводск: Петрозаводский государственный университет, 2025. 43 с.
2 Mahony filter documentation // AHRS Reader documentation. URL: https://ahrs.readthedocs.io/en/latest/filters/mahony.html (дата обращения: 13.05.2026).
Список литературы
Капирусов М.А. Обзор методов автономной навигации // Международный журнал информационных технологий и энергоэффективности. 2024. Т. 9, № 8(46). С. 31-38.
Alonso A.C. Evaluation of magnetometer-less inertial sensors for static spatial orientation estimation / A.C. Alonso, G. García Carro, J.C. Álvarez, D. Álvarez, A.M. López // 2025 IEEE International Instrumentation and Measurement Technology Conference (I2MTC). IEEE, May 2025. P. 1-6. DOI: 10.1109/I2MTC62753.2025.11079213
Almeida T.E.P. de. Low cost heading sensing system for an autonomous aquatic surface vehicle / T.E.P. de Almeida, J.R.B.A. Monteiro, G.T. de Paula, et all. // 2014 11th IEEE/IAS International Conference on Industry Applications. IEEE, 2014. P. 1-6. DOI: 10.1109/INDUSCON.2014.7059447
Bryne T.H. Attitude and heave estimation for ships using MEMS-based inertial measurements / T.H. Bryne, T.I. Fossen, T.A. Johansen, et all. // IFAC-PapersOnLine. 2016. Vol. 49, № 23. P. 568-575. DOI: 10.1016/j.ifacol.2016.10.496
Farrell J. Aided navigation: GPS with high rate sensors. New York: McGraw-Hill, 2008. 456 p.
Ludwig S.A. Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data / S.A. Ludwig, K.D. Burnham // 2018 International Conference on Unmanned Aircraft Systems (ICUAS). IEEE, 2018. P. 1236-1241. DOI: 10.1109/ICUAS.2018.8453465
Madgwick S.O.H. An efficient orientation filter for inertial and inertial/magnetic sensor arrays. Bristol: University of Bristol, 2010. 32 p. URL: https://x-io.co.uk/downloads/madgwick_internal_report.pdf (дата обращения: 13.05.2026).
Munarto R. Implementation of AHRS (attitude heading and reference systems) with Madgwick filter as hexapods robot navigation / R. Munarto, R. Wiryadinata, D.P. Utama // 2022 International Conference on Informatics Electrical and Electronics (ICIEE). IEEE, Oct. 2022. P. 1-9. DOI: 10.1109/ICIEE55596.2022.10010044
Simon H. Comparative analysis of attitude estimation algorithms on embedded systems / H. Simon, Á. Fehér, L. Márton // 2025 IEEE 19th International Symposium on Applied Computational Intelligence and Informatics (SACI). IEEE, 2025. P. 31-36. DOI: 10.1109/SACI66288.2025.11030114
Wang M. UAV attitude measurement based on enhanced Mahony complementary filter / M. Wang, Y. Gao, L. Guan, et all. // 2018 IEEE International Conference on Mechatronics and Automation (ICMA). IEEE, 2018. P. 545-550. DOI: 10.1109/ICMA.2018.8484522
Zhang S. A dual-linear Kalman filter for real-time orientation determination system using low-cost MEMS sensors / S. Zhang, S. Yu, C. Liu // Sensors. 2016. Vol. 16, № 2. P. 264. DOI: 10.3390/s16020264