Sviluppo di filtri di Kalman per sistemi di navigazione INS/GPS con sensori inerziali di basso costo.

Università degli studi di Pisa

DIPARTIMENTO DI INGEGNERIA CIVILE E INDUSTRIALE

Corso di Laurea Magistrale in Ingegneria Aerospaziale

Sviluppo di filtri di Kalman per sistemi di navigazione INS/GPS con

sensori inerziali di basso costo

Candidato:

Relatori:

Alice Ghisu

Ing. Francesco Schettini

Prof. Eugenio Denti

Ing. Gianpietro Di Rito

Prof. Roberto Galatolo

1

Ma tra la partenza ed il traguardo in mezzo c’è tutto il resto. E tutto il resto è giorno dopo giorno. E giorno dopo giorno è silenziosamente costruire. E costruire è sapere e potere rinunciare alla perfezione. Costruire, Niccolò Fabi

Al mio relatore Francesco, Al mio tutor Luca, Alla mia insegnante di teatro Marta, Al Dottor Staccioli.

2

Indice

SOMMARIO ... 4

1. Introduzione ... 5

2. Terne di riferimento e trasformazione ... 8

2.1 Terna ECI ... 8

2.2 Terna ECEF ... 8

2.3 Terna NDE ... 8

2.4 Terna assi corpo ... 9

3. Sistemi INS e GPS ... 11

3.1 INS ... 11

3.1.1 IMU ... 11

3.1.2 Algoritmo di calcolo ed equazioni di navigazione ... 12

3.2 GPS ... 15

4. Teoria del Filtro di Kalman ... 17

4.1 Formulazione discreta ... 17

4.2 Formulazione continua ... 20

5. Sistema di navigazione integrata INS/GPS con filtro di Kalman ... 23

5.1 Sistema integrato composto da INS e GPS ... 23

5.2 Applicazione della teoria del filtro di Kalman al sistema integrato INS/GPS ... 24

5.3 EKF: Linearizzazione delle equazioni di navigazione ... 27

5.3.1 Linearizzazione delle equazioni di velocità ... 29

5.3.2 Linearizzazione delle equazioni di posizione ... 30

5.3.3 Linearizzazione delle equazioni di assetto ... 31

5.3.4 Matrice G ... 38

5.3.2 Semplificazioni sotto le ipotesi di Terra piana e non rotante ... 39

6. Sviluppo del sistema integrato INS/GPS e simulazioni numeriche ... 41

3

6.2 INS in assenza di navigazione integrata ... 44

6.3 GPS ... 47

6.4 Filtro di Kalman ... 48

6.4.1 EKF discreto con formulazione di tipo total state ... 49

6.4.2 EKF discreto con formulazione di tipo error state ... 58

6.4.3 EKF continuo con formulazione di tipo error state ... 64

7. Prove sperimentali ... 70

7.1 Raccolta Dati ... 70

7.2 Risultati dei test ... 70

7.2.1 EKF continuo error state ... 70

7.2.2 EKF discreto error state ... 79

7.2.3 EKF discreto total state ... 88

8. Conclusioni e sviluppi futuri ... 96

Appendice 1 ... 98

Appendice 2 ... 98

Appendice 3 ... 98

Bibliografia ... 100

4

SOMMARIO

Argomento di questa tesi di laurea è lo studio e lo sviluppo di sistemi di navigazione in cui filtri di Kalman realizzano l’integrazione tra le misure inerziali, provenienti da sensori di basso costo, e le misure di posizione e velocità fornite da un ricevitore satellitare, con lo scopo di ricostruire la traiettoria di un corpo in movimento in termini di posizione, velocità ed assetto.

Nella prima parte del lavoro è stato esaminato lo stato dell’arte sui sistemi di integrazione INS/GPS e sulla teoria del filtro di Kalman per le architetture note in letteratura con la designazione Extended. Successivamente sono stati sviluppati diversi sistemi di navigazione in ambiente MATLAB/Simulink, differenti per metodologia di integrazione (continua o discreta) e definizione del vettore di stato oggetto della correzione operata dal filtro (grandezze totali o errori).

Le prestazioni di tali sistemi sono state valutate in un primo momento mediante simulazioni aventi come dati di input storie temporali di moto arbitrarie. Successivamente è stata effettuata un’ampia campagna di prove sperimentali, realizzate con un autoveicolo equipaggiato con un apposito sistema di navigazione commerciale, il quale è collegato ad un’antenna satellitare GPS ed usa sensori inerziali di basso costo, ma grazie ad un filtro di

Kalman Extended proprietario è capace di fornire elevate prestazioni nella determinazione

di posizione, velocità ed assetto dell’autoveicolo. Tale sistema di navigazione consente di accedere ai dati grezzi di accelerazione e velocità angolare forniti dall’IMU, alle misure di posizione e velocità fornite dal ricevitore satellitare ed anche alla traiettoria, in termini di posizione, velocità ed assetto del veicolo, ricostruita grazie al filtro di Kalman Extended proprietario.

Nelle prove sperimentali effettuate le storie temporali di accelerazione e velocità angolare ed il segnale del ricevitore satellitare sono stati utilizzati come dati di input per i sistemi sviluppati nella prima parte del lavoro; la traiettoria stimata da essi in termini di posizione, velocità ed assetto è stata infine confrontata con quella ricostruita dal sistema di navigazione commerciale.

5

1. Introduzione

I sistemi di navigazione inerziali rispondono alla necessità di determinare la di un corpo in movimento in termini di posizione, velocità ed assetto tramite integrazione dei segnali di accelerazione e velocità angolare provenienti da sensori inerziali (accelerometri e giroscopi). Tali sensori possono essere solidali o meno al corpo di cui si vuole stimare la traiettoria: nel primo caso si parla di piattaforma inerziale con sensori di tipo strapdown, nel secondo caso di piattaforma inerziale compensata, in cui una sospensione cardanica sostiene i sensori. Quest’ultima soluzione necessita di sensori di elevate prestazioni e costo e di un periodico intervento durante il funzionamento, al fine di correggere gli errori di disallineamento tra terna assi corpo e terna di navigazione.

Al giorno d’oggi i sensori maggiormente diffusi sono quelli di tipo strapdown MEMS (Micro Electro-Mechanical Systems), risultato della ricerca nell’ambito delle microtecnologie svolta nel corso del XXI secolo. L’impiego dei sensori MEMS per la realizzazione di sistemi di navigazione inerziale (Inertial Navigation System, INS) si è rapidamente diffuso grazie ai vantaggi che tali dispositivi apportano in termini di peso, ingombro e costo.

D’altro canto però, gli errori da cui sono affetti i sensori MEMS di basso costo, come il

bias e il drift all’accensione, rendono inaffidabile, già dopo pochi secondi di

