Robtica Mvil CC 5316 Clase 13 EKF y

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

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

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

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

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

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.

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

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

Situación Ideal: Filtro de Kalman •

Predicción •

Corrección •

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

Filtro Extendido de Kalman (EKF) •

Predicción •

Corrección •

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.

Muestreo pdf •

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

Sigma Puntos •

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:

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.

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

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

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: i = 0 1 i N N < i 2 N

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

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.

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

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.

UKF: Diagrama de Bloques

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

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

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

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

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

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

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 estadoobservación:

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

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