Filtro de Kalman (Kalman Filter)

La necesidad de la predicción

Comenzamos formulando el problema para entender por qué necesitamos un algoritmo de estimación y predicción de estado.

Para ilustrarlo, consideremos el ejemplo de un radar de seguimiento:

Supongamos que tenemos un radar que sigue a una aeronave. En este escenario, la aeronave es el sistema, y la magnitud a estimar es su posición, que representa el estado del sistema.

El radar muestrea el blanco dirigiendo hacia él un haz estrecho y proporciona mediciones de posición de la aeronave. Con base en estas mediciones, podemos estimar el estado del sistema (la posición de la aeronave).

Para seguir a la aeronave, el radar debe revisitar el blanco a intervalos regulares apuntando el haz en su dirección. Esto significa que el radar debe poder predecir la posición futura de la aeronave para el siguiente haz. Si no lo consigue, el haz puede apuntar en una dirección equivocada, produciendo una pérdida de seguimiento. Para realizar esta predicción, necesitamos cierta información sobre cómo se mueve la aeronave. En otras palabras, necesitamos un modelo que describa el comportamiento del sistema a lo largo del tiempo, conocido como modelo dinámico (dynamic model).

Para simplificar el ejemplo, consideremos un mundo unidimensional en el que la aeronave se mueve a lo largo de una línea recta hacia el radar o alejándose de él.

El estado del sistema se define como la distancia de la aeronave al radar, denotada por \( r \). El radar envía un pulso hacia la aeronave, que se refleja en el blanco y retorna al radar. Midiendo el tiempo transcurrido entre la transmisión y la recepción del pulso y sabiendo que el pulso es una onda electromagnética que viaja a la velocidad de la luz, el radar puede calcular fácilmente la distancia \( r \) de la aeronave. Además de la distancia, el radar también puede medir la velocidad \( v \) de la aeronave, de forma similar a como un radar policial detecta la velocidad de un automóvil utilizando el efecto Doppler.

Supongamos que en el instante \( t_{0} \) el radar mide la distancia y la velocidad de la aeronave con muy alta precisión. La distancia medida es 10.000 metros y la velocidad es 200 metros por segundo. Esto nos da el estado del sistema:

\[ r_{t_{0}} = 10,000m \]

El siguiente paso es predecir el estado del sistema en el instante \( t_{1}=t_{0}+\Delta t \), donde \( \Delta t \) es el tiempo de revisita del blanco. Dado que se espera que la aeronave mantenga una velocidad constante, puede utilizarse un modelo dinámico de velocidad constante para predecir su posición futura.

La distancia recorrida durante el intervalo de tiempo \( \Delta t \) viene dada por:

\[ \Delta r = v \cdot \Delta t \]

Suponiendo un intervalo de muestreo de 5 segundos, la posición prevista en el instante \( t_{1} \) es:

\[ r_{t_{1}} = r_{t_{0}} + \Delta r = 10,000 + 200 \cdot 5 = 11,000m \]

Este es un algoritmo elemental construido sobre principios simples. El estado actual del sistema se obtiene a partir de la medición, y el modelo dinámico se utiliza para predecir el estado futuro.

En la vida real, las cosas son más complejas. Primero, las mediciones del radar no son perfectamente precisas. Están afectadas por el ruido y contienen un cierto grado de aleatoriedad. Si diez radares diferentes midieran la distancia de la aeronave en el mismo instante, producirían diez resultados ligeramente distintos. Estos resultados probablemente serían cercanos entre sí, pero no idénticos. La variación en las mediciones está causada por el ruido de medición (Measurement Noise).

Esto conduce a una nueva pregunta: ¿Qué tan segura es nuestra estimación? Necesitamos un algoritmo que no solo proporcione una estimación, sino que también nos diga cuán confiable es esa estimación.

Otro aspecto es la precisión del modelo dinámico. Aunque podemos suponer que la aeronave se mueve a velocidad constante, factores externos como el viento pueden introducir desviaciones respecto a esa suposición. Estas influencias impredecibles se denominan ruido de proceso (Process Noise).

Al igual que queremos evaluar la certeza de nuestra estimación basada en la medición, también queremos entender el nivel de confianza de nuestra predicción.

El Filtro de Kalman es un algoritmo de estimación de estado que proporciona tanto una estimación del estado actual como una predicción del estado futuro, junto con una medida de su incertidumbre. Además, es un algoritmo óptimo que minimiza la incertidumbre de la estimación de estado. Por eso, el Filtro de Kalman se ha convertido en un algoritmo tan utilizado y confiable.