funzionamento, la ricostruzione della traiettoria fatta dai sistemi di navigazione inerziali. In questo scenario, una possibile soluzione per garantire, ad un costo contenuto, la precisione richiesta sulle informazioni di posizione, velocità ed assetto, è quella di integrare il sistema INS con le misure fornite da fonti indipendenti da esso. La scelta del metodo con cui giungere alla migliore stima possibile dello stato del sistema è il problema chiave che sta alla base dei cosiddetti sistemi integrati.

Uno dei sistemi di maggiore interesse è senza dubbio quello costituito da INS e GPS (Global Positioning System), grazie alle caratteristiche complementari che essi presentano. L’INS è un sistema il quale, indipendentemente da informazioni esterne e note che siano le condizioni iniziali, è in grado di fornire i dati di posizione, velocità ed assetto ad elevata frequenza e con un errore la cui caratteristica è quella di accumularsi nel tempo per effetto dell’integrazione numerica; il GPS è invece legato al segnale radio proveniente dai satelliti, la sua frequenza di aggiornamento è ridotta rispetto a quella dell’INS e le misure che fornisce sono soggette ad un elevato rumore.

6

L’algoritmo del filtro di Kalman, presentato nel 1960 da [1], rappresenta uno dei metodi più utilizzati per processare i dati di misura del sistema di navigazione integrato. Dallo studio della letteratura relativa all’applicazione della teoria del filtro di Kalman all’integrazione INS/GPS, emerge la necessità di linearizzare le equazioni di navigazione per poter applicare l’algoritmo ricorsivo di correzione di Kalman; Schmidt [2] è tra i primi a proporre tale soluzione e qualche tempo dopo Wolf [3] sviluppa l’architettura nota come

Extended Kalman Filter (EKF) e ne valuta le prestazioni realizzando un sistema che

integra le misure inerziali provenienti dalla piattaforma IMU Systron Donner’s Motion

ParkTM con le misure di posizione e velocità provenienti da un ricevitore GPS. In tale sistema lo stato oggetto della correzione del filtro è formulato in termini di errore e comprende anche la stima di bias e drift dell’IMU.

Seguono le esperienze di Julier [4], in cui sono esposte alcune problematiche riscontrate nell’implementazione dell’EKF, tra cui la possibilità di avere comportamenti instabili quando la dinamica fortemente non lineare del sistema violi le ipotesi alla base della linearizzazione del sistema stesso e dunque la necessità di intervalli di integrazione sufficientemente piccoli, che portano inevitabilmente ad una crescita del costo computazionale.

Sukkarich [5] sperimenta, nell’implementazione di un filtro per applicazioni su veicoli

terrestri, la difficoltà implementativa nel settaggio dei parametri dell’EKF, che consiste essenzialmente nella regolazione delle componenti delle matrici Q ed R.

Petovello [6] nel suo lavoro, pur riconoscendo i limiti, riscontrati dai suoi predecessori,

della struttura linearizzata del filtro, realizza un’EKF per veicoli terrestri e, come anche fa

Shin [7], riesce ad implementare una formulazione più robusta delle precedenti

concentrando l’attenzione sul funzionamento del sistema integrato in assenza momentanea del segnale GPS.

Seguono delle ricerche, portate avanti dallo stesso Shin [8], che si basa sul lavoro di Julier [4], riguardo altre possibili forme del filtro, tra cui l’UKF (Unscented Kalman Filter) nel tentativo di ovviare a quelli che sono i problemi implementativi dell’EKF.

Anche Mohamed [9,10] propone una possibile soluzione alle instabilità presentate dal filtro, sviluppando un filtro cosiddetto AKF (Adaptive Kalman Filter), in cui le matrici di covarianza dell’errore sul processo e sulle misure non sono costanti come nell’EKF, ma variano ogni qual volta venga acquisita la misura dal GPS, strada poi seguita più di recente anche da Wen [11].

7

Negli ultimi anni (2000-2015), come dimostrano studi come quelli di Hieu [12], Tang [13] e Wendel [14] la tendenza sembra quella di tornare a preferire l’EKF rispetto alle formulazioni UKF e AKF che non apportano miglioramenti tali da giustificarne la maggiore complessità implementativa.

Questo lavoro di tesi ha lo scopo di sviluppare e testare le prestazioni di un sistema integrato INS/GPS con filtro EKF nelle seguenti architetture:

- Sistema integrato INS/GPS discreto con EKF con formulazione del vettore di stato in termini di quantità totali;

- Sistema integrato INS/GPS discreto con EKF con formulazione del vettore di stato in termini di errore;

- Sistema integrato INS/GPS continuo con EKF con formulazione del vettore di stato in termini di errore.

Nel capitolo 1 è stata inquadrata l’attività svolta in questo lavoro di tesi dando una panoramica di quello che è lo stato dell’arte del sistema di integrazione INS/GPS con filtro di Kalman. Nei capitoli 2 e 3 sono presentati i modelli matematici dell’INS e del GPS e vengono spiegati i motivi per i quali la loro integrazione porta a risultati estremamente efficaci. Nel capitolo 4 è presentata la teoria del filtro di Kalman, mentre nel capitolo 5 la sua applicazione al sistema integrato INS/GPS. Nel capitolo 6 sono sviluppate e testate tramite simulazione numerica le varie architetture del filtro realizzate in ambiente MATLAB/Simulink.

Il capitolo 7 presenta i risultati della campagna di prove sperimentali realizzata per valutare le prestazioni del filtro ed infine nel capitolo 8 sono tratte le conclusioni sul lavoro svolto ed indicati i possibili sviluppi futuri.

8

2. Terne di riferimento e trasformazione

Esistono diverse terne di riferimento che possono essere utilizzate per la navigazione; la scelta dipende dal tipo di applicazione e strumentazione in uso.

Nei paragrafi successivi segue una descrizione dei sistemi di riferimento di interesse per questa trattazione ed infine le matrici di rotazione che permettono di passare da una terna ad un’altra.

2.1 Terna ECI

La terna ECI (Earth Centered Inertial) ha le seguenti caratteristiche:

- Origine: coincidente con il centro della Terra ad un dato istante di tempo; - Asse Zi: parallelo all’asse di spin terrestre;

- Asse Xi: parallelo alla linea dell’equinozio di inverno;

- Asse Yi : completa la terna destrorsa.

Nell’ambito della navigazione terrestre, la terna ECI è considerata come terna inerziale, nonostante essa non lo sia strettamente, in quanto coinvolta nel moto di rivoluzione attorno al Sole.

2.2 Terna ECEF

La terna ECEF (Earth Centered Earth Fixed) è definita come segue: - Origine: coincidente con il centro della Terra ad ogni istante; - Asse Ze: parallelo all’asse di spin terrestre;

- Asse Xe: giace sul piano equatoriale, risulta coincidente con il meridiano di

Greenwich;

