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

Основополагающие статьи, заложившие математические основы фильтров типа Калмана, были опубликованы в период с 1959 по 1961 год^[3][4][5]. Фильтр Калмана является оптимальным линейным оценивателем для линейных моделей систем с добавочным независимым белым шумом как в переходной, так и в измерительной подсистемах. Однако большинство реальных инженерных систем являются нелинейными, поэтому были предприняты попытки применить данный метод фильтрации к нелинейным системам; основная часть работ проводилась в центре NASA Ames^[6][7]. EKF применяет методы анализа, а именно много переменные разложения в ряд Тейлора, для линеаризации модели в рабочей точке. Если модель системы неизвестна или некорректна, для оценки состояния используют методы Монте-Карло, в частности частичные фильтры, которые появились раньше EKF, но требуют гораздо больше вычислительных ресурсов для систем средней размерности.

В расширенном фильтре Калмана модели перехода состояния и наблюдений могут быть не линейными функциями состояния, а дифференцируемыми функциями:

${\displaystyle {\boldsymbol {x}}_{k}=f({\boldsymbol {x}}_{k-1},{\boldsymbol {u}}_{k-1})+{\boldsymbol {w}}_{k-1}}$

${\displaystyle {\boldsymbol {z}}_{k}=h({\boldsymbol {x}}_{k})+{\boldsymbol {v}}_{k}}$

Здесь w_k и v_k — шумы процесса и наблюдения, предполагаемые как нулевого среднего, многомерные нормальные с ковариациями Q_k и R_k соответственно. u_k — управляющий вектор.

Функция f используется для вычисления априорного состояния из предыдущей оценки, а функция h — для априорного измерения по предсказанному состоянию. Однако f и h не могут быть напрямую применены к ковариации — вместо этого рассчитывается матрица Якоби по частным производным.

На каждом шаге времени Якобиан вычисляется в текущем прогнозируемом состоянии, после чего эти матрицы используются в уравнениях фильтра Калмана. Такой подход линеаризирует нелинейные функции вокруг текущей оценки.

Обозначение ${\displaystyle {\hat {\mathbf {x} }}_{n\mid m}}$ означает оценку ${\displaystyle \mathbf {x} }$ в момент времени n по данным наблюдений до и включая момент m ≤ n.

Оценка состояния по прогнозу	${\displaystyle {\hat {\boldsymbol {x}}}_{k|k-1}=f({\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k-1})}$
Оценка ковариации прогноза	${\displaystyle {\boldsymbol {P}}_{k|k-1}={{\boldsymbol {F}}_{k}}{\boldsymbol {P}}_{k-1|k-1}{{\boldsymbol {F}}_{k}^{T}}+{\boldsymbol {Q}}_{k-1}}$

Инновация или остаток измерения	${\displaystyle {\tilde {\boldsymbol {y}}}_{k}={\boldsymbol {z}}_{k}-h({\hat {\boldsymbol {x}}}_{k|k-1})}$
Ковариация инновации (остатка)	${\displaystyle {\boldsymbol {S}}_{k}={{\boldsymbol {H}}_{k}}{\boldsymbol {P}}_{k|k-1}{{\boldsymbol {H}}_{k}^{T}}+{\boldsymbol {R}}_{k}}$
(Почти оптимальный) калмановский коэффициент усиления	${\displaystyle {\boldsymbol {K}}_{k}={\boldsymbol {P}}_{k|k-1}{{\boldsymbol {H}}_{k}^{T}}{\boldsymbol {S}}_{k}^{-1}}$
Скорректированная оценка состояния	${\displaystyle {\hat {\boldsymbol {x}}}_{k|k}={\hat {\boldsymbol {x}}}_{k|k-1}+{\boldsymbol {K}}_{k}{\tilde {\boldsymbol {y}}}_{k}}$
Скорректированная оценка ковариации	${\displaystyle {\boldsymbol {P}}_{k|k}=({\boldsymbol {I}}-{\boldsymbol {K}}_{k}{{\boldsymbol {H}}_{k}}){\boldsymbol {P}}_{k|k-1}}$

где матрицы якобианов перехода состояния и наблюдения определяются как:

${\displaystyle {{\boldsymbol {F}}_{k}}=\left.{\frac {\partial f}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k-1}}}$