Ejemplo de Filtro de Kalman

Comencemos con un ejemplo sencillo: un radar unidimensional que mide la distancia y la velocidad transmitiendo un pulso hacia una aeronave y recibiendo el eco reflejado. El retraso temporal entre la transmisión del pulso y la recepción del eco proporciona información sobre la distancia \(r\) de la aeronave, y el desplazamiento de frecuencia del eco refleja información sobre la velocidad \(v\) de la aeronave (efecto Doppler).

En este ejemplo, el estado del sistema está descrito tanto por la distancia \(r\) como por la velocidad \(v\) de la aeronave. Definimos el estado del sistema mediante el vector de estado (State Vector) \(\boldsymbol{x}\), que incluye ambas magnitudes:

\[ \boldsymbol{x}=\left[\begin{matrix}r\\v\\\end{matrix}\right] \]

Denotamos los vectores con letras minúsculas en negrita y las matrices con letras mayúsculas en negrita.

Como el estado del sistema incluye más de una variable, usamos herramientas de álgebra lineal, como vectores y matrices, para describir las matemáticas del Filtro de Kalman. Si no te sientes cómodo con el álgebra lineal, revisa la sección Filtro de Kalman en una dimensión del tutorial en línea o del libro. Presenta las ecuaciones del Filtro de Kalman y su derivación usando matemáticas de nivel bachillerato, junto con cuatro ejemplos completamente resueltos.

Iteración 0

Inicialización del filtro

En este ejemplo, utilizaremos la primera medición para inicializar el Filtro de Kalman (para más información sobre técnicas de inicialización y su impacto en el desempeño del Filtro de Kalman, consulta el Capítulo 21 del libro). En el instante \(t_0\), el radar mide una distancia de \(10{,}000\,m\) y una velocidad de \(200\,m/s\). Las mediciones se denotan por la letra \(\boldsymbol{z}\).
Apilamos las mediciones en el vector de medición (Measurement Vector) \(\boldsymbol{z}\):

\[ \boldsymbol{z}_0=\left[\begin{matrix}10{,}000\\200\\\end{matrix}\right] \]

El subíndice \(0\) indica el tiempo \(t_0\).

La medición no refleja el estado exacto del sistema. Las mediciones están corrompidas por ruido aleatorio; por lo tanto, cada medición es una variable aleatoria.

¿Podemos confiar en esta medición? ¿Qué tan segura es? Cada medición va acompañada de una incertidumbre de medición al cuadrado (a veces llamada error de medición). Esta incertidumbre al cuadrado es la varianza de la medición. Puedes leer más sobre la varianza en la sección Conceptos esenciales I. Para una discusión más detallada de la incertidumbre de medición, consulta la sección Filtro de Kalman en una dimensión.

En sistemas de radar, la incertidumbre de medición está determinada en gran medida por la relación entre la potencia de la señal recibida y el ruido. Cuanto mayor es la relación señal‑ruido, menor es la varianza de la medición y mayor nuestra confianza en la medición.

La siguiente figura compara casos de señal baja y señal alta en presencia de ruido.

Supongamos que la desviación estándar de la medición de distancia es \(4\,m\) y la desviación estándar de la medición de velocidad es \(0.5\,m/s\). Dado que la varianza es el cuadrado de la desviación estándar, la incertidumbre de medición al cuadrado (denotada por \( \boldsymbol{R} \)) es:

\[ \boldsymbol{R}_0=\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right] \]

\( \boldsymbol{R} \) es una matriz de covarianza (Covariance Matrix). Los elementos de la diagonal principal contienen las varianzas, y los elementos fuera de la diagonal son las covarianzas entre mediciones.

\[ \boldsymbol{R}=\left[\begin{matrix}\sigma_r^2&\sigma_{rv}^2\\[0.5em]\sigma_{vr}^2&\sigma_v^2\\\end{matrix}\right] \]

En este ejemplo, suponemos que los errores en las mediciones de distancia y velocidad no están relacionados entre sí, por lo que los elementos fuera de la diagonal de la matriz de covarianza de medición se fijan en cero.

Para repasar varianza y desviación estándar, consulta la sección Conceptos esenciales I del tutorial en línea.
Para repasar las matrices de covarianza, consulta la sección Conceptos esenciales II.

Durante la inicialización, la única información que tenemos es una única medición. En este ejemplo, la medición y el estado del sistema están descritos por las mismas magnitudes (\(r\) y \(v\)). Por lo tanto, podemos usar la medición como estimación inicial del estado del sistema. Esto solo puede hacerse durante el paso de inicialización:

\[ \boldsymbol{\hat{x}}_{0,0}=\boldsymbol{z}_0=\left[\begin{matrix}10{,}000\\200\\\end{matrix}\right] \]

El subíndice \(0,0\) tiene el siguiente significado:

• El primer índice se refiere al tiempo del sistema, que en este ejemplo es \(t_0\).
• El segundo índice se refiere al instante en el que se realizó la estimación, que también es \(t_0\).

En otras palabras, la estimación es para el instante \(t_0\), y también se calculó en el instante \(t_0\).

Predicción

Ahora predecimos el siguiente estado. Supón que el tiempo de revisita del blanco es 5 segundos \((\Delta t=5s)\), por lo tanto \(t_1=5s\).

Para estimar el estado futuro del sistema, debemos describir cómo evoluciona el sistema en el tiempo. En este ejemplo, suponemos un modelo dinámico de velocidad constante (modelo de movimiento):

\[ v_{1} = v_{0} = v \] \[ r_{1} = r_{0} + v_{0}\Delta t \]

(Para ejemplos de modelos dinámicos con aceleración, consulta el Capítulo 9 del libro.)

Describamos el modelo dinámico en forma matricial:

\[ {\hat{\boldsymbol{x}}}_{1,0}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{0,0} \]

El subíndice \(1,0\) tiene el siguiente significado:

• El primer índice se refiere al tiempo del sistema, que es \(t_1\).
• El segundo índice se refiere al instante en que se realizó la estimación, que es \(t_0\).

Así, \( \hat{\boldsymbol{x}}_{1,0} \) es nuestra estimación del estado del sistema en el instante \(t_1\), calculada usando la información disponible en el instante \(t_0\). En otras palabras, es una predicción del estado futuro.

La matriz \( \boldsymbol{F} \) se denomina matriz de transición de estado (State Transition Matrix) y describe cómo evoluciona el estado del sistema en el tiempo:

\[ {\hat{\boldsymbol{x}}}_{1,0}=\left[\begin{matrix}{\hat{r}}_{1,0}\\{\hat{v}}_{1,0}\\\end{matrix}\right]=\left[\begin{matrix}1&\Delta t\\0&1\\\end{matrix}\right]\left[\begin{matrix}{\hat{r}}_{0,0}\\{\hat{v}}_{0,0}\\\end{matrix}\right]=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}10,000\\200\\\end{matrix}\right]=\left[\begin{matrix}11,000\\200\\\end{matrix}\right] \]

El Apéndice C del libro describe un método para modelar la dinámica de cualquier sistema lineal.

La ecuación

\[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} \]

es la ecuación de extrapolación de estado (State Extrapolation Equation). Nos indica cómo calcular el siguiente estado a partir del actual. Toma nuestra estimación del estado actual y utiliza el modelo de movimiento del sistema para predecir el estado en el siguiente instante de tiempo.

La forma completa de la ecuación de extrapolación de estado es:

\[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n \]

donde:

• \(\boldsymbol{u}_{n}\) es una variable de entrada
• \(\boldsymbol{G}\) es una matriz de entrada o matriz de control (Control/Input Transition Matrix)

El vector de entrada representa información adicional proporcionada al Filtro de Kalman, como lecturas de un acelerómetro a bordo.

En este ejemplo simple, suponemos que no hay entrada, por lo que \(\boldsymbol{u}_n=0\).

Para un ejemplo que incluye un término de entrada, consulta la página Ecuación de extrapolación de estado del tutorial en línea o el Ejemplo 10 completamente resuelto en el libro.

Cada medición y cada estimación en el Filtro de Kalman viene acompañada de información de incertidumbre. Después de predecir el siguiente estado, también debemos preguntarnos: ¿qué tan precisa es esta predicción?

La incertidumbre al cuadrado de la estimación del estado actual está representada por la matriz de covarianza (Covariance Matrix):

\[ \boldsymbol{P}_{0,0}=\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right] \]

Sin embargo, la covarianza de la predicción no se calcula como:

\[ \textcolor{red}{\xcancel{\textcolor{black}{ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0} }}} \]

Esto se debe a que \(\boldsymbol{P}\) es una matriz de covarianza, y las varianzas y covarianzas involucran términos al cuadrado.

La ecuación de extrapolación de covarianza (sin el ruido de proceso) está dada por:

\[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T \]

Puedes encontrar la derivación completa en la sección Ecuación de extrapolación de covarianza del tutorial en línea.

Para nuestro ejemplo:

$$ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0}\boldsymbol{F}^T=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\5&1\\\end{matrix}\right]=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}16&0\\1.25&0.25\\\end{matrix}\right]=\left[\begin{matrix}\colorbox{yellow}{$22.25$}&1.25\\1.25&\colorbox{yellow}{$0.25$}\\\end{matrix}\right] $$

Observa la diagonal principal de la matriz de covarianza.

La varianza de la velocidad \(\sigma_v^2\) sigue siendo \(0.25 \, m^2/s^2\). No cambió porque el modelo dinámico supone velocidad constante.

En cambio, la varianza de la distancia \(\sigma_r^2\) aumentó de \(16\,m^2\) a \(22.25\,m^2\). Esto refleja el hecho de que la incertidumbre en la velocidad conduce a un aumento de la incertidumbre en la distancia con el tiempo.

Como se señaló antes, la suposición de dinámica de velocidad constante no es totalmente precisa. En la realidad, la velocidad de la aeronave puede verse afectada por factores externos y desconocidos, como el viento. Como resultado, la incertidumbre real de la predicción es mayor que la que predice el modelo simple.

Estas influencias impredecibles se llaman ruido de proceso (Process Noise) y se denotan por \(\boldsymbol{Q}\). Para tener en cuenta estos efectos, añadimos \(\boldsymbol{Q}\) a la ecuación de covarianza de la predicción:

\[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T + \boldsymbol{Q}\]

Para ganar intuición sobre cómo el ruido de proceso afecta el desempeño del Filtro de Kalman, consulta el Ejemplo 6 en el tutorial en línea.

Supongamos que la desviación estándar de la aceleración aleatoria es \(\sigma_a=0.2\,m/s^2\). Esto representa incertidumbre en la aceleración aleatoria de la aeronave causada por influencias ambientales impredecibles.

En consecuencia, la varianza de la aceleración aleatoria \(\sigma_a^2=0.04\,m^2/s^4\).

Para nuestro ejemplo, la matriz de ruido de proceso (Process Noise Matrix) está dada por:

$$ \boldsymbol{Q} = \left[\begin{matrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\[0.5em] \frac{\Delta t^3}{2} & \Delta t^2 \end{matrix}\right] \sigma_a^2 $$

Con \(\Delta t=5\,\mathrm{s}\) y \(\sigma_a^2=0.04\,\mathrm{m}^2/\mathrm{s}^4\), esto se convierte en:

$$ \boldsymbol{Q}=\left[\begin{matrix}\frac{625}{4}&\frac{125}{2}\\[0.5em] \frac{125}{2}&25\\\end{matrix}\right]0.04=\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right] $$

La derivación de la matriz de ruido de proceso se presenta en la Sección 8.2.2 del libro.

Tras añadir el ruido de proceso, la incertidumbre al cuadrado de nuestra predicción es:

$$ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0}\boldsymbol{F}^T+\boldsymbol{Q}\ =\left[\begin{matrix}22.25&1.25\\1.25&0.25\\\end{matrix}\right]+\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right]\ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right] $$

Resumen de la Iteración 0

• Inicialización
  Inicializamos el Filtro de Kalman usando la primera medición como estimación inicial del estado \( {\hat{\boldsymbol{x}}}_{0,0} \), y la covarianza de medición como covarianza inicial del estado \(\boldsymbol{P}_{0,0}\).
  Ten en cuenta que esto puede hacerse solo durante la fase de inicialización.
• Predicción
  Predijimos el estado y su incertidumbre en el siguiente instante de tiempo, cuando el radar revisita la aeronave. Las ecuaciones de predicción del Filtro de Kalman son:
  
  
  Ecuación de extrapolación de estado (State Extrapolation Equation)
  \[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n \]
  Ecuación de extrapolación de covarianza (Covariance Extrapolation Equation)
  \[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T + \boldsymbol{Q}\]
  donde:
  
  • \(\hat{\boldsymbol{x}}_{n,n}\) es el vector de estado estimado del sistema en el paso \(n\)
  • \(\hat{\boldsymbol{x}}_{n+1,n}\) es el vector de estado del sistema previsto para el paso \(n+1\), calculado con información disponible en el tiempo \(n\)
  • \(\boldsymbol{u}_n\) es una variable de control o de entrada, que representa entradas externas conocidas del sistema
  • \(\boldsymbol{F}\) es la matriz de transición de estado
  • \(\boldsymbol{G}\) es la matriz de entrada (control) o matriz de transición de entrada, que mapea entradas a variables de estado
  • \(\boldsymbol{P}_{n,n}\) es la matriz de covarianza (incertidumbre al cuadrado) del estado actual
  • \(\boldsymbol{P}_{n+1,n}\) es la matriz de covarianza (incertidumbre al cuadrado) del estado previsto
  • \(\boldsymbol{Q}\) es la matriz de ruido de proceso

