Fondamenti di “sensor fusion”
Mariolino De Cecco, Luca Baglivo, Mattia Tavernini
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Un manipolatore può essere schematizzato da un punto di vista meccanico come
una catena cinematica costituita da corpi rigidi (bracci) connessi mediante giunti
rotoidali o prismatici.
Ad ogni giunto corrisponde un grado di mobilità
Nei manipolatori un estremo è vincolato
rigidamente ad una base, all’altro estremo è
fissato un organo terminale (end effector).
Conoscendo la variabile connessa con ogni
giunto (variabile di giunto: angolo per i
rotoidali, posizione relativa per i prismatici) la
posizione ed orientazione (posa o postura)
dell’organo terminale sono univocamente
determinati
Nel caso dei robot mobili non si hanno vincoli
rigidi che ne vincolano la posizione in maniera
univoca, bensì ruote (generalmente) che
determinano un vincolo di velocità
Conoscendo la variabile connessa con ogni
giunto (angolo complessivo di rotazione
della ruota) non si ha nessuna
informazione circa la posa del veicolo
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Cinematica dei Manipolatori e dei robot mobili
La navigazione autonoma è intesa come l’insieme delle tecniche per mezzo
delle quali si rende un sistema in grado di spostarsi con un certo livello di
autonomia in diverse tipologie di ambiente: terrestre, aereo, subacqueo,
spaziale, etc.
I problemi che nel campo della navigazione autonoma vengono affrontati
riguardano innanzitutto la localizzazione del sistema rispetto all’ambiente, la
pianificazione del proprio compito e il controllo del moto
Un’interessante categoria di sistemi a cui si applica lo studio della
navigazione autonoma è quella dei sistemi AGV (Autonomous Guided
Vehicles). Essi trovano ormai largo impiego in industrie, porti, aeroporti,
ospedali, etc, tuttavia misurare la posizione e l’assetto di un veicolo è ancora
un problema di consistente interesse
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Il sensor fusion è il processo che combina le informazioni provenienti da un
certo numero di sorgenti differenti per fornire una descrizione completa e
robusta (misura) di un insieme di variabili di interesse.
Il sensor fusion è di particolare utilità in ogni applicazione in cui molte misure
devono essere combinate assieme, fuse e ottimizzate allo scopo di ottenere
informazioni robuste, di qualità e integrità atte allo scopo dell’applicazione.
Le tecniche di sensor fusion trovano applicazione in molti sistemi industriali,
militari, di monitoraggio, di sorveglianza civile, in processi di controllo e in
sistemi informatici.
Il problema della localizzazione dei veicoli autonomi, in cui nella quasi
totalità dei casi i singoli trasduttori si rilevano insufficienti nel costituire un
sistema di localizzazione completo e robusto per la navigazione autonoma,
richiede l’impiego di tecniche di sensor fusion.
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Caso Robot Mobili
Le tecniche di sensor fusion trovano applicazione nel campo della navigazione
autonoma, in cui è necessario ottenere una stima di posizione e assetto (posa)
di un robot mobile
Vantaggi
Svantaggi
Misura incrementale
(encoders, inerziale,
dead rockoning)
auto-contenuti
integrano incrementi
relativamente semplici, relativi e quindi deriva
elevata frequenza di
aggiornamento,
assenza di rumore ad
alta frequenza
Misura riferita
all’ambiente (laser,
ultrasuoni, cavo
annegato)
misura riferita
all’ambiente quindi
assenza di deriva
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Caso Robot Mobili
bassa ripetibilità,
bassa frequenza di
aggiornamento della
misura
Esempio di localizzazione e mappa ottenuta con soli dati incrementali
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Esempio di localizzazione e mappa ottenuta con Sensor Fusion
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
La norma ISO definisce la misura come un insieme di elementi:
- un valore
- un livello di confidenza che definisce la probabilità che la misura si trovi
all’interno dell’intervallo indicato
- un intervallo di valori ad associato al livello di confidenza stabilito(ottenuto
mediante integrazione sulla funzione densità di probabilità)
- un’unità di misura
Per il Sensor Fusion non bastano le informazioni di cui sopra, ma è necessaria
anche la funzione densità di probabilità associata alla misura
Ad esempio, se si ipotizza una funzione densità di probabilità di tipo Gaussiano,
la misura sarà costituita dal valor medio m e da un intervallo di valori centrato
attorno alla media e di ampiezza pari a un certo coefficiente moltiplicato per la
deviazione standard s della distribuzione.
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Modelli probabilistici
1
p(zi >z)
0.9
0.8
0.7
0.6
0.5
0.4
p(zi<z) = F(z)
0.3
0.2
5
P x
1
2
x x0
e
2
0.1
2
0
-3
2
-2
-1
0
1
2
3
Valori notevoli:
P(x)=0 … perchè?
P x
z
1
2
x0
e
2
2
2
d
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Modelli probabilistici
p( x  m  s )  0.680
p( x  m  2s )  0.950
p( x  m  3s )  0.997
Intervallo pari a ±2s
x
Intervallo pari a ±2s
x
x0: misura
… quindi possiamo dire che la misura è pari a:
• x0 ± 2s con il 95% di probabilità
Ovvero che c’è una probabilità del 95% che il valore vero della variabile da
misurare cada all’interno dell’intervallo [x-2s x+2s]
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Modelli probabilistici
realizzazione di un valore y del processo aleatorio Y
y viene anche detta variabile casuale
La funzione densità di probabilità P(y), è valida se:
P( y ) 0, y
P( y )dy 1
y Y
Ricordiamo sempre che la probabilità che si manifesti la variabile casuale y 
[y0 y1] vale:
y
1
y0
P( y )dy
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Modelli probabilistici
La densità di probabilità congiunta P(x, y) è definita nella stessa maniera di
P(y) e rappresenta la distribuzione di probabilità del realizzarsi
congiuntamente di due variabili casuali
La densità di probabilità marginale P(y) è associata alla probabilità congiunta
ed è la funzione che per ogni valore di y esprime la probabilità che esso si
realizzi congiuntamente a tutti i possibili valori di x:
Py ( y )
Pxy ( x, y )dx
x
La pdf (Probability Density Function) condizionata P(x | y) è la densità di
probabilità associata alla variabile x condizionata al verificarsi
precedentemente di un dato valore di y
Viene definita nel modo seguente:
P( x y )
P ( x, y )
P( y )
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
In generale: P( x y )
P( x, y )
Consideriamo:
x e y -> variabili casuali discrete di due
processi aleatori X,Y
N -> numero totale di osservazioni delle
due variabili
ci -> # osservazioni con x = xi
rj -> # osservazioni con y = yi
nij -> # osservazioni con x = xi e y = yi
La probabilità P(x) è definita come la probabilità di verificarsi della condizione
x = xi ed è pari a:
c
P( x
xi )
i
N
N.B. : nel campo continuo si parla di densità di probabilità, mentre in quello
discreto ci si riferisce alla probabilità (gli intervalli di discretizzazione sono
infatti già definiti).
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
La probabilità congiunta P(x, y) è data dal verificarsi di entrambe le condizioni
x = xi e y = yj
P( x
xi , y
yj)
nij
N
E’ facile verificare che il numero di osservazioni rj è legato al numero di
rj
nij
osservazioni nij nel modo
i
Quindi è facile ricavare la seguente relazione:
P( y
yj)
P( x
xi , y
yj)
i
Si osserva che questa corrisponde alla probabilità marginale P(y). Il nome
marginale deriva quindi dal fatto che è ottenuta a partire dalla probabilità congiunta.
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
La probabilità condizionata P(x|y) è invece chiamata tale poiché esprime la
probabilità di verificarsi dell’osservazione x = xi condizionata ad un valore dato
di y = yj
P(x
xi | y
y j)
n ij
rj
Quindi la si può definire come già precedentemente visto
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
P(x | y)
P(x, y)
P(y)
Consideriamo:
x e y -> variabili casuali discrete di due
processi aleatori X,Y
N -> numero totale di osservazioni delle
due variabili
ci -> # osservazioni con x = xi
rj -> # osservazioni con y = yi
nij -> # osservazioni con x = xi e y = yi
P(x
xi | y
y j)
n ij
rj
N n ij
N rj
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
N n ij
rj N
P(x i , y j )
P(y j )
Inoltre si ha:
P(x
xi , y
y j)
n ij
n ij rj
N
rj N
P(x | y)P(y)
P(x, y)
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
n ij c i
ci N
P(y | x)P(x)
Dunque, per esprimere una pdf congiunta in termini di densità condizionate e
marginali si ha la regola di derivazione delle distribuzioni condizionate :
P ( x, y )
P ( x y ) P( y )
Le variabili x e y possono appartenere allo stesso insieme o a insiemi diversi, ciò
che conta ai fini della distribuzione congiunta è se le due variabili sono correlate
tra loro o meno
Nel caso in cui siano correlate:
(esempio dell’estrazione di
numeri della tombola)
P( x , y ) P ( x ) P ( y )
in quanto, dopo che y si è realizzato, se ci sono relazioni tra x e y l’insieme a cui
x appartiene risulta modificato e quindi anche la sua probabilità di realizzazione,
quindi in generale:
P( x )
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
P( x y )
invece se tra x e y non ci sono relazioni, esse risultano scorrelate (o indipendenti)
e quindi:
P ( x , y ) P( x ) P ( y )
Ovvero P( x ) P( x y ) in quanto, dopo che y si è verificato, nulla è cambiato
per il processo aleatorio X
La regola di derivazione può essere estesa a un numero qualsiasi di variabili nel
modo seguente:
P( x1 ,...., xn ) P( x1 x2, .., xn ) P( x2 x3 ,.., xn )...P( xn 1 xn ) P( xn )
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema della probabilità totale:
Py y
P x, y dx
x
P y | x P x dx
x
Esso asserisce che la densità di probabilità P(y) (densità marginale) può essere
ottenuta considerando i modi in cui y può verificarsi condizionatamente al
realizzarsi di uno specifico valore di x, pesata dalla probabilità che ciascuno di
questi valori di x si realizzi
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Supponiamo tre variabili:
P ( x , y , z ) P( x, y z ) P ( z ) P( x y , z ) P ( y z ) P ( z )
Si dice che x ed y sono indipendenti condizionatamente al verificarsi di z, se
P( x y, z ) P( x z )
E quindi si ha:
la x è indipendente dalla y data la
conoscenza di z. in altri termini z
contiene tutte le informazioni contenute
nella y riguardo a x
P ( x , y z ) P( x z ) P( y z )
NOTA: che due variabili x ed y siano indipendenti condizionatamente al verificarsi
di z, non vuol dire che sono indipendenti!
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Indipendenza condizionata
NOTA: che due variabili x ed y siano indipendenti condizionatamente al verificarsi
di z, non vuol dire che sono indipendenti:
P ( x, y )
P( x, y, z )dz
z Z
P( x z ) P( y z ) P( z )dz
z Z
Questi non sono uguali!
Mentre, se fossero indipendenti:
P ( x , y ) P( x ) P( y )
P( x | z ) P( z )dz
z Z
P( y | z ) P( z )dz
z Z
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Indipendenza condizionata
Il concetto di indipendenza condizionale sta alla base di molti algoritmi di data
fusion
Un esempio: consideriamo lo stato x di un sistema e due osservazioni z1 e z2
Le due osservazioni non sono indipendenti tra loro (in quanto dipendono entrambe
dallo stato x del sistema):
P( z1 , z2 )
P( z1 ) P( z2 )
Per contro, è ragionevole assumere che la sola cosa che le due osservazioni
hanno in comune è lo stato del sistema, perciò le osservazioni saranno
indipendenti una volta che lo stato si è realizzato, ossia si può scrivere che
P( z1 , z2 x )
P( z1 x) P( z2 x )
Questo si realizza ad esempio nel caso di
due misure della stessa grandezza in
presenza di rumore bianco
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Indipendenza condizionata
Laser rangefinder
x: distanza carrello-laser
La misura del laser è affetta
da rumore bianco
P x
La densità di probabilità è
triangolare in quanto maggiore
al centro che rappresenta il
punto di maggior passaggio.
Pz
La probabilità di ottenere una
misura z è circa uguale
P z|x
Pz
x
A carrello fermo la densità di
probabilità associata ad una
misura z condizionata allo
stato x è diversa dalla densità
del verificarsi semplicemente
una misura z (non
condizionata a nulla)
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Indipendenza condizionata - Esempio
Si considerino due variabili casuali x e z in cui x è lo stato di interesse
del sistema, o misurando, e z l’osservazione dello stato, o misura
La regola di derivazione della proprietà congiunta a partire dalle
probabilità condizionali può essere usata per esprimere la funzione
densità nei due modi diversi già visti:
P ( x, z )
P( x z ) P( z ) P ( z x ) P ( x )
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema di Bayes
Il teorema di Bayes si ottiene manipolando le probabilità condizionate nel seguente
modo:
P( x z )
P( z x ) P( x )
P( z )
… che esprime la densità di probabilità associata al misurando x una volta
ottenuta una misura z
Nota questa è proprio l’informazione che lo sperimentatore od il sistema
di controllo vuole avere!
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema di Bayes
Esempio applicativo caso discreto
• 2 box (B) -> red(r), blue(b)
• 2 tipi di oggetti (O) -> orange(o), green(g)
 Probabilità selezionare box rosso 40%
 Probabilità selezionare box blue 60%
