Universita degli studi di ParmaDipartimento dell’Ingegneria dell’Informazione

Anno accademico 2013-2014

Elettronica dei Sistemi Embedded

Implementazione del filtro di Kalman

Studenti:Arcola CristianPignoloni DavideVisioli Daniel

Sommario

Lista delle figure 4

1 Filtro di Kalman 61.1 Introduzione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61.2 Filtro di Kalman: trattazione matematica . . . . . . . . . . . . . . . . . . . . . . 71.3 Algoritmo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91.4 Implementazione in C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101.5 Implementazione in Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141.6 Quantizzazione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 191.7 Quantizzazione in Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201.8 Considerazioni sui risultati ottenuti . . . . . . . . . . . . . . . . . . . . . . . . . . 211.9 TCP-IP con Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

2 Verilog 242.1 kalman ALU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 252.2 kalman control unit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

2.2.1 Macchina a stati: dettagli . . . . . . . . . . . . . . . . . . . . . . . . . . . 272.2.2 Simulazioni . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

2.3 nios . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 302.3.1 Codice C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 312.3.2 Receive Measures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 312.3.3 Evaluate Measures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 322.3.4 Transmit Measures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

2.4 UDP-IP Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

3 Risultati Finali e conclusioni 35

2

Lista delle figure

1.1 Il susseguirsi delle fasi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91.2 Flowchart del filtro di Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101.3 Generazione del rumore . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111.4 Aggiunta del rumore alla misura ideale . . . . . . . . . . . . . . . . . . . . . . . . 111.5 Inizializzazione delle variabili . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111.6 Time update e measurement update . . . . . . . . . . . . . . . . . . . . . . . . . . 121.7 Algoritmo della media, dello stato del sistema e dell’errore commesso . . . . . . . 121.8 Istruzioni di stampa dei parametri . . . . . . . . . . . . . . . . . . . . . . . . . . 121.9 Risultati . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131.10 Generazione segnali Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141.11 Aggiunta del rumore Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151.12 Inizializzazione Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151.13 Implementazione e statistiche Matlab . . . . . . . . . . . . . . . . . . . . . . . . . 161.14 Matlab: valutazione di segnale sinusoidale . . . . . . . . . . . . . . . . . . . . . . 171.15 Matlab: valutazione di segnale a gradino . . . . . . . . . . . . . . . . . . . . . . . 171.16 Matlab: valutazione di segnale ad onda quadra . . . . . . . . . . . . . . . . . . . 181.17 Matlab: modello di traiettoria balistica di un proiettile in assenza di attrito con

l’aria . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181.18 Diagramma a blocchi dopo la quantizzazione . . . . . . . . . . . . . . . . . . . . 201.19 Quantizzazione: inizializzazione . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201.20 Quantizzazione: algoritmo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 211.21 Confronto fra covarianze . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 211.22 Discrepanza fra i risultati . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 221.23 Matlab: server TCP-IP codice sorgente . . . . . . . . . . . . . . . . . . . . . . . . 231.24 Matlab: client TCP-IP codice sorgente . . . . . . . . . . . . . . . . . . . . . . . . 23

2.1 Verilog : implementazione ALU, codice sorgente . . . . . . . . . . . . . . . . . . . 242.2 Verilog : dichiarazione kalman ALU . . . . . . . . . . . . . . . . . . . . . . . . . . 252.3 Verilog : implementazione calcoli . . . . . . . . . . . . . . . . . . . . . . . . . . . 252.4 Verilog : algoritmo macchina a stati . . . . . . . . . . . . . . . . . . . . . . . . . . 262.5 Macchina a stati . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 272.6 Reset del contatore count . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 272.7 Testbench . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 282.8 Simulazioni ModelSim . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 282.9 Simulazioni ModelSim: attesa del risultato corretto . . . . . . . . . . . . . . . . . 292.10 Filtro di Kalman: rete RTL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 292.11 Verilog : NIOS II . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 302.12 Qsys: NIOS II, architettura microprocessore . . . . . . . . . . . . . . . . . . . . . 302.13 Sorgente C: ricezione del pacchetto UDP-IP . . . . . . . . . . . . . . . . . . . . . 312.14 Sorgente C: elaborazione misure . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

