Clase 56,

1
Clase 5-6,

• Fusión de datos, Kalman, etc.
• Juan Cristóbal Zagal

2
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).

3
Estimación de pose

• 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
• Predicción de la nueva pose
• Actualización de las mediciones.

Sensores
Predicción
Odomet.
Actualización
Modelo M.
4
Estimación de pose

• Ojo I El paso de predicción no es necesario si
  el robot puede derivar su posición directamente
  de los datos sensoriales.
• Ojo II En la realidad esto casi nunca sucede!.

5
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
• El sistema es lineal
• El ruido asociado con el modelo del proceso y las
  mediciones es blanco y Gaussiano.

6
Filtro de Kalman

• Caso general
• xk Fxk-1,uk-1wk-1 (Modelo de la planta
  predicción)
• zk Hxk,Cvk (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.

7
Filtro de Kalman

• Caso lineal
• xk Axk-1 Buk-1wk-1 (Modelo de la planta
  predicción)
• zk Hxkvk (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.

8
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.
9
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

10
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.

11
Filtro de Kalman

• Realizando estas operaciones se obtienen las
  siguientes ecuaciones de recurrencia

12
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.

13
Filtro de Kalmanen funcionamiento
14
Filtro de Kalmanen funcionamiento

• Hay que tener en cuenta lo siguiente
• R usualmente se mide antes de poner el filtro en
  operación. Esto es generalmente posible.
• Determinar Q es en general más difícil.
• También es posible ajustar los parámetros R y Q
  del filtro, empleando otro filtro de Kalman.
• 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.

15
Filtro de Kalmanen 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.

16
Filtro de Kalmanen funcionamiento ejemplo1
17
Filtro de Kalmanen funcionamiento ejemplo1

• Q 1e-5 suponiendo una varianza muy pequeña del
  proceso.
• Valores iniciales

18
Filtro de Kalmanen funcionamiento ejemplo1

• Simulación 1, R constante

19
Filtro de Kalmanen funcionamiento ejemplo1

• Convergencia de Pk

20
Filtro de Kalmanen funcionamiento ejemplo1

• Simulación 2, R incrementado en factor 100

21
Filtro de Kalmanen funcionamiento ejemplo1

• Simulación 3, R decrece en factor 100