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

/ Настройка ardupilot / От

Все статьи

Что такое EKF3?

Extended Kalman Filter 3 (EKF3) — это расширенный фильтр Калмана, используемый в ArduPilot для оценки состояния БПЛА. Он объединяет данные от множества сенсоров (IMU, GPS, барометр, магнитометр, дальномер, optical flow) для точного определения позиции, скорости и ориентации аппарата.

Ключевое отличие EKF3 от EKF2 — поддержка множественных экземпляров (lanes), что обеспечивает отказоустойчивость при сбоях сенсоров.

Архитектура EKF3

Количество состояний

EKF3 отслеживает 24 состояния в базовой конфигурации:

Группа состоянийКоличествоОписание
Quaternion4Ориентация в пространстве
Velocity NED3Скорость (North, East, Down)
Position NED3Позиция (North, East, Down)
Gyro bias3Смещение гироскопов
Delta velocity bias3Смещение акселерометров
Earth magnetic field3Вектор магнитного поля Земли
Body magnetic field3Вектор магнитного поля корпуса
Wind velocity2Скорость ветра (North, East)
Итого24

Множественные lanes

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

  • Резервирование при отказе IMU
  • Возможность сравнения результатов
  • Автоматическое переключение на лучшую lane

Количество lanes определяется параметром EK3_IMU_MASK:

ЗначениеДвоичный кодАктивные IMU
1001IMU 0
2010IMU 1
3011IMU 0 + IMU 1
4100IMU 2
5101IMU 0 + IMU 2
6110IMU 1 + IMU 2
7111IMU 0 + IMU 1 + IMU 2

⚠️ Важно: Каждая lane потребляет CPU. Для Pixhawk (STM32F4) рекомендуется не более 2 lanes, для Cube Orange/Pixhawk 6 — до 3 lanes.

Выбор источников навигации

Современная система EK3_SRC1_*

Начиная с ArduPilot 4.2, выбор источников навигации осуществляется через параметры EK3_SRC1_*. Это заменило устаревшие EK3_GPS_TYPE, EK3_ALT_SOURCE, EK3_FLOW_USE и маски.

Основные параметры источников

ПараметрОписаниеВарианты значений
EK3_SRC1_POSXYИсточник горизонтальной позиции0=None, 1=Baro, 2=GPS, 3=Rangefinder, 4=Beacon, 5=OptFlow, 6=ExtNav
EK3_SRC1_VELXYИсточник горизонтальной скоростиТе же варианты
EK3_SRC1_POSZИсточник вертикальной позицииТе же варианты
EK3_SRC1_VELZИсточник вертикальной скоростиТе же варианты
EK3_SRC1_YAWИсточник курса1=Compass, 2=GPS, 3=GPS+Compass fallback, 6=ExtNav, 8=GSF

Дополнительные источники (SRC2)

Для резервных сенсоров используются параметры EK3_SRC2_*:

ПараметрОписание
EK3_SRC2_POSXYРезервный источник горизонтальной позиции
EK3_SRC2_VELXYРезервный источник горизонтальной скорости
EK3_SRC2_POSZРезервный источник вертикальной позиции
EK3_SRC2_VELZРезервный источник вертикальной скорости
EK3_SRC2_YAWРезервный источник курса

Опции слияния источников

Параметр EK3_SRC_OPTIONS управляет слиянием данных из SRC1 и SRC2:

БитЗначениеОписание
01Fuse all velocity sources (слияние всех источников скорости)
12Fuse all position sources (слияние всех источников позиции)

⚠️ По умолчанию EK3_SRC_OPTIONS = 0 — используется только основной источник (SRC1), резервный (SRC2) применяется только при отказе основного.

Калибровка магнитометра

Параметр EK3_MAG_CAL определяет, когда и как выполняется калибровка магнитометра:

ЗначениеОписаниеРекомендация
0Heading fusion на земле, 3-axis fusion в полётеDefault для Plane
13-axis fusion только при маневрированииДля приложений с магнитными помехами
2Heading fusion всегдаТолько при идеальной калибровке
3Heading fusion на земле, 3-axis после первого сброса yaw в воздухеDefault для Copter ✅
4Никогда не калиброватьТолько для внутренних тестов
5External Yaw sensor (GPS heading)Для RTK GPS с dual antenna
6External Yaw with Compass fallbackGPS heading + компас как резерв

Выбор компаса

Выбор основного компаса осуществляется через параметры:

ПараметрОписание
COMPASS_USEИспользовать первый компас (0/1)
COMPASS_USE2Использовать второй компас (0/1)
COMPASS_USE3Использовать третий компас (0/1)
COMPASS_PRIO1_ID, COMPASS_PRIO2_ID, COMPASS_PRIO3_IDПриоритет компасов

Affinity и переключение lanes

Sensor Affinity

Параметр EK3_AFFINITY определяет, какие сенсоры могут переключаться между lanes:

БитЗначениеОписание
01Airspeed affinity (переключение между датчиками скорости)
12Barometer affinity (переключение между барометрами)
24GPS affinity (переключение между GPS)
38Magnetometer affinity (переключение между компасами)

Пример: EK3_AFFINITY = 15 (все биты установлены) — все сенсоры могут переключаться между lanes.

Порог переключения lanes

Параметр EK3_ERR_THRESH определяет порог относительной ошибки для переключения на другую lane:

ЗначениеОписание
0.0Отключено (нет автоматического переключения)
0.5Переключение при превышении ошибки в 50% (рекомендуется)
1.0Переключение при превышении ошибки в 100%

⚠️ Важно: Переключение происходит только если другая lane имеет меньшую ошибку. Если все lanes имеют одинаковую ошибку, переключения не будет.

Ручной выбор основной lane

Параметр EK3_PRIMARY задаёт номер lane, которая будет основной:

ЗначениеОписание
0Первая lane из EK3_IMU_MASK
1Вторая lane из EK3_IMU_MASK
2Третья lane из EK3_IMU_MASK

Пороги инноваций (Innovation Gates)

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

Основные пороги

ПараметрОписаниеПо умолчаниюЗначение в σ
EK3_VEL_I_GATEПорог скорости GPS5005.0σ
EK3_POS_I_GATEПорог позиции GPS5005.0σ
EK3_HGT_I_GATEПорог высоты5005.0σ
EK3_MAG_I_GATEПорог магнитометра3003.0σ
EK3_TAS_I_GATEПорог воздушной скорости5005.0σ

Как работают пороги

Если измерение выходит за пределы порога:

  1. Измерение отвергается (не используется для обновления)
  2. Счётчик отказов увеличивается
  3. Если отказы продолжаются, источник может быть временно отключён

Пример: EK3_MAG_I_GATE = 300 означает, что измерение магнитометра принимается только если оно находится в пределах 3.0σ от предсказания.

Практические конфигурации

Конфигурация 1: Стандартный полёт с GPS

# Базовая конфигурация
EK3_ENABLE = 1
AHRS_EKF_TYPE = 3
EK3_IMU_MASK = 7 (3 IMU)
EK3_PRIMARY = 0

# Источники навигации
EK3_SRC1_POSXY = 2 (GPS)
EK3_SRC1_VELXY = 2 (GPS)
EK3_SRC1_POSZ = 1 (Baro)
EK3_SRC1_VELZ = 2 (GPS)
EK3_SRC1_YAW = 1 (Compass)

# Калибровка магнитометра
EK3_MAG_CAL = 3 (default для Copter)

# Affinity и переключение
EK3_AFFINITY = 15 (все сенсоры)
EK3_ERR_THRESH = 0.5

Конфигурация 2: Полёт без GPS (optical flow + дальномер)

# Базовая конфигурация
EK3_ENABLE = 1
AHRS_EKF_TYPE = 3
EK3_IMU_MASK = 3 (2 IMU)
EK3_PRIMARY = 0

# Источники навигации
EK3_SRC1_POSXY = 0 (None - нет абсолютной позиции)
EK3_SRC1_VELXY = 5 (OpticalFlow)
EK3_SRC1_POSZ = 2 (RangeFinder)
EK3_SRC1_VELZ = 2 (RangeFinder)
EK3_SRC1_YAW = 1 (Compass)

# Калибровка магнитометра
EK3_MAG_CAL = 3

# Affinity
EK3_AFFINITY = 8 (только магнитометр)
EK3_ERR_THRESH = 0.5

Конфигурация 3: Indoor полёт с Beacon системой

# Базовая конфигурация
EK3_ENABLE = 1
AHRS_EKF_TYPE = 3
EK3_IMU_MASK = 1 (1 IMU)
EK3_PRIMARY = 0

# Источники навигации
EK3_SRC1_POSXY = 4 (Beacon)
EK3_SRC1_VELXY = 0 (None - нет данных скорости)
EK3_SRC1_POSZ = 4 (Beacon)
EK3_SRC1_VELZ = 0 (None)
EK3_SRC1_YAW = 1 (Compass)

# Калибровка магнитометра
EK3_MAG_CAL = 3

Конфигурация 4: RTK GPS с dual antenna (GPS heading)

# Базовая конфигурация
EK3_ENABLE = 1
AHRS_EKF_TYPE = 3
EK3_IMU_MASK = 7 (3 IMU)
EK3_PRIMARY = 0

# Источники навигации
EK3_SRC1_POSXY = 2 (GPS)
EK3_SRC1_VELXY = 2 (GPS)
EK3_SRC1_POSZ = 1 (Baro)
EK3_SRC1_VELZ = 2 (GPS)
EK3_SRC1_YAW = 2 (GPS heading)

# Калибровка магнитометра
EK3_MAG_CAL = 5 (External Yaw sensor)

# Affinity
EK3_AFFINITY = 15
EK3_ERR_THRESH = 0.5

Диагностика и troubleshooting

Проверка состояния EKF

Для диагностики используйте параметры:

ПараметрОписание
EK3_VELinnovИнновации скорости
EK3_POSinnovИнновации позиции
EK3_HGTinnovИнновации высоты
EK3_MAGinnovИнновации магнитометра
EK3_VEL_test_ratioОтношение инноваций скорости к порогу
EK3_POS_test_ratioОтношение инноваций позиции к порогу
EK3_HGT_test_ratioОтношение инноваций высоты к порогу
EK3_MAG_test_ratioОтношение инноваций магнитометра к порогу

Интерпретация test_ratio

ЗначениеОписание
< 1.0✅ Нормально, измерения принимаются
1.0 — 1.5⚠️ Пограничное состояние, возможны отказы
> 1.5❌ Измерения отвергаются, проблема с сенсором

Типичные проблемы

Проблема 1: Высокие инновации магнитометра

Симптомы: EK3_MAG_test_ratio > 1.5

Возможные причины:

  • Магнитные помехи от моторов/проводов
  • Неправильная калибровка компаса
  • Неверный COMPASS_ORIENT

Решения:

  1. Проверить калибровку компаса
  2. Увеличить расстояние между компасом и источниками помех
  3. Использовать внешний компас
  4. Временно увеличить EK3_MAG_I_GATE до 400-500

Проблема 2: Высокие инновации высоты

Симптомы: EK3_HGT_test_ratio > 1.5

Возможные причины:

  • Неисправный барометр
  • Помехи от винтов на барометр
  • Неверный EK3_SRC1_POSZ

Решения:

  1. Проверить барометр (закрыть от ветра)
  2. Убедиться, что EK3_SRC1_POSZ = 1 (Baro)
  3. Проверить BARO_GND_PRESS

Проблема 3: Частое переключение lanes

Симптомы: EK3_PRIMARY постоянно меняется

Возможные причины:

  • Слишком низкий EK3_ERR_THRESH
  • Неисправный IMU
  • Вибрации

Решения:

  1. Увеличить EK3_ERR_THRESH до 0.7-1.0
  2. Проверить вибрации (VIBE в логах)
  3. Исключить неисправный IMU из EK3_IMU_MASK

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

Параметры задержек сенсоров

ПараметрОписаниеПо умолчанию
EK3_GPS_DELAYЗадержка GPS (мс)100
EK3_BARO_DELAYЗадержка барометра (мс)0
EK3_MAG_DELAYЗадержка магнитометра (мс)0
EK3_FLOW_DELAYЗадержка optical flow (мс)0

Параметры шумов

ПараметрОписаниеПо умолчанию
EK3_GPS_NOISEШум GPS (м/с)0.5
EK3_BARO_NOISEШум барометра (м)0.5
EK3_MAG_NOISEШум магнитометра0.05
EK3_GYRO_NOISEШум гироскопа (рад/с)0.01
EK3_ACC_NOISEШум акселерометра (м/с²)0.5

⚠️ Важно: Не меняйте параметры шумов без понимания их влияния. Значения по умолчанию оптимизированы для большинства случаев.

✅ Чек-лист настройки EKF3

Этап 1: Базовая конфигурация

  • [ ] EK3_ENABLE = 1
  • [ ] AHRS_EKF_TYPE = 3
  • [ ] EK3_IMU_MASK установлен правильно (1, 3 или 7)
  • [ ] EK3_PRIMARY = 0

Этап 2: Выбор источников

  • [ ] EK3_SRC1_POSXY установлен правильно
  • [ ] EK3_SRC1_VELXY установлен правильно
  • [ ] EK3_SRC1_POSZ установлен правильно
  • [ ] EK3_SRC1_VELZ установлен правильно
  • [ ] EK3_SRC1_YAW установлен правильно

Этап 3: Калибровка

  • [ ] Компас откалиброван
  • [ ] EK3_MAG_CAL установлен правильно (3 для Copter, 0 для Plane)
  • [ ] COMPASS_USE, COMPASS_USE2, COMPASS_USE3 настроены

Этап 4: Affinity и переключение

  • [ ] EK3_AFFINITY установлен (рекомендуется 15)
  • [ ] EK3_ERR_THRESH установлен (рекомендуется 0.5)

Этап 5: Проверка

  • [ ] Проверить EK3_*_test_ratio в полёте
  • [ ] Убедиться, что все значения < 1.0
  • [ ] Проверить логи на наличие отказов

📖 Источники

  1. EKF Source Selection and Switching
  2. Extended Kalman Filter (EKF) Overview
  3. EKF3 Affinity and Lane Switching
  4. ArduPilot Parameter Documentation
  5. EKF3 Tuning Guide

🎯 Заключение

EKF3 — мощный инструмент для навигации БПЛА, но требует правильного понимания архитектуры и параметров. Ключевые моменты:

  1. Используйте современные параметры EK3_SRC1_* вместо устаревших масок
  2. Настраивайте EK3_AFFINITY для отказоустойчивости
  3. Мониторьте EK3_*_test_ratio для диагностики проблем
  4. Не меняйте параметры шумов без необходимости
  5. Выбирайте количество lanes с учётом производительности CPU
Прокрутить вверх