Una introducción no matemática a los filtros de Kalman para programadores

 (praveshkoirala.com)
6 puntos por GN⁺ 2023-08-03 | 1 comentarios | Compartir por WhatsApp
  • Como las mediciones de posición y velocidad en el mundo real siempre fluctúan, el filtro de Kalman puede entenderse como una forma de combinar varias fuentes de información imperfectas para producir una estimación de estado más confiable
  • En el ejemplo de la posición de un barco, un modelo de velocidad del motor de 10m/s falla por el viento y las olas, y sensores como el GPS tampoco son siempre precisos por ruido o fallas
  • El código de ejemplo hace que 1000 pasajeros generen cada uno una estimación basada en velocidad y una medición del sensor, y luego combina ambos valores con un promedio ponderado para mostrar la intuición del filtro de Kalman
  • La confiabilidad de las fuentes de información se calcula con la varianza de las mediciones, y se usa 1/variance para confiar menos en lo que fluctúa más y dar más peso a lo que es consistente
  • Cuando el sensor funciona bien, se sigue más el valor del sensor, y en tramos inestables como t=3 y t=6 su influencia disminuye automáticamente para mantener una estimación más estable

Cuándo se necesita un filtro de Kalman

  • Un filtro de Kalman puede verse como un embudo que comprime varias fuentes de información con ruido en un único valor estadístico más preciso
  • Matemáticamente involucra álgebra lineal, probabilidad y cálculo, pero aquí el foco está en la intuición más que en la teoría completa
  • Supongamos que un barco parte del puerto en x=0, se mueve en una dimensión y el motor le da una velocidad constante de 10m/s
  • En un mundo ideal, después de 2 segundos su posición sería 2 * 10 = 20m, pero en el mundo real la velocidad y la posición no se mantienen con exactitud por el motor, el viento y las olas
  • Por lo tanto, es difícil estar seguro de la posición real del barco usando solo la fórmula de posición

Los sensores tampoco son perfectos

  • Si hay un sensor como GPS, se puede medir directamente la posición en un momento dado, pero las mediciones del sensor tampoco son siempre precisas
  • En el segundo 3, el GPS podría dar valores cercanos a la posición real como 29.998m o 30.002m, pero muy rara vez también podría devolver algo muy desviado como 100m
  • En zonas sin cobertura satelital, el sensor GPS podría prácticamente dejar de funcionar
  • Si el sensor nunca quedara fuera de línea y pudiera medir cualquier valor deseado con precisión arbitraria, no haría falta un filtro de Kalman
  • El filtro de Kalman puede combinar varias fuentes de información, como estimaciones de posición basadas en velocidad, estimaciones por GPS, radar o sonar, para estimar la posición con mayor precisión

Estimación de posición en código

  • El ejemplo supone que hay 1000 pasajeros en el barco y que cada pasajero tiene su propio dispositivo GPS
  • Cada pasajero primero estima su nueva posición sumando velocidad y variaciones de factores externos a la posición anterior
from random import gauss
def new_position(last):
    velocity = 10
    wind = gauss(0, 2)
    wave = gauss(0, 0.1)
    return last + velocity + wind + wave
  • gauss genera valores aleatorios positivos o negativos, y el segundo parámetro representa la magnitud de la variación
  • Como no se puede medir directamente el efecto del viento y las olas, el ejemplo modela ese ruido con números aleatorios de media 0 y desviación estándar 2 y 0.1
  • En el segundo paso, el sensor devuelve una medición sumando ruido del sensor a la posición real
def sensor(t):
    if t == 3:

# oops, passing through a thunderstorm. GPS fluctuating!
        sensor_noise = gauss(5, 10)
    elif t == 6:

# uh-oh, satellite unavailable!
        sensor_noise = gauss(-5, 10)
    else:
        sensor_noise = gauss(0, 1)
    return true_position[t] + sensor_noise
  • En t=3 se modela un GPS inestable por una tormenta eléctrica, y en t=6 una situación sin satélite disponible
  • Incluso en el mismo instante, las mediciones del sensor difieren entre pasajeros

Trayectoria real y promedio simple

  • La posición real del barco se da con la siguiente lista
true_position = [0, 9, 19.2, 28, 38.1, 48.5, 57.2, 66.2, 77.5, 85, 95.2]
  • El barco parte del puerto en x=0, está a 9m al cabo de 1 segundo, a 19.2m al cabo de 2 segundos, y luego sigue moviéndose según los valores de la lista
  • El objetivo de los pasajeros es predecir la posición de cada segundo tan exactamente como sea posible a partir de mediciones con ruido y poco confiables
  • Si en t=1 la estimación basada en velocidad de un pasajero es 9.37 y la medición del sensor es 8.98, el promedio simple es 9.17
  • Cuando la posición real es 9m, ese promedio simple tiene menos error que la estimación por velocidad, pero es peor que la medición del sensor en ese ejemplo

Promedio ponderado y confiabilidad

  • Como método mejor que el promedio simple, se usa un promedio ponderado
def combine(A, B, trustA, trustB):
    total_trust = trustA + trustB
    return (A * trustA + B * trustB) / total_trust
  • combine(9.37, 8.98, 10, 1) produce 9.33, más cercano a 9.37, porque se confía más en la estimación por velocidad
  • combine(9.37, 8.98, 1, 10) da 9.01, más cercano a 8.98, porque se confía más en la medición del sensor
  • Este promedio ponderado basado en confiabilidad es la intuición central del filtro de Kalman y el centro de gravedad de su capacidad para combinar datos
  • En qué fuente de información confiar más se decide con la varianza
    • Se confía menos en la fuente cuyas conclusiones fluctúan mucho
    • Se da más peso a la fuente cuyas conclusiones son consistentes
    • Si 4 de 10 estaciones de radio dicen que va a llover y 6 dicen que estará despejado, mientras que 9 de 10 sitios web dicen que va a llover, los sitios web tienen menor varianza y son más confiables

Paso de actualización

  • La actualización completa genera estimaciones por velocidad y mediciones del sensor para cada pasajero, y luego calcula la confiabilidad a partir de la varianza de ambos conjuntos de mediciones
from statistics import variance

def update(t, last):
    velocity_updates = []
    sensor_updates = []

    for p in range(1000):
        velocity_updates.append(new_position(last[p]))
        sensor_updates.append(sensor(t))

    fluctuation_velocity = variance(velocity_updates)
    fluctuation_sensor = variance(sensor_updates)

    trust_velocity = 1 / fluctuation_velocity
    trust_sensor = 1 / fluctuation_sensor

    combined = []
    for p in range(1000):
        combined.append(combine(
            A=velocity_updates[p],
            B=sensor_updates[p],
            trustA=trust_velocity,
            trustB=trust_sensor
        ))

    return sensor_updates, velocity_updates, combined
  • Como la confiabilidad baja a medida que aumenta la varianza, se usa 1/variance
  • Cada pasajero actualiza su posición de forma individual
  • Una vez que se completan las actualizaciones de posición de todos los pasajeros, la estimación de la posición real del barco puede inferirse a partir del promedio de las posiciones de los pasajeros

Cómo leer los resultados

  • La función update_plot guarda la posición real, la estimación del sensor, la estimación por velocidad y la estimación combinada de cada instante para generar una gráfica
  • El bucle principal sigue actualizando la estimación de posición en cada instante usando la mejor estimación actual que tienen los pasajeros
  • La envolvente alrededor de cada línea en la gráfica representa la incertidumbre; cuanto más ancha sea, mayor es la incertidumbre sobre ese valor
  • En el intervalo t=0.75 a t=1, cuando el sensor funciona normalmente, la estimación de posición combinada es mejor que usar solo la estimación por velocidad, aunque puede ser peor que usar solo la medición del sensor
  • En el intervalo t=2 a t=4, cuando el sensor falla, la estimación combinada muestra mejores resultados que usar solo la medición defectuosa del sensor
  • En el intervalo t=4 a t=5, cuando el sensor se recupera, el filtro de Kalman vuelve a empezar a preferir más al sensor

Apéndice: gauss y varianza

  • normal distribution function como gauss(0, 0.1) y gauss(0, 2) generan principalmente valores aleatorios cercanos a 0
  • El segundo parámetro, la desviación estándar, controla cuánto fluctúan las mediciones
    • gauss(0, 0.1) tiene alta probabilidad de dar valores pequeños cercanos a 0, como 0.06, -0.07 o 0.02
    • gauss(0, 2) tiene alta probabilidad de producir valores más dispersos, como 1.05, -1.06, 1.29 o -1.72
  • El código de ejemplo supone que el viento varía más y que las olas varían menos
  • La varianza es una medida de consistencia: si la consistencia es alta, la varianza es baja; si la consistencia es baja, la varianza es alta
  • Una distribución con desviación estándar 2 tiene varianza 4, y una con desviación estándar 0.1 tiene varianza 0.01

Lecturas relacionadas

