APPLICAZIONE DELL UNSCENTED KALMAN FILTER ALLA

Transcript

1 APPLICAZIONE DELL UNSCENTED KALMAN FILTER ALLA SOLUZIONE DELLE EQUAZIONI DELLA NAVIGAZIONE STEFANO LAGRASTA TELESPAZIO S.P.A., VIA TIBURTINA, , ROME (ITALY) ABSTRACT Imporan GNSS applicaion ha are of inere for Telepazio deal wih non-linear ae eimaion problem. Thi i he cae of ground egmen componen, like Fligh Dynamic Syem, o uppor he pace miion wih an accurae orbi deerminaion. Since May,, he US Governmen ha urned off Selecive Availabiliy (SA) on all GPS aellie; hi enforce he objecive of a Preciion Orbi Deerminaion, performed by uing fundamenally he daa colleced by a ingle, paceborne GPS receiver. The iuaion will furher benefi of he forhcoming Galileo conellaion. A he ame ime, he dual problem of implemening a POD of GPS and, in he fuure, of Galileo pace vehicle, i a maer of increaing inere for he pace indury. Thi paper how he uiabiliy of he o called Uncened Kalman Filer (UKF) o he poiion eimaion problem from GNSS ranging meauremen. The UKF i a equenial algorihm for eimaing he ae vecor of a dynamic yem, when in preence of ochaic diurbance; i applicaion domain i he ame a he Exended Kalman Filer. Now, he fundamenal difference beween EKF and UKF i in ha laer i fully non-linear. I equenial proceing avoid he calculaion of any Jacobian or deign marix and hu uppree he need of reconrucing any derivaive of plan model equaion. The propoed algorihm i eed wih real daa, coniing of raw navigaion obervable and po-proceed reference orbi available from Champ aellie miion. OSSERVABILI DI NAVIGAZIONE Le miure di navigazione generae da un riceviore GNSS coniono eenzialmene nelle oervabili di code range ( ρ ) e di carrier range ( Φ ); in paricolare, Φ i oiene moliplicando la carrier phae ( ϕ ) per la lunghezza d onda ( λ ) della porane del egnale di navigazione, avendoi:

2 ρ ( T R ) = ρ ( R, τ ) + c ( ε ET ε SV ) + () + c ( REL-SV + REL-ET + REL + IONO + TROPO ) + + noie + mulipah + ε EPH Φ ( T R ) = λ ϕ ( T R ) = ρ( R, τ ) + c ( ε ET ε SV ) +. () + c ( REL-SV + REL-ET + REL IONO + TROPO ) + + λ N + (very low) noie + (low) mulipah + ε EPH dove: ρ( R, τ ) = A( R, τ ) p SV ( R τ ) - r ET ( R ) () ed il ignificao dei vari ermini è di eguio illurao. Anziuo, il imbolo ET a per End Terminal (il riceviore di radionavigazione), menre SV per Space Vehicle. La variabile denoa una cala emporale di riferimeno aolua, corripondene alla migliore approimazione poibile del Terrerial Time : il empo che corre ulla uperficie del geoide errere, il profilo maemaico del noro pianea, dove gli effei relaiviici dovui al moo roazionale ed al gradiene di gravià vedono muuamene compenae le componeni variabili. Tempo locale e empo miurao dal riceviore uene Denoando T ET ( R ) il ime ag con cui il erminale di navigazione marcherà un dao campione di miure, R arà l iane di ricezione corripondene, nella cala di riferimeno aolua, avendoi: T ET ( R ) = R + REL-ET ( R ) + ε ET (4) dove ε ET è l errore di incronizzazione dell orologio del riceviore, ripeo alla cala aolua, menre REL-ET è una componene relaiviica, dovua alla differenza nello correre del empo in corripondenza della poizione dell ET, ripeo alla uperficie della Terra.