3

2.15 Sorgente C: inizializzazione pacchetto UDP-IP . . . . . . . . . . . . . . . . . . . 332.16 Sorgente C: trasmissione del pacchetto UDP-IP . . . . . . . . . . . . . . . . . . . 332.17 Inizializzazione UDP-IP . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

3.1 Risultati: grandezza costante . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353.2 Risultati: grandezza sinusoidale . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363.3 Risultati: grandezza sinusoidale . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

Obbiettivo

L’obbiettivo di questo progetto e la realizzazione del filtro di Kalman implementandolo in unaarchitettura FPGA attraverso il linguaggio di programmazione hardware Verilog. Nel dettaglio,il progetto realizzato permette, tramite script di Matlab, l’invio attraverso il protocollo di co-municazione TCP-IP ( e quindi attraverso la porta ethernet) le misure effettuate di uno statodi un processo conosciuto, affette da rumore, alla DemoBoard DE2 su cui e implementato ilfiltro. Il compito di tale filtro e il calcolo dei valori della stima dello stato e della covarianza.Tali dati verranno inviati nuovamente a Matlab, attraverso il protocollo UDP-IP apposito, chegrafichera i risultati. Si e scelto di svolgere questo tipo di attivita perche si e notato come questotipo di filtro occupera sempre piu una posizione di riferimento in vari ambiti tra questi quellodella robotica nonche la balistica e l’ingegneria aereo spaziale per il quale ha avuto la sua primaapplicazione.

5

Capitolo 1

Filtro di Kalman

1.1 Introduzione

Il filtro di Kalman porta il nome del suo inventore, l’ingegnere elettrico e matematico RudolfEmil Kalman che pubblico una primo approccio a questa tecnica nel 1960, sebbene in precedenzaaltri matematici svilupparono algoritmi simili.Durante la sua visita al NASA Ames Reserch Center di Kalman, Schmidt vide la possibilitadi applicare l’idea di Kalman sul programma Apollo che in quei anni si stava sviluppando. Difatto, il filtro di Kalman venne incorporato sul computer di navigazione di ogni navicella delprogramma Apollo.Dopo questa prima applicazione, il filtro di Kalman ebbe un ruolo vitale in campo militare,nell’implementazione dei sistemi di navigazione dei missili a testata nucleare dei sottomarini bal-istici di flotta US Navy, dei sistemi di guida e navigazione dei missili Tomahawk e Air LaunchedCruis Missile. In ambito civile e stato ed e tuttora utilizzato per il controllo di guida e nav-igazione dei Space Shuttle NASA e nel attitude control e di navigazione della stazione orbitanteISS.Per questa trattazione, Rudolf Emil Kalman e stato insignito della medaglia National Medal ofScience direttamente dal presidente americano in carica Barak Obama nel 2008.

6

1.2 Filtro di Kalman : trattazione matematica

Il filtro di Kalman non e propriamente un filtro ma e un algoritmo ricorsivo che permette distimare lo stato di un sistema dinamico lineare e nella versione piu estesa anche non lineare,perturbato da rumore. Tale filtro richiama il problema piu generale della stima dello statox ∈ Rn di un processo a tempo discreto che e governato da equazioni differenziali stocastiche

xk+1 = Akxk +Bkuk + wk (1.1)

dove:

• A e una matrice nxn che lega lo stato al tempo k con lo stato k+1, in assenza di di funzionidi comando (input) e di rumore di processo.

• B e la matrice nxl che lega l’input u ∈ Rl con lo stato x.

La stima viene effettuata per mezzo di successive misurazioni z ∈ Rm tali che

zk = Hkxk + vk (1.2)

dove: la matrice nxn H lega lo stato alla misurazione zk. Le variabili casuali wk e vk rappresen-tano rispettivamente il rumore di processo e di misurazione. Essi sono assunti come indipendenti,bianchi e con distribuzione di probabilita normale con valor medio nullo e varianza pari rispet-tivamente a Q ed R:

p(w) ∼ N (0, Q) (1.3)

p(v) ∼ N (0, R) (1.4)

Passaggio seguente e la definizione x′k ∈ Rn come la stima della stato eseguita a priori, ovvero al

passo k data la conoscenza del processo prima dello step k e senza, che sia nota la misurazionezk.Si definisce, inoltre, xk ∈ Rn come la stima dello stato eseguita a posteriori, cioe al passo k unavolta nota la misurazione zk.In questo momento si introduce l’errore di stima a priori e a posteriori come:

e−k ≡ xk − x−k (1.5)

e−k ≡ xk − xk (1.6)

Di cui l’equazione 1.5 esprime la capacita di prevedere mentre la 1.6 esprime la capacita di stimaa posteriori.Ora e possibile calcolare la covarianza dell’errore stimata a priori:

P−k = E[e−k e

−Tk ] (1.7)

e la covarianza dell’errore stimata a posteriori

P−k = E[e−k e

Tk ] (1.8)

Ricapitolando:

• xk stato reale che si vuole stimare

• zk misurazione rilevata dallo strumento ( affetta da rumore)

• xk stima a posteriori una volta nota la misurazione zk

7

• x−k stima a priori prima che sia nota la misurazione zk

• e−k errore stima a priori

• ek errore stima a posteriori

• P−k covarianza errore a priori

• Pk covarianza errore a posteriori

In questo momento si procede ricercando una equazione che computi una stima a posteriorixk come una combinazione lineare di un x−k stimato a priori e di una differenza pesata tra lamisurazione attuale zk ed una previsione per la misurazione Hkxk. La stima a posteriori risultapertanto:

xk = x−k +K(zk −Hkxk) (1.9)

La differenza (zk − Hkxk) nella 1.9 e detta innovazione di misura o residuo. Essa riflette ladiscrepanza tra previsione di misura Hkxk e la misurazione attuale zk.Un residuo nullo implica che le due quantita sono in perfetto accordo.Importante e la matrice nxm K della guadagno di Kalman o blending factor, nell’equazione 1.9,che si determina in modo da minimizzare la covarianza dell’errore a posteriori indicata nellaequazione 1.8Questa minimizzazione puo essere effettuata sostituendo l’equazione 1.9 nella definizione di ek,introducendo la relazione cosı ricavata nella 1.8, derivando rispetto K, eguagliando a zero edinfine risolvendo rispetto a K.Esistono vari modo per dare una formula a guadagno di Kalman. Una possibile forma di questoK e la seguente:

Kk =P−k H

Tk

(HkP−k H

Tk +Rk)

(1.10)

Come si puo osservare nell’equazione 1.10, al tendere a zero della covarianza dell’errore Rk, ilguadagno K pesa il residuo in modo via via piu ingente. Specificamente:

limRk−>0

Kk =1

Hk(1.11)

Con semplici operazioni matriciali e possibile esprimere il guadagno di Kalman in una formaleggermente differente:

Kk =PkHk

Rk(1.12)

dove:

Pk =Pk

(I +HTk R

−1HkP−k )

(1.13)

Se Hk = 1 e immediato che

Kk =Pk

Rk(1.14)

da cui si puo osservare che:

• tanto maggiore e l’errore che si e commesso nella stima precedente e tanto maggiore el’affinamento della stima attuale (guadagno) indotto dal filtraggio;

• tanto meno le misurazioni si discostano dal valore reale, tanto piu sara possibile ottenerestime piu precise.

8

1.3 Algoritmo

L’algoritmo, che consiste nello stimare un processo rumoroso in base ad uno stato passato eottenere un feedback sotto forma di misura affetta da rumore, e costituito da una sequenza dioperazioni da eseguirsi ricorsivamente.Queste operazioni si dividono in due gruppi: equazioni di time update (previsione) e equazionidi measurement update (correzione).Nel dettaglio, i passi svolti dalle equazioni di time update sono:

• Stima a priori di stato: lo stato attuale viene stimato sulla base dello stato all’istanteprecedente e degli ingressi.

• Calcolo a priori della covarianza di errore di stima.

I passi considerati nelle equazioni di measurement update sono:

• Calcolo del guadagno di Kalman.

• Calcolo a posteriori dello stato attuale sulla base della stima a priori e della misura.

• Calcolo della covarianza a posteriori.

In figura 1.1 mostra il susseguirsi delle fasi.

Figura 1.1: Il susseguirsi delle fasi

Ii pedici indicano:

• apri = valore calcolato a priori della nuova misura.

• pos = posteriori.

• measure = misurata.

La scelta di Q ed R impone una spiegazione piu accurata: queste due varianze riguardano laconoscenza del processo che si sta analizzando e del sistema di misura. La regolazione di questiparametri puo incidere sull’efficienza del filtro e tipicamente e effettuata avvalendosi di altri filtridi Kalman o comunque all’esterno dell’algoritmo che stima lo stato del processo.Si puo comunque dimostrare che, scelti opportunamente i valori di Q ed R nell’ambito in cuiquesti rimangano costanti, sia la covarianza di stima P che il guadagno di Kalman K si stabi-lizzano in un certo numero di cicli per poi rimanere costanti.

9

1.4 Implementazione in C

Per testare il funzionamento dell’algoritmo si e effettuata una semplice implementazione inlinguaggio “C ” che confronta l’algoritmo di Kalman con la media statistica. Il flowchart imple-mentato e mostrato in figura 1.11:

Figura 1.2: Flowchart del filtro di Kalman

10

Si vuole stimare l’andamento a gradino (zrealt=0− = 0.5 → zrealt=0+ = 30) e si sporcano lemisure aggiungendo alla misura ideale, un valore generato casualmente (rumore uniformementedistribuito):

Figura 1.3: Generazione del rumore

Figura 1.4: Aggiunta del rumore alla misura ideale

All’interno del corpo del programma, vi e la definizione delle variabili e la loro inizializzazionecome mostrato in figura 1.5:

Figura 1.5: Inizializzazione delle variabili

11

Il Passo successivo e l’implementazione delle fasi di time update e measurement update:

Figura 1.6: Time update e measurement update

L’algoritmo della media, l’aggiornamento dello stato del sistema e dell’errore commesso dai duealgoritmi per poterlo confrontare:

Figura 1.7: Algoritmo della media, dello stato del sistema e dell’errore commesso

Si eseguono un certo numero di iterazioni controllate da un ciclo for che permettono lo switchdella variabile zreal e, al termine di esse, si puo valutare quale delle due tecniche di stima avradato il risultato migliore stampando i vari parametri di interesse:

Figura 1.8: Istruzioni di stampa dei parametri

12

Figura 1.9: Risultati

Come riportato dai risultati in figura 1.9, l’utilizzo del filtro di Kalman riduce in modosostanziale l’errore relativo in confronto all’utilizzo della media in caso di grandezze variabili.

13

1.5 Implementazione in Matlab

L’uso di uno strumento piu potente come Matlab, da la possibilita di avere una rappresentazionegrafica dei risultati del filtro di Kalman.Il primo passo e quello di generare le forme d’onda ideali che vanno a rappresentare le misurenon affette da errore.In particolare, i segnali usati per questa implementazione sono: valore costante, sinusoide,gradino, onda quadra e traiettoria balistica di un proiettile in aria in assenza di attrito conl’aria.L’utilizzo di segnali differenti ha permesso uno studio piu accurato del filtro di Kalman e unaanalisi critica maggiormente approfondita dei risultati ottenuti. In figura 1.10 sono mostrati isegnali utilizzati:

Figura 1.10: Generazione segnali Matlab

14

Successivamente, come nell’implementazione in linguaggio C, si sporcano queste misure idealisommando ad esse un rumore casuale uniformemente distribuito:

Figura 1.11: Aggiunta del rumore Matlab

Si e inizializzato i parametri per l’algoritmo di Kalman, per la media e per il confronto delleprestazioni fra le due tecniche come mostrato in figura 1.12:

Figura 1.12: Inizializzazione Matlab

15

Come seguito nell’implementazione in C, si e implementato l’algoritmo di Kalman e quello dellamedia.Inoltre, si sono calcolate le statistiche per fare un confronto ovvero l’errore commesso fra lamisura reale affetta da rumore e quella ideale, l’errore commesso fra la misura reale e quella sti-mata tramite la media ed infine, l’errore fra la misura reale e quella stimata attraverso l’algoritmodi Kalman. Per avere un secondo metro di confronto, si e calcolato, per i tre casi, la sommadegli errori che si e commesso ad ogni iterazione.

Figura 1.13: Implementazione e statistiche Matlab

Come ultimo passaggio, si sono graficati i risultati e, come atteso, l’algoritmo funge da filtro dirumore riducendo l’incertezza sulla misura. I risultati sono riportati nelle seguenti figure:

16

Figura 1.14: Matlab: valutazione di segnale sinusoidale

Figura 1.15: Matlab: valutazione di segnale a gradino

17

Figura 1.16: Matlab: valutazione di segnale ad onda quadra

Figura 1.17: Matlab: modello di traiettoria balistica di un proiettile in assenza di attrito conl’aria

18

1.6 Quantizzazione

Il problema da risolvere per poter realizzare il filtro di Kalman su FPGA e la quantizzazione. Ilfunzionamento simulato attraverso Matlab senza limitazioni ha dato i risultati aspettati ma nonsi sono posti limiti ne alla lunghezza dei registri in cui si svolgono le operazioni, ne sul formatodelle variabili utilizzate. Naturalmente, per poter implementare su FPGA si deve conoscere efissare con cognizione di causa ogni lunghezza dei registri e prevedere ogni possibile overflowdelle operazioni.Questo passaggio non e stato banale in quanto si e dovuto studiare nel dettaglio le singoleoperazioni ed ogni singolo dato contenuto nei registri.Il formato dei dati standard utilizzato in Matlab e il tipo double ovvero ogni numero e rap-presentato con 8 byte con segno e compre un range di valori fra −10308 e 10308. Si e es-cluso l’uso di questo formato per l’implementazione e si e cercato un formato piu consono perl’implementazione.Le ipotesi fatte sono state:

• Architettura a 32 bit;

• Operazioni signed ;

• Bit di riporto delle somme e delle sottrazioni nulli vista la grandezza dei valori assunti intutti i registri;

• I dati in ingresso sono normalizzati rispetto ai valori massimi quindi sempre compresi fra1 e -1.

• I dati sono sempre contenuti nella parte bassa dei registri compresi tra i valori −215

(corrispondente al valore -1) e 215−1 (corrispondente al valore 1) tranne che in alcuni casiparticolari come dopo lo shift a sinistra o dopo i moltiplicatori.

Il risultato finale di questa fase e mostrato in figura 1.18:Passi salienti del diagramma blocchi:

• Shift di 15 ovvero moltiplicazione per 32767 : operazione necessaria per evitare che ilquoziente della divisione intera sia nullo;

• Divisori per 215: necessari per semplificare lo shift ed ottenere il corretto risultato finale;

• Valore 1: teoricamente e il numero 1 ma per poter semplificare nell’algoritmo deve essereil valore 32767;

• Registri x apri est REGISTER e P apo est REGISTER: vi sono contenute le statistichedi interesse e risultato dell’algoritmo.

19

Figura 1.18: Diagramma a blocchi dopo la quantizzazione

1.7 Quantizzazione in Matlab

Per verificare se le ipotesi fatte, si e simulata la quantizzazione attraverso Matlab. Questo hapermesso una simulazione completa dell’algoritmo e la sua effettiva realizzazione su FPGA. Perfissare la lunghezza dei registri a 32 bit con segno si e utilizzato l’istruzione ”int32 ” che effettuala saturazione del risultato dell’operazione tra i valori −231 e 231−1. I passi eseguito sono stati:

• Inizializzazione delle variabili:

Figura 1.19: Quantizzazione: inizializzazione

20

• Algoritmo di Kalman quantizzato:

Figura 1.20: Quantizzazione: algoritmo

L’istruzione ”idivide” restituisce il quoziente intero fra i due numeri posti fra parentesi.

• Plot dei risultati ottenuti.

1.8 Considerazioni sui risultati ottenuti

Inserendo gli stessi dati nelle simulazioni con e senza quantizzazione, i risultati ottenuti sono statiquelli sperati in quanto vi e poca diversita fra l’algoritmo quantizzato e quello non quantizzato.In particolare, la covarianza ha lo stesso andamento come mostrato in figura 1.21:

Figura 1.21: Confronto fra covarianze

La moltiplicazione per il valore 32767 e dovuto alla normalizzazione eseguita. I valori letti sono12130 e 12129,3 e si nota l’effetto della quantizzazione che produce un errore relativo percentualepari a:

εRelQuantCov% =|12130− 12129.3|

12130∗ 100 = 0.005% (1.15)

21

Per quanto riguarda la stima della misura, si e messo in mostra la discrepanza fra i risultatiottenuti nel caso quantizzato e non quantizzato. Come mostrato in figura 1.22 la diversita fra irisultati risulta essere davvero minima (caso di segnale step di valore 1):

Figura 1.22: Discrepanza fra i risultati

Questa discrepanza tra i risultati e dovuta alla quantizzazione che produce un errore pari a2−15 e da non aver considerato in alcun modo il resto nella divisione intera. L’errore massimoConcluse queste considerazioni, il risultato ottenuto e stato ritenuto soddisfacente per il progettoin questione e si e proceduto nella realizzazione.

22

1.9 TCP-IP con Matlab

Per quanto riguarda la comunicazione fra computer ed FPGA si e scelto di utilizzare il protocollodi comunicazione UDP-IP attraverso la porta ethernet a disposizione nella scheda di test. Si escelto di utilizzare Matlab come interfaccia di comunicazione aprendo un socket UDP-IP. Primadi implementare questo tipo di protocollo si e cercato di implementare il protocollo TCP-IP sullostesso computer con due script di Matlab (server e client) per capirne meglio il funzionamento.In figura 1.23 e mostrato l’algoritmo che apre una porta virtuale e trasmette il pacchetto di daticon il protocollo TCP-IP.

Figura 1.23: Matlab: server TCP-IP codice sorgente

Procedendo in modo analogo si e realizzato il programma per simulare il client. Una voltaterminato il progetto, il file sorgente client sara sostituito dalla scheda DE2 usata in cui verraimplementato questa modalita di comunicazione.

Figura 1.24: Matlab: client TCP-IP codice sorgente

23

Capitolo 2

Verilog

L’implementazione su FPGA comprende: filtro di Kalman, comunicazione client TCP-IP el’unita di controllo per la temporizzazione.Il modulo kalman racchiude racchiude questi tra elementi come mostrato in figura 2.1:

Figura 2.1: Verilog : implementazione ALU, codice sorgente

24

2.1 kalman ALU

Il modulo kalman ALU implementa il diagramma a blocchi quantizzato mostrato in figura 1.18.La dichiarazione del modulo e riportata in figura 2.2:

Figura 2.2: Verilog : dichiarazione kalman ALU

Nel dettaglio, in questo modulo vi sono tutte le operazioni necessarie per il calcolo della stimae della covarianza come mostrato in figura 2.3:

Figura 2.3: Verilog : implementazione calcoli

Per realizzare queste operazioni sono state utilizzate le megafunction all’interno di Quartus II.

25

2.2 kalman control unit

Ruolo fondamentale ricopre il modulo kalman control unit che ha il compito di far susseguirein ordine corretto i vari passi dell’algoritmo attraverso una macchina a stati come mostrato infigura 2.4:

Figura 2.4: Verilog : algoritmo macchina a stati

26

2.2.1 Macchina a stati: dettagli

Figura 2.5: Macchina a stati