- Asse Ye : completa la terna destrorsa.

Le terna ECEF ruota intorno alla terna ECI con pulsazione pari a .

2.3 Terna NDE

La terna di navigazione locale (North- Down- East), o terna verticale locale, è definita come segue:

9

- Asse Zn: punta verso l’interno in direzione del centro della Terra;

- Asse Xn: punta verso il Nord;

- Asse Yn : completa la terna destrorsa puntando verso Est.

La terna si muove con velocità angolare rispetto alla terna ECEF.

L’origine di questa terna sul globo terrestre è definita da Longitudine, Latitudine e quota, che costituiscono le cosiddette coordinate geodetiche. La trasformazione da coordinate geodetiche (oppure in modo equivalente [ ]) a coordinate cartesiane della terna ECEF è data da:

In figura le tre terne ECI, ECEF e NDE.

Figura 1: Terna ECI,ECEF,NDE 2.4 Terna assi corpo

Questa terna è solidale al corpo di cui si vogliono conoscere posizione, assetto e velocità, per cui l’origine del sistema coincide con quella del sistema di navigazione. Gli assi sono allineati con quelli di rollio, beccheggio ed imbardata del corpo stesso.

10

Figura 2: Terna assi corpo di un velivolo

Come sarà spiegato più ampiamente nel paragrafo successivo, risulta necessario conoscere la matrice di rotazione tra tale terna e quella scelta per esprimere le equazioni di navigazione.

Utilizzando la rappresentazione con gli angoli di Eulero , una terna di riferimento va a sovrapporsi ad un’altra tramite tre rotazioni successive attorno a tre differenti assi; tali rotazioni possono essere matematicamente espresse tramite tre differenti matrici:

- Rotazione di un angolo attorno all’asse z:

;

- Rotazione di un angolo attorno al nuovo asse y:

- Rotazione di un angolo attorno all’asse x:

.

La matrice di trasformazione che permette il passaggio da terna locale (NED) ad assi corpo è il prodotto di queste tre matrici, mentre quella che permette la trasformazione inversa non è altro che la trasposta, essendo la matrice in questione ortogonale.

11

3. Sistemi INS e GPS

3.1 INS

Un sistema di navigazione inerziale è un sistema autonomo composto sostanzialmente da due parti: l’Inertial Measurement Unit (IMU), e l’algoritmo di calcolo, il quale, note le condizioni iniziali, fornisce le informazioni riguardanti la posizione, la velocità e l’assetto del corpo.

Figura 3: INS 3.1.1 IMU

I sensori presenti nell’IMU sono giroscopi e accelerometri che possono essere solidali o meno con il corpo di cui si vuole tracciare il moto. Nel primo caso, quello in questione, si parla di INS di tipo strapdown.

I tre giroscopi forniscono le misure di velocità angolare tra terna assi corpo e terna inerziale, i tre accelerometri forniscono le misure di forza specifica agente sul corpo, misurata nella terna assi corpo.

Come già accennato precedentemente, i sensori di tipo MEMS sono soggetti ad errori di diversa natura, tra cui:

- errore bias per gli accelerometri e drift per i giroscopi, la cui caratteristica è quella di rimanere costante nel tempo ma cambiare in valore da un’accensione ad un’altra;

- errore di tipo random;

- errore legato a fattore di scala; - errore legato alle non linearità.

12

Il contributo considerato in questo lavoro sarà quello di bias,drift ed errore di tipo random, che sono quelli più rilevanti.

3.1.2 Algoritmo di calcolo ed equazioni di navigazione

A partire dai segnali di accelerometri e giroscopi, l’algoritmo di calcolo si occupa di fornire in uscita la posizione, la velocità e l’assetto del corpo, mediante la risoluzione delle equazioni di navigazione, che regolano il moto del corpo.

La dinamica di un punto P rispetto ad un sistema rotante generico, indicato con l’apice a è descritta dal teorema di Coriolis, in cui l’apice i si riferisce alla terna inerziale:

Con si indica la matrice antisimmetrica costruita con le componenti di , velocità angolare relativa tra la due terne, tale che . Per le proprietà delle matrici antisimmetriche si faccia riferimento all’Appendice 1.

Nella precedente espressione inoltre:

 è l’accelerazione dell’origine della terna rotante;

 è l’accelerazione di Coriolis;

 è l’accelerazione centripeta, con direzione perpendicolare all’asse di rotazione tra e due terne;

 è l’accelerazione tangenziale.

Nel caso in cui la terna rotante considerata sia proprio la terna ECEF, la cui origine è fissa rispetto alla terna inerziale ( ) e la cui velocità angolare è nulla ( =0), l’equazione precedente si semplifica nella seguente.

Nel sistema di riferimento inerziale è noto poi che , dove è la forza specifica agente sul punto P e l’accelerazione gravitazionale. Sostituendo nell’espressione precedente e definendo il vettore gravità , si ha dunque:

In un sistema INS di tipo strapdown, come già detto, gli accelerometri e i giroscopi sono solidali al corpo, per cui la forza specifica misurata, , è relativa alla terna assi corpo. Essa dovrà essere proiettata sul sistema ECEF tramite la matrice di trasformazione .

13

La matrice di trasformazione cambia con il tempo e le sue componenti possono essere derivate dalle misure di velocità angolare , fornite dai giroscopi, e dalla velocità angolare relativa tra terna ECI ed ECEF.

Il sistema di equazioni differenziali che costituisce l’insieme delle equazioni di navigazione, espresso nella terna ECEF è dunque:

Il vettore è dunque il vettore velocità relativa rispetto a terra, espresso nella terna ECEF.

Per il caso di navigazione terrestre, esso può essere convenientemente espresso nella terna NDE, scrivendo:

La sua derivata è calcolata immediatamente con la legge di Coriolis:

Il sistema di equazioni differenziali ottenuto [15,16] è dunque

Dove, ricordando che rappresenta la latitudine e la longitudine del punto

Per scrivere il sistema in forma scalare è necessario ricorrere, per le equazioni di assetto, al legame che intercorre tra il rateo degli angoli di Eulero e le componenti di velocità

14

angolare tra terna assi corpo e terna verticale locale, . Per la derivazione completa di questa relazione si faccia riferimento all’Appendice 2.

Il seguente schema riporta la classica meccanizzazione dell’equazioni di navigazione.

15

Le prestazioni del sistema di navigazione inerziale sono soggette ad un errore variabile nel tempo, il cui rateo di crescita è dominato dall’accuratezza con cui le condizioni iniziali sono fornite all’INS, dagli errori di cui sono affetti i sensori che costituiscono l’IMU, dalla dinamica seguita dal corpo ed infine dagli errori di integrazione propri del software di calcolo.

3.2 GPS

