Uno de los proyectos que tengo por el cajón, es un sistema de tracking para cuando voy a correr a los Karts. Es una pequeña caja con una IMU y un GPS, que guarda los datos en una SD. De esta forma, al final de la carrera puedo ver como he tomado las curvas, y las aceleraciones que he sufrido.

En mi caso llevo la caja en una riñonera y la orientación de la IMU no es conocida de ante mano, esto hace que las aceleraciones que marca la IMU no están referidas al Kart ni a nada conocido. Este problema se puede solucionar como ya comenté en el post anterior (Matriz de Rotación para IMU), buscando la zona de la IMU que se supone que es la línea de meta. Este punto lo podemos buscar porque es la zona donde habrá menos movimiento en los datos.

Una vez que tengo los datos de la IMU referidos al Kart tengo dos problemas, el primero y más importante es que no sé si la caja se habrá movido de posición (cambiando lo que es el frente del kart para la IMU). El segundo es que la orientación del Kart va cambiando a lo largo del circuito y solo tengo las aceleraciones con respecto al Kart. Así que si quiero luego poder juntar las aceleraciones (y velocidades) con las posiciones del GPS, necesito tener las aceleraciones referidas a un sistema de coordenadas global. Esto significa que mezclando aceleraciones y valores de giróscopo tengo que calcular la orientación real del kart cada momento.

En este post vamos a aprender qué es y cómo se usa un filtro de Kalman. Implementaré un ejemplo sencillo para comprobar que todo funciona como esperamos y así podamos comprobar como las diferentes variables afectan al sistema.

Empezando por lo fácil

Qué es y cómo funciona un filtro de Kalman aplicado a un vehículo?Un filtro de Kalman es un algoritmo usado para estimar las variables de un sistema basándose en medidas con ruido. Lo que hace este algoritmo es calcular las diferentes probabilidades del estado del sistema, superponiéndolas posteriormente con las diferentes mediciones teniendo en cuenta su componente de ruido añadido. Es por ello que el filtro de Kalman es perfecto para usar sistemas embarcados donde los sensores, para recoger la información del entorno, tienen bastante ruido (sobre todo con sensores baratos).

El filtro de Kalman se puede separar en dos pasos principalmente:

  1. La predicción donde, basándonos en el estado anterior del sistema y las ecuaciones que rigen su evolución, vamos a predecir el estado actual.
  2. La segunda parte es la corrección, parte en la cual, con los datos de medición de los sensores, corregimos la primera predicción.

Por ejemplo, si tenemos un coche moviéndose en línea recta, con un sensor de velocidad que tiene sin error. Si sabemos que el coche está a distancia 0\,m y va a 3\,m/s, sabemos que dentro de 1 segundo el coche estará a distancia 3\,m. Sin embargo, si no estamos seguros de la posición del coche y además tampoco la medida de velocidad es 100% fiable, entonces las operaciones no son tan sencillas. Para empezar vamos a suponer que tenemos una posición de la cuál no estamos seguros, pero sabemos exactamente y sin dudarlo la velocidad del coche.

Para el movimiento de un vehículo a velocidad constante podemos usar las ecuaciones básicas del movimiento rectilíneo y, aunque la posición sea una variable aleatoria, podemos expresar su movimiento como:

    \[p_{k} = p_{k-1} + v_{k-1}\cdot\Delta t\]

Es decir, la posición actual es la posición del instante anterior más la velocidad por el tiempo que ha pasado. En este caso, como sabemos exactamente que se ha movido una velocidad fija: si antes dudábamos que estuviese entre -1 m y 1 m, ahora dudaremos entre 2 y 4m. Por lo tanto el movimiento es muy sencillo y la representación de la posible posición del vehículo sería como en la siguiente figura.

Cambio de posición con velocidad conocida

Cambio de posición con velocidad conocida

Matemáticamente, eso se puede ver usando las propiedades de la media y la varianza. Estas propiedades son muy importantes, y las usaremos mucho a lo largo del post.

    \[\text{Siendo: }a,b\in \mathbb{R}, X \sim N(\mu, \sigma)\]

    \[E[aX+b] = aE[X] + b\]

    \[Var[aX+b] = a^2Var[X]\]

En nuestro caso, podemos ver que la media y la varianza de la nueva posición se tendrán que actualizar con la ecuación del movimiento. Como en este caso, el tiempo y la velocidad no son variables aleatorias:

    \[\mu_{p|k} = \mu_{p|k-1} + v_{k-1}\Delta t\]

    \[\sigma_{p|k}^2 = \sigma_{p|k-1}^2\]

Si ya todo son variables aleatorias…

