Фильтр Калмана

Фильтр Калмана отслеживает оценочное состояние системы и дисперсию или неопределенность оценки. Оценка обновляется с использованием модели перехода состояний и измерений. обозначает оценку состояния системы на временном шаге k до того, как k-е измерение y _k было учтено; - соответствующая неопределенность.${\ displaystyle {\ hat {x}} _ {k \ mid k-1}}$${\ displaystyle P_ {k \ mid k-1}}$

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

Фильтр Калмана находит множество применений в технике. Обычно применяется для наведения, навигации и управления транспортными средствами, особенно самолетами, космическими кораблями и динамически позиционируемыми кораблями. ^[1] Кроме того, фильтр Калмана - широко применяемая концепция в анализе временных рядов, используемая в таких областях, как обработка сигналов и эконометрика . Фильтры Калмана также являются одной из основных тем в области планирования и управления движением роботов и могут использоваться при оптимизации траектории . ^[2] Фильтр Калмана также работает для моделирования центральной нервной системы.контроль движения. Из-за временной задержки между выдачей моторных команд и получением сенсорной обратной связи использование фильтра Калмана поддерживает реалистичную модель для оценки текущего состояния моторной системы и выдачи обновленных команд. ^[3]

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

Оптимальность фильтра Калмана предполагает, что ошибки являются гауссовскими . По словам Рудольфа Э. Кальмана : «Таким образом, в отношении случайных процессов делаются следующие предположения: физические случайные явления можно рассматривать как связанные с первичными случайными источниками, возбуждающими динамические системы. Первичные источники считаются независимыми гауссовскими случайными процессами. с нулевым средним; динамические системы будут линейными ». ^[4] Несмотря на то, что независимо от гауссовости, если известны ковариации процесса и измерения, фильтр Калмана является наилучшей возможной линейной оценкой в смысле минимальной среднеквадратичной ошибки . ^[5]

Расширения и обобщение к способу также были разработаны, например, расширенный фильтр Калмана и неароматизированный фильтр Калмана , которые работают на нелинейных системах . Базовая модель является скрытой марковской моделью , где пространство состояний из латентных переменных является непрерывным и всеми скрытыми и наблюдаемым переменными имеет гауссово распределение. Кроме того , фильтр Калмана был успешно использован в слиянии нескольких датчиков , ^[6] и распределенных сетей датчиков для разработки распределенных или консенсуса фильтр Калмана. ^[7]

История [ править ]

Фильтр назван в честь венгерского эмигранта Рудольфа Э. Кальмана , хотя Торвальд Николай Тиле ^{[8] [9]} и Питер Сверлинг разработали аналогичный алгоритм ранее. Ричард С. Бьюси из Лаборатории прикладной физики Джона Хопкинса внес свой вклад в теорию, в результате чего ее иногда называют фильтром Калмана – Бьюси. Стэнли Ф. Шмидту обычно приписывают разработку первой реализации фильтра Калмана. Он понял, что фильтр можно разделить на две отдельные части, одна часть для периодов времени между выходами датчиков, а другая часть для включения измерений. ^[10] Это было во время визита Кальмана вИсследовательский центр NASA Ames, в котором Шмидт увидел применимость идей Кальмана к нелинейной задаче оценки траектории для программы Apollo, что привело к ее включению в навигационный компьютер Apollo. Этот фильтр Калмана был впервые описан и частично разработан в технических статьях Сверлинга (1958), Калмана (1960) и Калмана и Бьюси (1961).

Компьютер Apollo использовал 2k RAM магнитного сердечника и трос 36k [...]. ЦП был построен из микросхем [...]. Тактовая частота была ниже 100 кГц [...]. Тот факт, что инженеры Массачусетского технологического института смогли упаковать такое хорошее программное обеспечение (одно из самых первых применений фильтра Калмана) в такой крошечный компьютер, действительно примечателен.

-  Интервью с Джеком Креншоу, Мэтью Рид, TRS-80.org (2009) [1]

Кальман фильтры были жизненно важны в реализации навигационных систем ВМС США ядерных баллистических ракет подводных лодок , а также в руководящих и навигационных системах крылатых ракет , такие как ВМС США Томагавка ракета а ВВС США «s Air крылатая ракета . Они также используются в системах наведения и навигации многоразовых ракет-носителей, а также в системах ориентации и навигации космических кораблей, которые состыковываются с Международной космической станцией . ^[11]

Этот цифровой фильтр иногда называют фильтром Стратоновича – Калмана – Бьюси, потому что это частный случай более общего нелинейного фильтра, разработанного несколько ранее советским математиком Русланом Стратоновичем . ^{[12] [13] [14] [15]} Фактически, некоторые из уравнений линейного фильтра частного случая появились в этих статьях Стратоновича, которые были опубликованы до лета 1960 г., когда Калман встретился со Стратоновичем во время конференции в Москве. ^[16]

Обзор расчета [ править ]

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

Зашумленные данные датчиков, приближения в уравнениях, описывающих эволюцию системы, и внешние факторы, которые не учитываются, накладывают ограничения на то, насколько хорошо можно определить состояние системы. Фильтр Калмана эффективно справляется с неопределенностью из-за зашумленных данных датчика и, в некоторой степени, случайных внешних факторов. Фильтр Калмана производит оценку состояния системы как среднее от прогнозируемого состояния системы и нового измерения с использованием средневзвешенного значения . Назначение весов состоит в том, чтобы значениям с лучшей (т. Е. Меньшей) оцененной неопределенностью больше доверяли. Веса рассчитываются из ковариации, мера оценочной неопределенности прогноза состояния системы. Результатом средневзвешенного значения является новая оценка состояния, которая находится между прогнозируемым и измеренным состояниями и имеет более точную оценку неопределенности, чем любой другой. Этот процесс повторяется на каждом временном шаге, при этом новая оценка и ее ковариация определяют прогноз, используемый в следующей итерации. Это означает, что фильтр Калмана работает рекурсивно и требует только последнего «лучшего предположения», а не всей истории состояния системы для вычисления нового состояния.

Относительная достоверность измерений и оценки текущего состояния является важным соображением, и принято обсуждать отклик фильтра в терминах усиления фильтра Калмана . Коэффициент Калмана - это относительный вес, придаваемый измерениям и оценке текущего состояния, и его можно «настроить» для достижения определенной производительности. При высоком усилении фильтр придает больший вес самым последним измерениям и, следовательно, более оперативно отслеживает их. При низком усилении фильтр более точно следует прогнозам модели. В крайних случаях высокое усиление, близкое к единице, приведет к более скачкообразной расчетной траектории, в то время как низкое усиление, близкое к нулю, сгладит шум, но снизит чувствительность.

При выполнении фактических вычислений для фильтра (как обсуждается ниже) оценка состояния и ковариации кодируются в матрицы для обработки нескольких измерений, участвующих в одном наборе вычислений. Это позволяет представить линейные отношения между различными переменными состояния (такими как положение, скорость и ускорение) в любой из переходных моделей или ковариаций.

Пример приложения [ править ]

В качестве примера приложения рассмотрим задачу определения точного местоположения грузовика. Грузовик может быть оснащен устройством GPS, которое позволяет определять местоположение в пределах нескольких метров. Оценка GPS, вероятно, будет зашумленной; показания быстро «прыгают», но остаются в пределах нескольких метров от реального положения. Кроме того, поскольку ожидается, что грузовик будет следовать законам физики, его положение также можно оценить, интегрировав его скорость с течением времени, определенную путем отслеживания оборотов колес и угла поворота рулевого колеса. Это метод, известный как расплата . Как правило, точный расчет обеспечивает очень плавную оценку положения грузовика, но со временем он будет дрейфовать по мере накопления небольших ошибок.

В этом примере фильтр Калмана можно рассматривать как работающий в двух различных фазах: прогнозирование и обновление. На этапе прогнозирования старое положение грузовика будет изменено в соответствии с физическими законами движения.(динамическая модель или модель «перехода состояний»). Будет рассчитана не только новая оценка местоположения, но и новая ковариация. Возможно, ковариация пропорциональна скорости грузовика, потому что мы более не уверены в точности оценки точного определения местоположения на высоких скоростях, но очень уверены в оценке местоположения на низких скоростях. Затем на этапе обновления данные о местоположении грузовика снимаются с устройства GPS. Наряду с этим измерением возникает некоторая неопределенность, и ее ковариация относительно ковариации прогноза из предыдущей фазы определяет, насколько новое измерение повлияет на обновленный прогноз. В идеале, поскольку мертвые оценки имеют тенденцию отклоняться от реального положения,Измерение GPS должно подтянуть оценку местоположения к реальному положению, но не мешать ей до уровня шума и быстрого скачка.

Техническое описание и контекст [ править ]

Фильтр Калмана - это эффективный рекурсивный фильтр, который оценивает внутреннее состояние линейной динамической системы на основе серии зашумленных измерений. Он используется в широком диапазоне инженерных и эконометрических приложениях от радиолокационного и компьютерного зрения для оценки структурных макроэкономических моделей, ^{[17] [18]} и является важной темой в теории управления и системы управления инженерией. Вместе с линейно-квадратичным регулятором (LQR) фильтр Калмана решает линейно-квадратично-гауссовское управлениепроблема (LQG). Фильтр Калмана, линейно-квадратичный регулятор и линейно-квадратично-гауссовский регулятор являются решениями, возможно, наиболее фундаментальных проблем теории управления.

В большинстве приложений внутреннее состояние намного больше (больше степеней свободы ), чем несколько «наблюдаемых» параметров, которые измеряются. Однако, объединив серию измерений, фильтр Калмана может оценить все внутреннее состояние.

В теории Демпстера – Шейфера каждое уравнение состояния или наблюдение считается частным случаем линейной функции доверия, а фильтр Калмана - частным случаем объединения линейных функций доверия на дереве соединения или дереве Маркова . Дополнительные подходы включают фильтры убеждений, которые используют байесовские или доказательные обновления уравнений состояния.

На основе первоначальной формулировки Калмана было разработано большое количество фильтров Калмана, которые теперь называются «простым» фильтром Калмана, фильтром Калмана – Бьюси , «расширенным» фильтром Шмидта, информационным фильтром и множеством фильтров «квадратный корень». фильтры, разработанные Бирманом, Торнтоном и многими другими. Возможно, наиболее часто используемым типом очень простого фильтра Калмана является контур фазовой автоподстройки частоты , который теперь повсеместно используется в радиоприемниках, особенно в радиостанциях с частотной модуляцией (FM), телевизорах, приемниках спутниковой связи , системах космической связи и почти в любых других электронных устройствах. коммуникационное оборудование.

Базовая модель динамической системы [ править ]

Фильтры Калмана основаны на линейных динамических системах, дискретизированных во временной области. Они моделируются цепью Маркова, построенной на линейных операторах, подверженных ошибкам, которые могут включать гауссовский шум . Состояние системы представляется в виде вектора из действительных чисел . В каждый дискретный момент времениПриращение, линейный оператор применяется к состоянию для генерации нового состояния с добавлением некоторого шума и, возможно, некоторой информации от элементов управления в системе, если они известны. Затем другой линейный оператор, смешанный с большим количеством шума, генерирует наблюдаемые выходные данные из истинного («скрытого») состояния. Фильтр Калмана можно рассматривать как аналог скрытой модели Маркова с тем ключевым отличием, что скрытые переменные состояния принимают значения в непрерывном пространстве, а не в дискретном пространстве состояний, как в скрытой марковской модели. Существует сильная аналогия между уравнениями фильтра Калмана и уравнениями скрытой марковской модели. Обзор этой и других моделей дан в Roweis and Ghahramani (1999), ^[19] и Hamilton (1994), глава 13. ^[20]

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

• F _k - модель перехода между состояниями;
• H _k - модель наблюдения;
• Q _k - ковариация технологического шума;
• R _k - ковариация шума наблюдения;
• и иногда B _k , модель управляющих входов, для каждого временного шага k , как описано ниже.

Модель фильтра Калмана предполагает, что истинное состояние в момент времени k эволюционирует из состояния в ( k  - 1) в соответствии с

${\displaystyle \mathbf {x} _{k}=\mathbf {F} _{k}\mathbf {x} _{k-1}+\mathbf {B} _{k}\mathbf {u} _{k}+\mathbf {w} _{k}}$

куда

• F _k - модель перехода состояний, которая применяется к предыдущему состоянию x _{k -1} ;
• B _k - модель управляющего входа, которая применяется к вектору управления u _k ;
• ш _к является процесс шума, который предполагается извлечь из нулевого среднего многомерного нормального распределения , с ковариацией , Q _к : .${\displaystyle {\mathcal {N}}}$${\displaystyle \mathbf {w} _{k}\sim {\mathcal {N}}\left(0,\mathbf {Q} _{k}\right)}$

В момент времени k выполняется наблюдение (или измерение) z _k истинного состояния x _k в соответствии с

${\displaystyle \mathbf {z} _{k}=\mathbf {H} _{k}\mathbf {x} _{k}+\mathbf {v} _{k}}$

куда

• H _k - модель наблюдения, которая отображает истинное пространство состояний в наблюдаемое пространство и
• v _K является шум наблюдение, которое считается нулевым средним гауссовский белый шум с ковариационной R _к : .${\displaystyle \mathbf {v} _{k}\sim {\mathcal {N}}\left(0,\mathbf {R} _{k}\right)}$

Предполагается, что начальное состояние и векторы шума на каждом шаге { x ₀ , w ₁ , ..., w _k , v ₁ , ..., v _k } являются взаимно независимыми .

Многие реальные динамические системы не совсем подходят для этой модели. Фактически, немоделированная динамика может серьезно ухудшить характеристики фильтра, даже если он должен был работать с неизвестными стохастическими сигналами в качестве входных данных. Причина этого в том, что эффект немоделированной динамики зависит от входа и, следовательно, может привести алгоритм оценки к нестабильности (он расходится). С другой стороны, независимые сигналы белого шума не заставят алгоритм расходиться. Проблема различения шумов измерения и немоделированной динамики является сложной и рассматривается в теории управления в рамках робастного управления . ^{[21] [22]}

Подробности [ править ]

Фильтр Калмана - это рекурсивная оценка. Это означает, что для вычисления оценки текущего состояния необходимы только оценочное состояние из предыдущего временного шага и текущее измерение. В отличие от методов пакетной оценки, история наблюдений и / или оценок не требуется. В дальнейшем обозначение представляет собой оценку в момент времени n данных наблюдений до момента времени m ≤ n включительно .${\displaystyle {\hat {\mathbf {x} }}_{n\mid m}}$${\displaystyle \mathbf {x} }$

Состояние фильтра представлено двумя переменными:

• ${\displaystyle {\hat {\mathbf {x} }}_{k\mid k}}$, То апостериорная оценка состояния в момент времени к даны замечания вплоть до и включая в момент времени к ;
• ${\displaystyle \mathbf {P} _{k\mid k}}$, ковариационная матрица апостериорной оценки (мера предполагаемой точности оценки состояния).

Фильтр Калмана можно записать в виде одного уравнения, однако он чаще всего концептуализируется как две отдельные фазы: «Прогнозирование» и «Обновление». Фаза прогнозирования использует оценку состояния из предыдущего временного шага, чтобы произвести оценку состояния на текущем временном шаге. Эта прогнозируемая оценка состояния также известна как априорная оценка состояния, потому что, хотя это оценка состояния на текущем временном шаге, она не включает в себя информацию наблюдения из текущего временного шага. На этапе обновления текущее априорное предсказание объединяется с информацией текущего наблюдения для уточнения оценки состояния. Эта улучшенная оценка называется апостериорной оценкой состояния.

Как правило, две фазы чередуются, причем прогнозирование продвигает состояние до следующего запланированного наблюдения, а обновление включает наблюдение. Однако в этом нет необходимости; если наблюдение недоступно по какой-либо причине, обновление может быть пропущено и выполнено несколько шагов прогнозирования. Аналогичным образом, если одновременно доступны несколько независимых наблюдений, можно выполнить несколько этапов обновления (обычно с разными матрицами наблюдений H _k ). ^{[23] [24]}

Прогнозировать [ править ]

Прогнозируемая ( априорная ) оценка состояния	${\displaystyle {\hat {\mathbf {x} }}_{k\mid k-1}=\mathbf {F} _{k}{\hat {\mathbf {x} }}_{k-1\mid k-1}+\mathbf {B} _{k}\mathbf {u} _{k}}$
Прогнозируемая ( априорная ) ковариация оценки	${\displaystyle \mathbf {P} _{k\mid k-1}=\mathbf {F} _{k}\mathbf {P} _{k-1\mid k-1}\mathbf {F} _{k}^{\textsf {T}}+\mathbf {Q} _{k}}$

Обновить [ изменить ]

Нововведение или остаток предварительной настройки измерения	${\displaystyle {\tilde {\mathbf {y} }}_{k}=\mathbf {z} _{k}-\mathbf {H} _{k}{\hat {\mathbf {x} }}_{k\mid k-1}}$
Новаторская (или предварительная остаточная) ковариация	${\displaystyle \mathbf {S} _{k}=\mathbf {H} _{k}\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}+\mathbf {R} _{k}}$
Оптимальное усиление Калмана	${\displaystyle \mathbf {K} _{k}=\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}\mathbf {S} _{k}^{-1}}$
Обновленная ( апостериорная ) оценка состояния	${\displaystyle {\hat {\mathbf {x} }}_{k\mid k}={\hat {\mathbf {x} }}_{k\mid k-1}+\mathbf {K} _{k}{\tilde {\mathbf {y} }}_{k}}$
Обновленная ( апостериорная ) ковариация оценки	${\displaystyle \mathbf {P} _{k|k}=\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)\mathbf {P} _{k|k-1}}$
Измерение после посадки остаточного	${\displaystyle {\tilde {\mathbf {y} }}_{k\mid k}=\mathbf {z} _{k}-\mathbf {H} _{k}{\hat {\mathbf {x} }}_{k\mid k}}$

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

Более интуитивно понятный способ выразить обновленную оценку состояния ( ):${\displaystyle {\hat {\mathbf {x} }}_{k\mid k}}$

${\displaystyle {\hat {\mathbf {x} }}_{k\mid k}=(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k})({\hat {\mathbf {x} }}_{k\mid k-1})+(\mathbf {K} _{k})(\mathbf {H} _{k}\mathbf {x} _{k}+\mathbf {v} _{k})}$

Это выражение напоминает нам о линейной интерполяции, для между [0,1]. В нашем случае:${\displaystyle x=(1-t)(a)+t(b)}$${\displaystyle t}$

• ${\displaystyle t}$- коэффициент Калмана ( ), матрица, которая принимает значения от (высокая ошибка датчика) до (низкая ошибка).${\displaystyle \mathbf {K} _{k}}$${\displaystyle 0}$${\displaystyle I}$
• ${\displaystyle a}$ - значение, оцененное по модели.
• ${\displaystyle b}$ это значение из измерения.

Это выражение также напоминает этап обновления фильтра Alpha beta .

Инварианты [ править ]

Если модель точна, а значения и точно отражают распределение значений начального состояния, то сохраняются следующие инварианты:${\displaystyle {\hat {\mathbf {x} }}_{0\mid 0}}$${\displaystyle \mathbf {P} _{0\mid 0}}$

${\displaystyle {\begin{aligned}\operatorname {E} [\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k}]&=\operatorname {E} [\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k-1}]=0\\\operatorname {E} [{\tilde {\mathbf {y} }}_{k}]&=0\end{aligned}}}$

где это ожидаемое значение из . То есть все оценки имеют нулевую среднюю ошибку.${\displaystyle \operatorname {E} [\xi ]}$${\displaystyle \xi }$

Также:

${\displaystyle {\begin{aligned}\mathbf {P} _{k\mid k}&=\operatorname {cov} \left(\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k}\right)\\\mathbf {P} _{k\mid k-1}&=\operatorname {cov} \left(\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k-1}\right)\\\mathbf {S} _{k}&=\operatorname {cov} \left({\tilde {\mathbf {y} }}_{k}\right)\end{aligned}}}$

поэтому ковариационные матрицы точно отражают ковариацию оценок.

Оценка ковариаций шума Q _k и R _k [ править ]

Практическая реализация фильтра Калмана часто затруднена из-за трудности получения хорошей оценки матриц ковариации шума Q _k и R _k . В этой области были проведены обширные исследования для оценки этих ковариаций на основе данных. Одним из практических подходов к этому является метод наименьших квадратов автоковариации (ALS) , в котором для оценки ковариаций используются автоковариации стандартных рабочих данных с запаздыванием по времени . ^{[25] [26]} Код GNU Octave и Matlab, используемый для вычисления ковариационных матриц шума с помощью метода ALS, доступен в Интернете под Стандартной общественной лицензией GNU .^[27] Полевой фильтр Калмана (FKF), байесовский алгоритм, который позволяет одновременно оценивать состояние, параметры и ковариацию шума, был предложен в ^[28] Алгоритм FKF имеет рекурсивную формулировку, хорошую наблюдаемую сходимость и относительно низкую сложность. Это дает возможность того, что алгоритм FKF может быть альтернативой методам наименьших квадратов автоковариации.

Оптимальность и производительность [ править ]

Из теории следует, что фильтр Калмана является оптимальным линейным фильтром в случаях, когда а) модель идеально соответствует реальной системе, б) входящий шум белый (некоррелированный) и в) ковариации шума точно известны. В течение последних десятилетий было предложено несколько методов оценки ковариации шума, в том числе ALS, упомянутый в разделе выше. После оценки ковариаций полезно оценить производительность фильтра; т.е. возможно ли улучшить качество оценки состояния. Если фильтр Калмана работает оптимально, последовательность нововведений (ошибка прогнозирования выходных данных) представляет собой белый шум, поэтому свойство белизны нововведений измеряет эффективность фильтра. Для этого можно использовать несколько различных методов. ^[29]Если шумовые составляющие распределены не по Гауссу, в литературе известны методы оценки эффективности оценки фильтра, в которых используются вероятностные неравенства или теория большой выборки. ^{[30] [31]}

Пример приложения, технический [ править ]

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

Поскольку они постоянны, их временные индексы опускаются.${\displaystyle \mathbf {F} ,\mathbf {H} ,\mathbf {R} ,\mathbf {Q} }$

Положение и скорость грузовика описываются линейным пространством состояний

${\displaystyle \mathbf {x} _{k}={\begin{bmatrix}x\\{\dot {x}}\end{bmatrix}}}$

где - скорость, то есть производная положения по времени.${\displaystyle {\dot {x}}}$

Мы предполагаем , что между ( к  - 1) и K временного шага неконтролируемые силы вызывают постоянное ускорение в _к , который обычно распространяется , со средним значением 0 и стандартным отклонением сг . Из законов движения Ньютона заключаем, что

${\displaystyle \mathbf {x} _{k}=\mathbf {F} \mathbf {x} _{k-1}+\mathbf {G} a_{k}}$

(нет термина , так как там нет известных входных сигналов управления. Вместо этого, _к является эффект неизвестного входа и применяет этот эффект вектора состояния) , где${\displaystyle \mathbf {B} u}$${\displaystyle \mathbf {G} }$

${\displaystyle {\begin{aligned}\mathbf {F} &={\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}}\\[4pt]\mathbf {G} &={\begin{bmatrix}{\frac {1}{2}}{\Delta t}^{2}\\[6pt]\Delta t\end{bmatrix}}\end{aligned}}}$

так что

${\displaystyle \mathbf {x} _{k}=\mathbf {F} \mathbf {x} _{k-1}+\mathbf {w} _{k}}$

куда

${\displaystyle {\begin{aligned}\mathbf {w} _{k}&\sim N(0,\mathbf {Q} )\\\mathbf {Q} &=\mathbf {G} \mathbf {G} ^{\textsf {T}}\sigma _{a}^{2}={\begin{bmatrix}{\frac {1}{4}}{\Delta t}^{4}&{\frac {1}{2}}{\Delta t}^{3}\\[6pt]{\frac {1}{2}}{\Delta t}^{3}&{\Delta t}^{2}\end{bmatrix}}\sigma _{a}^{2}.\end{aligned}}}$

Матрица не имеет полного ранга (она имеет ранг 1, если ). Следовательно, распределение не является абсолютно непрерывным и не имеет функции плотности вероятности . Другой способ выразить это, избегая явных вырожденных распределений, дает${\displaystyle \mathbf {Q} }$${\displaystyle \Delta t\neq 0}$${\displaystyle N(0,\mathbf {Q} )}$

${\displaystyle \mathbf {w} _{k}\sim \mathbf {G} \cdot N\left(0,\sigma _{a}^{2}\right).}$

На каждом временном шаге производится измерение истинного положения грузовика с шумом. Предположим, что шум измерения v _k также имеет нормальное распределение со средним значением 0 и стандартным отклонением σ _z .

${\displaystyle \mathbf {z} _{k}=\mathbf {Hx} _{k}+\mathbf {v} _{k}}$

куда

${\displaystyle \mathbf {H} ={\begin{bmatrix}1&0\end{bmatrix}}}$

и

${\displaystyle \mathbf {R} =\mathrm {E} \left[\mathbf {v} _{k}\mathbf {v} _{k}^{\textsf {T}}\right]={\begin{bmatrix}\sigma _{z}^{2}\end{bmatrix}}}$

Мы знаем начальное стартовое состояние грузовика с идеальной точностью, поэтому инициализируем

${\displaystyle {\hat {\mathbf {x} }}_{0\mid 0}={\begin{bmatrix}0\\0\end{bmatrix}}}$

и чтобы сообщить фильтру, что мы знаем точное положение и скорость, мы даем ему нулевую ковариационную матрицу:

${\displaystyle \mathbf {P} _{0\mid 0}={\begin{bmatrix}0&0\\0&0\end{bmatrix}}}$

Если начальное положение и скорость неизвестны точно, ковариационная матрица должна быть инициализирована с подходящей дисперсией на ее диагонали:

${\displaystyle \mathbf {P} _{0\mid 0}={\begin{bmatrix}\sigma _{x}^{2}&0\\0&\sigma _{\dot {x}}^{2}\end{bmatrix}}}$

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

Асимптотическая форма [ править ]

Для простоты предположим, что control input . Тогда фильтр Калмана можно записать:${\displaystyle \mathbf {u} _{k}=\mathbf {0} }$

${\displaystyle {\hat {\mathbf {x} }}_{k\mid k}=\mathbf {F} _{k}{\hat {\mathbf {x} }}_{k-1\mid k-1}+\mathbf {K} _{k}[\mathbf {z} _{k}-\mathbf {H} _{k}\mathbf {F} _{k}{\hat {\mathbf {x} }}_{k-1\mid k-1}].}$

Аналогичное уравнение выполняется, если мы включаем ненулевой управляющий вход. Матрицы коэффициентов усиления развиваются независимо от измерений . Сверху четыре уравнения, необходимые для обновления коэффициента Калмана, следующие:${\displaystyle \mathbf {K} _{k}}$${\displaystyle \mathbf {z} _{k}}$

${\displaystyle \mathbf {P} _{k\mid k-1}=\mathbf {F} _{k}\mathbf {P} _{k-1\mid k-1}\mathbf {F} _{k}^{\textsf {T}}+\mathbf {Q} _{k},}$

${\displaystyle \mathbf {S} _{k}=\mathbf {R} _{k}+\mathbf {H} _{k}\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}},}$

${\displaystyle \mathbf {K} _{k}=\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}\mathbf {S} _{k}^{-1},}$

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

Поскольку матрицы усиления зависят только от модели, а не от измерений, их можно вычислить в автономном режиме. Сходимость матриц усиления к асимптотической матрице выполняется при условиях, установленных Вальрандом и Димакисом. ^[32] Моделирование устанавливает количество шагов к сходимости. Для примера движущегося грузовика, описанного выше, с . и моделирование показывает сходимость в итерациях.${\displaystyle \mathbf {K} _{k}}$${\displaystyle \mathbf {K} _{\infty }}$${\displaystyle \Delta t=1}$${\displaystyle \sigma _{a}^{2}=\sigma _{z}^{2}=\sigma _{x}^{2}=\sigma _{\dot {x}}^{2}=1}$${\displaystyle 10}$

Используя асимптотическое усиление и предполагая, что и не зависят от , фильтр Калмана становится линейным инвариантным во времени фильтром:${\displaystyle \mathbf {H} _{k}}$${\displaystyle \mathbf {F} _{k}}$${\displaystyle k}$

${\displaystyle {\hat {\mathbf {x} }}_{k}=\mathbf {F} {\hat {\mathbf {x} }}_{k-1}+\mathbf {K} _{\infty }[\mathbf {z} _{k}-\mathbf {H} \mathbf {F} {\hat {\mathbf {x} }}_{k-1}].}$

Асимптотический коэффициент усиления , если он существует, можно вычислить, сначала решив следующее дискретное уравнение Риккати для асимптотической ковариации состояния : ^[32]${\displaystyle \mathbf {K} _{\infty }}$${\displaystyle \mathbf {P} _{\infty }}$

${\displaystyle \mathbf {P} _{\infty }=\mathbf {F} \left(\mathbf {P} _{\infty }-\mathbf {P} _{\infty }\mathbf {H} ^{\textsf {T}}\left(\mathbf {H} \mathbf {P} _{\infty }\mathbf {H} ^{\textsf {T}}+\mathbf {R} \right)^{-1}\mathbf {H} \mathbf {P} _{\infty }\right)\mathbf {F} ^{\textsf {T}}+\mathbf {Q} .}$

Затем асимптотический коэффициент усиления вычисляется, как и раньше.

${\displaystyle \mathbf {K} _{\infty }=\mathbf {P} _{\infty }\mathbf {H} ^{\textsf {T}}\left(\mathbf {H} \mathbf {P} _{\infty }\mathbf {H} ^{\textsf {T}}+\mathbf {R} \right)^{-1}.}$

Производные [ править ]

Получение ковариационной матрицы апостериорной оценки [ править ]

Начиная с нашего инварианта ковариации ошибок P _{k  | k} как указано выше

${\displaystyle \mathbf {P} _{k\mid k}=\operatorname {cov} \left(\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k}\right)}$

заменить в определении ${\displaystyle {\hat {\mathbf {x} }}_{k\mid k}}$

${\displaystyle \mathbf {P} _{k\mid k}=\operatorname {cov} \left[\mathbf {x} _{k}-\left({\hat {\mathbf {x} }}_{k\mid k-1}+\mathbf {K} _{k}{\tilde {\mathbf {y} }}_{k}\right)\right]}$

и заменить ${\displaystyle {\tilde {\mathbf {y} }}_{k}}$

${\displaystyle \mathbf {P} _{k\mid k}=\operatorname {cov} \left(\mathbf {x} _{k}-\left[{\hat {\mathbf {x} }}_{k\mid k-1}+\mathbf {K} _{k}\left(\mathbf {z} _{k}-\mathbf {H} _{k}{\hat {\mathbf {x} }}_{k\mid k-1}\right)\right]\right)}$

и ${\displaystyle \mathbf {z} _{k}}$

${\displaystyle \mathbf {P} _{k\mid k}=\operatorname {cov} \left(\mathbf {x} _{k}-\left[{\hat {\mathbf {x} }}_{k\mid k-1}+\mathbf {K} _{k}\left(\mathbf {H} _{k}\mathbf {x} _{k}+\mathbf {v} _{k}-\mathbf {H} _{k}{\hat {\mathbf {x} }}_{k\mid k-1}\right)\right]\right)}$

и собирая векторы ошибок, получаем

${\displaystyle \mathbf {P} _{k\mid k}=\operatorname {cov} \left[\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)\left(\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k-1}\right)-\mathbf {K} _{k}\mathbf {v} _{k}\right]}$

Поскольку ошибка измерения v _k не коррелирует с другими членами, это становится

${\displaystyle \mathbf {P} _{k\mid k}=\operatorname {cov} \left[\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)\left(\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k-1}\right)\right]+\operatorname {cov} \left[\mathbf {K} _{k}\mathbf {v} _{k}\right]}$

по свойствам векторной ковариации это становится

${\displaystyle \mathbf {P} _{k\mid k}=\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)\operatorname {cov} \left(\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k-1}\right)\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)^{\textsf {T}}+\mathbf {K} _{k}\operatorname {cov} \left(\mathbf {v} _{k}\right)\mathbf {K} _{k}^{\textsf {T}}}$

что, используя наш инвариант на P _{k  | k −1,} и определение R _k становится

${\displaystyle \mathbf {P} _{k\mid k}=\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)\mathbf {P} _{k\mid k-1}\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)^{\textsf {T}}+\mathbf {K} _{k}\mathbf {R} _{k}\mathbf {K} _{k}^{\textsf {T}}}$

Эта формула (иногда известная как форма Джозефа уравнения обновления ковариации) действительна для любого значения K _k . Оказывается, что если K _k является оптимальным коэффициентом усиления Калмана, это можно упростить, как показано ниже.

Расчет усиления Калмана [ править ]

Фильтр Калмана - это средство оценки минимальной среднеквадратичной ошибки . Ошибка апостериорной оценки состояния составляет

${\displaystyle \mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k}}$

Мы стремимся минимизировать математическое ожидание квадрата величины этого вектора . Это эквивалентно минимизация следов от апостериорной оценки ковариационной матрицы . Расширяя члены в приведенном выше уравнении и собирая, мы получаем:${\displaystyle \operatorname {E} \left[\left\|\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k|k}\right\|^{2}\right]}$ ${\displaystyle \mathbf {P} _{k|k}}$

${\displaystyle {\begin{aligned}\mathbf {P} _{k\mid k}&=\mathbf {P} _{k\mid k-1}-\mathbf {K} _{k}\mathbf {H} _{k}\mathbf {P} _{k\mid k-1}-\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}\mathbf {K} _{k}^{\textsf {T}}+\mathbf {K} _{k}\left(\mathbf {H} _{k}\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}+\mathbf {R} _{k}\right)\mathbf {K} _{k}^{\textsf {T}}\\[6pt]&=\mathbf {P} _{k\mid k-1}-\mathbf {K} _{k}\mathbf {H} _{k}\mathbf {P} _{k\mid k-1}-\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}\mathbf {K} _{k}^{\textsf {T}}+\mathbf {K} _{k}\mathbf {S} _{k}\mathbf {K} _{k}^{\textsf {T}}\end{aligned}}}$

След минимизируется, когда его матричная производная по матрице усиления равна нулю. Используя правила градиентной матрицы и симметрию задействованных матриц, мы находим, что

${\displaystyle {\frac {\partial \;\operatorname {tr} (\mathbf {P} _{k\mid k})}{\partial \;\mathbf {K} _{k}}}=-2\left(\mathbf {H} _{k}\mathbf {P} _{k\mid k-1}\right)^{\textsf {T}}+2\mathbf {K} _{k}\mathbf {S} _{k}=0.}$

Решение этого для K _k дает выигрыш Калмана:

${\displaystyle {\begin{aligned}\mathbf {K} _{k}\mathbf {S} _{k}&=\left(\mathbf {H} _{k}\mathbf {P} _{k\mid k-1}\right)^{\textsf {T}}=\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}\\\Rightarrow \mathbf {K} _{k}&=\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}\mathbf {S} _{k}^{-1}\end{aligned}}}$

Это усиление, известное как оптимальное усиление Калмана , при использовании дает оценки MMSE .

Упрощение формулы ковариации апостериорной ошибки [ править ]

Формулу, используемую для вычисления ковариации апостериорной ошибки, можно упростить, если коэффициент усиления Калмана равен оптимальному значению, полученному выше. Умножая обе части нашей формулы усиления Калмана справа на S _k K _k ^T , получаем, что

${\displaystyle \mathbf {K} _{k}\mathbf {S} _{k}\mathbf {K} _{k}^{\textsf {T}}=\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}\mathbf {K} _{k}^{\textsf {T}}}$

Возвращаясь к нашей расширенной формуле для ковариации апостериорной ошибки,

${\displaystyle \mathbf {P} _{k\mid k}=\mathbf {P} _{k\mid k-1}-\mathbf {K} _{k}\mathbf {H} _{k}\mathbf {P} _{k\mid k-1}-\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}\mathbf {K} _{k}^{\textsf {T}}+\mathbf {K} _{k}\mathbf {S} _{k}\mathbf {K} _{k}^{\textsf {T}}}$

мы обнаруживаем, что последние два члена сокращаются, давая

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

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

Анализ чувствительности [ править ]

Уравнения фильтрации Калмана рекурсивно обеспечивают оценку состояния и его ковариации ошибок . Оценка и ее качество зависят от параметров системы и статистики шума, подаваемой в качестве входных данных для оценщика. В этом разделе анализируется влияние неопределенностей в статистических входных данных для фильтра. ^[33] При отсутствии надежной статистики или истинных значений ковариационных матриц шума и выражение${\displaystyle {\hat {\mathbf {x} }}_{k\mid k}}$${\displaystyle \mathbf {P} _{k\mid k}}$${\displaystyle \mathbf {Q} _{k}}$${\displaystyle \mathbf {R} _{k}}$

${\displaystyle \mathbf {P} _{k\mid k}=\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)\mathbf {P} _{k\mid k-1}\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)^{\textsf {T}}+\mathbf {K} _{k}\mathbf {R} _{k}\mathbf {K} _{k}^{\textsf {T}}}$

больше не обеспечивает фактическую ковариацию ошибок. Другими словами, . В большинстве приложений реального времени ковариационные матрицы, используемые при разработке фильтра Калмана, отличаются от реальных (истинных) матриц ковариаций шума. ^{[ Править ]} Этот анализ чувствительности описывает поведение оценки ошибки ковариации , когда ковариация шума, а также система матрица и которые подаются в качестве входных сигналов фильтра является неправильной. Таким образом, анализ чувствительности описывает устойчивость (или чувствительность) оценщика к неверно указанным статистическим и параметрическим входным данным оценщика.${\displaystyle \mathbf {P} _{k\mid k}\neq E\left[\left(\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k}\right)\left(\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k}\right)^{\textsf {T}}\right]}$${\displaystyle \mathbf {F} _{k}}$${\displaystyle \mathbf {H} _{k}}$

