M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion 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à 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

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion 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. Caso Robot Mobili

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion 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 VantaggiSvantaggi 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

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion 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  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

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Valori notevoli: px px px (). (). ().       z p(z i <z) = F(z) p(z i >z) P(x)=0 … perchè? Modelli probabilistici

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion x Intervallo pari a ±2  x 0 : misura Intervallo pari a ±2  … quindi possiamo dire che la misura è pari a: x 0 ± 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  ] Modelli probabilistici x

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

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion 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:

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Consideriamo: x e y -> variabili casuali discrete di due processi aleatori X,Y N -> numero totale di osservazioni delle due variabili c i -> # osservazioni con x = x i r j -> # osservazioni con y = y i n ij -> # osservazioni con x = x i e y = y i La probabilità P(x) è definita come la probabilità di verificarsi della condizione x = x i ed è pari a: 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 = x i e y = y j E’ facile verificare che il numero di osservazioni r j è legato al numero di osservazioni n ij 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.

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Quindi la si può definire come già precedentemente visto La probabilità condizionata P(x|y) è invece chiamata tale poiché esprime la probabilità di verificarsi dell’osservazione x = x i condizionata ad un valore dato di y = y j

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Consideriamo: x e y -> variabili casuali discrete di due processi aleatori X,Y N -> numero totale di osservazioni delle due variabili c i -> # osservazioni con x = x i r j -> # osservazioni con y = y i n ij -> # osservazioni con x = x i e y = y i

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Inoltre si ha:

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion 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: 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: (esempio dell’estrazione di numeri della tombola)

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion 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: La regola di derivazione può essere estesa a un numero qualsiasi di variabili nel modo seguente:

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion 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

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Supponiamo tre variabili: Si dice che x ed y sono indipendenti condizionatamente al verificarsi di z, se E quindi si ha: 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

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion NOTA: che due variabili x ed y siano indipendenti condizionatamente al verificarsi di z, non vuol dire che sono indipendenti: Mentre, se fossero indipendenti: Questi non sono uguali! Indipendenza condizionata

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion 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 z 1 e z 2 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

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion 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

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Teorema di Bayes 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:

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: … 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 trovare P( O=g ) -> teorema probabilità totale 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 1)Quale è la probabilità totale di selezionare un oggetto green? Dove: P(x,y) = P( Box,Object ) = P( B,O )

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Teorema di Bayes trovare P( B=b | O=o ) = ? -> 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 2) Supponendo di aver scelto un oggetto orange, quale è la probabilità che sia stato selezionato dal box blue?

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: 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 - 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 = x p 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|x p ) è considerata funzione di z Strumento di Misura z x=x p P(x p ) P(z/x) Ripetendo più volte le misure della stessa grandezza si ottengono delle funzioni densità di probabilità

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion 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 = z p 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(z p |x) è considerata funzione di x ed è nota come funzione di verosimiglianza: Strumento di Misura z = F(x) z=z p x=F -1 (z) P(x)= P(z p /x) P(z p /x)

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion 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

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

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.995 = This is 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|+):

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! ! If P(+|N)  P(N) = 0 it would had been 1 instead !

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