Fusión - Filtro de Kalman

8/16/2019 Fusión - Filtro de Kalman

1/14

Filtro de Kalman para la fusión de

datos de la IMU.

Teoría + Implementación

Definiciones

Probabilidad y variables aleatorias

Probabilidad

La probabilidad que el resultado de un evento discreto (por ej. el lanzamiento de unamoneda) favorecerá un evento particular, definido por:

En el caso de eventos independientes, la probabilidad que ocurra A o B está dada por:

si la probabilidad de dos resultados, es decir una no afecta a otra, entonces la probabilidadde que ambos ocurran está dada por:

Si A y B son dos sucesos, la probabilidad de que ocurra B dado A se denota por p(B | A) yes llamada probabilidad condicional. Está dada por:


2/14

Variables aleatorias

Una variable aleatoria es básicamente una función que mapea todos los puntos en elespacio de muestra a los números reales. Las variables aleatorias pueden ser discretas,

cuando se pueden contar el conjunto de sus resultados posibles, o continuas, cuando tomavalores en una escala continua.Por ejemplo la variable aleatoria continua X(t) puede mapear tiempo a posición. Encualquier punto en el tiempo, X(t) nos dirá la posición esperada. En el caso de variablesaleatorias continuas, la probabilidad de cualquier evento simple discreto A es de hecho 0.Esto es, P(A)=0. Una función común que representa la probabilidad de una variablealeatoria, es definida como la función de distribución acumulativa. Esta funciónrepresenta la probabilidad acumulativa de las variables aleatorias continuas X para todos(los no contables) eventos hasta x.

Más comúnmente usada en forma de su derivada y se la conoce como la función dedensidad de probabilidad:

Ésta función de densidad tiene propiedades:

Media y Varianza

La media de una variable aleatoria X es el promedio de una secuencia de números paraalgún espacio muestral N (Se define espacio muestral como todos los resultados posibles

de un experimiento estadístico) de una variable aleatoria discreta X. Se expresa con la letra


3/14

griega mu. Nos podemos referir a esta media como el valor esperado de la variablealeatoria X, y se expresa como E(X). La media está dada por

Si X es una variable aleatoria discreta con distribución de probabilidad f(x) la media o valoresperado de X es:

si la variable aleatoria X es continua:

Si X es una variable aleatoria discreta con distribución de probabilidad f(x) y media , laμ

varianza de X se expresa como y se define de la siguiente forma:σ2

cuando la variable aleatoria X es continua:

Si X es continua la raíz cuadrada de la varianza , se llama desviación estándar de√σ2 = σ

X.

Distribución Normal o Gaussiana

Muchos procesos de la naturaleza parecen tener una distribución normal o muy cercana aesta. En efecto, bajo ciertas condiciones, se puede probar que una suma de variables

aleatorias con cualquier distribución tiende hacia una distribución normal. El teorema quedefine esta propiedad se llama el teorema del límite central. La distribución normal tienealgunas propiedades que la hacen matemáticamente manejable y hasta interesante.


4/14

Una variable aleatoria continua X que tiene la distribución en forma de campana de la figurase llama variable aleatoria normal. La ecuación matemática para la distribución deprobabilidad de la variable normal depende de los parámetros y . Por lo tanto seμ σ

representan los valores de densidad de X por n(x; , ) .μ σ

La función de densidad de probabilidad para una variable aleatoria normal X, con media muy varianza sigma^2 está dada por:

Cualquier función lineal de un proceso aleatorio normalmente distribuido también es unproceso aleatorio normalmente distribuido. En particular si X ~ N( , ) y Y = aX + b,μ σ2

entonces

Si X1 y X2 son independientes, X1 ~ N( , ) y X2 ~ N( , ) entonces.1μ 1σ2 2μ 2σ2


5/14

Independencia contínua y probabilidad condicional

Dos variables aleatorias continuas X y Y son llamadas estadísticamente independientes sisu probabilidad conjunta es igual a la suma de sus probabilidades marginales, es( x, ) f xy y

decir, se considera independiente si .( x, ) f ( x) f ( y) f xy y = x y

Adicionalmente la Regla de Bayes, tomando la ecuación de probabilidad condicional ,ofrece una manera de especificar la densidad de probabilidad condicional de unavariable aleatoria X dada (en la presencia de) Y (Variable aleatoria). En este caso la Reglade Bayes está dada por:

Dado un proceso discreto X y un proceso continuo Y, la función discreta de masa deprobabilidad para X condicionado sobre Y = y está dada por:

Espacio vs Espectro (señales)

