Una explicación del filtro de Kalman

Question

En los últimos 3 meses he estado tratando de entender el Filtro Kalman. He tratado de implementarlo, he visto los tutoriales de youtube, y he leído algunos artículos sobre él y su funcionamiento (actualización, predicado, etc.)

Pero todavía soy incapaz de entenderlo completamente, o en profundidad.

¿Puede alguien explicarlo con un método muy simple, y cómo funciona para múltiples datos?

Answer 1

Partamos de lo que es un filtro de Kalman: es un método de predicción del estado futuro de un sistema basado en los anteriores.

Para entender lo que hace, eche un vistazo a los siguientes datos: si le dieran los datos en azul, podría ser razonable predecir que el punto verde debería seguir, simplemente extrapolando la tendencia lineal de las pocas muestras anteriores. Sin embargo, ¿qué confianza tendría en predecir el punto rojo oscuro de la derecha utilizando ese método? ¿qué confianza tendría en predecir el punto verde, si se le diera la serie roja en lugar de la azul?

$\quad\quad\quad$

De este sencillo ejemplo podemos aprender tres importantes principios:

1. No basta con dar una predicción: también hay que saber el nivel de confianza.
2. Predecir el futuro con mucha antelación es menos fiable que las predicciones más cercanas.
3. La fiabilidad de sus datos (el ruido), influye en la fiabilidad de sus predicciones.

$$\color{red} {\longleftarrow \backsim *\ *\ *\sim\longrightarrow}$$

Ahora, intentemos utilizar lo anterior para modelar nuestra predicción.

Lo primero que necesitamos es un estado . El estado es una descripción de todos los parámetros que necesitaremos para describir el sistema actual y realizar la predicción. Para el ejemplo anterior, utilizaremos dos números: La posición vertical actual ( $y$ ), y nuestra mejor estimación de la pendiente actual (llamémosla $m$ ). Por lo tanto, el es en general un vector , comúnmente denotado como $\bf{x}$ Y, por supuesto, puede incluir muchos más parámetros si desea modelar sistemas más complejos.

Lo siguiente que necesitamos es un modelo : El modelo describe cómo creemos que se comporta el sistema. En un filtro Kalman ordinario, el modelo es siempre una función lineal del estado. En nuestro caso simple, nuestro modelo es: $$y(t) = y(t-1) + m(t-1)$$ $$m(t) = m(t-1)$$ Expresado como una matriz, esto es: $${\bf{x}}_{t} = \left(\begin{array}{c} y(t)\\ m(t)\\ \end{array} \right) = \left(\begin{array}{c} 1 & 1\\0 & 1\\ \end{array} \right)\cdot \left(\begin{array}{c} y(t-1)\\ m(t-1)\\ \end{array} \right) \equiv F {\bf{x}}_{t-1}$$

Por supuesto, nuestro modelo no es perfecto (¡si no necesitaríamos un filtro de Kalman!), así que añadimos un término adicional al estado: el ruido de proceso , $\bf{v_t}$ que se supone que se distribuye normalmente. Aunque no conocemos el valor real del ruido, suponemos que podemos estimar lo "grande" que es el ruido, como veremos a continuación. Todo esto nos da la ecuación de estado que es simplemente:

$${\bf{x}}_{t} = F {\bf{x}}_{t-1} + {\bf{v}}_{t-1}$$ La tercera parte, y última que nos falta es la medición . Cuando obtengamos nuevos datos, nuestros parámetros deberán cambiar ligeramente para perfeccionar nuestro modelo actual y las próximas predicciones. Lo que es importante entender es que uno no tienen que medir exactamente los mismos parámetros que los del estado. Por ejemplo, un filtro Kalman que describa el movimiento de un coche puede querer predecir la aceleración, la velocidad y la posición del coche, pero sólo medir, por ejemplo, el ángulo de la rueda y la velocidad de rotación. En nuestro ejemplo, sólo "medimos" la posición vertical de los nuevos puntos, no la pendiente. Es decir: $$\text{measurement} = \left(\begin{array}{c} 1 & 0 \end{array} \right) \cdot \left(\begin{array}{c} y(t) \\ m(t) \end{array} \right) $$ En el caso más general, podemos tener más de una medida, por lo que la medida es un vector, denotado por $\bf{z}$ . Además, las propias mediciones son ruidosas, por lo que la ecuación general de medición adopta la forma $${\bf{z}}_t = H {\bf{x}}_t +{\bf{w}}_t$$ Dónde $\bf{w}$ es el ruido de medición y $H$ es, en general, una matriz con la anchura del número de variables de estado y la altura del número de variables de medición.

$$\color{orange} {\longleftarrow \backsim *\ *\ *\sim\longrightarrow}$$

Ahora que hemos entendido lo que supone modelar el sistema, podemos empezar con el predicción etapa, el corazón del filtro Kalman.

Empecemos por suponer que nuestro modelo es perfecto, sin ruido. ¿Cómo vamos a predecir cuál será nuestro estado en el momento $t+1$ ? Muy sencillo. Es nuestra ecuación estatal: $${\bf{\hat x}}_{t+1} = F {\bf{x}}_{t}$$ ¿Qué hacemos? esperar para medir? simplemente la ecuación de medición: $${\bf{\hat z}}_{t+1} = H {\bf{\hat x}}_{t+1}$$ Ahora, ¿qué hacemos medir realmente probablemente algo un poco diferente: $$\bf{y} \equiv {\bf{z}}_{t+1} - {\bf{\hat z}}_{t+1} \neq 0 $$ La diferencia $\bf{y}$ (también llamado innovación ) representa lo errónea que es nuestra estimación actual: si todo fuera perfecto, la diferencia sería cero. Para incorporar esto a nuestro modelo, añadimos la innovación a nuestra ecuación de estado, multiplicada por un factor matricial que nos dice cuánto debería cambiar el estado en función de esta diferencia entre las mediciones previstas y las reales: $${\bf{\hat x}}_{t+1} = F {\bf{x}}_{t} + W \bf{y}$$ La matriz $W$ es conocido como el Ganancia de Kalman Pero la parte realmente importante es entender por qué la predicción adopta esta forma. Pero antes de entrar en la fórmula de $W$ Debemos reflexionar sobre el aspecto que debe tener:

• Si el ruido de la medición es grande, quizá el error sea sólo un artefacto del ruido, y no la "verdadera" innovación. Por lo tanto, si el ruido de la medición es grande $W$ debe ser pequeño.
• Si el ruido del proceso es grande, es decir, esperamos que el estado cambie rápidamente, deberíamos tomar la innovación más en serio, ya que es plausible que el estado haya cambiado realmente.
• Sumando estos dos, esperamos: $$W \sim \frac{\text{Process Noise}}{\text{Measurement Noise}} $$

