STRATEGIE DI SENSOR FUSION APPLICATE AL TRACKING Progettazione di Sistemi di Controllo a.a. 2008/2009 Università degli Studi di Padova Dipartimento di Ingegneria dellInformazione Corso di laurea in Ingegneria dellAutomazione Azzollini Gennaro IAM Lunardi Luciano IAM Segantin Sebastiano IAM 19 Febbraio 2009

SOMMARIO G. Azzollini – L. Lunardi – S. Segantin 2 Progettazione di Sistemi di Controllo a.a. 2008/ Sensor Fusion & Data Fusion 2.Interazione tra sensori 3.Data Fusion con Filtro dInformazione 4.Data Fusion con Filtro Particellare 5.Architettura del sistema di tracking 6.Gestione delle videocamere & Data Association 7.Esperimenti presso il laboratorio NAVLAB

SENSOR & DATA FUSION SENSOR FUSION: Organizzazione e gestione intelligente di una moltitudine di sensori integrati in un unico sistema. Dipende dalle possibili interazioni tra i sensori. ESEMPI: rete di videocamere, monitoraggio ambientale, ecc… DATA FUSION: Elaborazione integrata delle misure acquisite da diversi sensori anche eterogenei. Dipende dallapproccio matematico e dalla struttura del sistema. ESEMPI: filtro dinformazione, filtro particellare, reti neurali, ecc… architettura centralizzata o distribuita 3 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009

INTERAZIONE TRA SENSORI (1) 4 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/ COMPLEMENTARIETÀ: Tipo 1 – Sensori che misurano diverse grandezze fisiche relative allo stesso fenomeno. ESEMPIO: tachimetro e odometro Tipo 2 – Sensori della stessa natura ma agenti su diverse zone di rilevamento. ESEMPIO: videocamere con aree visive contigue

INTERAZIONE TRA SENSORI (2) 5 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/ CONCORRENZA: Sensori che misurano la stessa grandezza fisica sulla stessa area di rilevamento. Aumenta laffidabilità del sistema di misura. Aumenta il livello di tolleranza a malfunzionamenti. Richiede ridondanza quantitativa di dispositivi. ESEMPI: videocamere che inquadrano la stessa porzione di ambiente termometri che misurano la temperatura della stessa stanza

INTERAZIONE TRA SENSORI (3) 6 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/ COOPERAZIONE: Sensori le cui osservazioni sono combinate allo scopo di ottenere informazioni non deducibili dalle misure di un singolo sensore. Diminuisce laffidabilità del sistema di misura. Implementazione complessa. ESEMPIO: due videocamere configurate per la stereo-visione

7 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 Operatore di data-fusion in sistemi multi sensore. Gestisce in modo naturale linformazione fornita da sensori complementari e concorrenti. Implementabile in architetture centralizzate o distribuite. Deducibile formalmente dalla teoria classica del Filtro di Kalman. Considerazioni generali FILTRO DI INFORMAZIONE (1)

FILTRO DI INFORMAZIONE (2) 8 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 Deduzione delle equazioni Filtro di Kalman classico (spazio di stato) Cambio di base Filtro di Kalman in forma dinformazione (spazio dinformazione) matrice di Informazione

FILTRO DI INFORMAZIONE (3) 9 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 Ipotesi per il Data-Fusion Sistema di misura con N sensori: Sensori con rumori di misura scorrelati:

FILTRO DI INFORMAZIONE (4) 10 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 Data-Fusion centralizzato Aggiornamento con la somma dei contributi di misura dei singoli sensori. Utilizzo di sensori privi di capacità computazionale. Utilizzo di sensori omogenei.

FILTRO DI INFORMAZIONE (5) 11 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 Data-Fusion parallelo Aggiornamento con la somma di stime e predizioni prodotte localmente. Necessita di sensori dotati di capacità computazionale. Utilizzo di sensori anche eterogenei.

FILTRO DI INFORMAZIONE (6) 12 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 Applicazione Simulazione di traiettoria e tracking

FILTRO DI INFORMAZIONE (7) 13 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 Applicazione Varianza derrore di stima e inizializzazione dei filtri

IL PARTICLE FILTER (1) Motivazioni: Ricerca di una soluzione con applicabilità più generale (non linearità, non gaussianità delle V.A. in gioco). Approccio risolutivo tramite generazione di numeri pseudo casuali. Appartenenza ai metodi statistici Monte Carlo: Approssimazione delle densità di probabilità come un insieme di campioni distribuiti come la densità di partenza. Simulazione della densità di probabilità per una successiva esplorazione dello spazio dei parametri del fenomeno in esame. Curiosità, origine dei metodi Monte Carlo: Prima formalizzazione: von Neumann. Prima applicazione: Progetto Manhattan. 14 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009

IL PARTICLE FILTER (2) Problema generale di stima: ricavarsi laspettazione di una qualsiasi funzione della distribuzione a posteriori: Applicando quindi il metodo Monte Carlo riformuliamo il problema esprimendo la distribuzione in singoli campioni : 15 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 (basta applicare la funzione ai singoli campioni dellintera distribuzione, per ricavarsi la densità di probabilità cercata) Concetto n°1 (1)

