Navigazione geodetica o navigazione senza filtro di kalman

Transcript

1 Navigazione geodetica o navigazione senza filtro di kalman Alberta ALBERTELLA Barbara BETTI Fernando SANSÒ Vincenza TRNATRE DIIAR Sez. Rilevamento Politecnico di Milano p.zza L. Da Vinci, Milano t f e vincenza.tornatore@polimi.it Parole chiave > Filtro di Kalman, Minimi quadrati, Navigazione, soluzione geodetica Riassunto > Da sempre si è ritenuto il filtro di Kalman un ottima alternativa alle soluzioni minimi quadrati anche per stime richieste non in tempo reale: ciò è principalmente dovuto all insostenibile carico numerico che la soluzione minimi quadrati necessariamente implica. In questo lavoro dimostriamo che con semplici accorgimenti è possibile raggiungere soluzioni minimi quadrati equivalenti sia in termini di valori stimati che in termini di carico numerico richiesto. Abstract > Kalman filter is considered an efficient and valuable method alternative to the classic least squares algorithm even for no real time estimates, due to the huge computational load that least squares require. In this work we demonstrate how, by exploiting some simple expedients, we can obtain least squares solutions equivalent to Kalman solutions with comparable computational load. 1. Introduzione I problemi di navigazione richiedono sempre una soluzione in tempo reale poiché devono dare risposte precise e immediate sulla base delle osservazioni disponibili fino all istante corrente di misura. Per questo motivo normalmente la soluzione è ottenuta applicando il filtro di Kalman. D altra parte la soluzione geodetica, che è la soluzione ottenuta in post-elaborazione, richiede l applicazione di un algoritmo batch che usi tutte le osservazioni disponibili al termine del procedimento di misura. Il trattamento dei dati di campagne di rilievo cinematico, anche in post-elaborazione, è di solito effettuato con algoritmi ricorsivi, cioè Kalman filtering e Kalman smoothing. Questa scelta è la conseguenza del fatto che la soluzione, ottenuta combinando il vettore delle osservazioni avanti e indietro, fornisce una stima ottimale (cioè la soluzione che soddisfa il principio di Wiener-Kolmogorov). La forza di que- 117

2 Alberta ALBERTELLA, Barbara BETTI, Fernando SANSÒ, Vincenza TRNATRE sto approccio consiste nel fatto che esso richiede un basso carico numerico. Un altra possibilità di soluzione batch consiste nell applicare il classico algoritmo dei minimi quadrati, ma in questo caso, il guadagno della conoscenza della matrice di covarianza della soluzione è in conflitto con il grosso carico numerico richiesto. In questo lavoro noi dimostreremo che è possibile ottenere una stima minimi quadrati ottimale con uno sforzo numerico comparabile con quello del filtro di Kalman. Questo obbiettivo viene raggiunto applicando una proprietà algebrica delle matrici, che permette di scrivere la matrice normale come una matrice a blocchi disposti in banda che chiameremo d ora in poi matrice in banda a blocchi. Il secondo passo consiste nel determinare la soluzione ottimale applicando l algoritmo di Cholesky per la soluzione di sistemi triangolari. 2. Navigazione in tempo reale: la soluzione con il filtro di Kalman Consideriamo un sistema lineare a tempi discreti e supponiamo che questo sistema sia descritto da un vettore di stato finito che evolve con una dinamica nota: X t= = X X t +1 = T t +1 X t +ν t +1 C ν t ν t ' =δ tt' C t ν, C x x = C, C x ν t = (1) X t= = stato iniziale T = matrice di transizione (dinamica) H = matrice disegno ν = rumore di sistema, relativo alla dinamica C ν = matrice di covarianza del rumore di sistema C = matrice di covarianza dell epoca iniziale, con correlazione nulla ad ogni istante. Introduciamo inoltre il vettore delle osservazioni Y t +1 = H t +1 X t +1 +ε t +1, E[ε t ] = C ε t ε t ' =δ tt' C ε t, C ν t ε t ' =, C x ε t = (2) dove: Y = vettore delle misure ε = errore di misura C ε = matrice di covarianza dell errore di misura Bollettino SIFET 1/25