$$\color{green} {\longleftarrow \backsim *\ *\ *\sim\longrightarrow}$$

Una forma de evaluar la incertidumbre de un valor es observar su desviación . La primera varianza que nos interesa es la varianza de nuestra predicción del estado:

$$P_t = Cov({\bf{\hat x}}_t)$$

Al igual que con ${\bf{x}}_t$ podemos derivar $P_t$ de su estado anterior:

$$P_{t+1} = Cov({\bf{\hat x}}_{t+1}) = \\ Cov(F {\bf{x}}_t) = \\ F Cov({\bf{x}}_t) F^\top = \\ F P_t F^\top$$

Sin embargo, esto supone que nuestro modelo de proceso es perfecto y que no hay nada que no podamos predecir. Pero normalmente hay muchas incógnitas que pueden influir en nuestro estado (quizá haya viento, fricción, etc.). Lo incorporamos como una matriz de covarianza $Q$ del ruido del proceso ${\bf{v}}_t$ y la varianza de la predicción se convierte en

$$P_{t+1} = F P_t F^\top + Q$$

La última fuente de ruido en nuestro sistema es la medición. Siguiendo la misma lógica obtenemos una matriz de covarianza para ${\bf{\hat z}}_{t+1}$ .

$$S_{t+1} = Cov({\bf{\hat z}}_{t+1}) \\ Cov(H {\bf{\hat x}}_{t+1}) = \\ H Cov({\bf{\hat x}}_{t+1}) H^\top = \\ H P_{t+1} H^\top$$

Como antes, recuerda que dijimos que nuestras medidas ${\bf{z}}_t$ tienen un ruido normalmente distribuido ${\bf w}_t$ . Digamos que la matriz de covarianza que describe este ruido se llama $R$ . Añádelo a la matriz de covarianza de la medición:

$$S_{t+1} = H P_{t+1} H^\top + R$$

Finalmente podemos obtener $W$ observando cómo se combinan dos estados normalmente distribuidos (predichos y medidos):

$$W = P_{t+1} H^{\top} S_{t+1}^{-1}$$

Para continuar...

Answer 2

Proporcionaré una derivación sencilla del KF de forma directa y concisa desde una perspectiva probabilística. Este enfoque es la mejor manera de entender primero un KF. Personalmente, considero que las derivaciones no probabilísticas (por ejemplo, el mejor filtro lineal en términos de alguna función de coste de mínimos cuadrados arbitraria) son poco intuitivas y no tienen sentido. Puede omitir los primeros epígrafes que personalmente habría encontrado útiles al aprender los KFs si ya está familiarizado con el material y pasar directamente a la derivación del KF o puede volver a ellos más tarde.

Varianza y covarianza

La varianza es una medida de la dispersión de la densidad de una variable aleatoria es. Se define como: \begin {Ecuación} \text {Var}(x) = \mathbb {E} \left [{ ( x - \mathbb {E} \left [{x} \right ] )^2 } \right ]. \end {equation} La covarianza es una medida no normalizada de cuánto se correlacionan dos variables entre sí. La covarianza de dos variables aleatorias $x$ y $y$ es \begin {Ecuación} \sigma (x,y) = \mathbb {E} \left [{ ( x - \mathbb {E} \left [{x} \right ] )( y - \mathbb {E} \left [{y} \right ] ) } \right ]. \end {Ecuación} La ecuación anterior puede reescribirse como \begin {align} \sigma (x,y) &= \mathbb {E} \left [{ ( x - \mathbb {E} \left [{x} \right ] )( y - \mathbb {E} \left [{y} \right ] ) } \right ] \\ &= \mathbb {E} \left [{ xy - x \mathbb {E} \left [{y} \right ] - y \mathbb {E} \left [{x} \right ] + \mathbb {E} \left [{x} \right ] \mathbb {E} \left [{y} \right ] } \right ] \\ &= \mathbb {E} \left [{xy} \right ] - \mathbb {E} \left [{x} \right ] \mathbb {E} \left [{y} \right ] - \mathbb {E} \left [{y} \right ] \mathbb {E} \left [{x} \right ] + \mathbb {E} \left [{x} \right ] \mathbb {E} \left [{y} \right ] \\ &= \mathbb {E} \left [{xy} \right ] - \mathbb {E} \left [{x} \right ] \mathbb {E} \left [{y} \right ]. \end {align} Tenga en cuenta que $\text{Var}(x) = \sigma(x,x)$ . Porque $\mathbb{E}\left[{xy}\right] = \mathbb{E}\left[{x}\right]\mathbb{E}\left[{y}\right]$ para variables aleatorias independientes, la covarianza de las variables aleatorias independientes es cero. La matriz de covarianza de las variables aleatorias vectoriales se define como: \begin {align} \sigma ( \boldsymbol {x}, \boldsymbol {y}) &= \mathbb {E} \left [{ ( \boldsymbol {x} - \mathbb {E} \left [{ \boldsymbol {x}} \right ] ) {( \boldsymbol {y} - \mathbb {E} \left [{ \boldsymbol {y}} \right ] )}^T } \right ] \\ &= \mathbb {E} \left [{ \boldsymbol {x}{ \boldsymbol {y}}^T} \right ] - \mathbb {E} \left [{ \boldsymbol {x}} \right ]{ \mathbb {E} \left [{ \boldsymbol {y}} \right ]}^T. \end {align}

Media y covarianza de una combinación lineal de variables aleatorias

Dejemos que $\boldsymbol{y} = \boldsymbol{A}\boldsymbol{x}_1 + \boldsymbol{B}\boldsymbol{x}_2$ donde $\boldsymbol{x}_1$ y $\boldsymbol{x}_2$ son variables aleatorias multivariantes variables aleatorias. El valor esperado de $\boldsymbol{y}$ es \begin {align} \mathbb {E} \left [{ \boldsymbol {y}} \right ] &= \mathbb {E} \left [{ \boldsymbol {A} \boldsymbol {x}_1 + \boldsymbol {B} \boldsymbol {x}_2 } \right ] \\ &= \boldsymbol {A} \mathbb {E} \left [{ \boldsymbol {x}_1} \right ] + \boldsymbol {B} \mathbb {E} \left [{ \boldsymbol {x}_2} \right ]. \end {align} La matriz de covarianza para $\boldsymbol{y}$ es \begin {align} \sigma ( \boldsymbol {y}, \boldsymbol {y}) &= \mathbb {E} \left [{ ( \boldsymbol {y} - \mathbb {E} \left [{ \boldsymbol {y}} \right ] ) {( \boldsymbol {y} - \mathbb {E} \left [{ \boldsymbol {y}} \right ] )}^T } \right ] \\ &= \mathbb {E} \left [{ ( \boldsymbol {A}( \boldsymbol {x}_1 - \mathbb {E} \left [{ \boldsymbol {x}_1} \right ]) + \boldsymbol {B}( \boldsymbol {x}_2 - \mathbb {E} \left [{ \boldsymbol {x}_2} \right ]) ) {( \boldsymbol {A}( \boldsymbol {x}_1 - \mathbb {E} \left [{ \boldsymbol {x}_1} \right ]) + \boldsymbol {B}( \boldsymbol {x}_2 - \mathbb {E} \left [{ \boldsymbol {x}_2} \right ]) )}^T } \right ] \\ &= \boldsymbol {A} \sigma ( \boldsymbol {x}_1, \boldsymbol {x}_1){ \boldsymbol {A}}^T + \boldsymbol {B} \sigma ( \boldsymbol {x}_2, \boldsymbol {x}_2){ \boldsymbol {B}}^T + \boldsymbol {A} \sigma ( \boldsymbol {x}_1, \boldsymbol {x}_2){ \boldsymbol {B}}^T + \boldsymbol {B} \sigma ( \boldsymbol {x}_2, \boldsymbol {x}_1){ \boldsymbol {A}}^T. \end {align} Si $\boldsymbol{x}_1$ y $\boldsymbol{x}_2$ son independientes, entonces \begin {Ecuación} \sigma ( \boldsymbol {y}, \boldsymbol {y}) = \boldsymbol {A} \sigma ( \boldsymbol {x}_1, \boldsymbol {x}_1){ \boldsymbol {A}}^T + \boldsymbol {B} \sigma ( \boldsymbol {x}_2, \boldsymbol {x}_2){ \boldsymbol {B}}^T. \end {Ecuación}

Densidad condicional de variables aleatorias normales multivariantes

Dejemos que $\boldsymbol{x}$ , $\boldsymbol{y}$ ser conjuntamente normal con los medios $\mathbb{E}\left[{\boldsymbol{x}}\right]=\boldsymbol{\mu}_{\boldsymbol{x}}$ , $\mathbb{E}\left[{\boldsymbol{y}}\right] = \boldsymbol{\mu}_{\boldsymbol{y}}$ y la covarianza \begin {Ecuación} \left [ \begin {array}{cc} \boldsymbol { \Sigma }_{xx} & \boldsymbol { \Sigma }_{xy} \\ \boldsymbol { \Sigma }_{yx} & \boldsymbol { \Sigma }_{yy} \end {array} \right ]. \end {Ecuación}

Dejemos que \begin {Ecuación} \boldsymbol {z} = \boldsymbol {x} - \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol {y}. \end {Ecuación}

$\boldsymbol{z}$ y $\boldsymbol{y}$ son independientes porque $\boldsymbol{z}$ y $\boldsymbol{y}$ son conjuntamente normales y \begin {align} \sigma ( \boldsymbol {z}, \boldsymbol {y} ) &= \sigma ( \boldsymbol {x} - \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol {y}, \boldsymbol {y} ) \\ \sigma ( \boldsymbol {z}, \boldsymbol {y} ) &= \mathbb {E} \left [{ \left ( \boldsymbol {x} - \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol {y} \right ) { \boldsymbol {y}}^T } \right ] - \mathbb {E} \left [{ \boldsymbol {x} - \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol {y} } \right ] { \mathbb {E} \left [{ \boldsymbol {y}} \right ]}^T \\ \sigma ( \boldsymbol {z}, \boldsymbol {y} ) &= \mathbb {E} \left [{ \boldsymbol {x}{ \boldsymbol {y}}^T } \right ] - \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \mathbb {E} \left [{ \boldsymbol {y} { \boldsymbol {y}}^T} \right ]- \mathbb {E} \left [{ \boldsymbol {x}} \right ]{ \mathbb {E} \left [{ \boldsymbol {y}} \right ]}^T - \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \mathbb {E} \left [{ \boldsymbol {y}} \right ] { \mathbb {E} \left [{ \boldsymbol {y}} \right ]}^T \\ \sigma ( \boldsymbol {z}, \boldsymbol {y} ) &= \boldsymbol { \Sigma }_{xy} - \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yy} = \boldsymbol {0} \\ \end {align}

Dejemos que $\boldsymbol{t}\sim \mathcal{N}(\boldsymbol{\mu}_{\boldsymbol{t}},\boldsymbol{T})$ . Tenga en cuenta que debido a que $\boldsymbol{z}$ y $\boldsymbol{y}$ son independientes, $\mathbb{E}\left[{\boldsymbol{z}|\boldsymbol{y}=\boldsymbol{t}}\right] = \mathbb{E}\left[{\boldsymbol{z}}\right] = \boldsymbol{\mu}_{\boldsymbol{x}} - \boldsymbol{\Sigma}_{xy} \boldsymbol{\Sigma}_{yy}^{-1}\boldsymbol{\mu}_{\boldsymbol{y}}$ . La expectativa condicional para $\boldsymbol{x}$ dado $\boldsymbol{y}$ es \begin {align} \mathbb {E} \left [{ \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}} \right ] &= \mathbb {E} \left [{ \boldsymbol {z}+ \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol {y}| \boldsymbol {y}= \boldsymbol {t}} \right ] \\ \mathbb {E} \left [{ \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}} \right ] &= \mathbb {E} \left [{ \boldsymbol {z}| \boldsymbol {y}= \boldsymbol {t}} \right ]+ \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \mathbb {E} \left [{ \boldsymbol {y}| \boldsymbol {y}= \boldsymbol {t}} \right ] \\ \mathbb {E} \left [{ \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}} \right ] &= \boldsymbol { \mu }_{ \boldsymbol {x}} - \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \mu }_{ \boldsymbol {y}}+ \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \mu }_{ \boldsymbol {t}} \\ \mathbb {E} \left [{ \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}} \right ] &= \boldsymbol { \mu }_{ \boldsymbol {x}} + \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1}( \boldsymbol { \mu }_{ \boldsymbol {t}} - \boldsymbol { \mu }_{ \boldsymbol {y}} ) \\ \end {align}

