Что ж, для меня фильтры Калмана были огромной проблемой, но, в конце концов, я добился этого, и здесь я делюсь некоторыми вещами, которые я узнал во время проекта.
Фильтры Калмана - общая картина
Фильтр Калмана - это модель, которая позволяет сгладить зашумленные измерения. Вы синтезируете свои измерения и некоторые априорные знания о мире. Вы также отслеживаете, насколько ваше прогнозируемое состояние отличается от ваших измерений, и синтезируете эту ошибку в свои прогнозы. По сути, это алгоритм фильтра Калмана:
- Предскажите свое следующее состояние (x)
- Убедитесь, что вы знаете, насколько сомневаетесь в своем прогнозе (P).
- Обновите свое состояние с помощью измерения
- Обновите свою неуверенность
Часть Прогноз
(формулы немного упрощены)
x - ваше состояние
F - ваши знания о своем мире
P - ваша неуверенность в своем состоянии
Q - ковариационная неопределенность
x ’, P’ - ваше прогнозируемое состояние и неопределенность
Вы предсказываете свое следующее состояние, используя свои знания, и делаете то же самое со своей неуверенностью:
x’ = F * x
P’ = F * P + Q
Немного матричной алгебры
Хорошо, но как я могу обновить свое состояние, выполняя простое умножение? Начнем с раздела знаний. Мы пытаемся предсказать следующее состояние движущегося автомобиля, поэтому я знаю следующее:
x’ = x + vx * t
y’ = y + vy * t
vx = vx
vy = vy
Вы прогнозируете свое положение (x ' и y') - и у вас уже есть все остальные переменные: ваше время (t) и ваша скорость. (vx и vy); ваши предыдущие позиции (x и y). Хорошо, у вас их нет на самом деле в начале, но это нормально, потому что вы можете составить несколько случайных величин и позволить фильтру Калмана их найти.
И вот самое лучшее. Вы можете записать свои знания в виде одной матрицы-умножения:
Вы делаете в основном то же самое со своей матрицей неопределенности (P ’= F * P + Q); но после корректировки с вашими временными переменными вы также добавляете Q - дополнительный термин. Я не буду вдаваться в подробности, но ключевым моментом является то, что ваши знания о мире немного сложнее, чем я упоминал ранее:
v’ = v + vt + at
x’ =x + vt + 1/2*at²
Итак, теперь мы используем полные кинематические формулы и добавляем также ускорение (посмотрите на последние члены с a). В нашей модели мы предполагаем, что ускорение равно 0 в долгосрочной перспективе, но оно может варьироваться в зависимости от измерений. Итак, мы добавляем его как шум, однако мы знаем некоторые подробности об этом шуме (как он связан со временем).
Итак, чем больше время между двумя прогнозами, тем больше неопределенность в отношении ускорения; поэтому мы добавляем шум Q к неопределенности, полученной из a.
Обработка измерения - часть обновления
При обработке измерения мы делаем следующее:
- вычисление разностного члена, который в основном выглядит следующим образом: измерение - состояние
- вычисление так называемого коэффициента усиления Калмана, который выводится из неопределенности P
- регулировка состояния x с помощью разностного члена P, скорректированного усилением Калмана
- регулировка P с усилением Калмана
Таким образом, чем больше погрешность P, тем меньше фактический эффект измерения.
Чем больше неопределенность, тем больше мы используем наши априорные знания F о мире в режиме состояний. Чем меньше погрешность, тем больше мы используем фактические измерения.
Это означает, что если измерения становятся более шумными, чем модель, они начнут использовать их меньше; вот как мы получаем «эффект сглаживания».
2 типа измерений
В проекте Udacity мы моделируем ситуацию, когда мы измеряем положение нашей машины с помощью двух датчиков: лидара и радара.
Они иначе видят мир. Лидар может довольно хорошо измерить положение объекта по осям X и Y, ничего не зная о его скорости. Измерения координат радара зашумлены, но он также измеряет скорость объектов.
Таким образом, измерения радара представлены тремя значениями. Почему 3? Почему не 4, как для x, y, vx, vy? Это потому, что радар видит мир как полярную систему координат:
Он возвращает следующие значения:
- фи - угол между линией вашего объекта и осью
- rho - расстояние от радара до объекта
- d-rho - движение на секунду
Отсюда легко понять, как преобразовать полярные измерения в декартову систему координат.
x = cos (phi) * rho
у = грех (фи) * ро
vx = cos (phi) * d-rho
vy = грех (фи) * д-ро
Фактически, мы можем отбросить измерения vx, vy и просто передать измерения x, y в наш существующий фильтр Калмана.
Но подожди минутку
Таким образом, мы подаем шумные измерения положения радара в фильтр, но не используем очень ценные измерения скорости! Таким образом, вместо преобразования полярных измерений в декартовы, нам нужно напрямую подавать полярные измерения.
Когда вы вводите измерения в метод обновления, вы выполняете следующие шаги:
Здесь самое интересное - термин ошибки y. Поскольку вы вводите вектор другой формы (phi, rho, drho вместо x, y), вам необходимо настроить матрицу H. Матрица H преобразует ваш вектор измерения z так, чтобы ваш член ошибки y имел ту же форму, что и состояние x. И это сложно, потому что ваш вектор z бывает разных форм! (1x2 для лидара, 1x3 для радара).
Для лидара это просто:
[[1,0,0,0],[0,1,0,0]]
Если вы умножите его на лидарное измерение, вы получите матрицу той же формы. Он также используется в расчетах коэффициента усиления Калмана, где он выводит необходимые значения неопределенности из вектора P.
Для радара все сложнее. Во-первых, вам нужно получить матрицу формы 2x1 из вектора 1x3, используя H. Во-вторых, вы также получаете неопределенность для значений скорости. В-третьих, (и эта часть для меня не совсем ясна) ваши предположения о равномерно распределенном шуме ускорения неприменимы в мире полярных измерений.
Короче говоря
Итак, когда вы используете 2 типа измерений, вам всегда нужно настраивать матрицу H.
- Для лидара используйте простую матрицу 2x4
- Для радара используйте так называемую матрицу Якоби, полученную на основе ваших радиолокационных измерений.
Несколько дополнительных советов:
- Убедитесь, что вы используете правильное время (dt)! Когда вы имеете дело с лидарными измерениями, может возникнуть ошибка в десятичном разряде, но это испортит ваши радарные измерения!
- Рекомендуется начать с модели, которая имеет дело только с лидарными измерениями. После этого подайте преобразованные (полярные в декартовы) радиолокационные измерения. Если это сработает, вы готовы добавить логику радара.
- Поскольку единственная разница между логикой радара и лидара заключается в части обновления, рекомендуется разработать метод обновления, который получает уже вычисленные значения H и H * x ценности.
Удачи;) Вы можете прочитать код Python здесь:
