Introduzione
IV
L’informazione che si cerca è contenuta nella posizione iniziale dell’eco ricevuta, l’onset,
che non è possibile determinare accuratamente da un segnale ultrasonico acquisito a causa
del rumore sovrapposto al segnale utile e della distorsione che questo subisce.
Il profilo dell’eco può essere infatti deformato da vari fattori:
ξ proprietà geometriche e meccaniche della superficie riflettente
ξ posizione ed orientamento del target
ξ percorsi di trasmissione del segnale ultrasonico
ξ fattori ambientali (temperatura, umidità, pressione)
Il rumore impedisce di poter applicare semplici metodi a soglia, mentre le distorsioni della
forma d’onda inducono una notevole polarizzazione delle misure nel caso di metodi basati
su cross-correlazione o calcolo di fiducial point (come il massimo della derivata
dell’inviluppo).
Per superare tali inconvenienti, la stima della distanza viene effettuata mediante un metodo
di stima dei parametri di forma dell’inviluppo ultrasonico, tra cui l’onset stesso, a partire
da acquisizioni dell’eco ultrasonica. Tale metodo è stato messo a punto in lavori precedenti
ed è basato sul filtro di Kalman esteso (EKF), nella sua veste numerica (DEKF).
E’ ben nota in letteratura l’espressione di una eco come funzione non lineare dei suoi
parametri caratteristici, fra cui il TOF. Considerando tale relazione come l’equazione di
misura di un sistema dinamico, la cui matrice di stato è l’identica, che ad ingresso nullo
risponde con un’uscita che è il segnale ecografico stesso, è possibile, tramite il DEKF,
stimarne lo stato, ovvero i suoi parametri caratteristici. Viste le specifiche numeriche, in
termini di dinamica e precisione, richieste da tale tipo di filtraggio, per l’implementazione
dell’algoritmo è stato utilizzato un moderno floating-point DSP a 32-bit, l’ADSP-21364
della ANALOG DEVICES. Tale DSP provvede alla gestione ed alla esecuzione delle
diverse fasi della operazione di misura, (i) dalla generazione del segnale di eccitazione del
trasduttore ultrasonico (un Polaroid, serie 9000), (ii) al pilotaggio del convertitore
Analogico/Digitale impiegato (un AD 9244 dell’ANALOG DEVICES) per la conversione
dell’eco di ritorno ed (iii) alla successiva elaborazione della stessa, mediante DEKF.
Introduzione
V
Inevitabilmente, la realizzazione del prototipo ha richiesto l’interazione e la coordinazione
di una sezione analogica e di una digitale. La prima, il cosiddetto front-end analogico, per
l’interazione con il mondo reale, la seconda demandata all’elaborazione numerica.
Il lavoro svolto va dalla progettazione e realizzazione dell’hardware del front-end
analogico, fino alla stesura e verifica del progetto software di coordinamento e di
elaborazione sull’ADSP-21364.
Quest’ultimo è stato realizzato mediante l’uso dell’ambiente di sviluppo e debugging
integrato (IDDE) proprietario della Analog Devices, il VisualDSP++, a supporto dei
processori a 32-bit della famiglia SHARC di cui l’ADSP-21364 fa parte.
In particolare, l’implementazione software del filtraggio di Kalman esteso ha richiesto la
valutazione sul campo di parametri critici per l’applicazione stessa, sia in termini di
precisione della stima fornita sia ai fini della convergenza dell’algoritmo. Oltre a una stima
della soglia di rumore e dei parametri dell’inviluppo, che è stato possibile effettuare in base
alle acquisizioni effettuate, di particolare rilievo sono state le scelte relative ai parametri
statistici: le covarianze dell’errore di stima dei singoli parametri con cui inizializzare
l’algoritmo, e la stima della varianza del rumore sulla misura. Quest’ultima stima è stata
effettuata mediante la caratterizzazione statistica del solo rumore di misura, acquisito in
assenza di eco ultrasonica.
Le prestazioni del prototipo sono state validate mediante varie misure sperimentali di
distanza, realizzate in differenti condizioni di prova. I risultati soddisfacenti propongono il
prototipo realizzato come una valida alternativa alle soluzioni attualmente presenti in
letteratura e sul mercato.
Capitolo 1 – Il filtro di Kalman
1
1. CAPITOLO 1: IL FILTRO DI KALMAN
1.1. Introduzione al filtro di Kalman
Il filtro di Kalman è una tecnica per la risoluzione del problema Gaussiano lineare
quadratico, che si può trasporre, nella pratica ingegneristica, nella stima dello stato
istantaneo di un sistema dinamico a partire dalla sua uscita, corrotta da rumore bianco.
Lo stimatore sviluppato è statisticamente ottimale rispetto a qualsiasi funzione quadratica
di stima dell’errore.
Il filtro di Kalman è considerato il più grande risultato raggiunto nella teoria della stima nel
ventesimo secolo. E’ stato, fra l’altro, una delle tecnologie fondamentali per la conquista
dello spazio. La navigazione precisa ed efficiente dei velivoli spaziali attraverso il sistema
solare non sarebbe potuta avvenire senza di esso. Basti pensare che il ritorno sulla Terra
della missione Apollo 11, non sarebbe stato possibile se il sistema di guida del SaturnoV
non avesse utilizzato tale algoritmo per la predizione della rotta ottimale, consentendo un
risparmio di carburante tale da permettere il regolare rientro anche in condizione di
emergenza.
L’approccio usato permette di "correggere l’impatto degli errori” di misura sulla stima
dello stato in sistemi anche molto complessi e sofisticati.
Il filtro di Kalman include la soluzione di un problema inverso.
Si supponga, infatti, di sapere come rappresentare le variabili misurabili in funzione delle
variabili d’interesse. Il filtro di Kalman inverte questa relazione funzionale e stima le
variabili indipendenti come funzione inversa delle variabili dipendenti (misurabili).
Ovvero dato il sistema dinamico:
() ( (), (),)
() ( (), (),)
x tfxtutt
yt hxtutt
↑
→
↓
&
(1.1)
Il filtro di Kalman risolve il problema di risalire allo stato ()x t a partire dall’uscita ()yt
quando essi siano perturbati da rumore gaussiano bianco.
Capitolo 1 – Il filtro di Kalman
2
1.2. La minimizzazione ai minimi quadrati
Storicamente, il primo metodo per fornire una stima ottima di dati rumorosi è stato il
metodo dei minimi quadrati.
Si scriva un sistema di equazioni in forma matriciale, come:
11 12 1 1 1
21
1
n
mmnn
hh h x y
h
hhxy
♠ ≡ ♠ ≡ ♠ ≡
↔ ≈ ↔ ≈ ↔ ≈
↔ ≈ ↔ ≈ ↔ ≈
↔ ≈ ↔ ≈ ↔ ≈
↔ ≈ ↔ ≈ ↔ ≈
↔ ≈ ↔ ≈ ↔ ≈
↔ ≈ ↔ ≈ ↔ ≈
← … ← … ← …
LL
OMMM
MOMMM
LLL
(1.2)
o, in forma sintetica:
Hx y (1.3)
Il problema è quello di trovare un valore ˆx che minimizzi l’errore stimato di misura.
Caratterizzando tale errore secondo il quadrato della sua norma euclidea:
2
2
2
11
ˆˆ
()
mn
ij j j
ij
ex Hx y hx y
♠ ≡
↔ ≈
← …
ƒ ƒ
(1.4)
La norma, vista come funzione delle n variabili
k
x , è continua, differenziabile in tutto
n
,
e và all’infinito per
ˆ
k
x ο φ.
E’ quindi possibile trovare il suo minimo tramite l’annullamento del gradiente senza
andare a stimare anche l’Hessiano.
Si ottiene quindi un sistema di n equazioni:
2
11
ˆ
02
ˆ
1, 2, 3, . .
mm
ik ij j i
ij
k
e
hhxy
x
kn
♠ ≡
ω
↔ ≈
ω
← …
ƒ ƒ
(1.5)
L’equazione (1.5) può essere riscritta in forma matriciale come:
ˆˆ
02 [ ]2 2
TTT
HHxy HHx Hy (1.6)
Ovvero:
ˆ
TT
HHx Hy (1.7)
Capitolo 1 – Il filtro di Kalman
3
Tale equazione è detta equazione normale o forma normale dell’equazione per il problema
lineare ai minimi quadrati.
La forma normale ha la seguente soluzione:
1
ˆ
()
TT
x HH Hy
(1.8)
sotto il vincolo che la matrice:
T
HH (1.9)
detta matrice Gramiana, sia invertibile.
La proprietà di invertibilità di tale matrice definisce anche l’osservabilità di un set di
variabili sconosciute. Difatti esse sono osservabili se sono unicamente determinabili dato
un set di vincoli.
La condizione di invertibilità della matrice Gramiana caratterizza quindi algebricamente
l’osservabilità di un set di variabili quando i vincoli sono lineari.
1.3. Il filtro di Wiener
Il filtro di Wiener è una evoluzione successiva per risolvere il problema di predizione
dell’errore ai minimi quadrati in termini della funzione di autocorrelazione del segnale e
del rumore.
La soluzione è in forma di un operatore integrale che può essere sintetizzato con circuiti
analogici, dati alcuni vincoli sulla regolarità della funzione di autocorrelazione.
L’approccio è quello di rappresentare la natura probabilistica di fenomeni stocastici in
termini di densità spettrale di potenza.
1.4. Il filtro di Kalman
L’idea del filtro di Kalman è quella di applicare la nozione di variabile di stato al filtro di
Wiener.
In sintesi si tratta di una procedura utilizzata per il filtraggio dei dati costruito sulla base di
una media pesata tra il prossimo valore predetto e il prossimo valore stimato. Esso è
costituito da un set di equazioni matematiche che offrono un efficiente soluzione
Capitolo 1 – Il filtro di Kalman
4
(ricorsiva) del metodo dei minimi quadrati. Questo filtro è spesso utilizzato per ottenere
una valutazione più accurata di un dato ottenuto dalla lettura di più sensori, ognuno
caratterizzato da un rumore di misura avente caratteristiche differenti nel dominio del
tempo e della frequenza. Tale filtraggio è molto potente, in quanto consente una stima
dello stato di un sistema se sono noti i seguenti elementi :
1) una serie di misure sul sistema da stimare;
2) la conoscenza di un modello matematico descrittivo del sistema;
3) il modello statistico dei rumori sulle misure;
1.4.1. Il filtro di Kalman discreto: caso lineare (DKF)
Il problema generale da risolvere è quello di valutare lo stato
n
x di un sistema tempo
discreto governato dalle seguenti equazioni alle differenze :
11 11 1kkk kk k
kkkk
x Ax Bu w
yCxv
↑
°
→
°
↓
(1.10)
Le notazioni usate saranno [Grewal and Andrews, 1993]:
ξ k è il generico istante di campionamento t
k;
ξ
n
k
x è il vettore di stato. In un sistema lineare, lo stato rappresenta una
particolare condizione dei parametri che lo caratterizzano;
ξ
I
k
u è il vettore degli ingressi.
ξ
nxn
k
A è la matrice di transizione degli stati. Descrive l’evoluzione libera
delle variabili di stato a partire da uno stato iniziale;
ξ
nxI
k
B
è la matrice degli ingressi. Descrive l'evoluzione forzata della
variabile di stato a partire dal valore del vettore degli ingressi u
k-1
;
ξ
mxn
k
C è la matrice delle uscite. Descrive il valore assunto dalle variabili
misurate in funzione del valore attuale della variabile di stato x
k
;
ξ
m
k
y è il vettore delle misure.
Capitolo 1 – Il filtro di Kalman
5
ξ
n
k
w è il disturbo sullo stato. Rappresenta gli ingressi che agiscono in
maniera incontrollabile sullo stato del sistema; è un rumore gaussiano a media
nulla e con matrice di covarianza Q
k
(p(w) ~ N(0,Q
k
));
ξ
n
k
v
è rumore sulle misure. Rappresenta la differenza tra il vettore delle
misure ed il valore che le uscite avrebbero in conseguenza del solo valore
attuale dello stato x
k
; è un rumore gaussiano a media nulla e con matrice di
covarianza R
k
(p(v) ~ N(0,R));
ξ
nxn
k
Q è la matrice di covarianza del disturbo sullo stato. Rappresenta la
variabilità statistica del vettore dei disturbi sullo stato (il vettore ha media
nulla) ovvero la potenza del disturbo introdotto nel sistema che devia
l'andamento delle variabili di stato rispetto a quello prevedibile dalla
conoscenza del vettore degli ingressi e dalla legge lineare che ne governa
l'evoluzione;
ξ
mxm
k
R è la matrice di covarianza del rumore sulle misure. Rappresenta
la variabilità statistica del vettore dei disturbi di misura (vettore a media nulla)
e rappresenta la potenza del disturbo sulla misura introdotta su ciascuna delle
misure accessibili;
ξ
nxn
k
P
n
è la matrice di covarianza dell'errore di stima dello stato.
Rappresenta la variabilità dell'errore sulla stima dello stato conseguente ai due
fattori di disturbo (errore di misura e disturbo dello stato);
ξ
mxn
k
K è la matrice di correzione dello stato. Prende il nome di
Guadagno di Kalman e indica il livello di fiducia assegnata alla misura rispetto
alla fiducia assegnata alla stima dello stato in base al valore precedente e al
modello matematico che ne rappresenta l'evoluzione. Tanto maggiore è il
valore di K
k
, tanto minore fiducia merita la stima basata sul modello rispetto
alla misura riportata;
Le grandezze elencate hanno le seguenti proprietà (descrizione del modello statistico degli
errori: E[·] rappresenta l'operatore di calcolo della media) e definizioni :
ξ Q
k
= E[w
k
w
k
T
] essendo E[w
j
w
k
T
] = 0 per j diverso da k
Capitolo 1 – Il filtro di Kalman
6
ξ R
k
= E[v
k
v
k
T
] essendo E[v
j
v
k
T
] = 0 per j diverso da k
ξ E[w
j
v
k
T
] = 0 per ogni j,k
ξ P
0
= E[(x
0
- µ
x
) (x
0
- µ
x
)] avendo definito µ
x
= E[x
0
]
Si definiscono :
ξ
ˆ
()
n
k
x
: stima a priori del vettore di stato all’istante k;
ξ
ˆ
()
n
k
x
: stima a posteriori condizionata dal valore attuale della misura
del vettore di stato all’istante k;
ξ ˆ() ()
kkk
exx (1.11)
errore di stima a priori ;
ξ ˆ() ()
kkk
exx (1.12)
errore di stima a posteriori;
ξ () [ () ()]
T
kkk
PEee (1.13)
matrice di covarianza dell’errore di stima dello stato a priori ;
ξ []
T
kk
PEee (1.14)
matrice di covarianza dell’errore di stima dello stato a posteriori .
L'operazione di filtraggio si articola nelle seguenti operazioni (Fig.1-1):
ξ Inizializzazione dell’algoritmo
ξ Previsione dello stato attuale dato il precedente
ξ Aggiornamento della matrice di correzione dello stato
ξ Stima pesata della variabile di stato
ξ Preparazione del ciclo di calcolo successivo
Nel resto della trattazione verrà trascurato il rumore di processo.
Capitolo 1 – Il filtro di Kalman
7
Figura 1-1
Nelle formule riportate in questo capitolo, se la variabile è seguita dal segno (-) si intende
come un valore stimato per predizione; se è, invece, seguita dal segno (+) essa si intende
condizionata dal valore attuale della misura. L'uso del simbolo "^" sulla variabile x indica
che esso è il valore di x predetto.
Capitolo 1 – Il filtro di Kalman
8
Passo 1: Inizializzazione dell’algoritmo
ƒ È necessario possedere le matrici che rappresentano l’evoluzione dello stato
(A
k
, B
k
e C
k
)
ƒ È necessario aggiornare le matrici che rappresentano la varianza dei disturbi
sullo stato (Q
k
) e del rumore sulla misura (R
k
)
ƒ Si usa un valor medio dello stato iniziale x
0
per inizializzare nell’algoritmo
il valore dello stato stimato all’istante k-1, x
k-1
(+)
Come primo valore per P
k
può essere utilizzato un valore qualsiasi, ad esempio una matrice
unitaria. Le matrici Q
k
e R
k
sono solitamente utilizzate e definite come matrici costanti.
Spesso, infatti, non ha senso modificarne il valore durante le stime successive dello stato.
Talvolta, può essere consigliabile modificare Q
k
e R
k
per tener conto di particolari fattori,
come ad esempio, una condizione di maggiore disturbo sullo stato del sistema, ovvero di
presenza di maggiori errori sulla misura, motivati ad esempio dal malfunzionamento di un
sensore.
Passo 2: Previsione dello stato
ξ Predizione della variabile di stato: si esegue la predizione della variabile di stato in
base alle informazioni certe che si hanno del modello:
11
ˆˆ
() ()
kkk
xAx
(1.15)
ξ Aggiornamento della varianza dell’errore sullo stato: si aggiorna il valore della
varianza dell’errore in base all’ultima stima del disturbo sullo stato:
11 1 1
() ()
T
kkkkk
PAPAQ
(1.16)
E’ da notare come, nella prima formula sia applicata la legge di evoluzione dello stato,
senza il contributo dovuto agli ingressi. Se questi sono noti, la legge di evoluzione dello
stato deve essere applicata nella sua versione integrale (x
k
= A
k-1
x
k-1
+ B
k-1
u
k-1
). Al
contrario, se gli ingressi non sono noti è necessario considerare il contributo degli ingressi
come un disturbo non misurabile e quindi da inserire nella matrice Q
k
. La legge di
Capitolo 1 – Il filtro di Kalman
9
aggiornamento della matrice di varianza dell’errore sullo stato è di semplice
interpretazione: se Q
k
è nullo, la varianza dell’errore sullo stato andrà a diminuire quanto
più stabilizzante è la matrice di stato A
k
. Al contrario, quanto maggiore è Q
k
, tanto
maggiore sarà l’incidenza del disturbo sullo stato nell’errore complessivo di stima.
Passo 3: Aggiornamento della matrice di correzione dello stato
Il guadagno K
k
risulterà maggiore o minore a seconda se prevale l’incertezza dello stato
P
k
(-) o l’incertezza sulla misura R
k
:
1
() [ () ]
TT
kk kkk k k
KPCCPCR
(1.17)
Esempio : C
k
= I:
1
()[ () ]
kk k k
KP P R I
(1.18)
(per P
k
(-) >> R
k
)
1
()[ () ] 0
kk k k
KP P R
ο(1.19)
(per P
k
(-) << R
k
)
Con K
k
=I, posto che C
k
= I, l’algoritmo dà piena fiducia alla misura effettuata. Per K
k
tendente a zero, l’algoritmo tenderà a confermare il valore predetto dalla stima, come
illustrato nel passo successivo.
Passo 4: Aggiornamento della stima dello stato
Si pesa con la matrice K
k
il valore della variabile predetta per propagazione dello stato x
k
(-)
con l’errore attuale sulla misura:
ˆˆ ˆ
() () [ ()]
kk kkk
xxKyCx (1.20)
Come si diceva nello step precedente, la matrice K
k
indica la fiducia posta dall'algoritmo
nella misura. K
k
tendente a zero indica una scarsa affidabilità della misura rispetto alla
fiducia posta nella propagazione del modello.
Passo 5: Aggiornamento della varianza dei disturbi sullo stato e aggiornamento del
ciclo
Capitolo 1 – Il filtro di Kalman
10
Al contrario dello step 2, l'aggiornamento della matrice di varianza dei disturbi sullo stato
viene fatta a posteriori, utilizzando le informazioni contenute nella matrice K
k
sulla
affidabilità della misura rispetto alla qualità della previsione dello stato. Come si ricorda
dal passo precedente, la matrice K
k
indica la fiducia posta dall'algoritmo nella misura. Con
K
k
tendente a zero, la matrice P
k
rimane sostanzialmente invariata nell'aggiornamento
dell'algoritmo da un passo al successivo.
Una rappresentazione tramite schema a blocchi del filtro di Kalman discreto lineare è la
seguente:
Figura 1-2 Schema a blocchi del sistema, del modello di misura e del DKF
Si può dimostrare che la matrice di guadagno del filtro di Kalman tempo invariante, che ha
questa forma:
()
T
KP CR φ (1.21)
è identica al filtro di Wiener.
Ovvero, come detto, il filtro di Kalman tempo variante costituisce un’estensione del filtro
di Wiener a sistemi non stazionari, tempo discreti, ma con il vincolo di una dimensione del
vettore di stato finita.
1.4.2. Il filtro di Kalman discreto: caso non lineare (EKF)
Capitolo 1 – Il filtro di Kalman
11
Il filtro di Kalman, in origine pensato per la soluzione di problemi lineari, è semplice nella
forma ed estremamente potente nei risultati.
In considerazione del considerevole successo ottenuto da esso nella stima in problemi
lineari, sono state applicate delle estensioni di questo metodo anche a problemi non lineari.
Difatti molti sistemi dinamici e sensori sono di tipo non lineare, ma sono di tipo “smooth”
ovvero la dipendenza funzionale delle misure o dello stato sono lineari per piccole
perturbazioni dei valori delle variabili di stato.
La non linearità può entrare nel problema sia nella dinamica del processo che nella
relazione della misura. In generale il processo da stimare e la relazione associata alla
misura possono essere rappresentate nella seguente forma:
(,,) ()
(,) ()
x fxut wt
yhxt vt
↑
→
↓
&
(1.22)
dove x è il vettore di stato all’istante t, y è il vettore delle misure all’istante t, f e h sono
funzioni non lineari note, u è il valore dell’ingresso all’istante t, e w e v sono due processi
Gaussiani bianchi a media nulla ed indipendenti.
Appare quindi lecito, nell’ipotesi di sistema smooth, considerare come approssimazione
lineare del sistema lo sviluppo in serie di Taylor del modello arrestato al primo ordine
intorno ad una traiettoria dello stato.
Nascono quindi due idee:
ξ Linearizzare il sistema attorno ad una traiettoria nominale, definita a
priori
ξ Linearizzare il sistema attorno ad una traiettoria stimata ad ogni passo
dell’algoritmo
Il primo approccio è detto quello filtro di Kalman linearizzato.
Il secondo approccio, di cui si fa utilizzo in questo lavoro, è chiamato generalmente filtro
di Kalman esteso.
Il problema con la linearizzazione attorno ad una traiettoria nominale, che ha portato allo
sviluppo del filtro di Kalman esteso, è che la deviazione della traiettoria effettiva rispetto a
quella nominale può aumentare col tempo. All’aumentare della differenza fra le due
traiettorie, il peso dei termini di ordine più alto della serie di Taylor tende ad aumentare.
Ricevi informazioni sui nostri servizi, sulle offerte e non perdere news e consigli su università e lavoro.