Vamos a complicar un poco las cosas, ahora la velocidad que tiene el vehículo tampoco es segura. Podemos volver a modelar la incertidumbre de la velocidad como otra variable aleatoria normal, con media la velocidad que creemos que lleva. Matemáticamente lo expresamos así:

    \[P \sim \mathcal{N}(\mu_p, \sigma_p^2)\]

    \[V \sim \mathcal{N}(\mu_v,\sigma_v^2)\]

En el caso de la media de la posición podemos suponer que es el punto \mu_p = 0 y por su parte la velocidad tendrá media \mu_v = 3. En este caso y usando la misma ecuación de antes para el movimiento, pero teniendo en cuenta que ahora tanto la posición como la velocidad son variables aleatorias.

    \[p_{k} = p_{k-1} + v_{k-1}\cdot\Delta t\]

En este caso, y teniendo en cuenta que la posición y la velocidad son variables independientes, podemos calcular la media y la varianza de la nueva posición. Para ello tenemos que tener en cuentas las propiedades de la media y la varianza de la suma de dos variables aleatorias:

    \[E[aX+bY] = aE[X] + bE[Y]\]

    \[Var[aX+bY] = a^2Var[X] + b^2Var[Y]\]

Con esto, la nueva posición tendría:

    \[E[P+V\Delta t] = E[P] + E[V]\Delta t\]

    \[Var[P + V\Delta t] = Var[P] + {\Delta t}^2Var[V]\]

Es decir, vemos que según avanza el tiempo la varianza de la nueva posición se incrementa , proporcionalmente a la varianza de la velocidad. Es lo mismo que decir, que cada instante de tiempo que pasa tenemos menos certeza de cuál es la posición de verdad del vehículo. Esto lo podemos ver en la siguiente imagen, donde se ve la comparación entre la posición que tendríamos con velocidad fija, y el resultado cuando la velocidad tiene una componente de ruido aleatorio.

Cambio de posición con velocidad aleatoria

Cambio de posición con velocidad imprecisa

Si además, tenemos en cuenta que este proceso se repetiría indefinidamente mientras el vehículo, se mueve, podemos observar como la varianza de la posición va creciendo a medida que pasa el tiempo y se va recalculando la posición.

Mútiples pasos avanzando la velocidad

Mútiples pasos avanzando la velocidad

Fase de corrección

Hasta ahora hemos terminado la primera fase de la predicción. Ya sabemos como predecir el siguiente estado, y actualizar las variables aleatorias. En este paso, vamos a hacer la magia de Kalman, que consiste en unir la estimación con nuestra medición. Lo primero que tenemos que hacer es relacionar nuestro estado con los datos que recogen los sensores. Para nuestro ejemplo no tiene mucha importancia (ni sentido), pero es necesario verlo para cuando veamos las operaciones con sistemas más complejos y matrices.

Este paso consiste en: suponiendo que estamos en el estado que hemos predicho en el paso anterior \hat{p_k} (le ponemos el gorrito para decir que es la estimación antes de las mediciones), vemos que medimos con los sensores para luego relacionar esta medida con las medidas reales. Por ejemplo, imaginemos que nuestro sensor nos da la velocidad entre 0 y 1, donde 0 representa 0\, m/s y 1 son 100\,m/s (por poner un ejemplo), entonces para adaptar nuestras medidas a nuestra realidad habría que multiplicar nuestra predicción por 100. De momento, vamos a obviar esto y pensar que nuestro sensor nos devuelve el valor ya adaptado.

Si ahora tomamos una medida z del sistema y la medida tiene también ruido, por lo que no es fiable, es como si la medida también nos diese un variable aleatoria alrededor de la medida real del sistema. Entonces, necesitamos una manera de calcular cual es la posición más probable en función de la medición y de la predicción. Para ello tenemos que juntar dos variables aleatorias. El resultado de unir dos variables aleatorias normales no es otra distribución normal sin embargo su distribución conjunta si que lo es.

La pdf de una variable aleatoria normal N \sim \mathcal{N}(\mu,\sigma^2) es:

    \[f(x) = \frac{1}{\sqrt{2\pi \sigma^2}} e^{\frac{(x-\mu)^2}{2\sigma^2}}\]