1 comentarios

 
GN⁺ 2023-08-03
Opiniones de Hacker News

  • El artículo me pareció entretenido, pero la implementación está mal. El error más grande es que no propaga la incertidumbre a lo largo del eje temporal, por lo que subestima el error.
    También se nota en la gráfica: el rango de error debería incluir el estado real la mayor parte del tiempo, pero en el resultado no lo hace.
    No es que los pasajeros agreguen ruido a la estimación; desde el punto de vista de un pasajero, dado el estado en el momento k, el valor esperado del estado en el momento k+1 es simplemente position_k+1 = position_k + velocity * Delta_t. La dinámica real tiene ruido, y el filtro lo refleja sumándolo a la covarianza estimada.
    La razón por la que el código no se rompe de inmediato es que toma muchas muestras de la dinámica con 1000 pasajeros y calcula numéricamente la varianza del resultado, pero eso es bastante distinto de lo que normalmente se hace en la práctica.
    Además, que el GPS se vea afectado por el clima es un malentendido común, y en realidad no es así. Y la definición de consistencia que usa el artículo tampoco es la estándar. En teoría de estimación, decir que un estimador es consistente significa que, a medida que aumenta la cantidad de datos, la estimación converge al valor verdadero.
    Está bien que lo explique de forma accesible para el público general, pero parece que varios malentendidos están causando problemas. Soy estudiante de posgrado en teoría de estimación, así que puedo ayudar si quieren profundizar más.

    • ¿Podrías explicar un poco más la parte de que “la dinámica real tiene ruido, y el filtro lo refleja sumándolo a la covarianza estimada”? En particular, me interesa qué es la covarianza estimada y qué se le suma.

  • Me hizo preguntarme por qué en las clases universitarias de procesamiento de señales no enseñan el Kalman Filter de una forma tan simple. Es correcto enseñar matemáticamente los conceptos matemáticos, pero para la gente que no tiene suficiente contexto se pierde información.
    Hace tiempo enseñé la transformada discreta del coseno y la transformada wavelet con un enfoque centrado en imágenes, y dar primero la intuición en vez del rigor siempre funcionó mejor que hacerlo al revés.

    • Si eres estudiante universitario de procesamiento de señales, deberías tener los conocimientos previos necesarios. Aun así, es fácil olvidar lo básico.
      Puede haber varias razones por las que los profesores no dan primero la intuición. Puede ser que el profesor tenga una especialización más profunda en números y manipulación de ecuaciones que en intuición; que, como la capacidad docente no se recompensa, use el tiempo que tomaría explicarlo intuitivamente en propuestas de financiamiento o en gestionar estudiantes de doctorado; o que, una vez que entiende las matemáticas, la explicación intuitiva le parezca el “camino difícil” y su cerebro se niegue a volver atrás.
      Creo que la tercera es la razón principal. Esto también va más allá de la educación matemática: es la diferencia entre expertise y capacidad docente. Si quieres aprender a pegar un drive de golf, tal vez te convenga más aprender de alguien que empezó pegando 100 yardas y llegó de forma constante a 300, que de la persona que pega más lejos.
    • Totalmente de acuerdo. Uno de los profesores que me gustaban en la universidad, después de presentar un teorema, no pasaba directamente a la demostración; primero mostraba para qué era útil y luego lo demostraba.
    • Recuerdo que antes había una clase en línea del MIT con un enfoque de pulir la intuición sin fórmulas.

  • Como advertencia general para quien quiera implementar un Kalman Filter, para manejar la inestabilidad numérica conviene leer las primeras páginas de https://www.stat.berkeley.edu/~brill/Stat248/kalmanfiltering...

    • En ese paper se cita una generalización exacta e interesante del Kalman Filter para condiciones iniciales planas. Es la primera vez que la veo, y resuelve los problemas numéricos que surgen de la práctica común de elegir arbitrariamente una covarianza inicial grande.
      En la práctica, haciendo eso casi nunca vi artefactos numéricos visibles, pero es una solución bastante atractiva.

  • Como complemento a este artículo, si quieres una introducción más rigurosa y matemática a la familia de Kalman Filters, recomiendo mucho este libro: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Pyt...
    Es un libro escrito por un ingeniero de software que tuvo que implementar Kalman Filters por trabajo, así que la forma en que motiva y transmite los conceptos puede encajar bien con este público. Está escrito como notebooks de Jupyter interactivos, así que puedes clonar el repositorio, ejecutarlos y seguirlos por tu cuenta.
    Empieza con filtros simples, luego incorpora la regla de Bayes y se expande a distribuciones de probabilidad, mejorando gradualmente; por eso ofrece una vía de entrada suave hacia el Kalman Filter.

  • Hay un aspecto que falta. Cuando se calcula el promedio ponderado entre la predicción y la medición, los pesos del Kalman Filter pueden cambiar con el tiempo. Si no, creo que se llamaría de otra manera.
    Un buen ejemplo es medir con un solo sensor un valor que cambia lentamente. Por ejemplo, en un medidor de combustible conviene suponer que no cambia en cuestión de segundos, pero las mediciones pueden tener ruido, como el movimiento del combustible dentro del tanque.
    En ese caso, el Kalman Filter se ve como un filtro pasa-bajos de primer orden con una ganancia que decae exponencialmente. La frecuencia de corte cambia, de modo que al inicio puede encontrar el nivel en pocos segundos y después ignorar el ruido con una frecuencia de corte muy baja, como 0.01 Hz.

  • Es un buen artículo sobre una herramienta importante.
    Según entiendo, el filtro de Kalman lineal es la solución óptima para problemas lineales, y es relativamente fácil de entender e implementar. Pero la mayoría de las aplicaciones que he visto eran no lineales.
    El filtro de Kalman extendido y el filtro de Kalman Unscented son mucho más difíciles de entender e implementar, y había menos materiales y bibliotecas, además de ser menos útiles.
    Por ejemplo, al trabajar con un dispositivo CAN AHRS/GNSS para UAV pequeños, el filtro de Kalman extendido que vi en PX4 o Ardupilot era muy complejo y tenía muchos parámetros. Por eso resultó más simple partir de los principios básicos de los cuaterniones e ir corrigiendo poco a poco la solución del giroscopio hacia el “arriba” del acelerómetro y hacia el vector de inclinación del magnetómetro.
    Si la magnitud de la aceleración difiere mucho de 1G o si el vector del campo magnético difiere mucho de la intensidad del campo magnético terrestre local, se reduce el peso de la actualización de ese sensor o se omite, y se deja que el giroscopio siga avanzando. Probablemente el EKF sea la respuesta correcta, pero desistí de ajustarlo a una forma que fuera fácil de entender, construir, ajustar y diagnosticar.

    • Como referencia, el EKF no es mucho más complejo que un filtro de Kalman normal. Básicamente, se linealiza la dinámica o el modelo del sensor y luego se hace la actualización del filtro de Kalman con ese modelo lineal.
      Sin embargo, en un cuadricóptero la gran dificultad es la rotación. El modelo lineal del filtro de Kalman asume que todo está en un espacio euclidiano, pero las rotaciones están sobre una variedad. En el caso de los cuaterniones, esa variedad es el conjunto de cuaterniones unitarios.
      Si se aplica ingenuamente un EKF para estimar cuaterniones, dejan de ser cuaterniones unitarios y la estimación se arruina. Hay métodos bien conocidos para manejar esta restricción de variedad, pero las ecuaciones que he llevado a código están entre las más horribles que he visto.
      Como ejemplo simple, se puede pensar en un estado (x, y) que, por las leyes de la física, siempre debe estar sobre el círculo unitario. La dinámica real f(x, y) devuelve un nuevo punto sobre el círculo, pero la dinámica aproximada linealizada no garantiza permanecer sobre el círculo unitario, lo que puede llevar a estados no físicos o a una estimación de estado del EKF no física.
    • Usar lenguajes de programación probabilística puede facilitar mucho la implementación de filtros no lineales. La idea es implementar el modelo generativo y que la inferencia salga compilada.
      Algunas opciones que vale la pena revisar son ForneyLab.jl, Infer.net, Gen.jl y Pyro.
    • Si el material de clases públicas te parece bien, recomiendo este recurso: http://mocha-java.uccs.edu/ECE5550/index.html
    • Si sabes qué quieres modelar, es muy fácil usar un modelo que exprese el filtro de Kalman lineal tradicional.
      Pero hay muchas formas de usar un filtro de Kalman, y según desde dónde empieces, manejar correctamente las transformaciones no lineales puede volverse extremadamente engorroso.

  • Tengo miopía y astigmatismo, y descubrí que si cierro un ojo y miro algo como un reloj de pared, cada ojo produce una imagen distorsionada de forma distinta. Los dos ojos son ligeramente diferentes.
    Pero cuando miro el reloj con ambos ojos, la imagen es mucho más nítida y mejor que con cualquiera de los dos ojos por separado. El escenario del artículo en el que 1000 pasajeros de un barco reportan sus coordenadas GPS me hizo pensar en este fenómeno.
    El cerebro probablemente también use ampliamente algoritmos inteligentes como el filtro de Kalman.

    • Si te interesa ese fenómeno visual, se puede llamar un ejemplo de hiperagudeza. Casualmente, yo también llegué a conocer ese término después de observar lo mismo con mis propios ojos.

  • ¿Es correcto entender que el filtro de Kalman estima un valor mejor que un promedio simple a partir de observaciones con ruido?
    Por ejemplo, si se mide algo 3 veces y salen 7, 8 y 9, uno supondría que el valor real es 8. ¿El filtro de Kalman daría otra estimación?

    • Sí, pero con una advertencia importante. Las observaciones tienen que estar conectadas entre sí por algo como un mecanismo de transición. No se puede esperar que aplicar un filtro de Kalman a cualquier problema dé mejores resultados.
      El filtro de Kalman se usa tradicionalmente para estimar cosas que se mueven con el tiempo. Piensa en una persona en un video o en algún tipo de caminata aleatoria.
      Si se asume que entre dos instantes o mediciones consecutivas hay relaciones como la velocidad y la dirección actual, se puede mezclar la información del modelo de movimiento con la del modelo de medición ruidoso para estimar mejor la posición o el valor, o incluso todo el historial de movimiento.
      Si el modelo de movimiento está significativamente equivocado, la estimación no mejora. Muchas extensiones posteriores se enfocan en incluir modelos de movimiento más sofisticados, como el deslizamiento de ruedas en robótica.
    • Así es. A diferencia de un promedio simple, tiene un modelo del objeto que se quiere medir y de la medición en sí.
      Por ejemplo, si se mide una constante, se puede tomar como modelo básico una constante con incertidumbre inicial, por ejemplo una distribución Gaussian con desviación estándar, y mediciones que también tienen ruido Gaussian y desviación estándar. Se puede ajustar la incertidumbre inicial alrededor de la constante que se quiere estimar y la incertidumbre de las mediciones.
      En este ejemplo, el filtro de Kalman no se comporta como un promedio. Si las mediciones son buenas, es decir, tienen poca incertidumbre, converge rápidamente; si las mediciones son malas, la estimación fluctúa y tarda más en converger.
      Y no es cierto que el filtro de Kalman solo se use para cosas en movimiento. También se usa todo el tiempo para estimar constantes, solo que es más famoso en objetos en movimiento.
    • El filtro de Kalman es mejor que un promedio simple cuando las muestras no son independientes e idénticamente distribuidas. Si fueran independientes e idénticamente distribuidas, bastaría con promediarlas según la ley de los grandes números.
      El filtro de Kalman maneja casos en los que las muestras están correlacionadas debido a alguna dinámica lineal. La medición ni siquiera tiene que ser el objeto de interés en sí; también puede ser una función lineal de ese objeto con ruido Gaussian agregado.
      Por lo tanto, saber que en la primera medición se vio 7 cambia la probabilidad de ver 8 en la segunda medición. Si solo se promedian las muestras como arriba, en general no se converge al valor medio real.
    • El filtro de Kalman da la estimación óptima desde el punto de vista de máxima verosimilitud para modelos Gaussian multivariados.
      Lo que devolvería en el ejemplo depende del modelo exacto. Incluso en el escenario más simple imaginable, como en Bayes, hay que especificar con qué valor esperado se empieza.
    • Si hay historial previo, puede cambiar. Supongamos que mediste el mismo objeto antes y quieres reflejar en principio esa información previa, pero si las nuevas mediciones difieren mucho, quieres que el nuevo valor domine más.
      Si codificas esa expresión como un modelo Gaussian lineal, eso se convierte en un filtro de Kalman.

  • Otro texto breve que me ayudó a entender el Kalman Filter y me dejó otra revelación fue una nota de John D. Cook: https://www.johndcook.com/blog/applied-kalman-filtering/
    Mientras que el modelado tradicional de sistemas basado en cálculo y ecuaciones diferenciales asume que no hay incertidumbre en los datos y que todo está contenido en el modelo del sistema, los estimadores basados en datos consideran que “todo está en los datos” e ignoran por completo el modelo del proceso físico que genera esos datos.
    La belleza del Kalman Filter está en que combina estos dos enfoques.

  • También hay otro artículo con buenas visualizaciones: https://www.bzarg.com/p/how-a-kalman-filter-works-in-picture...
    Es un artículo que ya había aparecido tres veces en HN.