Фільтр Калмана

Два недовірливих джерела, одна краща відповідь

Припустимо, ви хочете знати точну позицію та швидкість дрона. У вас є два способи здогадуватися, і жоден з них не надійна сам по собі. Ваша модель може повідомити вам, з останнього відомого стану та застосованих вами керуючих входів, де повинен бути дрон зараз — точна в коротких часових масштабах, але вона повільно відхиляється через накопичення немодельованих поривів вітру та недосконалості двигунів. Ваш датчик — GPS, наприклад — вимірює позицію безпосередньо, але він шумний (зазвичай ±3 м) і оновлюється повільно (1-10 Гц). Жодне з цих джерел само по собі недостатньо. Фільтр Кальмана, отриманий Рудольфом Кальманом у 1960 році та відомо використовувався для навігації Apollo на Місяць, є математично оптимальним способом об'єднання двох: він обернено важить кожен джерело залежно від того, наскільки воно непевно в даний момент, довіряючи датчику більше, коли модель відхиляється, і довіряючи моделі більше, коли датчик шумний.

Підґрунтя стану простору

Лінійну динамічну систему описують дві рівня – одна для того, як змінюється стан, а інша для того, як він спостерігається:

x_k = A · x_(k-1) + B · u_k + w_k    // process model
y_k = C · x_k + v_k                   // measurement model

x  state vector (position, velocity, ...)     A  state-transition matrix
B  control-input matrix; u  control input     C  observation matrix
w ~ N(0, Q)  process noise                    v ~ N(0, R)  measurement noise

1D constant-velocity example (state = [position, velocity]):
  A = [[1, dt], [0, 1]]      C = [1, 0]   // we only measure position

Цикл прогнозирования-обновления

В каждый момент времени выполняются две фазы последовательно. Прогнозирование (Predict) распространяет состояние вперёд с помощью модели только, и позволяет его неопределённости расти: x̂⁻ₖ = A·x̂ₖ₋₁ + B·uₖ, а ошибка ковариации P⁻ₖ = A·Pₖ₋₁·Aᵀ + Q растёт за счёт шумовой матрицы Q. Обновление (Update) затем включает новое измерение: оно вычисляет коэффициент Кальмана K = P⁻·Cᵀ·(C·P⁻·Cᵀ + R)⁻¹, корректирует оценку состояния с помощью разности между измерением и прогнозом, x̂ₖ = x̂⁻ₖ + K·(yₖ − C·x̂⁻ₖ), и соответственно уменьшает ковариацию, Pₖ = (I − K·C)·P⁻ₖ. Термин (yₖ − C·x̂⁻ₖ) называется инновацией — насколько фильтр удивлён новым показанием — и K решает, во сколько раз в это удивление поверить.

Визначення коефіцієнта підсилення

У випадку з ма scalary, коефіцієнт підсилення стискається до одного, дуже зрозумілого співвідношення:

K = P⁻ / (P⁻ + R)

K → 1 коли P⁻/R → ∞ (модель дуже невизначена, датчик точний) → довіряйте датчику

K → 0 коли P⁻/R → 0 (модель точна, датчик шумний) → довіряйте моделі

Приклад: невизначеність моделі P⁻ = 9 м², дисперсія GPS R = 4 м²

K = 9 / (9 + 4) ≈ 0.69 → більше схиляємося до даних GPS-пристрою

Корисною практичною швидкістю є: коли A, C, Q та R залишаються постійними з часом, коваріація P сходиться до сталої стаціонарної величини лише після кількох ітерацій, і ви можете попередньо обчислити цю сталу величину заздалегідь — це значна економія для вбудованих систем, які не можуть дозволити собі розв’язувати матричні обчислення що циклічно.

K = P⁻ / (P⁻ + R)

K → 1   as P⁻/R → ∞   (model very uncertain, sensor accurate)  → trust sensor
K → 0   as P⁻/R → 0   (model accurate, sensor noisy)           → trust model

Example: model uncertainty P⁻ = 9 m², GPS variance R = 4 m²
K = 9 / (9 + 4) ≈ 0.69  →  lean mostly toward the GPS reading

Коли світ нелінійний: EKF та UKF

Стандартний фільтр передбачає, що обидва A і C є фіксованими матрицями, що порушується в момент, коли динаміка стає справді нелінійною — наприклад, робот, де зміна напрямку орієнтації змінює швидкість синусоїдальним чином, або датчик лише з урахуванням азимуту. Розширений Кальманівський фільтр (EKF) зберігає структуру передбачення-оновлення, але на кожному кроці перераховує нелінійні функції f і h за допомогою їхніх Якобіанів матриць, F = ∂f/∂x і H = ∂h/∂x, обчислених на поточному оцінці — це стандартний вибір для більшості робочих систем, включаючи пакет robot_localization ROS та типові трубопрогінні системи. Коли нелінійність значна, щоб першого порядку Якобіана призводив до розбіжностей, Несчислений Кальманівський фільтр (UKF) замість цього просуває невеликий детермінований набір точок-вибірок — сигма точки — безпосередньо через справжню нелінійну функцію, захоплюючи розподіл з точністю до третього порядку, не обчислюючи Якобіан.

Де куди воно насправді працює

За межами керовання космічними апаратами, той самий цикл передбачення-оновлення з'являється майже скрізь, де потрібне поєднувати шумні сенсори з моделлю: злиття GPS та IMU у телефонах і дронах, відстеження цілей радаром, багатооб’єктний відстежувач в комп’ютерних зорових конвеєрних лініях, таких як SORT і DeepSORT (де фільтр Калмана прогнозує рух між кадрами для кожного обмежувального прямокутника), оцінка прихованої волатильності з шумних фінансових часових рядів, а навіть шумоподавлення мови від шумного мікрофона. Його довготривала привабливість полягає в тому, що йому не потрібні дані для навчання, він працює у закритому вигляді з невеликою кількістю матричних операцій на крок і доведено, що це оптимальний лінійний оціник під Гаусовим шумом – властивості, які важко перевершити в вбудованих, реальному часі застосунках навіть у епоху навчених альтернатив.

Frequently asked questions

Спробуйте наживо

Усе, що вище, працює прямо у вашому браузері — відкрийте the simulation і змінюйте параметри під час роботи. Нічого не встановлюється, нічого не завантажується на сервер, уся модель живе в одній вкладці.