3 Navigazione geodetica o navigazione senza filtro di kalman ra il problema è quello di ottenere la stima lineare ottimale dello stato X t considerando tutte le informazioni note fino al tempo t stesso, cioè Y 1, Y 2.. Y t. Il filtro di Kalman permette di ottenere la soluzione in tempo reale, cioè la stima ˆX t t, per mezzo di uno schema iterativo (figura 1), dove K t +1 rappresenta la matrice di covarianza, K t +1 = C ξ t+1 ξ t+1, del vettore ξ t+1 =X t+1 X t+1 t, mentre C etet rappresenta la matrice di covarianza del vettore dell errore di predizione e t = X t ˆX t t. In questo schema l aggiornamento è ottenuto calcolando ad ogni passo la matrice di covarianza K t +1, che ovviamente deve essere simmetrica e definita positiva. Figura 1: diagramma a blocchi del Kalman filtering La forza del filtro di Kalman è che dal punto di vista numerico la soluzione si ottiene con l inversione di una matrice, H t K t H + t + C ε t, con dimensione uguale al numero di osservazioni all istante t. In ogni caso occorre prestare attenzione ai problemi di stabilità della soluzione su lunghi periodi di tempo: può infatti accadere che la covarianza K t +1 non sia più simmetrica e strettamente definita positiva. Per ovviare a questo inconveniente, di solito viene implementato un algoritmo chiamato root square (cfr. Mayback) 3. Navigazione batch: Kalman filtering più Kalman smoothing e soluzione minimi quadrati Al termine del periodo di osservazione l informazione disponibile è rappresentata da tutte le osservazioni Y 1, Y 2.. Y t. Così, se rinunciamo alla richiesta di una stima in tempo reale, una volta che tutte le osservazioni sono disponibili, possiamo cercare la migliore stima lineare di X t : questa è la stima batch. Il nostro problema consiste nel trovare ˆX t = X t Y 1, 2,...T e, chiamato e t l errore di stima, trovare la sua matrice di covarianza Cêê = E{ ê t ê t }. Questa + operazione è nota come Kalman smoothing e può essere effettuata 119

4 Alberta ALBERTELLA, Barbara BETTI, Fernando SANSÒ, Vincenza TRNATRE applicando una procedura sequenziale a partire dalle ultime stime del filtro scorrendo i dati all indietro. La procedura del Kalman smoothing è riassunta nello schema in figura 2. Figura 2: schema ricorsivo del Kalman smoothing In questo schema appare chiaro che trovare una soluzione batch richiede uno sforzo numerico molto basso: infatti, una volta che siano state memorizzate in maniera opportuna le quantità stimate nella soluzione in avanti (forward) si devono solo calcolare prodotti di matrici. D altra parte, per raggiungere questo obbiettivo spesso sono state accettate ipotesi errate. In particolare qualche volta la soluzione di Kalman in avanti viene combinata con una soluzione ottenuta considerando solo osservazioni future rispetto all istante t: X = (X t Y t +1...Y T ) ; in t questo caso la stima finale è semplicemente la media pesata delle soluzioni avanti e indietro : 1 X t = (C et e t con matrice di covarianza 1 C = (C et e t + C 1 t X ) 1 (C 1 et e t X t t + C 1 t X X t ) + C t X 1 ) 1 (3) (4) Tuttavia questa procedura non è rigorosa: per esempio, non tiene conto del fatto che Y 1,t e Y t+1,t non sono linearmente indipendenti. Per questa ragione la matrice di covarianza delle due stime non può essere semplicemente ottenuta come in (4). Questa scelta è una conseguenza del fatto che a una implementazione semplice e a basso costo di questa procedura non corrisponde una teoria semplice e altrettanto a basso costo. Per questa ragione abbiamo studiato se esiste una strategia più semplice dal punto di vista metodologico, che fornisca risultati equivalenti. La risposta si trova nel classico approccio dei minimi quadrati. Questo metodo non usa una procedura ricorsiva, ma considera tutte le osservazioni Y 1.T in un unico passo, per stimare tutti i parametri di stato X 1,..T. Noi vogliamo dimostrare che seguendo questo approccio è possibile ottenere una soluzione rigorosa con un carico numerico comparabile a quello del Kalman filtering seguito dal Kalman smoothing. Bollettino SIFET 1/25

5 Navigazione geodetica o navigazione senza filtro di kalman Partiamo considerando ancora le relazioni per X t e Y t date in (1) e (2). Introduciamo la seguente notazione: x = X 1 Y 1 M, y = M X T ν 1 Y T ν T, v = M, ε= ε 1 M ε T D = I T 2 I T 3 I T T I, M = H 2 H T (5) C V = C 1 v C T v, C ε = C 1 ε C T ε e introduciamo anche ω= ν 1 + T 1 X ν 2 M (6) ν T con matrice di covarianza C ν T 1 C T 1 C ω = C 2 ν C T ν (7) In questo modo possiamo scrivere (4) come Dx =ω Mx +ε=y C ω ec ε C ωε = (8) (9) (1) che considerando x = D 1 ω possiamo anche scrivere come MD 1 ω+ε=y (11) (12) 121

