Принцип работы EKF3 в ArduPilot

Введение

EKF3 (Extended Kalman Filter 3) — это алгоритм оценки состояния по умолчанию в ArduPilot. Он обеспечивает точную навигацию за счёт расширенной математической модели, сенсорной фузии и архитектуры с множественными независимыми «полосами» (lanes).

Математические основы

Что делает EKF3?

EKF3 решает две задачи в каждом цикле:

Шаг	Что происходит	Частота
Предсказание	На основе данных IMU прогнозирует новое положение	400-1000 Гц
Коррекция	Корректирует прогноз данными GPS, барометра, компаса	5-50 Гц

Цикл работы EKF3

Работа фильтра представляет собой непрерывный циклический процесс, который делится на два основных этапа: предсказание и коррекция. Частота выполнения этих этапов различается.

Шаг 1: Предсказание (Prediction Step)

Этот этап выполняется с высокой частотой, синхронно с поступлением данных от IMU (обычно 400–1000 Гц).

Чтение IMU: Фильтр считывает данные с гироскопов и акселерометров.
Прогноз движения: На основе угловых скоростей и ускорений математическая модель рассчитывает, где должен оказаться дрон через следующий промежуток времени.
Обновление ковариации: Увеличивается неопределённость (ошибка) оценки, так как модель не идеальна, а сенсоры имеют шум.
Результат: Получается «априорная» оценка состояния (до учёта внешних датчиков).

Шаг 2: Коррекция (Update Step)

Этот этап выполняется асинхронно, только когда поступают новые данные от внешних сенсоров (GPS, барометр, компас, дальномер). Частота ниже (5–50 Гц).

Проверка данных: Фильтр проверяет, пришли ли новые данные от какого-либо вспомогательного сенсора.
Расчёт инновации: Вычисляется разница между предсказанным значением (из Шага 1) и реальным измерением сенсора. Например, фильтр предсказал высоту 50 м, а барометр прислал 51 м. Инновация = 1 м.
Тест инновации (Innovation Test): Рассчитывается отношение инновации к допустимому порогу (gate).
- Если отношение меньше 1.0: Измерение считается достоверным.
- Если отношение больше 1.0: Измерение считается ошибочным (спуфинг, помеха, сбой) и отклоняется.
Обновление состояния: Если измерение принято, вектор состояний корректируется в сторону измерения, а неопределённость (ковариация) уменьшается.
Обучение смещений: На основе остаточной ошибки фильтр корректирует оценки смещений гироскопов и акселерометров, чтобы уменьшить ошибку предсказания в будущем.

Шаг 3: Мониторинг здоровья (Health Monitoring)

Выполняется постоянно в фоне каждого цикла.

Фильтр отслеживает рост дисперсии ошибок. Если ошибка позиции или скорости растёт beyond допустимых пределов, полосе присваивается флаг неисправности.
Проверяется согласованность данных между разными полосами.

Шаг 4: Вывод данных (Output)

Выполняется синхронно с основным циклом полётного контроллера.

Менеджер EKF3 выбирает данные от самой здоровой полосы.
Эти данные (позиция, скорость, кватернион) передаются в навигационный контроллер и систему стабилизации.
Данные телеметрии отправляются на наземную станцию.

24 состояния EKF3

EKF3 оценивает ключевые параметры полёта:

Группа	Состояния	Назначение
Позиция	3 оси (NED)	Где находится дрон
Скорость	3 оси (NED)	Куда и как быстро летит
Ориентация	Кватернион (4)	Углы наклона и курс
Смещения IMU	6 осей (gyro + accel)	Компенсация дрейфа сенсоров
Магнитное поле	6 осей (earth + body)	Коррекция магнитных помех
Ветер	2 оси (N, E)	Оценка ветра для фиксированного крыла

Ключевое улучшение vs EKF2: EKF3 оценивает смещения акселерометра по всем трём осям, а не только по вертикали. Это критично для вертолётов, VTOL и аппаратов, выполняющих полёты в нестандартных ориентациях.

Архитектура множественных полос

EKF3 запускает 3-4 независимых экземпляра фильтра параллельно:

┌─────────────────────────────────────────────────┐
│  Lane 0 (Primary) → Используется для управления │
├─────────────────────────────────────────────────┤
│  Lane 1 (Secondary) → Мониторинг здоровья       │
├─────────────────────────────────────────────────┤
│  Lane 2 (Tertiary) → Резерв при сбоях           │
└─────────────────────────────────────────────────┘

