Fusión Sensorial

Fusión sensorial: Filtro de Kalman (I)


1. Objetivo de la práctica.
Se pretende estimar la posición de un móvil a partir de las medidas de distancia a tres
balizas de posición conocida. Para abordar el problema se han tomado las siguientes
simplificaciones:
a) El teléfono se mueve en un espacio plano delimitado por sólo tres repetidores en
posiciones conocidas: (0,0), (500,1000) y(1000,500)
b) El error de medida se puede considerar gaussiano de media nula con una
desviación típica de 10m más el 10% del valor de la medida.
c) Se asume que el teléfono puede estar en movimiento siguiendo una trayectoria
aleatoria y que la variación de posición típica entre dos instantes de muestreo es
de 50m.

Se pretende diseñar un filtro de Kalman que permita localizar la posición delteléfono.

2. Cálculo de los parámetros del filtro.
Vector de estado: queremos calcular la posición del móvil en cada instante de muestro
por lo que el vector de estado serán sus coordenadas respecto al sistema de referencia en
que se dan las posiciones de las balizas.
X= [x1, x2]t
Ecuaciones de propagación de estado: como la evolución de la posición del móvil es
aleatoria, se supone que estáen reposo y cualquiera variación la asumiremos al error de
la ecuación de propagación de estado.

Ecuación de medida: es la distancia Euclídea a cada una de las balizas, el resultado es
no lineal por lo que habrá que linealizarlo. También se incluye el error de medida que,
como se ha indicado anteriormente, se estima en 10m más el 10% del valor medido.

Ecuación de medida linealizada: secalcula como el Jacobiano de la ecuación de medida.

Inicialización del filtro: se supone que inicialmente el móvil está próximo al origen y se
le asigna una covarianza muy grande (500m de desviación típica en cada coordenada)

3. Ecuaciones del filtro de Kalman
Como la ecuación de medida no es lineal es necesario emplear el filtro extendido de
Kalman. Se compone de dos etapas básicas, elciclo de medida y el ciclo de
propagación.
Modelo del sistema: como la ecuación de medida no es lineal es necesario calcular su
Jacobiano tal y como se ha especificado anteriormente, de este modo el modelo del
sistema es:

De este modo, se realiza un programa principal en Matlab en el que se define el modelo
y las posiciones de las balizas y se realiza la ejecución del filtro llamando asubrutinas
que realizan las funciones de propagación, lectura de las medidas y actualización del
filtro

Propagación de estado: se realiza a través de la función propaga, que implementa las
ecuaciones de evolución del estado y de su covarianza.

Lectura de medidas: se realiza a través de la función lee_medidas que devuelve el valor
de la distancia medida a cada una de las balizas y lacovarianza del error en cada
instante de muestreo.

Predicción: mediante la función jacobiano se calcula el jacobiano de la ecuación de
medida y se predice la medida en función del estado actual y las posiciones de las
balizas.

Actualización: implementa el ciclo de actualización del filtro de Kalman a partir de las
innovaciones (diferencia entre la medida obtenida y su predicción). Se realiza através
de la función actualiza.

Si se ejecuta el programa principal obtendremos la estimación de la posición del móvil
en cada instante de muestreo.

Las cruces verdes representan la
posición del móvil, las elipses
amarillas la incertidumbre de la
estimación de posición, los
círculos rojos son las balizas y
el círculo rojo centrado en el
origen es la incertidumbre del
estado inicial.4. Comportamiento del filtro.
Se pretende estudiar el comportamiento del filtro ante las siguientes circunstancias:
a) Aumentar la covarianza del ruido en el sistema.
Implica añadir más incertidumbre al modelo de propagación, la covarianza del ruido del
sistema se representa con la matriz Q y si aumentamos su valor el resultado es el
siguiente:

Aumenta la incertidumbre de la...