La covarianza condicional es \begin {align} \text {Var}( \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}) &= \text {Var}( \boldsymbol {z}+ \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol {y}| \boldsymbol {y}= \boldsymbol {t}) \\ \text {Var}( \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}) &= \text {Var}( \boldsymbol {z}| \boldsymbol {y}) + \text {Var}( \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol {y}| \boldsymbol {y}= \boldsymbol {t}) \\ \text {Var}( \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}) &= \text {Var}( \boldsymbol {z}) + \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol {T} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yx} \\ \text {Var}( \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}) &= \text {Var}( \boldsymbol {x} - \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol {y}) + \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol {T} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yx} \\ \text {Var}( \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}) &= \boldsymbol { \Sigma }_{xx} + \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yy} {( \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1})}^T - \boldsymbol { \Sigma }_{xy} {( \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1})}^T - \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yx} + \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol {T} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yx} \\ \text {Var}( \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}) &= \boldsymbol { \Sigma }_{xx} + \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yx} - \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yx} - \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yx} + \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol {T} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yx} \\ \text {Var}( \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}) &= \boldsymbol { \Sigma }_{xx} - \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yx} + \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol {T} \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yx} \\ \text {Var}( \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}) &= \boldsymbol { \Sigma }_{xx} + \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \left ( \boldsymbol {T} - \boldsymbol { \Sigma }_{yy} \right ) \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yx} \\ \end {align}

En resumen, \begin {align} \mathbb {E} \left [{ \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}} \right ] &= \boldsymbol { \mu }_{ \boldsymbol {x}} + \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1}( \boldsymbol { \mu }_{ \boldsymbol {t}} - \boldsymbol { \mu }_{ \boldsymbol {y}} ) \\ \text {Var}( \boldsymbol {x}| \boldsymbol {y}= \boldsymbol {t}) &= \boldsymbol { \Sigma }_{xx} + \boldsymbol { \Sigma }_{xy} \boldsymbol { \Sigma }_{yy}^{-1} \left ( \boldsymbol {T} - \boldsymbol { \Sigma }_{yy} \right ) \boldsymbol { \Sigma }_{yy}^{-1} \boldsymbol { \Sigma }_{yx} \\ \mathbb {E} \left [{ \boldsymbol {y}| \boldsymbol {x}= \boldsymbol {s}} \right ] &= \boldsymbol { \mu }_{ \boldsymbol {y}} + \boldsymbol { \Sigma }_{yx} \boldsymbol { \Sigma }_{xx}^{-1}( \boldsymbol { \mu }_{ \boldsymbol {s}} - \boldsymbol { \mu }_{ \boldsymbol {x}} ) \\ \text {Var}( \boldsymbol {y}| \boldsymbol {x}= \boldsymbol {s}) &= \boldsymbol { \Sigma }_{yy} + \boldsymbol { \Sigma }_{yx} \boldsymbol { \Sigma }_{xx}^{-1} \left ( \boldsymbol {S} - \boldsymbol { \Sigma }_{xx} \right ) \boldsymbol { \Sigma }_{xx}^{-1} \boldsymbol { \Sigma }_{xy} \end {align}

Descripción del sistema

Dejemos que $\boldsymbol{x}_t \sim \mathcal{N}(\boldsymbol{\hat{x}}_t,\boldsymbol{P}_t)$ sea el vector de estado que hay que estimar. La ecuación de transición de estado es \begin {Ecuación} \boldsymbol {x}_{t+1} = \boldsymbol {A}_t \boldsymbol {x}_t + \boldsymbol {B}_t \boldsymbol {u}_t + \boldsymbol {q}_t, \end {ecuación} donde $\boldsymbol{A}_t$ es una matriz de transición de estados, $\boldsymbol{u}_t \sim \mathcal{N}(\boldsymbol{\hat{u}}_t,\boldsymbol{U}_t)$ es un vector de entrada, y $\boldsymbol{q}_t \sim \mathcal{N}(\boldsymbol{0},\boldsymbol{Q}_t)$ es un ruido. La ecuación de medición es \begin {Ecuación} \boldsymbol {y}_{t} = \boldsymbol {C}_t \boldsymbol {x}_t + \boldsymbol {D}_t \boldsymbol {u}_t + \boldsymbol {r}_t, \end {ecuación} donde $\boldsymbol{y}_{t}$ es la medida, $\boldsymbol{C}_t$ es la matriz de salida, $\boldsymbol{D}_t$ es la matriz feed-forward, y $\boldsymbol{r}_t \sim \mathcal{N}(\boldsymbol{0},\boldsymbol{R}_t)$ . Se supone que $\boldsymbol{q}_t$ , $\boldsymbol{u}_t$ , $\boldsymbol{x}_t$ y $\boldsymbol{r}_t$ no están correlacionados.

Actualización de la Propagación Estatal

La media y la covarianza de la estimación de estado a priori (La estimación de estado a priori es la estimación de estado antes de tener en cuenta cualquier medición). se deduce directamente de la media y la covarianza de una combinación lineal de variables aleatorias, \begin {align} \boldsymbol { \hat {x}}_{t} &= \boldsymbol {A}_{t-1} \boldsymbol { \hat {x}}_{t-1} + \boldsymbol {B}_{t-1} \boldsymbol { \hat {u}}_{t-1} \\ \boldsymbol {P}_t &= \boldsymbol {A}_{t-1} \boldsymbol {P}_{t-1} { \boldsymbol {A}_{t-1} }^T + \boldsymbol {B}_{t-1} \boldsymbol {U}_{t-1} { \boldsymbol {B}_{t-1}}^T + \boldsymbol {Q}_{t-1} \end {align}

Actualización de la Propagación de la Medida

Sea la distribución conjunta entre $\boldsymbol{x}_t$ y $\boldsymbol{y}_t$ sea \begin {Ecuación} \left [ \begin {array}{c} \boldsymbol {x}_t \\ \boldsymbol {y}_t \end {array} \right ] \sim \mathcal {N} \left ( \left [ \begin {array}{c} \boldsymbol { \hat {x}}_t \\ \boldsymbol { \hat {y}}_t \end {array} \right ], \left [ \begin {array}{cc} \boldsymbol { \Sigma }_{xx,t} & \boldsymbol { \Sigma }_{xy,t} \\ \boldsymbol { \Sigma }_{yx,t} & \boldsymbol { \Sigma }_{yy,t} \end {array} \right ] \right ) \end {Ecuación}

La medición prevista $\boldsymbol{y}_t$ es \begin {Ecuación} \boldsymbol { \hat {y}}_{t} = \boldsymbol {C}_t \boldsymbol { \hat {x}}_t + \boldsymbol {D}_t \boldsymbol { \hat {u}}_t. \end {Ecuación}