Преимущества:

При сбое одной полосы система автоматически переключается на здоровую
Каждая полоса может использовать разные комбинации датчиков
Повышенная отказоустойчивость без прерывания полёта

3. Сенсорная фузия в EKF3

Поддерживаемые сенсоры

Сенсор	Частота	Роль в EKF3
IMU	400-1000 Гц	Основная модель процесса
GPS/GNSS	5-10 Гц	Позиция и скорость
Магнитометр	10-50 Гц	Ориентация (рыскание)
Барометр	10-50 Гц	Высота
Дальномер	10-50 Гц	Высота над поверхностью
Оптический поток	10-50 Гц	Скорость (без GPS)
Воздушная скорость	10-50 Гц	Ветер, TAS

Sensor Affinity (Аффинность датчиков)

EKF3 может динамически переключаться между резервными датчиками. Если основной компас создаёт помехи, полоса переключается на резервный без прерывания полёта.

4. Подробный чек-лист настройки EKF3

ЭТАП 1: Базовая конфигурация (перед первым полётом)

Параметр	Описание	Рекомендуемое значение	Как проверить
`EK3_ENABLE`	Включение EKF3	1 (включено)	Mission Planner → Config → Extended Kalman Filter
`EK3_GPS_TYPE`	Тип GPS	0 (авто) или 3 (3D fix)	Проверьте, что GPS имеет 3D фикс перед взлётом
`EK3_MAG_TYPE`	Тип магнитометра	0 (внутренний) или 1 (внешний)	Если есть оба — используйте внешний (меньше помех)
`EK3_ALT_SOURCE`	Источник высоты	0 (барометр) или 1 (дальномер)	Для полётов над землёй — барометр, для посадки — дальномер
`EK3_PRIMARY`	Выбор первичной полосы	0 (автовыбор)	Система сама выберет наиболее здоровую полосу
`EK3_LANES`	Количество полос	3 (баланс) или 4 (макс. надёжность)	Больше полос = больше RAM/CPU нагрузки

Проверка после настройки:

[ ] Подключите Mission Planner / QGroundControl
[ ] Перезагрузите контроллер
[ ] Убедитесь, что статус EKF показывает «Healthy»
[ ] Проверьте, что все полосы инициализированы (LED статус)

ЭТАП 2: Калибровка сенсоров (обязательно)

Параметр	Описание	Процедура
Акселерометр	Калибровка нуля	`CAL_ACC` → Положите дрон на 6 сторон, дождитесь завершения
Гироскоп	Калибровка дрейфа	`CAL_GYRO` → Дрон должен быть неподвижен 10-20 сек
Магнитометр	Калибровка компаса	`CAL_MAG` → Вращайте дрон во всех направлениях 2-3 мин
Барометр	Проверка стабильности	Убедитесь, что высота не «прыгает» более чем на 1-2 м

Дополнительные параметры калибровки:

Параметр	Описание	Значение
`EK3_MAG_CAL`	Автокалибровка магнитометра	3 (рекомендуется)
`EK3_ACC_BCH_LRN`	Обучение смещений акселерометра	1 (включено)
`EK3_GYRO_BCH_LRN`	Обучение смещений гироскопа	1 (включено)

Проверка после калибровки:

[ ] Значения смещений акселерометра < 0.5 м/с²
[ ] Значения смещений гироскопа < 0.01 рад/с
[ ] Магнитометр показывает сферу после калибровки (проверка в MP)
[ ] Барометр стабильный при включённых моторах

ЭТАП 3: Настройка порогов инноваций (для продвинутых)

Пороги инноваций определяют, насколько EKF3 «доверяет» каждому сенсору:

Параметр	Описание	По умолчанию	Когда менять
`EK3_VEL_I_GATE`	Порог скорости	5.0	Увеличить при частых отклонениях GPS
`EK3_POS_I_GATE`	Порог позиции	5.0	Увеличить при плохом GPS сигнале
`EK3_HGT_I_GATE`	Порог высоты	5.0	Уменьшить для более точной высоты
`EK3_MAG_I_GATE`	Порог магнитометра	3.0	Увеличить при магнитных помехах
`EK3_YAW_I_GATE`	Порог рыскания	0.5 рад	Увеличить при нестабильном курсе
`EK3_GLITCH_RAD`	Фильтр скачков GPS	5-10 м	Увеличить в зонах с помехами GPS