I sistemi satellitare forniscono, con elevata accuratezza, la posizione e la velocità di un corpo il quale, dotato di un’antenna ed un ricevitore, si muova sulla Terra o in prossimità di essa. Tra questi ricordiamo, oltre il sistema americano GPS, il sistema russo GLONASS, il sistema europeo Galileo, quello per ora regionale cinese Beidou.

Il sistema preso in considerazione in questa tesi è quello GPS, il quale funziona grazie ad una costellazione di 31 satelliti che orbitano attorno alla Terra e ad un sistema di controllo al suolo. Le orbite dei satelliti sono tali per cui in ogni istante di tempo, almeno sei satelliti sono sempre disponibili per fornire le informazioni all’utente che utilizzi il sistema.

Figura 5: Costellazione di satelliti orbitanti intorno alla Terra

16

La posizione del corpo è calcolata a partire dalla misura della distanza da ciascuno dei satelliti in vista.

L’intervallo di tempo necessario per allineare il segnale radio emesso da ciascun satellite con una replica del medesimo segnale generato nel ricevitore è il tempo impiegato dal segnale stesso per raggiungere il ricevitore; questo dato moltiplicato per la velocità della luce è la distanza dal satellite trasmittente. Il segnale trasmesso è di tipo pseudo-random, il che permette ad ogni satellite di essere identificato in modo univoco.

Le misure di velocità vengono calcolate invece sfruttando l’effetto Doppler, nota che sia la velocità con cui ciascun satellite percorre la propria orbita.

L’accuratezza del dato è chiaramente legata alla sincronizzazione del satellite con il ricevitore GPS; mentre sui trasmettitori sono presenti orologi atomici che garantiscono l’accuratezza necessaria al segnale emesso dal satellite, sui ricevitori vengono utilizzati orologi meno accurati, che comportano un notevole errore nel calcolo della posizione. Nello spazio risulta dunque necessario ricorrere a quattro satelliti, il quarto deputato al calcolo dell’errore dell’orologio del ricevitore.

Altro fattore influente è la conoscenza dell’esatta posizione nello spazio del satellite stesso: è compito delle stazioni al suolo controllare la traiettoria di ciascun satellite ed intervenire con opportune correzioni.

L’errore nella misura fornita dal sistema GPS è dovuta, oltre alle imprecisioni, già citate, degli orologi del ricevitore e di quelli atomici dei satelliti, anche al ritardo dovuto all’attraversamento della ionosfera dei segnali.

17

4. Teoria del Filtro di Kalman

La Teoria del Filtro di Kalman è presentata per la prima volta nel 1960 [1]. L’obiettivo che l’algoritmo di calcolo si pone è quello di ottimizzare la valutazione di un segnale corrotto da rumore, basandosi su misure, anch’esse affette da rumore, provenienti da una fonte esterna ed indipendente.

La teoria del filtro di Kalman è presentata sia in forma discreta che nella corrispettiva forma continua, a seconda di quale sia la formulazione del processo a cui essa fa riferimento. Verrà qui esposta in un primo momento la formulazione discreta e successivamente da essa si ricaverà quella continua.

4.1 Formulazione discreta

Per applicare ad un sistema dinamico la teoria del filtro di Kalman, il processo e l’equazione che regola la misura devono essere modellati nella forma seguente

dove

vettore di stato all’istante ;

vettore degli input deterministici all’istante ; vettore del rumore sugli stati all’istante ; vettore del rumore sulle misure all’istante ; vettore delle misure all’istante ;

matrice di transizione di stato all’istante ;

matrice di connessione tra input deterministici e stati all’istante ; matrice di connessione tra stati e misure all’istante ;

matrice di connessione tra input e misure all’istante .

Per quanto riguarda i rumori e , la teoria si basa sull’ipotesi che entrambi i processi siano Gaussiani, a media nulla e non correlati. Tali ipotesi sono di seguito riportate in formulazione matematica.

18

La simbologia rappresenta la funzione Delta di Kronecker, le cui proprietà sono riportate in Appendice 3. Le matrici e sono rispettivamente legate al rumore sulle misure e sugli stati.

Si assuma, a questo punto, che sia a disposizione una stima iniziale del processo ad un certo istante , basata sulla conoscenza del processo all’istante precedente (stima a priori). Questo valore si indichi con , dove il simbolo “ ” indica stima ed il segno “ ” indica che tale stima è fornita senza l’acquisizione della misura all’istante .

Si definisce quindi un errore relativo alla stima a priori ( ) ed una relativa matrice di covarianza dell’errore ( ), dati da:

Una volta acquisita la misura , essa viene utilizzata per aggiornare la stima dello stato del sistema secondo la seguente relazione, di tipo lineare

dove

è la stima a posteriori dello stato del sistema;

è la matrice dei guadagni di Kalman, che moltiplica quello che in letteratura è noto come residuo.

Il criterio per determinare la matrice dei guadagni è quello di minimizzazione l’errore quadratico medio.

Il primo passo è quello di definire la matrice di covarianza dell’errore basata sulla stima a posteriori dello stato.

Sostituendovi l’espressione della stima a posteriori , si ottiene

19

Il valore di ottimale è quello che minimizza i termini sulla diagonale principale di , in quanto tali termini rappresentano le varianze dell’errore sugli stati stimati del sistema. Il guadagno di Kalman è calcolato uguagliando a zero la derivata di rispetto a .

Sostituendo il valore di trovato in

Per arrivare a tale formulazione è stato necessario essere a conoscenza di e , per cui possiamo immaginare una stessa esigenza anche per lo step successivo, in modo tale da utilizzare in modo efficace la misura .

La predizione del vettore di stato all’istante successivo è data dunque da:

La matrice di covarianza dell’errore associata con è ottenuta a partire dall’espressione della stima dell’errore a priori:

Si hanno adesso a disposizione, all’istante , tutte le quantità necessarie per aggiornare il valore del guadagno di Kalman e la successiva stima a posteriori, secondo lo schema della figura seguente.

20

Figura 6: Algoritmo ricorsivo di Kalman 4.2 Formulazione continua

Per ottenere le equazioni del filtro di Kalman nel continuo, il punto di partenza sono il modello del processo e l’equazione delle misure, stavolta entrambe in formulazione continua [17,18].

Le matrici ed i vettori che compaiono sono i corrispettivi continui della formulazione discreta. Per quanto riguarda le caratteristiche del rumore, anche in questo caso si tratta di processi Gaussiani a media nulla:

La simbologia rappresenta la funzione Delta di Dirac, le cui proprietà sono riportate in Appendice 3.

I parametri Q ed R presenti nelle matrici di covarianza giocano un ruolo analogo ai corrispettivi valori Qk ed Rk della formulazione discreta, ma non hanno i medesimi valori

21

numerici. Questo perché nel caso discreto è indicato l’errore sullo stato del sistema, mentre invece nella formulazione continua l’errore è dato sulla derivata prima dello stato.