P(B=r) = 4/10
P(B=b)= 6/10
1) Quale è la probabilità totale di selezionare un oggetto green?
2) Supponendo che abbiamo scelto un orange, quale è la probabilità che sia
selezionato dal box blue?
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema di Bayes
1) Quale è la probabilità totale di selezionare un oggetto green?
trovare P( O=g ) -> teorema probabilità totale
Py ( y )
Py x ( y x )Px ( x )dx
x
Dove:
P(x,y) = P(Box ,Object) = P(B,O)
Quindi integriamo su x che rappresenta i 2 Box:
P(O=g) = P(O=g|B=r) P(B=r) + P(O=g|B=b) P(B=b) = 2/8*4/10 + 3/4*6/10 = 11/20
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema di Bayes
2) Supponendo di aver scelto un oggetto orange, quale è la probabilità che sia
stato selezionato dal box blue?
trovare P( B=b | O=o ) = ?
P( x z )
P( z x ) P( x )
P( z )
-> teorema di Bayes
P(B=b | O=o) = P(O=o|B=b) P(B=b) / P(O=o)
P(B=b | O=o) = ( 1/4 * 6/10 ) / (1 - 11/20 ) = 1/3
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema di Bayes
Nel teorema di Bayes la distribuzione marginale P(z) serve semplicemente a
normalizzare la distribuzione condizionata, anche detta a posteriori (è un termine
che non dipende dalla variabile ma dal dato ricavato mediante misurazione che è
costante)
Grazie alla normalizzazione tramite P(z), la P(x|z) è proprio una funzione densità di
probabilità, infatti vale:
P( x z ) dx
x
x
P( z x ) P( x )
dx
P( z )
x
P( x, z )
dx
P( z )
P( z )
1
P( z )
Il teorema di Bayes fornisce un metodo diretto per combinare informazioni di
taratura dello strumento di misura P(z|x) con la stima a priori della densità di
probabilità associata al misurando P(x) per ottenere la densità di probabilità di x
condizionata alla misura z
Il teorema di Bayes è alla base di numerosi algoritmi di data fusion (come ad esempio il Filtro di Kalman)
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema di Bayes
Per comprendere a fondo il valore e l’utilità di questa (apparentemente) semplice
manipolazione delle funzioni densità di probabilità occorre analizzare il significato di
P(x), P(z), P(x|z), P(z|x)
La pdf P(x) è la funzione densità di probabilità precedente e quantifica
l’incertezza con cui si conosce lo stato, prima della nuova osservazione z
Al fine di ottenere maggiori informazioni circa la stato x, si effettua
un’osservazione (misura) z.
Le osservazioni sono modellate dalla funzione di densità di probabilità condizionata
P(z|x) che descrive, per ogni fissato stato x, la probabilità che sarà riscontrata
l’osservazione z
La densità di probabilità condizionata P(z|x) ricopre il ruolo di quello che è
chiamato modello del sensore. In tale modello x è il misurando, z è la misura
(osservazione dal punto di vista dello strumento di misura)
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema di Bayes
La funzione P(z|x) può essere pensata in due modi distinti
il primo è riferito alla taratura del sensore in tal caso la distribuzione si ottiene
fissando un particolare valore di x = xp e trovando quale risulta la pdf relativa
alla variabile z
Tale processo coincide con la taratura in cui si determina l’incertezza dello
strumento in funzione dell’ingresso
In questo caso P(z|xp) è considerata funzione di z
z
x=xp
P(xp)
Strumento
di Misura
z
P(z/x)
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Ripetendo più volte le misure
della stessa grandezza si
ottengono delle funzioni
densità di probabilità
P(z|x2 )
P(z|x1 )
x1
Teorema di Bayes - Taratura
x2
x
il secondo è riferito all’utilizzo del sensore per la misura in tal caso la
distribuzione si ottiene rilevando un particolare valore di z = zp e trovando quale
risulta la pdf relativa alla variabile x misurando
Tale processo coincide con il risultato del processo di misurazione in cui si
determina l’incertezza dello strumento in funzione dell’ingresso
In questo caso P(zp|x) è considerata funzione di x ed è nota come funzione di
verosimiglianza:
z
( x ) P( z p | x )
P(z 1|x)
x=F-1(z)
P(x)=
P(zp/x)
Strumento
di Misura z = F(x)
z=zp
P(zp/x)
P(z 2|x)
z2
z1
x
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema di Bayes - Misura
Suppose a certain drug test is 99% sensitive and 99% specific
that is, the test will correctly identify a drug user as testing positive 99% of the time,
and will correctly identify a non-user as testing negative 99% of the time.
This would seem to be a relatively accurate test, but Bayes' theorem will reveal a
potential flaw.
Let's assume a corporation decides to test its employees for use of a particular
drug, and 0.5% of the employees actually use the drug (from general statistics
about the local population).
We want to know the probability that, given a positive drug test, an employee is
actually a drug user
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema di Bayes – Esempio Drug Testing
Let "D" be the event of being a drug user
"N" indicate being a non-user
"+" be the event of a positive drug test
Noi vogliamo conoscere la probabilità P(D|+)
Abbiamo le seguenti informazioni:
Pr(D), or the probability that the employee is a drug user, regardless of any other
information. This is 0.005, since 0.5% of the employees are drug users
Pr(N), or the probability that the employee is not a drug user. This is 1-Pr(D), 0.995
Pr(+|D), or the probability that the test is positive, given that the employee is a drug
user. This is 0.99, since the test is 99% accurate
Pr(+|N), or the probability that the test is positive, given that the employee is not a
drug user. This is 0.01, since the test will produce a false positive for 1% of nonusers
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema di Bayes – Esempio Drug Testing
Pr(+), or the probability of a positive test event, regardless of other information.
Pr(+) = P(+|D)·P(D) + P(+|N) ·P(N) = 0.99·0.005 + 0.01·0.995 = 0.0149
This is 0.0149 or 1.49%, found by adding the probability that the test will produce a
true positive result in the event of drug use (= 99% x 0.5% = 0.495%) plus the
probability that the test will produce a false positive in the event of non-drug use (=
1% x 99.5% = 0.995%)
Mediante il teorema di Bayes possiamo adesso calcolare la probabilità che un
soggetto sia un consumatore di droghe a fronte di un test risultato positivo, ovvero
possiamo calcolare P(D|+):
P( | D ) P( D )
P( D | )
P( )
0.00495
0.00495+0.00995
P( | D ) P( D)
P( | D ) P( D ) P( | N ) P ( N )
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema di Bayes – Esempio Drug Testing
Notare che si tratta di una partizione tra P(+|D)· P(D) e P(+|N)· P(N) ed anche se
P(+|D) è molto alta e P(+|N) è molto bassa i contributi dovuti ai casi di positivo
P(+|D)· P(D) e quelli di falso positivo P(+|N)· P(N) divengono comparabili in quanto
la condizione che si sta cercando è rara (P(D) = 0.005) mentre quella
complementare è elevata (P(N) = 0.995) dando vita in generale ad un numero
comparabile di test effettivamente positivi e test falsamente positivi
Tale condizione è in generale vera per test su condizioni rare anche in presenza di
macchinari di rilevazione piuttosto accurati
Per questo è necessario effettuare test di verifica soprattutto in tali condizioni!
P( | D ) P( D )
P( | D ) P( D)
P( )
P( | D ) P( D ) P( | N ) P ( N )
0.00495
0.332 !
If P(+|N)P(N) = 0 it would had
0.00495+0.00995
been 1 instead !
P( D | )
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema di Bayes – Esempio Drug Testing
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Teorema di Bayes