Filtro Kalman

Hay muchos artículos diferentes sobre el filtro de Kalman, pero es difícil encontrar el que contenga una explicación, de donde provienen todas las fórmulas de filtrado. Creo que sin entender eso, esta ciencia se vuelve completamente incomprensible. En este artículo intentaré explicar todo de una manera simple.

El filtro Kalman es una herramienta muy poderosa para filtrar diferentes tipos de datos. La idea principal detrás de esto es que uno debe usar una información sobre el proceso físico. Por ejemplo, si está filtrando datos del velocímetro de un automóvil, su inercia le da derecho a tratar una gran desviación de velocidad como un error de medición. El filtro de Kalman también es interesante por el hecho de que de alguna manera es el mejor filtro. Discutiremos con precisión qué significa. Al final del artículo mostraré cómo es posible simplificar las fórmulas.

Preliminares

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

Variable aleatoria

Cuando uno dice que se le da una variable aleatoria $$$\ xi$ , significa que puede tomar valores aleatorios. Diferentes valores vienen con diferentes probabilidades. Por ejemplo, si alguien tira un dado, entonces el conjunto de valores es discreto $$$\ {1,2,3,4,5,6 \}$ . Cuando manejas una velocidad de movimiento de partículas, entonces obviamente deberías trabajar con un conjunto continuo de valores. Valores que salen después de cada experimento (medición) que denotaríamos por $$$x_1, x_2, ...$ , pero a veces usamos 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 un vecindario pequeño $$$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 ancho característico es de alrededor $$$\ sigma$ .
Como estamos hablando de la distribución gaussiana, sería un pecado no mencionar de dónde viene. Así como el número $$$e$ y $$$\ pi$ están firmemente penetrados en las matemáticas y se pueden encontrar en los lugares más inesperados, por lo que Gaussian Distribution tiene raíces profundas en la teoría de la probabilidad. La siguiente declaración notable explica en parte la presencia de la Distribución de Gauss en muchos procesos:
Deje una variable aleatoria $$$\ xi$ tiene una distribución arbitraria (de hecho, existen algunas restricciones a la arbitrariedad, pero no son restrictivas en absoluto). Vamos a realizar $$$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. En otras palabras, esta suma es una variable aleatoria con su propia ley de distribución. Resulta que para lo suficientemente grande $$$n$ , la ley de distribución de esta suma tiende a una distribución gaussiana (por cierto, el ancho característico de una campana está creciendo como $$$\ sqrt n$ . Lea más en Wikipedia: Teorema del límite central . En la vida real hay muchos valores que son la suma de un gran número de variables aleatorias independientes e idénticamente distribuidas. Entonces, estos valores tienen distribución Gauss.

Valor del significado

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 denotan por $$$E \ xi$ (expectativa), los físicos lo denotan por $$$\ overline {\ xi}$ o $$$<\ xi>$ . Lo denotaremos como lo hacen los matemáticos.

Por ejemplo, un valor medio de distribución gaussiana $$$\ rho (x) \ sim e ^ {- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}}$ es igual a $$$\ mu$ .

Varianza

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

En la imagen, se puede ver que un ancho característico de una región donde los valores caen principalmente es $$$\ sigma$ . ¿Cómo podemos estimar este ancho 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 cuán problemático es usar valores absolutos en las fórmulas, por lo que esta fórmula rara vez se usa 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 la desviación estándar.

Por ejemplo, uno puede calcular eso para la distribución gaussiana $$$\ rho (x) \ sim e ^ {- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}}$ la varianza es igual a $$$\ sigma ^ 2$ así la desviación estándar es $$$\ sigma$ . Este resultado realmente corresponde a nuestra intuición geométrica. De hecho, una pequeña trampa está oculta aquí. En realidad, en una definición de la distribución de Gauss, ves el número $$$2$ en un denominador de expresión $$$- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}$ . Este $$$2$ se encuentra allí a propósito, para la desviación estándar $$$\ sigma_ \ xi$ ser igual exactamente a $$$\ sigma$ . Entonces, la fórmula de la distribución de Gauss está escrita de una manera, que tiene en cuenta que uno calcularía su desviación estándar.

Variables aleatorias independientes

Las variables aleatorias pueden depender unas de otras o no. Imagine que está tirando una aguja al piso y midiendo las coordenadas de sus dos extremos. Estas dos coordenadas son variables aleatorias, pero dependen unas de otras, ya que la distancia entre ellas siempre debe ser 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$

Filtro de Kalman

Declaración de problemas

Dejar denotar por $$$x_k$ un valor que pretendemos medir y luego filtrar. Puede ser una coordenada, velocidad, aceleración, humedad, temperatura, presión, etc.

Comencemos con un ejemplo simple, que nos llevará a la formulación del problema general. Imagine que tiene un auto de juguete con radio control que solo puede correr hacia adelante y hacia atrás. Conociendo su masa, forma y otros parámetros del sistema, hemos calculado cómo actúa un joystick de control sobre la velocidad de un automóvil $$$v_k$ .


La coordenada del automóvil 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 actúan en el automóvil como viento, golpes, piedras en la carretera, por lo que la velocidad real del automóvil será diferente de la calculada . Entonces agregamos una variable aleatoria $$$\ xi_k$ a la derecha de la última ecuación:

$$

$$x_ {k + 1} = x_k + v_kdt + \ xi_k$$

También tenemos un sensor GPS en el automóvil que intenta medir la coordenada del automóvil. $$$x_k$ . Por supuesto, hay un error en esta medición, que es una variable aleatoria $$$\ eta_k$ . Entonces, desde el sensor obtendríamos datos incorrectos:

$$

$$z_k = x_k + \ eta_k$$

Nuestro objetivo es encontrar una buena estimación de coordenadas verdaderas $$$x_k$ , conociendo los datos de un sensor incorrecto $$$z_k$ . Esta buena estimación denotaremos por $$$x ^ {opt}$ .
En general la coordenada $$$x_k$ may representa cualquier valor (temperatura, humedad, ...) y el miembro de control que denotaríamos por $$$u_k$ (en el ejemplo con un auto $$$u_k = v_k \ cdot dt$ ) Las ecuaciones para la coordenada y las mediciones 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 un error del 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$ .
• Es posible que no conozcamos una ley de distribución de las variables aleatorias, pero sí conocemos sus variaciones. $$$\ sigma_ \ xi$ y $$$\ sigma_ \ eta$ . Tenga en cuenta que las variaciones 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 ese momento $$$k$ no depende del error en ese momento $$$k ’$ .

Tenga en cuenta que un problema de filtración no es un problema de suavizado. Nuestro objetivo no es suavizar los datos de un sensor, solo queremos obtener el valor lo más cerca posible de la coordenada real $$$x_k$ .

Algoritmo de Kalman

Usaríamos una inducción. Imagina eso 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$ . Recuerde 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 indicar que mostraría el valor que está cerca de $$$x ^ {opt} + u_k$ . Desafortunadamente 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 centro dorado entre la lectura del sensor no preciso $$$z_ {k + 1}$ y $$$x ^ {opt} + u_k$ - nuestra predicción, lo que esperábamos ver en el sensor. Le daremos un peso $$$K$ al valor del sensor y $$$(1-K)$ al valor predicho:

$$

$$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 buena forma, omitiríamos el índice de $$$K$ .
Deberíamos elegir el coeficiente de Kalman de manera que la coordenada estimada $$$x ^ {opt} _ {k + 1}$ sería lo más cercano posible a la coordenada real $$$x_ {k + 1}$ .
Por ejemplo, si sabemos que nuestro sensor es muy súper preciso, entonces confiaríamos en su lectura y le daremos un gran peso ( $$$K$ está cerca de uno). Si el sensor, por el contrario, no es del todo preciso, entonces confiaríamos en nuestro valor teóricamente predicho $$$x ^ {opt} _k + u_k$ .
En una 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 para el error:

$$

$$e_ {k + 1} = (1-K) (e_k + \ xi_k) - K \ eta_ {k + 1}$$

Ahora llega el momento de discutir, ¿qué significa la expresión "para 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 para esa pregunta. Del mismo modo, fue en el caso de la varianza de una variable aleatoria, cuando estábamos tratando de estimar el ancho característico de su función de densidad de probabilidad. Entonces elegiríamos un criterio simple. Minimizaríamos una 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$$

La última expresión toma su valor mínimo, cuando su derivación es cero. Entonces 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, por lo que enfatizamos el hecho de que depende del paso de iteración. Sustituimos la ecuación por el 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}$$

Entonces hemos resuelto nuestro problema. Obtuvimos la fórmula iterativa para calcular el coeficiente de Kalman.

Ejemplo

En la trama desde el comienzo de este artículo hay datos filtrados del sensor GPS ficticio, instalados en el automóvil ficticio, que se mueve con la aceleración constante $$$a$ .

$$

$$x_ {t + 1} = x_t + en \ cdot dt + \ xi_t$$

Análisis

Si uno mira cómo el coeficiente de Kalman $$$K_k$ cambios de la iteración $$$k$ , es posible ver que se estabiliza en cierto valor $$$K_ {puñalada}$ . Por ejemplo, si los errores medios cuadrados del sensor y el modelo se respetan entre sí como diez a uno, entonces la gráfica de la dependencia del coeficiente de Kalman del paso de iteración sería la siguiente:


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

Segundo ejemplo

En la práctica, sucede que no sabemos casi nada del modelo físico que estamos filtrando. Imagine que ha decidido filtrar esas mediciones de su acelerómetro favorito. En realidad, no sabes de frente cómo se movería el acelerómetro. Lo único que puede saber es la variación 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 le hemos impuesto a la variable aleatoria $$$\ xi_k$ . Dado que contiene la información desconocida para nosotros la física del movimiento. No podemos decir que en diferentes momentos los errores son independientes entre sí y sus medias son iguales a cero. En otras palabras, significa que para este tipo de situaciones no se aplica la teoría de Kalman. Pero de todos modos podemos tratar de usar la maquinaria de la teoría de Kalman simplemente eligiendo algunos valores razonables para $$$\ sigma_ \ xi ^ 2$ y $$$\ sigma_ \ eta ^ 2$ para obtener solo un buen gráfico de datos filtrados.
Pero hay una manera mucho más simple. Vimos eso con el aumento del paso $$$k$ el coeficiente de Kalman siempre se estabiliza a cierto valor $$$K_ {puñalada}$ . Entonces, en lugar de adivinar los valores de los coeficientes $$$\ sigma ^ 2_ \ xi$ y $$$\ sigma ^ 2_ \ eta$ y calcular el coeficiente de Kalman $$$K_k$ mediante fórmulas difíciles, podemos suponer que este coeficiente es constante y seleccionar solo esta constante. Esta suposición no afectaría mucho los resultados de filtrado. Al principio, de todos modos, la maquinaria de Kalman no es exactamente aplicable a nuestro problema y, en segundo lugar, el coeficiente de Kalman se estabiliza rápidamente a la constante. Al final todo se vuelve muy simple. No necesitamos casi ninguna fórmula de la teoría de Kalman, solo necesitamos seleccionar un valor razonable $$$K_ {puñalada}$ 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 puede ver las mediciones filtradas por dos formas diferentes desde un sensor imaginario. El primer método es 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 dos de estos métodos. Hay una pequeña variación al principio, cuando el coeficiente de Kalman todavía no está estabilizado.

Discusión

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 estaría lo más cerca posible de 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$ en sí es una función lineal de la medición del sensor $$$z_k$ y el valor filtrado anterior $$$x ^ {opt} _ {k-1}$ . Y así sucesivamente hasta el final de la cadena. Por lo tanto, el valor filtrado depende linealmente de todas las lecturas anteriores del sensor:

$$

$$x ^ {opt} _ {k + 1} = \ lambda + \ lambda_0z_0 + \ ldots + \ lambda_ {k + 1} z_ {k + 1}$$

Esa es una razón por la cual el filtro de Kalman se llama filtro lineal. Es posible demostrar que el filtro de Kalman es el mejor de todos los filtros lineales. Lo mejor en el sentido de que minimiza una media cuadrática del error.

Caso multidimensional

Es posible generalizar toda la teoría de Kalman al caso multidimensional. Las fórmulas allí 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 video puedes ver el ejemplo.

Literatura

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