Se puede decir que la magnitud de la varianza de una señal indica cuánto “ruido” tiene laseñal. Sin embargo, la varianza de una señal no dice nada acerca del espaciamiento o latasa de cambio con el paso del tiempo.Una característica útil tiempo-relación de una señal aleatoria es su función deautocorrelación Rx , su relación con sí misma a través del tiempo. Formalmente se puededefinir la función de autocorrelación de una señal aleatoria X(t) de la siguiente forma:

Para los tiempos de muestra t1 y t2. Si el proceso es estacionario, es decir que la función dedensidad no varía con el tiempo, la ecuación sólo depende de la diferencia . Ent τ = 1 − t 2

este caso la autocorrelación se escribe como:


6/14

Dos funciones de autocorrelación se muestran en la figura siguiente, se observa que encomparación con la señal aleatoria X2, la señal aleatoria X1 es relativamente más pequeñay más ancha. Mientras | | aumenta (cuando se mueve lejos de en el centro de laτ τ = 0

curva) la señal de autocorrelación para X2 baja relativamente rápido. Esto indica que X2está menos correlacionada con sí misma que X1.

Características en el dominio de frecuencia.

Se puede ver que la autocorrelación es una función del tiempo. Esto significa quetambién tiene una interpretación espectral en el dominio de la frecuencia. Nuevamente paraun proceso estacionario, existe una importante relación temporal-espectral conocida comola relación o el teorema Wiener-Khinchine:

