Práctico 5 Filtro de Kalman

Transcripción

1 Práctico 5 Filtro de Kalman Tratamiento Estadístico de Señales Pablo Musé, Ernesto López & Luís Di Martino {pmuse, elopez, dimartino}@fingeduuy Departamento de Procesamiento de Señales Instituto de Ingeniería Eléctrica Facultad de Ingeniería Curso 25

2 Formulación básica y notación Filtro de Kalman Dado un proceso expresado en un modelo de variables de estado, x k+ = Φ k x k +w k (ecuación del proceso) El proceso se observa a partir de las mediciones y k = C k x k +v k, (ecuación de observación) donde xk vector n de estados del proceso Φk matriz n n de transición de estados Relaciona x k a x k+ en ausencia de una fuerza o entrada exterior wk vector n de ruido blanco Representa el error en el modelo yk vector m de observaciones o medidas Ck matriz m n de observación Relaciona los estados (sin ruido) y las observaciones vk vector m de ruido blanco Modela el error de medición Bibliografía: [Brown and Hwang, 996, Hayes, 996]

3 Formulación básica y notación Filtro de Kalman Se supone además que las matrices de covarianza del ruido del modelo y del ruido de observación son conocidas y cumplen que, E ( w i w T j ) = { Qi k = i i j E ( v i v T j ) = { Ri i = j i j E ( w i vj T ) = i, j El objetivo del filtro de Kalman es encontrar un estimador del estado del sistema ˆx k para cada k dadas las observaciones y k hasta el tiempo k inclusive El estimador obtenido con el filtro de Kalman es óptimo en el sentido de que minimiza el error cuadrático medio del error de estimación del estado para cada k, ξ k = tr(p k ), con P k = E [(x k ˆx k )(x k ˆx k ) T]

4 Algoritmo Filtro de Kalman Condiciones iniciales: estimación a priori del estado y la matriz de covarianza del error de estimación en k =, x P (x = E[ ˆx )( x ˆx ) T ] Para k =,, 2,, se calcula (C k P k C T k +R k ) La ganancia de Kalman: Kk = P k C T k Actualización de la estimación con la observación k-ésima, ˆx k = ˆx ( k +K k yk C kˆx ) k Actualización de la covarianza del error de estimación, P k = (I K k C k )P k Proyecciones del estado y la matriz de covarianza del error del paso k + a partir de los datos actuales, ˆx k+ = Φ kˆx k Proyección del estado P k+ = Φ k P k Φ T k +Q k Proyección de la covarianza

5 Filtro de Kalman Observaciones Si las condiciones iniciales se eligen como [ (x x = E(x ) P = E ˆx )( x ˆx el esitmador ˆx k es insesgado para todo k ) T ], La ganancia de Kalman K k y la matriz de covarianza del error de estimación P k no dependen de las observaciones y k, y por lo tanto pueden ser calculadas previamente antes de realizar la operación de filtrado

6 Ejemplo: estimación de una constante Se quiere estimar el nivel de DC a partir de observaciones contaminadas con ruido blanco gaussiano de potencia σ 2 v no correlacionado El modelo en variables de estado del problema se expresa como x k+ = x k y k = x k +v k Los parámetros del filtro de Kalman son Φ k =, C k =, Q k =, R k = σ 2 v Cálculo de la ganancia de Kalman y la covarianza del error La proyección de la covarianza del error es en este caso, P k+ = Φ kp k Φ T k +Q k P k+ = P k La ganancia de Kalman queda K k = P k CT k ( Ck P k CT k +R k ) K k = P k P k +σ 2 v

7 Ejemplo: estimación de una constante La actualización de la covarianza del error es P k = (I K k C k )P k P k = ( K k )P k ( = P ) k P k +σv 2 P k = P k σ 2 v P k +σ 2 v Teniendo en cuenta la condición inicial de la covarianza del error, la ecuación en diferencias puede resolverse recursivamente, P = P P = P σ 2 v P +σ 2 v P 2 = P σ 2 v P +σ 2 v = P σ 2 v 2P +σ 2 v y generalizando queda P k = P σ 2 v kp +σ 2 v P 3 = P 2σ 2 v P 2 +σ 2 v = P σ 2 v 3P +σ 2 v

8 Ejemplo: estimación de una constante Sustituyendo la covarianza del error en la ganancia de Kalman, se tiene que P k K k = P k +σv 2 P K k = kp +σv 2 Cálculo de la estimación del estado La predicción del estado a partir del estado anterior es ˆx k+ = Φ kˆx k ˆx k+ = ˆx k La actualización de la estimación del estado es ˆx k = ˆx k +K ( k yk C kˆx ) k ˆx k = ˆx k + P kp +σv 2 (y k ˆx k )

9 Observaciones Ejemplo: estimación de una constante P ˆx k = ˆx k + kp +σv 2 (y k ˆx k ) P k = P σ 2 v kp +σ 2 v Si σ 2 v, es decir, las observaciones son completamente erroneas, para todo k se cumple que K k = ˆx k = ˆx k P k = P Las observaciones son ignoradas y el estimador es el estado inicial ˆx k = ˆx con varianza P Si P, significa que no hay conocimiento a priori sobre el proceso x En ese caso se cumple que K k = k P k = σ2 v k ˆx k = ˆx k + k (y k ˆx k ) = k ˆx k + k k y k El estimador es una implementación recursiva de la media muestral En concordancia, la varianza del estimador es P k = σ 2 v/k

10 Ejemplo: estimación de una constante Comparación al variar la matriz de covarianza inicial cuando la estimación inicial del estado es incorrecta x( )=, P( )=, Q=, R=σ 2 v Valor verdadero 2 Observaciones Estimacion muestra x( )=, P( )=, Q=, R=σ 2 v Valor verdadero 2 Observaciones Estimacion muestra

11 Ejemplo: estimación de una constante Comparación al variar la matriz de covarianza inicial cuando la estimación inicial del estado es correcta x( )=5, P( )=, Q=, R=σ 2 v Valor verdadero 2 Observaciones Estimacion muestra x( )=5, P( )=, Q=, R=σ 2 v Valor verdadero 2 Observaciones Estimacion muestra

12 Ejemplo: estimación de una constante Análisis cualitativo El modelo es una buena representación del proceso real: en general, el estado estimado será cercano al valor verdadero, incluso si las observaciones son muy ruidosas No hay conocimiento a priori del estado: se elige el estado inicial arbitrario y matriz covarianza grande En ese caso la estimación estará fuertemente guiada por el modelo y las observaciones Si se conoce a priori la estadística del proceso y la varianza del error de estimación inicial es pequeña, la estimación está guiada principalmente por el modelo y el estado inicial El modelo no representa fielmente el proceso real La matriz de covarianza del ruido del modelo puede ser usada para representar la confianza en el modelo

13 Ejemplo: estimación de una constante El proceso no es constante pero varía lentamente Relajación del modelo mediante la introducción de ruido en el modelo x( )=, P( )=, R=σ 2 v Valor verdadero Observaciones Estimacion, Q= Estimacion, Q=5e muestra

14 Ejemplo: estimación de una constante Error en el modelo La relajación del modelo mediante la introducción de ruido no es suficiente para una buena estimación x( )=, P( )=, R=σ 2 v Valor verdadero Observaciones Estimacion, Q= Estimacion, Q= Estimacion, Q= muestra

15 Ejemplo: estimación de una constante Error en el modelado Si el modelo no es correcto, la varianza del error de estimación puede ser mayor que la varianza del error de las observaciones Esto ocurre si el ruido del modelo es pequeño, porque implica confianza en el modelo Relajar demasiado el modelo tiende a hacer que la estimación no difiera demasiado con las observaciones

16 Ejemplo: estimación de una recta El modelo de una recta se puede expresar como Ecuación del proceso { rk+ = r k +p k Ecuación de observaciones p k+ = p k En notación vectorial queda x k+ = [ ] x k +w k y k = [ ] x k +v k Φ k = [ y k = r k +v k ] [ ] σ 2 Q k = σ2 2 C k = [ ] R k = σ 2 v

17 Ejemplo: estimación de una recta Evolución de los estados en el problema de estimación de una recta Estimacion de una recta x( )=, P( )= xi, R=σ 2 v x [k] Valor verdadero Observaciones Estimacion, Q= muestra x 2 [k] muestra

18 Ejemplo: estimación de parámetros de sinusoide Se quiere estimar la amplitud A y la fase ϕ de una sinusoide a partir de observaciones contaminadas con ruido blanco gaussiano de potencia σ 2 v no correlacionado La frecuencia ω se asume conocida El modelo se puede expresar como Ecuación del proceso Ecuación de observaciones { Ak+ = A k ϕ k+ = ϕ k y k = A k cos(ωk +ϕ k )+v k En este caso el modelo no es lineal Una alternativa es usar un filtro de Kalman extendido

19 Ejemplo: estimación de parámetros de sinusoide Filtro de Kalman Extendido (EKF) Se considera el modelo en variables de estado no lineal de la forma { xk+ = f k (x k,w k ) y k = c k (x k,v k ) El filtro de Kalman extendido se basa en una aproximación de primer orden del modelo, { xk+ = f k (ˆx k,)+f k (x k ˆx k )+G k w k y k = c k (ˆx k,)+c k(x k ˆx k )+U kv k donde F k = f k x (ˆx k,) G k = f k w (ˆx k,) C k = c k x (ˆx k,) U k = c k v (ˆx k,) Se aplica el algoritmo de Kalman sobre el modelo linealizado

20 Ejemplo: estimación de parámetros de sinusoide En el problema de la estimación de los parámetros de una sinusoide solo la ecuación de las observaciones es no lineal Las únicas alteraciones respecto al algoritmo de Kalman básico son La matriz Ck, que cambia en cada iteración, y vale C k = c k x (ˆx k,) [ Ak cos(ωk +ϕ k ) A k cos(ωk +ϕ k ) = A k ϕ k = [ cos(ωk + ˆϕ k )  k sen(ωk + ˆϕ k ) ] ]  k,ˆϕ k La actualización de la estimación con la observación k-ésima, ) ˆx k = ˆx k +K k (y k  k cos(ωk + ˆϕ k )

21 Ejemplo: estimación de parámetros de sinusoide Estimación de la amplitud y la fase de una sinusoide Estimacion de la amplitud y fase de una sinusoide en WGN (SNR = db) 3 Valor verdadero Observaciones Estimacion muestra

22 Ejemplo: estimación de parámetros de sinusoide Estimación de la amplitud y la fase Evolución de los estados 2 5 Estimacion de parametros de sinusoide en WGN (SNR = db) Evolucion de los estados Amplitud Fase 5 Estados muestra

23 Ejemplo: estimación de parámetros de sinusoide Estimación de la amplitud, fase y frecuencia de una sinusoide Estimacion de la amplitud, fase y frecuencia de una sinusoide en WGN (SNR = db) 3 Valor verdadero Observaciones Estimacion muestra

24 Ejemplo: estimación de parámetros de sinusoide Estimación de la amplitud, fase y frecuencia Evolución de los estados 2 5 Estimacion de parametros de sinusoide en WGN (SNR = db) Evolucion de los estados Amplitud Fase Frecuencia 5 Estados muestra

25 Ejemplo: ecualización de canal + Canal Ecualizador s[n] señal a transmitir (IID) v[n] ruido blanco introducido por el canal de media nula y potencia σ 2 v independiente de s[n] Ejemplo El canal se modela como un filtro FIR de N coeficientes s[n] es un proceso Bernoulli que toma el valor con probabilidad p y el valor con probabilidad p µ s = ( p)+p = p E(s 2 [n]) = 2 ( p)+ 2 p = p σ 2 s = E(s 2 [n]) µ 2 s = p( p)

26 Ejemplo: ecualización de canal Filtro de Wiener Matriz de autocorrelación de la entrada La entrada al filtro es u[n] = x[n]+v[n], con x[n] y v[n] procesos independientes Por lo tanto, r u [k] = r x [k]+r v [k] v[n] es ruido blanco de potencia σ 2 v, así que r v [k] = σ 2 vδ[k] Teniendo en cuenta que x[n] es un proceso filtrado, se cumple que, r x [k] = r s [k] h[k] h[ k], Considerando que s[n] tiene media µ s y potencia σs, 2 la función de autocorrelación es { E(s r s [k] = E(s[n]s[n k]) = 2 [n]) = σs 2 +µ 2 s k = E(s[n])E(s[n k]) = µ 2 s k r s [k] = σ 2 sδ[k]+µ 2 s

27 Ejemplo: ecualización de canal Por lo tanto, la autocorrelación de la señal en la salida del canal es r x [k] = (σ 2 sδ[k]+µ 2 s) h[k] h[ k] = σsδ[k] h[k] h[ k]+µ 2 2 s h[k] h[ k] ( N ) 2 = σs(h[k] h[ k])+µ 2 2 s h[k] Finalmente, la autocorrelación de la entrada del filtro Wiener es r u [k] = σ 2 s(h[k] h[ k])+µ 2 s i= ( N i= ) 2 h[k] +σvδ[k] 2

28 Ejemplo: ecualización de canal 2 Correlación cruzada entre la entrada y la señal deseada Se vio previamente (transparencias del práctico de Filtros Adaptivos, pag 47), que la correlación cruzada es y en este caso queda, p[ k] = p[ k] = N l= N l= h[l]r s [ l k +L], h[l](σ 2 sδ[ l k +L]+µ 2 s) = σ 2 sh[ k +L]+µ 2 s N l= h[l] Construyendo el sistema de ecuaciones de Wiener-Hopf M M y resolviendo, se obtienen los coeficientes del filtro de Wiener de M coeficientes

29 Ejemplo: ecualización de canal Filtro de Kalman [Lawrence and Kaufman, 97] Se asume conocida la respuesta al impulso del canal h n, con n =,,N Planteo del problema Los estados w,,w N son la entrada en la ĺınea de retardos del filtro, s[n],,s[n N +] w [k +] w 2[k +] w 3[k +] w N[k +] = w [k] w 2[k] w 3[k] w N[k] + }{{} G s[k+]

30 Ejemplo: ecualización de canal La ecuación de observación es u[k] = (h h h N ) w [k] w N [k] +v[k] Los parámetros del filtro de Kalman son Φ k = G = C k = (h h h N ) Q k = σ 2 s R k = σ 2 v Como estimador de la señal transmitida s[n] se usa el estado i-ésimo, w i = ŝ[n i] Se decide que cada muestra de la señal ecualizada corresponde a un si el estimador supera 5 y en el caso contrario

31 Algoritmo Ejemplo: ecualización de canal Condiciones iniciales: estimación a priori del estado y la matriz de covarianza del error de estimación en k =, x = P = I Para k =,, 2,, se calcula (C k P k C T k +R k ) La ganancia de Kalman: Kk = P k C T k Actualización de la estimación con la observación k-ésima, ˆx k = ˆx ( k +K k yk C kˆx ) k Actualización de la covarianza del error de estimación, P k = (I K k C k )P k Proyecciones del estado y la matriz de covarianza del error del paso k + a partir de los datos actuales, ˆx k+ = Φ kˆx k +Gµ s Proyección del estado P k+ = Φ k P k Φ T k +GQ k G T Proyección de la covarianza

32 Ejemplo: ecualización de canal Simulación La señal a transmitir es binomial con p = 5 La potencia es σ 2 s = p( p) = 25 El canal se modela como un filtro FIR de N = 6 coeficientes con respuesta al impulso h[n] = [ 77, 355, 59,, 59, 273] El ruido aditivo del canal es blanco, gaussiano, de media nula y potencia σ 2 v = (SNR 4 db) Se construye un ecualizador usando un filtro de Wiener de M = 9 coeficientes Se construye un ecualizador usando un filtro de Kalman usando w 6 como estimación de la señal transmitida Se compara el desempeño a través del porcentaje de bits erroneos

33 Ejemplo: ecualización de canal Características del filtro de Wiener 5 5 Respuesta al impulso del canal muestra 3 2 Respuesta al impulso del filtro de Wiener muestra Respuesta en frecuencia Respuesta al impulso del sistema muestra Canal Wiener Sistema frecuencia

34 Ejemplo: ecualización de canal Realización de señal transmitida, señal distorsionada por el canal y señal recibida en el receptor s[n] x[n] u[n]=x[n]+v[n] muestra

35 Ejemplo: ecualización de canal Señales ecualizadas usando el filtro de Wiener y el filtro de Kalman 5 s[n] Senal ecualizada con el filtro de Wiener alineada con s[n] Senal ecualizada con el filtro de Kalman alineada con s[n] muestra

36 Ejemplo: ecualización de canal Señales reconstruidas mediante el umbralizado con ambos ecualizadores s[n] s[n] reconstruida usando ecualizador con Wiener Numero de errores: s[n] reconstruida usando ecualizador con Kalman Numero de errores: muestra

37 Ejemplo: ecualización de canal Comparación de los ecualizadores al cambiar los parámetros Porcentaje de error al variar el orden del filtro Wiener SNR = 4 db Porcentaje de error al variar la SNR (σ v 2 ) Filtro Wiener con M=3 3 Wiener Kalman 3 Wiener Kalman % de error 2 5 % de error Numero de coeficientes (M) SNR (db)

38 Ejemplo: ecualización de canal Observaciones Dado que se conoce la respuesta al impulso del canal, el problema consiste en encontrar la filtro inverso al canal (en el caso con σ 2 v = ) h[n] es un filtro FIR con función de transferencia y el filtro inverso es H(z) = h +h z + +h N z (N ) H inv (z) = h +h z + +h N z (N ) (puede ser inestable) El ecualizador Wiener es un filtro FIR, mientras que el ecualizador Kalman es IIR Esto explica porque el desempeño del ecualizador Wiener es peor que el ecualizador Kalman en ciertas condiciones

39 + + + Ejemplo: ecualización de canal Modelo de canal genérico El canal es un filtro con función de transferencia racional arbitraria, + - H(z) = b +bz + +b qz q +a z + +a pz p + La salida del filtro se caracteriza por las siguientes ecuaciones en recurrencia, w [k +] = a w [k] a 2w [k ] a pw [n p+]+s[k +] + u[k] = b w [k]+b w [k ] b qw [n q]+v[k] +

40 Ejemplo: ecualización de canal Modelo de canal genérico La ecuación del proceso es w [k +] w 2 [k +] w 3 [k +] w p[k +] = a a 2 a 3 a p w [k] w 2 [k] w 3 [k] w p[k] + s[k+] La ecuación de observación es u[k] = (b b b q ) w [k] w q+ [k] w p[k] +v[k]

41 Ejemplo: ecualización de canal Coeficientes del canal desconocidos Se asume ahora que los coeficientes del filtro son desconocidos Se expande el vector de estados incluyendo los coeficientes del filtro como estados, x[k] = (w [k]w N [k] h [k] h N [k]) T La ecuación de evolución de estados es ahora x[k+] = Φ[k]x[k]+Gs[k+], Φ[k] = ( Φ[k] I con Φ[k] de tamaño 2N 2N y G de tamaño 2N La ecuación de observación es y[k] = c[k](x[k])+v[k], con c[k](x[k]) = ), G = N x j [k]x j+n [k] j=

42 Ejemplo: ecualización de canal Coeficientes del canal desconocidos Como la ecuación de observación no es lineal, hay que usar un filtro de Kalman Extendido En el algoritmo de Kalman, la matriz C[k] queda C[k] = c[k] x (ˆx [k],) = (x N+ [k]x 2N [k] x [k]x N [k]) x[k]=ˆx [k] La actualización de la estimación con la observación k-ésima es N ˆx[k] = ˆx [k]+k[k] y[k] x j [k]x j+n [k]) j=

43 Ejemplo: identificación y ecualización de canal Evolución de los estados 5 Evolucion de los estados correspondientes a los coeficientes del filtro iteracion

44 Ejemplo: identificación y ecualización de canal Señal transmitida, señal ecualizada y señal reconstruida s[n] Senal ecualizada con el filtro de Kalman alineada con s[n] s[n] reconstruida usando ecualizador con Kalman Numero de errores: muestra

45 Ejercicio [Zhong and Sclaroff, 23] Se considera el modelo en variables de estado { xk+ = Ax k +v k v k N(,Q) y k = Hx k +w k w k N(,R) Las ecuaciones del filtro de Kalman pueden deducirse empleando el estimador MAP del estado x(k): el estimador del estado actual ˆx k es aquel que maximiza la probabilidad del estado x k dada la observación y k y la secuencia de estados previos x k, x k 2,, x, ˆx k = argmax x k p(x k y k,x k,,x )

46 Ejercicio Empleando la regla de Bayes, escribir la probabilidad a posteriori de x k en función de la densidad de probabilidad de transición de estados p(x k x k ) y la observación p(y k x k ) 2 Demostrar que maximizar la probabilidad a posteriori equivale a minimizar la función de costo, J(x k ) = (y k Hx k ) T R (y k Hx k )+(x k x k )T Q (x k x k ) x k es la predicción de x k, x k = Ax k 3 Demostrar mediante la minimización de la función de costo que la ecuación de actualización del estimador óptimo puede expresarse como ˆx k = ˆx k +K(y k Hˆx k ) Indicar el valor de la matriz K

47 Referencias I Brown, R G and Hwang, P Y C (996) Introduction to Random Signals and Applied Kalman Filtering, chapter 5 Wiley, st edition Hayes, M H (996) Statistical Digital Signal Processing and Modeling, chapter 7 Wiley, st edition Lawrence, R and Kaufman, H (97) The Kalman Filter for the Equalization of a Digital Communications Channel IEEE Transactions on Communication Technology, 9(6):37 4 Zhong, J and Sclaroff, S (23) Segmenting foreground objects from a dynamic textured background via a robust Kalman filter In Proceedings of the Ninth IEEE International Conference on Computer Vision, pages 44 5