Las submatrices de covarianza son \begin {align} \boldsymbol { \Sigma }_{xx,t} &= \boldsymbol {P}_t \\ \boldsymbol { \Sigma }_{xy,t} &= \mathbb {E} \left [{ \boldsymbol {x}_t { \boldsymbol {y}_t}^T} \right ] - \mathbb {E} \left [{ \boldsymbol {x}_t} \right ] { \mathbb {E} \left [{ \boldsymbol {y}_t} \right ]}^T \\ &= \mathbb {E} \left [{ \boldsymbol {x}_t { \left ( \boldsymbol {C}_t \boldsymbol {x}_t + \boldsymbol {D}_t \boldsymbol {u}_t + \boldsymbol {r}_t \right )}^T} \right ] - \mathbb {E} \left [{ \boldsymbol {x}_t} \right ] { \mathbb {E} \left [{ \boldsymbol {C}_t \boldsymbol {x}_t + \boldsymbol {D}_t \boldsymbol {u}_t + \boldsymbol {r}_t} \right ]}^T \\ &= \boldsymbol {P}_t { \boldsymbol {C}_t }^T \\ \boldsymbol { \Sigma }_{yx,t} &= { \boldsymbol { \Sigma }_{xy,t}}^T = \boldsymbol {C}_t \boldsymbol {P}_t \\ \boldsymbol { \Sigma }_{yy,t} &= \boldsymbol {C}_t \boldsymbol {P}_t{ \boldsymbol {C}_t}^T + \boldsymbol {D}_t \boldsymbol {U}_t { \boldsymbol {D}_t}^T + \boldsymbol {R}_t \end {align}

Dada una medida $\boldsymbol{m}$ la distribución condicional de x dada y es una distribución normal con las siguientes propiedades \begin {align} \boldsymbol { \hat {x}}_{t| \boldsymbol {y}_t= \boldsymbol {m}} &= \mathbb {E} \left [{ \boldsymbol {x}_t| \boldsymbol {y}_t= \boldsymbol {m}} \right ] = \boldsymbol { \hat {x}_t} + \boldsymbol { \Sigma }_{xy,t} \boldsymbol { \Sigma }_{yy,t}^{-1}( \boldsymbol {m} - { \hat { \boldsymbol {y}}_t} ) \\ \boldsymbol {P}_{t| \boldsymbol {y}_t= \boldsymbol {m}} &= \text {Var}( \boldsymbol {x}_t| \boldsymbol {y}_t= \boldsymbol {m}) = \boldsymbol { \Sigma }_{xx,t} - \boldsymbol { \Sigma }_{xy,t} \boldsymbol { \Sigma }_{yy,t}^{-1} \boldsymbol { \Sigma }_{yx,t} \\ \end {align} Sustituyendo, \begin {align} \boldsymbol { \hat {x}}_{t| \boldsymbol {y}_t= \boldsymbol {m}} &= \boldsymbol { \hat {x}_t} + \boldsymbol {P}_t { \boldsymbol {C}_t }^T \left ( \boldsymbol {C}_t \boldsymbol {P}_t{ \boldsymbol {C}_t}^T + \boldsymbol {D}_t \boldsymbol {U}_t { \boldsymbol {D}_t}^T + \boldsymbol {R}_t \right )^{-1}( \boldsymbol {m} - \boldsymbol {C}_t \boldsymbol { \hat {x}}_t - \boldsymbol {D}_t \boldsymbol { \hat {u}}_t ) \\ \boldsymbol {P}_{t| \boldsymbol {y}_t= \boldsymbol {m}} &= \boldsymbol {P}_t - \boldsymbol {P}_t { \boldsymbol {C}_t }^T \left ( \boldsymbol {C}_t \boldsymbol {P}_t{ \boldsymbol {C}_t}^T + \boldsymbol {D}_t \boldsymbol {U}_t { \boldsymbol {D}_t}^T + \boldsymbol {R}_t \right )^{-1} \boldsymbol {C}_t \boldsymbol {P}_t \\ \end {align}

Utilizando un enfoque similar, también es sencillo derivar el RTS-Smoother. Intente escribir la distribución conjunta entre $x_t$ y $x_{t+1}$ y escribir la distribución condicional de $x_t$ dado $x_{t+1}$ y el resultado es el siguiente.

Answer 3

Me atrevo a publicar el enlace de mi explicación: http://david.wf/kalmanfilter/

Filtro Kalman

Hay muchos artículos diferentes sobre el filtro de Kalman, pero es difícil encontrar uno que contenga una explicación, de donde provienen todas las fórmulas de filtrado. Creo que sin la comprensión de que esta ciencia se vuelve completamente incomprensible. Aquí voy a tratar de explicar todo de una manera sencilla. El filtro de Kalman es una herramienta muy poderosa para filtrar diferentes tipos de datos. La idea principal detrás de esto es que uno debe utilizar una información sobre el proceso físico. Por ejemplo, si estamos filtrando datos del velocímetro de un coche, su inercia nos da derecho a tratar una gran desviación de la velocidad como un error de medición. El filtro de Kalman también es interesante por el hecho de que en cierto modo es el mejor filtro. Discutiremos precisamente lo que significa. Al final mostraré cómo es posible simplificar las fórmulas.

Preliminares

Al principio, vamos a memorizar algunas definiciones y hechos de la teoría de la probabilidad.

Variable aleatoria

Cuando se dice que está dada una variable aleatoria $\xi$ significa que puede tomar valores aleatorios. Los diferentes valores tienen diferentes probabilidades. Por ejemplo, si alguien tira un dado, el conjunto de valores es discreto $\{1,2,3,4,5,6\}$ . Cuando se trata de la velocidad de una partícula en movimiento, obviamente se debe trabajar con un conjunto continuo de valores. Los valores que salen después de cada experimento (medición) los denotaríamos por $x_1, x_2,...$ pero a veces usaremos la misma letra que usamos para una variable aleatoria $\xi$ . En el caso de un conjunto continuo de valores, una variable aleatoria se caracteriza por su función de densidad de probabilidad $\rho(x)$ . Esta función muestra una probabilidad de que la variable aleatoria caiga en una pequeña vecindad $dx$ del punto $x$ . Como podemos ver en la imagen, esta probabilidad es igual al área del rectángulo sombreado debajo del gráfico $\rho(x)dx$ .

Muy a menudo en nuestra vida, las variables aleatorias tienen la Distribución de Gauss, cuando la densidad de probabilidad es $\rho(x)\sim e^{-\frac{(x-\mu)^2}{2\sigma^2}}$ .