Как подобрать значения:

Запишите лог полёта с LOG_BITMASK = +EKF3
Откройте в MAVExplorer → графики EK3.VIE (innovation test ratio)
Если значения часто > 1.0 — увеличьте соответствующий gate
Если значения всегда < 0.3 — можно уменьшить gate для большей чувствительности

Проверка после настройки:

[ ] Innovation test ratios в полёте < 1.0 (в норме)
[ ] Нет частых отклонений измерений (логи EKF3)
[ ] Стабильное удержание позиции в Loiter

📋 ЭТАП 4: Настройка множественных IMU и компасов

Для максимальной отказоустойчивости используйте несколько IMU и компасов:

Параметр	Описание	Рекомендуемое значение
`EK3_IMU_MASK`	Какие IMU использовать	7 (IMU 0, 1, 2 — если доступны)
`EK3_MAG_MASK`	Какие компасы использовать	3 (компас 0 и 1)
`EK3_BCN_MASK`	Beacon-системы	0 (если не используются)
`EK3_RNG_MASK`	Дальномеры	1 (если есть)

Требования к размещению:

Компасы должны быть разнесены минимум на 10-15 см
Избегайте размещения рядом с силовыми проводами и моторами
Используйте внешние компасы на штангах для снижения помех

Проверка после настройки:

[ ] Все IMU показывают схожие данные (проверка в MP → HUD → IMU)
[ ] Компасы показывают схожий курс (разница < 10°)
[ ] При отключении одного сенсора система переключается без сбоев

ЭТАП 5: Оптимизация производительности

EKF3 требует ресурсов контроллера. Настройте под ваше железо:

Контроллер	`EK3_LANES`	`EK3_IMU_MASK`	`LOG_BITMASK`	Примечание
Cube Orange	4	7 (3 IMU)	Полный	Макс. производительность
Pixhawk 4/5	3	3 (2 IMU)	EKF3 + GPS	Баланс
Pixhawk 2.4.8	2	1 (1 IMU)	Минимальный	Ограниченные ресурсы
Pixhawk 1	1	1 (1 IMU)	Только ошибки	Не рекомендуется для EKF3

Параметры оптимизации:

Параметр	Описание	Для слабых контроллеров	Для мощных контроллеров
`EK3_GPS_RATE`	Частота GPS	5 Гц	10 Гц
`EK3_BARO_RATE`	Частота барометра	10 Гц	50 Гц
`EK3_MAG_RATE`	Частота магнитометра	10 Гц	50 Гц
`EK3_FLOW_RATE`	Частота оптического потока	10 Гц	25 Гц

Проверка после оптимизации:

[ ] CPU load < 60% (проверка в MP → Status)
[ ] Нет пропусков циклов EKF (логи EKF3)
[ ] Стабильная частота цикла контроллера

📋 ЭТАП 6: Настройка для полётов без GPS

Для полётов в помещении или при потере GPS:

Параметр	Описание	Значение
`EK3_GPS_TYPE`	Тип GPS	1 (no GPS) для помещений
`EK3_FLOW_USE`	Использование оптического потока	1 (включено)
`EK3_RNG_USE_HGT`	Дальномер для высоты	1 (включено)
`EK3_VELNE_SOURCE`	Источник горизонтальной скорости	2 (optical flow)

Требования для полётов без GPS:

Оптический поток с хорошей текстурой поверхности
Дальномер для точной высоты
Стабильное освещение (для optical flow)
Высота над поверхностью < 3-5 м (зависит от сенсора)

Проверка после настройки:

[ ] Дрон удерживает позицию в помещении (режим Loiter)
[ ] Нет дрейфа при отсутствии GPS
[ ] Innovation test ratios стабильны без GPS

ЭТАП 7: Защита от помех и спуфинга

Параметр	Описание	Рекомендуемое значение
`EK3_GLITCH_RAD`	Радиус фильтрации скачков GPS	10 м (город), 5 м (открытая местность)
`EK3_GPS_CHECK`	Проверки здоровья GPS	1 (включено)
`EK3_SOURCE_MASK`	Маска источников навигации	Настроить под доступные сенсоры
`EK3_VELNE_SOURCE`	Резервный источник скорости	1 (GPS), 2 (optical flow)