Si ahora multiplicamos dos normales N_1 \sim \mathcal{N}(\mu_1,\sigma_1^2) y N_2 \sim \mathcal{N}(\mu_2,\sigma_2^2), el resultado es una normal N' \sim \mathcal{N}(\mu',\sigma'^2) donde:

    \[\mu' =  \mu_1 + k(\mu_1 - \mu_2)\]

    \[\sigma'^2 = \sigma_1^2 - k\sigma_1^2\]

    \[k = \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2}\]

Multiplicación de variables normales

Multiplicación de variables normales

Lo más interesante de este resultado es que al multiplicar dos variables normales, el resultado tiene forma de variable gaussiana, pero con la varianza nueva menor que la anterior. Lo que significa, que se disminuye la incertidumbre de la nueva variable. Para nuestro caso es muy interesante ya que nos permite unir el estado predicho y la medida, decrementando la incertidumbre final (Sorpresa, es justo lo que necesitábamos).

Entonces que nos vamos del tema. Si ahora tenemos nuestra medida z \sim \mathcal{N}(\mu_z, \sigma_z^2) y la multiplicamos por nuestra predicción \hat{p}, nuestra posición corregida \hat{p}' tendra:

    \[k = \frac{\sigma_p^2}{\sigma_z^2 + \sigma_p^2}\]

    \[\mu'_{p} = \mu_p - k(\mu_z - \mu_p)\]

    \[\sigma'_p = \sigma_p -k\sigma_p\]

De este modo hemos mejorado la estimación a través de unir las dos probabilidades. Y con esto concluiría la estimación de la posición a través de filtros de Kalman. En la siguiente iteración el estado anterior sería la salida de la fase de corrección.

Ciclo de Kalman

Ciclo de Kalman

No todo es tan bonito… Matrices incoming!!

Predicción con matrices

Hasta ahora estamos usando variables simples para representar los valores de nuestros sistema. Sin embargo, a medida que los estados de nuestro sistema aumentan, mantener todas las ecuaciones de manera ordenada se convierte en una tarea más difícil. Así que podemos escribir de forma genérica:

  • \mathbf{x}: Vector con las variables del sistema.
  • \mathbf{A}: Matriz de predicción del siguiente estado.
  • \mathbf{B}: Matriz de entradas externas al sistema
  • \mathbf{u}: Vector de entradas

Con estas matrices los sistemas que queramos representar vienen dados por la siguiente ecuación (w_k es el ruido o error de la medida):

    \[\mathbf{\hat x_k} = \mathbf{x_{k-1}} + \mathbf{A}\mathbf{\hat{x}'_k} + \mathbf{B}\mathbf{u} + w_k\]

    \[\mathbf{P_k} = \mathbf{A}\mathbf{P_{k-1}}\mathbf{A}^T + \mathbf{Q_k}\]

Donde \mathbf{P_k} es la matriz de covarianza de nuestra variable de estado en el instante k y \mathbf{Q_k} es el ruido que añade el entorno a nuestro sistema (en el caso del vehículo puede ser que haya rachas de viento que alteren un poco la dirección o la posición…). La matriz \mathbf{P} estará formada en la diagonal principal por las varianzas de cada uno de los elementos de nuestro vector de estado, y fuera de la diagonal principal, estarán las covarianzas entre las diferentes variables.

Para entender mejor esta parte vamos a aplicarlo a nuestro ejemplo anterior. Si tenemos el vehículo andando en línea recta y con velocidad v, solo necesitamos la posición y la velocidad para definir nuestro estado. Así que, nuestro vector de estado será:

    \[\mathbf{x} = \left[ \begin{array}{c} p \\ v\end{array} \right]\]

    \[\mathbf{P} = \left[ {\begin{array}{cc}    \sigma_p^2 & 0\\    0 & \sigma_v^2 \\   \end{array} } \right]\]

Para adaptar nuestro sistema del vehículo al sistema matricial solo tenemos que definir \mathbf{A} ya que no tenemos inputs y la entrada u va a ser siempre cero. Así nuestra matriz \mathbf{A} es:

    \[A=   \left[ {\begin{array}{cc}    1 & \Delta t\\    0 & 1 \\   \end{array} } \right]\]

Ya que si ahora hacemos la cuenta \mathbf{A}\mathbf{x} nos quedan las ecuaciones de antes:

    \[\left[ \begin{array}{c} \hat{p_k} \\ \hat{v_k}\end{array} \right] =   \left[ {\begin{array}{cc}    1 & \Delta t\\    0 & 1 \\   \end{array} } \right] \left[ \begin{array}{c} \hat{p}'_{k-1} \\ \hat{v}'_{k-1}\end{array} \right]\]

    \[\hat{p_k} = \hat{p}'_{k-1} + \hat{v}'_{k-1}\Delta t\]

    \[\hat{v}_k = \hat{v}'_{k-1}\]

Para terminar nuestra fase de predicción solo tenemos que implementar la función que actualiza la variable \mathbf{P} que he explicado más arriba.

Corrección con matrices

Siguiendo el razonamiento de caso de variables no matriciales, ahora nos queda corregir con la medida. Antes he explicado que cambiar entre el valor de variable real y la medida del sensor, no tenía mucho sentido. En este caso como tenemos todas las variables del estado juntas, puede que no estemos midiendo una de ellas, por lo que nos interesa multiplicar por 0 esa estimación, ya que no vamos a tener lectura de sensor asociada.

La matriz que asocia el estado del sistema con las medidas de los sensores es la matriz \mathbf{H}. Si por ejemplo estamos midiendo la posición nueva en cada ciclo mediante GPS pero no medimos la velocidad, la matriz sería:

    \[\mathbf{H} =    \left[ {\begin{array}{cc}    1 & 0\\    0 & 0 \\   \end{array} } \right]\]

Si medimos tanto la velocidad como la posición, la matriz tendría un 1 en todos los elementos de la diagonal principal.

    \[\mathbf{H} =    \left[ {\begin{array}{cc}    1 & 0\\    0 & 1 \\   \end{array} } \right]\]

Y la varianza de la medición esperada sería \Sigma = \mathbf{H}\mathbf{P}\mathbf{H}^T. Todo esto sería como la medición que esperaríamos tener del estado previsto a priori. Por lo que tendríamos que juntar esta medición esperada, con los valores leídos del sensor. Suponiendo que tenemos un sensor con ruido aleatorio:

    \[Sensor \sim \mathcal{N}(\mathbf{z_k}, \mathbf{R_k})\]

Y creyéndonos que en el caso de matrices las ecuaciones equivalentes para el caso de multiplicación de variables normales son:

    \[\mathbf{K} = \Sigma_1(\Sigma_1 + \Sigma_2)^{-1}\]

    \[\mu' = \mu_1 - \mathbf{K}(\mu_1 - \mu_0)\]

    \[\Sigma' = \Sigma_1 -\mathbf{K}\Sigma_1\]

El resultado sería que: el valor medido del sistema corregido con las medidas (\mathbf{H}\hat{x}'_k) es igual al resultado de multiplicar las variables de la medición de la estimación(\mathbf{H}\hat{x}_k) con la medida (\mathbf{z_k}).

    \[\mathbf{H}\hat{x}'_k = \mathbf{H}\mathbf{\hat{x}_k} - \mathbf{K}(\mathbf{z_k} - \mathbf{H}\mathbf{\hat{x}_k})\]

    \[\mathbf{H}\mathbf{P'}\mathbf{H}^T = \mathbf{H}\mathbf{P}\mathbf{H}^T - \mathbf{K}\mathbf{H}\mathbf{P}\mathbf{H}^T\]

    \[\mathbf{K} = \mathbf{H}\mathbf{P}\mathbf{H}^T(\mathbf{H}\mathbf{P}\mathbf{H}^T - \mathbf{R})^{-1}\]

Como podemos quitar \mathbf{H} en las ecuaciones (\mathbf{H} está dentro de \mathbf{K}), y \mathbf{H}^T del cálculo de la matriz de covarianza, el resultado final para la corrección por filtro de Kalman sería:

    \[\mathbf{\hat{x}}'_k = \mathbf{\hat{x}_k} - \mathbf{K'}(\mathbf{z_k} - \mathbf{H}\mathbf{\hat{x}_k})\]

    \[\mathbf{P'} = \mathbf{P} - \mathbf{K'}\mathbf{H}\mathbf{P}\]

    \[\mathbf{K'} = \mathbf{P}\mathbf{H}^T(\mathbf{H}\mathbf{P}\mathbf{H}^T - \mathbf{R})^{-1}\]

Conclusión

La dificultad de los filtros de Kalman recae en definir correctamente las variables y de la estimación correcta de las varianzas de ruido de las variables. Pero una vez hecho esto, se puede calcular fácilmente el algoritmo con tan solo 5 ecuaciones:

  • Predicción:
    • \mathbf{\hat x_k} = \mathbf{x_{k-1}} + \mathbf{A}\mathbf{\hat{x}'_k} + \mathbf{B}\mathbf{u}
    • \mathbf{P_k} = \mathbf{A}\mathbf{P_{k-1}}\mathbf{A}^T + \mathbf{Q_k}
  • Corrección:
    • \mathbf{\hat{x}}'_k = \mathbf{\hat{x}_k} - \mathbf{K'}(\mathbf{z_k} - \mathbf{H}\mathbf{\hat{x}_k})
    • \mathbf{P'} = \mathbf{P} - \mathbf{K'}\mathbf{H}\mathbf{P}
    • \mathbf{K'} = \mathbf{P}\mathbf{H}^T(\mathbf{H}\mathbf{P}\mathbf{H}^T - \mathbf{R})^{-1}

Más adelante veremos como implementar esto y usarlo en algún ejemplo práctico. Deja cualquier duda o sugerencia en los comentarios!

Categorías: Teoría

Gluón

Teleco con ganas de aprender más y compartirlo. Viajero empedernido y amante de la fotografía y la tecnología. Espero dejar mi granito de arena y que este pueda servir de ayuda.