2122-2-F1801Q149: Domande Particle Filter (e KF)

Buongiorno,
faccio un altro post perché ho diverse domande da fare, non necessariamente collegate all'argomento precedente.
1) Metodo per l'aggiornamento dei pesi:
Cercando informazioni online ho trovato il seguente algoritmo per l'aggiornamento dei pesi delle particelle di un particle filter, avendo a disposizione più misure:

Così ad occhio mi sembrava un metodo molto elegante, anche perché nel calcolo di p(z | xj) è possibile usare una qualsiasi pdf e grazie alla sommatoria non è necessario normalizzare.
Fonte:

https://robotics.stackexchange.com/questions/6423/particle-filter-weight-function

Nella prima consegna ho usato questo algoritmo con una normale di media 0 ed andavo a modulare la varianza in modo da avere risultati soddisfacenti.
Durante gli ultimi due incontri però, ho iniziato a chiedermi se questo metodo sia corretto e mi piacerebbe avere la sua opinione su di esso.
Ho provato anche ad utilizzare una mistura di gaussiane per il calcolo di p(z | xj) e, nel caso migliore, non ho ottenuto alcun miglioramento nelle performance del filtro.

2) Calcolo della distanza tra due coppie di punti in coordinate camera:
Per calcolare p(z | xj) ho usato la distanza tra i due punti (in coordinate camera) provenienti dalle misure e la pose della predizione, trasformata in coordinate camera.
Ho provato in due modi:
- Unendo le due coppie di punti in due vettori 4x1, facendo la differenza tra essi e poi prendendo la norma del vettore differenza (es. norm([4,9,5,7] - [3,9,6,2]))
- Prendendo la norma della differenza tra le due coppie di punti corrispondenti e poi sommando i due valori risultanti (es. norm([4,9]-[3,9]) + norm([5,7] - [6,2]))

Ho ottenuto risultati leggermente migliori con il secondo metodo e questo mi porta a pensare che ce ne possano essere altri.
Vorrei sapere se c'è un metodo preferibile per ottenere questa informazione.

3) Aggiunta del rumore sulle misure:
Le avevo parlato del problema che le particelle si condensavano sopra ad un unico stato a lezione ed ho già risolto quel problema sommando del rumore gaussiano (con matrice di covarianza specificata) alla predizione.
Mi è sembrato che si fosse detto anche di aggiungere del rumore alle misure, come abbiamo fatto con Kalman. (purtroppo, ho una pessima memoria e di questa cosa non ne sono sicuro al 100%)
Ho provato a farlo ed ho ottenuto risultati terribili, non importa come cambiassi i valori della matrice covarianza.
Ha quindi senso aggiungere del rumore alle misure (che in teoria sono già rumorose)?

4) Scrivendo il punto 3, mi è venuto in mente che anche durante lo sviluppo dell 'assignment sul filtro di Kalman, la matrice Q (4x4) non avesse un grosso impatto sull'output del filtro.
È possibile che ci sia un problema con le mie formule? Le ho controllate e confrontate più volte con quelle di Lisa, ma suppongo che sia possibile che abbiamo sbagliato entrambi.
Potrebbe essere causato da altro?

Questo è l'output del mio esercizio su KF in questo momento:
Lo stato del filtro sembra preferire di molto la predizione alle misure. Precedentemente mi sembrava normale, ma a questo punto ho paura di aver sbagliato qualcosa.
Output KF

Ri: Domande Particle Filter (e KF)

by Emanuele Giannuzzi - Tuesday, 8 February 2022, 4:33 PM

Scrivo qui per farlo tornare in cima.

Re: Domande Particle Filter (e KF)

by Domenico Giorgio Sorrenti - Wednesday, 9 February 2022, 11:52 PM

In primis, mi spiace per il ritardo, ma in questi giorni sono stato un po' sommerso, soprattutto per un impegno familiare. Risponderò un po' alla volta.