6 Così, alla fine, il problema diventa trovare la stima Alberta ALBERTELLA, Barbara BETTI, Fernando SANSÒ, Vincenza TRNATRE ˆω =(ω Y) = C ωy C 1 YY Y e la covarianza dell errore di stima C δδ = C ωω C ωy C 1 YY C Yω La soluzione è rappresentata da (13) (14) ˆω =C ω (D + ) 1 M + (MD 1 C ω (D + ) 1 M + + C ε ) 1 Y (15) dove il carico numerico è principalmente dovuto all inversione della matrice MD -1 C ω (D + ) -1 M + +C ε, che è una matrice piena con dimensione uguale al prodotto del numero totale di osservazioni per il numero totale di epoche (uguale al numero totale di misure). E per questo motivo che solitamente non viene usato il metodo dei minimi quadrati. A questo punto noi ci poniamo la seguente domanda: esistono dei trucchi algebrici che permettono di ridurre le dimensioni del problema? Partiamo considerando la ben nota relazione (A BCS) 1 = A 1 A 1 B(C 1 + SA 1 B) 1 SA 1 Applichiamo questa identità alla (15) considerando A = C ε B = M C = D 1 C ω (D + ) 1 S = M + (16) (17) ponendo W ε = (C ε ) 1 W ω = (C ω ) 1 così che ˆω =C ω D + 1 [ M + W ε M + W ε M(D + W ω D + M + W ε M) 1 M + W ε ]Y = = C ω D + 1 [ M + W ε M + W ε + D + W ω D(D + W ω D + M + W ε M) 1 M + W ε ]Y = = D(D + W ω D + M + W ε M) 1 M + W ε Y (18) (19) La differenza tra la (15) e la (19) è nella struttura della matrice normale. Infatti, come abbiamo appena visto, la matrice normale della (15) ha dimensioni uguali al numero di osservazioni per il numero di epoche (dimy T), mentre la matrice normale della (19) ha dimensioni uguali al numero di osservazioni per il numero di parametri (dimy dimx) così che la prima può essere più piccola della seconda. D altra parte la struttura della matrice normale nella (15) è quella di una matrice piena, mentre nella (19) è quella di una matrice in banda a blocchi. Cioè: D + W ω D = W ω 1 + T + 2 W ω 2 T T + ω 2 W 2 W ω 2 T 2 W ω 2 + T + 3 W ω 3 T 3 T + ω 3 W 3 W ω 3 T 3 W ω 3 + T + 4 W ω 4 T 4 T + ω 4 W 4 T 4 + W 4 ω W 4 ω (2) Bollettino SIFET 1/25

7 Navigazione geodetica o navigazione senza filtro di kalman M + W ε M = H + 1 W ε 1 H 1 H + T W ε T H T (21) ra la matrice normale, che risulta dalla somma di (2) e (21), è una matrice in banda con una lunghezza pari a 2dimX e quindi può convenientemente essere risolta con un applicazione standard dell algoritmo di Cholesky. Tra l altro tale algoritmo consente anche di conoscere, senza grandi sforzi numerici, l inversa entro il profilo stesso della banda. Ricordando la (11) e definendo e = x - xˆ troviamo la soluzione ˆX =(D + W ω D + M + W ε M) 1 M + W ε Y e la matrice di covarianza (22) C ee =(D + W ω D + M + W ε M) 1 (23) Come già osservato, le (22) e (23) possono essere risolte, almeno per la parte in banda a blocchi, applicando un algoritmo di Cholesky e con due passaggi avanti e indietro per la soluzione di sistemi triangolari. Possiamo quindi concludere che lo sforzo numerico intrinseco a questo metodo, che usa il classico algoritmo dei minimi quadrati e riscrive la matrice normale in un modo più conveniente, è strettamente comparabile a quello del Kalman filtering seguito dal Kalman smoothing. I vantaggi di questo approccio, che chiameremo approccio geodetico, rispetto all approccio di Kalman sono la sua semplicità dal punto di vista concettuale e la migliore conoscenza della matrice di covarianza dell errore di stima. Bibliografia Heiskanen W.A., Moritz H., Physical Geodesy. Reprint: 199 Institute of Physical Geodesy. Technical University, Graz, Austria. Hildebrand F.B., Advanced Calculus for Engineers. Prentice-Hall Inc., Englewood Cliffs, N.Y. Jekeli C., 21. Inertial Navigation Systems with Geodetic Applications. Walter de Gruyter, Berlin-New York. Kalman R.E., 196. A New Approach to Linear Filtering and Prediction Problems. Transactions of the ASME Journal of Basic Engineering, 82 (Series D): pp Mayback P.S., 1994 Stochastic models, estimation and control In Mathematics in Science Engineering Ed. R. Ballman Univ. of S. Carolina Manzino A. M., 23. Introduzione all uso del filtro di Kalman in Geodesia. Bollettino SIFET 2/23: pp Skaloud J., ptimizing Georeferencing of Airborne Survey Systems by INS/DGPS. UCGE Report No. 2126, Department of Geomatics Engineering, University of Calgary, Calgary, Canada. 4. Conclusioni e prospettive future Sulla base di quanto esposto possiamo affermare che in molti casi può essere più conveniente usare l approccio geodetico. Infatti: abbiamo dimostrato che, se la matrice normale è scritta come matrice in banda a blocchi, il carico numerico di questo approccio è comparabile con quello del Kalman filtering seguito dal Kalman smoothing; dal punto di vista teorico, la derivazione della soluzione geodetica è più semplice; la soluzione geodetica può essere utilizzata quando vogliamo inserire informazioni note a priori (equazioni di vincolo); la soluzione geodetica è uno strumento affermato per i problemi di geodesia integrata. Ultimo passo per completare la nostra verifica sarà quello di applicare Kalman filtering seguito dal Kalman smoothing ad alcuni esempi simulati per confermare la completa equivalenza con le stime ottenute mediante minimi quadrati. 123

8

9