Как кодировать и применять фильтр Калмана в 2-мерной системе

Освоение важного инструмента для построения автономных систем

Представленный в 1960 году Рудольфом Э. Калманом в его статье «Новый подход к задачам линейной фильтрации и прогнозирования», фильтр Калмана является эффективным оптимальным оценщиком для линейных систем, который может помочь отфильтровать шум датчиков и неточности. в измерениях [1].

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

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

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

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

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

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

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

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

Поскольку и скорость, и положение объекта зависят друг от друга в этой системе, определяется ковариационная матрица состояний P, чтобы зафиксировать корреляционные отношения между переменными:

Однако в момент времени t=0 и положение, и скорость не зависят друг от друга, что позволяет вычислить P0, начальное значение P, как:

Для учета ошибок измерения, которые могут присутствовать в системе из-за ограничений датчиков (или из-за ошибки наблюдения человека), ковариационная матрица шума измерения, R , просто определяется как:

Для учета отклонений, вызванных шумом и неопределенностями в системе, ковариационная матрица шума процесса, Q, определяется как:

Наконец, необходимо определить матрицу измерений, Z, вектор-столбец, содержащий все измерения. Поскольку в этой системе имеется только одно измеряемое значение,

Фух. Вот и все матрицы. Пора переходить к математике!

Для объекта, движущегося с постоянной скоростью, положение объекта должно увеличиваться на кратное (в зависимости от временного шага) значение скорости. Это дает нам следующие уравнения:

Оба уравнения также могут быть представлены в следующей матричной форме:

Чтобы обновить Z, матрица состояния измерения, H, определяется как:

Проще говоря, H — это матрица, которая работает для переноса прогнозируемых значений из пространства состояний в пространство измерений [3]. Поскольку система измеряет только положение объекта, матрица состоит из одной единицы, которая помогает записать элемент положения из X-шляпы (t+1).

Последняя матрица, которая должна быть предсказана, это P. Можно использовать следующее уравнение:

После прогнозирования различных матриц фильтр Калмана сравнивает прогнозируемые значения с измерениями, полученными в момент времени t+1. Во-первых, инновационная ковариационная матрица S определяется как:

Затем усиление Калмана, K, рассчитывается как:

С помощью коэффициента Калмана новую оценку состояния системы можно найти с помощью:

Наконец, новую ковариационную матрицу ошибок состояния системы можно найти с помощью:

После того, как X(t+1) и P(t+1) найдены, алгоритм возвращается к предсказанию X-hat(t+2), и уравнения с 8 по 14 повторяются для каждого нового измерения. После каждого цикла обновления сохраняется только новый X.

Ограничения фильтра Калмана

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

Поскольку фильтр является дискретным, время выборки для измерений должно быть фиксированным и постоянным [1]. В то же время система, к которой применяется фильтр, должна быть линейной, а шум ожидается гауссовским белым шумом [2].

Хотя время дискретизации можно легко зафиксировать, и можно предположить, что большая часть шума следует распределению Гаусса из-за центральной предельной теоремы, линейность системы не является чем-то, что можно гарантировать все время.

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

Применение фильтра Калмана к двумерной системе

Чтобы продемонстрировать возможности фильтра Калмана, здесь рассматривается простая задача двумерного отслеживания. Целевой объект в этой системе движется в двух направлениях, x, и y с постоянной скоростью 12,5 мс^-1. Для этой системы принято стандартное отклонение погрешности измерения равное 10 м для обоих направлений.

Инициализация необходимых компонентов кода

import numpy as np
import matplotlib.pyplot as plt

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

Затем была импортирована библиотека matplotlib.pyplot, чтобы можно было строить различные графики на основе данных, сгенерированных при выполнении алгоритма фильтра Калмана.

#opening the file and saving data as NumPy array
data = np.genfromtxt("MeasGT.csv", delimiter=",")

Затем данные были извлечены из CSV-файла «MeasGT» с использованием функции NumPy «genfromtxt()». Данные были сохранены с использованием переменной 'data' и содержат следующие столбцы данных (по порядку): время измерения, измерение положения по координате x, измерение положения по координате y, наземная истина для положение координаты x и истинное значение положения координаты y.

#defining system conditions 
velocity = 12.5
measurementerrorstcd = 10

Наконец, были определены условия для цели в системе. Поскольку цель движется с постоянной скоростью, переменной «скорость» было присвоено значение 12,5. Для x и y также было указано стандартное отклонение ошибки измерения, равное 10.

Инициализация состояния системы

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

#estimating initial velocities, and defining X, initial state variable matrix
initialxvel = data[1][1] - data[0][1]
initialyvel = data[1][2] - data[0][2]
X = np.array([[data[0][1]], [data[0][2]], [initialxvel], [initialyvel]])

Здесь линейная аппроксимация с использованием первых двух измерений положения использовалась для расчета начальных скоростей для обоих направлений x и y. После чего X определяется матрица переменных исходного состояния с использованием первых измерений (при t = 1) и аппроксимированных начальных скоростей. Однако, поскольку эта система включает 2 измерения, X имеет здесь 4 переменные, в отличие от той, что определена в уравнении 1. 4 переменные: x, y, x-точка и y-точка.

#defining estimated P, initial state covarariance matrix
P = np.array([[measurementerrorstcd**2, 0 , 0, 0], [0 , measurementerrorstcd**2, 0, 0], [0,0,(velocity/3)**2,0],[0,0,0,(velocity/3)**2]])

После определения X следующим шагом было определение P, ковариационной матрицы состояний. Следуя уравнению 3 (и расширив его, чтобы оно содержало 4 переменные), P был определен с использованием предоставленного стандартного отклонения ошибки измерения и скорости. Поскольку можно предположить, что максимальная скорость x или y, с которой может двигаться цель, ограничена 12,5 мс^-1 (это случай, когда цель движется только в одном направлении, x или y), скорость используется в расчет P.

Кроме того, поскольку цель теперь движется в двух разных направлениях, R, ковариационная матрица измерения, больше не может быть определена как матрица 1 × 1. Вместо этого R будет определен следующим образом:

Точно так же Q, A, и H также должны быть определены как более крупные матрицы, чтобы алгоритм работал. Таким образом, эти матрицы определяются с помощью следующего кода:

#defining R, the measurement covariance matrix
R = np.array([[measurementerrorstcd**2, 0], [0, measurementerrorstcd**2]])
#defining an estimated Q, the process noise covariance matrix
Q =  10 * np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]])
#defining A, the state transition matrix, sampling time = 1 
A = np.array([[1,0,1,0],[0,1,0,1],[0,0,1,0],[0,0,0,1]])
#defining H, the state to measurement matrix
H = np.array([[1,0,0,0], [0,1,0,0]])

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

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

Подобно R, A, матрица переходов состояний также отличается от уравнения 8, поскольку система теперь имеет 2 измерения. Здесь A становится:

Поскольку время выборки робота можно принять равным 1 секунде, обе дельты заменяются на 1. В то же время H просто расширяется от матрицы 2x1 до матрицы 2x4, как показано на рисунке. код.

#defining an array to store data
kalmanarray = np.array([[data[0][1], data[0][2], 0, 0]])

Прежде чем перейти к следующей части алгоритма, была также определена переменная под названием «kalmanarray» для хранения всех данных, сгенерированных алгоритмом. Поскольку первая точка измерения принимается за начальную точку, первая позиция x и y определяется как первая точка в массиве. В этот момент скорость принимается равной 0.

Прогноз и обновление

Для этой части алгоритма был написан «цикл for», чтобы циклы прогнозирования и обновления можно было повторить для всех точек измерения.