Inizio da "metodo per aggiornamento dei pesi". Non è che avessi ben capito la proposta che ha trovato su stack-exchange e sono andato a vedere il thread originale. A parte la sorpresa (qualcuno ha copiato e dato ai suoi studenti lo stesso identico problema), ho visto che il tipo che sembra aver posto la domanda (che non sembra quello indicato al termine del post-domanda) poi alla fine ha deciso di procedere... circa come dice il libro! Quindi non ho capito la proposta e nemmeno il commento sulla non necessità di normalizzare grazie alla sommatoria (la necessità di normalizzare mi pare che derivi dal fatto che uso un numero finito di particelle, mentre per arrivare ad 1 ci vorrebbe l'integrale su tutto il dominio della PDF). L'unica questione che non mi convince della risposta del thread è che parla di usare una normale scalare con varianza unitaria; noi abbiamo la varianza della misura che peraltro non è scalare; non capisco perché, dopo aver detto la cosa giusta non la implementi giusta (piazzando la incertezza della misura sulla misura effettiva e trovando il valore della PDF 4D in corrispondenza alla misura attesa 4D con x_i). PS Quando dice "utilizzare una mistura di gaussiane per il calcolo di p(z | xj)" non capisco bene: abbiamo una "misura" che invece di avere un modo ne ha 2 / 3 / 4; la mistura è quindi di normali corrispondenti ad una misura "singola". Inoltre, non capisco bene "per il calcolo di p( z | x_i)", noi usiamo la misura per determinare p( x_i | z). PPS: il teorema, per quello che so io, è della probabilità "totale", non assoluta.

Re: Ri: Domande Particle Filter (e KF)

by Domenico Giorgio Sorrenti - Sunday, 13 February 2022, 6:05 PM

Veniamo al punto 2) calcolo della distanza tra 2 coppie di punti in coordinate camera

Non mi è chiaro perché vuole calcolare questa distanza, se non perché non le è chiaro il discorso sull'uso della mistura. Provo a ripassare sulla questione cercando di "fare riferimento alla teoria".

Abbiamo il belief al tempo precedente, costituito dal set di particelle che mi arrivano dal passo precedente.
A ciascuna particella in modo indipendente applichiamo la "legge di moto" ovvero la transizione di stato; questo implica anche l'eventuale applicazione del rumore sulla transizione di stato (eventuale nel senso che esistono anche i sistemi deterministici, mentre quello dell'esercizio non lo è).
Ci ritroviamo, dopo il passo precedente, con una particella in un valore atteso della variabile di stato (una pose attesa). NB: collettivamente il set di particelle nei loro nuovi valori della variabile di stato (pose attese) costituiscono la predizione.
Vogliamo adesso capire quanto questa pose attesa sia compatibile con la misura. Chiamiamo "peso" la cifra che attribuiamo alla particella per rappresentare quanto questa risulti in accordo con la misura (il sistema sensoriale non è deterministico, così come la transizione di stato e quindi la predizione avrà una PDF diversa da un impulso).
A questo punto torniamo all'argomento affrontato più volte di cosa sia "la misura": la misura è il set di 2 o 3 o 4 uscite di tipo "bi-punto" prodotte dal sistema sensoriale {sensore + software}.
Cosa può significare "rappresentare quanto lo stato atteso sia in accordo con la misura"? Significa che certamente "trasformo mediante l'equazione di misura" lo stato atteso in una misura attesa, ma significa anche che ci vuole una PDF, che vive nello spazio delle misure (\( \mathbb{R}^4 \)), che rappresenti l'incertezza della misura. Il valore della PDF in corrispondenza alla misura attesa ci dice se quel valore dello stato è tanto o poco in accordo con quanto prodotto dal sistema sensoriale.
Come ottenere una PDF a partire dai 2 o 3 o 4 bi-punti generati dal sistema sensoriale ("pazzerello", ma realistico) costituisce l'argomento "mistura", più volte toccato.

>Per calcolare p(z | xj) ho usato la distanza tra i due punti
>(in coordinate camera) provenienti dalle misure e la pose della
>predizione, trasformata in coordinate camera.

Io parlerei di "distanza tra la misura attesa e la misura effettiva", quella che abbiamo chiamato innovazione quando si parlava di KF. I "due punti (in coordinate camera) provenienti dalle misure" sono la misura effettiva. La "pose della predizione, trasformata in coordinate camera" è la misura attesa.

>Ho provato in due modi:
>- Unendo le due coppie di punti in due vettori 4x1, facendo la
>  differenza tra essi e poi prendendo la norma del vettore differenza
>  (es. norm([4,9,5,7] - [3,9,6,2]))

Se questa distanza ci venisse utile, mi pare che così sarebbe ok.

>- Prendendo la norma della differenza tra le due coppie di punti
>  corrispondenti e poi sommando i due valori risultanti (es. norm([4,9]-[3,9])
>  + norm([5,7] - [6,2]))

