Lezione 2 Aprile 19, Il filtro di Kalman: derivazione delle equazioni

Transcript

1 PSC: Progettazione di sistemi di controllo III Trim Lezione 2 Aprile 19, 2007 Docente: Luca Schenato Stesori: Schenato 2.1 Il filtro di Kalman: derivazione delle equazioni Si consideri il modello stocastico lineare tempo invariante: { xk+1 = Ax k + w k y k = Cx k + v k (2.1) dove: v k N (0, R), E[v k v h ] = Rδ(k h) w k N (0, Q), E[w k w h ] = Qδ(k h) x 0 N ( x 0, P 0 ) e v k, w k, x 0 sono v.a. gaussiane 1 a media nulla, incorrelate fra di loro. Il filtro di Kalman è definito come: ˆx k+1 k+1 = E [x k+1 y 0,..., y k+1 ] = E [ x k+1 y k+1, Y (2.2) dove Y k = (y k,..., y 1, y 0 ). Vogliamo trovarne un espressione ricorsiva che ne faciliti il calcolo ad ogni passo. L espressione esplicita di E [X Y ] è facilmente calcolabile se X e Y sono [ due variabili ] ΣXX Σ aleatorie congiuntamente gaussiane e si conoscono le espressioni di µ X, µ Y e XY. Nel nostro caso consideriamo X = x k+1 Y k e Y = y k+1 Y k ; è, quindi, necessario calcolare le espressioni esplicite di: Σ Y X Σ Y Y µ X = E [ x k+1 Y (2.3) µ Y = E [ y k+1 Y (2.4) P k+1 k = Σ XX = V ar [ x k+1 Y (2.5) Σ Y Y = V ar [ y k+1 Y (2.6) Σ XY = Σ T Y X = Cov [ x k+1, y k+1 Y. (2.7) da esse dedurremo l espressione dello stimatore ottimo come: ( ) E [X Y ] = ˆx k+1 k+1 = ˆx k+1 k + Σ XY Σ 1 Y Y yk+1 ŷ k+1 k (2.8) 1 Si ricordi che il simbolo N indica una v.a. gaussiana 2-1

2 e la varianza dell errore di stima come: Σ X Y = P k+1 k+1 = Σ XX Σ XY Σ 1 Y Y Σ Y X (2.9) Calcoliamo in forma esplicita i termini 2.3, 2.4, 2.5, 2.6 e 2.7 ricordando che le medie di prodotti di termini incorrelati sono nulle. E [ x k+1 Y = E [ Ax k + w k Y = AE [ x k Y + E [ w k Y = Aˆx k k = ˆx k+1 k E [ y k+1 Y = E [ Cx k+1 + v k+1 Y = CE [ x k+1 Y + E [ v k+1 Y = C ˆx k+1 k = CAˆx k k V ar [ x k+1 Y [ (xk+1 ) ( ) T = E ˆx k+1 k xk+1 ˆx k+1 k Y = P k+1 k [ (Axk ) ( ) T = E + w k Aˆx k k Axk + w k Aˆx k k Y [ (xk ) ( ) T ˆx k k xk ˆx k k Y A T + AE [( ) x k ˆx k k w Tk Y = AE +E [ ( ) ] T w k xk ˆx k k Y k A T + E [ w k w Tk Y = AP k k A T + Q V ar [ y k+1 Y [ (yk+1 ) ( ) T = E ŷ k+1 k yk+1 ŷ k+1 k Y [ (Cxk+1 ) ( ) T = E + v k+1 C ˆx k+1 k Cxk+1 + v k+1 C ˆx k+1 k Y [ (xk+1 ) ( ) T ˆx k+1 k xk+1 ˆx k+1 k Y C T + CE [( ) x k+1 ˆx k+1 k v Tk+1 Y = CE +E [ ( ) ] T v k+1 xk+1 ˆx k+1 k Y k C T + E [ v k+1 v Tk+1 Y = CP k+1 k C T + R Cov [ x k+1, y k+1 Y [ (xk+1 ) ( ) T = E ˆx k+1 k yk+1 ŷ k+1 k Y [ (Axk ) ( ) T Aˆx k k + w k CAxk CAˆx k k + v k+1 + Cw k Y = E = AE [ (xk ˆx k k ) ( xk ˆx k k ) T Y A T C T + E [ w k w T k Y C T = AP k k A T C T + QC T = P k+1 k C T 2-2

3 con: Si deduce che, definito z = p ( z Y k) N Calcoliamo quindi: [ xk+1 y k+1 ([ ˆxk+1 k C ˆx k+1 k ], si ha: ] [ Pk+1 k P, k+1 k C T CP k+1 k CP k+1 k C T + R ˆx k+1 k = Aˆx k k P k+1 k = AP k k A T + Q p ( x k+1 Y k+1) N (ˆx k+1 k+1, P k+1 k+1 ) Sostituendo i termini calcolati nelle espressioni 2.8 e 2.9 si ottiene per lo stimatore ottimo: che posto: diviene: ˆx k+1 k+1 = ˆx k+1 k + P k+1 k C T ( CP k+1 k C T + R ) 1 ( yk+1 C ˆx k+1 k ) K k+1 k+1 = P k+1 k C T ( CP k+1 k C T + R ) 1 ˆx k+1 k+1 = ˆx k+1 k + K k+1 k+1 ( yk+1 C ˆx k+1 k ) dove K k+1 k+1 è detto guadagno di Kalman. E per quanto riguarda l errore di stima: ]) P k+1 k+1 = P k+1 k P k+1 k C T ( CP k+1 k C T + R ) 1 CPk+1 k Riassumendo le equazioni ricorsive del filtro di Kalman risultano essere: ˆx k+1 k+1 = ˆx k+1 k + K k+1 k+1 ( yk+1 C ˆx k+1 k ) con le condizioni iniziali: (2.10) P k+1 k+1 = P k+1 k P k+1 k C T ( CP k+1 k C T + R ) 1 CPk+1 k (2.11) K k+1 k+1 = P k+1 k C T ( CP k+1 k C T + R ) 1 (2.12) ˆx 0 1 = E [x 0 ] P 0 1 = V ar [x 0 ]. (2.13) Nel caso in cui la varianza dell errore sia semplicemente semidefinita positiva, cioe R 0, le equazioni del filtro di Kalman possono essere generalizzate semplicemente sostituendo l operazione di inversa con quella di pseudoinversa: P k+1 k+1 = P k+1 k P k+1 k C T ( CP k+1 k C T + R ) CPk+1 k (2.14) K k+1 k+1 = P k+1 k C T ( CP k+1 k C T + R ) (2.15) 2-3

4 2.2 Il filtro di Kalman: ricapitolazione Dalle definizioni considerate precedentemente e riscritte qui per convenienza: ˆx k h E[x k y h,..., y 0, x 0 ] P k h E[(x k ˆx k h )(x k ˆx k h ) T y h,..., y 0, x 0 ] (2.16) e considerando che ˆx k h è anch essa una variabile aleatoria Gaussiana, si possono ricavare le seguenti equazioni del filtro di Kalman, che rappresentano un metodo ricorsivo per il calcolo della stima ottima: ˆx k+1 k = Aˆx k k P k+1 k = AP k k A T + Q } predizione ˆx k+1 k+1 = ˆx k+1 k + K k+1 (y k+1 C ˆx k+1 k ) P k+1 k+1 = P k+1 k P k+1 k C T (CP k+1 k C T + R) 1 CP k+1 k K k+1 P k+1 k C T (CP k+1 k C T + R) 1 } guadagno di Kalman ˆx 0 1 = x 0 P 0 1 = P 0 } inizializzazione } aggiornamento Riassumiamo le ipotesi sotto cui sono valide le equazioni del filtro (2.17): - gaussianità di w k e v k (2.17) - bianchezza del rumore: E[w k wh T ] = Qδ(k h), E[v kvh T ] = Rδ(k h) (da cui si ricava che E[(x k ˆx k k )wk T y h,..., y 0 ] = 0 e E[(x k ˆx k k 1 )vk T y h,..., y 0 ] = 0) - w k e v k a media nulla - incorrelazione tra v k e w k, cioe E[w k vh T ] = 0, h, k - R > 0, Q 0 Osservazioni: Si ritiene utile a questo riassumere alcune importanti osservazioni sulle equazioni del filtro di Kalman scritte. 1. Il processo non deve necessariamente essere stazionario e puo essere generalizzato per un sistema tempo variante semplicemente apportando la seguente sostituzione: A A k ; C C k+1 ; R R k+1 ; Q Q k. 2. L ipotesi di incorrelazione tra v k e w k non è necessaria, e le equazioni possono essere modificate in maniera opportuna per tenere conto del fatto che E[v k w T k ] = S 0 (vedi equazioni in [1], pag. 290). 2-4

5 3. La condizione di positivita di R puo essere rilassata a semidefinita-positiva (R 0) semplicemente sostituendo nelle equazioni la pseudoinversa al posto dell inversa. 4. L ipotesi di media nulla non e fondamentale. Infatti, se consideriamo w k = N (µ Q, Q), v k = N (µ R, R) e sufficiente considerare modificare le stesse equazioni del filtro di Kalman dove il sistema dinamico diventa x k+1 = Ax k + µ Q + w k, y k = Cx k + µ R + ṽ k, dove w k = N (0, Q), ṽ k = N (0, R) 5. La stima è lineare nelle misure. Infatti possiamo scrivere l equazione di aggiornamento del filtro come ˆx k+1 k+1 = (I K k+1 C)ˆx k+1 k + K k+1 y k+1 6. E[x k y k,..., y 0 ] = E[x k ˆx k 1 k 1, y k ] cioè tutta l informazione dall istante 0 all istante (k 1) è contenuta nella stima all istante (k 1). 7. Il guadagno ottimo, K = K k, è tempo variante, anche nel caso in cui il sistema sia tempo invariante. 8. una ipotesi che non puo essere rilassata e la bianchezza del segnale. Nel caso in cui w k, v k siano rumori colorati, cioe [v k v T h ] = R h k, [w k w T h ] = Q h k, le precedenti equazioni non possono essere generalizzate. In genere, si cerca di ottenere w k, v k come l uscita di un sistema lineare stabile (filtro passa-basso) che ha come ingresso del rumore bianco w k, ṽ k. Si tratta quindi di identificare una rappresentazione di tale rumore con un sistema del tipo w k+1 = F w k + w k, cioe matrice F e varianza di w k a partire dallo spettro R h k, Q h k. 9. Un altra ipotesi che non puo essere rilassata e la gaussianita dei rumori. Nel caso questi non siano gaussiani le precedenti equazioni non sono corrette. Riguardo a questa ipotesi si vedano anche le considerazioni riguardo il filtro con guadagno costante nella prossima sezione. Viene di seguito riportato un semplice esempio riguardante la stima di posizione e velocità relativo ad un generico veicolo. Esempio Si consideri un generico veicolo in movimento, le cui grandezze di interesse sono ovviamente posizione e velocità. Si suppone di considerare un movimento in due dimensioni, pertanto si avranno due componenti per ciascuna delle grandezze elencate. Si può perciò definire un vettore di stato per il sistema in esame: dove le grandezze indicate sono: x = 2-5 p x p y v x v y

6 - p x : componente della posizione lungo la direzione x; - p y : componente della posizione lungo la direzione y; - v x : componente della velocità lungo la direzione x; - v y : componente della velocità lungo la direzione y. Si vuole effettuare una stima della posizione del veicolo istante per istante, facendo affidamento anche alle misure ottenute per mezzo di un apparecchio radar. Si deve tenere presente che sia le misure sia le stime saranno inevitabilmente affette da incertezza. Fig Stima della posizione e della velocità di un veicolo In figura 2.1 sono riportate le seguenti grandezze: - ˆx k k : stima dello stato all istante k date le misure fino a k; - ˆx k+1 k : predizione dello stato all istante k + 1 date le misure fino a k; - ˆx k+1 k+1 : stima ottima dello stato all istante k + 1 date le misure fino a k + 1; - y k+1 : misura fornita dal radar all istante k + 1. Le ellissi riportate in corrispondenza ad ogni valore indicato rappresentano la relativa incertezza. Si noti che in realtà le ellissi dovrebbero essere considerate in quattro dimensioni, visto che questa è la dimensione del vettore di stato. Alcune considerazioni sulle dimensioni di queste rappresentazioni dell incertezza: - l ellisse relativa a ˆx k k ha dimensione proporzionale a P k k ; 2-6

7 - l ellisse relativa a ˆx k+1 k ha dimensione proporzionale a P k+1 k ; - l ellisse relativa a ˆx k+1 k+1 ha dimensione proporzionale a P k+1 k+1 ; - l ellisse relativa a y k+1 ha dimensione proporzionale a R. Per quanto riguarda la stima ottima, ˆx k+1 k+1, si può dire che essa si trova sicuramente in una posizione intermedia tra la predizione ˆx k+1 k e la misura y k+1, cioè sul segmento indicato in figura 2.1, infatti la sua espressione è la seguente: ˆx k+1 k+1 = ˆx k+1 k + K k+1 (y k+1 C ˆx k+1 k ) La relativa incertezza è, come già detto, un ellisse di dimensione proporzionale a P k+1 k+1 e centrata in ˆx k+1 k+1. Si conoscono pertanto la dimensione ed il centro dell ellisse in questione. Infine, si vuole sottolineare il fatto che la dimensione dell ellisse relativa alla predizione avrà dimensione maggiore rispetto a quella relativa alla stima all istante k viste la dinamica del sistema e l aggiunta di rumore, mentre, per quanto riguarda la dimensione dell ellisse relativa alla stima ottima all istante k + 1, si avrà una diminuzione della stessa, infatti nell equazione di aggiornamento della matrice P k+1 k+1 si può notare la presenza di un segno negativo. Si noti inoltre come l equazioni di aggiornamento della varianza d errore sia una funzione non-lineare della varianza di misura R e della varianza d errore di predizione P k+1 k. 2.3 Filtro a guadagno costante Osservando le equazioni (2.17), si può pensare di sostituire a K k un guadagno costante K ovvero di introdurre un nuovo stimatore non più ottimo ma simile al precedente: Si definisce allora: Come nel caso del filtro di Kalman si avrà: ĝ(y) ˆx k+1 k+1 = Aˆx k k + K k+1 (y k+1 CAˆx k k ) (2.18) g(y) x k+1 k+1 = A x k k + K(y k+1 CA x k k ) (2.19) x k+1 k = A x k k (2.20) P k k = E[(x k x k k )(x k x k k ) T y k,..., y 0, x 0 ] (2.21) P k+1 k = E[(x k+1 x k+1 k )(x k+1 x k+1 k ) T y k,..., y 0, x 0 ] (2.22) Da (2.19) e (2.20) è facile ricavare: x k+1 k = A(A x k 1 k 1 }{{} x k k 1 +K(y k CA x k 1 k 1)) (2.23) = A x k k 1 + AK(y k C x k k 1 ) 2-7

8 da cui si ottiene la seguente espressione per l errore di stima del filtro appena introdotto: ẽ k+1 k x k+1 x k+1 k (2.24) = Ax k + w k (A x k k 1 + AK(Cx k + v k C x k k 1 )) = A(I KC)ẽ k k 1 + w k AKv k Per ipotesi dall incorrelazione dei rumore w k, v k, si deduce che anche ẽ k k 1 è incorrelato sia da w k che da v k, in quanto ẽ k k 1 = f(x 0, w 0,..., w k 1, v 0,..., v k 1 ), cioe dipende solo dai rumori passati. È evidente che la scelta del guadagno K sia un compromesso tra stabilità del sistema (l errore non deve divergere) e riduzione del rumore v k, cioe si vuole che A(I KC) sia piccolo, ma allo stesso tempo non si vuole scegliere K troppo grande per non amplificare troppo il rumore che entra nella dinamica dell errore con il termine AKv k. Dalle (2.22) e (2.24), ricordando l ipotesi di incorrelazione tra v k, w k e ẽ k k 1, si ricava: P k+1 k = E[ẽ k+1 k ẽ T k+1 k y k,..., y 0, x 0 ] (2.25) = E[A(I KC)ẽ k k 1 ẽ T k k 1(I KC) T A T ] + E[w k w T k ] + E[AKv k v T k K T A T ] = A(I KC) P k k 1 (I KC) T A T + Q + AKRK T A T con, al solito: P 0 1 = P 0 (2.26) Il filtro così introdotto non richiede inversioni di matrici e la memorizzazione di P k k 1, tuttavia ciò si paga con un peggioramento delle prestazioni. Infatti non essendo ottimo il filtro con guadagno statico necessariamente dobbiamo avere: P k k 1 P k k 1 k (2.27) Dimostreremo comunque che esiste un opportuno valore del guadagno K = K opt tale che: P k k 1 P P k k 1 P (2.28) P = P cioè a regime le covarianze degli errori di stima dei due filtri coincidono. Ciò significa che, terminato il transitorio iniziale, si deve avere: e quindi i due filtri hanno comportamento identico. x k+1 k ˆx k+1 k (2.29) 2-8

9 Bibliografia [1] Giorgio Picci. Fitraggio Statistico (Wiener, Levinson, Kalman) e Applicazioni. Libreria Progetto,