${\displaystyle {{\boldsymbol {H}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}}$

В отличие от линейного фильтра Калмана, расширенный фильтр Калмана в общем случае не является оптимальным оценивателем (он оптимален только для линейных моделей измерения и перехода состояния — в этом случае EKF совпадает с обычным фильтром Калмана). При ошибочной начальной оценке состояния или при неточной модели процесса фильтр может быстро расходиться из-за ошибок линеаризации. Другой недостаток EKF — склонность к недооценке истинной ковариации (переменной) ошибки, вследствие чего он может стать статистически неконсистентным без добавления «стабилизирующего шума»^[8].

Следует учесть бесконечномерную природу общей нелинейной задачи фильтрации и ограниченность простого оценивания по среднему и ковариации для представления оптимального фильтра. Кроме того, EKF может давать плохие результаты даже для простых одномерных систем, таких как кубический сенсор^[9], где оптимальный фильтр может быть бимодальным^[10] и неописуемым единственной оценкой по среднему и ковариации, а также для задач с квадратичным сенсором^[11]. В таких случаях изучаются проекционные фильтры (projection filters)^[12] и полные частичные фильтры (particle filters).

Тем не менее, EKF часто обеспечивает удовлетворительную точность оценки и считается де-факто стандартом в задачах навигации и GPS.

Модель

${\displaystyle {\begin{aligned}{\dot {\mathbf {x} }}(t)&=f{\bigl (}\mathbf {x} (t),\mathbf {u} (t){\bigr )}\\\mathbf {z} (t)&=h{\bigl (}\mathbf {x} (t){\bigr )}\end{aligned}}}$

Инициализация

${\displaystyle {\hat {\mathbf {x} }}(t_{0})=E{\bigl [}\mathbf {x} (t_{0}){\bigr ]},\quad \mathbf {P} (t_{0})=Var{\bigl [}\mathbf {x} (t_{0}){\bigr ]}}$

Прогноз-коррекция

${\displaystyle {\begin{aligned}{\dot {\hat {\mathbf {x} }}}(t)&=f{\bigl (}{\hat {\mathbf {x} }}(t),\mathbf {u} (t){\bigr )}+\mathbf {K} (t){\Bigl (}\mathbf {z} (t)-h{\bigl (}{\hat {\mathbf {x} }}(t){\bigr )}{\Bigr )}\\{\dot {\mathbf {P} }}(t)&=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} (t)^{T}-\mathbf {K} (t)\mathbf {H} (t)\mathbf {P} (t)+\mathbf {Q} (t)\\\mathbf {K} (t)&=\mathbf {P} (t)\mathbf {H} (t)^{T}\mathbf {S} (t)^{-1}\\\mathbf {F} (t)&=\left.{\frac {\partial f}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t),\mathbf {u} (t)}\\\mathbf {H} (t)&=\left.{\frac {\partial h}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t)}\end{aligned}}}$

В отличие от дискретного EKF, предсказание и коррекция в непрерывном времени являются связанными процессами^[13].

Большинство физических систем описываются как непрерывные модели, в то время как измерения часто фиксируются дискретно с помощью цифрового процессора. Поэтому система описывается так:

${\displaystyle {\begin{aligned}{\dot {\mathbf {x} }}(t)&=f{\bigl (}\mathbf {x} (t),\mathbf {u} (t){\bigr )}+\mathbf {w} (t),\qquad \mathbf {w} (t)\sim {\mathcal {N}}(\mathbf {0} ,\mathbf {Q} (t))\\\mathbf {z} _{k}&=h(\mathbf {x} _{k})+\mathbf {v} _{k},\qquad \mathbf {v} _{k}\sim {\mathcal {N}}(\mathbf {0} ,\mathbf {R} _{k})\end{aligned}}}$

где ${\displaystyle \mathbf {x} _{k}=\mathbf {x} (t_{k})}$.

Инициализация

${\displaystyle {\hat {\mathbf {x} }}_{0|0}=E{\bigl [}\mathbf {x} (t_{0}){\bigr ]},\quad \mathbf {P} _{0|0}=E{\bigl [}(\mathbf {x} (t_{0})-{\hat {\mathbf {x} }}(t_{0}))(\mathbf {x} (t_{0})-{\hat {\mathbf {x} }}(t_{0}))^{T}{\bigr ]}}$

Коррекция

${\displaystyle \mathbf {K} _{k}=\mathbf {P} _{k|k-1}\mathbf {H} _{k}^{T}(\mathbf {H} _{k}\mathbf {P} _{k|k-1}\mathbf {H} _{k}^{T}+\mathbf {R} _{k})^{-1}}$

