Расширенный фильтр Калмана

Расширенный фильтр Калмана (Extended Kalman Filter, EKF) — это рекурсивный алгоритм оптимальной оценки состояния нелинейной динамической системы по зашумлённым измерениям. Является обобщением классического фильтра Калмана на случай, когда модель эволюции системы и/или модель измерений описываются нелинейными функциями. EKF широко применяется в задачах навигации, управления, компьютерного зрения и робототехники.

Принцип работы

В отличие от линейного фильтра Калмана, который предполагает, что все уравнения системы линейны, EKF работает с нелинейными моделями путём их локальной линеаризации в окрестности текущей оценки состояния. Линеаризация выполняется с помощью разложения в ряд Тейлора первого порядка (вычисления матрицы Якоби). После линеаризации алгоритм применяет стандартные уравнения фильтра Калмана.

Модели системы

Рассматривается нелинейная система, описываемая следующими уравнениями:

• Уравнение динамики (модель процесса): \( x_k = f(x_{k-1}, u_{k-1}) + w_{k-1} \), где \( x_k \) — вектор состояния в момент \( k \), \( f \) — нелинейная функция перехода, \( u_{k-1} \) — вектор управления, \( w_{k-1} \) — шум процесса (обычно предполагается гауссовским с нулевым средним и ковариацией \( Q_k \)).
• Уравнение измерений: \( z_k = h(x_k) + v_k \), где \( z_k \) — вектор измерений, \( h \) — нелинейная функция измерений, \( v_k \) — шум измерений (гауссовский с нулевым средним и ковариацией \( R_k \)).

Этапы алгоритма

Алгоритм EKF состоит из двух основных этапов, повторяемых на каждом временном шаге \( k \):

1. Этап прогноза (предсказания):

• Прогноз состояния: \( \hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1}, u_{k-1}) \). Оценка состояния на текущий момент вычисляется путём применения нелинейной функции \( f \) к предыдущей оценке.
• Прогноз ковариации ошибки: \( P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k \), где \( F_k = \frac{\partial f}{\partial x} \bigg|_{\hat{x}_{k-1|k-1}} \) — матрица Якоби функции \( f \), вычисленная в точке предыдущей оценки. \( P \) — ковариационная матрица ошибки оценки.

1. Этап коррекции (обновления):

• Вычисление инновации (невязки): \( y_k = z_k - h(\hat{x}_{k|k-1}) \). Разница между фактическим измерением и его прогнозом, полученным через нелинейную функцию \( h \).
• Вычисление ковариации инновации: \( S_k = H_k P_{k|k-1} H_k^T + R_k \), где \( H_k = \frac{\partial h}{\partial x} \bigg|_{\hat{x}_{k|k-1}} \) — матрица Якоби функции \( h \), вычисленная в точке прогноза состояния.
• Вычисление коэффициента усиления Калмана: \( K_k = P_{k|k-1} H_k^T S_k^{-1} \). Этот коэффициент определяет, насколько сильно следует доверять новому измерению по сравнению с прогнозом модели.
• Обновление оценки состояния: \( \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k y_k \). Коррекция прогноза на основе взвешенной инновации.
• Обновление ковариации ошибки: \( P_{k|k} = (I - K_k H_k) P_{k|k-1} \).

Особенности и ограничения

Преимущества

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

Недостатки

• Линеаризация первого порядка: При сильной нелинейности системы или при больших неопределённостях в начальном состоянии линеаризация может приводить к значительным ошибкам и даже к расходимости фильтра (когда оценка состояния перестаёт соответствовать реальному состоянию).
• Вычисление матриц Якоби: Требует аналитического или численного вычисления производных, что может быть сложным для систем с разрывными или сложными функциями. Ошибки в вычислении Якобиана также могут ухудшить работу фильтра.
• Предположение о гауссовости шумов: Как и классический фильтр Калмана, EKF предполагает, что шумы процесса и измерений являются гауссовскими. На практике это допущение может нарушаться, что снижает оптимальность оценки.
• Чувствительность к начальным условиям: Неверная инициализация ковариационной матрицы \( P_0 \) или начального состояния \( \hat{x}_0 \) может привести к медленной сходимости или расходимости.

Применение

Расширенный фильтр Калмана используется в широком спектре задач, где требуется оценивать скрытые переменные в нелинейных динамических системах.

Навигация и системы управления

• Глобальные навигационные спутниковые системы (ГНСС): EKF применяется для совместной обработки данных от GPS/ГЛОНАСС и инерциальных навигационных систем (ИНС). Он позволяет сглаживать ошибки ИНС и уточнять позицию, скорость и ориентацию объекта.
• Автономные транспортные средства: В системах управления беспилотными автомобилями, дронами и роботами EKF используется для слияния данных от различных датчиков (лидаров, радаров, камер, одометрии) для построения карты окружения и оценки собственного положения (SLAM — Simultaneous Localization and Mapping).
• Авиация и космонавтика: В системах управления полётом летательных аппаратов и космических кораблей для оценки углов ориентации, скорости и положения на основе данных от гироскопов, акселерометров, магнитометров и звёздных датчиков.

Робототехника

• Оценка положения и ориентации (Pose Estimation): EKF используется для фильтрации данных с инерциальных измерительных блоков (IMU) и датчиков внешней среды для получения стабильной оценки положения и ориентации робота в пространстве.
• Отслеживание объектов: В задачах слежения за движущимися объектами (например, пешеходами или другими роботами) EKF позволяет предсказывать их траекторию на основе зашумлённых измерений.

Экономика и финансы

• Оценка параметров моделей: EKF применяется для оценки скрытых переменных в нелинейных экономических и финансовых моделях, таких как стохастические модели волатильности или модели ценообразования опционов.

Обработка сигналов

• Оценка фазы и частоты: В системах связи и радиолокации EKF используется для отслеживания несущей частоты и фазы сигнала в условиях помех.

Сравнение с другими фильтрами

История

Основы фильтра Калмана были заложены Рудольфом Калманом в 1960 году для линейных систем. Вскоре после этого, в начале 1960-х годов, были предложены первые варианты расширения алгоритма на нелинейные системы. Одним из ключевых вкладов стала работа Стэнли Шмидта (Stanley Schmidt), который в 1966 году разработал версию EKF для задачи наведения космического аппарата «Аполлон». Этот алгоритм, известный как «фильтр Шмидта-Калмана», успешно применялся в программе «Аполлон» для оценки траектории и навигации. С тех пор EKF стал одним из наиболее широко используемых алгоритмов в теории управления и обработке сигналов.

Источники

1. Kalman, R. E. (1960). A New Approach to Linear Filtering and Prediction Problems. Journal of Basic Engineering, 82(1), 35-45.
2. Schmidt, S. F. (1966). Application of State-Space Methods to Navigation Problems. Advances in Control Systems, 3, 293-340.
3. Maybeck, P. S. (1979). Stochastic Models, Estimation, and Control (Vol. 1). Academic Press.
4. Welch, G., & Bishop, G. (2006). An Introduction to the Kalman Filter. University of North Carolina at Chapel Hill.
5. Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.