Robótica Móvil CC5316 Clase 13: EKF y UKF Semestre Primavera 2012 Profesor: Pablo Guerrero.

Presentación del tema: "Robótica Móvil CC5316 Clase 13: EKF y UKF Semestre Primavera 2012 Profesor: Pablo Guerrero."— Transcripción de la presentación:

1 Robótica Móvil CC5316 Clase 13: EKF y UKF Semestre Primavera 2012 Profesor: Pablo Guerrero

2 Estimación de Estado Se desea estimar el estado de un sistema dinámico no lineal.

3 Planteamiento Bayesiano Modelo del Proceso Probabilidad a Priori Probabilidad a Posteriori Anterior Predicción: Incorporar información de entrada

4 Planteamiento Bayesiano Modelo Observacional Probabilidad de observación Probabilidad a Priori Evidencia: Información de observación

5 Estimadores Bayesianos Ecuaciones bayesianas recursivas para Predicción: Evidencia: Solución Bayesiana:

6 No computabilidad La solución bayesiana es generalmente no computable. Sólo se puede resolver cuando el problema tiene ciertas restricciones. Si el problema no tiene dichas restricciones, a veces se puede obtener una solución aproximada asumiendo las restricciones.

7 Restricciones PDF: – Gaussiana – Discreta Modelos: – Lineales Se puede describir con 2 primeros momentos Se puede describir con un conjunto de muestras Se puede usar algebra lineal con los jacobianos, que son constantes

8 Aproximaciones Notables EKF Modelos Linealizados + 2 momentos PDF Discreta Grillas de Markov, Filtros de Partículas UKF2 momentos

9 Situación Ideal: Filtro de Kalman

10 Predicción

11 Corrección

12 Filtros de Kalman no Lineales Se puede usar el filtro de Kalman cuando los modelos no son lineales? No en forma exacta Sí con aproximaciones: – Linealizando los modelos: EKF – Submuestreando las PDF: UKF

13 Filtro Extendido de Kalman (EKF)

14 Predicción

15 Corrección

16 Unscented Kalman Filter “Unscented Filtering and Nonlinear Estimation” Simon J. Julier and Jeffrey K. Uhlmann. “The Unscented Kalman Filter for Nonlinear Estimation” Eric A. Wan and Ruldolph van der Merwe.

17 Muestreo pdf

18 Aproximación Integrales De esta forma, las integrales sobre una pdf se pueden aproximar por sumatorias: Ej: Predicción:

19 Sigma Puntos

20 Pesos de los Sigma Puntos Cada sigma punto x (i) tiene un peso asociado W (i) el cual puede ser positivo o negativo. Para evitar el sesgo, los pesos deben cumplir con la condición:

21 Diferencias con MC Los sigma puntos no son muestreados aleatoriamente sino escogidos en forma determinística de forma que tengan ciertas propiedades. De esta forma se pueden mantener estadísticas de alto orden con un número fijo y pequeño de puntos. El peso de los sigma puntos no es consistente con el de las partículas de MC.

22 Transformación no Lineal Los sigma puntos permiten aproximar la propagación de la distribución de probabilidades a través de una transformación no lineal h(x). Aplicamos a cada punto la transformación no lineal. Transformación no lineal

23 Cálculo de Estadísticas La media está dada por el promedio ponderado de los sigma puntos transformados La covarianza es el promedio ponderado del producto externo de las desviaciones transformadas

24 Selección de Sigma Puntos Existen distintas alternativas, dependiendo de cuánta información se desee preservar. En el paper se propone la siguiente alternativa: N < i  2N1  i  N i = 0

25 Raíz de una matriz Dada una matriz P, se dice que otra matriz L es raíz de P si se cumple:

26 Transformada de Cholesky Para una matriz simétrica definida positiva P, existe una descomposición LU especial conocida como la transformada de Cholesky tal que: Y por lo tanto, L es una raíz de P.

27 Transformada Unscented Consiste en transformar una pdf gaussiana a un conjunto de sigma puntos usando la raíz dada por la transformada de Cholesky.

28 Aplicación de la UT: UKF Se usa la transformada Unscented para representar y propagar las distribuciones de probabilidades en el espacio de estados y de las observaciones. Se calculan las estadísticas necesarias para actualizar el filtro de Kalman a partir de los sigma puntos.

29 UKF: Diagrama de Bloques

30 UKF: Espacio de Estados Se aumenta el espacio de estados, incorporando los ruidos del proceso y de la observación, de la siguiente forma:

31 UKF: Modelos Se reescriben los modelos del proceso y observacional en función del nuevo espacio de estado:

32 UKF: Sigma Puntos Se calculan los sigma puntos sobre el espacio de estados aumentado, considerando las siguientes media y covarianza:

33 UKF: Modelo del Proceso Se propagan los sigma puntos a través del modelo del proceso.

34 UKF: Estadísticas a Priori Los sigma puntos predichos se usan para calcular las estadísticas a priori.

35 UKF: Modelo Observacional Los sigma puntos predichos se propagan a través del modelo observacional.

36 UKF: Estadísticas Observación Los sigma puntos en el espacio de las observaciones se usan para calcular las estadísticas de la observación: – Observación Esperada: – Matrices de Innovación y de covarianza estado- observación:

37 UKF: Corrección Finalmente, para realizar corrección, se usan las ecuaciones convencionales del Fitro de Kalman:

38 Comparación Filtros de Kalman Aparentemente, UKF tiene un performance superior a EKF en la mayoría de las aplicaciones de filtrado no lineal. KFEKFUKF Linealidad del Problema LinealNo Lineal Nivel de Aproximación Solución Óptima Primer Orden Segundo Orden+ Complejidad Computacional N2N2 N2N2 N 3  N 2