3 ε ET è legao alla impreciione con cui un orologio reale implemena la miura del empo, menre REL-ET dipende dallo ao di moo e dal gradiene di gravià perimenao dal riferimeno locale olidale al riceviore. REL-ET è ignificaivo nel cao in cui il erminale ia inallao a bordo di un aellie LEO con eccenricià orbiale non racurabile, avendoi: µ REL-ET bia + c R a ( r ET ) T r& ET c (5) dove r SV rappreena la poizione del cenro di maa del aellie, in ai ECF, R = [m], µ = coane di gravià erree = [ m ], e = eccenricià orbiale, a = emiae orbiale maggiore, E = valore correne dell anomalia eccenrica = E(). La pare coane dell effeo, collegaa al ermine indicao come bia, è in genere aimilaa a ε ET ed ε SV. Nella (5) appare un ermine in grado di indurre una deviazione monoonamene crecene del empo egnao dal clock, ripeo alla cala ; queo effeo è ignificaivo nel cao in cui il erminale ia monao a bordo di un aellie LEO privo di pre-compenazione a bordo. Tempo di propagazione del egnale di radionavigazione Il paramero τ denoa il empo di ranio del egnale di radionavigazione; l iane di emiione E del frone d onda ricevuo a R è quindi dao da: E = R τ (6) I ermini IONO, TROPO rendono cono della variabilià dell indice di rifrazione n dell amofera, ripeo al cao ideale di pazio vuoo (n = ). La preenza di un gradiene graviazionale, lungo il percoro di propagazione del egnale, richiede di coniderare un uleriore exce pah, rappreenao ramie inroduzione del ermine di riardo REL. Il empo di propagazione τ è quindi più preciamene epreo dai egueni ermini: τ = ( R E ) = ρ( R, τ ) c + REL + IONO + TROPO (7) Per quano aiene a REL, uie la eguene modellazione:

4 + γ REL ( R, τ ) = µ c ( PPN ) R ( ) ( ) ( ) ET R + R SV R τ + ρ R, τ ln R ( ) + R ( τ ) ρ(, τ ) ET R SV R R dove γ PPN è il "Po-Newonian Parameer" della Teoria della Relaivià. (8) Range Geomerico ρ( R, τ ) denoa la miura di range geomerico, ovvero la dianza ra la collocazione del cenro di fae dell anenna del payload di navigazione ( p SV ) all iane E e le coordinae careiane dell uene ( veore r ET ) all iane E. Poiché p SV, r ET ono eprei in coordinae ECF (Earh-Cenred and Fixed) e dianziai emporalmene del empo di propagazione τ, occorre ener cono del movimeno compiuo dalla Terra nell inervallo τ. A queo i provvede con l operaore mariciale A( R, τ ). Con buona approimazione, A eprime fondamenalmene un moo aorno all ae z, dell angolo di roazione accumulao dalla Terra durane il empo τ : co( τ ω ) in( τ ω ) A ( R, τ) A( τ) = - in( τ ω ) co( τ ω ) (9) ω >, ω [rad] Tempo locale e empo miurao dal payload di bordo L orologio di bordo del aellie fa corripondere E al empo T SV ( E ), eendo: T SV ( E ) = E REL-SV ( E ) + ε SV () Similmene a quano oervao per il erminale uene, ε SV decrive il ime error del clock di bordo, ripeo alla cala aolua, menre REL-SV dipende dal empo locale di un riferimeno olidale al aellie; REL-SV non è un offe racurabile. Si ha: REL-SV bia + ( r SV ) T SV r& c () laddove non compare un ermine proporzionale a, come nella (5); difai, nel cao del aellie di navigazione, è applicaa una appoia pre-compenazione, operando uno hif della frequenza del clock aomico.