Iteración 1

Actualización del filtro

Supón la segunda medición en \(t_1\):

\[ \boldsymbol{z}_1=\left[\begin{matrix}11{,}020\\202\\\end{matrix}\right] \]

Debido a un pico de ruido intenso durante esta medición, la relación señal‑ruido es significativamente menor que en la primera medición. Como resultado, la incertidumbre de la segunda medición es mayor.

Supongamos que la desviación estándar de la medición de distancia es \(6\,m\) y la desviación estándar de la medición de velocidad es \(1.5\,m/s\). La matriz de covarianza de medición correspondiente es:

\[ \boldsymbol{R}_1=\left[\begin{matrix}\colorbox{yellow}{$36$}&0\\0&\colorbox{yellow}{$2.25$}\\\end{matrix}\right] \]

Queremos estimar el estado actual del sistema \(\hat{\boldsymbol{x}}_{1,1}\). En el instante \(t_1\), tenemos dos piezas de información:

• El estado previsto \(\hat{\boldsymbol{x}}_{1,0}\) (calculado en el paso anterior), y
• La nueva medición \(\boldsymbol{z}_1\)

¿En cuál deberíamos confiar más?

Intuitivamente, podríamos preferir usar la medición como estimación actual, es decir \(\hat{\boldsymbol{x}}_{1,1}=\boldsymbol{z}_1\), porque está más actualizada que la predicción.

Por otro lado, la medición también es más ruidosa. Si comparamos los elementos de la diagonal principal de la covarianza de la predicción \(\boldsymbol{P}_{1,0}\) con la covarianza de la medición \(\boldsymbol{R}_1\), vemos que la incertidumbre de la predicción es menor que la incertidumbre de la medición:

\[ \boldsymbol{P}_{1,0}=\left[\begin{matrix}\colorbox{yellow}{$28.5$}&3.75\\3.75&\colorbox{yellow}{$1.25$}\\\end{matrix}\right] \]

Entonces, quizá deberíamos ignorar la nueva medición y mantener la predicción, es decir \(\hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}\)?

En ese caso, perdemos la información nueva aportada por la medición actual.

La idea clave del Filtro de Kalman es que no hacemos ninguna de las dos cosas. En su lugar, combinamos la predicción y la medición, dando más peso a la que tiene menor incertidumbre.

La solución es un promedio ponderado entre la medición y la predicción:

\[ \hat{x}_{1,1}=K_1 z_1\ +\ \left({1-\ K}_1\right){\hat{x}}_{1,0}, \quad 0\leq K_1 \leq 1 \]

Aquí, el peso \(K_1\) es la Ganancia de Kalman (Kalman Gain). Determina cuánto peso se asigna a la medición frente a la predicción de manera que minimiza la incertidumbre de la estimación. Esto es lo que hace del Filtro de Kalman un filtro óptimo (siempre que el sistema y el ruido se comporten según las suposiciones del modelo).

Presentaré la ecuación de la ganancia de Kalman en breve, pero primero enfoquémonos en la Ecuación de actualización de estado (State Update Equation). En forma matricial, se escribe así:

\[ \hat{\boldsymbol{x}}_{1,1}=\boldsymbol{K}_1\boldsymbol{z}_1 + (\boldsymbol{I} - \boldsymbol{K}_1)\hat{\boldsymbol{x}}_{1,0} \]

donde \(\boldsymbol{I}\) es la matriz identidad (Identity Matrix), una matriz cuadrada con unos en la diagonal principal y ceros en el resto.

Reescribamos esta ecuación:

\[ \hat{\boldsymbol{x}}_{1,1}=\boldsymbol{K}_1\boldsymbol{z}_1 + \hat{\boldsymbol{x}}_{1,0} - \boldsymbol{K}_1\hat{\boldsymbol{x}}_{1,0}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}) \]

Esta forma muestra que el estado actualizado es la predicción \(\hat{\boldsymbol{x}}_{1,0}\) más un término de corrección \(\boldsymbol{K}_1\left(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}\right)\).

La corrección es proporcional a la diferencia entre la medición y la predicción \(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}\), llamada innovación (Innovation) o residuo (Residual).

En nuestro ejemplo, tanto el estado del sistema como la medición son vectores que representan las mismas magnitudes físicas (distancia y velocidad). Por lo tanto, podemos restar directamente \(\hat{\boldsymbol{x}}_{1,0}\) de \(\boldsymbol{z}_1\).

Sin embargo, esto no siempre es así. En general, la medición y el estado del sistema pueden pertenecer a dominios físicos diferentes. Por ejemplo, un termómetro digital mide una señal eléctrica, mientras que el estado del sistema es la temperatura.

Por esta razón, el estado previsto debe transformarse primero al dominio de la medición:

\[ \boldsymbol{H} \hat{\boldsymbol{x}}_{1,0} \]

La matriz \(\boldsymbol{H}\) se denomina matriz de observación (Observation Matrix), también llamada matriz de medición. Mapea las variables de estado a las magnitudes que realmente se miden.

En nuestro ejemplo, la matriz de observación es simplemente la matriz identidad:

\[ \boldsymbol{H}=\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]=\boldsymbol{I} \]

Para más información sobre la matriz de observación, consulta la sección Ecuación de medición del tutorial en línea y los Ejemplos 9 y 10 del libro.

Ahora podemos reescribir la ecuación de actualización de estado como:

\[ \hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}) \]

La innovación \(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}\) representa información nueva.

La ganancia de Kalman determina cuánto debe esta información nueva modificar el estado previsto, es decir, cuán fuertemente corregimos la predicción.

Caso unidimensional

En el caso unidimensional, la ganancia de Kalman está dada por:

\[ K_n=\frac{p_{n,\ n-1}}{p_{n,\ n-1}+r_n} \]

donde:

• \(p_{n,\ n-1}\) es la varianza del estado previsto
• \(r_n\) es la varianza de la medición

La ganancia de Kalman se elige para minimizar la varianza de la estimación actualizada \(p_{n,n}\), por lo que el Filtro de Kalman es óptimo.

Para construir intuición y ver la derivación completa en el caso unidimensional, consulta la sección Filtro de Kalman en una dimensión del tutorial en línea.

Caso multidimensional

Para el Filtro de Kalman multidimensional, la ganancia de Kalman se convierte en una matriz y está dada por:

\[ \boldsymbol{K}_n=\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T+\boldsymbol{R}_n\right)^{-1} \]

Para la derivación de la ecuación de la ganancia de Kalman multidimensional, consulta la sección Ganancia de Kalman del tutorial en línea.

Calculemos la ganancia de Kalman para \(t_1\):

\[ \boldsymbol{K}_1=\boldsymbol{P}_{1,0}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{1,0}\boldsymbol{H}^T+\boldsymbol{R}_1\right)^{-1} \]

En nuestro ejemplo, \(\boldsymbol{H}=\boldsymbol{I}\) y \(\boldsymbol{H}^T=\boldsymbol{I}\).

Sustituye las matrices:

\[ \boldsymbol{P}_{1,0}=\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right], \quad \boldsymbol{R}_1=\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right] \]

\[ \boldsymbol{K}_1=\boldsymbol{P}_{1,0}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{1,0}\boldsymbol{H}^T+\boldsymbol{R}_1\right)^{-1}=\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]\left(\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]+\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right]\right)^{-1} \]

\[ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left(\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]+\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right]\right)^{-1} =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left(\left[\begin{matrix}64.5&3.75\\3.75&3.5\\\end{matrix}\right]\right)^{-1} \]

\[ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}0.0165&-0.0177\\-0.0177&0.3047\\\end{matrix}\right]=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right] \]

\[ \boldsymbol{K}_1=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right] \]

La estimación de estado actualizada es:

\[ \hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}) \]

En nuestro ejemplo, \(\boldsymbol{H}=\boldsymbol{I}\), así que la innovación es simplemente:

\[ \boldsymbol{z}_1 - \boldsymbol{I}\hat{\boldsymbol{x}}_{1,0}=\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}=\left[\begin{matrix}11{,}020\\202\\\end{matrix}\right] - \left[\begin{matrix}11{,}000\\200\\\end{matrix}\right]=\left[\begin{matrix}20\\2\\\end{matrix}\right] \]

Ahora aplica la corrección:

\[ \boldsymbol{K}_1\left[\begin{matrix}20\\2\\\end{matrix}\right]=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right]\left[\begin{matrix}20\\2\\\end{matrix}\right]=\left[\begin{matrix}9.37\\1.43\\\end{matrix}\right] \]

Finalmente:

\[ \hat{\boldsymbol{x}}_{1,1}=\left[\begin{matrix}11{,}000\\200\\\end{matrix}\right]+\left[\begin{matrix}9.37\\1.43\\\end{matrix}\right]=\left[\begin{matrix}11{,}009.37\\201.43\\\end{matrix}\right] \]

Una vez que hemos estimado el estado actual, también queremos cuantificar la incertidumbre de esa estimación.

Caso unidimensional

En el caso unidimensional, la Ecuación de actualización de la covarianza (Covariance Update Equation) es:

\[ p_{n,n}=(1-K_n)p_{n,\ n-1} \]

Para la derivación, consulta la sección Filtro de Kalman en una dimensión del tutorial en línea.

Caso multidimensional

Forma de Joseph (Joseph form)

Para el Filtro de Kalman multidimensional, la ecuación de actualización de la covarianza suele escribirse en una forma numéricamente estable, conocida como la forma de Joseph (Joseph form), introducida por Peter Joseph.

\[ \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 \]

donde:

• \(\boldsymbol{P}_{n,n}\) es la covarianza de la estimación de estado actualizada (posterior)
• \(\boldsymbol{P}_{n,n-1}\) es la covarianza de la estimación de estado prevista (a priori)
• \(\boldsymbol{K}_n\) es la ganancia de Kalman
• \(\boldsymbol{H}\) es la matriz de observación (medición)
• \(\boldsymbol{R}_n\) es la matriz de covarianza del ruido de medición
• \(\boldsymbol{I}\) es la matriz identidad (matriz cuadrada con unos en la diagonal principal y ceros en el resto)

Para la derivación, consulta la sección Ecuación de actualización de la covarianza del tutorial en línea.

Forma simplificada

En la literatura, también verás a menudo la actualización de la covarianza en forma simplificada:

\[ \boldsymbol{P}_{n,n}=(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})\boldsymbol{P}_{n,n-1} \]

Para su derivación, consulta la sección Ecuación de actualización de la covarianza (simplificada).

Ambas formas dan el mismo resultado en aritmética exacta. Sin embargo, para implementaciones en computadora, se prefiere generalmente la forma de Joseph porque es más estable numéricamente.

Para este ejemplo únicamente, usemos la ecuación de actualización de la covarianza simplificada:

\[ \boldsymbol{P}_{1,1}=(\boldsymbol{I} - \boldsymbol{K}_1\boldsymbol{H})\boldsymbol{P}_{1,0} \]

En nuestro ejemplo, \(\boldsymbol{H}=\boldsymbol{I}\), por lo tanto:

\[ \boldsymbol{P}_{1,1}=(\boldsymbol{I} - \boldsymbol{K}_1)\boldsymbol{P}_{1,0} \]

Ahora sustituye las matrices:

\[ \boldsymbol{P}_{1,1}=\left(\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right] - \left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right]\right)\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right] \]

\[ =\left[\begin{matrix}0.5952&-0.6377\\-0.0399&0.6856\\\end{matrix}\right]\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]=\left[\begin{matrix}14.57&1.43\\1.43&0.71\\\end{matrix}\right] \]

Análisis de resultados

La incertidumbre de la estimación actualizada es menor tanto que la incertidumbre de la predicción como la incertidumbre de la medición:

\[ \boldsymbol{P}_{1,1}=\left[\begin{matrix}\colorbox{yellow}{$14.57$}&1.43\\1.43&\colorbox{yellow}{$0.71$}\\\end{matrix}\right]\ \ \ \ \ \ \boldsymbol{P}_{1,0}=\ \left[\begin{matrix}\colorbox{yellow}{$28.5$}&3.75\\3.75&\colorbox{yellow}{$1.25$}\\\end{matrix}\right]\ \ \ \ \ \boldsymbol{R}_\mathbf{1}=\left[\begin{matrix}\colorbox{yellow}{$36$}&0\\0&\colorbox{yellow}{$2.25$}\\\end{matrix}\right] \]

Al combinar la medición con la predicción, y ponderarlas usando la ganancia de Kalman, obtenemos una estimación con menor incertidumbre.

Agregar información nueva, incluso cuando tiene alta incertidumbre, siempre reduce la incertidumbre de la estimación. Consulta el capítulo de Fusión de sensores en el libro y los Apéndices G y H para la demostración matemática. Desde un punto de vista teórico, las nuevas mediciones nunca deberían ignorarse.

En la práctica, sin embargo, a menudo es necesario rechazar ciertas mediciones. Consulta el capítulo de Tratamiento de valores atípicos en el libro para métodos prácticos de manejo de mediciones poco confiables.

Predicción