${\displaystyle {\hat {\mathbf {x} }}_{k|k}={\hat {\mathbf {x} }}_{k|k-1}+\mathbf {K} _{k}(\mathbf {z} _{k}-h({\hat {\mathbf {x} }}_{k|k-1}))}$

${\displaystyle \mathbf {P} _{k|k}=(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k})\mathbf {P} _{k|k-1}}$

где

${\displaystyle {\textbf {H}}_{k}=\left.{\frac {\partial h}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}_{k|k-1}}}$

Уравнения коррекции идентичны таковым в дискретном EKF.

Вышеприведённая рекурсия — это EKF первого порядка. EKF высших порядков получают, сохраняя больше членов разложения в ряд Тейлора. Описаны EKF второго и третьего порядка^[14]. Однако значимые преимущества проявляются только при малых уровнях измерительного шума.

Стандартная формулировка EKF предполагает аддитивность шумов процесса и измерения, однако это условие не обязательно для его построения^[15]. Рассмотрим более общий вид системы:

${\displaystyle {\boldsymbol {x}}_{k}=f({\boldsymbol {x}}_{k-1},{\boldsymbol {u}}_{k-1},{\boldsymbol {w}}_{k-1})}$

${\displaystyle {\boldsymbol {z}}_{k}=h({\boldsymbol {x}}_{k},{\boldsymbol {v}}_{k})}$

Тогда ${\displaystyle {\boldsymbol {w}}_{k-1}}$ и ${\displaystyle {\boldsymbol {v}}_{k}}$ являются многомерными нормальными шумами с ковариациями ${\displaystyle {\boldsymbol {Q}}_{k-1}}$ и ${\displaystyle {\boldsymbol {R}}_{k}}$. Уравнения ковариации прогноза и инновации:

${\displaystyle {\boldsymbol {P}}_{k|k-1}={{\boldsymbol {F}}_{k-1}}{{\boldsymbol {P}}_{k-1|k-1}}{{\boldsymbol {F}}_{k-1}^{T}}+{{\boldsymbol {L}}_{k-1}}{{\boldsymbol {Q}}_{k-1}}{{\boldsymbol {L}}_{k-1}^{T}}}$

${\displaystyle {\boldsymbol {S}}_{k}={{\boldsymbol {H}}_{k}}{{\boldsymbol {P}}_{k|k-1}}{{\boldsymbol {H}}_{k}^{T}}+{{\boldsymbol {M}}_{k}}{{\boldsymbol {R}}_{k}}{{\boldsymbol {M}}_{k}^{T}}}$

где матрицы ${\displaystyle {\boldsymbol {L}}_{k-1}}$ и ${\displaystyle {\boldsymbol {M}}_{k}}$ — Якобианы:

${\displaystyle {{\boldsymbol {L}}_{k-1}}=\left.{\frac {\partial f}{\partial {\boldsymbol {w}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k-1}}}$

${\displaystyle {{\boldsymbol {M}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {v}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}}$

Предсказанная оценка состояния и остаток измерения вычисляются по согласно среднему шумов (нулю). Все остальные шаги совпадают с addитивной формулировкой EKF.

В ряде случаев модель наблюдения нелинейной системы не может быть явно выражена относительно ${\displaystyle {\boldsymbol {z}}_{k}}$, но может быть записана как неявная функция:

${\displaystyle h({\boldsymbol {x}}_{k},{\boldsymbol {z}}'_{k})={\boldsymbol {0}}}$

где ${\displaystyle {\boldsymbol {z}}_{k}={\boldsymbol {z}}'_{k}+{\boldsymbol {v}}_{k}}$ — зашумлённые наблюдения.

Обычный EKF можно применять с заменой^[16][17]:

${\displaystyle {{\boldsymbol {R}}_{k}}\leftarrow {{\boldsymbol {J}}_{k}}{{\boldsymbol {R}}_{k}}{{\boldsymbol {J}}_{k}^{T}}}$

${\displaystyle {\tilde {\boldsymbol {y}}}_{k}\leftarrow -h({\hat {\boldsymbol {x}}}_{k|k-1},{\boldsymbol {z}}_{k})}$

где

${\displaystyle {{\boldsymbol {J}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {z}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1},\,{\boldsymbol {z}}_{k}}}$

В этом случае исходная ковариационная матрица наблюдения ${\displaystyle {{\boldsymbol {R}}_{k}}}$ преобразуется, а инновация ${\displaystyle {\tilde {\boldsymbol {y}}}_{k}}$ определяется иначе. Матрица Якоби ${\displaystyle {{\boldsymbol {H}}_{k}}}$ вычисляется по неявной модели наблюдения.

Итеративный EKF совершенствует линеаризацию, рекурсивно изменяя центр разложения Тейлора, что сокращает ошибку линеаризации с увеличением вычислительных затрат^[17].

«Робастный» EKF строится линеаризацией сигнальной модели вокруг текущей оценки и применением линейного фильтра Калмана для предсказания. Такой подход обеспечивает локальную оптимальность, но не гарантирует устойчивости, так как решения уравнения Риккати не всегда положительно определены. Для повышения устойчивости можно использовать псевдоалгебраическую технику Риккати^[18], подменяя оптимальность на устойчивость, выбирая положительно определённое решение псевдоалгебраического уравнения Риккати для выбора усиления.

Другой подход — применение H-инфинити-теории из робастного управления. Робастный фильтр формируется добавлением положительно определённого члена в проектное уравнение Риккати^[19]. Добавочный член регулируется, обеспечивая компромисс между критериями СКО и пиковых ошибок.

Инвариантный расширенный фильтр Калмана (IEKF) — модифицированный EKF для нелинейных систем с симметриями (инвариантностями). Он объединяет достоинства EKF и симметрия-сохраняющих фильтров. Вместо линейной коррекции по выходной ошибке IEKF применяет геометрически адаптированную коррекцию по инвариантной ошибке, а матрица усиления рассчитывается по инвариантной ошибке состояния. Преимущество IEKF — сходимость ковариационных уравнений к постоянным значениям для большей области траекторий, что ускоряет сходимость оценки по сравнению с обычным EKF.

Несцентный фильтр Калмана (UKF) — нелинейный калмановский фильтр, предлагающий улучшения по сравнению с EKF. В UKF аппроксимация распределения вероятности осуществляется детерминированной выборкой точек, представящих заданное распределение как нормальное. Нелинейное преобразование этих точек служит для оценки апостериорного распределения и его моментов. Это преобразование называется несцентным преобразованием. UKF обычно более робастен и точнее EKF в оценке ошибки во всех направлениях.

«Расширенный фильтр Калмана (EKF) — вероятно, наиболее широко применяемый алгоритм оценивания для нелинейных систем. Однако опыт показывает, что его сложно реализовать и настраивать, и он надёжен только для систем, близких к линейным на временах между обновлениями. Многие сложности связаны именно с линеаризацией.»^[1]

Существуют данные, что некоторые варианты UKF уступают по точности EKF второго порядка (SOEKF), также называемому аугментированным фильтром Калмана^[20]. SOEKF был предложен за 35 лет до UKF (см. Bass и др., 1966^[21]). Основная сложность практической реализации любых калмановских фильтров для нелинейных систем связана с вопросами численной устойчивости^[22]. UKF также использует линеаризацию (методы линейной регрессии). Для UKF устойчивость может страдать из-за аппроксимации корня из ковариационной матрицы, а для EKF и SOEKF — из-за ошибок ряда Тейлора.

Ансамблевый фильтр Калмана (Ensemble Kalman filter), предложенный Эвенсеном в 1994 году, появился до UKF, но обладает преимуществом: число ансамблевых членов может быть меньше размерности состояния, что делает его применимым к очень большим системам (например, к моделированию атмосферы размером в миллиарды переменных).

Предложен нечёткий фильтр Калмана с новым методом представления распределения возможностей (possibility distributions), где вероятностные распределения заменяются распределениями возможностей для получения по-настоящему «возможностного» фильтра, что позволяет использовать несимметричные шумы процесса и измерения, а также большие моделирующие ошибки^[23].

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

В 2017 году значительное внимание уделялось анализу сходимости и состоятельности (англ. consistency) фильтра. Были представлены и доказаны свойства сходимости для инвариантного EKF (Invariant-EKF) в задачах одновременной локализации и построения карты (SLAM) без ограничительных предположений о вычислении якобианов в точке истинного состояния^[24]. Другие работы предлагали условия для выбора параметров фильтра, чтобы гарантировать состоятельность оценок, при которой расчётная ковариация ошибки соответствует реальной среднеквадратичной ошибке^[25].

В 2018 году продолжалась разработка модификаций EKF, направленных на улучшение его работы в сложных условиях. В частности, для систем с неопределённостью параметров шума развивались адаптивные подходы (AEKF), в которых ковариационные матрицы шумов обновляются рекурсивно^[26], а для сетевых систем были предложены консенсусные расширенные фильтры Калмана (CEKF) с целью улучшения наблюдаемости^[27].

В 2019 году ключевой проблемой стала вычислительная устойчивость EKF, связанная с возможной потерей положительной определённости ковариационной матрицы. Для её решения был предложен метод «стабильного расширенного фильтра Калмана» (англ. Stable Extended Kalman Filter, SEKF), использующий матричное разложение для построения фильтра на основе квадратного корня из ковариационной матрицы, что гарантирует сохранение её симметричности и положительной определённости^[28]. В качестве другого метода обеспечения численной стабильности предлагалось использовать QR-разложение для вычисления матрицы усиления Калмана^[29].

Кроме того, для снижения высоких вычислительных затрат, связанных с необходимостью линеаризации модели на каждом шаге, в 2019 году был представлен метод условной линеаризации. Его суть заключается в выполнении линеаризации не на каждой итерации, а только тогда, когда ожидаемые изменения в матрицах Якоби превышают заданный порог, что позволяет сократить количество вычислений без существенного увеличения ошибок оценки^[30].

В этот период исследования были направлены на повышение точности EKF, анализ его поведения в сложных условиях и расширение на новые классы систем. Были предложены подходы для повышения точности оценок, использующие методы, которые заменяют отбрасываемые члены ряда Тейлора остаточным членом, идентифицируемым пошагово с помощью метода наименьших квадратов^[31]. Продолжилось развитие инвариантного EKF (IEKF) для систем, обладающих симметриями. Были проанализированы его ключевые свойства, такие как лог-линейность ошибки, которая устраняет ошибку линеаризации на этапе прогноза, и предоставлены гарантии сходимости для определённого класса систем^[32][33].

Значительное внимание было уделено анализу ограничений EKF. В 2021 году было показано, что при оценке временной задержки в задачах слияния данных с нескольких датчиков включение задержки в вектор состояния приводит к структурным проблемам, вызывающим смещение и несостоятельность (англ. inconsistency) оценок^[34]. Также было исследовано поведение фильтра в сетевых системах с передачей данных по событию (англ. event-triggered). Было доказано, что при определённых начальных условиях ошибка оценки и её ковариационная матрица остаются ограниченными^[35].

Ключевым теоретическим достижением 2022 года стало расширение EKF на полулинейные бесконечномерные системы, что актуально для процессов, описываемых уравнениями в частных производных. В рамках этой работы была доказана корректность (англ. well-posedness) уравнений фильтра и проанализирована локальная экспоненциальная устойчивость динамики ошибки оценивания^[36].

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

В 2023 году было введено понятие «структурной состоятельности» (англ. structural consistency), связанное с корректностью работы фильтра при различных формах записи задачи, и проанализировано его выполнение в задачах навигации^[37]. Продолжилась разработка гибридных методов: для обработки нелинейных полётных данных была предложена комбинация EKF с фильтром частиц (EKF+PF)^[38], а для компенсации ошибок при работе с нестационарными сигналами исследовалась система, объединяющая EKF и адаптивный цифровой фильтр (АЦФ) с алгоритмом NLMS^[39]. Хотя сама идея объединения EKF с адаптивными фильтрами не нова, в работе была представлена её новая реализация^[39].

В 2024 году была предложена новая модификация — расширенный фильтр Калмана высокого порядка (англ. Rounding error-based Extended Kalman Filter, REKF), который использует статистические свойства ошибок округления для замены отбрасываемых членов ряда Тейлора и повышения точности оценки^[40]. Кроме того, EKF был применён для решения обратной оптимальной задачи управления в реальном времени, для чего была разработана новая постановка задачи и представлены «первые в своём роде теоретические гарантии ошибок» для таких систем^[41].

Исследования 2024—2025 годов также были сосредоточены на итеративных и адаптивных подходах. Был представлен итерированный инвариантный EKF (англ. Iterated Invariant Extended Kalman Filter, IterIEKF), использующий метод Гаусса — Ньютона для уточнения оценки на этапе обновления^[42]. Для работы с нестационарными и негауссовскими шумами была разработана адаптивная версия — англ. Adaptive Dual Augmented EKF, ADAEKF, включающая механизм автоматизированной оптимизации гиперпараметров^[43].