Modelos de estado no-lineales: EKF y UKF Tratamiento Estadístico de Señales Pablo Musé, Ernesto López, Luis Di Martino {pmuse,elopez,dimartino}@fing.edu.uy Departamento de Procesamiento de Señales Instituto de Ingeniería Eléctrica Facultad de Ingeniería Junio de 2015 Biblio: I Anderson and Moore, Optimal Filtering, 1979 (re-edición Dover 2005). I Julier, S.J.; Uhlmann, J.K., Unscented filtering and nonlinear estimation, Proceedings of the IEEE , vol.92, no.3, pp.401,422, Mar 2004. Introducción - Motivación Consideremos los modelos de estado en sus versiones general y lineal: xk+1 = fk+1 (xk , vk+1 ) xk+1 = Fk+1 xk + Gk+1 vk+1 yk = hk (xk , nk ) yk = Hk xk + Uk nk Hasta ahora hemos visto sobre el filtro de Kalman, que: I I I Si los ruidos no son necesariamente Gaussianos, el filtro de Kalman es óptimo entre todos los filtros lineales, en el sentido del MSE. Si los ruidos son Gaussianos, el filtro de Kalman es universalmente óptimo (i.e. aún dentro del conjunto de los filtros no-lineales) en el sentido del MSE. Todo proceso ARMA se puede representar como un filtro de Kalman, pasando a una representación en variables de estado. Hoy nos concentraremos en dos técnicas para modelos no-lineales: 1. Extended Kalman Filter (EKF): básicamente una linealización del modelo y una aplicación del Kalman clásico. 2. Unscented Kalman Filter (UKF): básicamente, mapear por las transformaciones no-lineales un conjunto de puntos bien elegidos, y a partir de ellos calcular nuevas medias y matrices de covarianzas. El filtro de Kalman Extendido (EKF) La linealización Consideramos ahora un modelo en el espacio de estados no-lineal, de la forma: xk+1 = fk+1 (xk , vk+1 ) yk = hk (xk , nk ). El filtro de Kalman extendido se basa en una aproximación de primer orden del modelo, seguido del loop de Kalman con ecuaciones lineales. I La ecuación de estado se linealiza entorno a (x, v) = (xk|k , 0). I La ecuación de medida se linealiza entorno a (x, n) = (xk|k−1 , 0). xk+1 yk = fk+1 (xk|k , 0) + Fk+1 (xk+1 − xk|k ) + Gk+1 vk+1 = hk (xk|k−1 , 0) + Hk (xk − xk|k−1 ) + Uk nk , dónde ∂fk+1 (xk|k , 0), ∂x ∂hk Hk = (xk|k−1 , 0), ∂x Fk+1 = ∂fk+1 (xk|k , 0), ∂v ∂hk Uk = (xk|k−1 , 0). ∂n Gk+1 = El filtro de Kalman Extendido (EKF) Las iteraciones de Kalman Las ecuaciones para el EKF quedan entonces: I Inicialización: x0 , P0 . I Para k = 0, 1, 2, . . . , calcular: x̂k|k−1 = fk (x̂k−1|k−1 , 0) Pk|k−1 = Fk Pk−1|k−1 FTk + Gk Qk GTk Kk = Pk|k−1 HTk (Hk Pk|k−1 HTk + Uk Rk UTk )−1 x̂k|k = x̂k|k−1 + Kk (yk − hk (x̂k|k−1 , 0)) Pk|k = (I − Kk Hk )Pk|k−1 Observaciones y comentarios: I El problema del filtro EKF es que la linealización del modelo introduce un sesgo en x̂k|k y una covarianza errónea. I La calidad de la aproximación debida a la linealización será mejor cuanto menor sean kx − x̂k|k k2 y kx − x̂k|k−1 k2 . I Por ende, es de esperar que el EKF se comporte mejor en situaciones de alto SNR. Ver [Anderson & Moore] para una discusión más profunda sobre el tema. El Unscented Kalman Filter (UKF) Julier & Uhlmann 1996 Este método busca dejar de lado las aproximaciones y trabajar directamente sobre las transformaciones no lineales, mapeando un conjunto de puntos bien elegidos. La transformación unscented I Sea x ∼ N (x̄, Px ) un vector aleatorio de Rn , g : Rn → Rm una función, e y = g(x) ∈ Rm . I La unscented transform busca calcular estimadores insesgados de ȳ y de Py . I En lugar de linealizar g como en el EKF, la propuesta es evaluar ȳ y Py (1er y 2do momento de y) mediante estimaciones empíricas ponderadas, calculadas a partir de la transformación exacta g de un conjunto de puntos bien elegidos alrededor de x̄, denominados σ-points. UKF: Los σ-points Tenemos x ∈ Rn , x ∼ N (x̄, Px ). Los 2n + 1 σ-points y sus pesos correspondientes se definen como: κ κ+n 1 Wi = , i = 1, . . . , n 2(κ + n) 1 Wi = , i = n + 1, . . . , 2n. 2(κ + n) X0 = x̄ W0 = Xi = x̄ + ( p (κ + n)Px )i p Xi = x̄ − ( (κ + n)Px )i−n √ donde κ es un escalar y ( Px )i representa a la i-ésima columna de alguna factorización de Px , e.g. Cholesky, i.e. Px = p Px × p T Px = n p X p ( Px )i ( Px )Ti . i=1 En lo que sigue veremos porqué Julier & Uhlmann (1996) proponen esta elección. Justificación de la elección de los σ-points A partir de los σ-points y sus transformados, se puede calcular estadísticos empíricos de x y de y. Sea Yi = g(Xi ), definimos = P2n Wi X i PX = P2n Wi (Xi − X̄ )(Xi − X̄ )T Ȳ = P2n Wi Y i = P2n Wi (Yi − Ȳ)(Yi − Ȳ)T . X̄ PY i=0 i=0 i=0 i=0 Ejercicio: demostrar que la media y covarianza empíricas de X = {Xi }2n i=0 coinciden con las de x. Claramente, X̄ = x̄. √ √ Pn 1 Luego, PX = 2(n+κ) × 2 i=1 (n + κ)( Px )i ( Px )Ti = Px . Justificación de la elección de los σ-points (cont.) Resta ver qué pasa con la media y covarianza empíricas de los {Yi }2n+1 i=1 en relación a y. Suponemos que g = [g1 , g2 , . . . , gm ]T , g : Rn → Rm , admite un desarrollo de Taylor entorno a x̄. Entonces tenemos 1 g(x̄ + δ) = g(x̄) + Dδ g(x̄) + Dδ2 g(x̄) + . . . , 2 dónde Dδ g(x̄) = ∇T g(x̄)δ Dδ2 g(x̄) = Dδ [∇g(x̄)δ] = ∇T [∇T g(x̄)δ]δ = [Tr(∇2 g1 (x̄)δδ T ), . . . , Tr(∇2 gm (x̄)δδ T )]T , con ∇g la matriz Jacobiana de g, y ∇2 g la matriz Hessiana de g. Justificación de la elección de los σ-points (cont.) Luego, ȳ = E[g(x)] = E[g(x̄ + δ)] = g(x̄) + E[∇T g(x̄)δ] + 1 E[[Tr(∇2 g1 (x̄)δδ T ), . . . , Tr(∇2 gm (x̄)δδ T )]T ] + . . . 2 Como x = x̄ + δ, tenemos δ ∼ N (0, Px ), por lo que 1 [Tr(∇2 g1 (x̄)Px ), . . . , Tr(∇2 gm (x̄)Px )]T + . . . 2 p Por otra parte, considerando σ i = ( (κ + n)Px )i , ȳ = g(x̄) + Ȳ = W0 g(x̄) + n X Wi (g(x̄ + σ i ) + g(x̄ − σ i )) i=1 = g(x̄) + 0 + = g(x̄) + 2n 2n X X σi σT σi σT 1 i i E[[Tr(∇2 g1 (x̄) , . . . , Tr(∇2 gm (x̄) )]T ] + . . . 2 2(κ + n) 2(κ + n) i=1 i=1 1 [Tr(∇2 g1 (x̄)Px ), . . . , Tr(∇2 gm (x̄)Px )]T + . . . 2 En ambos casos, debido a la simetría de los σ-points entorno a x̄, los términos impares se anulan, por los que la media empírica de los {Yi }2n i=0 coincide con ȳ hasta el tercer orden. Justificación de la elección de los σ-points (cont.) Ahora vemos qué pasa con Py y PY . Py = E[(g(x̄ + δ) − g(x̄))(g(x̄ + δ) − g(x̄))T ] = E[(∇T g(x̄)δ)(∇T g(x̄)δ)T ] + . . . = ∇T g(x̄)Px ∇g(x̄) + . . . Por otro lado, P2n PY = i=0 Wi (Yi − Ȳ)(Yi − Ȳ)T Pn 1 = 2(n+κ) × 2 i=1 (∇T g(x̄)σ i + . . . )(∇T g(x̄)σ i + . . . )T = ∇T g(x̄)Px ∇g(x̄) + . . . Por los que ambas coinciden en el primer orden. Ajustando κ para que n + κ = 3, se puede mostrar que las medias pueden coincidir hasta orden 4. Sin embargo, la covarianza empírica ponderada puede volverse semidefinida negativa. Este problema fue resuelto por Julier en 2002, introduciendo la scaled unscented transformation. Scaled Unscented Transform I La idea es escalar la distancia entre los σ-points remplazando Xi por Xi0 = X0 + α(Xi − X0 ), con α el factor de escala. I En este caso W0 debe ser diferente. Llamando λ = α2 (κ + n) + n, la scaled unscented transform se resume así: X0 = x̄ λ , λ+n (m) = (m) =Wi (m) =Wi W0 Xi = x̄ + ( p (λ + n)Px )i Wi Xi = x̄ − ( p (λ + n)Px )i−n Wi (c) (c) (c) W0 = λ + (1 − α2 + β) λ+n 1 , i = 1, . . . , n 2(κ + n) 1 = , i = n + 1, . . . , 2n, 2(κ + n) = donde 0 ≤ α ≤ 1, κ puede ser seteado a 0, y β = 2 es el valor óptimo en el caso Gaussiano. De esta forma, las medias y covarianzas empíricas de x e y quedan: P2n (m) X̄ = i=0 Wi Xi P2n (c) PX = i=0 Wi (Xi − X̄ )(Xi − X̄ )T P2n (m) Ȳ = i=0 Wi Yi P2n (c) PY = i=0 Wi (Yi − Ȳ)(Yi − Ȳ)T Unscented Kalman Filter (UKF) El UKF se obtiene juntando las formulas del filtro de Kalman con las de la Unscented Transform. Considerando nuevamente el modelo no-lineal xk+1 = fk+1 (xk , vk+1 ) yk = hk (xk , nk ), y usando una representación de estado aumentada xak = [xTk vkT nTk ]T con su correspondiente matriz de covarianza, el UKF se resume como: 1. Inicialización: x0|0 , P0|0 2. Iteraciones: para k = 1, 2, . . . , a P = Pk−1|k−1 0 0 0 Qk 0 0 0 Rk x Xk|k−1 x Xk−1|k−1 h i p a v a = = xa | x ± (n + λ)P Xk k−1|k−1 k−1|k−1 Xkn x = fk (Xk−1|k−1 , Xkv ) xk|k−1 = P2n Wi Px k|k−1 = P2n Wi Yk|k−1 x = h(Xk|k−1 , Xkn ) yk|k−1 = P2n Py k|k−1 = P2n Pxy k|k−1 = P2n Kk = Pxy (Py )−1 k|k−1 k|k−1 xk|k = xk|k−1 + Kk (yk − yk|k−1 ) Pk|k = Pk|k−1 − Kk Py KT k. k|k−1 Xa i=0 i=0 i=0 (m) (c) x x (Xk|k−1,i − xk|k−1 )(Xk|k−1,i − xk|k−1 )T (m) Wi x Xk|k−1,i Yk|k−1,i (c) (Yk|k−1,i i=0 Wi i=0 (c) Wi − yk|k−1 )(Yk|k−1,i − yk|k−1 )T x (Xk|k−1,i − xk|k−1 )(Yk|k−1,i − yk|k−1 )T