El paso de predicción de la Iteración 1 (de \( t_1 \) a \( t_2 \)) es idéntico al paso de predicción de la Iteración 0 (de \( t_0 \) a \( t_1 \)), salvo que ahora partimos de la estimación actualizada \(\hat{\boldsymbol{x}}_{1,1}\) y \(\boldsymbol{P}_{1,1}\).

Predicción del estado

\[ \hat{\boldsymbol{x}}_{2,1}=\boldsymbol{F}\hat{\boldsymbol{x}}_{1,1} \]

\[ \hat{\boldsymbol{x}}_{2,1}=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}11,009.37\\201.43\\\end{matrix}\right]=\left[\begin{matrix}12,016.5\\201.43\\\end{matrix}\right] \]

Predicción de la covarianza

\[ \boldsymbol{P}_{2,1}=\boldsymbol{F}\boldsymbol{P}_{1,1}\boldsymbol{F}^\top + \boldsymbol{Q} \]

\[ \boldsymbol{P}_{2,1}=\ \left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}14.57&1.43\\1.43&0.71\\\end{matrix}\right]\left[\begin{matrix}1&0\\5&1\\\end{matrix}\right]+\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right]=\left[\begin{matrix}52.86&7.47\\7.47&1.71\\\end{matrix}\right] \]

Observa que ambas varianzas vuelven a aumentar durante el paso de predicción. Esto sucede porque, a medida que transcurre el tiempo sin una nueva medición, la incertidumbre crece naturalmente. En particular, la incertidumbre en la velocidad causa incertidumbre adicional en la distancia, por lo cual la varianza de la distancia aumenta más rápidamente que la varianza de la velocidad.

Resumen de la Iteración 1

• Actualización
  
  • Estimamos el estado actual del sistema \(\hat{\boldsymbol{x}}_{1,1}\) como una combinación ponderada del estado previsto \(\hat{\boldsymbol{x}}_{1,0}\) y la medición \(\boldsymbol{z}_1\).
    La ponderación está determinada por la Ganancia de Kalman \(K_1\). La Ganancia de Kalman se calcula a partir de la covarianza del estado previsto \(\boldsymbol{P}_{1,0}\) y la covarianza de la medición \(\boldsymbol{R}_1\), y minimiza la incertidumbre de la estimación actualizada \(\boldsymbol{P}_{1,1}\).
  • Las ecuaciones de actualización del Filtro de Kalman son:
    
    
    Ecuación de actualización de estado (State Update Equation)
    \[ \hat{\boldsymbol{x}}_{n,n}=\hat{\boldsymbol{x}}_{n,n-1}+\boldsymbol{K}_n\left(\boldsymbol{z}_n\ -\ \boldsymbol{H}\hat{\boldsymbol{x}}_{n,n-1}\right) \]
    Ecuación de actualización de la covarianza (forma de Joseph)
    \[ \boldsymbol{P}_{n,n}=\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)\boldsymbol{P}_{n,n-1}\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)^T+\boldsymbol{K}_n\boldsymbol{R}_n\boldsymbol{K}_n^T \]
    O su forma simplificada
    \[\boldsymbol{P}_{n,n}=\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)\boldsymbol{P}_{n,n-1}\]
    Ecuación de la ganancia de Kalman (Kalman Gain)
    \[ \boldsymbol{K}_n=\ \boldsymbol{P}_{n,n-1}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T+\boldsymbol{R}_n\right)^{-1}\]
  donde:
  
  • \( \hat{\boldsymbol{x}}_{n,n} \) es la estimación de estado actualizada en el paso de tiempo \(n\)
  • \( \hat{\boldsymbol{x}}_{n,n-1} \) es el estado previsto en el paso de tiempo \(n\), calculado usando información disponible en el tiempo \(n-1\)
  • \( \boldsymbol{z}_n \) es el vector de medición
  • \( \boldsymbol{P}_{n,n} \) es la covarianza de la estimación de estado actualizada
  • \( \boldsymbol{P}_{n,n-1} \) es la covarianza de la estimación de estado prevista
  • \( \boldsymbol{K}_n \) es la ganancia de Kalman
  • \( \boldsymbol{H} \) es la matriz de observación (medición)
  • \( \boldsymbol{R}_n \) es la matriz de covarianza del ruido de medición
  • \( \boldsymbol{I} \) es la matriz identidad
• Predicción
  El paso de predicción en la Iteración 1 es el mismo que en la Iteración 0.
  Propagamos la estimación del estado actual y su covarianza hacia el siguiente instante de tiempo, cuando el radar revisita la aeronave, usando el modelo de transición de estado.

Resumen del ejemplo