Это обсуждение ограничивается анализом чувствительности к ошибкам для случая статистических неопределенностей. Здесь фактический шум ковариация обозначается и , соответственно, в то время как расчетные значения , используемые в оценках являются и , соответственно. Фактическая ковариация ошибок обозначается и, вычисленная фильтром Калмана, называется переменной Риккати. Когда и , это значит, что . При вычислении фактической ковариации ошибки с использованием , заменой и использованием того факта, что и , приводят к следующим рекурсивным уравнениям для  :${\displaystyle \mathbf {Q} _{k}^{a}}$${\displaystyle \mathbf {R} _{k}^{a}}$${\displaystyle \mathbf {Q} _{k}}$${\displaystyle \mathbf {R} _{k}}$${\displaystyle \mathbf {P} _{k\mid k}^{a}}$${\displaystyle \mathbf {P} _{k\mid k}}$${\displaystyle \mathbf {Q} _{k}\equiv \mathbf {Q} _{k}^{a}}$${\displaystyle \mathbf {R} _{k}\equiv \mathbf {R} _{k}^{a}}$${\displaystyle \mathbf {P} _{k\mid k}=\mathbf {P} _{k\mid k}^{a}}$${\displaystyle \mathbf {P} _{k\mid k}^{a}=E\left[\left(\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k}\right)\left(\mathbf {x} _{k}-{\hat {\mathbf {x} }}_{k\mid k}\right)^{\textsf {T}}\right]}$${\displaystyle {\widehat {\mathbf {x} }}_{k\mid k}}$${\displaystyle E\left[\mathbf {w} _{k}\mathbf {w} _{k}^{\textsf {T}}\right]=\mathbf {Q} _{k}^{a}}$${\displaystyle E\left[\mathbf {v} _{k}\mathbf {v} _{k}^{\textsf {T}}\right]=\mathbf {R} _{k}^{a}}$${\displaystyle \mathbf {P} _{k\mid k}^{a}}$

${\displaystyle \mathbf {P} _{k\mid k-1}^{a}=\mathbf {F} _{k}\mathbf {P} _{k-1\mid k-1}^{a}\mathbf {F} _{k}^{\textsf {T}}+\mathbf {Q} _{k}^{a}}$

и

${\displaystyle \mathbf {P} _{k\mid k}^{a}=\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)\mathbf {P} _{k\mid k-1}^{a}\left(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k}\right)^{\textsf {T}}+\mathbf {K} _{k}\mathbf {R} _{k}^{a}\mathbf {K} _{k}^{\textsf {T}}}$

При вычислении фильтр по своей конструкции неявно предполагает, что и . Рекурсивные выражения для и идентичны, за исключением наличия и вместо проектных значений и соответственно. Были проведены исследования для анализа устойчивости системы фильтров Калмана. ^[34]${\displaystyle \mathbf {P} _{k\mid k}}$${\displaystyle E\left[\mathbf {w} _{k}\mathbf {w} _{k}^{\textsf {T}}\right]=\mathbf {Q} _{k}}$${\displaystyle E\left[\mathbf {v} _{k}\mathbf {v} _{k}^{\textsf {T}}\right]=\mathbf {R} _{k}}$${\displaystyle \mathbf {P} _{k\mid k}^{a}}$${\displaystyle \mathbf {P} _{k\mid k}}$${\displaystyle \mathbf {Q} _{k}^{a}}$${\displaystyle \mathbf {R} _{k}^{a}}$${\displaystyle \mathbf {Q} _{k}}$${\displaystyle \mathbf {R} _{k}}$

Форма квадратного корня [ править ]

Одной из проблем фильтра Калмана является его числовая стабильность . Если ковариация шума процесса Q _k мала, ошибка округления часто приводит к тому, что небольшое положительное собственное значение вычисляется как отрицательное число. Это делает численное представление ковариационной матрицы состояния P неопределенным , в то время как его истинная форма является положительно определенной .

Положительные определенные матрицы обладают тем свойством , что они имеют треугольную матрицу квадратный корень P  =  S · S ^T . Это можно эффективно вычислить с помощью алгоритма факторизации Холецкого , но, что более важно, если ковариация сохраняется в этой форме, она никогда не может иметь отрицательную диагональ или стать асимметричной. Эквивалентной формой, которая позволяет избежать многих операций извлечения квадратного корня, требуемых для квадратного корня матрицы, но сохраняет желаемые числовые свойства, является форма разложения UD, P  =  U · D · U ^T , где U - этоединичная треугольная матрица (с единичной диагональю), а D - диагональная матрица.

Между этими двумя факторизация UD использует тот же объем памяти и несколько меньше вычислений и является наиболее часто используемой формой квадратного корня. (Ранняя литература об относительной эффективности несколько вводит в заблуждение, поскольку предполагалось, что извлечение квадратного корня занимает гораздо больше времени, чем деление ^{[35] : 69,} тогда как на компьютерах 21-го века они лишь немного дороже.)

Эффективные алгоритмы для предсказания Калмана и шагов обновления в форме квадратного корня были разработаны GJ Bierman и CL Thornton. ^{[35] [36]}

L · D · L Т разложение в инновационной ковариационной матрицы S _к является основой для другого типа численно эффективной и надежной квадратного корня фильтра. ^[37] Алгоритм начинается с разложения LU, как реализовано в пакете линейной алгебры ( LAPACK ). Эти результаты далее вносятся в структуру L · D · L ^T с помощью методов, предложенных Голубом и Ван Лоаном (алгоритм 4.1.2) для симметричной невырожденной матрицы. ^[38] Любая сингулярная ковариационная матрица поворачивается так, чтобы первое диагональное разбиение былонеособо и хорошо кондиционером . Алгоритм поворота должен сохранять любую часть ковариационной матрицы нововведений, непосредственно соответствующую наблюдаемым переменным состояния H _k · x _{k | k-1} , которые связаны со вспомогательными наблюдениями в y _k . Л · д · л ^т квадратного корня фильтра требует ортогонализации вектора наблюдения. ^{[36] [37]} Это можно сделать с помощью обратного квадратного корня из ковариационной матрицы для вспомогательных переменных, используя метод 2 в Higham (2002, стр. 263). ^[39]

Связь с рекурсивным байесовским оцениванием [ править ]

Фильтр Калмана можно представить как одну из простейших динамических байесовских сетей . Фильтр Калмана вычисляет оценки истинных значений состояний рекурсивно с течением времени, используя входящие измерения и математическую модель процесса. Точно так же рекурсивная байесовская оценка вычисляет оценки неизвестной функции плотности вероятности (PDF) рекурсивно с течением времени с использованием входящих измерений и математической модели процесса. ^[40]

В рекурсивной байесовской оценке истинное состояние предполагается ненаблюдаемым марковским процессом , а измерения - наблюдаемыми состояниями скрытой марковской модели (HMM).

из-за предположения Маркова истинное состояние условно не зависит от всех предыдущих состояний при непосредственно предшествующем состоянии.

${\displaystyle p(\mathbf {x} _{k}\mid \mathbf {x} _{0},\dots ,\mathbf {x} _{k-1})=p(\mathbf {x} _{k}\mid \mathbf {x} _{k-1})}$

Точно так же измерение на k-м временном шаге зависит только от текущего состояния и условно не зависит от всех других состояний с учетом текущего состояния.

${\displaystyle p(\mathbf {z} _{k}\mid \mathbf {x} _{0},\dots ,\mathbf {x} _{k})=p(\mathbf {z} _{k}\mid \mathbf {x} _{k})}$

Используя эти предположения, распределение вероятностей по всем состояниям скрытой марковской модели можно записать просто как:

${\displaystyle p\left(\mathbf {x} _{0},\dots ,\mathbf {x} _{k},\mathbf {z} _{1},\dots ,\mathbf {z} _{k}\right)=p\left(\mathbf {x} _{0}\right)\prod _{i=1}^{k}p\left(\mathbf {z} _{i}\mid \mathbf {x} _{i}\right)p\left(\mathbf {x} _{i}\mid \mathbf {x} _{i-1}\right)}$

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

Это приводит к тому, что шаги прогнозирования и обновления фильтра Калмана записываются вероятностно. Распределение вероятностей, связанное с прогнозируемым состоянием, представляет собой сумму (интеграл) произведений распределения вероятностей, связанных с переходом от ( k  - 1) -го временного шага к k- му, и распределения вероятностей, связанного с предыдущим состоянием, над всем возможным .${\displaystyle x_{k-1}}$

${\displaystyle p\left(\mathbf {x} _{k}\mid \mathbf {Z} _{k-1}\right)=\int p\left(\mathbf {x} _{k}\mid \mathbf {x} _{k-1}\right)p\left(\mathbf {x} _{k-1}\mid \mathbf {Z} _{k-1}\right)\,d\mathbf {x} _{k-1}}$

Настройка измерения до момента t :

