Фільтр Калмана
GPS «пливе». IMU накопичує похибку. Але в поєднанні з фільтром Калмана вони дають плавні, точні оцінки положення. Фільтр Калмана — це оптимальний лінійний оцінювач: він математично мінімізує середньоквадратичну похибку, поєднуючи прогноз моделі із зашумленими вимірюваннями.
1. Задача оцінювання
Ви хочете знати справжній стан системи — скажімо, положення й швидкість дрона. Ви маєте два недосконалі джерела інформації:
- Прогноз моделі: «За останнім станом і моїми керувальними впливами (тяга тощо), де він має бути?» — точний на короткому проміжку, але «пливе» через немодельовані сили.
- Вимірювання: GPS дає положення безпосередньо — але воно зашумлене (±3 м) і повільне (1–10 Гц).
Фільтр Калмана поєднує обидва оптимально. Він зважує кожне джерело обернено до його невизначеності: більше довіряти вимірюванню, коли модель невизначена; більше довіряти моделі, коли сенсор зашумлений.
2. Модель у просторі станів
Лінійна динамічна система описується двома рівняннями:
y_k = C·x_k + v_k (модель вимірювання)
x — вектор стану (положення, швидкість, …)
A — матриця переходу станів
B — матриця керувального входу; u — керувальний вхід
C — матриця спостереження (відображає стан → вимірювання)
w ~ N(0, Q) — шум процесу;
v ~ N(0, R) — шум вимірювання
Для одновимірної моделі зі сталою швидкістю з положенням x і швидкістю v:
A = [[1, dt],[0, 1]]; C = [1, 0] (ми вимірюємо лише положення).
3. Цикл «прогноз–оновлення»
На кожному кроці виконуються дві фази:
🔵 Прогноз (оновлення в часі)
Проєктуємо стан уперед за допомогою моделі.
x̂⁻ₖ = A·x̂_{k-1} + B·u_k
P⁻ₖ = A·P_{k-1}·Aᵀ + Q
P — матриця коваріації похибки
Q — коваріація шуму процесу
🟢 Оновлення (за вимірюванням)
Коригуємо прогноз вимірюванням.
K = P⁻·Cᵀ·(C·P⁻·Cᵀ + R)⁻¹
x̂ₖ = x̂⁻ₖ + K·(yₖ − C·x̂⁻ₖ)
Pₖ = (I − K·C)·P⁻ₖ
K — підсилення Калмана
Член (y_k − C·x̂⁻_k) називають інновацією або нев'язкою — наскільки вимірювання відрізняється від прогнозу. Підсилення Калмана K масштабує, яку частину цієї нев'язки використати.
4. Інтуїція щодо підсилення Калмана
Підсилення Калмана K ∈ [0, 1] відображає довіру до вимірювання:
- K → 1: повністю довіряти вимірюванню (P⁻ велике, R крихітне — модель дуже невизначена, сенсор дуже точний)
- K → 0: довіряти прогнозу моделі (P⁻ крихітне, R велике — модель точна, сенсор дуже зашумлений)
Для скалярного одновимірного випадку: K = P⁻ / (P⁻ + R). Якщо невизначеність моделі P⁻ = 9 м², а дисперсія GPS R = 4 м²: K = 9/(9+4) ≈ 0,69 — схиляємося до GPS.
5. Розширений та сигма-точковий фільтри Калмана
Розширений фільтр Калмана (EKF)
Коли модель переходу стану чи вимірювання нелінійна — наприклад, робот повертає із синусоїдальним оновленням положення — стандартний фільтр Калмана дає збій. EKF лінеаризує нелінійні функції за допомогою матриць Якобі на кожному кроці:
y_k = h(x_k) + v_k (нелінійне вимірювання)
F = ∂f/∂x |_{x̂} (Якобіан f, обчислений у x̂)
H = ∂h/∂x |_{x̂} (Якобіан h)
EKF замінює A на F, а C на H у стандартних рівняннях. Це найпопулярніший фільтр для робототехніки (ROS robot_localization, SLAM для автономних транспортних засобів).
Сигма-точковий фільтр Калмана (UKF)
EKF може розбігатися для сильно нелінійних систем, бо Якобіан — це лише наближення першого порядку. UKF використовує детерміновані сигма-точки (2n+1 ретельно дібраних вибірок стану), щоб пропустити розподіл крізь нелінійну функцію — досягаючи точності третього порядку без обчислення жодних Якобіанів.
6. Застосування
- Злиття GPS + IMU: GPS дає абсолютне положення з частотою 10 Гц; акселерометр IMU — 1000 Гц. Фільтр Калмана поєднує їх: плавне положення на 1000 Гц із корекцією дрейфу GPS.
- Радари супроводу: бортовий комп'ютер наведення Apollo використовував фільтр Калмана для навігації до Місяця (1960-ті). Рудольф Калман вивів його 1960 року.
- Фінансові часові ряди: оцінювання прихованої волатильності за зашумленими ціновими даними.
- Комп'ютерний зір: супровід обмежувальних рамок між кадрами (SORT, DeepSORT використовують фільтр Калмана як прогнозувач руху).
- Зниження аудіошуму: фільтр Калмана оцінює чисте мовлення із зашумленого мікрофонного сигналу.
7. Реалізація на JavaScript (одновимірна, стала швидкість)
// Одновимірний фільтр Калмана — відстеження положення за зашумленими вимірюваннями
class KalmanFilter1D {
constructor({ processNoise = 0.1, measurementNoise = 4 } = {}) {
this.Q = processNoise; // дисперсія шуму процесу
this.R = measurementNoise; // дисперсія шуму вимірювання
this.x = 0; // оцінка стану (положення)
this.P = 1; // коваріація похибки
this.dt = 1; // крок за часом
}
predict(velocity = 0) {
// Перехід стану: pos += vel * dt
this.x = this.x + velocity * this.dt;
// Коваріація зростає разом із шумом процесу
this.P = this.P + this.Q;
}
update(measurement) {
const K = this.P / (this.P + this.R); // підсилення Калмана
this.x = this.x + K * (measurement - this.x); // оновлення
this.P = (1 - K) * this.P; // зменшення невизначеності
return this.x;
}
}
// Використання: поєднання зашумленого GPS із прогнозом швидкості
const kf = new KalmanFilter1D({ processNoise: 0.5, measurementNoise: 9 });
const gpsReadings = [10.2, 10.9, 12.1, 13.8, 15.3]; // зашумлені
for (const gps of gpsReadings) {
kf.predict(/*velocity=*/ 1.5);
const estimate = kf.update(gps);
console.log(`GPS: ${gps.toFixed(1)}, Estimate: ${estimate.toFixed(2)}`);
}