Podemos ver que la función en forma de campana $\rho(x)$ está centrado en el punto $\mu$ y su anchura característica es de alrededor de $\sigma$ . Ya que estamos hablando de la distribución gaussiana, sería un pecado no mencionar de dónde procede. Así como el número $e$ y $\pi$ están firmemente penetradas en las matemáticas y pueden encontrarse en los lugares más inesperados, por lo que la distribución gaussiana tiene profundas raíces en la teoría de la probabilidad. La siguiente afirmación notable explica en parte la presencia de la distribución de Gauss en muchos procesos:
Sea una variable aleatoria $\xi$ tiene una distribución arbitraria (de hecho hay algunas restricciones a la arbitrariedad, pero no son restrictivas en absoluto). Realicemos $n$ experimentos y calcular una suma $\xi_1+...+\xi_n$ de valores caídos. Hagamos muchos experimentos. Está claro que cada vez obtendremos un valor diferente de la suma. Es decir, esta suma es una variable aleatoria con su propia ley de distribución. Resulta que para valores suficientemente grandes $n$ la ley de distribución de esta suma tiende a una distribución gaussiana (por cierto, la anchura característica de una campana crece como $\sqrt n$ . Más información en la Wikipedia: Teorema del límite central . En la vida real hay muchos valores que son una suma de un gran número de variables aleatorias independientes e idénticamente distribuidas. Así que estos valores tienen la distribución de Gauss.

Valor medio

Por definición, un valor medio de una variable aleatoria es un valor que obtenemos en un límite si realizamos más y más experimentos y calculamos una media de valores caídos. Un valor medio se denota de diferentes maneras: los matemáticos lo denotan por $E\xi$ (expectativa), los físicos la denotan por $\overline{\xi}$ o $<\xi>$ . Lo denotaremos como lo hacen los matemáticos. Por ejemplo, un valor medio de la distribución gaussiana $\rho(x)\sim e^{-\frac{(x-\mu)^2}{2\sigma^2}}$ es igual a $\mu$ .

Desviación

Para la distribución gaussiana, vemos claramente que la variable aleatoria tiende a caer dentro de una determinada región de su valor medio $\mu$ . Disfrutemos de nuevo de la distribución gaussiana:

En la imagen, se puede ver que la anchura característica de una región donde los valores caen mayoritariamente es $\sigma$ . ¿Cómo podemos estimar esta anchura para una variable aleatoria arbitraria? Podemos dibujar un gráfico de su función de densidad de probabilidad y simplemente evaluar visualmente el rango característico. Sin embargo, sería mejor elegir una forma algebraica precisa para esta evaluación. Podemos encontrar una longitud media de desviación del valor medio: $E|\xi-E\xi|$ . Este valor es una buena estimación de una desviación característica de $\xi$ . Sin embargo, sabemos muy bien lo problemático que es utilizar valores absolutos en las fórmulas, por lo que esta fórmula se utiliza raramente en la práctica. Un enfoque más simple (simple desde el punto de vista del cálculo) es calcular $E(\xi-E\xi)^2$ . Este valor se llama varianza y se denota por $\sigma_\xi^2$ . La raíz cuadrática de la varianza es una buena estimación de la desviación característica de la variable aleatoria. Se llama desviación estándar. Por ejemplo, se puede calcular que para la distribución gaussiana $\rho(x)\sim e^{-\frac{(x-\mu)^2}{2\sigma^2}}$ la varianza es igual a $\sigma^2$ por lo que la desviación estándar es $\sigma$ . Este resultado corresponde realmente a nuestra intuición geométrica. De hecho, aquí se esconde una pequeña trampa. En realidad en una definición de la distribución de Gauss se ve el número $2$ en un denominador de expresión $-\frac{(x-\mu)^2}{2\sigma^2}$ . Este $2$ está ahí a propósito, para la desviación estándar $\sigma_\xi$ sea exactamente igual a $\sigma$ . Así que la fórmula de la distribución de Gauss se escribe de una manera, que tenga en cuenta que uno calcularía su desviación estándar.

Variables aleatorias independientes

Las variables aleatorias pueden depender unas de otras o no. Imagina que lanzas una aguja al suelo y mides las coordenadas de sus dos extremos. Estas dos coordenadas son variables aleatorias, pero dependen la una de la otra, ya que la distancia entre ellas debe ser siempre igual a la longitud de la aguja. Las variables aleatorias son independientes entre sí si la caída de los resultados de la primera no depende de los resultados de la segunda. Para dos variables independientes $\xi_1$ y $\xi_2$ la media de su producto es igual al producto de su media: $E(\xi_1\cdot\xi_2) = E\xi_1\cdot\xi_2$

Prueba
Por ejemplo, tener los ojos azules y terminar una escuela con honores son variables aleatorias independientes. Digamos que hay $20\% = 0.2$ de personas con ojos azules y $5\%=0.05$ de las personas con mayores honores. Así que hay $0.2\cdot 0.5 = 0.01 = 1\%$ de personas con ojos azules y mayores honores. Este ejemplo nos ayuda a entender lo siguiente. Para dos variables aleatorias independientes $\xi_1$ y $\xi_2$ que vienen dados por su densidad de probabilidad $\rho_1(x)$ y $\rho_2(y)$ la densidad de probabilidad $\rho(x,y)$ (la primera variable cae en $x$ y el segundo en $y$ ) se puede encontrar mediante la fórmula $$\rho(x,y) = \rho_1(x)\cdot\rho_2(y)$$ Esto significa que $$ \begin{array}{l} \displaystyle E(\xi_1\cdot\xi_2)=\int xy\rho(x,y)dxdy=\int xy\rho_1(x)\rho_2(y)dxdy=\\ \displaystyle \int x\rho_1(x)dx\int y\rho_2(y)dy=E\xi_1\cdot E\xi_2 \end{array} $$ Como ves, la prueba se hace para variables aleatorias que tienen un espectro continuo de valores y están dadas por su densidad de función de probabilidad. La prueba es similar para el caso general.

Filtro Kalman

Planteamiento del problema

Denotemos por $x_k$ un valor que pretendemos medir y luego filtrar. Puede ser una coordenada, velocidad, aceleración, humedad, temperatura, presión, etc. Empecemos con un ejemplo sencillo, que nos llevará a la formulación del problema general. Imagina que tienes un coche de juguete de radiocontrol que sólo puede ir hacia delante y hacia atrás. Conociendo su masa, su forma y otros parámetros del sistema hemos calculado cómo actúa un joystick de control sobre la velocidad del coche $v_k$ .

La coordenada del coche sería por la siguiente fórmula $$x_{k+1}=x_k+v_kdt$$ En la vida real no podemos, no podemos tener una fórmula precisa para la coordenada ya que algunas pequeñas perturbaciones que actúan sobre el coche como el viento, los baches, las piedras en la carretera, por lo que la velocidad real del coche diferirá de la calculada. Así que añadimos una variable aleatoria $\xi_k$ al lado derecho de la última ecuación: $$x_{k+1}=x_k+v_kdt+\xi_k$$

También tenemos un sensor GPS en el coche que intenta medir la coordenada del coche $x_k$ . Por supuesto que hay un error en esta medición, que es una variable aleatoria $\eta_k$ . Así que del sensor obtendríamos un dato erróneo: $$z_k=x_k+\eta_k$$

Nuestro objetivo es encontrar una buena estimación de la coordenada verdadera $x_k$ , conociendo los datos de un sensor erróneo $z_k$ . Esta buena estimación la denotaremos por $x^{opt}$ . En general, la coordenada $x_k$ puede representar cualquier valor (temperatura, humedad,...) y el miembro controlador lo denotaremos por $u_k$ ( en el ejemplo con un coche $u_k = v_k\cdot dt$ ). Las ecuaciones para la coordenada y las medidas del sensor serían las siguientes:
  (1)
Discutamos, qué sabemos en estas ecuaciones.

• $u_k$ es un valor conocido que controla la evolución del sistema. Lo sabemos por el modelo del sistema.
• La variable aleatoria $\xi$ representa el error en el modelo del sistema y la variable aleatoria $\eta$ es el error de un sensor. Sus leyes de distribución no dependen del tiempo (del índice de iteración $k$ ).
• Las medias de los errores son iguales a cero: $E\eta_k = E\xi_k = 0$ .
• Puede que no conozcamos una ley de distribución de las variables aleatorias, pero sí sus varianzas $\sigma_\xi$ y $\sigma_\eta$ . Obsérvese que las varianzas no dependen del tiempo (en $k$ ) ya que las leyes de distribución correspondientes tampoco.
• Suponemos que todos los errores aleatorios son independientes entre sí: el error en el momento $k$ no depende del error en el momento $k’$ .

Nótese que un problema de filtración no es un problema de suavización. Nuestro objetivo no es suavizar los datos de un sensor, sólo queremos obtener el valor que más se acerque a la coordenada real $x_k$ .

Algoritmo Kalman

Usaríamos una inducción. Imaginemos que en el paso $k$ ya hemos encontrado el valor del sensor filtrado $x^{opt}$ que es una buena estimación de la coordenada real $x_k$ . Recordemos que conocemos la ecuación que controla la coordenada real: $$x_{k+1} = x_k + u_k + \xi_k$$

Por lo tanto, antes de obtener el valor del sensor podríamos afirmar que mostraría el valor que se acerca a $x^{opt}+u_k$ . Por desgracia, hasta ahora no podemos decir algo más preciso. Pero en el paso $k+1$ tendríamos una lectura no precisa del sensor $z_{k+1}$ . La idea de Kalman es la siguiente. Para obtener la mejor estimación de la coordenada real $x_{k+1}$ deberíamos obtener un medio dorado entre la lectura del sensor no preciso $z_{k+1}$ y $x^{opt}+u_k$ - nuestra predicción, lo que hemos esperado ver en el sensor. Daremos un peso $K$ al valor del sensor y $(1-K)$ al valor previsto: $$x^{opt}_{k+1} = K\cdot z_{k+1} + (1-K)\cdot(x_k^{opt}+u_k)$$ El coeficiente $K$ se llama coeficiente de Kalman. Depende del índice de iteración, y estrictamente hablando deberíamos escribir $K_{k+1}$ . Pero para mantener las fórmulas en una forma agradable omitiríamos el índice de $K$ . Debemos elegir el coeficiente de Kalman de manera que la coordenada estimada $x^{opt}_{k+1}$ sería lo más parecido a la coordenada real $x_{k+1}$ . Por ejemplo, si sabemos que nuestro sensor es muy superpreciso entonces confiaríamos en su lectura y le daríamos un gran peso ( $K$ está cerca de uno). Si, por el contrario, el sensor no es preciso en absoluto, entonces nos basaríamos en nuestro valor teórico previsto $x^{opt}_k+u_k$ . En la situación general, debemos minimizar el error de nuestra estimación: $$e_{k+1} = x_{k+1}-x^{opt}_{k+1}$$ Utilizamos las ecuaciones (1) (las que están en un marco azul), para reescribir la ecuación del error: $$e_{k+1} = (1-K)(e_k+\xi_k) - K\eta_{k+1}$$

Prueba $$ \begin{array}{l} { e_{k+1}=x_{k+1}-x^{opt}_{k+1}=x_{k+1}-Kz_{k+1}-(1-K)(x^{opt}_k+u_k)=\\ =x_k+u_k+\xi_k-K(x_k+u_k+\xi_k+\eta_{k+1})-(1-K)(x^{opt}_k+u_k)=\\=(1-K)(x_k-x_k^{opt}+\xi_k)-K\eta_{k+1}=(1-K)(e_k+\xi_k)-K\eta_{k+1} } \end{array} $$

Ahora llega el momento de discutir, ¿qué significa la expresión "minimizar el error"? Sabemos que el error es una variable aleatoria por lo que cada vez toma valores diferentes. En realidad no hay una respuesta única a esta pregunta. Lo mismo ocurría en el caso de la varianza de una variable aleatoria, cuando intentábamos estimar la anchura característica de su función de densidad de probabilidad. Entonces elegiríamos un criterio simple. Minimizaríamos la media del cuadrado: $$E(e^2_{k+1})\rightarrow min$$ Reescribamos la última expresión: $$E(e^2_{k+1})=(1-K)^2(E_k^2+\sigma^2_\xi)+K^2\sigma^2_\eta$$

Clave de la prueba A partir del hecho de que todas las variables aleatorias en la ecuación de $e_{k+1}$ no dependen unos de otros y los valores medios $E\eta_{k+1}=E\xi_k=0$ se deduce que todos los términos cruzados en $E(e^2_{k+1})$ se convierten en ceros: $$E(\xi_k\eta_{k+1})=E(e_k\xi_k)=E(e_k\eta_{k+1})=0.$$ De hecho, por ejemplo $E(e_k\xi_k) = E(e_k)E(\xi_k)=0.$
También hay que tener en cuenta que las fórmulas para las desviaciones parecen mucho más sencillas: $\sigma^2_\eta = E\eta^2_k$ y $\sigma^2_\eta = E\eta^2_{k+1}$ (ya que $E\eta_{k+1}=E\xi_k=0$ )

La última expresión toma su valor mínimo, cuando su derivación es cero. Así que cuando: $$\displaystyle K_{k+1} = \frac{Ee^2_k + \sigma^2_\xi}{Ee^2_k+\sigma^2_\xi+\sigma^2_\eta}$$ Aquí escribimos el coeficiente de Kalman con su subíndice, para enfatizar el hecho de que sí depende del paso de iteración. Sustituimos a la ecuación del error cuadrático medio $E(e^2_{k+1})$ el coeficiente de Kalman $K_{k+1}$ que minimizan su valor: $$\displaystyle E(e^2_{k+1}) = \frac{\sigma^2_\eta(Ee^2_k + \sigma^2_\xi)}{Ee^2_k+\sigma^2_\xi+\sigma^2_\eta}$$ Así que hemos resuelto nuestro problema. Tenemos la fórmula iterativa para calcular el coeficiente de Kalman. Todas las fórmulas en un solo lugar:

Ejemplo

En el gráfico del principio de este artículo hay datos filtrados del sensor GPS ficticio, asentado en el coche ficticio, que se mueve con la aceleración constante $a$ . $$x_{t+1} = x_t+ at\cdot dt+ \xi_t$$ Vuelve a mirar los resultados filtrados:

El código en matlab: clear all; N=100 % number of samples a=0.1 % acceleration sigmaPsi=1 sigmaEta=50; k=1:N x=k x(1)=0 z(1)=x(1)+normrnd(0,sigmaEta); for t=1:(N-1) x(t+1)=x(t)+a*t+normrnd(0,sigmaPsi); z(t+1)=x(t+1)+normrnd(0,sigmaEta); end; %kalman filter xOpt(1)=z(1); eOpt(1)=sigmaEta; % eOpt(t) is a square root of the error dispersion (variance). % It's not a random variable. for t=1:(N-1) eOpt(t+1)=sqrt((sigmaEta^2)*(eOpt(t)^2+sigmaPsi^2)/(sigmaEta^2+eOpt(t)^2+sigmaPsi^2)) K(t+1)=(eOpt(t+1))^2/sigmaEta^2 xOpt(t+1)=(xOpt(t)+a*t)*(1-K(t+1))+K(t+1)*z(t+1) end; plot(k,xOpt,k,z,k,x)

Análisis

Si se observa cómo el coeficiente de Kalman $K_k$ cambios de la iteración $k$ es posible ver que se estabiliza al valor determinado $K_{stab}$ . Por ejemplo, si los errores medios cuadrados del sensor y del modelo respecto a cada uno de ellos son de diez a uno, entonces el gráfico de la dependencia del coeficiente de Kalman del paso de iteración sería el siguiente:

En el siguiente ejemplo hablaremos de cómo eso puede simplificar nuestra vida.

Segundo ejemplo

En la práctica ocurre que no sabemos casi nada del modelo físico que estamos filtrando. Imagina que has decidido filtrar las mediciones de tu acelerómetro favorito. En realidad no sabes de antemano cómo se moverá el acelerómetro. Lo único que puedes saber es la varianza del error del sensor $\sigma^2_\eta$ . En este difícil problema podríamos poner toda la información desconocida del modelo físico a la variable aleatoria $\xi_k$ : $$x_{k+1} = x_k + \xi_k$$ Estrictamente hablando este tipo de sistema no satisface la condición que hemos impuesto a la variable aleatoria $\xi_k$ . Ya que tiene la información desconocida para nosotros la física del movimiento. No podemos decir que en diferentes momentos de tiempo los errores son independientes entre sí y sus medias son iguales a cero. En otras palabras, significa que para este tipo de situaciones la teoría de Kalman no se aplica. Pero de todos modos podemos intentar utilizar la maquinaria de la teoría de Kalman simplemente eligiendo algunos valores razonables para $\sigma_\xi^2$ y $\sigma_\eta^2$ para obtener sólo un bonito gráfico de datos filtrados. Pero hay una forma mucho más sencilla. Vimos que con el aumento del paso $k$ el coeficiente de Kalman siempre se estabiliza en el valor determinado $K_{stab}$ . Así que en lugar de adivinar los valores de los coeficientes $\sigma^2_\xi$ y $\sigma^2_\eta$ y calcular el coeficiente de Kalman $K_k$ por fórmulas difíciles, podemos suponer que este coeficiente es constante y seleccionar sólo esta constante. Esta suposición no afectaría mucho a los resultados del filtrado. Al principio, de todas formas, la maquinaria de Kalman no es exactamente aplicable a nuestro problema y, en segundo lugar, el coeficiente de Kalman se estabiliza rápidamente en la constante. Al final todo se vuelve muy simple. No necesitamos casi ninguna fórmula de la teoría de Kalman, sólo tenemos que seleccionar un valor razonable $K_{stab}$ e insertarlo en la fórmula iterativa $$x^{opt}_{k+1} = K_{stab}\cdot z_{k+1}+(1-K_{stab})\cdot x_k^{opt}$$ En el siguiente gráfico se pueden ver las mediciones filtradas de dos maneras diferentes desde un sensor imaginario. El primer método es el honesto, con todas las fórmulas de la teoría de Kalman. El segundo método es el simplificado.

Vemos que no hay una gran diferencia entre estos dos métodos. Hay una pequeña variación al principio, cuando el coeficiente de Kalman aún no está estabilizado.

Debate

Hemos visto que la idea principal del filtro de Kalman es elegir el coeficiente $K$ de manera que el valor filtrado $$x^{opt}_{k+1}= Kz_{k+1}+(1-K)(x^{opt}_k+u_k)$$ en promedio sería lo más cercano a la coordenada real $x_{k+1}$ . Vemos que el valor filtrado $x^{opt}_{k+1}$ es una función lineal de la medición del sensor $z_{k+1}$ y el valor filtrado anterior $x^{opt}_k$ . Pero el valor filtrado anterior $x^{opt}_k$ es una función lineal de la medición del sensor $z_k$ y el valor filtrado anterior $x^{opt}_{k-1}$ . Y así hasta el final de la cadena. Así que el valor filtrado depende linealmente de todas las lecturas de los sensores anteriores: $$x^{opt}_{k+1}= \lambda + \lambda_0z_0 + \ldots + \lambda_{k+1}z_{k+1}$$ Esta es la razón por la que el filtro Kalman se denomina filtro lineal. Es posible demostrar que el filtro de Kalman es el mejor de todos los filtros lineales. El mejor en el sentido de que minimiza la media cuadrada del error.

Caso multidimensional

Es posible generalizar toda la teoría de Kalman al caso multidimensional. Las fórmulas parecen un poco más elaboradas, pero la idea de su derivación sigue siendo la misma que en una dimensión. Por ejemplo, en este bonito vídeo se puede ver el ejemplo.

Literatura

El artículo original, escrito por Kalman, puede descargarse aquí: http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf