Extended Kalman Filter (EKF)

Расширенный фильтр Калмана (EKF) — это рекурсивный алгоритм, оценивающий состояние системы с нелинейным движением или измерениями, например ориентацию дрона или положение цели, отслеживаемой радаром, который измеряет дальность и азимут. Стандартный фильтр Калмана (KF) даёт точный результат только для линейных моделей с гауссовым шумом. EKF работает с нелинейными моделями, линеаризуя их на каждом шаге относительно текущей оценки с помощью матриц Якоби, составленных из первых производных, а затем применяя привычные уравнения прогнозирования и обновления. Джеральд Смит, Стэнли Шмидт и Леонард Макги из NASA Ames разработали его в начале 1960-х для бортовой навигации аппарата, совершающего облёт Луны. Эта работа легла в основу навигационного компьютера Apollo. EKF остаётся фактическим стандартом нелинейной оценки состояния в навигационных системах, GPS-приёмниках, дронах и роботах.

Как работает EKF#

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

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

Состояние и прогнозируемое измерение проходят через истинные нелинейные функции; только для ковариации и коэффициента усиления используются линеаризованные матрицы Якоби. Если f и h линейны, матрицы Якоби совпадают с матрицами моделей, и EKF сводится к фильтру Калмана. Такое приближение работает хорошо, когда модели близки к линейным в диапазоне неопределённости, и ухудшается, если в этом диапазоне они имеют сильную кривизну.

Классический пример — радар, измеряющий дальность и азимут: цель, движущаяся по прямой в координатах x и y, описывается линейно в этих координатах, но дальность √(x² + y²) и азимут atan2(y, x) — нелинейны. Этот EKF отслеживает такую цель и оценивает её скорость, которую радар непосредственно не измеряет:

import numpy as np

rng = np.random.default_rng(0)
F = np.eye(4) + np.eye(4, k=2)  # constant velocity, state = [x, y, vx, vy], 1 s per step
Q, R = 1e-4 * np.eye(4), np.diag([1.0**2, 0.02**2])  # process noise; radar noise of 1 m and 0.02 rad
s = np.array([20.0, 50.0, 1.0, -0.5])  # true state
x, P = np.array([20.0, 50.0, 0.0, 0.0]), np.diag([1.0, 1.0, 4.0, 4.0])  # estimate starts with unknown velocity

raw_err, ekf_err = [], []
for _ in range(60):
    s = F @ s
    z = np.array([np.hypot(s[0], s[1]), np.arctan2(s[1], s[0])]) + rng.normal(0, [1.0, 0.02])  # range, bearing
    x, P = F @ x, F @ P @ F.T + Q  # predict
    r = np.hypot(x[0], x[1])
    H = np.array([[x[0] / r, x[1] / r, 0, 0], [-x[1] / r**2, x[0] / r**2, 0, 0]])  # Jacobian of h(x)
    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
    x = x + K @ (z - np.array([r, np.arctan2(x[1], x[0])]))  # update with the nonlinear residual
    P = (np.eye(4) - K @ H) @ P
    raw_err.append(np.hypot(z[0] * np.cos(z[1]) - s[0], z[0] * np.sin(z[1]) - s[1]))
    ekf_err.append(np.hypot(x[0] - s[0], x[1] - s[1]))

print(f"Mean position error: raw radar {np.mean(raw_err):.2f} m, EKF {np.mean(ekf_err):.2f} m")
print(f"Estimated velocity: ({x[2]:.2f}, {x[3]:.2f}) m/s, true (1.00, -0.50)")

Преобразование каждого исходного измерения дальности и азимута в координаты x и y даёт среднюю ошибку положения 1,43 м за 60 шагов, тогда как ошибка оценки EKF составляет 0,57 м; кроме того, фильтр восстанавливает скорость как (1,01, −0,50) м/с.

Ограничения и варианты#

EKF не является оптимальным оценивателем. Если начальная оценка сильно ошибочна или модели в значительной степени нелинейны, ошибки линеаризации могут привести к чрезмерной уверенности фильтра, а затем и к расходимости, поэтому инженеры часто с запасом увеличивают шум процесса. Матрицы Якоби нужно выводить вручную или вычислять с помощью автоматического дифференцирования; неверная производная часто становится причиной незаметных сбоев. Невязки для углов, таких как азимут или курс, также нужно приводить к диапазону от −π до π. Эти проблемы решают несколько вариантов фильтра: итеративный EKF повторно линеаризует модель измерений во время обновления, а EKF с ошибкой состояния оценивает небольшую поправку относительно номинального состояния. Его часто используют для отслеживания 3D-ориентации с помощью кватернионов в инерциальной навигации.

EKF и другие фильтры#

Выбор фильтра зависит от того, насколько модели отличаются от линейных, а также от формы распределения неопределённости:

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

Примеры применения в реальных условиях#

• Дроны и летательные аппараты: Оцениватель EKF2 автопилота PX4 выполняет EKF с 24 состояниями, объединяя данные IMU, GPS, магнитометра, барометра и других датчиков для оценки ориентации, скорости, положения, смещений датчиков, магнитного поля Земли и ветра.
• Робототехника: Пакет ROS robot_localization предоставляет узлы EKF и UKF, которые объединяют данные одометрии колёс, IMU и GPS; EKF-SLAM — классический подход к одновременной локализации и построению карты. Визуально-инерциальная одометрия на основе EKF, например фильтр Калмана с ограничениями для нескольких состояний (MSCKF), отслеживает позу камеры по признакам изображения и показаниям IMU.
• Автомобильное слияние данных датчиков: Системы помощи водителю и автономные транспортные средства объединяют измерения дальности и азимута радара с детекциями камеры и лидара, чтобы отслеживать положение и скорость окружающих автомобилей.
• Локализация с помощью камеры: Одна камера измеряет только направление на объект. В конвейерах, которые размещают обнаруженные объекты на карте, детекции Ultralytics YOLO26 используются как измерения EKF: центр каждой рамки — это положение в пикселях, соответствующее нелинейной проекции 3D-положения объекта через модель камеры. Фильтр объединяет эти данные с дальностью радара либо данными GPS и IMU платформы камеры.