¿Cómo usar el filtro de Kalman en Python para datos de ubicación?
sensor noise (2)
[EDITAR] La respuesta de @Claudio me da un muy buen consejo sobre cómo filtrar los valores atípicos. Sin embargo, quiero comenzar a usar un filtro de Kalman en mis datos. Así que cambié los datos de ejemplo a continuación para que tenga un ruido de variación sutil que no sea tan extremo (que también veo mucho). Si alguien más pudiera darme alguna dirección sobre cómo usar PyKalman en mis datos, sería genial. [/EDITAR]
Para un proyecto de robótica, estoy tratando de rastrear una cometa en el aire con una cámara. Estoy programando en Python y pegué algunos resultados de ubicación ruidosos a continuación (cada elemento también tiene un objeto de fecha y hora incluido, pero los dejé afuera para mayor claridad).
[ # X Y
{''loc'': (399, 293)},
{''loc'': (403, 299)},
{''loc'': (409, 308)},
{''loc'': (416, 315)},
{''loc'': (418, 318)},
{''loc'': (420, 323)},
{''loc'': (429, 326)}, # <== Noise in X
{''loc'': (423, 328)},
{''loc'': (429, 334)},
{''loc'': (431, 337)},
{''loc'': (433, 342)},
{''loc'': (434, 352)}, # <== Noise in Y
{''loc'': (434, 349)},
{''loc'': (433, 350)},
{''loc'': (431, 350)},
{''loc'': (430, 349)},
{''loc'': (428, 347)},
{''loc'': (427, 345)},
{''loc'': (425, 341)},
{''loc'': (429, 338)}, # <== Noise in X
{''loc'': (431, 328)}, # <== Noise in X
{''loc'': (410, 313)},
{''loc'': (406, 306)},
{''loc'': (402, 299)},
{''loc'': (397, 291)},
{''loc'': (391, 294)}, # <== Noise in Y
{''loc'': (376, 270)},
{''loc'': (372, 272)},
{''loc'': (351, 248)},
{''loc'': (336, 244)},
{''loc'': (327, 236)},
{''loc'': (307, 220)}
]
Primero pensé en calcular manualmente los valores atípicos y luego simplemente eliminarlos de los datos en tiempo real. Luego leí sobre los filtros de Kalman y cómo están diseñados específicamente para suavizar los datos ruidosos. Así que después de algunas búsquedas encontré la biblioteca PyKalman que parece perfecta para esto. Como estaba un poco perdido en toda la terminología del filtro de Kalman, leí la wiki y algunas otras páginas sobre los filtros de Kalman. Tengo la idea general de un filtro de Kalman, pero estoy realmente perdido en cómo debería aplicarlo a mi código.
En los documentos de PyKalman encontré el siguiente ejemplo:
>>> from pykalman import KalmanFilter
>>> import numpy as np
>>> kf = KalmanFilter(transition_matrices = [[1, 1], [0, 1]], observation_matrices = [[0.1, 0.5], [-0.3, 0.0]])
>>> measurements = np.asarray([[1,0], [0,0], [0,1]]) # 3 observations
>>> kf = kf.em(measurements, n_iter=5)
>>> (filtered_state_means, filtered_state_covariances) = kf.filter(measurements)
>>> (smoothed_state_means, smoothed_state_covariances) = kf.smooth(measurements)
Simplemente sustituí las observaciones por mis propias observaciones de la siguiente manera:
from pykalman import KalmanFilter
import numpy as np
kf = KalmanFilter(transition_matrices = [[1, 1], [0, 1]], observation_matrices = [[0.1, 0.5], [-0.3, 0.0]])
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)])
kf = kf.em(measurements, n_iter=5)
(filtered_state_means, filtered_state_covariances) = kf.filter(measurements)
(smoothed_state_means, smoothed_state_covariances) = kf.smooth(measurements)
pero eso no me da ningún dato significativo. Por ejemplo, smoothed_state_means convierte en lo siguiente:
>>> smoothed_state_means
array([[-235.47463353, 36.95271449],
[-354.8712597 , 27.70011485],
[-402.19985301, 21.75847069],
[-423.24073418, 17.54604304],
[-433.96622233, 14.36072376],
[-443.05275258, 11.94368163],
[-446.89521434, 9.97960296],
[-456.19359012, 8.54765215],
[-465.79317394, 7.6133633 ],
[-474.84869079, 7.10419182],
[-487.66174033, 7.1211321 ],
[-504.6528746 , 7.81715451],
[-506.76051587, 8.68135952],
[-510.13247696, 9.7280697 ],
[-512.39637431, 10.9610031 ],
[-511.94189431, 12.32378146],
[-509.32990832, 13.77980587],
[-504.39389762, 15.29418648],
[-495.15439769, 16.762472 ],
[-480.31085928, 18.02633612],
[-456.80082586, 18.80355017],
[-437.35977492, 19.24869224],
[-420.7706184 , 19.52147918],
[-405.59500937, 19.70357845],
[-392.62770281, 19.8936389 ],
[-388.8656724 , 20.44525168],
[-361.95411607, 20.57651509],
[-352.32671579, 20.84174084],
[-327.46028214, 20.77224385],
[-319.75994982, 20.9443245 ],
[-306.69948771, 21.24618955],
[-287.03222693, 21.43135098]])
¿Podría un alma más brillante que yo darme algunas pistas o ejemplos en la dirección correcta? Todos los consejos son bienvenidos!
Por lo que puedo ver, el uso del filtro de Kalman tal vez no sea la herramienta adecuada en su caso.
¿Qué hay de hacerlo de esta manera? :
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)] )
Aquí la salida:
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
Vea here para más información y la fuente de la que obtuve el código anterior.
TL; DR, vea el código y la imagen en la parte inferior.
Creo que un filtro de Kalman podría funcionar bastante bien en tu aplicación, pero requerirá un poco más de reflexión sobre la dinámica / física de la cometa.
Recomiendo encarecidamente leer esta página web . No tengo ninguna conexión ni conocimiento del autor, pero pasé aproximadamente un día tratando de ubicarme alrededor de los filtros de Kalman, y esta página realmente lo hizo clic para mí.
Brevemente; para un sistema que es lineal y que tiene una dinámica conocida (es decir, si conoce el estado y las entradas, puede predecir el estado futuro), proporciona una forma óptima de combinar lo que sabe sobre un sistema para estimar su estado verdadero. El bit inteligente (que se encarga de todo el álgebra matricial que ve en las páginas que lo describen) es la forma en que combina de manera óptima las dos piezas de información que tiene:
Mediciones (que están sujetas a "ruido de medición", es decir, sensores que no son perfectos)
Dinámica (es decir, cómo cree que evolucionan los estados en función de las entradas, que están sujetas al "ruido de proceso", que es solo una forma de decir que su modelo no coincide perfectamente con la realidad).
Usted especifica qué tan seguro está de cada uno de estos (a través de las matrices de covarianza R y Q respectivamente), y la ganancia de Kalman determina cuánto debe creer su modelo (es decir, su estimación actual de su estado) y cuánto debe. cree tus medidas
Sin más preámbulos, construyamos un modelo simple de tu cometa. Lo que propongo a continuación es un posible modelo muy simple. Quizás sepas más sobre la dinámica de la cometa, así que puedes crear una mejor.
Tratemos la cometa como una partícula (obviamente, una simplificación, una cometa real es un cuerpo extendido, por lo que tiene una orientación en 3 dimensiones), que tiene cuatro estados que, por conveniencia, podemos escribir en un vector de estado:
x = [x, x_dot, y, y_dot],
Donde x e y son las posiciones, y los puntos son las velocidades en cada una de esas direcciones. A partir de su pregunta, asumo que hay dos mediciones (potencialmente ruidosas), que podemos escribir en un vector de medición:
z = [x, y],
Podemos anotar la matriz de medición ( aquí se explica H y observation_matrices pykalman observation_matrices en la biblioteca de pykalman ):
z = H x => H = [[1, 0, 0, 0], [0, 0, 1, 0]]
Entonces necesitamos describir la dinámica del sistema. Aquí asumiré que no hay fuerzas externas que actúen, y que no hay amortiguación en el movimiento de la cometa (con más conocimiento puede ser capaz de hacerlo mejor, esto trata efectivamente las fuerzas externas y la amortiguación como una perturbación desconocida / no modelada).
En este caso, la dinámica para cada uno de nuestros estados en la muestra actual "k" como una función de los estados en las muestras anteriores "k-1" se da como:
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)
Donde "dt" es el paso del tiempo. Asumimos que (x, y) la posición se actualiza según la posición y la velocidad actuales, y la velocidad permanece sin cambios. Dado que no se dan unidades, podemos decir que las unidades de velocidad son tales que podemos omitir "dt" de las ecuaciones anteriores, es decir, en unidades de posición_unidades / muestra_intervalo (supongo que sus muestras medidas están en un intervalo constante). Podemos resumir estas cuatro ecuaciones en una matriz dinámica como ( F se analiza aquí y transition_matrices pykalman transition_matrices en la biblioteca de pykalman ):
x (k) = Fx (k-1) => F = [[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0 , 0, 1]].
Ahora podemos probar el filtro de Kalman en python. Modificado de su código:
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()
Lo que produjo lo siguiente, muestra que hace un trabajo razonable de rechazar el ruido (azul es la posición x, rojo es la posición y, y el eje x es solo el número de muestra).
Supongamos que miras la trama de arriba y piensas que se ve demasiado irregular. ¿Cómo pudiste arreglar eso? Como se discutió anteriormente, un filtro de Kalman está actuando sobre dos piezas de información:
- Mediciones (en este caso de dos de nuestros estados, xey)
- Dinámica del sistema (y la estimación actual del estado)
Las dinámicas capturadas en el modelo anterior son muy simples. Tomados literalmente, dicen que las posiciones se actualizarán con las velocidades actuales (de una manera obvia, físicamente razonable), y que las velocidades se mantienen constantes (esto claramente no es físicamente cierto, pero capta nuestra intuición de que las velocidades deben cambiar lentamente).
Si pensamos que el estado estimado debería ser más suave, una forma de lograrlo es decir que tenemos menos confianza en nuestras mediciones que en nuestra dinámica (es decir, tenemos una mayor state_covariance observation_covariance , en relación con nuestra state_covariance ).
Comenzando desde el final del código anterior, fije la observation covariance a 10 veces el valor estimado previamente, se em_vars que em_vars se em_vars como se muestra para evitar la em_vars de la covarianza de observación (vea here )
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()
Lo que produce la gráfica a continuación (mediciones como puntos, estimaciones de estado como línea de puntos). La diferencia es bastante sutil, pero espero que pueda ver que es más suave.
Finalmente, si desea usar este filtro instalado en línea, puede hacerlo usando el método filter_update . Tenga en cuenta que esto utiliza el método de filter lugar del método smooth , ya que el método smooth solo se puede aplicar a las mediciones por lotes. Más here :
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()
El gráfico a continuación muestra el rendimiento del método de filtro, incluidos los 3 puntos encontrados utilizando el método filter_update . Los puntos son mediciones, la línea discontinua son las estimaciones de estado para el período de entrenamiento del filtro, la línea continua son las estimaciones de estados para el período "en línea".
Y la información de tiempo (en mi portátil).
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