5 Se f è la frequenza nominale deideraa, l orologio arà regolao dalla frequenza f definia come: µ f = f () c R a Nel cao di moo puramene Kepleriano, i ha la uile equivalenza: r T r& = e µ a in (E ) () applicabile ano al cao del aellie di navigazione che per une evenuale veicolo LEO equipaggiao di riceviore GNSS. Errore di Effemeridi Il ermine ε EPH rappreena una componene di errore originaa dall uilizzo di una inerpolazione approimane per il veore p SV ( ). ε EPH figura ogni qual vola il veore p SV non ia compreo ra le grandezze da imare, eendo ricoruio in bae ad un modello dao a priori. Fenomeni Amoferici Il ermine TROPO eprime il riardo di propagazione, dovuo al ranio del egnale di navigazione negli rai elericamene neuri dell amofera, denoai compleivamene, anche e in modo imprecio, come ropofera. La regione dove ha ede il fenomeno ropoferico i compone in realà di re rai: la ropofera propriamene dea, fino a km di quoa, eguia dalla ropopaua, fino a circa km, infine dalla raofera, che ermina approimaivamene a km. Nel cao di un aellie LEO, TROPO è ovviamene racurabile, daa la quoa alla quale i rova collocao. Nel cao di un riceviore a erra, ciiamo il modello di Hopfield come un uile puno di parenza, ovvero una formulazione emplice e uavia efficace a dipoizione: ˆ TROPO = + c c N N d () h w d () h w in - ( E +.94 ) in -4 ( E ) (4)

6 P() () = c (5) T() N d e() e() N w () c + c (6) T() T() dove E rappreena l elevazione locale del aellie di navigazione ripeo all orizzone locale del riceviore, P(), e(), T() ono ripeivamene la preione (oale) dell aria, la preione del vapore d acqua e la emperaura aolua a quoa nulla, menre h d rappreena lo peore della ropofera, inea per emplicià come ingolo layer, eendo: h d = ,7 ( T() 7,6 ) [m] (7) h w = Per coefficieni { c, c, c } i poono uilizzare i egueni valori: c = [ mbar K ], c =.96 [ mbar K ], c =.78 5 [ mbar K ]. IONO rappreena invece il riardo di propagazione, dovuo al ranio del egnale negli rai dell amofera dove eia una denià non racurabile di eleroni liberi; anche in queo cao, i vari layer che danno origine al fenomeno ono accomunai dal ermine ionofera. Il modello (di Klobuchar) aualmene diponibile per una ima del ermine IONO, in bae ai parameri ramei dal meaggio di navigazione della coellazione GPS, non è paricolarmene accurao. Il iema Galileo, uilizzando il modello NeQuick, promee maggior preciione. In una approimazione al prim ordine, IONO appare inveramene proporzionale al quadrao della frequenza della porane di navigazione: [m] IONO f (8) Tano baa a uggerire l uilizzo di combinazioni lineari iono free, eprimeni oervabili derivae di code range ( ρ IF ) e di carrier range ( Φ IF ) nel cui modello non figuri il ermine IONO :

7 L L L L f ρ IF = L f ρ L L ρl f f f f (9) L L L L f Φ IF = L f L Φ L Φ L f f f f () con riferimeno, epreo nei pedici, alla circoanza che le miure iano rae dai egnali GPS L e L. Il modello maemaico di IF ρ, Φ IF è del uo idenico a quello fornio dalle (), () per ρ, Φ, faa alva la mancanza della componene IONO e la preenza di una ambiguià iniziale non inera per Φ, eendo pari a: c IF f L N L f L N L f L f L () MODELLAZIONE GENERALE DEL PROBLEMA DI STIMA Il problema di ima cui i aaglia l algorimo UKF (Uncened Kalman Filer) è epreo dalle relazioni e dalle figure egueni: d d x() = f (x(), u(), ) + w() () z = h(x, u, ) + v () k k x k = x( k ), u k = u( k ), z k = z( k ), k = + k * T, k=,,, k k k w() u() f( x(), u(), ) x() Sae vecor dynamic