Per ottenere la corrispondenza tra discreto e continuo, si riparte dalla definizione della matrice di covarianza del rumore [17,18]:

Si ipotizzi ora che l’intervallo sia sufficientemente piccolo per cui si possa ragionevolmente porre . Svolgendo l’integrale si ottiene dunque

Per quanto riguarda invece la relazione tra e , il punto di partenza è il legame esistente tra la misura della formulazione discreta e la corrispettiva misura continua . Si può pensare alla misura discreta come una media del valore assunto da nell’intervallo .

dove l’ultimo termine è dunque .

La matrice di covarianza dell’errore sulle misure è dunque data da:

Per sufficientemente piccolo, svolgendo l’integrale si ottiene

Per ottenere l’espressione del guadagno di Kalman si sostituisce in un primo momento, nell’espressione di la formulazione di appena ricavata. Notando inoltre che

si ottiene:

22

Nel passaggio dalla formulazione discreta a quella continua, è necessario notare che per , tende a , per cui non è più necessario fare distinzione tra covarianza a priori e a posteriori.

Definito _{, il legame tra continuo e discreto è dato da}

Per quanto invece riguarda l’equazione di covarianza dell’errore, si può scrivere quanto segue:

Approssimando ora , osservando che è dell’ordine di e trascurando tutti gli infinitesimi di ordine superiore ad uno in , l’espressione diviene

Sostituendo l’espressione di e e passando al limite per della forma differenziale, si ottiene l’equazione nota come equazione di Riccati, che regola la propagazione nel tempo della matrice di covarianza P.

Infine, operando di nuovo l’approssimazione , trascurando gli infinitesimi di ordine superiore al primo in e apportando le opportune sostituzioni precedentemente ricavate, l’equazione di stima dello stato diviene:

Il passaggio al limite della forma differenziale porta dunque alla seguente espressione .

23

5. Sistema di navigazione integrata INS/GPS con filtro di Kalman

 elevata accuratezza  basso costo

Tali requisiti sono di base contrastanti, nel senso che l’accuratezza dipende dal grado di precisione delle misure dei sensori e generalmente maggiore accuratezza comporta un maggior costo. In un sistema di navigazione integrato di basso costo questi due aspetti vengono messi d’accordo tra loro, confrontando uno o più output del sistema “principale” con le misure fornite da una fonte esterna ed indipendente in modo da correggere gli errori dei sensori di basso costo. Tale confronto è mediato da opportuni algoritmi, di cui il filtro di Kalman è una delle più diffuse realizzazioni.

5.1 Sistema integrato composto da INS e GPS

L’integrazione tra sistemi differenti è tanto più efficace quanto più essi presentano caratteristiche complementari; è questo il caso dell’integrazione tra sistema INS e GPS. Le caratteristiche in questione riguardano essenzialmente la loro dipendenza o meno da fonti esterne, la grandezza misurata, le caratteristiche dell’errore ed infine la frequenza di aggiornamento.

Infatti mentre l’INS è un sistema autonomo, in grado di fornire assetto, velocità e posizione del corpo con una elevata frequenza di aggiornamento dei dati e con un errore piccolo, ma tendente a divergere col tempo, il GPS fornisce, tramite il segnale trasmesso dai satelliti, posizione e velocità del corpo con una frequenza di aggiornamento minore rispetto all’INS e la cui misura è soggetta ad un rumore più elevato ma costante con il tempo.

Sono ad oggi state sviluppate una serie di architetture di integrazione tra i due sistemi che possono essere raggruppate in quattro classi, caratterizzate da un livello crescente di integrazione:

- Uncoupled systems: in questa architettura il segnale GPS è utilizzato unicamente per resettare ad intervalli di tempo regolari la posizione indicata dall’INS;

- Loosely coupled systems: la posizione e la velocità fornita da INS e GPS sono confrontate e la loro differenza costituisce l’ingresso al filtro di Kalman;

24

- Tightly coupled systems: la differenza con l’architettura precedente è l’ingresso al filtro di Kalman è costituito dalla misura fornita dall’INS e dai segnali non filtrati forniti dal GPS;

- Ultra-tightly coupled systems: in questa architettura la struttura del ricevitore GPS è sostanzialmente differente da quella utilizzata nei sistemi precedenti; i due sistemi non sono più di fatto indipendenti perché il ricevitore GPS sfrutta nel proprio algoritmo di calcolo le informazioni provenienti dall’INS stesso.

Nel lavoro di tesi si è utilizzata un’architettura del tipo loosely coupled system, illustrata nella figura sottostante.

Figura 7: Loosely Coupled System

5.2 Applicazione della teoria del filtro di Kalman al sistema integrato INS/GPS

Quanto esposto finora presuppone che i sistemi dinamici trattati siano lineari. Nel caso ci si trovi, come nel caso delle equazioni di navigazione, davanti a processi non lineari, per poter applicare la teoria del filtro di Kalman è necessario linearizzare il sistema.

Prima di procedere con l’operazione di linearizzazione, è necessario avere confidenza con il significato di traiettoria nominale. Dato dunque un qualunque sistema dinamico, definito nel continuo, del tipo

25

si definisce traiettoria la soluzione di tale sistema, l’aggettivo nominale fa riferimento alla situazione in cui le variabili presenti assumono il valore atteso. D’ora in avanti la traiettoria nominale sarà indicata come .

La traiettoria attuale si trova, nei confronti della traiettoria nominale, nella seguente relazione:

Come mostrato nella figura sottostante, rappresenta la differenza tra le due traiettorie ad un dato istante, ed è chiamata perturbazione.

Figura 8: Traiettoria nominale ed attuale per un filtro di Kalman linearizzato

In modo analogo, si definisca per il vettore degli input:

L’equazione del sistema può essere dunque riscritta, omettendo l’indicazione della dipendenza esplicita dal tempo, come

Sotto l’ipotesi che e siano piccoli, la precedente espressione può essere scritta utilizzando lo sviluppo di Taylor della funzione , troncato al primo ordine.

26 Dove

Essendo , l’equazione precedente diventa dunque:

Essa descrive l’andamento, nel tempo, della perturbazione dello stato rispetto alla traiettoria nominale.

Nel caso in cui si volessero sfruttare le equazioni discrete del filtro di Kalman, il passo successivo è la discretizzazione del sistema lineare ricavato; si giunge dunque alla seguente formulazione

Dove, essendo T l’intervallo di campionamento del segnale continuo, supposto sufficientemente piccolo:

E’ bene precisare che la scelta della traiettoria nominale, ovvero la scelta dello stato del sistema attorno a cui andare a linearizzare il sistema non lineare, porta ad una importante distinzione tra le possibili architetture del filtro. Si distingue infatti tra:

- LKF: Linearized Kalman Filter, nel caso in cui la linearizzazione avvenga nell’intorno di una traiettoria indipendente dalla misura esterna fornita dal GPS; - EKF: Extended Kalman Filter, nel caso in cui la linearizzazione avvenga

nell’intorno di una traiettoria che viene continuamente aggiornata tramite la misura del GPS.

27

Figura 9: Traiettoria attuale e stimata in un EKF

Risulta chiaro che nel primo caso il vantaggio è quello di poter effettuare il calcolo delle matrici di stato e dei guadagni di Kalman off-line; d’altro canto l’EKF garantisce una maggiore robustezza nei confronti degli errori derivanti dalla linearizzazione stessa ma vede aumentare il costo computazionale a causa della necessità di aggiornare in tempo reale i parametri del filtro.

5.3 EKF: Linearizzazione delle equazioni di navigazione

Prima di procedere alla linearizzazione delle equazioni di navigazione, sfruttando la teoria delle piccole perturbazioni, è necessario definire le variabili di stato ed il vettore degli input.

Nel vettore di stato, come era prevedibile, sono presenti le tre componenti di velocità in direzione nord, est e down, latitudine, longitudine, quota ed infine gli angoli di rollio, beccheggio ed imbardata. Per quanto riguarda il vettore degli input, esso è costituito dagli output dell’IMU.

28

Linearizzare il sistema si traduce, come esposto nel capitolo precedente, nel trovare le matrici F e G.

A partire dal sistema scritto in forma scalare, per il calcolo delle derivate parziali è necessario esplicitare il legame esistente tra le componenti dei vettori forza specifica e velocità angolare nella terna verticale locale e nella terna assi corpo.

Le componenti delle matrici F e G sono dunque

29

Segue il calcolo di ciascuna componente delle matrici.

5.3.1 Linearizzazione delle equazioni di velocità

30

5.3.2 Linearizzazione delle equazioni di posizione

31

5.3.3 Linearizzazione delle equazioni di assetto

32

33

Molte delle espressioni precedenti si semplificano tenendo conto delle derivate del vettore

34

Anche in questo caso è necessario tenere conto delle espressioni delle velocità angolari in forma esplicita.

35 Le semplificazioni da effettuare tenendo conto della derivata del vettore

36 Dove

37

38 In cui infine 5.3.4 Matrice G

39

5.3.2 Semplificazioni sotto le ipotesi di Terra piana e non rotante