La macchina a stati implementata e composta da 3 stati ed ha il compito di temporizzare inmodo corretto l’intero algoritmo. Il cambio di stato e controllato da 2 segnali:

• start : se ad 1, indica che il dato e in ingresso e si potrebbe iniziare ad elaborare;

• busy counter overflow : indica l’overflow del contatore count. Se ad 0, indica che il filtro eancora in fase di elaborazione del dato precedentemente posto in ingresso. Nel momentoin cui passa ad 1 significa che ha concluso l’elaborazione.

Gli stati sono caratterizzati da 3 segnali:

• busy : indica se il modulo kalman ALU e occupato oppure e possibile elaborare un altrodato;

• enable: abilita il contatore count utilizzato per la temporizzazione;

• reset cu: segnale utilizzato per resettare il contatore count.

Di seguito e riportato il susseguirsi degli stati:

• Stato S0: il dato non e presente in ingresso e non si sta elaborando. Nel momento in cuiarriva il dato, start e posto ad 1 e si verifica che il modulo kalman ALU sia libero. Nelcaso in cui e libero, ovvero busy counter overflow e a 1, si passa allo stato S1 altrimenti sirimane in questo stato in attesa.

• Stato S1: questo e lo stato di elaborazione del dato attraverso kalman ALU. Le vari-abili busy e enable vengono poste ad 1 e il contatore count conta. In questo stato,busy counter overflow rimane a 0 finche non si e conclusa l’intera elaborazione e questoavviene quando il contatore count raggiunge il valore massimo con il controllo:

Figura 2.6: Reset del contatore count

Si rimane in questo stato finche l’elaborazione non e completata dopodiche si passa allostato S3.

• Stato S3: si procede al reset della control unit attraverso il segnale reset cu. Esso resettail contatore count dopo di che si torna allo stato S0.

27

2.2.2 Simulazioni

Una volta implementato sia la parte di ALU che di controllo, si e proceduto in una prima fasedi test attraverso ModelSim. Si e voluto verificare, in particolare, la corretta tempistica perdare il sufficiente tempo ai blocchi combinatori di elaborare correttamente il dato. Attraversoil testbench, si e simulata una possibile misura affetta da rumore e si e studiata la risposta delsistema. In figura 2.7 e riportato il codice utilizzato per la simulazione.

Figura 2.7: Testbench

Sono riportati in figura 2.8 i risultati ottenuti da queste simulazioni:

Figura 2.8: Simulazioni ModelSim

In linea con le aspettative, il valore a cui tende x apo est e 15000 ovvero il valore impostatocome z real.

28

Studiando un piu nel dettaglio, per ottenere il corretto risultato occorre aspettare un certonumero di colpi di clock. Questa attesa e regolata dal contatore count come mostrato in figura2.9:

Figura 2.9: Simulazioni ModelSim: attesa del risultato corretto

Fino al termine del conteggio di count il valore non e pronto ed instabile. Se avviene il cam-pionamento in istanti in cui il dato non e corretto si verra a creare un errore non tollerabileche si ripercuotera sul corretto funzionamento dell’intero filtro in quanto ricorsivo. Il risultatofinale del filtro di Kalman implementato attraverso un sistema embedded basato su FPGA (inparticolare sulla scheda DE2 in dotazione) e il seguente:

Figura 2.10: Filtro di Kalman: rete RTL

29

2.3 nios

Implementazione NIOS II e necessaria per il controllo della comunicazione attraverso il protocolloTCP-IP. Di seguito e riportata la dichiarazione di tale modulo:

Figura 2.11: Verilog : NIOS II

Il NIOS II e stato settato attraverso il tool Qsys all’interno di Quartus. Mostrato in figura 2.12la struttura del microprocessore implementato e le periferiche annesse:

Figura 2.12: Qsys: NIOS II, architettura microprocessore

30

2.3.1 Codice C