8 iming clock v( k ) u() h( x(), u(), ) z( k ) w() Sae vecor dynamic x() dove v e w ono rumori gauiani bianchi, raandoi di ricoruire il veore di ao x, non direamene acceibile. Un puno di forza dell algorimo, ideao da Simon J. Julier and Jeffrey K. Uhlmann, è nel fao che non occorre procedere ad alcuna linearizzazione delle funzioni f( ) ed h( ), né ad una loro epliciazione in forma analiica: quello che cona, è che i conoca il modo di calcolarle o, nel cao della f( ), di inegrare il modello dinamico () ra due iani ucceivi, k e k+. La procedura di calcolo previa dall algorimo UKF è decria in S.J.Julier, J.K.Uhlmann [4], [5], [6]. MODELLAZIONE DI DETTAGLIO Le miure offere da ρ, Φ, (o anche ρ IF, Φ IF ) ono foremene non-lineari, ripeo alle grandezze che i inendono imare; i peni al compleo legame che vede le precedeni come funzioni del veore delle coordinae uene r ET, coniderao incognio nella claico problema del poiioning. Nel eguio, i omeerà empre il pedice ET per emplificare la noazione, facendoi peralro riferimeno ad un ingolo erminale, uilizzando perciò r anziché r ET, ed ε al poo di ε ET. Cao di Modello Dinamico Lineare Quando il riceviore uene è fio a erra, oppure raporao da un pedone o veicolo la cui legge di accelerazione ia incognia, l unico vincolo dinamico è ipicamene rappreenao dalla cinemaica, avendoi:

9 d d r = v (4) dove v è la velocià relaiva del erminale, in ai ECF. La caua del movimeno non è modellabile in forma deerminiica, raandoi di un ingreo aimilabile ad un fenomeno ocaico: e m denoa la maa aociaa al modo del erminale ed f la riulane delle forze ageni, arà: m d d f = v = r = rumore di proceo = w d () (5) d L errore di clock del riceviore, ia eo ε, arà aimilabile ad un random walk, ponendo: d d ε = w () (6) quea modellazione i prea infai al cao in cui ε ia una coane o una grandezza lenamene variabile nel empo. Il veore x dello ao che i inende imare aume dunque la forma: x = [ x T, x T, x ] T, con: x = r, x = v, x = c ε (7) da cui il modello dinamico: d d x = A x + w (8) I A =, w = w, w S = I (9) dove S rappreena lo pero di denià di poenza per il veore w. La dicreizzazione del modello, con pao di campionameno, fornice immediaamene:

10 w x ) (k x + = + A, con: = I I I A () dove ciò che neceia conocere è la covarianza Q del rumore gauiano bianco w, che riula pari a: = Q () Cao di Modello Dinamico Non Lineare Quando il riceviore è collocao a bordo di un aellie LEO, que ulimo è oggeo a forze reali; uavia, crivendone le equazioni del moo in un iema non-inerziale, occorre inrodurre alreì accelerazioni appareni di racinameno e Corioli. In quano egue, r denoa le coordinae del Cenro di Maa (C.o.M) del veicolo LEO in ai ECF. Per emplicià, il e propoo con dai reali i baa u un modello di accelerazione migliorao per la ola componene J (momeno di quadrupolo) della forza di gravià, in aggiuna a quella Newoniana fondamenale. Alre forze porebbero eere inrodoe, coì come un modello per la gravià arbirariamene compleo: in que ulimo cao, l algorimo di ima non ne ubirebbe complicazioni in ermini di formulazione. In bae alle ipoei fae, la riulane f delle forze reali ageni ul C.o.M, proieaa in ai ECF, riula pari a: + = g r r g f m z r () con:

11 g ( 5in φ ) µ a E g r = & + J c () r r z µ a E =& J in φc, r r r in φ x c = r (4) dove il veore coane ω eprime la velocià angolare della erra, proieaa ea pure in ai ECF. Anche in queo cao, il veore di ao che i inende imare conie (al minimo) in 7 componeni: x = [ x T, x T, x ] T, con: x = r, x = v, x = c ε (5) Le leggi dinamiche del modello i crivono: d d d d x = x (6) x = ω ( ω x) ω x + g r x + g z x (7) d d x = (8) con le opporune oiuzioni nelle epreioni di g r, g z, enrambe dipendeni dal modulo e dalle componeni del veore x. Modello delle Miure Il veore di miura z k, diponibile al empo k, dipende ovviamene dalla cela operaa delle oervabili di navigazione da proceare. Sani anche i modelli dinamici propoi, con veore di ao a 7 elemeni, i è opao per le miure di code range iono free, rinunciando quindi alla incluione nel veore x di evenuali ambiguià iniziali o grandezze acceorie legae all uilizzo delle miure di fae. Supponendo che m aellii di navigazione iano in via, i avrà che h( ) eprime un veore di m funzioni calari, la j-eima delle quali è:

12 h j ( x, x, x ) = ρ j, j =,,...,m (9) dove ρ j, con noazione emplificaa, denoi appuno una oervabile derivaa di peudorange, oenua ramie la combinazione lineare iono free di cui alla (9). Il veore di miura riula perano: ρ h( x, x, x ) h ( x, x, x ) z ρ k = = + v k (4) ρm h m ( x, x, x ) Ora, i raa di epliciare la meccanizzazione del calcolo delle h j ( ). Se p j denoa la poizione del j-eimo aellie di navigazione, ε j l aociao errore di clock e τ j il empo di ranio del egnale da emeo, i avrà: h j ( x, x, x ) = j p x + ( x c ε j ) + (4) + c ( ˆ REL-ET + ˆ REL-SV ) j p = A( τ j ) p j ( j ) (4) j = T k x c τ j (4) τ j ρ j c x c + ε j REL-SV (44) Un approccio alernaivo per il calcolo del empo di ranio del egnale conie nell ierazione: τ j c A( τ j ) p j ( T k x c τ j ) x + (45) + c ( ˆ REL + ˆ IONO ) da eeguire fino a convergenza u un valore abile di τ j. Il calcolo delle precedeni relazioni preuppone che, ramie il meaggio di navigazione GPS o alra fone, i ia in grado di calcolare la poizione del cenro di fae del payload di navigazione, p j ( ), nonché l offe di clock del veicolo paziale, ε j.

13 Per i ermini relaiviici ˆ REL-ET, ˆ REL-SV e per la marice di roazione A( τ ), ono ai fornii ragguagli nei precedeni paragrafi. Allo copo di migliorare l accuraezza, i può ener cono del fao che il modello dinamico eprime con x la poizione del C.o.M. del aellie LEO; vicevera il veore da coniderare, per il modello delle miure, è la poizione del cenro di fae dell anenna del riceviore LEO, in ai ECF. Si ha: r ET x + M a (46) dove a ia la poizione dell anenna del erminale LEO in ai body fixed, olidali al aellie, ed M ia la marice di raformazione da ai body fixed alla erna ECF. In bae alla (46), la (4) i crive, più preciamene: h j ( x, x, x ) = p j x M a + ( x c ε j ) + (47) + c ( ˆ REL-ET + ˆ REL-SV ) la marice M può peralro coruiri immaginando un aeo ideale del aellie LEO, econdo le convenzioni correni per l orienameno degli ai: M = [ u, u, u ] T (48) u = u u u = ( v I x ) v I x, con: v I = x + ω x u = x x TEST CON DATI REALI Cao di Modello Dinamico Lineare Il modello di evoluzione lineare dello ao, rappreenao dalle (), (), i prea ad eere perimenao con i dai grezzi prodoi da un riceviore in poizione fia noa, georeferenziaa. Perano, i è volo un e u dei dai collezionai nel da un riceviore NovAel collocao a Kiruna (Svezia), durane la campagna di prove previe dal Programma EGNOS TRAN.

14 La poizione dell anenna è aa calcolaa con accuraezza da ESACNES, riulando il valore di riferimeno: r REF = [ , 894.6, ] T. A parire dalle ole oervabili di code range in banda L, non pre-filrae, l algorimo UKF ha prodoo una ima della poizione rˆ per ogni campione di miure; la dianza rˆ r REF è rappreenaa come errore in figura. figura : e UKF u miure grezze di peudorange in banda L collezionae da una azione in poizione fia, georeferenziaa, collocaa a Kiruna L errore reiduo i deve alla componene ionoferica non compenaa (è ao uao il modello di Klobuchar), agli errori di effemeridi, al mulipah. Cao di Modello Dinamico Non Lineare Il modello di evoluzione non-lineare dello ao orbiale di un aellie LEO, imperniao ulla (7), è ao adoao per un e di oervabili collezionae dal riceviore di bordo del aellie CHAMP. E ao neceario carare moli e di oervabili, rienui oulier (quindi miure erronee, non conieni ra loro). Inolre, è aa adoaa la oervabile derivaa di code range del ipo iono free daa dalla (9). Come ermine di paragone ( r REF ), i è conideraa un orbia di riferimeno ulraprecia, prodoa dai proceameni POD congiuni NASAJPLGFZ.

15 Di nuovo, la dianza rˆ r REF ra il valore della ima di poizione oenua mediane UKF e il dao POD è rappreenaa come errore nella figura che egue: figura : e UKF u miure di peudorange iono free, a parire dalle miure grezze di code range in banda LL collezionae dal aellie CHAMP La maggior pare dei campioni ono caraerizzai da un errore acceabile; gli pike denoano e di oervabili non corree. Le orbie oenue dal proceameno ono racciae, ripeo ad ai ECF, in figura.

16 figura rappreenazione, in ai ECF, delle orbie del aellie CHAMP, ricoruie dai dai grezzi a dipoizione CONCLUSIONI L applicazione dell Uncened Kalman Filer al problema foremene non-lineare della ricoruzione dello ao orbiale di un aellie LEO, a parire da miure di peudorange GNSS, ha dao prova di oima funzionalià e robuezza. A differenza del cao di uilizzo di un Exended Kalman Filer (EKF), la Traformazione Uncened ideaa da S.J.Julier e J.K.Uhlmann non ha richieo alcuna fae o procedimeno di linearizzazione, né eplicioanaliico, né implicionumerico. Si raa ora di migliorare l accuraezza del riulao coneguio, uilizzando in modo più eeo, olre a quelle di code range, le miure di fae. Inolre, è foremene raccomandabile, per i aellii GPS, l uilizzo di elemeni orbiali di riferimeno ed errori di clock imai da IGS, anziché quello dei modelli di inerpolazione aociai al meaggio di navigazione.

17 BIBLIOGRAFIA [] A. Gelb, edior." Applied Opimal Eimaion". The MIT Pre, 974 [] A.H. Jazwinki. "Sochaic Proce and Filering Theory" Academic Pre, 97 [] P. S. Maybeck. "Sochaic Model,Eimaion and Conrol" (Vol. & ). Academic Pre, 98 [4] S.J.Julier and J.K.Uhlmann. "A General Mehod for Approximaing Nonlinear Tranformaion of Probabiliy Diribuion", 994. [5] S.J.Julier, J.K.Uhlmann and H.F.Durran-Whye. "A New Approach for he Nonlinear Tranformaion of Mean and Covariance in Linear Filer". IEEE Tranacion on Auomaic Conrol, 996 [6] S.J.Julier, J.K.Uhlmann and H.F.Durran-Whye. "A New Approach for Filering Nonlinear Syem". The proceeding of he American Conrol Conference, Seale, Wahingon, 995, page 68-6