que expresa que para un proceso estacionario, la función (llamada la función( jω)S X

densidad espectral de potencia (PSD: power spectral density function ) de la señalaleatoria puede ser obtenida por la transformada de Fourier ( F[Rx(t)] ) de la función deautocorrelación de la señal aleatoria, donde indica el número de ciclos por segundo (2 f).ω π

Como se puede ver, esta relación enlaza las representaciones del espectro de tiempo y defrecuencia de la misma señal.Esta función PSD muestra cuánta potencia está almacenada en cada componente del

espectro. Por ejemplo, para una onda sinusoidal con frecuencia fija, el gráfico de PSDcontendrá sólo un componente del espectro presente en la frecuencia dada.


7/14

Un proceso aleatorio (o señal) con una función PSD constante es un proceso de ruido

blanco.

Ruido Blanco.

Una señal de ruido blanco (proceso) está constituida por un conjunto de variablesaleatorias independientes e idénticamente distribuidas (IID). En sentido discreto, laseñal de ruido blanco constituye una serie de muestras que son independientes y generadaa partir de la misma distribución de probabilidad .Un importante caso de una señal aleatoria es cuando la función de autocorrelación es unafunción Dirac delta que tiene valor cero en todas partes excepto cuando . En otras(τ)δ τ = 0

palabras donde:

para alguna magnitud constante A. En este caso especial donde la autocorrelación es un”pulso”, la transformada de Fourier resulta en un espectro de frecuencia constante como se

muestra en la figura.


8/14

Esta es una descripción de ruido blanco, donde todas las frecuencias son igualmenteimportantes o tienen la misma potencia espectral, y están completamente nocorrelacionadas, excepto en el tiempo actual cuando . Esta por esto que se denominanτ = 0

independientes a las señales de ruido blanco. Cualquier muestra de la señal en un instante

tiempo es completamente independiente (sin correlación) de otra muestra en otro instantede tiempo.

Aunque no es posible ver ni alcanzar en la práctica (ningún sistema puede exhibir energíainfinita a través de un espectro infinito), el ruido blanco es una parte importante en el diseñoy el análisis. Las señales aleatorias pueden ser modeladas como ruido blanco filtrado.Literalmente esto quiere decir que uno podría filtrar la salida de una (hipotética) fuente deruido blanco para lograr una fuente de ruido no blanca o “ colorida” que es delimitada en eldominio de la frecuencia, y más correlacionada con el dominio de tiempo.

Por ejemplo, se puede generar una señal de ruido blanco utilizando un generador denúmeros aleatorios donde todas las muestras siguen una distribución de Gauss dada. Estose conoce como ruido blanco gaussiano o ruido blanco con distribución normal. Del mismomodo, una señal de ruido blanco generado a partir de una distribución uniforme sedenomina ruido blanco uniforme. Ruido blanco gaussiano y ruido blanco uniforme se utilizancon frecuencia en el modelado del sistema. Ruido blanco gaussiano se puede generarutilizando la función "randn" en Matlab que genera números aleatorios que siguen unadistribución de Gauss. Del mismo modo, función "rand" se puede utilizar para generar ruidoblanco uniforme en Matlab que sigue una distribución uniforme.Por ejemplo, generamos un ruido blanco con distribución gaussiana de longitud L=100000,media 0 y desviación estándar = 1 (en Matlab con la función randn).Para calcular la función de densidad de la variable aleatoria:

En el gráfico de autocorrelación, se observa que el pulso alcanzado es sigma^2 = 1 en t=0lo cuál se verifica la función de autocorrelación.El gráfico de PSD nos muestra casi una potencia fija en todas las frecuencias. Osea quepara una señal de ruido blanco la PSD es constante en todas las frecuencias (-infinito;+infinito). El eje y está expresado en [dB/Hz].


9/14

Función/Matriz Covarianza.

La función de covarianza se define como:

Se utiliza porque estamos tratando con procesos aleatorios compuestos por n variablesaleatorias (10 en el ej anterior).Dicho proceso es visto como vector aleatorio multivariado o variable aleatorio multivariada.Para estas variables aleatorias multivariadas, la función de covarianza especifica cómo

cada una de las n variables en el proceso aleatorio se comporta respecto a la otra. Lafunción de covarianza generaliza el concepto de varianza en múltiples dimensiones.


10/14

La representación matricial se llama la matriz de covarianza de la señal aleatoria de ruidoblanco:

Dado que las variables aleatorias en la señal de ruido blanco son independientes (nocorrelacionadas) la función de covarianza contiene valores a lo largo de su diagonal. Lamatriz anterior muestra que sólo existe la función de autocorrelación para cada variablealeatoria. Los valores fuera de la diagonal son cero. Los elementos de la diagonal son iguala la varianza. La función del conjunto autocorrelacionado de ruido blanco está dada por:

Esto indica que la función de autocorrelación de la señal de ruido blanco es cero en todaspartes excepto en el retardo .τ = 0

Estimación estocástica

Modelos Espacio-Estado

Problema de diseño del observador

Ruido de mediciones y proceso.

Filtro de Kalman

Definiciones

El filtro de Kalman, pese a ser llamado filtro, es un algoritmo de estimación. Fue inventadopor Rudolph E. Kalman, quien en 1960 publicó un artículo describiendo una soluciónrecursiva para el problema de filtrado lineal de datos discretos (Kalman 1960).Es básicamente un conjunto de ecuaciones matemáticas que implementan un estimador deltipo predictor-corrector, óptimo en el sentido que minimiza la covarianza de error estimado

(cuando algunas condiciones supuestas se cumplen).


11/14

El filtro de Kalman se ha utilizado ampliamente para el seguimiento interactivo porcomputadora. También utilizado para predecir movimiento y para la fusión de múltiplessensores (inerciales).Es un modelo matemático en términos de Espacio-Estado.El filtro de Kalman es una ampliación del algoritmo LMS de Mínimos cuadrados, pero mássofisticado.Es una solución recursiva, válida tanto para ambientes estacionarios y no estacionarios, queteóricamente es ideal para fusionar los datos con ruido.Sirve como base para un gran número de algoritmos de estimación utilizados ennavegación. Sus funciones incluyen la de mantener una solución de navegación por satélitesuavizada, el alineamiento y calibración de INS. Esta integración es clave en la obtenciónde una solución de navegación óptima a partir de las mediciones disponibles en el sistemade navegación.

Realiza estimaciones en tiempo real de distintos parámetros de un sistema, como porejemplo posición y velocidad, que pueden cambiar continuamente. Las estimaciones sonactualizadas a partir de distintas mediciones realizadas sobre el sistema, que puedencontener ruidos. Estas mediciones son realizadas por sensores, y han de ser función de losparámetros estimados. No es necesario que las mediciones contengan en todos los pasosde tiempo suficiente información para determinar los valores de los parámetros.

El FK utiliza el conocimiento de las propiedades determinísticas y estadísticas de losparámetros del sistema y de las mediciones para obtener estimaciones óptimas de susestados a partir de la información disponible. El FK incluye una incertidumbre sobre lasestimaciones realizadas y una medida de las correlaciones entre los errores en lasestimaciones de los distintos parámetros.

El FK es una técnica de estimaciones Bayesiana. Se le proporciona un grupo deestimaciones iniciales y este opera de forma recursiva, actualizando las estimaciones enfunción de los valores previos y de los nuevos obtenidos de las últimas mediciones. Esteasume que el sistema en estudio es lineal, lo que lleva a realizar ciertas asuncionesdescritas posteriormente. Para aplicaciones en tiempo real, como es el caso de lanavegación, el enfoque recursivo de este algoritmo es más eficiente, dado que solo sedeben procesar los datos de las nuevas mediciones en cada iteración.

Principalmente minimiza el error, pero lo hace basándose en más parámetros. Además

predice cuál debería ser la salida para el estado siguiente y da una estimación a partir delas medidas en el mismo instante de tiempo. Todas estas medidas tienen su propiaestadística y dependiendo de cual sea más fiable, se otorga más peso a la predicción o a laestimación. En el fondo se integran estados redundantes para obtener información másprecisa.

Para nuestro proyecto, se utilizará el filtro de Kalman para obtener la posición, la velocidad yel rumbo del móvil (en 3D). Por un lado están las medidas de la IMU, que son muy precisas

en un espacio de tiempo reducido y por otro las medidas del magnetómetro y del GPS (en


12/14

caso de usar uno) cuyos errores no aumentan con el tiempo sino que es constante, perotienen un error más grande.

Elementos y fases del FK

Está compuesto por 5 elementos principales:- vector de estados y su covarianza- modelo del sistema- vector de mediciones y su covarianza- modelo de mediciones- algoritmo

El vector de estados es un grupo de parámetros que describen el sistema, conocidas

como estados, que el filtro de Kalman utiliza para estimar una solución óptima. Un estadopuede ser constante o variable en el tiempo.

x = x x x ... x[ 1 2 3 n] T

El vector de estado incluye no sólo los parámetros deseados, sino además aquellos que sehan añadido al filtro con tal de introducir redundancia al problema.

Asociada a este vector de estados va la matriz de covarianza del error , que representa laincertidumbre sobre las estimaciones de los estados del FK, y el grado de correlación entrelos errores en esas estimaciones. Conocer estas correlaciones es de vital importancia en

algunos casos, dado que en ocasiones no existe suficiente información para estimar cadauno de los estados de forma independiente. Dado que el FK es un procedimiento iterativo,los valores iniciales del vector de estado y los de la matriz de covarianza han de serproporcionados por el usuario o determinados por un proceso externo al FK.

El modelo del sistema, o modelo de transición en el tiempo, describe la variación en eltiempo del vector de estados y de su matriz de covarianza. Aplica el efecto de cadaparámetro de estado del sistema de iteraciones previas al estado actual del sistema. Porejemplo, un estado de posición varía con el tiempo como la integral del estado de velocidad,la incertidumbre de la posición aumenta con el tiempo como la integral de la incertidumbrede la velocidad. Con esto, se puede ver que los errores en la estimación de la velocidad y laposición están correlacionados. El modelo del sistema es determinista para los estados,dado que está basado en propiedades del sistema conocidas. Asociado a este modelo estáel ruido del sistema, que hace que la incertidumbre de los estados pueda aumentar con eltiempo debido a cambios desconocidos en el sistema, que hacen que la estimación de losestados sea errónea. Este ruido suele venir de los sensores o de parámetros dinámicos delos cuales no tenemos información. Este ruido y sus propiedades estadísticas han de serdefinidos y estimados previamente a la implementación del Filtro de Kalman (por eldiseñador).


13/14

El vector de mediciones está formado por un grupo de variables cuya medida estádisponible, realizadas en el mismo paso de tiempo, que son función del vector de estados. Apartir de la información de este vector se calculan las estimaciones de los estados despuésde la inicialización.

z = z z z ... z [ 1 2 3 m] T

Se suele dar el caso de que la dimensión (n) del vector de estados x sea más grande que ladimensión (m) del vector de medidas z . Ambos vectores están relacionados de maneralineal tal que a partir del vector de estados, se calcula el vector de variables.

Asociada a este vector está la matriz de covarianza del ruido en las mediciones, quedefine las propiedades estadísticas del ruido en las mediciones realizadas.

El modelo de mediciones o de observación describe la variación del vector demediciones como función del vector de estados real (no el estimado) en ausencia de ruidoen las mediciones. Al igual que el modelo del sistema, este es determinista, dado que sebasa en propiedades conocidas del sistema. Mapea los parámetros del vector de estado enel dominio de la medición.

El algoritmo del KF emplea el vector de mediciones, el modelo de mediciones, y el modelodel sistema para calcular las estimaciones óptimas del vector de estados. Cuenta con dosetapas:

1) Predicción - Actualización del tiempo2) Corrección - Actualización de la medición

En cada iteración, el algoritmo realiza 10 pasos, donde los 4 primeros forman la fase depredicción, y los 6 siguientes la fase de actualización.

Filtro de Kalman Discreto

Se describe a continuación el filtro, según la formulación original en Kalman 1960, en el quese producen las mediciones y el estado es estimado en puntos discretos en el tiempo.


14/14

El proceso a estimar

Modelos

Modelo del sistema

Modelo de la medición

Modelo a priori

Origen computacional del FK

Origen probabilístico del FK

El Algoritmo del FKD

Filtro de Kalman Extendido (FKE)

Documents

Fusión - Filtro de Kalman