M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Fondamenti di sensor fusion Mariolino De...

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Fondamenti di “sensor fusion”

Mariolino De Cecco, Luca Baglivo


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à

Cinematica dei Manipolatori e dei robot mobili

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


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


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.

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 relativamente semplici, elevata frequenza di aggiornamento, assenza di rumore ad alta frequenza

integrano incrementi relativi e quindi deriva

Misura riferita all’ambiente (laser, ultrasuoni, cavo annegato)

misura riferita all’ambiente quindi assenza di deriva

bassa ripetibilità, bassa frequenza di aggiornamento della misura

Caso Robot Mobili


Esempio di localizzazione ottenuta con soli dati incrementali


Esempio di localizzazione ottenuta con 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 e da un intervallo di valori centrato attorno alla media e di ampiezza pari a un certo coefficiente moltiplicato per la deviazione standard della distribuzione.

Modelli probabilistici


Valori notevoli:

p x

p x

p x

( ) .

( ) .

( ) .

0 680

2 0 950

3 0 997

-5

0.1

0.2

0.3

0.4

5

-3 -2 -1 0 1 2 30

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

z

p(zi<z) = F(z)

p(zi >z)

( )( )20

221

2

x x

P x e σ

π σ

−−

⋅=⋅

( )( )20

221

2

x

P x e dξμ σ

σ

μ σ

μ σ ξπ σ

−+−

⋅

−

− < = ⋅⋅∫

P(x)=0 … perchè?



x

Intervallo pari a ±2

x0: misura

Intervallo pari a ±2

… quindi possiamo dire che la misura è pari a:

• x0 ± 2 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-2 x+2]


x


Modello di Stato ed Osservazione

I concetti di stato e di osservazione di un sistema sono alla base di varie teorie, tra cui la teoria dei controlli e la teoria del data fusion.

Una volta definito un modello fisico della natura di un sistema, più o meno vicino alla realtà, il concetto di stato di un sistema è associato al concetto di valore vero delle variabili di interesse di quel sistema, ossia dei valori che possono assumere le grandezze fisiche o le proprietà in genere che definiscono il modello del sistema. Il valore vero di uno stato è anche detto misurando, ossia grandezza fisica il cui valore è oggetto di indagine nel processo di misurazione

( ) ( ) ) ( ) ( ) ( ) ( )x t F t x t B t u t G t v t= + +(&

Forma standard del modello di osservazione (o modello di misura di un sistema):

( ) ( ) ( ) ( ) ( )z t H t x t D t w t= +

stato ingresso (controllo)

stato

rumore

derivata dello stato

misura

rumore


Il modello di stato, se rispecchiasse esattamente la natura del sistema, fornirebbe una conoscenza completa della sua evoluzione, una volta note le condizioni iniziali e le variabili di controllo

Purtroppo, a causa di fattori di incertezza di varia natura, tra cui la stessa inevitabile approssimazione con cui il modello è costruito, spesso la conoscenza delle variabili di interesse derivante dal modello di stato non è sufficiente.

Occorre perciò ottenere delle informazioni aggiuntive effettuando delle osservazioni (misure) dello stato.

Anch’esse rispondono a un modello più o meno completo che descrive come l’osservazione è legata allo stato da cui dipende e vi si può associare un’incertezza (di tipo probabilistico o di altro tipo)

( ) ( ) ) ( ) ( ) ( ) ( )x t F t x t B t u t G t v t= + +(&

( ) ( ) ( ) ( ) ( )z t H t x t D t w t= +

( )( )

) ( )

) ( )

x t v t

z t w t

ε

ε

±

±

(

(

Modello di Stato ed Osservazione



Conseguenza :

Il risultato della misura può differire ogni qualvolta si ripete l’operazione

di misura stessa, a causa di variazioni nei modelli non controllabili

nell’esperimento.

Un esperimento che porta a misure differenti anche se la procedura di

misura è la stessa ad ogni tempo, è definito RANDOM EXPERIMENT.

Il set di tutti i possibili risultati di un random experiment, è definito come

SAMPLE SPACE, e può essere:

• discreto -> numero finito o contabile di possibili risultati

• continuo -> quando è riferito ad un intervallo di numeri reali



realizzazione di un valore y del processo aleatorio Y

y viene anche detta variabile casuale

La funzione densità di probabilità P(y), è valida se: ( ) 0,P y y> ∀

1

0

( )y

yP y dy∫

Ricordiamo sempre che la probabilità che si manifesti la variabile casuale y

[y0 y1] vale:

( ) 1y Y

P y dy∈

=∫


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:

( ) ( , )y xy

x

P y P x y dx=∫

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 yP x y

P y≡

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:

N

cxxP ii == )(

N.B. : nel campo continuo si parla di densità di probabilità, mentre in quello

discreto ci si riferisce alla probabilità.


N

nyyxxP ijji === ),(

La probabilità congiunta P(x, y) è data dal verificarsi di entrambe le condizioni

x = xi e y = yj

∑=i

ijj nrE’ facile verificare che il numero di osservazioni rj è legato al numero di

osservazioni nij nel modo

∑ ====i

jij yyxxPyyP ),()(Quindi è facile ricavare la seguente relazione:

Si osserva che questa corrisponde alla probabilità marginale P(y). Il nome

marginale deriva quindi dal fatto che è ottenuta a partire dalla probabilità congiunta.


€

P(x | y) =P(x, y)

P(y)Quindi la si può definire come già precedentemente visto

€

P(x = x i | y = y j ) =n ijrj

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


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 = x i | y = y j ) =n ijrj

=N

N

n ijrj

=N

rj

n ijN

=P(x i , y j )

P(y j )


€

P(x = x i , y = y j ) =n ijN

=n ijrj

rjN

=n ijc i

c iN

P(x, y) = P(x | y)P(y) = P(y | x)P(x)

Inoltre si ha:


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

( , ) ( ) ( )P x y P x P y≠ ⋅

Nel caso in cui siano correlate:

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 P x y≠

(esempio dell’estrazione di numeri della tombola)


Ovvero in quanto, dopo che y si è verificato, nulla è cambiato

per il processo aleatorio X

invece se tra x e y non ci sono relazioni, esse risultano scorrelate (o indipendenti)

e quindi:( , ) ( ) ( )P x y P x P y= ⋅

( ) ( )P x P x y=

La regola di derivazione può essere estesa a un numero qualsiasi di variabili nel

modo seguente:

1 1 2, 2 3 1( ,...., ) ( .., ) ( ,.., )... ( ) ( )n n n n n nP x x P x x x P x x x P x x P x−=


Teorema della probabilità totale:

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€

Py y( ) = P x, y( )dx =x

∫ P y | x( )P x( )dxx

∫


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:

( , ) ( ) ( )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!

la x è indipendente dalla y data la conoscenza di z. in altri termini z contiene tutte le informazioni contenute nella y riguardo a x

Indipendenza condizionata


( , ) ( , , ) ( ) ( ) ( )z Z z Z

P x y P x y z dz P x z P y z P z dz∈ ∈

= =∫ ∫

NOTA: che due variabili x ed y siano indipendenti condizionatamente al verificarsi

di z, non vuol dire che sono indipendenti:

Mentre, se fossero indipendenti:

( , ) ( ) ( ) ( | ) ( ) ( | ) ( )z Z z Z

P x y P x P y P x z P z dz P y z P z dz∈ ∈

= ⋅ = ⋅ ⋅ ⋅∫ ∫

Questi non sono uguali!



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):

1 2 1 2( , ) ( ) ( )P z z P z P z≠

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

1 2 1 2( , ) ( ) ( )P z z x P z x P z x=Questo si realizza ad esempio nel caso di due misure della stessa grandezza in presenza di rumore bianco



Laser rangefinderx: distanza carrello-laser

La misura del laser è affetta da rumore bianco

La densità di probabilità è triangolare in quanto maggiore al centro che rappresenta il punto di maggior passaggio.

La probabilità di ottenere una misura z è circa uguale

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)

Indipendenza condizionata - Esempio

€

P x( ) ≅ P z( )

€

x

€

P z | x( ) ≠ P z( )


Teorema di Bayes

Si considerino due variabili casuali x e z

Ai fini applicativi del teorema di Bayes al problema del data fusion si attribuisce

alla variabile x lo stato di interesse del sistema, o misurando, e a z

l’osservazione di quello stato, o misura

Esempio: lo stato di interesse può essere la velocità lineare di un veicolo e

l’osservazione può essere un valore misurato di velocità angolare delle ruote,

che attraverso la frequenza rilevata sull’encoder associato e la conoscenza del

raggio delle ruote, porta alla stima della velocità lineare

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= =


Teorema di Bayes

Il teorema di Bayes si ottiene manipolando le probabilità condizionate nel seguente modo:

( ) ( )( )

( )

P z x P xP x z

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!


Teorema di Bayes

Esempio applicativo caso discreto

• 2 box (B) -> red(r), blue(b)

• 2 tipi di oggetti (T) -> 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?


Teorema di Bayes

trovare P( T=g ) -> teorema probabilità totale

( ) ( ) ( )y xy xx

P y P y x P x dx=∫

P(T=g) = P(T=g|B=r) P(B=r) + P(T=g|B=b) P(B=b) = 11/20

1) Quale è la probabilità totale di selezionare un oggetto green?


Teorema di Bayes

P(T=g) = 11/20

trovare P( B=b | T=o ) -> teorema di Bayes

P(B=b | T=o) = P(T=o|B=b) P(B=b) / P(T=o)( ) ( )

( )( )

P z x P xP x z

P z=

P(B=b | O=o) = ( 1/4 * 6/10 ) / (1 - 11/20 ) = 1/3

2) Supponendo che abbiamo scelto un orange, quale è la probabilità che sia selezionato dal box blue?


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:

( ) ( ) ( , ) ( )( ) 1

( ) ( ) ( )x x x

P z x P x P x z P zP x z dx dx dx

P z P z P z⋅ = ⋅ = ⋅ = =∫ ∫ ∫

Il teorema di Bayes fornisce un metodo diretto per combinare le informazioni sullo

strumento di misura P(z|x) con la stima della densità di probabilità associata al

misurando P(x) per ottenere la densità di probabilità di x (ad esempio precedente)

condizionata alla misura z, quindi l’informazione cercata sullo stato

Il teorema di Bayes è alla base di numerosi algoritmi di data fusion (come ad esempio il Filtro di Kalman)


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 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 dello strumento di misura)


Teorema di Bayes - Taratura

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

xx2x1

z

P(z|x2)

P(z|x1)

Strumento di Misura

zx=xp

P(xp) P(z/x)

Ripetendo più volte le misure della stessa grandezza si ottengono delle funzioni densità di probabilità


Teorema di Bayes - Misura

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:

Strumento di Misura z = F(x)

z=zpx=F-1(z)

P(x)= P(zp/x)

P(zp/x)

x

z2

z1

z

P(z2|x)P(z1|x)

( ) ( | )px P z xΛ =


Teorema di Bayes – Esempio Drug Testing

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



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 non-users



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|+):

( | ) ( ) ( | ) ( )( | )

( ) ( | ) ( ) ( | ) ( )

0.00495

0.00495+0.00995

P D P D P D P DP D

P P D P D P N P N

+ ⋅ + ⋅+ = =

+ + ⋅ + + ⋅

=



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!

( | ) ( ) ( | ) ( )( | )

( ) ( | ) ( ) ( | ) ( )

0.004950.332

0.00495+0.00995

P D P D P D P DP D

P P D P D P N P N

+ ⋅ + ⋅+ = =

+ + ⋅ + + ⋅

= = ! If P(+|N)P(N) = 0 it would had been 1 instead !


Teorema di Bayes

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Fondamenti di sensor fusion Mariolino De...

Documents

Transcript of M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Fondamenti di sensor fusion Mariolino De...