Дополнительные меры:

[ ] Используйте множественные GPS-приёмники с разнесёнными антеннами
[ ] Включите SBAS/EGNOS для улучшения точности GPS
[ ] Рассмотрите RTK GPS для сантиметровой точности
[ ] Для критических применений — внешние CRPA-антенны

Проверка после настройки:

[ ] При временной потере GPS дрон не «улетает»
[ ] Резкие скачки координат фильтруются
[ ] Система переключается на резервные источники без сбоев

ЭТАП 8: Финальная проверка перед полётом

Проверка	Как выполнить	Критерий успеха
EKF Health	Mission Planner → Status → EKF	Все полосы «Healthy»
Variance	MAVExplorer → графики `EKF3.VAR`	Значения стабильны, не растут
Innovation	MAVExplorer → графики `EKF3.VIE`	Test ratio < 1.0 в большинстве случаев
Lane Switching	Логи полёта → `EKF3`	Нет частых переключений полос
GPS Quality	MP → HUD → GPS	HDOP < 1.5, спутников > 10
Compass Consistency	MP → HUD → Compass	Разница между компасами < 10°
Barometer Stability	Логи → `BARO`	Высота не «прыгает» при включённых моторах

5. Типовые проблемы и решения

Проблема	Симптомы	Диагностика	Решение
EKF не инициализируется	Дрон не武装, статус «EKF Unhealthy»	Проверьте логи `EKF3` на ошибки инициализации	Откалибруйте акселерометр и компас, проверьте барометр
Частые переключения полос	Нестабильный полёт, рывки	Логи `EKF3` → `Lane Switch` события	Найдите проблемный сенсор, разнесите компасы, проверьте экранирование
Дрейф позиции в Loiter	Дрон медленно смещается	Innovation test ratio для GPS > 1.0	Увеличьте `EK3_POS_I_GATE`, проверьте GPS антенну
Ошибки магнитометра	Нестабильный курс, вращение	Compass inconsistency > 15°	Перекалибруйте компас, уберите источники помех
Высокая нагрузка CPU	Контроллер греется, лаги	Status → CPU Load > 70%	Уменьшите `EK3_LANES`, отключите лишние сенсоры
Потеря высоты	Дрон набирает/теряет высоту	Барометр нестабилен при включённых моторах	Используйте `EK3_ALT_SOURCE = 1` (дальномер), проверьте вентиляцию барометра

6. Мониторинг в реальном времени

Через Mission Planner:

Подключитесь к дрону
Перейдите в Status → EKF3
Следите за:

Variance (должно быть стабильно)
Innovation Test Ratio (должно быть < 1.0)
Health Flags (должно быть «Healthy»)

Через логи (после полёта):

Скачайте логи с контроллера
Откройте в MAVExplorer или Mission Planner Log Analyser
Графики для анализа:

EKF3.VAR — ковариация состояний
EKF3.VIE — innovation test ratios
GPS — качество GPS сигнала
IMU — вибрации и смещения

Итоговый чек-лист перед первым полётом с EKF3

[ ] EK3_ENABLE = 1
[ ] Акселерометр откалиброван (6 сторон)
[ ] Гироскоп откалиброван (неподвижен)
[ ] Магнитометр откалиброван (вращение во всех направлениях)
[ ] Барометр стабилен (проверка при включённых моторах)
[ ] GPS имеет 3D фикс, HDOP < 1.5
[ ] Все полосы EKF3 показывают «Healthy»
[ ] Innovation test ratios < 1.0 в тестах на земле
[ ] CPU load < 60%
[ ] Логи включены (LOG_BITMASK включает EKF3)
[ ] Проведён тестовый полёт в режиме Stabilize
[ ] Проведён тестовый полёт в режиме Loiter (удержание позиции)

Выводы

EKF3 — мощный инструмент, но требует правильной настройки:

✅ Базовая настройка — достаточно для большинства применений
✅ Калибровка сенсоров — критична для точной работы
✅ Множественные сенсоры — повышают отказоустойчивость
✅ Мониторинг логов — помогает выявить проблемы до критических сбоев

Для стабильной работы следуйте чек-листу и регулярно проверяйте здоровье EKF3 через телеметрию и логи.

Источники: