Как использовать фильтр Калмана в Python для данных о местоположении?

TL; DR, смотрите код и картинку внизу.

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

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

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

1. Измерения (которые подвержены «измерительному шуму», т. Е. Датчики не идеальны)

2. Динамика (то есть то, как, по вашему мнению, состояния развиваются в зависимости от входных данных, которые подвержены «технологическому шуму», что является просто способом сказать, что ваша модель не полностью соответствует реальности).

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

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

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

x = [x, x_dot, y, y_dot],

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

z = [x, y],

Мы можем записать матрицу измерений (H обсуждался здесь и observation_matrices в pykalman библиотеке):

z = H x => H = [[1, 0, 0, 0], [0, 0, 1, 0]]

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

В этом случае динамика для каждого из наших состояний в текущей выборке «k» как функция состояний в предыдущих выборках «k-1» задается как:

х (к) = х (к-1) + dt * x_dot (к-1)

x_dot (k) = x_dot (k-1)

y (k) = y (k-1) + dt * y_dot (k-1)

y_dot (k) = y_dot (k-1)

Где «dt» - временной шаг. Мы предполагаем, что положение (x, y) обновляется на основе текущего положения и скорости, а скорость остается неизменной. Учитывая, что единицы измерения не указаны, мы можем просто сказать, что единицы скорости таковы, что мы можем опустить «dt» в приведенных выше уравнениях, то есть в единицах position_units / sample_interval (я предполагаю, что ваши измеренные образцы находятся с постоянным интервалом). Мы можем суммировать эти четыре уравнения в матрицу динамики как (рассмотренный здесь F и transition_matrices в pykalman библиотеке):

x (k) = Fx (k-1) => F = [[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]].

Теперь мы можем попробовать использовать фильтр Калмана в Python. Изменено из вашего кода:

from pykalman import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import time

measurements = np.asarray([(399,293),(403,299),(409,308),(416,315),(418,318),(420,323),(429,326),(423,328),(429,334),(431,337),(433,342),(434,352),(434,349),(433,350),(431,350),(430,349),(428,347),(427,345),(425,341),(429,338),(431,328),(410,313),(406,306),(402,299),(397,291),(391,294),(376,270),(372,272),(351,248),(336,244),(327,236),(307,220)])

initial_state_mean = [measurements[0, 0],
                      0,
                      measurements[0, 1],
                      0]

transition_matrix = [[1, 1, 0, 0],
                     [0, 1, 0, 0],
                     [0, 0, 1, 1],
                     [0, 0, 0, 1]]

observation_matrix = [[1, 0, 0, 0],
                      [0, 0, 1, 0]]

kf1 = KalmanFilter(transition_matrices = transition_matrix,
                  observation_matrices = observation_matrix,
                  initial_state_mean = initial_state_mean)

kf1 = kf1.em(measurements, n_iter=5)
(smoothed_state_means, smoothed_state_covariances) = kf1.smooth(measurements)

plt.figure(1)
times = range(measurements.shape[0])
plt.plot(times, measurements[:, 0], 'bo',
         times, measurements[:, 1], 'ro',
         times, smoothed_state_means[:, 0], 'b--',
         times, smoothed_state_means[:, 2], 'r--',)
plt.show()

В результате получилось следующее, показывающее, что он выполняет разумную работу по подавлению шума (синий - это положение x, красный - положение y, а ось x - это просто номер образца).

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

1. Измерения (в данном случае двух наших состояний, x и y)
2. Системная динамика (и текущая оценка состояния)

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

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

Начиная с конца приведенного выше кода, исправьте observation covariance до 10-кратного значения, оцененного ранее, установка em_vars, как показано, требуется, чтобы избежать повторной оценки ковариации наблюдения (см. здесь)

kf2 = KalmanFilter(transition_matrices = transition_matrix,
                  observation_matrices = observation_matrix,
                  initial_state_mean = initial_state_mean,
                  observation_covariance = 10*kf1.observation_covariance,
                  em_vars=['transition_covariance', 'initial_state_covariance'])

kf2 = kf2.em(measurements, n_iter=5)
(smoothed_state_means, smoothed_state_covariances)  = kf2.smooth(measurements)

plt.figure(2)
times = range(measurements.shape[0])
plt.plot(times, measurements[:, 0], 'bo',
         times, measurements[:, 1], 'ro',
         times, smoothed_state_means[:, 0], 'b--',
         times, smoothed_state_means[:, 2], 'r--',)
plt.show()

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

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

time_before = time.time()
n_real_time = 3

kf3 = KalmanFilter(transition_matrices = transition_matrix,
                  observation_matrices = observation_matrix,
                  initial_state_mean = initial_state_mean,
                  observation_covariance = 10*kf1.observation_covariance,
                  em_vars=['transition_covariance', 'initial_state_covariance'])

kf3 = kf3.em(measurements[:-n_real_time, :], n_iter=5)
(filtered_state_means, filtered_state_covariances) = kf3.filter(measurements[:-n_real_time,:])

print("Time to build and train kf3: %s seconds" % (time.time() - time_before))

x_now = filtered_state_means[-1, :]
P_now = filtered_state_covariances[-1, :]
x_new = np.zeros((n_real_time, filtered_state_means.shape[1]))
i = 0

for measurement in measurements[-n_real_time:, :]:
    time_before = time.time()
    (x_now, P_now) = kf3.filter_update(filtered_state_mean = x_now,
                                       filtered_state_covariance = P_now,
                                       observation = measurement)
    print("Time to update kf3: %s seconds" % (time.time() - time_before))
    x_new[i, :] = x_now
    i = i + 1

plt.figure(3)
old_times = range(measurements.shape[0] - n_real_time)
new_times = range(measurements.shape[0]-n_real_time, measurements.shape[0])
plt.plot(times, measurements[:, 0], 'bo',
         times, measurements[:, 1], 'ro',
         old_times, filtered_state_means[:, 0], 'b--',
         old_times, filtered_state_means[:, 2], 'r--',
         new_times, x_new[:, 0], 'b-',
         new_times, x_new[:, 2], 'r-')

plt.show()

График ниже показывает эффективность метода фильтрации, включая 3 точки, найденные с помощью метода filter_update. Точки - измерения, пунктирная линия - оценки состояния для периода обучения фильтра, сплошная линия - оценки состояний для периода "онлайн".

И информация о сроках (на моем ноутбуке).

Time to build and train kf3: 0.0677888393402 seconds
Time to update kf3: 0.00038480758667 seconds
Time to update kf3: 0.000465154647827 seconds
Time to update kf3: 0.000463008880615 seconds

Из того, что я вижу, использование фильтрации Калмана, возможно, не подходит для вашего случая.

А как насчет того, чтобы сделать это ТАК? :

lstInputData = [
    [346, 226 ],
    [346, 211 ],
    [347, 196 ],
    [347, 180 ],
    [350, 2165],  ## noise
    [355, 154 ],
    [359, 138 ],
    [368, 120 ],
    [374, -830],  ## noise
    [346, 90  ],
    [349, 75  ],
    [1420, 67 ],  ## noise
    [357, 64  ],
    [358, 62  ]
]

import pandas as pd
import numpy as np
df = pd.DataFrame(lstInputData)
print( df )
from scipy import stats
print ( df[(np.abs(stats.zscore(df)) < 1).all(axis=1)] )

Вот вывод:

      0     1
0    346   226
1    346   211
2    347   196
3    347   180
4    350  2165
5    355   154
6    359   138
7    368   120
8    374  -830
9    346    90
10   349    75
11  1420    67
12   357    64
13   358    62
      0    1
0   346  226
1   346  211
2   347  196
3   347  180
5   355  154
6   359  138
7   368  120
9   346   90
10  349   75
12  357   64
13  358   62

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