for entry in data[1:]:
#defining Z, measurement output state
Z = np.array([[entry[1]], [entry[2]]])
#prediction stage
Xpredicted = np.matmul(A,X)
Ppredicted = np.matmul((np.matmul(A,P)), A.transpose()) + Q
#defining S, innovation covariance matrix
S = np.matmul((np.matmul(H, Ppredicted)), H.transpose()) + R
#updating stage
Kalmangain = np.matmul((np.matmul(Ppredicted, H.transpose())), np.linalg.inv(S))
Zpredicted = np.matmul(H, Xpredicted)
X = Xpredicted + np.matmul(Kalmangain, (Z - Zpredicted))
kalmanarray = np.concatenate((kalmanarray, X.transpose()),axis=0)
#updating state covariance P
P = Ppredicted - np.matmul((np.matmul(Kalmangain, S)), Kalmangain.transpose())

Поскольку первая точка данных измерения уже сохранена в выходном массиве kalmanarray, цикл начинается только со второй записи и далее. На этапе прогнозирования уравнения 8 и 10 используются для расчета прогнозируемых матриц как для X-hat(t+1) («Xpredicted» в коде), так и для P-hat(t+1) («Ppredicted»). После этого с помощью уравнения 11 рассчитывается инновационная ковариационная матрица S.

Затем, на этапе обновления, уравнение 12 используется для расчета усиления Калмана («Калмангаин») измерений. Затем это используется для обновления и поиска новой оценки состояния системы X(t+1)('X') с использованием уравнения 13. Это новое значение X(t+1 ) сохраняется в «kalmanarray» с помощью функции «конкатенации» NumPy. Наконец, используя уравнение 14, новая ковариационная матрица ошибок состояния системы находится и сохраняется как «P». Затем цикл продолжается для каждого измерения, сохраненного в массиве данных.

Создание визуализаций данных

Чтобы лучше понять данные, сохраненные данные в «kalmanarray» использовались для построения графика. Во-первых, был настроен стиль графика, а оси x и y были помечены положением x или y в зависимости от того, какое измерение он отражал:

#customising the style of the plots
plt.style.use("ggplot")
plt.xlabel("x position")
plt.ylabel("y position")

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

#invert x axis since object is moving negatively
plt.gca().invert_xaxis()

Затем из массива «данных» были собраны точки для наземной истины, и была создана диаграмма рассеяния вместе с линией наилучшего соответствия для диаграммы рассеяния.

#collating the x and y points
groundxpoints = np.array([point[3] for point in data])
groundypoints = np.array([point[4] for point in data])
#creating a scatter plot
plt.scatter(groundxpoints, groundypoints)
#creating a best fit line for the scatter graph
ag, bg = np.polyfit(groundxpoints, groundypoints, 1)
plt.plot(groundxpoints, ag*groundxpoints+bg)

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

#repeat for measured points and the points calculated by the kalman filter
measurexpoints = np.array([point[1] for point in data])
measureypoints = np.array([point[2] for point in data])
plt.scatter(measurexpoints, measureypoints)
am, bm = np.polyfit(measurexpoints, measureypoints, 1)
plt.plot(measurexpoints, am*measurexpoints+bm)
kalmanxpoints = np.array([point[0] for point in kalmanarray])
kalmanypoints = np.array([point[1] for point in kalmanarray])
plt.scatter(kalmanxpoints, kalmanypoints)
ak, bk = np.polyfit(kalmanxpoints, kalmanypoints, 1)
plt.plot(kalmanxpoints, ak*kalmanxpoints+bk)
plt.show()

Функция «показать» используется последней для распечатки построенного графика.

Возможно, вам будет интересно прочитать:

Вездесущая платформа потокового видео (с красным логотипом) никуда не денется
Я говорю о YouTube (а не о Netflix!)среде. datadriveninvestor.com

Осознанное переживание боли: почему сложно разработать настоящий «роботизированный опыт боли
Роботы, возможно, никогда не будут чувствовать боль так, как мы, независимо от того, насколько развиты технологииmedium.com»

Повышение уровня кодирования

Спасибо, что являетесь частью нашего сообщества! Перед тем, как ты уйдешь:

👏 Хлопайте за историю и подписывайтесь на автора 👉
📰 Смотрите больше контента в публикации Level Up Coding
🔔 Подписывайтесь на нас: Twitter | ЛинкедИн | "Новостная рассылка"

🚀👉 Присоединяйтесь к коллективу талантов Level Up и найдите прекрасную работу