4.6 El filtro de Kalman Introducción Filtro de Kalman discreto Algoritmo: predicción + corrección Características probabilísticas El filtro de Kalman extendido Filtros de Partículas Conclusiones
Introducción Formulación matemática en términos del concepto espacio del estado Sistema Dinámico Estado x(t) Sistema de Medida Observación z(t) Estimador Errores del sistema Errores de medida Información previa Solución recursiva Estima del estado ˆx(t) Solución válida para ambientes estacionarios y no estacionarios Soporta estimas de estados presentes, pasados y futuros (filtrado, suavizado y predicción) Método eficiente para resolver el problema de mínimos cuadrados (incluye al RLS y sus variantes)
Filtro de Kalman discreto Plantemiento: Estima del estado x IR n de un proceso lineal discreto en el tiempo a partir de un conjunto de medidas z IR m Ecuación del proceso x k = Ax k 1 + Bu k + w k 1 Ecuación de medida z k = Hx k + v k Ruido de proceso y ruido de medida: independientes, blancos y con distribución gaussiana p(w) N(0, Q), p(v) N(0, R)
Definiciones Estima a priori del estado: ˆx k IRn Error de estima a priori: e k = x k ˆx k Estima a posteriori del estado: ˆx k IR n Error de estima a posteriori: e k = x k ˆx k Matriz de correlación del error de estima a priori: P k = E [ ] e k e H k Matriz de correlación del error de estima a posteriori: P k = E [ ] e k e H k Proceso de innovación (o residuo) α k = z k H ˆx k
Aspectos computacionales del filtro Evaluación del estado a posteriori ˆx k = ˆx k + K k ( zk H ˆx k ) = ˆx k + K kα k Ganancia del filtro de Kalman K k (n m) Minimiza la correlación de error a posteriori P k K k = P k HH ( HP k HH + R ) 1 = P k HH HP k HH + R Propiedades lím K k = H 1, R k 0 lím K k = 0 P k 0
Algoritmo: predicción+corrección Inicialización Predicción Corrección.. Ecuaciones de predicción ˆx k = Aˆx k 1 + Bu k P k = AP k 1A H + Q Ecuaciones de corrección K k = P ( k HH HP k HH + R ) 1 ˆx k = ˆx k + K ( k zk H ˆx ) k P k = (I K k H) P k
Características probabilísticas El filtro de Kalman mantiene los dos primeros momentos de la distribución del estado E [x k ] = ˆx k, E [ (x k ˆx k )(x k ˆx k ) H] = P k Si los ruidos del proceso y de la medida son gaussianos p(x k z k ) N(E [x k ], E [ (x k ˆx k )(x k ˆx k ) H] ) = N(ˆx k, P k ) Condiciones de ortogonalidad del proceso de innovación E [ α k z H n E [ α k α H n ] = 0, 1 n k 1 ] = 0, 1 n k 1 {z 1, z 2,, z k } {α 1, α 2,, α k }
Selección de parámetros Parámetros a seleccionar Estimas iniciales ˆx 0 y P 0 : no demasiado críticas Matrices de covarianzas de ruido R y Q R se puede estimar a través de las medidas Q es difícil de estimar (no hay acceso al estado) Habitualmente se realiza un ajuste (tuning) de dichos parámetros Ejemplo: estima de una constante aleatoria Voltaje=-0.5 V Varianza de ruido (blanco y gaussiano): 0.01 50 observaciones
Ejemplo: R = 0.01
Ejemplo: R = 1
Ejemplo: R = 0.0001
El filtro de Kalman extendido (EKF) Las ecuaciones del proceso y/o de medida son no lineales x k = a (x k 1, u k ) + w k 1 z k = h(x k ) + v k, Filtro de Kalman extendido: linealiza el sistema Ecuación de proceso Ecuación de medida A k = a x (ˆx k 1, u k ) H k = h x (ˆx k )
Algoritmo EKF Inicialización Predicción Corrección.. Ecuaciones de predicción ˆx k = a (ˆx k 1, u k ) P k = A kp k 1 A H k + Q Ecuaciones de corrección K k = P k HH k ( Hk P k HH k + R ) 1 ˆx k = ˆx k + K k ( zk h(ˆx k )) P k = (I K k H k ) P k
Formulación alternativa del EKF Las ecuaciones del proceso y/o de medida son no lineales x k = a (x k 1, u k, w k 1 ) z k = h (x k, v k ) Filtro de Kalman extendido: linealiza el sistema Ecuación de proceso A k = a x (ˆx k 1, u k, 0), Ecuación de medida H k = h x (ˆx k, 0), W k = a w (ˆx k 1, u k, 0) V k = h v (ˆx k, 0)
Algoritmo EKF (formulación alternativa) Inicialización Predicción Corrección.. Ecuaciones de predicción ˆx k = a (ˆx k 1, u k, 0) P k = A kp k 1 A H k + W k QW H k Ecuaciones de corrección K k = P k HH k ( Hk P k HH k + V k RV H k ) 1 ˆx k = ˆx k + K k ( zk h(ˆx k, 0)) P k = (I K k H k ) P k
Filtros de partículas Permiten considerar no linealidad y no gaussianidad del sistema dinámico Basados en la teoría Bayesiana y en el uso de muestreo enfatizado (sequential importance sampling) Método secuencial que estima las distribuciones de probabilidad relevantes Filtrado: p(x k z 1:k ) Predicción: p(x k+n z 1:k ) Smoothing: p(x n z 1:k ), con n < k Las distribuciones se aproximan mediante suma de partículas aleatorias (muestras en el espacio de la variable)
Muestreo enfatizado Estima de una distribucion p(x) π(x) Se definen muestras de una densidad enfatizada q(x) x i q(x), i = 1,, N s Aproximación de la densidad p(x) p(x) N s i=1 ω i δ(x x i ), ω i π(xi ) q(x i ) Estima de esperanzas E[g(X)] = g(x)p(x)dx E[g(X)] N s ω i g(x i ) i=1
Filtro de partículas Aplicación del muestreo enfatizado a la estima del estado de un sistema dinámico Estima de p(x 0:k z 1:k ) ω i k p(xi 0:k z 1:k) q(x i 0:k z 1:k) Si se cumple q(x k x 0:k 1, z 1:k ) = q(x k x k 1, z k ) sólo es necesario obtener p(x k z 1:k ) y almacenar x i k ω i k w i k 1 p(z k x i k )p(xi k xi k 1 ) q(x i k xi k 1, z k) p(x k z 1:k ) N s i=1 ω i kδ(x k x i k)
Degeneración Remuestreo Después de algunas iteraciones muchas partículas pueden tener pesos despreciables Solución: remuestreo con la representación discreta de p(x k z 1:k )
Selección de la función de densidad enfatizada Función óptima (minimiza la varianza de los pesos) q(x k x i k 1, z k ) opt = p(x k x i k 1, z k ) ω i k ω i k 1p(z k x i k 1) = ω i k 1 p(z k x k)p(x k x i k 1)dx k Problema: hay que conocer p(x k x i k 1, z k) y la evaluar la integral Función conveniente q(x k x i k 1, z k ) = p(x k x i k 1) ωk i ωk 1p(z i k x i k) Existen múltiples elecciones para la función (es un paso esencial en el diseño del filtro)
Conclusiones Problema: Estima del estado de un sistema dinámico a partir de un conjunto de medidas relacionadas con el mismo Filtro de Kalman Modelo lineal y gaussiano Estima recursiva: Predicción + Corrección Filtro de Kalman extendido Modelo no lineal gaussiano Filtro de partículas Modelo no lineal no gaussiano Muestreo enfatizado y remuestreo