Ответ 1
TL; DR, см. код и рисунок внизу.
Я думаю, что фильтр Калмана может хорошо работать в вашем приложении, но для этого потребуется немного больше думать о динамике/физике кайта.
Я бы настоятельно рекомендовал прочитать эту веб-страницу. Я не имею никакого отношения к знанию автора или не знаю, но я потратил около дня, пытаясь опрокинуть фильтры Калмана, и эта страница действительно заставила меня щелкнуть.
Коротко; для системы, которая является линейной и которая имеет известную динамику (т.е. если вы знаете состояние и входы, вы можете предсказать будущее состояние), она обеспечивает оптимальный способ объединения того, что вы знаете о системе, чтобы оценить ее истинное состояние. Умный бит (который берет на себя вся матричная алгебра, которую вы видите на страницах, описывающих его) состоит в том, как оптимально сочетать две части информации, которые у вас есть:
-
Измерения (которые подвержены "шуму измерения", то есть датчики не идеальны)
-
Динамика (т.е. как вы верите, что состояния эволюционируют под воздействием входных данных, которые подвержены "шуму процесса", что является всего лишь способом сказать, что ваша модель отлично не соответствует действительности).
Вы указываете, насколько вы уверены в каждом из них (через матрицы со-дисперсий R и Q соответственно), а коэффициент Kalman Gain определяет, сколько вы должны верить ваша модель (т.е. текущая оценка вашего состояния) и насколько вы должны верить своим измерениям.
Без дальнейших церемоний вы можете создать простую модель вашего кайта. То, что я предлагаю ниже, является очень простой возможной моделью. Возможно, вы знаете больше о динамике кайта, чтобы создать лучший.
Рассматривая кайт как частицу (очевидно, упрощение, настоящий кайт является расширенным телом, поэтому имеет ориентацию в трех измерениях), который имеет четыре состояния, которые для удобства мы можем записать в векторе состояния:
x= [x, x_dot, y, y_dot],
Где x и y - позиции, а _dot - скорости в каждом из этих направлений. Из вашего вопроса я предполагаю, что есть два (потенциально шумных) измерения, которые мы можем записать в векторе измерения:
z= [x, y],
Мы можем записать матрицу измерений ( H, обсуждаемую здесь, и observation_matrices в pykalman библиотека):
z= H x = > H= [[1, 0, 0, 0], [0, 0, 1, 0]]
Затем нам нужно описать динамику системы. Здесь я буду предполагать, что никакие внешние силы не действуют, и что нет никакого затухания в движении Кайта (с большим знанием, вы можете быть в состоянии сделать лучше, это эффективно обрабатывает внешние силы и затухание как неизвестное/немоделированное нарушение).
В этом случае динамика для каждого из наших состояний в текущем образце "k" как функция состояний в предыдущих образцах "k-1" дается как:
x (k) = x (k-1) + dt * x_dot (k-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]].
Теперь мы можем использовать фильтр Kalman в 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 - просто номер образца).
Предположим, вы посмотрите на сюжет выше и подумайте, что он выглядит слишком ухабистым. Как вы могли это исправить? Как обсуждалось выше, фильтр Калмана воздействует на две части информации:
- Измерения (в данном случае двух наших состояний, x и y)
- Динамика системы (и текущая оценка состояния)
Динамика, зафиксированная в приведенной выше модели, очень проста. В буквальном смысле они говорят, что позиции будут обновляться текущими скоростями (очевидным, физически разумным образом) и что скорости остаются постоянными (это явно не является физически истинным, но отражает нашу интуицию, что скорости должны медленно меняться).
Если мы считаем, что оценочное состояние должно быть более плавным, один из способов добиться этого - сказать, что мы меньше уверены в наших измерениях, чем наша динамика (т.е. мы имеем более высокий observation_covariance относительно нашего state_covariance).
Начиная с конца вышеприведенного кода, исправьте observation covariance до 10x ранее оцененное значение, установка 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. Точки - это измерения, пунктирная линия - это оценки состояний для периода обучения фильтра, сплошная линия - это оценки состояний для периода "on-line".
И информация о времени (на моем ноутбуке).
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