${\displaystyle \mathbf {Z} _{t}=\left\{\mathbf {z} _{1},\dots ,\mathbf {z} _{t}\right\}}$

Распределение вероятностей обновления пропорционально произведению вероятности измерения и прогнозируемого состояния.

${\displaystyle p\left(\mathbf {x} _{k}\mid \mathbf {Z} _{k}\right)={\frac {p\left(\mathbf {z} _{k}\mid \mathbf {x} _{k}\right)p\left(\mathbf {x} _{k}\mid \mathbf {Z} _{k-1}\right)}{p\left(\mathbf {z} _{k}\mid \mathbf {Z} _{k-1}\right)}}}$

Знаменатель

${\displaystyle p\left(\mathbf {z} _{k}\mid \mathbf {Z} _{k-1}\right)=\int p\left(\mathbf {z} _{k}\mid \mathbf {x} _{k}\right)p\left(\mathbf {x} _{k}\mid \mathbf {Z} _{k-1}\right)\,d\mathbf {x} _{k}}$

это нормализационный член.

Остальные функции плотности вероятности:

${\displaystyle {\begin{aligned}p\left(\mathbf {x} _{k}\mid \mathbf {x} _{k-1}\right)&={\mathcal {N}}\left(\mathbf {F} _{k}\mathbf {x} _{k-1},\mathbf {Q} _{k}\right)\\p\left(\mathbf {z} _{k}\mid \mathbf {x} _{k}\right)&={\mathcal {N}}\left(\mathbf {H} _{k}\mathbf {x} _{k},\mathbf {R} _{k}\right)\\p\left(\mathbf {x} _{k-1}\mid \mathbf {Z} _{k-1}\right)&={\mathcal {N}}\left({\hat {\mathbf {x} }}_{k-1},\mathbf {P} _{k-1}\right)\end{aligned}}}$

Индуктивно предполагается, что PDF на предыдущем временном шаге является оцененным состоянием и ковариацией. Это оправдано, поскольку в качестве оптимального средства оценки фильтр Калмана наилучшим образом использует измерения, поэтому PDF для данных измерений является оценкой фильтра Калмана.${\displaystyle \mathbf {x} _{k}}$${\displaystyle \mathbf {Z} _{k}}$

Предельная вероятность [ править ]

Связанный с описанной выше рекурсивной байесовской интерпретацией, фильтр Калмана можно рассматривать как генеративную модель , то есть процесс генерации потока случайных наблюдений z = ( z ₀ , z ₁ , z ₂ , ...). В частности, процесс

1. Выберите скрытое состояние из априорного распределения Гаусса .${\displaystyle \mathbf {x} _{0}}$${\displaystyle p\left(\mathbf {x} _{0}\right)={\mathcal {N}}\left({\hat {\mathbf {x} }}_{0\mid 0},\mathbf {P} _{0\mid 0}\right)}$
2. Пример наблюдения из модели наблюдения .${\displaystyle \mathbf {z} _{0}}$${\displaystyle p\left(\mathbf {z} _{0}\mid \mathbf {x} _{0}\right)={\mathcal {N}}\left(\mathbf {H} _{0}\mathbf {x} _{0},\mathbf {R} _{0}\right)}$
3. Ибо делать${\displaystyle k=1,2,3,\ldots }$
   1. Пример следующего скрытого состояния из модели перехода${\displaystyle \mathbf {x} _{k}}$${\displaystyle p\left(\mathbf {x} _{k}\mid \mathbf {x} _{k-1}\right)={\mathcal {N}}\left(\mathbf {F} _{k}\mathbf {x} _{k-1}+\mathbf {B} _{k}\mathbf {u} _{k},\mathbf {Q} _{k}\right).}$
   2. Пример наблюдения из модели наблюдения${\displaystyle \mathbf {z} _{k}}$${\displaystyle p\left(\mathbf {z} _{k}\mid \mathbf {x} _{k}\right)={\mathcal {N}}\left(\mathbf {H} _{k}\mathbf {x} _{k},\mathbf {R} _{k}\right).}$

Этот процесс имеет структуру, идентичную скрытой марковской модели , за исключением того, что дискретное состояние и наблюдения заменяются непрерывными переменными, выбранными из гауссовых распределений.

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

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

${\displaystyle p(\mathbf {z} )=\prod _{k=0}^{T}p\left(\mathbf {z} _{k}\mid \mathbf {z} _{k-1},\ldots ,\mathbf {z} _{0}\right)}$,

и поскольку фильтр Калмана описывает марковский процесс, вся соответствующая информация из предыдущих наблюдений содержится в оценке текущего состояния. Таким образом, предельное правдоподобие определяется выражением${\displaystyle {\hat {\mathbf {x} }}_{k\mid k-1},\mathbf {P} _{k\mid k-1}.}$

${\displaystyle {\begin{aligned}p(\mathbf {z} )&=\prod _{k=0}^{T}\int p\left(\mathbf {z} _{k}\mid \mathbf {x} _{k}\right)p\left(\mathbf {x} _{k}\mid \mathbf {z} _{k-1},\ldots ,\mathbf {z} _{0}\right)d\mathbf {x} _{k}\\&=\prod _{k=0}^{T}\int {\mathcal {N}}\left(\mathbf {z} _{k};\mathbf {H} _{k}\mathbf {x} _{k},\mathbf {R} _{k}\right){\mathcal {N}}\left(\mathbf {x} _{k};{\hat {\mathbf {x} }}_{k\mid k-1},\mathbf {P} _{k\mid k-1}\right)d\mathbf {x} _{k}\\&=\prod _{k=0}^{T}{\mathcal {N}}\left(\mathbf {z} _{k};\mathbf {H} _{k}{\hat {\mathbf {x} }}_{k\mid k-1},\mathbf {R} _{k}+\mathbf {H} _{k}\mathbf {P} _{k\mid k-1}\mathbf {H} _{k}^{\textsf {T}}\right)\\&=\prod _{k=0}^{T}{\mathcal {N}}\left(\mathbf {z} _{k};\mathbf {H} _{k}{\hat {\mathbf {x} }}_{k\mid k-1},\mathbf {S} _{k}\right),\end{aligned}}}$

т. е. продукт гауссовых плотностей, каждая из которых соответствует плотности одного наблюдения z _k при текущем распределении фильтрации . Это можно легко вычислить как простое рекурсивное обновление; однако, чтобы избежать потери числового значения , в практической реализации обычно желательно вместо этого вычислить логарифмическую предельную вероятность . Принимая соглашение , это можно сделать с помощью правила рекурсивного обновления.${\displaystyle \mathbf {H} _{k}{\hat {\mathbf {x} }}_{k\mid k-1},\mathbf {S} _{k}}$${\displaystyle \ell =\log p(\mathbf {z} )}$${\displaystyle \ell ^{(-1)}=0}$

${\displaystyle \ell ^{(k)}=\ell ^{(k-1)}-{\frac {1}{2}}\left({\tilde {\mathbf {y} }}_{k}^{\textsf {T}}\mathbf {S} _{k}^{-1}{\tilde {\mathbf {y} }}_{k}+\log \left|\mathbf {S} _{k}\right|+d_{y}\log 2\pi \right),}$

где - размерность вектора измерения. ^[41]${\displaystyle d_{y}}$

Важным приложением, в котором используется такая (логарифмическая) вероятность наблюдений (с учетом параметров фильтра), является отслеживание нескольких целей. Например, рассмотрим сценарий отслеживания объекта, в котором поток наблюдений является входом, однако неизвестно, сколько объектов находится в сцене (или количество объектов известно, но больше единицы). В таком сценарии может быть неизвестно априори, какие наблюдения / измерения были произведены каким объектом. Устройство отслеживания множественных гипотез (MHT) обычно формирует разные гипотезы ассоциации треков, где каждую гипотезу можно рассматривать как фильтр Калмана (в линейном гауссовском случае) с определенным набором параметров, связанных с гипотетическим объектом. Таким образом, важно вычислить вероятность наблюдений для различных рассматриваемых гипотез,так что наиболее вероятный можно найти.

