ESTIMACIÓN

Dr. Manuel Adam Medina

Profesor Investigador
Agosto 2012
FUSIÓN DE DATOS
 ¿Cómo combinar datos de diversas fuentes, y
distintos grados de precisión?
 Diferentes sensores.
 Diferentes posiciones.
 Medidas obtenidas en diferentes instantes.

 En general deseamos:
 Contar con un estimador X de lo que buscamos.
 Conocer el grado de confianza del estimador (varianza,
covarianza).
ESTIMACIÓN
 Supongamos que contamos con cierto modelo del
mundo (mapa).
 El desafío de localización consiste en realizar
asociaciones correctas entre datos sensoriales y
nuestro modelo.
 Estimación de pose X = f (Zk,…,M)
 En general se divide en dos pasos:
1. Predicción de la nueva posición
2. Actualización de las mediciones.
Sensores
Predicción

Actualización
Modelo M.
FILTRO DE KALMAN
 Una buena herramienta para estimar la pose de
un robot y realizar fusión de datos.
 Es un algoritmo recursivo de procesamiento de
datos, bajo ciertas condiciones permite estimar el
estado de un sistema en forma óptima.
 Entrega el estimador óptimo de X dado un
conjunto de medidas Z sólo si se cumplen las
siguientes condiciones:
1. El sistema es lineal
2. El ruido asociado con el modelo del proceso y las
mediciones es blanco y Gaussiano.
FILTRO DE KALMAN
Caso general:
xk = F[xk-1,uk-1]+wk-1 (Modelo de la planta; predicción)
zk = H[xk,C]+vk (Modelo de mediciones; corrección, actualización de
mediciones)

xk Variable en tiempo k (estado)

uk señal de entrada al sistema. (acciones de control)
zk mediciones en tiempo k (datos de los sensores)
w y v se asumen independientes.
FILTRO DE KALMAN
Caso lineal:
xk = Axk-1 + Buk-1+wk-1 (Modelo de la planta; predicción)
zk = Hxk+vk (Modelo de mediciones; corrección, actualización de
mediciones)

xk pose en tiempo k (estado)

uk señal de entrada al sistema. (acciones de control)
zk mediciones en tiempo k (datos de los sensores)
w y v se asumen independientes.
FILTRO DE KALMAN

W corresponde al ruido del proceso con covarianza Q.

V corresponde al ruido en la medición con covarianza R.

Se asume que poseen las siguientes distribuciones :

Ojo: A, B, y H podrían cambiar en el tiempo, pero se asume

que son constantes.
FILTRO DE KALMAN
Los orígenes del filtro:
Definimos:
el estimador a priori del estado, i.e. función del conocimiento del
proceso previo al paso k.
el estimador a posteriori del estado, i.e. función de los pasos
hasta k y de la medición efectuada en el paso k.
Se definen errores de estimación a priori y a posteriori:

Y las correspondientes matrices de covarianza a priori y a

posteriori
FILTRO DE KALMAN
Objetivo: Encontrar una ecuación que exprese el estimador a
posteriori del estado como una combinación lineal del estimador a
priori del estado y la diferencia entre la medida actual y la
predicción de la medida:
(*)

corresponde al residuo o innovación de la medición.

¿Cómo elegir K?

El objetivo es encontrar K tal que se minimice la covarianza del error

a posteriori.
Es posible encontrar una expresión para K reemplazando (*) en la
expresión del error ek y luego en la expresión de la covarianza del
error a posteriori. Derivando el resultado c/r a K y despejando K.
FILTRO DE KALMAN
Realizando estas operaciones se obtienen las siguientes ecuaciones de
recurrencia:
FILTRO DE KALMAN
Es importante notar lo siguiente:
1. Si la covarianza del error en la medición se aproxima a cero,
la ganancia K otorga mayor peso a la innovación de la
medición, i.e. se confía más en la medición:

2. Si el estimador a priori de la covarianza del error se

aproxima a cero, la ganancia K otorga menor importancia a
la innovación de la medición.
FILTRO DE KALMAN
EN FUNCIONAMIENTO
FILTRO DE KALMAN
EN FUNCIONAMIENTO
 Hay que tener en cuenta lo siguiente:
1. R usualmente se mide antes de poner el filtro en
operación. Esto es generalmente posible.
2. Determinar Q es en general más difícil.
3. También es posible ajustar los parámetros R y Q del
filtro, empleando otro filtro de Kalman.
4. Pk y Kk se estabilizaran rápidamente y encontraran en
general un valor de régimen permanente. Si esto ocurre,
podrían determinarse antes de la ejecución del filtro y
dejarse como constantes.
FILTRO DE KALMAN
EN FUNCIONAMIENTO; EJEMPLO1
Estimar una constante escalar
sujeta a ruido. Caso de un
tester ruidoso y la
determinación de un voltaje.
Las medidas son corrompidas por
un ruido blanco de amplitud
0.1 volts RMS.
FILTRO DE KALMAN
EN FUNCIONAMIENTO; EJEMPLO1

Nuestras ecuaciones de actualización en el

tiempo son

Nuestras ecuaciones de actualización de las

mediciones son
FILTRO DE KALMAN
EN FUNCIONAMIENTO; EJEMPLO1

Q = 1e-5 suponiendo
una varianza muy
pequeña del
proceso.
Valores iniciales:
FILTRO DE KALMAN
EN FUNCIONAMIENTO; EJEMPLO1

Simulación 1, R constante :
FILTRO DE KALMAN
EN FUNCIONAMIENTO; EJEMPLO1

Convergencia de Pk
FILTRO DE KALMAN
EN FUNCIONAMIENTO; EJEMPLO1

Simulación 2, R incrementado en factor

100:
FILTRO DE KALMAN
EN FUNCIONAMIENTO; EJEMPLO1

Simulación 3, R decrece en factor 100: