Fondamenti di “sensor fusion” Mariolino De Cecco, Luca Baglivo

Cinematica dei Manipolatori e dei robot mobili 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 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

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

Vantaggi Svantaggi 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

Modelli probabilistici 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. Modelli probabilistici

p x ( ) .      680 2 950 3 997 p(zi >z) p(zi<z) = F(z) -3 -2 -1 1 2 3 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 z p(zi<z) = F(z) p(zi >z) 5 Valori notevoli: P(x)=0 … perchè? p x ( ) .      680 2 950 3 997 Valori notevoli spiegare il significato di questi valori Modelli probabilistici

Modelli probabilistici 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] Modelli probabilistici

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 rumore derivata dello stato stato ingresso (controllo) Forma standard del modello di osservazione (o modello di misura di un sistema): stato misura rumore Modello di Stato ed Osservazione

Modello di Stato ed Osservazione 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) Modello di Stato ed Osservazione

Modelli probabilistici 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 Modelli probabilistici

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: Ricordiamo sempre che la probabilità che si manifesti la variabile casuale y  [y0 y1] vale: 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: 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: In generale:

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.B. : nel campo continuo si parla di densità di probabilità, mentre in quello discreto ci si riferisce alla probabilità.

La probabilità congiunta P(x, y) è data dal verificarsi di entrambe le condizioni x = xi e y = yj E’ facile verificare che il numero di osservazioni rj è legato al numero di osservazioni nij nel modo 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.

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 Quindi la si può definire come già precedentemente visto

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

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

invece se tra x e y non ci sono relazioni, esse risultano scorrelate (o indipendenti) e quindi: Ovvero 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:

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

Indipendenza condizionata Supponiamo tre variabili: Si dice che x ed y sono indipendenti condizionatamente al verificarsi di z, se la x è indipendente dalla y data la conoscenza di z. in altri termini z contiene tutte le informazioni contenute nella y riguardo a x E quindi si ha: NOTA: che due variabili x ed y siano indipendenti condizionatamente al verificarsi di z, non vuol dire che sono indipendenti! Indipendenza condizionata

Indipendenza condizionata NOTA: che due variabili x ed y siano indipendenti condizionatamente al verificarsi di z, non vuol dire che sono indipendenti: Questi non sono uguali! Mentre, se fossero indipendenti: Indipendenza condizionata

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): 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 Questo si realizza ad esempio nel caso di due misure della stessa grandezza in presenza di rumore bianco Indipendenza condizionata

Indipendenza condizionata - Esempio Laser rangefinder x: 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

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: Teorema di Bayes

Il teorema di Bayes si ottiene manipolando le probabilità condizionate nel seguente modo: … 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

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

Quale è la probabilità totale di selezionare un oggetto green? trovare P( T=g ) -> teorema probabilità totale P(T=g) = P(T=g|B=r) P(B=r) + P(T=g|B=b) P(B=b) = 11/20 Teorema di Bayes

Teorema di Bayes P(T=g) = 11/20 2) Supponendo che abbiamo scelto un orange, quale è la probabilità che sia selezionato dal box blue? 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(B=b | O=o) = ( 1/4 * 6/10 ) / (1 - 11/20 ) = 1/3 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: 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

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 Ripetendo più volte le misure della stessa grandezza si ottengono delle funzioni densità di probabilità x=xp z Strumento di Misura P(xp) P(z/x) Teorema di Bayes - Taratura

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: x=F-1(z) z=zp Strumento di Misura z = F(x) P(x)= P(zp/x) P(zp/x) Teorema di Bayes - Misura

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 Teorema di Bayes – Esempio Drug Testing

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 non-users Teorema di Bayes – Esempio Drug Testing

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|+): Teorema di Bayes – Esempio Drug Testing

! 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! ! If P(+|N)P(N) = 0 it would had been 1 instead ! Teorema di Bayes – Esempio Drug Testing

Teorema di Bayes