Le simulazioni che si pensa di effettuare sono spazialmente e temporalmente limitate, per cui nell’implementare le equazioni di navigazione, si è operata la semplificazione di considerare la terra piana ( e non rotante ( .

Questa assunzione porta a notevoli semplificazioni nel sistema non lineare delle equazioni di navigazione.

40

Nel sistema le coordinate di latitudine e longitudine sono state sostituite con le posizioni Nord ed Est. Le velocità angolari che compaiono nel sistema sono in questo caso quella misurate dai giroscopi e non devono essere corrette.

Come diretta conseguenza, anche le matrici che costituiscono il sistema linearizzato si semplificano ed assumono la forma seguente.

Dove

41

6. Sviluppo del sistema integrato INS/GPS e simulazioni numeriche

Oltre alla già citata differenza tra LKF ed EKF ed alla possibilità di utilizzare una formulazione continua o discreta dell’algoritmo di Kalman, una terza ma altrettanto importante distinzione riguarda le componenti del vettore di stato del sistema interessato dalla correzione del filtro: in letteratura ci si riferisce ad un’architettura a formulazione diretta (o total state) quando le variabili presenti nel vettore di stato sono gli stati stessi del sistema (posizione, velocità ed assetto); si parla invece di formulazione indiretta (o error

state) quando le variabili di stato sono scritte in termini di errore.

Nel caso di formulazione di tipo error state il vettore di stato è dunque nella forma

Si può facilmente verificare che le matrici che regolano la dinamica con cui l’errore evolve nel tempo sono le stesse matrici Jacobiane già calcolate nel capitolo precedente. Le perturbazioni sugli stati e sugli input e rappresentano in questo caso gli errori e .

Nel caso di formulazione del tipo error state, è possibile aggiungere allo stato del sistema sei ulteriori variabili, ovvero i bias e i drift di cui sono affetti i sensori dell’IMU.

L’equazione della dinamica diviene

L’informazione riguardante la stima degli errori dei sensori presenti nell’IMU può essere retroazionata e correggere a monte il dato proveniente da accelerometri e giroscopi.

In questo lavoro di tesi si è scelto di sviluppare il sistema integrato INS/GPS con le seguenti architetture del filtro:

- EKF discreto con formulazione di tipo total state; - EKF discreto con formulazione di tipo error state; - EKF continuo con formulazione di tipo error state.

42

Il grande vantaggio nell’utilizzare una formulazione discreta è la riduzione dei costi computazionali: quella che sarebbe un’operazione di integrazione da svolgere con un metodo di integrazione numerico che richiede l’esistenza di una libreria come quella presente in MATLAB, si trasforma in una operazione tra matrici, che prevede costi dunque minori in termini di software da installare sull’eventuale hardware.

Per poter testare le performance dei sistemi sviluppati, il primo passo è stato quello di ricreare, grazie agli strumenti della simulazione numerica, il comportamento dei due sistemi INS e GPS in assenza di integrazione; successivamente per ciascuna delle architetture sopra elencate, si è validato il modello linearizzato ed infine si è operato il

setting del filtro tramite simulazioni numeriche, assegnando storie di moto arbitrarie ai

segnali dell’IMU.

6.1 INS

Per quanto riguarda l’INS, si è trattato di sviluppare un modello che rispecchiasse il comportamento dell’IMU, che fornisse cioè l’output dei sensori che la compongono in presenza di una accelerazione e/o una velocità angolare del corpo ed infine implementare l’algoritmo di calcolo che a partire da tali output, fosse in grado di risolvere le equazioni di navigazione e fornire lo stato del sistema.

Per quanto riguarda gli accelerometri, è necessario considerare che la misura fornita in output tiene conto della presenza della gravità; la figura 10 mostra come variano i valori del segnale sui tre assi sensibili in dipendenza dal posizionamento del sistema nello spazio.

43

Figura 10: Dipendenza dell’output dal posizionamento della piattaforma nello spazio

La forza specifica misurata dall’accelerometro è dunque la somma di quella dovuta all’accelerazione inerziale più un contributo dovuto alla gravità, che tiene conto dell’orientamento del corpo e dunque del posizionamento della terna assi corpo rispetto a quella verticale locale.

Tale contributo è stato calcolato in fase di simulazione proiettando il vettore gravità sulla terna assi corpo tramite gli angoli di Eulero ideali, ovvero quei valori che si otterrebbero integrando le equazioni non lineari di navigazione in cui i segnali di accelerometri e giroscopi sono quelli ideali, cioè non sono affetti da alcun tipo di errore.

44

L’output effettivo del blocco IMU è a questo punto ottenuto come la somma del segnale ideale (in figura 11 indicati come ACCELERAZIONI LINEARI REALI e VELOCITA’ ANGOLARI REALI) con un contributo costante rappresentante il bias (o il drift in caso di velocità angolare) ed un segnale random rappresentante il contributo del rumore.

Figura 12: Output IMU

Per quanto riguarda le caratteristiche di tale errore, i valori inseriti sono quelli dichiarati dalla ditta SBG per il sistema integrato IG-500N con EKF, riportati nelle tabelle sottostanti.

BIAS accelerometri 0.5 mg DRIFT giroscopi 0.05 deg/s

Tabella 1: bias e drift IMU

0.005m/s2 0.001m/s2 0.015m/s2 0.11deg/s 0.08deg/s 0.05deg/s

Tabella 2: Deviazioni standard sul rumore dei segnali IMU

Figura 13: Sistema IG-500N con EKF 6.2 INS in assenza di navigazione integrata

45

Come già detto nei paragrafi precedenti, gli errori a cui è soggetto l’INS fa sì che le informazioni rese disponibili da esso risultino inaffidabili già dopo poco tempo dall’accensione.

Figura 14: Schema Simulink INS

Per avere un ordine di grandezza di tali errori, sono state fatte diverse simulazioni assegnando arbitrariamente dei segnali ai valori ideali di accelerazione e velocità angolare dell’IMU. Le caratteristiche di durata del segnale sono mostrate nella figura sottostante, mentre l’intensità è 10m/s2

sugli accelerometri e 5deg/s sui giroscopi.

Figura 15: Segnale in input dato nelle simulazioni numeriche

I grafici nelle pagine seguenti mostrano l’errore dell’INS rispetto allo stato ideale, quello che sarebbe cioè il risultato dell’integrazione delle equazioni non lineari di navigazione in assenza di errore sull’IMU.

46

Figura 16: Errore dell’INS sulla posizione

47

Figura 18: Errore dell’INS sull’assetto

6.3 GPS

In modo analogo a quanto già fatto per l’IMU, il segnale GPS è la somma dello stato ideale e di un rumore, rappresentante l’errore random a cui tale sistema è soggetto. Come detto in precedenza, la frequenza di lavoro dell’INS è maggiore rispetto a quella del GPS; di tale differenza si è tenuto conto campionando il segnale proveniente dal blocco in questione ad una frequenza pari a 10Hz.

Figura 19: Schema Simulink del segnale GPS

Per quanto riguarda le caratteristiche del rumore bianco da cui è affetto il GPS, i valori di deviazione standard sono stati scelti effettuando delle prove in automobile e registrando tramite la piattaforma SBG i dati filtrati e quelli grezzi del GPS.

48

Per quanto riguarda la velocità, la piattaforma rende disponibile il dato filtrato sulla terna assi-corpo, per cui una volta espresso il vettore in termini di velocità rispetto alla terna NDE tramite la matrice di rotazione , si è potuto riscontrare un errore del GPS dell’ordine di 1 m/s.

Per le posizioni invece, considerato che il GPS normalmente fornisce le informazioni di posizione in termine di latitudine, longitudine e quota, si è utilizzata la seguente approssimazione per ottenere l’errore in termini di posizione Nord ed Est:

L’ordine di grandezza dell’errore sulla posizione è risultato essere di 1 m circa. I valori ipotizzati sono stati poi verificati andando a visualizzare su Google Earth il percorso tracciato con i dati filtrati e quello tracciato con le informazioni provenienti dal GPS. Tale verifica ha confermato l’ordine di grandezza calcolato.

I dati appena discussi sono sintetizzati in tabella.

1m/s 1m/s 1m/s 1 m 1 m 1 m

Tabella 3: Deviazione standard del segnale GPS 6.4 Filtro di Kalman

Una volta costruiti i modelli dei due sistemi, si procede con l’implementazione del filtro nelle differenti architetture.

Per ognuna di esse, il procedimento è il seguente: - Validazione del modello linearizzato sviluppato;

- Regolazione del filtro attraverso simulazioni numeriche con parametri riportati in tabella.

49 - Parametri simulazione Frequenza di aggiornament o IMU 100Hz Frequenza di aggiornament o GPS 10Hz Durata simulazione 1000s Input su accelerometri e giroscopi

Tabella 4: Parametri di simulazione 6.4.1 EKF discreto con formulazione di tipo total state

Figura 20: Sistema integrato INS/GPS con EKF discreto con formulazione total state

Il filtro, come si vede dalla figura, è composto di due blocchi, uno dedicato alla stima a priori dello stato tramite il modello linearizzato, ed uno dedicato alla correzione vera e propria ad opera del guadagno di Kalman.

50

Se al posto di un EKF fosse stato sviluppato un LKF, come input al blocco MODELLO

LINEARIZZATO ci sarebbe stata la stima a priori dello stato, come indicato nella figura

sottostante.

Figura 21: Sistema integrato INS/GPS con LKF

Lo stato ad un determinato istante è dato dunque dalla somma della traiettoria nominale con la perturbazione; dalla teoria è noto che quanto più piccola è la perturbazione e tanto meglio il modello lineare approssima quello non lineare.

Nel calcolare lo stato tramite il modello linearizzato, è necessario fare attenzione al contenuto dei segnali provenienti dall’IMU: come detto in precedenza, essi contengono anche il contributo dovuto alla gravità; tale contributo è sottratto, nel sistema di equazioni non lineari, nell’espressione che regola l’andamento della , in cui alla forza specifica in direzione down viene sottratta l’accelerazione di gravità g.

Nel modello linearizzato il contributo della gravità deve essere sottratto a monte e questo è possibile ottenerlo scomponendo il vettore sulla terna assi corpo tramite la matrice di trasformazione e sottraendo le componenti così ricavate da quelle di forza specifica fornite dall’IMU. Le componenti della matrice sono calcolate utilizzando gli angoli di assetto ricavati dallo stato filtrato, coerentemente con l’architettura EKF.

La validazione del modello linearizzato è stata effettuata tramite una simulazione in cui, a parità di input ed in assenza di errori sull’IMU, si fa un confronto tra gli stati ottenuti integrando il sistema di equazioni non lineare e quelli ottenuti dal modello linearizzato. Nelle figure seguenti è rappresentato l’errore che il modello linerizzato commette rispetto a quello non lineare.

51

Figura 22: Errore sulla posizione del modello linearizzato rispetto al modello non lineare

52

Figura 24: Errore sulla velocità del modello linearizzato rispetto a quello non lineare

Su una simulazione di 100 secondi, si vede la buona accuratezza del modello, che risulta però essere fortemente legata all’intervallo di integrazione: quanto più esso aumenta e tanto più vengono meno le ipotesi alla base della teoria delle piccole perturbazioni ed il modello mostra allora notevoli differenze, soprattutto in termini di posizione.

Le figure precedenti fanno riferimento ad un intervallo di integrazione pari a 0.001, quelle che seguono sono invece ricavate per un intervallo di integrazione pari a 0.01.

53

Figura 26: Errore sull’assetto del modello linearizzato rispetto a quello non lineare

54

Una volta validato il modello linearizzato, è stato introdotto il filtro e la sua correzione. Tramite simulazione numerica è stata effettuata dunque l’operazione di setting del filtro, che consiste nel regolare gli elementi delle matrici ed , la cui forma è diagonale nell’ipotesi che i disturbi presenti siano non correlati. I valori numerici ottimali trovati sono riportati qui di seguito.

I risultati della simulazione e il confronto delle prestazioni del sistema integrato rispetto al solo INS sono riportati nelle figure alle pagine seguenti.

55

Figura 29: Errore sulla posizione

56

Figura 31: Confronto errore di velocità INS e EKF

57

Figura 33: Confronto errore di assetto INS e KF

E’ necessario osservare che, con la sola misura di posizione e velocità fornita dal GPS, non è possibile correggere l’errore sul valore dell’angolo di imbardata , per il quale sarebbe necessario introdurre come fonte esterna di misura il segnale di un magnetometro.

In questa prima fase di simulazione dunque, il sistema integrato e mediato dal filtro sembra apportare notevoli miglioramenti rispetto al solo INS; il comportamento dell’errore è però differente per le varie grandezze: mentre quello sull’assetto, seppur con una dinamica abbastanza lenta tende a ridursi con il trascorrere del tempo, l’errore di velocità rimane costante ed ancora quello in posizione tende a divergere. Sembrerebbe dunque necessario, almeno per quanto riguarda le posizioni, effettuare un reset per evitare che le prestazioni degradino eccessivamente.

58

6.4.2 EKF discreto con formulazione di tipo error state

Figura 34: Sistema integrato INS/GPS con EKF discreto con formulazione di tipo error state

A differenza dell’architettura con formulazione di tipo total state precedentemente sviluppata, nella formulazione di tipo error state ad essere interessati dalla correzione del filtro non sono le quantità totali di posizione, velocità e assetto, ma gli errori commessi sulla loro stima ed eventualmente anche gli errori bias e drift dei sensori dell’IMU.

Come già osservato in precedenza, per ottenere le equazioni che descrivono la dinamica con cui evolve l’errore, il processo matematico da seguire è formalmente uguale a quello di linearizzazione, dove la perturbazione deve essere interpretata come un errore.

E’ necessario, prima di proseguire con la trattazione, fare due importanti precisazioni. La prima riguarda il trattamento dei dati provenienti dall’IMU: mentre nella formulazione total state veniva sottratto il contributo dovuto alla gravità, nella formulazione error state quest’operazione non deve essere effettuata e ciò può essere compreso se si considera che in tal modo si perderebbe l’errore commesso sulle componenti del vettore gravità sulla terna assi corpo, legato dunque all’errore sull’assetto.

La seconda osservazione riguarda invece le equazioni del filtro: la teoria di Kalman riporta la seguente espressione per la stima a posteriori dello stato del sistema.

Nel caso di formulazione error state, la formulazione diviene

59

Se però ad essere considerata è l’architettura EKF, il modello dell’errore viene ricavato rispetto ad uno stato , ovvero la stima a posteriori dell’istante precedente, che si assume essere sufficientemente affidabile da poter porre l’errore su di essa nullo.

Le equazioni del filtro si semplificano ponendo a zero la stima a priori dell’errore e divengono semplicemente:

Come nel caso della formulazione total state, anche in questo caso il primo passo è stato quello di verificare il modello sulla dinamica dell’errore sviluppata.

Per farlo si confronta il modello dell’errore a priori con l’errore calcolato come differenza tra lo stato dell’INS in assenza di navigazione integrata, ottenuto dalla risoluzione delle equazioni non lineari di navigazione, e lo stato ideale del sistema. Nella simulazione effettuata la correzione dovuta al filtro viene azzerata imponendo nulla la matrice dei guadagni di Kalman e sono messe inoltre a zero anche le componenti costanti del rumore (bias e drift dei sensori).

I due modelli messi a confronto sono dunque i seguenti

, errore a priori predetto dal modello lineare , errore reale.

60

Figura 35: Evoluzione dinamica errore in velocità del modello non lineare e linearizzato

61

Figura 37: Evoluzione dinamica dell’errore sull’assetto del modello non lineare e linearizzato

Anche in questo caso, ciò che è emerso è che il modello dell’errore risulta essere tanto più accurato quanto più si rimane fedeli alle ipotesi alla base della teoria delle piccole perturbazioni; quindi quanto più è piccolo l’intervallo tra due istanti di campionamento e quanto più piccole sono le perturbazioni a cui è soggetto il sistema.

Le componenti delle matrici ed sono, restando fedeli alla teoria nel caso di formulazione error state, i valori delle deviazioni standard attribuite ai rumori sugli stati e sulle misure, per cui sembrerebbe essere sufficiente considerare le caratteristiche dell’errore inserite nell’IMU e trovare i corrispettivi valori discreti.

Durante la fase di settaggio del filtro ciò che è emerso è stato un comportamento divergente del filtro, per cui è stato necessario regolare manualmente i valori delle matrici ed . Inoltre il migliore comportamento di questa architettura si ha quando si escludono dal vettore di stato le stime sui bias e drift; i risultati delle pagine seguenti e i valori delle matrici ed fanno dunque riferimento ad un’architettura in cui il vettore di stato non comprende la stima di bias e drift.

62

Figura 38: Errore sulla velocità

63

Figura 40: errore sull’assetto

Così come era per l’architettura total state, anche in questo caso l’errore tende a divergere, seppur più lentamente. L’errore sulla quota risulta più rilevante e sembrerebbe esservi la necessità di resettare il filtro per assicurare il mantenimento di buone prestazioni a lungo termine. Si può attribuire questo comportamento ai limiti del modello lineare, che combinati con una dinamica fortemente non lineare, non forniscono risultati accurati a sufficienza.

64

6.4.3 EKF continuo con formulazione di tipo error state

Figura 41: Sistema integrato INS/GPS con EKF continuo con formulazione di tipo error state

L’architettura del filtro è del tutto analoga a quella discreta, con la differenza che l’uscita del filtro è in forma derivativa.

Nelle figure seguenti i risultati delle simulazioni effettuate per validare il modello linearizzato di errore e successivamente i risultati della correzione del filtro.

65

Figura 42: Evoluzione dinamica errore sulla posizione del modello non lineare e linearizzato