IL PARTICLE FILTER (3) La distribuzione a posteriori può essere espressa anche in questo modo: dove i pesi e il vettore mi identificano la densità di probabilità. Introduzione della Proposal Distribution: esprimere, in modo tale che sia possibile valutare dei suoi campioni. Si supponga esista una distribuzione, ad essa proporzionale, ed unaltra,, in cui è possibile estrarre dei campioni. I pesi possono quindi essere espressi come: Dove è appunto chiamata Proposal Distribution. 16 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 Concetto n°2 Concetto n°3

IL PARTICLE FILTER (4) Riprendendo quindi la (1), e dopo vari passaggi si arriva ad esprimere laspettazione di una qualsiasi funzione della distribuzione a posteriori in questo modo: Esprimendo tramite lapprossimazione fornita dal metodo Monte Carlo: si giunge al primo risultato (Importance sampling): dove rappresentano i pesi dimportanza normalizzati. 17 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 Concetto n°4

IL PARTICLE FILTER (5) Per esprimere quindi lImportance sampling in forma ricorsiva, ricorriamo ad una ipotesi aggiuntiva: processo markoviano. Grazie a questa ipotesi, è sufficiente conoscere la condizione dello stato al momento t-1 e le misure disponibili al momento t per potersi ricavare la distribuzione a posteriori. Lepressione dei pesi della versione approssimata della distribuzione a posteriori può quindi essere massaggiata fino ad arrivare al cosiddetto Sequential-Importance-Sampling: Dove i pesi sono appunto definiti da: 18 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 Concetto n°5

IL PARTICLE FILTER (6) RICAPITOLANDO: - scelta di q Scelta di = (sub-ottima) Applicazione di resampling ad ogni passo 19 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009

IL PARTICLE FILTER (7) Problema: centralizzare o decentralizzare? Il Particle Filter viene implementato tendenzialmente centralizzato in quanto la stima distribuita porta i seguenti problemi: Lutilizzo di n Particle filter, che apportano approssimazioni alla densità a posteriori, fornisce alleventuale algoritmo di fusione le stime che a loro volta possono essere approssimate ulteriormente: si evita quindi questa situazione e si cerca di consegnare allalgoritmo di fusione le misure integre. Lutilizzo di n Particle filter con il successivo algoritmo di fusione comporterebbe un maggiore peso computazionale rispetto la successione fusione-inseguimento. Ci si propone quindi di implementare due versioni diverse di Particle Filter: una parzialmente centralizzata (primo approccio) ed una completamente centralizzata (secondo approccio). 20 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009

IL PARTICLE FILTER (8) Primo approccio di sensor fusion: z1 z2.. zn Secondo approcio di sensor fusion: z1 z2.. zn 21 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 Stimatore di Markov (ai minimi quadrati) Particle Filter Particle Filter Layered z ^ x ^ x ^

IL PARTICLE FILTER (9) Primo approccio di sensor fusion: 1.fusione misure 2. Particle Filter con un ingresso e un uscita. 22 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009

IL PARTICLE FILTER (10) Algoritmo Ricorsivo 1 1.Estrazione di Ns particelle 2.Aggiornamento delle traiettorie 3.Aggiornamento del peso di ogni particella 4.Normalizzazione dei pesi 5.Resampling 23 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009

IL PARTICLE FILTER (11) Secondo approccio di sensor fusion, Layered Sampling Se valgono queste ipotesi aggiuntive: 1.Misure condizionatamente indipendenti tra di loro la Likelyhood dellm-esima misura viene fatta partecipare dopo aver applicato lm-esimo stato del modello di evoluzione Allora vale la seguente formulazione: dove 24 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009

Algoritmo ricorsivo 2: Solo 1 volta,Inizializzazione Per m=1:Q misure 1.Estrazione di Ns particelle (indice i) 2.Aggiorno i pesi 3.Resampling 4.Predisposizione per il passo successivo 25 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 IL PARTICLE FILTER (12)

IL PARTICLE FILTER (13) Simulazioni effettuate: Data Fusion con approccio del primo tipo, simulazione con modello lineare traiettorie RMSE 26 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009

IL PARTICLE FILTER (14) Simulazioni effettuate: Data Fusion con Layered Sampling, simulazione con modello lineare traiettorie RMSE 27 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009

IL PARTICLE FILTER (15) Simulazioni effettuate: Data Fusion con Layered Sampling, simulazione con modello non lineare traiettorie RMSE 28 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009

IL PARTICLE FILTER (17) Conclusioni Pregi: Robustezza ed affidabilità. Possibilità di impiego con modelli lineari e non lineari. Utilizzo conveniente in casistiche di sensor fusion centralizzato. Fusione integrata nel filtro nel caso del Layered Sampling. Difetti: Onere di taratura iniziale: legame tra varianza di ingresso e velocità di inseguimento; e tra numero di particelle e tolleranza di inseguimento. Comportamento ad alta frequenza. Complessità computazionale legata al numero di particelle ed al tipo di implementazione. 29 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009

