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

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

Погодите, а где снова эти два уравнения?

Давайте быстро резюмируем, что такое объединение датчиков, включая уравнения предсказать и обновить. Для этого мы вернемся к примеру самолета, впервые представленному в части 1 этой серии. Если вы чувствуете себя потерянным, я настоятельно рекомендую вам прочитать его.

Хорошо. Мы используем радарный датчик, чтобы отслеживать самолет во времени. Нам интересно узнать о состоянии x_k плоскости, где k обозначает временной шаг. Состояние содержит динамические свойства самолета, которые мы хотим оценить. Это может быть, например, положение, скорость, крен, рыскание и т. Д.

У нас есть некоторые знания о динамике самолета между временными шагами, которую мы представляем как модель движения. Однако в модели есть неопределенность, поэтому мы рассматриваем ее как распределение вероятностей.

Выражаясь словами, модель утверждает, что состояние x_k выводится из распределения возможных значений x_k, и что это распределение зависит от предыдущего состояния x_ { к-1}. Мы также можем выразить модель движения как

Это уравнение говорит то же самое, но в этой формулировке мы используем детерминированную функцию f () и случайную величину q_ {k-1}. Таким образом, выражаясь словами, мы имеем, что состояние x_k является функцией предыдущего состояния x_ {k-1} и некоторого случайного шума движения q_ {k-1}, который является стохастическим (т. е. взятым из некоторого распределения).

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

Проще говоря, модель утверждает, что измерение y_k основано на распределении возможных значений измерений y_k, и что это распределение зависит от состояния x_k . Мы также можем выразить модель измерения как

В этой формулировке у нас есть детерминированная модель измерения h (), которая получает текущее состояние x_k, а также случайную величину r_k. Это говорит о том, что измерение y_k является функцией состояния x_k, а также некоторого стохастического (т. Е. Взятого из некоторого распределения) шума измерения r_k.

Мы ищем способ объединить эти две модели, чтобы, наблюдая измерения с радара, мы могли сформулировать следующую плотность:

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

Мы можем выразить эту плотность, вычислив два уравнения: прогноз и обновление.

Уравнение прогнозирования использует апостериорную величину предыдущего временного шага k-1 вместе с моделью движения для прогнозирования текущего состояния x_k. Это убеждение затем обновляется с помощью уравнения обновления с использованием теоремы Байеса для объединения наблюдаемого измерения y_k с моделью измерения и прогнозируемым состоянием. Затем мы получаем апостериорное распределение! Для следующего измерения мы просто повторяем эти шаги, и вместо этого текущая апостериорная зона становится предыдущей апостериорной.

Хорошо, теперь я в курсе. Итак, что такое фильтр Калмана?

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

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

Здесь на помощь приходит фильтр Калмана. Фильтр Калмана построен на одной ключевой концепции.

Причина в том, что гауссовские плотности обладают множеством хороших свойств:

  1. Если мы извлекаем значения из гауссиана и выполняем линейную операцию (т. Е. Умножение и / или сложение), эти значения все равно будут распределяться в соответствии с гауссианом.
  2. Еще одно приятное свойство гауссовских плотностей - то, что они самосопряжены. Это означает, что если у нас есть гауссовское правдоподобие и гауссово априорное, то апостериорное также гарантированно будет гауссовым.
  3. Наконец, гауссовские плотности можно полностью описать их первыми двумя моментами: средним и дисперсией.

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

Линейные и гауссовские модели движения и измерения могут быть выражены как

Как и раньше, мы можем выразить обе эти модели как функции, а не как плотности.

Имея это на месте, мы можем переписать уравнения прогноза и обновления, где все плотности являются гауссианами.

Мы ищем уравнения для вычисления моментов, отмеченных выше оранжевым и пурпурным цветом: прогнозируемое среднее и ковариация, а также обновленное среднее и ковариация. Я собираюсь пропустить вывод, но в итоге мы получим следующее

Вот и все, теперь у нас есть аналитическое решение для вычисления апостериорного распределения состояния x_k для каждого временного шага! Поскольку мы получаем гауссовское вычисление, MMSE или MAP тривиальны, поскольку среднее апостериорное служит обоими. Еще один способ увидеть все это так:

«Апостериорная (пурпурная) средняя - это оптимальная оценка состояния, а апостериорная (пурпурная) ковариация - это неопределенность оценки».

Прежде чем перейти к практическому примеру, давайте кратко рассмотрим каждую строку и разберемся, что происходит.

  1. Мы получаем прогнозируемое среднее значение, беря прошлое апостериорное среднее и умножая его на матрицу A_ {k-1}, что имеет смысл, поскольку A_ {k-1} описывает, как государство со временем развивается.
  2. Прогнозируемая ковариация вычисляется аналогичным образом, где мы дважды умножаем прошлую апостериорную ковариацию на A_ {k-1} и добавляем Q_ {k-1}. Мы добавляем ковариацию Q_ {k-1}, поскольку у нас есть неопределенность в нашей модели движения, поэтому она добавляется к преобразованной ковариации.
  3. v_k называется нововведением и может рассматриваться как представление новой информации, полученной при сравнении реального измерения с прогнозируемым измерением, которое мы получаем из модели измерения.
  4. S_k представляет собой прогнозируемую ковариацию измерения. R_k представляет неопределенность измерения в модели измерения, а с S_k мы объединяем неопределенность прогнозируемого состояния с неопределенностью модели измерения.
  5. K_k называется усилением Калмана и представляет, насколько прогнозируемое состояние и ковариация должны быть скорректированы с учетом новой информации, полученной в результате измерения.
  6. Апостериорное среднее вычисляется путем взятия прогнозируемого среднего значения и его корректировки с учетом новой полученной информации. Как отмечалось выше, K_k - это коэффициент масштабирования, определяющий, сколько v_k следует добавить.
  7. Апостериорная ковариация вычисляется путем принятия предсказанной ковариации и корректировки ее с учетом информации, полученной в результате измерения. Новая информация позволяет снизить неопределенность относительно состояния, поэтому стоит знак минус.

Да, математика хороша, но я все еще немного заблудился ...

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

Для начала мы можем представить себе, что у нас есть две разные плоскости (или размеры): плоскость измерения (красная, обозначена Y) и состояние самолет (синий, обозначается X). Состояние x_k со временем изменяется на уровне состояний. Проблема в том, что мы не можем получить доступ или явно наблюдать за состоянием Плоскость в любом случае, у нас есть доступ только к плоскости измерения. Плоскость измерения - это место, где мы наблюдаем измерения, которые вызваны состоянием.

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

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

Хорошо, а как это использовать на практике?

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

В примере с игрушкой мы будем использовать пример с самолетом, о котором я упоминал ранее. Чтобы упростить задачу, предположим, что самолет летит на постоянной высоте. Итак, в этом случае мы можем выбрать включение xy-положения, а также xy-скорости в состояние

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

Параметр T обозначает время выборки, которое используется в системе, а σ² используется в качестве коэффициента масштабирования для ковариационной матрицы (т. Е. Указывает, сколько неопределенности у нас есть в модели. ). Причина, по которой ковариационная модель выглядит так странно, заключается в том, что мы используем дискретную модель CV (подробнее см. Это). Это важно, поскольку наш выбор времени выборки T повлияет на производительность фильтра (я более подробно остановлюсь на этой теме в более поздней публикации).

В нашем случае, когда у нас есть движение в обоих направлениях x и y, модель движения становится

Компоненты 1D ковариации Σ для каждого направления имеют индексы x и y соответственно. Это связано с тем, что в обоих направлениях не обязательно должно быть одинаковое количество шума.

Итак, теперь мы знаем, как сформулировать A и Q. Перейдем к модели измерения. Теперь эта модель довольно проста. Мы предполагаем, что мы измеряем положение самолета на каждом временном шаге, и измерения являются стохастическими (с учетом аддитивного гауссовского шума). Это можно выразить как

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

Прежде чем мы перейдем к реализации фильтра Калмана для этой системы, мы будем использовать эти две модели для генерации данных движения и измерений. Мы будем использовать эти данные позже, чтобы протестировать фильтр Калмана и посмотреть, насколько хорошо он работает. Для этого нам понадобится скрипт, который может моделировать систему и создавать все параметры модели (обратите внимание, что оператор @ совпадает с np.matmul в numpy, поэтому A @ B совпадает с np.matmul (A, Б)).

После запуска simulate_model.py мы получаем следующий график, иллюстрирующий, как позиционные компоненты (x и y) состояния эволюционировали в течение 20 временных шагов (с временем выборки T = 1), а также наблюдаемые измерения. Теперь имейте в виду, что мы моделируем систему, и именно так мы узнаем, что такое истинное состояние.

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

Теперь, когда фильтр Калмана установлен, мы можем запустить его и посмотреть, как он работает с нашими смоделированными данными. Для этого мы напишем сценарий, который объединяет функциональные возможности, найденные в kalman_filter.py и simulate_model.py.

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

Просто взглянув на график, становится ясно, что фильтр Калмана дает лучшие оценки x- и y-положения, чем если бы можно было просто использовать необработанные измерения. Превосходные характеристики фильтра Калмана по сравнению с простыми измерениями также могут быть проанализированы численно. Используя евклидово расстояние в качестве метрики ошибки для координат x и y, мы можем вычислить среднеквадратичную ошибку (RMSE) как для фильтра Калмана, так и для измерений. Для этого сценария длиной 20 временных шагов RMSE для каждого метода составляет

(20 time-step simulation)
RMSE Kalman Filter: 0.2917
RMSE Measurements:  0.4441

Фильтр Калмана имеет более низкое значение RMSE, чем измерения, с довольно большим запасом. Поскольку 20 временных шагов - это довольно мало, давайте посмотрим, верны ли результаты RMSE для более длительного моделирования, менее подверженного статистической неопределенности.

(100,000 time-step simulation)
RMSE Kalman Filter: 0.3162
RMSE Measurements:  0.4241

Как и раньше, фильтр Калмана - лучший вариант, чем просто измерения.

(Сравнение было сосредоточено только на позиционных состояниях, поскольку их легче построить. Но не забывайте, что с нашей выбранной моделью движения мы также получаем оценки x- и y-скоростей из фильтра Калмана! )

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

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

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

Спасибо за прочтение!