Non capisco perché si inventi questo. È ancora una norma? Così sui due piedi non saprei nemmeno se segue la disuguaglianza triangolare... Nel caso questa distanza servisse, non andrebbe bene la normale norma L2 (sqrt( (x1-x2)^2 + (y1-y2)^2 + (z1-z2)^2 + (s1-s2)^2 )?

>Ho ottenuto risultati leggermente migliori con il
>secondo metodo e questo mi porta a pensare che ce
>ne possano essere altri. Vorrei sapere se c'è un metodo
>preferibile per ottenere questa informazione.

Di norme ce ne possono essere tante, il problema che vedo, a parte la casualità del fatto che abbia avuto risultati migliori in un modo o in un altro, è che non capisco come il procedere in questo modo segua la teoria di PF (una PDF il cui valore in corrispondenza alla misura attesa rappresenta quanto quel valore della variabile di stato sia in accordo con quanto prodotto dal sistema sensoriale).

Re: Domande Particle Filter (e KF)

by Domenico Giorgio Sorrenti - Sunday, 13 February 2022, 6:18 PM

Vengo al punto 3) Aggiunta del rumore sulle misure:

>Le avevo parlato del problema che le particelle si condensavano
>sopra ad un unico stato a lezione ed ho già risolto quel problema
>sommando del rumore gaussiano (con matrice di covarianza specificata)
>alla predizione.

Non userei il termine "predizione" in quanto "la predizione" è costituita dall'intero set di particelle, non da una singola particella. Quindi secondo me dovrebbe dire "sommando una estrazione da un'urna gaussiana di covarianza specificata a ciascuna particella". Inoltre, da quanto scrive, deduco che ritenga essere giusto soltanto aggiungere rumore normale alla pose, ma chi ha detto che l'urna debba essere gaussiana? In robotica mobile è noto che il rumore sull'odometria non è proprio gaussiano (veda le banane relative agli odometry e velocity models sul libro di testo). Vi dicevo infatti che si potrebbe anche ipotizzare normali gli errore sugli archi e propagarli attraverso la funzione odometria oppure seguire uno degli approcci del libro, ad esempio odometry model (una rotazione sul posto, una traslazione, un'altra rotazione sul posto).

>Mi è sembrato che si fosse detto anche di aggiungere del rumore
>alle misure, come abbiamo fatto con Kalman. (purtroppo, ho una
>pessima memoria e di questa cosa non ne sono sicuro al 100%)
>Ho provato a farlo ed ho ottenuto risultati terribili, non importa
>come cambiassi i valori della matrice covarianza.
>Ha quindi senso aggiungere del rumore alle misure (che in teoria
>sono già rumorose)?

La frase "le misure sono soggette a rumore" è vera, ma i numeri che otteniamo dal sistema sensoriale hanno già subito il degrado del rumore. Quindi ha senso trovare un buon modello di come sono rumorose le misure, ma non ha senso aggiungere rumore ai dati ottenuti dal sistema sensoriale perché come dice "sono già rumorose".

Re: Domande Particle Filter (e KF)

by Domenico Giorgio Sorrenti - Sunday, 13 February 2022, 6:30 PM

Circa il 4)

>Scrivendo il punto 3, mi è venuto in mente che anche
>durante lo sviluppo dell'assignment sul filtro di Kalman,
>la matrice Q (4x4) non avesse un grosso impatto
>sull'output del filtro. 
>È possibile che ci sia un problema con le mie formule?
>Le ho controllate e confrontate più volte con quelle di
>Lisa, ma suppongo che sia possibile che abbiamo sbagliato
>entrambi.
>Potrebbe essere causato da altro?
>Questo è l'output del mio esercizio su KF in questo momento:
>Lo stato del filtro sembra preferire di molto la predizione
>alle misure. Precedentemente mi sembrava normale, ma a questo
>punto ho paura di aver sbagliato qualcosa.

Le formule non penso siano sbagliate, sono in generale quelle del libro e non credo abbia sbagliato gli jacobiani (avrà, presumo, usato il sistema di matematica simbolica di matlab). In generale ci potrebbe essere un problema di valori inseriti nelle matrici di covarianza (missing data problem). Guardando le figure non vedo la predizione, ma effettivamente c'è qualcosa di strano: ad ogni curva (se la legenda è corretta) la sua stima "gira prima delle misure". Se il valore della incertezza sulle misure è troppo piccolo, allora dovrebbe seguire, come dice, la predizione, ma come fa la stima a girare a sinistra prima delle misure (ad esempio alla prima curva)? In effetti questo non mi torna.