Kalman Filter (KF)

Фильтр Калмана (KF) — это рекурсивный алгоритм, который по потоку зашумлённых измерений оценивает скрытое состояние меняющейся системы, например положение и скорость объекта. На каждом временном шаге он предсказывает следующее состояние на основе модели движения системы, а затем корректирует предсказание с учётом нового измерения, взвешивая оба значения согласно их неопределённости. Рудольф Э. Калман опубликовал этот метод в 1960 году; для линейных систем с гауссовским шумом он даёт оценку с минимальной среднеквадратичной ошибкой. Фильтр применяется в GPS-приёмниках, навигации самолётов и космических аппаратов, локализации роботов и почти каждом трекере нескольких объектов в компьютерном зрении, где он сглаживает скачки детекций и предсказывает, где каждый объект появится дальше.

Как работает фильтр Калмана#

Фильтр хранит две величины: оценку состояния x, например положения и скорости, и ковариационную матрицу P, описывающую неопределённость этой оценки. Каждый цикл состоит из двух шагов:

1. Предсказание: модель движения F проецирует состояние вперёд, x = F x, а неопределённость увеличивается на величину шума процесса Q, учитывающего движение, которое модель не описывает: P = F P Fᵀ + Q.
2. Обновление: поступает новое измерение z, связанное с состоянием моделью измерения H. Фильтр вычисляет невязку z − H x и коэффициент Калмана K = P Hᵀ (H P Hᵀ + R)⁻¹, где R — это шум измерения. Скорректированное состояние равно x + K (z − H x), а неопределённость уменьшается до (I − K H) P.

Коэффициент Калмана задаёт баланс между моделью и датчиком. Когда измерения точны относительно предсказания, коэффициент приближается к 1 и оценка следует за измерением; когда измерения зашумлены, коэффициент приближается к 0 и оценка следует за предсказанием. Поскольку состояние может включать величины, которые никогда не измеряются напрямую, фильтр также выводит их косвенно: трекер, наблюдающий только положения, всё равно оценивает скорость. Если измерение отсутствует, фильтр пропускает обновление и продолжает использовать предсказание:

Фильтры Калмана в отслеживании объектов#

В отслеживании объектов для каждого трека используется собственный фильтр Калмана. Состояние включает центр и размер ограничивающей рамки, а также их скорости; модель с постоянной скоростью предсказывает, где рамка окажется в следующем кадре. Эти предсказания сопоставляются с новыми детекциями по пересечению над объединением (IoU), а каждая сопоставленная детекция становится измерением для обновления трека. Если объект скрыт несколько кадров, трек продолжает двигаться по предсказанной траектории, поэтому повторно появившаяся детекция оказывается достаточно близко к предсказанию, чтобы сохранить прежний ID.

Так работают все трекеры в режиме отслеживания Ultralytics YOLO: KalmanFilterXYAH из ByteTrack отслеживает восьмимерное состояние, включающее центр рамки, соотношение сторон, высоту и их скорости; BoT-SORT и TrackTrack используют вариант с шириной и высотой, а оба метода масштабируют шум в соответствии с размером рамки, поэтому для крупных близких объектов допустимы большие скачки в пикселях. Те же шаги предсказания и обновления можно реализовать всего в нескольких строках NumPy для одной координаты центра рамки:

import numpy as np

F = np.array([[1.0, 1.0], [0.0, 1.0]])  # constant-velocity model, state = [position, velocity]
H = np.array([[1.0, 0.0]])  # the detector measures position only
Q = 0.001 * np.eye(2)  # process noise: how far real motion may stray from the model
R = np.array([[25.0]])  # measurement noise: detections jitter by about 5 px

rng = np.random.default_rng(0)
truth = 100 + 3.0 * np.arange(60)  # box center moving 3 px per frame
z = truth + rng.normal(0, 5.0, truth.size)  # noisy detections

x, P = np.array([z[0], 0.0]), np.diag([25.0, 100.0])  # start from the first detection
estimates = [x[0]]
for measurement in z[1:]:
    x, P = F @ x, F @ P @ F.T + Q  # predict
    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)  # Kalman gain
    x = x + K @ (measurement - H @ x)  # update with the weighted residual
    P = (np.eye(2) - K @ H) @ P
    estimates.append(x[0])

print(f"Detection error: {np.sqrt(np.mean((z - truth) ** 2)):.2f} px")
print(f"Kalman error: {np.sqrt(np.mean((np.array(estimates) - truth) ** 2)):.2f} px")
print(f"Estimated velocity: {x[1]:.2f} px/frame")

Среднеквадратичная ошибка составляет 4.50 px для исходных детекций и 2.48 px для отфильтрованных оценок, а оценённая скорость — 2.98 px за кадр, что близко к истинным 3 px, хотя скорость ни разу не измерялась.

Родственные фильтры#

Стандартный фильтр Калмана предполагает линейные модели движения и измерений с гауссовским шумом. Родственные методы ослабляют эти допущения:

• Расширенный фильтр Калмана (EKF): на каждом шаге линеаризует нелинейные модели вокруг текущей оценки с помощью матриц Якоби. Это стандартный выбор для GPS и инерциальной навигации.
• Несмещённый фильтр Калмана (UKF): пропускает небольшой набор тщательно выбранных пробных точек через нелинейную модель. Обычно это точнее линеаризации, и производные не требуются.
• Фильтр частиц: представляет распределение состояния множеством взвешенных случайных выборок. Это позволяет работать с негауссовскими и мультимодальными распределениями, например когда робот не знает, в каком из двух коридоров находится, но требует значительно больших вычислительных затрат.
• Скрытая марковская модель (HMM): использует ту же рекурсию предсказания и обновления для дискретных состояний; фильтр Калмана — её аналог для непрерывных состояний с линейной динамикой и гауссовским шумом.

Применение за пределами отслеживания#

• Робототехника и объединение данных датчиков: роботы объединяют одометрию колёс, IMU, лидар и камеры в единую оценку позы, а методы на основе EKF — классический подход к одновременной локализации и построению карты.
• Дополненная реальность: гарнитуры фильтруют измерения положения головы, чтобы виртуальные объекты оставались неподвижными при малой задержке.
• Экономика и обработка сигналов: аналитики используют сглаживание Калмана, чтобы оценивать ненаблюдаемые тренды по зашумлённым временным рядам.
• Навигация: самолёты, корабли, дроны и телефоны объединяют данные GPS и инерциальных датчиков, чтобы положение оставалось стабильным между спутниковыми коррекциями. Навигация стала и первым крупным применением фильтра: в начале 1960-х Стэнли Ф. Шмидт из NASA Ames адаптировал его для оценки нелинейных траекторий навигационным компьютером Apollo.