ARCHITETTURA DEL SISTEMA G. Azzollini – L. Lunardi – S. Segantin 30 Progettazione di Sistemi di Controllo a.a. 2008/2009

ARCHITETTURA DEL SISTEMA (1) G. Azzollini – L. Lunardi – S. Segantin 31 Progettazione di Sistemi di Controllo a.a. 2008/2009 Rete di videocamere +Offre la possibilità di effettuare misure molto precise -Richiede lutilizzo di un programma per lassociazione delle osservazioni Se ne ipotizza lutilizzo in aree particolarmente sensibili

ARCHITETTURA DEL SISTEMA (2) G. Azzollini – L. Lunardi – S. Segantin 32 Progettazione di Sistemi di Controllo a.a. 2008/2009 Rete WSN +Estensione sullintera area da controllare +Grazie al sistema Teseo permette un labeling inequivocabile - Fornisce misure poco precise Indicatore fondamentale in aree occluse al campo visivo delle camere

ARCHITETTURA DEL SISTEMA (3) G. Azzollini – L. Lunardi – S. Segantin Sensori Rete di dispositivi in radiofrequenza Smart sensor 33 Progettazione di Sistemi di Controllo a.a. 2008/2009 Unità di base: Data fusion Video management Target association Organizzazione gerarchica del sistema

ARCHITETTURA DEL SISTEMA (4) G. Azzollini – L. Lunardi – S. Segantin Caratterizzazione architettura per filtro dinformazione parallelo Filtri di stima locali Detection & Association 34 Progettazione di Sistemi di Controllo a.a. 2008/2009

DETECTION G. Azzollini – L. Lunardi – S. Segantin 35 Progettazione di Sistemi di Controllo a.a. 2008/2009 Detection con background subtraction Inizializzazione di background con limmagine della pedana Individuazione iterativa del motion relativo al frame i-esimo Tresholding per eliminazione rumore ancora presente Oggetto del tracking E-puck Estrazione delle feature

ASSOCIATION (1) G. Azzollini – L. Lunardi – S. Segantin 36 Progettazione di Sistemi di Controllo a.a. 2008/2009 Approccio Nearest Neighbors Proiezione delle tracce raccolte nellistante di tempo della nuova osservazione Associazione dellosservazione alla traccia più vicina Facilmente decentralizzabile Vantaggi Se i robot sono troppo vicini gli errori di proiezione si ripercuotono sul risultato dellassociazione Svantaggi

ASSOCIATION (2) G. Azzollini – L. Lunardi – S. Segantin 37 Progettazione di Sistemi di Controllo a.a. 2008/2009 Algoritmo decentralizzato proposto Si basa sul principio del Nearest Neighbors introducendo un funzionale di costo Realizza un metodo ad hoc per lidentificazione univoca del robot

ASSOCIATION (2) G. Azzollini – L. Lunardi – S. Segantin 38 Progettazione di Sistemi di Controllo a.a. 2008/2009 1)Ogni camera riceve dal modulo centrale di Target association le stime dei robot ricavate con il filtro di fusione 2)Etichettazione delle feature 3)Costruzione della matrice delle adiacenze tra tutti i possibili robot inizializzata con la direzione 4)Per ogni led si effettua una minimizzazione rispetto alle stime precedenti utilizzando il funzionale di costo proposto 5)Associazione al robot che minimizza il funzionale con relativa cancellazinone delle adiacenze interessate id00 2Xid0 3XXid 4XXXi d 1,2 d 2,3 d 3,4 Algoritmo decentralizzato proposto

ASSOCIATION (3) G. Azzollini – L. Lunardi – S. Segantin 39 Progettazione di Sistemi di Controllo a.a. 2008/2009 Tracking efficace anche in situazioni critiche come lo scontro tra due robot

RISULTATI SPERIMENTALI (1) G. Azzollini – L. Lunardi – S. Segantin 40 Progettazione di Sistemi di Controllo a.a. 2008/2009 Risultati sperimentali del Multi Tracking Link esterno

Sperimentazione: Particolari 41 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009 RISULTATI SPERIMENTALI (2)

Conclusioni G. Azzollini – L. Lunardi – S. Segantin 42 Progettazione di Sistemi di Controllo a.a. 2008/2009 Raffinamento del modello usato nel Filtro Particellare. Implementazione del Filtro dInformazione secondo la teoria EKF. Utilizzo di un modello non lineare più complesso e fedele alla realtà. Utilizzare sensori più sofisticati (che ad esempio possano fornire velocità, accelerazioni, etc..) Modellizzazione del Particle Filter includendo la perdita di pacchetti. Utilizzo di un singolo Particle Filter Modificato per MultiTracking

43 G. Azzollini – L. Lunardi – S. Segantin Progettazione di Sistemi di Controllo a.a. 2008/2009