Tutorial del filtro de Kalman
(kalmanfilter.net)- El filtro de Kalman es un algoritmo que usa en conjunto mediciones de sensores con ruido y un modelo dinámico imperfecto para estimar el estado actual y el siguiente, junto con su incertidumbre
- El tutorial sigue numéricamente, con un ejemplo de seguimiento por radar de una aeronave, el proceso de combinar el valor predicho y el valor medido usando la distancia (r) y la velocidad (v) como vector de estado
- Si se usan la medición inicial (10,000m), (200m/s) y un intervalo de muestreo de (5s), en un modelo de velocidad constante la siguiente posición se predice como (11,000m), y el ruido de medición (R) y el ruido de proceso (Q) se reflejan en la covarianza
- La segunda medición, (11,020m) y (202m/s), es más incierta, pero la ganancia de Kalman (K) combina con pesos la predicción y la medición para calcular el estado actualizado como (11,009.37m) y (201.43m/s)
- Después de la inicialización, se repite el bucle de predicción-actualización; en una implementación real también hay que considerar fórmulas de actualización de covarianza estables, como la forma de Joseph, y el tratamiento de mediciones atípicas
El problema de estimación que resuelve el filtro de Kalman
- El filtro de Kalman es un algoritmo para estimar y predecir el estado de un sistema en un entorno con incertidumbre
- Datos de sensores con ruido de medición
- Factores externos desconocidos
- Diferencias entre el modelo dinámico y el movimiento real
- Se usa en seguimiento de objetos, navegación, robótica, control, análisis de mercados financieros y predicción meteorológica
- Si se aplica a la estimación de la trayectoria del mouse de una computadora, puede reducir el ruido y corregir el temblor de la mano para producir una ruta de movimiento más estable
- El tutorial está diseñado para entender el filtro de Kalman mediante ejemplos numéricos y explicaciones intuitivas, más que con una explicación matemática compleja
- También incluye un ejemplo en el que un filtro de Kalman mal planteado no logra seguir correctamente un objeto, y cómo corregirlo
Ruta de aprendizaje
- Este proyecto está organizado para aprender el filtro de Kalman en tres niveles de profundidad
- Resumen de una sola página: explica la idea central y las ecuaciones esenciales sin derivaciones, asumiendo conocimientos básicos de estadística y álgebra lineal
- Tutorial web gratuito basado en ejemplos: desarrolla la intuición con ejemplos numéricos y avanza paso a paso hasta la derivación de las ecuaciones del filtro de Kalman, indicando que no se requieren conocimientos previos
- Kalman Filter from the Ground Up: incluye 14 ejemplos numéricos completamente resueltos, gráficas y tablas de rendimiento, Extended Kalman Filter, Unscented Kalman Filter, fusión de sensores y lineamientos de implementación
La necesidad de predicción vista con un ejemplo de seguimiento por radar
- En un radar que rastrea una aeronave, la aeronave es el sistema y la posición que se debe estimar es el estado del sistema
- El radar orienta un haz estrecho hacia la dirección de la aeronave, así que para decidir a dónde enviar el siguiente haz necesita predecir la posición futura
- Si falla la predicción, el haz puede apuntar en la dirección equivocada y perder el seguimiento
- Se necesita un modelo dinámico que represente el movimiento del sistema a lo largo del tiempo
- En el ejemplo simplificado de una dimensión, se asume que la aeronave se mueve en línea recta acercándose o alejándose del radar
- El radar calcula la distancia (r) a partir del tiempo de transmisión y recepción del pulso
- También puede medir la velocidad (v) mediante el efecto Doppler
- Si en (t_0) se miden con mucha precisión una distancia de (10,000m) y una velocidad de (200m/s), el intervalo de muestreo es (\Delta t=5s) y se asume velocidad constante, entonces la siguiente posición es (11,000m)
- (\Delta r = v \cdot \Delta t)
- (r_{t_1}=10,000+200\cdot5=11,000m)
Ruido de medición y ruido de proceso
- Las mediciones reales de radar no son perfectamente precisas, así que incluso si varios radares miden en el mismo instante pueden dar valores ligeramente distintos
- Esta variación se expresa como ruido de medición
- No solo hay que calcular la estimación del estado, sino también qué tan confiable es esa estimación
- El modelo dinámico tampoco es perfecto
- Aunque se asuma que la aeronave se mueve a velocidad constante, el movimiento real puede diferir por factores externos como el viento
- Estas influencias impredecibles son el ruido de proceso
- El filtro de Kalman proporciona en conjunto la estimación del estado actual, la predicción del estado futuro y la incertidumbre de cada uno
- Es un algoritmo óptimo que minimiza la incertidumbre de la estimación del estado, bajo la condición de que el sistema y el ruido sigan los supuestos del modelo
Vector de estado e inicialización
- El estado del sistema en el ejemplo está compuesto por la distancia (r) y la velocidad (v) de la aeronave
[ \boldsymbol{x}= \begin{bmatrix} r\ v \end{bmatrix} ]
- La primera medición en (t_0) es la siguiente
[ \boldsymbol{z}_0= \begin{bmatrix} 10{,}000\ 200 \end{bmatrix} ]
- Como las mediciones tienen incertidumbre, a cada medición se le asocia una incertidumbre de medición en forma de varianza
- Desviación estándar de la medición de distancia: (4m)
- Desviación estándar de la medición de velocidad: (0.5m/s)
- La varianza es el cuadrado de la desviación estándar
[ \boldsymbol{R}_0= \begin{bmatrix} 16 & 0\ 0 & 0.25 \end{bmatrix} ]
- En este ejemplo se asume que los errores de medición de distancia y velocidad no están relacionados entre sí, por lo que los elementos fuera de la diagonal de la matriz de covarianza se fijan en 0
- En la etapa de inicialización, como la medición y el estado del sistema representan las mismas magnitudes físicas (r) y (v), se puede usar la primera medición como estimación inicial del estado
[ \hat{\boldsymbol{x}}_{0,0}= \boldsymbol{z}_0= \begin{bmatrix} 10{,}000\ 200 \end{bmatrix} ]
- Este método solo puede usarse en la etapa de inicialización
Etapa de predicción: propagación del estado y la covarianza
- La predicción calcula el estado del siguiente instante usando el estado actual y la matriz de transición de estado (\boldsymbol{F})
- En el modelo de velocidad constante se usa la siguiente ecuación
[ v_1=v_0=v ]
[ r_1=r_0+v_0\Delta t ]
- La ecuación de predicción de estado en forma matricial es la siguiente
[ \hat{\boldsymbol{x}}_{n+1,n}
\boldsymbol{F} \hat{\boldsymbol{x}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n ]
- (\boldsymbol{u}_n): variable de entrada
- (\boldsymbol{G}): matriz de transición de entrada
- En este ejemplo simple no hay entrada, así que (\boldsymbol{u}_n=0)
- Cuando (\Delta t=5s), la matriz de transición de estado es la siguiente, y el resultado de la predicción es (11,000m) y (200m/s)
[ \boldsymbol{F}= \begin{bmatrix} 1 & 5\ 0 & 1 \end{bmatrix} ]
[ \hat{\boldsymbol{x}}_{1,0}
\begin{bmatrix} 11{,}000\ 200 \end{bmatrix} ]
- La predicción de la covarianza no usa simplemente (\boldsymbol{F}\boldsymbol{P}), sino (\boldsymbol{F}\boldsymbol{P}\boldsymbol{F}^T)
[ \boldsymbol{P}_{n+1,n}
\boldsymbol{F} \boldsymbol{P}_{n,n} \boldsymbol{F}^T + \boldsymbol{Q} ]
- Si se excluye el ruido de proceso, la covarianza predicha es la siguiente
[ \boldsymbol{P}_{1,0}
\begin{bmatrix} 22.25 & 1.25\ 1.25 & 0.25 \end{bmatrix} ]
- La varianza de la velocidad se mantiene en (0.25) debido al modelo de velocidad constante
- La varianza de la distancia aumenta de (16) a (22.25) porque la incertidumbre de la velocidad incrementa la incertidumbre de la distancia con el tiempo
Incorporación del ruido de proceso
- Como la velocidad real de la aeronave puede verse afectada por factores externos impredecibles como el viento, se suma el ruido de proceso (\boldsymbol{Q}) a la predicción de la covarianza
- En el ejemplo se asume una desviación estándar de aceleración aleatoria de (\sigma_a=0.2m/s^2)
- La varianza es (\sigma_a^2=0.04m^2/s^4)
- Cuando (\Delta t=5s), la matriz de ruido de proceso es la siguiente
[ \boldsymbol{Q}
\begin{bmatrix} 6.25 & 2.5\ 2.5 & 1 \end{bmatrix} ]
- La covarianza predicha al sumar el ruido de proceso queda así
[ \boldsymbol{P}_{1,0}
\begin{bmatrix} 28.5 & 3.75\ 3.75 & 1.25 \end{bmatrix} ]
Etapa de actualización: combinación ponderada de predicción y medición
- En (t_1), la segunda medición es la siguiente
[ \boldsymbol{z}_1= \begin{bmatrix} 11{,}020\ 202 \end{bmatrix} ]
- Se asume que esta medición tiene mayor incertidumbre que la primera porque una fuerte ráfaga de ruido redujo la relación señal-ruido
- Desviación estándar de la distancia: (6m)
- Desviación estándar de la velocidad: (1.5m/s)
[ \boldsymbol{R}_1= \begin{bmatrix} 36 & 0\ 0 & 2.25 \end{bmatrix} ]
- Como los elementos diagonales de la covarianza predicha (\boldsymbol{P}_{1,0}) son menores que los de la covarianza de medición (\boldsymbol{R}_1), la incertidumbre del lado de la predicción es menor
- El filtro de Kalman no usa solo la predicción ni solo la medición, sino que las combina dando más peso al lado con menor incertidumbre
- El promedio ponderado en forma unidimensional es el siguiente
[ \hat{x}_{1,1}
K_1 z_1 + (1-K_1)\hat{x}_{1,0} ]
- (\boldsymbol{K}) es la ganancia de Kalman, y determina los pesos de la medición y la predicción para minimizar la incertidumbre de la estimación actualizada
Innovación, matriz de observación y ganancia de Kalman
- La ecuación de actualización del estado puede escribirse como una corrección sumada al valor predicho
[ \hat{\boldsymbol{x}}_{1,1}
\hat{\boldsymbol{x}}_{1,0} + \boldsymbol{K}_1 ( \boldsymbol{z}_1
\boldsymbol{H}\hat{\boldsymbol{x}}_{1,0} ) ]
- (\boldsymbol{z}1-\boldsymbol{H}\hat{\boldsymbol{x}}{1,0}) es la innovación (innovation) o residuo (residual), y representa la información que aporta la nueva medición
- (\boldsymbol{H}) es la matriz de observación o matriz de medición, que mapea las variables de estado a las magnitudes físicas realmente medidas
- En este ejemplo, como tanto el estado como la medición son distancia y velocidad, (\boldsymbol{H}=\boldsymbol{I})
- En general, la medición y el estado pueden pertenecer a dominios físicos distintos, como en el caso de un termómetro digital
- La ganancia de Kalman multivariable es la siguiente
[ \boldsymbol{K}_n
\boldsymbol{P}{n,n-1} \boldsymbol{H}^T ( \boldsymbol{H} \boldsymbol{P}{n,n-1} \boldsymbol{H}^T + \boldsymbol{R}_n )^{-1} ]
- La ganancia de Kalman calculada en el ejemplo es la siguiente
[ \boldsymbol{K}_1= \begin{bmatrix} 0.4048 & 0.6377\ 0.0399 & 0.3144 \end{bmatrix} ]
- El cálculo de la inversa de una matriz puede hacerse con
inv(A)de MATLAB onumpy.linalg.inv(A)de Python, pero en implementaciones reales suele ser mejor resolver directamente el sistema lineal conA\bonumpy.linalg.solve(A, b)en lugar de calcular la inversa explícita
Resultado de la actualización y reducción de la covarianza
- La innovación en este ejemplo es la siguiente
[ \boldsymbol{z}1-\hat{\boldsymbol{x}}{1,0}
\begin{bmatrix} 20\ 2 \end{bmatrix} ]
- Si se calcula el término de corrección con la ganancia de Kalman, se obtiene lo siguiente
[ \boldsymbol{K}_1 \begin{bmatrix} 20\ 2 \end{bmatrix}
\begin{bmatrix} 9.37\ 1.43 \end{bmatrix} ]
- La estimación actualizada del estado es la siguiente
[ \hat{\boldsymbol{x}}_{1,1}
\begin{bmatrix} 11{,}009.37\ 201.43 \end{bmatrix} ]
- Para la actualización de la covarianza multivariable se suele usar la forma de Joseph, que es numéricamente estable
[ \boldsymbol{P}_{n,n}
(\boldsymbol{I}-\boldsymbol{K}n\boldsymbol{H}) \boldsymbol{P}{n,n-1} (\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H})^T + \boldsymbol{K}_n \boldsymbol{R}_n \boldsymbol{K}_n^T ]
- También es común ver en la literatura una fórmula simplificada de actualización de la covarianza
[ \boldsymbol{P}_{n,n}
(\boldsymbol{I}-\boldsymbol{K}n\boldsymbol{H}) \boldsymbol{P}{n,n-1} ]
- En aritmética exacta, ambas formas producen el mismo resultado, pero en implementaciones por computadora la forma de Joseph suele ser numéricamente más estable
- En el ejemplo, la covarianza actualizada calculada con la fórmula simplificada es la siguiente
[ \boldsymbol{P}_{1,1}
\begin{bmatrix} 14.57 & 1.43\ 1.43 & 0.71 \end{bmatrix} ]
- Los elementos diagonales de la covarianza actualizada son menores que los de la covarianza predicha ((28.5, 1.25)) y los de la covarianza de medición ((36, 2.25))
- La información nueva reduce la incertidumbre de la estimación incluso si su propia incertidumbre es alta, y en teoría no debería ignorarse una nueva medición
- En implementaciones reales puede ser necesario rechazar mediciones no confiables, y los métodos de tratamiento de valores atípicos se explican en el capítulo Outlier Treatment del libro
Siguiente predicción y bucle repetitivo
- La etapa de predicción de la Iteration 1 es igual a la de Iteration 0, pero el punto de partida cambia a (\hat{\boldsymbol{x}}{1,1}) y (\boldsymbol{P}{1,1})
- El resultado de la predicción del estado es el siguiente
[ \hat{\boldsymbol{x}}_{2,1}
\boldsymbol{F} \hat{\boldsymbol{x}}_{1,1}
\begin{bmatrix} 12{,}016.5\ 201.43 \end{bmatrix} ]
- El resultado de la predicción de la covarianza es el siguiente
[ \boldsymbol{P}_{2,1}
\begin{bmatrix} 52.86 & 7.47\ 7.47 & 1.71 \end{bmatrix} ]
- Si pasa el tiempo sin nuevas mediciones, la incertidumbre aumenta de forma natural, por lo que en la etapa de predicción la varianza vuelve a crecer
- La incertidumbre de la velocidad aumenta todavía más la incertidumbre de la distancia
- Por eso la varianza de la distancia crece más rápido que la de la velocidad
- El ejemplo muestra las tres etapas del filtro de Kalman
- Inicialización: se realiza una sola vez al comienzo
- Predicción: propaga el siguiente estado y su incertidumbre con el modelo dinámico
- Actualización: combina la nueva medición y la predicción mediante la ganancia de Kalman
- Después de la inicialización, el filtro de Kalman sigue funcionando continuamente en un bucle de predicción-actualización
1 comentarios
Opiniones en Hacker News
Siempre termino diciendo que aprender el filtro de Kalman por separado es hacerlo en el orden inverso, porque es fácil perderse la gran revelación que habilita la teoría que lo rodea.
Para entenderlo bien, conviene ver en orden mínimos cuadrados (regresión lineal), mínimos cuadrados recursivos y el filtro de información (otra formulación del KF).
Entonces se ve que el KF no es más que mínimos cuadrados recursivos reformulado para priorizar la eficiencia del paso de actualización.
Este PDF da un resumen conciso: http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/...
Aun así, existe la curiosidad intelectual, así que hace falta un camino que permita mantener esa curiosidad mientras uno avanza poco a poco hacia la comprensión.
Releer The Six (Not So) Easy Pieces sigue siendo valioso aunque no lo entienda, y jugar con el gato de Arnold permite experimentar, sin un procedimiento científico riguroso, conceptos que originalmente estaban detrás de la puerta del contexto, con una curiosidad casi de simio desnudo.
http://gerdbreitenbach.de/arnold_cat/cat.html
En una dimensión, se obtiene la distribución previa a partir de la predicción lineal X'1 = X0*a + b, donde mean(X'1) = mean(X0)*a + b y var(X'1) = var(X0)*a^2, y a y b representan la dinámica asumida.
La distribución posterior gaussiana es un promedio ponderado por precisión entre la distribución previa y la observación, así que X1 = (1 - K)X'1 + YK, y K = (1/var(X'1))/(1/var(X'1) + 1/var(Y)), donde Y es una observación gaussiana.
Si repites esto, obtienes el filtro de Kalman; y si conoces la linealidad de las gaussianas multidimensionales, generalizarlo a varias dimensiones también resulta intuitivo.
Dicho eso, puede que la linealidad de las gaussianas multidimensionales y la distribución posterior gaussiana en sí no sean temas fáciles.
Cada vez que surge este tema, también aparece este recurso, y viceversa: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Pyt...
También me encanta que use Jupyter notebooks.
Parece que todavía no hay una herramienta de cálculo simbólico para distribuciones de probabilidad.
Me refiero, por ejemplo, a una herramienta que multiplique dos funciones de densidad de probabilidad gaussianas multivariadas para obtener la matriz de covarianza, o que, al definir todos los componentes de un filtro de Kalman (el modelo de predicción y el proceso de observación), genere las fórmulas necesarias, como lambdify de sympy.
Eso sí, no sé si Sympy puede manejar la distribución condicional necesaria para el filtro de Kalman, es decir, la posterior bayesiana.
En cualquier caso, si quieres jugar con filtros de Kalman en Sympy, conviene trabajar directamente con la media y la varianza, o con la matriz de covarianza.
Referencia: https://reference.wolfram.com/language/howto/WorkWithStatist...
Y también: https://reference.wolfram.com/language/ref/MultinormalDistri...
https://www.squiggle-language.com/docs
Si Q y R son constantes, como suele ocurrir, la ganancia converge rápido y el filtro de Kalman termina siendo casi igual a un filtro exponencial con una etapa de predicción añadida.
Para mucha gente esta explicación es mucho más fácil de entender y también encaja bien con la forma en que se usa en la práctica.
Porque normalmente Q y R se ajustan manualmente hasta que “se ve bien”, y luego no se vuelven a cambiar.
Además, en vez de ajustar manualmente varios valores como Q y R, solo hay que ajustar una ganancia.
¿Uno simplemente los ajusta hasta que el resultado parezca razonable? Entonces no entiendo cómo funciona correctamente incluso en situaciones que no estén completamente sobreajustadas.
Por ejemplo, si estás siguiendo un ave en un video, podrías elegir algún Q, pero las estadísticas del ruido pueden cambiar según la hora del día. ¿Qué se hace en ese caso?
Artículo relacionado: Kalman filter from the ground up - https://news.ycombinator.com/item?id=37879715 - octubre de 2023, 150 comentarios
También me pregunto cuál sería el mejor año para poner en el título de arriba.
El filtro de Kalman aparece dentro de un tema más general en Optimization by Vector Space Methods, de David G. Luenberger, John Wiley and Sons, Inc., Nueva York, 1969.
Se me acaba de ocurrir esto. ¿Sería posible tomar casos donde solo hay testimonios de testigos, codificarlos de alguna forma como vectores y tratarlos con un filtro de Kalman para reforzar el valor probatorio de las observaciones?
La idea sería tratar tanto las mentiras como las imprecisiones como “errores”.
Pienso en las Phoenix lights o los ovnis en general, fantasmas, experiencias cercanas a la muerte y, en algo más cotidiano, acusaciones de violación.
El mejor recurso casi siempre es este: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Pyt...
Es excelente incluso si no usas Python, y realmente cubre muy bien todo el panorama.
¿Alguien más vio las clases sobre filtros de Kalman de Michael van Biezem, el de la corbata de moño, cuando aprendía este tema?
https://www.youtube.com/watch?v=CaCcOwJPytQ&list=PLX2gX-ftPV...
La única frase que realmente hay que saber es esta: “Este filtro lleva el nombre de Rudolf E. Kálmán (19 de mayo de 1930–2 de julio de 2016). En 1960, Kálmán publicó su famoso artículo en el que describía una solución recursiva al problema del filtrado lineal de datos discretos”