Predizione e regolarizzazione (smoothing) alla Kalman
Transcript
Predizione e regolarizzazione (smoothing) alla Kalman
Predizione e regolarizzazione (smoothing) alla Kalman per gli studenti del corso di Stima e identificazione Luigi Chisci, 22 Maggio 2012 Si consideri il sistema dinamico { xπ‘+1 = Aπ‘ xπ‘ + Bπ‘ uπ‘ + Dπ‘ wπ‘ y π‘ = Cπ‘ x π‘ + v π‘ (0.1) con le solite ipotesi statistiche standard sui disturbi wπ‘ e vπ‘ . Il problema della stima dello β³ stato xπ‘ basata sulle osservazioni yπ = {y1 , y2 , . . . , yπ } prende il nome di β§ se π‘ > π  β¨ predizione filtraggio se π‘ = π  β© regolarizzazione (smoothing) se π‘ < π Il filtro di Kalman propriamente detto determina, in modo ricorsivo, ad ogni istante temporale π‘ sia la stima filtrata xΜπ‘β£π‘ che la stima predittiva ad un passo xΜπ‘+1β£π‘ . Predizione Ci si concentra adesso sul problema della predizione, ovvero della determinazione della stima xΜπ‘β£π e della relativa covarianza Pπ‘β£π per π‘ > π. Esistono, in particolare, tre tipologie di predizione alle quali si può essere interessati: β Predizione a punto fisso: si vuole determinare xΜπ‘β£π e Pπ‘β£π , fissato π‘ e partendo da j<t, per π = π, π + 1, . . . , π‘ β 1; β Predizione ad intervallo fisso: si vuole determinare xΜπ‘β£π e Pπ‘β£π , fissato π, per π‘ = π + 1, π + 2, . . . ; β Predizione ad anticipo fisso: si vuole determinare xΜπ‘+ββ£π‘ e Pπ‘+ββ£π‘ , fissato lβanticipo β > 0, per ogni π‘ > 0. Predizione a punto fisso Definendo la matrice di transizione dello stato β³ Ξ¦π‘,π = Aπ‘β1 β β β Aπ = Ξ¦π‘,π+1 Aπ = Aπ‘β1 Ξ¦π‘β1,π è immediato constatare che xΜπ‘β£π = Ξ¦π‘,π xΜπβ£π + π‘β1 β Ξ¦π‘,π+1 Bπ uπ = Ξ¦π‘,π+1 xΜπ+1β£π + π=π da cui si ha π‘β1 β Ξ¦π‘,π+1 Bπ uπ π=π+1 xΜπ‘β£π+1 = Ξ¦π‘,π+1 xΜπ+1β£π+1 + π‘β1 β Ξ¦π‘,π+1 Bπ uπ π=π+1 ( Per la covarianza si ha: β³ ) π‘β1 β = Ξ¦π‘,π+1 xΜπ+1β£π + Lπ+1 eπ+1 + Ξ¦π‘,π+1 Bπ uπ π=π+1 ) ( = xΜπ‘β£π + Ξ¦π‘,π+1 Lπ+1 yπ+1 β Cπ+1 xΜπ+1β£π [ ] β³ Pπ‘β£π+1 = πΈ xΜπ‘β£π+1 xΜππ‘β£π+1 = Pπ‘β£π β Ξ¦π‘,π+1 Lπ+1 Sπ+1 Lππ+1 Ξ¦ππ‘,π+1 β³ π Sβ1 dove Lπ+1 = Pπ+1β£π πΆπ+1 π+1 è il guadagno di correzione del filtro di Kalman e Sπ+1 = β³ Rπ+1 +Cπ+1 Pπ+1β£π Cππ+1 la matrice di covarianza dellβinnovazione eπ+1 = yπ+1 βCπ+1 xΜπ+1β£π . Riassumendo, si hanno dunque le seguenti ricorsioni del predittore a punto fisso: { ) ( xΜπ‘β£π+1 = xΜπ‘β£π + Ξ¦π‘,π+1 Lπ+1 yπ+1 β Cπ+1 xΜπ+1β£π (0.2) Pπ‘β£π+1 = Pπ‘β£π β Ξ¦π‘,π+1 Lπ+1 Sπ+1 Lππ+1 Ξ¦ππ‘,π+1 Predizione ad intervallo fisso Per la predizione ad intervallo fisso si nota che: { xΜπ‘+1β£π = Aπ‘ xΜπ‘β£π + Bπ‘ uπ‘ Pπ‘+1β£π = Aπ‘ Pπ‘β£π Aππ‘ + Dπ‘ Qπ‘ Dππ‘ (0.3) Predizione ad anticipo fisso Combinando in sequenza il passo di predizione (0.3) ed il passo di correzione (0.2) si β³ ottengono le ricorsioni di aggiornamento della predizione ad anticipo β = π‘ β π fisso: { xΜπ‘+1β£π+1 = Aπ‘ xΜπ‘β£π + Bπ‘ uπ‘ + Ξ¦π‘+1β£π+1 Lπ+1 eπ+1 (0.4) Pπ‘+1β£π+1 = Aπ‘ Pπ‘β£π Aππ‘ + Dπ‘ Qπ‘ Dππ‘ β Ξ¦π‘+1β£π+1 Lπ+1 Sπ+1 Lππ+1 Ξ¦ππ‘+1β£π+1 2 Regolarizzazione (smoothing) Ci si concentra adesso sul problema della regolarizzazione, ovvero della determinazione della stima xΜπ‘β£π e della relativa covarianza Pπ‘β£π per π‘ < π. Esistono, in particolare, tre tipologie di regolarizzazione alle quali si può essere interessati: β Regolarizzazione a punto fisso: si vuole determinare xΜπ‘β£π e Pπ‘β£π , fissato π‘ e partendo da j>t, per π = π, π + 1, . . . ; β Regolarizzazione a ritardo fisso: si vuole determinare xΜπ‘β£π‘+β e Pπ‘β£π‘+β , fissato il ritardo β > 0, per ogni π‘ β₯ 0; β Regolarizzazione ad intervallo fisso: si vuole determinare xΜπ‘β£π e Pπ‘β£π , fissato π > 0, per π‘ = 0, 1, . . . , π. Regolarizzazione a punto fisso Un possibile approccio alla regolarizzazione a punto fisso è il seguente. Si definisce lo stato esteso: ] [ xπ β³ sπ = ππ β³ dove π π = xπ‘ rappresenta lo stato allβistante π‘, obiettivo della regolarizzazione. Poiché [ ]π [ ]π sπ+1 = xππ+1 , π ππ+1 = xππ+1 , xππ‘ , è immediato verificare le sequenti equazioni nello stato esteso: β§ ] ] [ ] [ [ Dπ Bπ Aπ 0  β¨ sπ+1 = wπ = νπ sπ + νπ uπ + ν π wπ uπ + sπ + 0 0 0 I (0.5)  β© y = [C 0] s + v = ν π sπ + v π π π π π Applicando al sistema (0.5) il filtro di Kalman, questβultimo fornisce in uscita la predizione ad un passo dello stato esteso: ] [ [ ] xΜπ+1β£π xΜπ+1β£π = sΜπ+1β£π = xΜπ‘β£π πΛπ+1β£π e la corrispondente matrice di covarianza β³ ν π+1β£π = πΈ sΜπ+1β£π sΜππ+1β£π = [ ] [ Pπ+1β£π Pπ+1β£π π Pπ+1β£π Pπ‘β£π ] dove [ ] [ ] [ ] Pπ+1β£π = πΈ xΜπ+1β£π xΜππ+1β£π , Pπ+1β£π = xΜπ‘β£π xΜππ+1β£π , Pπ‘β£π = xΜπ‘β£π xΜππ‘β£π 3 Pertanto, per la regolarizzazione a punto fisso si può procedere nel seguente modo. Inizialmente si applica il filtro di Kalman al sistema originario, con le misure yπ‘β1 e uπ‘β2 , per determinare xΜπ‘β£π‘β1 e Pπ‘β£π‘β1 . Successivamente si applica il filtro di Kalman al sistema esteso (0.5), a partire dallβistante π‘ con la seguente inizializzazione: ] ] [ [ Pπ‘β£π‘β1 Pπ‘β£π‘β1 xΜπ‘β£π‘β1 , ν π‘β£π‘β1 = sΜπ‘β£π‘β1 = Pπ‘β£π‘β1 Pπ‘β£π‘β1 xΜπ‘β£π‘β1 Definendo il guadagno di Kalman relativo al sistema esteso (0.5) [ ] K ( ) π β1 νπ = νπ ν πβ£πβ1 ν ππ Rπ + ν π ν πβ£πβ1 ν ππ = Kπ e sfruttando la forma particolare delle matrici νπ , νπ , ν π , ν π si ottiene il seguente algoritmo di regolarizzazione a punto fisso. Algoritmo FPKS (Fixed-Point Kalman Smoother) Dati xΜπ‘β£π‘β1 e Pπ‘β£π‘β1 , si procede ciclicamente come segue per π = π‘, π‘ + 1, π‘ + 2, . . . : Sπ = Rπ + Cπ Pπβ£πβ1 Cππ Kπ = Aπ Pπβ£πβ1 Cππ Sβ1 π Kπ = Pπβ£πβ1 Cππ Sβ1 π ) ( xΜπ‘β£π = xΜπ‘β£πβ1 + Kπ yπ β Cπ xΜπβ£πβ1 ) ( xΜπ+1β£π = Aπ xΜπβ£πβ1 + Bπ uπ + Kπ yπ β Cπ xΜπβ£πβ1 Pπ+1β£π = Pπβ£πβ1 (Aπ β Kπ Cπ ) Pπ+1β£π = Aπ Pπβ£πβ1 Aππ + Dπ Qπ Dππ β Kπ Sπ Kππ π Pπ‘β£π = Pπ‘β£πβ1 β Kπ Sπ Kπ Regolarizzazione a ritardo fisso Un possibile approccio alla regolarizzazione a ritardo fisso è il seguente. Si definisce lo stato esteso: β€ β‘ xπ‘ β’ xπ‘β1 β₯ β₯ β’ sπ‘ = β’ β₯ β IRπ(β+2) .. β¦ β£ . xπ‘βββ1 dove β è il ritardo di regolarizzazione (in realtà, come si vedrà in seguito, è il massimo ritardo che interessa considerare). Partendo dal sistema originario, è immediato constatare 4 come lo stato esteso così definito evolva in accordo alle seguenti equazioni di stato ⧠†①†①†① Dπ‘ Bπ‘ Aπ‘ 0 β β β 0    β’ 0 β₯ β’ 0 β₯ β’ I  0 β β β 0 β₯  β₯ β’ β₯ β’ β₯ β¨ sπ‘+1 = β’ u + s + β’ .. .. β₯ π‘ β’ .. β₯ π‘ β’ .. β₯ wπ‘ = νπ‘ sπ‘ + νπ‘ uπ‘ + νwπ‘ β£ . β¦ β£ . β¦ β£ . . ⦠  0 0 0 β β β I 0     β© y = [C 0 β β β 0] s + v = ν π‘ sπ‘ + v π‘ π‘ π‘ π‘ π‘ (0.6) Il predittore di Kalman ad un passo associato al sistema esteso (0.6) assume la forma ) ( (0.7) sΜπ‘+1β£π‘ = νπ‘ sΜπ‘β£π‘β1 + νπ‘ uπ‘ + νπ‘ yπ‘ β ν π‘ sΜπ‘β£π‘β1 dove sΜπ‘+1β£π‘ β‘ β’ β’ β’ =β’ β’ β£ xΜπ‘+1β£π‘ xΜπ‘β£π‘ xΜπ‘β1β£π‘ .. . xΜπ‘βββ£π‘ β‘ β€ β’ β₯ β’ β₯ β’ β₯ β₯ , sΜπ‘β£π‘β1 = β’ β’ β₯ β£ β¦ xΜπ‘β£π‘β1 xΜπ‘β1β£π‘β1 xΜπ‘β2β£π‘β1 .. . xΜπ‘βββ1β£π‘β1 β‘ β€ β’ β₯ β’ β₯ β’ β₯ β₯ , νπ‘ = β’ β’ β₯ β£ β¦ Kπ‘ (1) Kπ‘ (2) Kπ‘ .. . (β+1) Kπ‘ β€ β₯ β₯ β₯ β₯ β₯ β¦ Si noti, in particolare, che i due blocchi superiori di νπ‘ non sono altro che il guadagno del (1) predittore di Kalman ad un passo Kπ‘ ed il guadagno del correttore di Kalman Lπ‘ = Kπ‘ . Sfruttando la forma particolare della matrici di stato νπ‘ , νπ‘ , ν π‘ , ν π‘ in (0.6), (0.7) fornisce le seguenti equazioni ricorsive: β§ xΜπ‘+1β£π‘ = Aπ‘ xΜπ‘β£π‘β1 + Bπ‘ uπ‘ + Kπ‘ eπ‘       xΜπ‘β£π‘ = xΜπ‘β£π‘β1 + Lπ‘ eπ‘     (2)   β¨ xΜπ‘β1β£π‘ = xΜπ‘β1β£π‘β1 + Kπ‘ eπ‘ (0.8) .. ..  . .     (β)   xΜπ‘ββ+1β£π‘ = xΜπ‘ββ+1β£π‘β1 + Kπ‘ eπ‘     (β+1)  β© xΜ e = xΜ +K π‘βββ£π‘ π‘βββ£π‘β1 π‘ π‘ che permettono di aggiornare la stima preditiva xΜπ‘+1β£π‘ , la stima filtrata xΜπ‘β£π‘ e le stime regolarizzate con ritardo π, xΜπ‘βπβ£π‘ , per π = 1, 2, . . . , β. Si noti, in particolare, che (0.8) fornisce tutte le stime regolarizzate fino al ritardo massimo β. Poiché la complessità computazionale per ciclo di elaborazione del filtro di Kalman è π(π3 ), dove π è la dimensione del vettore di stato, il regolarizzatore con ritardo β > 0 risulta avere un carico computazionale, rispetto al filtro, aumentato di un fattore moltiplicativo π(β + 2)3 . 5 Regolarizzazione ad intervallo fisso La regolarizzazione ad intervallo fisso è effettuata off-line supponendo di avere a disposizione le osservazioni {y1 , y2 , . . . , yπ } sullβintervallo temporale di interesse e di volere determinare da queste le stime xΜπ‘β£π per π‘ = 0, 1, . . . , π. Un modo possibile per affrontare questo problema, analogo a quello impiegato per la regolarizzazione a ritardo fissato, è ]π [ quello di introdurre lo stato esteso sπ‘ = xππ‘ , xππ‘β1 , . . . , xππ‘βπ ma, poiché solitamente π è molto grande, questo approccio risulta computazionalmente molto oneroso e difficilmente applicabile in pratica. Un metodo molto più efficiente è quello di procedere in due fasi come segue: β nella prima fase si elaborano in avanti i dati, nellβintervallo ( filtro di) ( [0, π], con) un Kalman memorizzando tutte le stime e relative covarianze xΜπ‘β£π‘β1 , Pπ‘β£π‘β1 e xΜπ‘β£π‘ , Pπ‘β£π‘ per π‘ = 1, 2, . . . , π; β nella seconda fase si rielaborano i dati allβindietro, nello stesso intervallo, utilizzando le stime e covarianze memorizzate nella prima fase. Un algoritmo di stima che opera in questo modo è presentato in [?, pp. 334-338] ed è riportato di seguito. Algoritmo FIKS (Fixed-Interval Kalman Smoother) ) ( 1. Date le condizioni iniziali xΜ(1β£0 , P1β£0 , si determinano )ricorsivamente in avanti, me) ( diante un filtro di Kalman, xΜπ‘β£π‘β1 , Pπ‘β£π‘β1 e xΜπ‘β£π‘ , Pπ‘β£π‘ per π‘ = 1, 2, . . . , π. 2. Per π‘ = π β 1, . . . , 1, 0 si procede allβindietro nel seguente modo: Kπ‘ = Pπ‘β£π‘ Aππ‘ Pβ1 π‘+1β£π‘ ) ( xΜπ‘β£π = xΜπ‘β£π‘ + Kπ‘ xΜπ‘+1β£π β xΜπ‘+1β£π‘ ) π ( Pπ‘β£π = Pπ‘β£π‘ β Kπ‘ Pπ‘+1β£π‘ β Pπ‘+1β£π Kπ‘ 6