ЗЛИТТЯ СЕНСОРІВ
EKF3

Розширений фільтр Калмана — найважливіший алгоритм у автономному польоті дрона. Він відповідає на два фундаментальних питання: «Де я?» і «Куди я дивлюся?». Ця сторінка пояснює, що він робить, чому це важливо і як це працює у GPS-denied середовищі РЕБ.

Що робить EKF3 — простою мовою

Уявіть, що ви з зав'язаними очима в автомобілі. Ви ВІДЧУВАЄТЕ, як машина повертає (це гіроскоп). Ви ВІДЧУВАЄТЕ прискорення і гальмування (це акселерометр). Хтось каже вам тиск повітря (це барометр — нижчий тиск означає більшу висоту). І у вас є компас.

Кожне з цих відчуттів дає вам підказку про те, де ви і що робите. Але кожне недосконале. Гіроскоп повільно дрейфує — через 5 хвилин він думає, що ви повернули на 3° більше, ніж насправді. Акселерометр ловить вібрації від двигуна. Компас плутається біля металевих об'єктів. Барометр змінюється з погодою.

EKF3 — це алгоритм, який каже: «Я знаю, що кожен сенсор трохи бреше. Я знаю, НАСКІЛЬКИ кожен сенсор зазвичай бреше. Я візьму всі їхні показники, зважу за надійністю кожного і обчислю НАЙКРАЩУ МОЖЛИВУ ОЦІНКУ того, де я і як рухаюся». Він робить це 400 разів на секунду.

Це означає: якщо гіроскоп каже, що ви повернули на 30°, а компас — на 27°, і EKF3 знає, що гіроскоп дрейфує, а компас точний у цій зоні, він оцінить 27,5° — довіряючи компасу більше. Якщо компас біля металу і дає божевільні показники, EKF3 автоматично зменшує довіру до компаса і покладається більше на гіроскоп.

Коли GPS заглушено — режим AHRS

У GPS-denied середовищі (ворог глушить усі супутникові сигнали) EKF3 втрачає найкращий сенсор позиції. Він перемикається в режим AHRS (Attitude and Heading Reference System). У цьому режимі він знає орієнтацію дрона — крен, тангаж і рискання — з IMU (гіроскоп + акселерометр) і барометра. Висоту знає з барометра. Але НЕ знає латеральну позицію (північ/південь/схід/захід).

Що це означає операційно: дрон усе ще може літати рівно, тримати висоту і підтримувати курс. Пілот літає за візуальним орієнтиром через FPV-окуляри. AI усе ще може виявляти цілі і обчислювати ВІДНОСНУ позицію з математики виявлення (див. §6.1). Але абсолютні координати на карті потребують або зіставлення рельєфу з попередньо завантаженими ортофото, або ручної оцінки оператором.

Додавання сенсора оптичного потоку (PMW3901, €8) відновлює оцінку швидкості відносно землі. Дрон не дрейфуватиме некеровано — він може тримати позицію. Але оцінка позиції дрейфує з часом без GPS-корекцій. Очікуйте приблизно 1–3 м/хв дрейфу при стабільному висінні.

Сенсори — що вимірює кожен

Математика — для інженерів

EKF3 запускає цикл прогноз-корекція 400 разів на секунду. Простою мовою це означає: «На основі того, що мені кажуть гіроскоп і акселерометр, я ПРОГНОЗУЮ, де дрон має бути зараз. Потім ПЕРЕВІРЯЮ цей прогноз проти барометра, компаса і будь-якого іншого доступного сенсора. Якщо прогноз був хибним, я КОРИГУЮ його. Величина корекції залежить від того, наскільки я довіряю кожному сенсору зараз».

Основне рівняння (прогноз стану):

# State prediction (runs every 2.5ms)
x_predicted = F @ x_previous + B @ u
# F = state transition matrix (physics model)
# x = state vector [position, velocity, attitude, gyro_bias, accel_bias]
# u = IMU readings [accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z]

# Covariance prediction (how uncertain are we?)
P_predicted = F @ P_previous @ F.T + Q
# Q = process noise (how much we expect the model to be wrong)

# Measurement update (when baro/compass/GPS data arrives)
K = P_predicted @ H.T @ inv(H @ P_predicted @ H.T + R)
# K = Kalman gain (how much to trust this measurement)
# H = measurement matrix (maps state to sensor reading)
# R = measurement noise (how much this sensor typically lies)

x_corrected = x_predicted + K @ (measurement - H @ x_predicted)
P_corrected = (I - K @ H) @ P_predicted

Що це означає простою мовою: підсилення Калмана K — це «фактор довіри». Коли K велике, система довіряє сенсору більше, ніж прогнозу. Коли K мале, вона довіряє прогнозу більше. K автоматично регулюється на основі того, наскільки послідовним був кожен сенсор останнім часом. Якщо компас починає давати нестабільні показники (наприклад, біля полярного сяйва), K для компаса падає до нуля, і система покладається на гіроскоп плюс акселерометр натомість.

EKF3 використовує 24-мірний вектор стану: позиція (3), швидкість (3), орієнтація (4 кватерніон), зсуви гіроскопа (3), зсуви акселерометра (3), магнітне поле (3), швидкість вітру (3), зсув барометра (1), зсув далекоміра (1). Фільтр виконує два кроки 400 разів на секунду: прогноз (інтегрувати гіроскоп і акселерометр для оцінки нового стану — швидко, але дрейфує) і корекція (порівняти прогноз з вимірюваннями GPS/барометра/оптичного потоку і скоригувати — повільно, але точно). Вага кожного сенсора залежить від його поточної надійності. Якщо GPS-сигнал зашумлений (глушіння): EKF3 автоматично зменшує вагу GPS до нуля — плавний перехід, без різких стрибків у навігації.

Математичне виведення — чому коефіцієнт Калмана є оптимальним

Повне п\'ятикрокове виведення оновлення Калмана опубліковано в англійській версії. Виведення відповідає оригінальній статті Калмана (1960) та стандартному підручнику (Beard & McLain, Small Unmanned Aircraft, Princeton 2012, розділ 8; Simon Optimal State Estimation, Wiley 2006). Внесок сторінки — спеціалізація загальної теореми до сенсорного набору Fischer 26 і показ того, чому кожне значення матриці R має саме таку величину.

Стисло: (1) Лінійна модель вимірювання z = H·x + v з v ~ N(0, R). Значення R беруться з даташитів: R_baro = 0,25 м², R_mag = 0,008 рад², R_flow = 0,01 м²/с². (2) Мінімізація trace(P⁺) по K → K = P⁻·Hᵀ · (H·P⁻·Hᵀ + R)⁻¹ є унікальною оптимальною вагою. (3) У скалярному випадку K = P⁻/(P⁻+R) показує, що K → 0 коли прогноз впевнений, K → 1 коли сенсор набагато кращий. (4) Вентиль інновацій із 5σ порогом запобігає корупції стану хибними вимірами. (5) Лінеаризація навколо x̂⁻ забезпечує нелінійне опрацювання EKF; потребує початкової конвергенції (~30 с стаціонарно перед озброєнням), щоб P⁻ було досить малим для валідності лінеаризації.

Опрацьований приклад 1 — оновлення барометра прийнято

Інтеграція IMU змістила оцінку висоти до 118,2 м із 1σ = 2,0 м (P⁻ = 4,0 м²). Барометр показує 120,1 м із R = 0,25 м². K = 4,0/4,25 = 0,941. Оновлена оцінка: 118,2 + 0,941 × 1,9 = 119,99 м. Нова невизначеність: σ = √((1−0,941)·4,0) = 0,485 м — строго менше за обидва джерела (прогноз 2 м, сенсор 0,5 м). Це математична причина того, чому злиття багатьох сенсорів перевершує окремі сенсори. Перевірено в provable_claims.py під EKF_SCALAR_UPDATE_GAIN та EKF_SCALAR_UPDATE_POSTERIOR.

Опрацьований приклад 2 — хибне вимірювання відхилене вентилем інновацій

Той самий prior (118,2 м, P⁻ = 4,0 м²). Хибне показання барометра 85 м (тисковий імпульс від гвинта під час агресивного маневру). Інновація y = −33,2 м; дисперсія інновації S = 4,25 м²; σ-співвідношення = 33,2/2,06 = 16,1σ. Перевищує 5σ-вентиль ArduPilot → вимірювання ВІДХИЛЕНО, стан збережено. За стійких хибних показань > 2 с EKF3 позначає сенсор як несправний і GCS показує попередження «EK3 IMU0 baro inconsistent». Перевірено в EKF_INNOVATION_GATE_REJECTS_OUTLIER.

Чому це виведення є операційно важливим

Чотири оперативні рішення залежать від коректності математики Калмана. Закупівля сенсорів: значення R-матриці беруться напряму з даташитів; дешевший сенсор із 3× шумом дає 9× значення R і передбачувано погіршує коефіцієнт Калмана — планувальник може обчислити результуючу позиційну невизначеність, перш ніж заощадити €30 на планер. Рекомендація оптичного потоку: PMW3901 за €8 зменшує дрейф у завислому режимі з 200 м/10 хв (тільки IMU) до ~10 м/10 хв (IMU + flow). Без виведення Калмана це лише заява; із ним видно чому: flow має малий R (0,01 м²/с²) порівняно з процесним шумом Q IMU. Дисципліна вентиля інновацій: 5σ-поріг — це різниця між «надійним злиттям» і «збоєм сенсора → катастрофічним польотом». Вимога початкового вирівнювання: Fischer 26 повинен стояти нерухомо 30 с перед озброєнням, щоб P⁻ стало досить малим для валідності лінеаризації — командир, який пропускає цей крок заради «швидшого розгортання», відправляє в повітря планер, який може ніколи не збігатись.

Конфігурація ArduPilot для GPS-Denied

Практичний вплив

Без GPS Fischer 26 може кружляти і збирати ISR-дані, але не може точно геолокувати цілі. Виявлення прив'язуються до оціненої позиції дрона, яка дрейфує. Для FPV-наведення (пілот бачить ціль на відео) дрейф не критичний. Для артилерійських координат (потрібно <50 м CEP) — дрейф неприйнятний без додаткової корекції, такої як зіставлення рельєфу з попередньо завантаженим ортофото або візуальна ідентифікація оператором відомих орієнтирів.

← Частина Архітектура Lisa 26

Джерела

Параметричні джерела. Характеристики BMI270 IMU (400 Гц, дрейф ~0,5°/хв) — datasheet Bosch Sensortec 2022. Характеристики BMP390 барометра (±0,5 м, 50 Гц) — datasheet Bosch 2023. Специфікації PMW3901 оптичного потоку (±0,1 м/с, працює нижче 30 м AGL, 80 Гц) — datasheet PixArt 2023. Специфікації магнітометра IST8310 — datasheet iSentek. Специфікації VL53L1X далекоміра (діапазон 0–4 м, точність ±2 см) — datasheet STMicroelectronics. Параметри ArduPilot (AHRS_EKF_TYPE, EK3_SRC1_*, FLOW_TYPE, RNGFND1_TYPE) — документація ArduPilot.

Математичні джерела. 24-мірний вектор стану EKF3 — визначено в коді ArduPilot (libraries/AP_NavEKF3). Рівняння прогнозу x = F·x + B·u та коваріації P = F·P·Fᵀ + Q — стандартний розширений фільтр Калмана. Підсилення Калмана K = P·Hᵀ·(H·P·Hᵀ + R)⁻¹ — стандартне виведення.

Операційні оцінки — не верифіковано FSG-A в польових умовах. Дрейф позиції 1–3 м/хв при висінні без GPS — типова продуктивність EKF3 з оптичним потоком у публічних звітах, не виміряна FSG-A на дроновій платформі. Дрейф 50–200 м за 10-хвилинний політ — масштабування від хвилинної швидкості, залежить від стабільності маневру. Час виявлення втрати GPS <2 секунди — опубліковане значення ArduPilot для переходу у режим fallback. Усі ці цифри мають бути перевалідовані перед оперативним використанням.

Зовнішні стандарти та джерела. Документація ArduPilot EKF3 (ardupilot.org/dev/docs/ekf3, 2024). «Extended Kalman Filtering for Battery and GPS-Denied Navigation» — Beard & McLain, Small Unmanned Aircraft (Princeton, 2012). Datasheet BMI270 IMU (Bosch, 2022). Datasheet BMP390 барометра (Bosch, 2023). Datasheet PMW3901 оптичного потоку (PixArt, 2023). Теорія фільтра Калмана (Welch & Bishop, UNC). Меморандум FOI 8336 про автономні UAS (2024). FSG-A не має власних даних GPS-denied польотів — параметри конфігурації з документації ArduPilot.

Пов'язані розділи