Нежное введение в фильтр Калмана
Если, как и я, вы были разочарованы тонкостями фильтра Калмана (KF), вы попали в нужное место. Не слишком углубляясь в скучную базовую математику, я привожу прямое описание фильтра Калмана.
Предыстория: простой пример
Допустим, вы хотите смоделировать робота, который может двигаться в направлении X. Робот оснащен GPS, который сообщает нам его состояние. На временном шаге t такой робот находится в состоянии:
где вектор P обозначает положение робота, а вектор V — его скорость. Настоящий робот в 3D-мире будет иметь 3D-позицию (x, y, z) и скорость для каждой оси (то есть скорость робота в этом конкретном направлении), но пока не будем усложнять. Для робота, движущегося в одном направлении (например, x). Его положение и скорость следующие:
Тогда состояние робота
А что делает КФ? Почему мы хотим его использовать?
Фильтр Калмана по определению
Фильтр Калмана — это математический процесс, используемый для вычисления оценки истинного значения определенной переменной, когда измерительное устройство несовершенно или когда имеются случайные ошибки измерений из-за внешних условий (таких как погода).
Вернемся к нашему роботу…
Как оказалось, наша система GPS устарела, что делает невозможным отслеживание нашего робота в режиме реального времени. Как показано на графике ниже, GPS возвращает одно измерение каждые 5 секунд. Однако мы хотели бы знать местоположение робота каждую секунду; поэтому мы можем использовать фильтр Калмана, чтобы предсказать следующее местоположение (ожидая измерений GPS).
Как мы на самом деле получаем эти значения?
Я пытался не доходить до этой части, но, похоже, у меня нет выбора. Чтобы получить эти значения, нам нужно выполнить некоторые математические вычисления, но не волнуйтесь, я сведу их к минимуму.
Чтобы получить прогнозы фильтра Калмана, вам нужно рассчитать три вещи:
- Прирост Калмана - кг
- Текущая оценка (это ваше прогнозируемое значение)
- Ошибка в оценке (извините, ваша оценка не идеальна)
Но сначала давайте познакомимся с некоторыми обозначениями, которые мы собираемся использовать.
1. Расчет коэффициента усиления Калмана (KG)
Усиление Калмана (уравнение ниже) представляет собой значение от 0 до 1, которое говорит нам, какое внимание мы должны уделять выходным данным нашего датчика (в нашем случае GPS).
Мы можем заметить, что по мере того, как ошибка измерения приближается к нулю, усиление Калмана приближается к единице; это означает, что мы уверены в результатах наших измерений (GPS) и сосредоточимся на этом в нашем следующем прогнозе. И наоборот, большая ошибка измерения приведет к очень маленькому значению KG; поэтому очень низкий акцент на значение измерения.
Откуда берутся все эти ошибки, спросите вы. Некоторые датчики поставляются со своими оценками погрешности, и если наш GPS этого не делает, мы можем случайным образом инициализировать ошибку в измерении. Исходная ошибка в оценке также может быть инициализирована случайным образом. KF, как итерационный процесс, быстро исправит случайно назначенные значения в будущих итерациях.
2. Расчет оценок
Теперь, когда у нас есть усиление Калмана, мы можем рассчитать наш прогноз (также называемый оценкой). Мы просто добавляем нашу предыдущую оценку к разнице между результатом измерения и предыдущей оценкой. Если это звучит странно, доверьтесь своему чутью. Вот формула:
KF является линейной оценкой, поэтому предполагает, что новые оценки не будут далеко отходить от старых. Тем не менее, нам нужно проверить измерение (выход GPS), так как он может быть на что-то. На итерации 1 предыдущая оценка (или исходная оценка) может быть инициализирована случайным образом; на итерации 2 предыдущей оценкой будет оценка итерации 1; на итерации 3… (вы поняли).
Стоит отметить, что в случае с нашим роботом мы будем использовать последнее измерение, пока не появится новое. Другими словами, наши прогнозы в момент времени 5 ≤ t ‹ 10будут использовать GPS вывод (измерение) временного шага 5, поскольку он последний, который у нас есть.
3. Ошибка в оценке
KF всегда предполагает, что его оценки могут быть неверными (или, по крайней мере, не идеальными). Помните, что мы использовали начальную ошибку в оценке для расчета усиления Калмана? Теперь нам нужно обновить эту ошибку для использования следующей итерации фильтра Калмана.
Новая ошибка в оценке — это просто инверсия Калмановского усиления, умноженная на предыдущую ошибку в оценке. Помните, что когда усиление Калмана мало (приближается к нулю), это означает, что наша ошибка измерения велика. При поступлении таких зашумленных данных мы хотим обнулить истинное значение, медленно уменьшая ошибку в оценке. С другой стороны, если усиление Калмана велико (т. е. измерение является точным), этот шаг быстро уменьшит ошибку оценки, поскольку мы знаем, что эти новые измерения (данные GPS) очень быстро приведут нас к истинному значению.
Вот и все. Это все, что вам нужно знать, чтобы предсказать следующее положение (при постоянной скорости) робота, движущегося в одном направлении. Теперь вы можете попробовать подставить некоторые числа и посчитать. Или вы можете посмотреть, как я это делаю.
Учитывая эти начальные значения:
Вы можете применить три волшебные формулы, описанные ранее, для расчета следующих оценок.
На практике вы будете продолжать повторять этот процесс, попутно допуская новые значения измерений по мере того, как ваши оценки будут становиться все лучше и лучше.
Вот блок-схема 1D-фильтра Калмана, чтобы подвести итог:
Что дальше?
Я думал о включении многомерного фильтра Калмана в этот пост, но это определенно утомило бы читателя. Описанные здесь приемы применимы и к многомерной модели (о которой я напишу еще одну статью).
Прежде чем вы уйдете… Пожалуйста, выделите несколько секунд, чтобы похлопать этой статье и следовать за мной. Вы удивитесь, насколько обнадеживает такой жест.
PS: На создание этой статьи меня вдохновила отличная лекция на youtube.