Информационный фильтр [ править ]

В информационном фильтре или фильтре обратной ковариации оцененная ковариация и оцененное состояние заменяются информационной матрицей и информационным вектором соответственно. Они определены как:

${\displaystyle {\begin{aligned}\mathbf {Y} _{k\mid k}&=\mathbf {P} _{k\mid k}^{-1}\\{\hat {\mathbf {y} }}_{k\mid k}&=\mathbf {P} _{k\mid k}^{-1}{\hat {\mathbf {x} }}_{k\mid k}\end{aligned}}}$

Точно так же предсказанная ковариация и состояние имеют эквивалентные информационные формы, определяемые как:

${\displaystyle {\begin{aligned}\mathbf {Y} _{k\mid k-1}&=\mathbf {P} _{k\mid k-1}^{-1}\\{\hat {\mathbf {y} }}_{k\mid k-1}&=\mathbf {P} _{k\mid k-1}^{-1}{\hat {\mathbf {x} }}_{k\mid k-1}\end{aligned}}}$

а также ковариация измерения и вектор измерения, которые определяются как:

${\displaystyle {\begin{aligned}\mathbf {I} _{k}&=\mathbf {H} _{k}^{\textsf {T}}\mathbf {R} _{k}^{-1}\mathbf {H} _{k}\\\mathbf {i} _{k}&=\mathbf {H} _{k}^{\textsf {T}}\mathbf {R} _{k}^{-1}\mathbf {z} _{k}\end{aligned}}}$

Обновление информации теперь превращается в банальную сумму. ^[42]

${\displaystyle {\begin{aligned}\mathbf {Y} _{k\mid k}&=\mathbf {Y} _{k\mid k-1}+\mathbf {I} _{k}\\{\hat {\mathbf {y} }}_{k\mid k}&={\hat {\mathbf {y} }}_{k\mid k-1}+\mathbf {i} _{k}\end{aligned}}}$

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

${\displaystyle {\begin{aligned}\mathbf {Y} _{k\mid k}&=\mathbf {Y} _{k\mid k-1}+\sum _{j=1}^{N}\mathbf {I} _{k,j}\\{\hat {\mathbf {y} }}_{k\mid k}&={\hat {\mathbf {y} }}_{k\mid k-1}+\sum _{j=1}^{N}\mathbf {i} _{k,j}\end{aligned}}}$

Для прогнозирования информационного фильтра информационная матрица и вектор могут быть преобразованы обратно в их эквиваленты в пространстве состояний, или, в качестве альтернативы, может использоваться прогнозирование информационного пространства. ^[42]

${\displaystyle {\begin{aligned}\mathbf {M} _{k}&=\left[\mathbf {F} _{k}^{-1}\right]^{\textsf {T}}\mathbf {Y} _{k-1\mid k-1}\mathbf {F} _{k}^{-1}\\\mathbf {C} _{k}&=\mathbf {M} _{k}\left[\mathbf {M} _{k}+\mathbf {Q} _{k}^{-1}\right]^{-1}\\\mathbf {L} _{k}&=\mathbf {I} -\mathbf {C} _{k}\\\mathbf {Y} _{k\mid k-1}&=\mathbf {L} _{k}\mathbf {M} _{k}\mathbf {L} _{k}^{\textsf {T}}+\mathbf {C} _{k}\mathbf {Q} _{k}^{-1}\mathbf {C} _{k}^{\textsf {T}}\\{\hat {\mathbf {y} }}_{k\mid k-1}&=\mathbf {L} _{k}\left[\mathbf {F} _{k}^{-1}\right]^{\textsf {T}}{\hat {\mathbf {y} }}_{k-1\mid k-1}\end{aligned}}}$

Если F и Q не зависят от времени, эти значения можно кэшировать, а F и Q должны быть обратимыми.

Сглаживание с фиксированной задержкой [ править ]

Оптимальный сглаживание с фиксированной задержкой обеспечивает оптимальную оценку для данной фиксированной задержки с использованием измерений от до . ^[43] Его можно вывести, используя предыдущую теорию через расширенное состояние, и основное уравнение фильтра следующее:${\displaystyle {\hat {\mathbf {x} }}_{k-N\mid k}}$${\displaystyle N}$${\displaystyle \mathbf {z} _{1}}$${\displaystyle \mathbf {z} _{k}}$

${\displaystyle {\begin{bmatrix}{\hat {\mathbf {x} }}_{t\mid t}\\{\hat {\mathbf {x} }}_{t-1\mid t}\\\vdots \\{\hat {\mathbf {x} }}_{t-N+1\mid t}\\\end{bmatrix}}={\begin{bmatrix}\mathbf {I} \\0\\\vdots \\0\\\end{bmatrix}}{\hat {\mathbf {x} }}_{t\mid t-1}+{\begin{bmatrix}0&\ldots &0\\\mathbf {I} &0&\vdots \\\vdots &\ddots &\vdots \\0&\ldots &\mathbf {I} \\\end{bmatrix}}{\begin{bmatrix}{\hat {\mathbf {x} }}_{t-1\mid t-1}\\{\hat {\mathbf {x} }}_{t-2\mid t-1}\\\vdots \\{\hat {\mathbf {x} }}_{t-N+1\mid t-1}\\\end{bmatrix}}+{\begin{bmatrix}\mathbf {K} ^{(0)}\\\mathbf {K} ^{(1)}\\\vdots \\\mathbf {K} ^{(N-1)}\\\end{bmatrix}}\mathbf {y} _{t\mid t-1}}$

куда:

• ${\displaystyle {\hat {\mathbf {x} }}_{t\mid t-1}}$ оценивается с помощью стандартного фильтра Калмана;
• ${\displaystyle \mathbf {y} _{t\mid t-1}=\mathbf {z} _{t}-\mathbf {H} {\hat {\mathbf {x} }}_{t\mid t-1}}$ инновация, произведенная с учетом оценки стандартного фильтра Калмана;
• различные с - это новые переменные; т.е. они не появляются в стандартном фильтре Калмана;${\displaystyle {\hat {\mathbf {x} }}_{t-i\mid t}}$${\displaystyle i=1,\ldots ,N-1}$
• выигрыши рассчитываются по следующей схеме:

  ${\displaystyle \mathbf {K} ^{(i+1)}=\mathbf {P} ^{(i)}\mathbf {H} ^{\textsf {T}}\left[\mathbf {H} \mathbf {P} \mathbf {H} ^{\textsf {T}}+\mathbf {R} \right]^{-1}}$

и

${\displaystyle \mathbf {P} ^{(i)}=\mathbf {P} \left[\left(\mathbf {F} -\mathbf {K} \mathbf {H} \right)^{\textsf {T}}\right]^{i}}$

где и - ковариация ошибки предсказания и коэффициенты усиления стандартного фильтра Калмана (т. е. ).${\displaystyle \mathbf {P} }$${\displaystyle \mathbf {K} }$${\displaystyle \mathbf {P} _{t\mid t-1}}$

Если ковариация ошибки оценки определена так, что

${\displaystyle \mathbf {P} _{i}:=E\left[\left(\mathbf {x} _{t-i}-{\hat {\mathbf {x} }}_{t-i\mid t}\right)^{*}\left(\mathbf {x} _{t-i}-{\hat {\mathbf {x} }}_{t-i\mid t}\right)\mid z_{1}\ldots z_{t}\right],}$

тогда мы получаем, что улучшение оценки определяется следующим образом:${\displaystyle \mathbf {x} _{t-i}}$

${\displaystyle \mathbf {P} -\mathbf {P} _{i}=\sum _{j=0}^{i}\left[\mathbf {P} ^{(j)}\mathbf {H} ^{\textsf {T}}\left(\mathbf {H} \mathbf {P} \mathbf {H} ^{\textsf {T}}+\mathbf {R} \right)^{-1}\mathbf {H} \left(\mathbf {P} ^{(i)}\right)^{\textsf {T}}\right]}$