Attraverso il software Nios II 13.0sp1 Software Build Tools for Eclipse, in linguaggio C, si eimplementata la trasmissione con protocollo UDP-IP. Il codice e diviso in 3 sezioni in cui laprima parte e di ricezione delle misure, la seconda e di caricamento del dato per l’elaborazioneattraverso il filtro ed attesa dei risultati mentre la terza realizza l’invio dei risultati a Matlab.Si e scelto di inviare un intero set di misure sullo stesso pacchetto UDP-IP per un motivo dicomodita e risparmiando sui pacchetti inviati. Questo non influenza l’obbiettivo di questo pro-getto perche si sarebbe ottenuto lo stesso risultato inviando e ricevendo un valore alla volta.

2.3.2 Receive Measures

Ricezione dei dati da parte del dispositivo attraverso la comunicazione Ethernet. Mostrato infigura 2.13 la parte di codice che descrive questo passaggio:

Figura 2.13: Sorgente C: ricezione del pacchetto UDP-IP

Il primo controllo e sull’avvenuta ricezione. Se esso da esito positivo, si controlla se l’indirizzodel destinatario del pacchetto coincide con quello della periferica (in questo caso la DemoBoard2). Se anche questo ha dato esito positivo, si preleva la misura da elaborare e si costruisce unarray di dati.

31

2.3.3 Evaluate Measures

Le misure da elaborare contenute nell’array RX PCKT data vengono poste, una alla volta, sulbus dati per poter essere lette dal modulo kalman descritto in Verilog. Tale modulo preleva lamisurazione dal bus dati attraverso il buffer di ingresso e la elabora. Il segnale BUSY BASE, chee il segnale busy del modulo kalman, viene controllato in polling per verificare se l’elaborazionesi e conclusa. Una volta terminata, si leggono i valori X APO EST BASE e P APO EST BASEdal bus AVALON e si salvano in due array differenti in particolare x apo est e P apo est. Questaoperazione e eseguita finche vi sono misure da elaborare. In figura 2.14 e riportato il codice diquesta sezione:

Figura 2.14: Sorgente C: elaborazione misure

32

2.3.4 Transmit Measures

Ultimo passo e la trasmissione dalla scheda DE2 al computer dei dati elaborati. Per la trasmis-sione si e utilizzato il protocollo UDP-IP: come mostrato in figura 2.15, si sono definite le varieparti che compongono il pacchetto con i corretti valori IP.

Figura 2.15: Sorgente C: inizializzazione pacchetto UDP-IP

Successivamente si riempie il pacchetto con i valori di misura stimata e covarianza calcolate dalfiltro e si procede con la trasmissione.

Figura 2.16: Sorgente C: trasmissione del pacchetto UDP-IP

33

2.4 UDP-IP Matlab

La comunicazione avviene tramite protocollo UDP-IP leggermente differente dal TCP-IP. Loscript di Matlab necessario per decodificare il pacchetto arrivato dalla scheda e mostrato infigura 2.17: Come si puo vedere, si e applicata una piccola modifica alla funzione che apre il

Figura 2.17: Inizializzazione UDP-IP

socket UDP che svolge anche la funzione di ”port bilding”

34

Capitolo 3

Risultati Finali e conclusioni

Il sistema embedded basato su FPGA che implementa il filtro di Kalman e la comunicazionefunziona in modo corretto, in linea con l’obbiettivo prefissato. In figura 3.1 e mostrato il risultatoper quanto riguarda La grandezza costante:

Figura 3.1: Risultati: grandezza costante

Come si puo notare, il filtro elimina quasi completamente il rumore di cui e affetta la misurarestituendo una stima che si avvicina maggiormente al valore reale che si vuole misurare. Lacovarianza ha lo stesso andamento di quella calcolata via software da Matlab quindi il funzion-amento e corretto.

35

Di seguito e riportato il caso di grandezza sinusoidale:

Figura 3.2: Risultati: grandezza sinusoidale

Ed infine, la grandezza di maggiore interesse ovvero la traiettoria balistica di un proiettile inassenza di attrito con l’aria.

Figura 3.3: Risultati: grandezza sinusoidale

Il filtraggio risulta essere corretto in tutti i casi come la comunicazione, ritenendo raggiuntol’obbiettivo prefissato.

36