Filtro di Kalman e IMU.

Ciao a tutti,
sto frequentando ingegneria elettronica e per un progetto con degli amici devo utilizzare questo filtro.
Dobbiamo realizzare una sorta di IMU che permetta il tracking 3d di un oggetto.
Sfruttiamo un giroscopio a 3 assi ed un accelerometro a 3 assi.
Il giroscopio ha la funzione di individuare la direzione del vettore g, così da permetterci di eliminarlo.
L'accelerometro invece deve effettuare le vere misure di accelerazione che poi dovranno essere integrate 2 volte per trovare la distanza.
Noi dobbiamo raggiungere una precisione piuttosto elevata, dell'ordine del 0.5m.
Il sistema deve mantenere l'errore massimo sotto il mezzo metro per 60 secondi di utilizzo.
Il problema è che l'accelerometro è un oggetto piuttosto impreciso, infatti se il segnale minimo che dobbiamo riuscire a misurare ha ampiezza x, il rumore massimo che si somma ad esso ha ampiezza 200x.

Un mio professore mi ha orientato in questa direzione dicendomi che avrei trovato la soluzione. Il problema è che io non ho mai utilizzato il filtro di Kalman, e in aggiunta da quel che ho capito quello che dovrei utilizzare io sarebbe il FILTRO DI KALMAN ESTESO perchè il sistema non è lineare.

Quindi la mia richiesta è questa: c'è qualche testo di riferimento che ne spiega il funzionamento descrivendone anche un approccio pratico in ITALIANO?

Grazie mille in anticipo!

ps. Sono aperto anche a soluzioni alternative al filtro di kalman :slight_smile:

il filtro Kallmann permette di pulire le uscite dei sensori dal rumore, ma poi ti serve un algoritmo di "sensor fusion" che faccia la fusione delle letture per darti l'output.

Io non ho mai lavorato col kallmann, uso la DCM (direct-cosine-matrix, un pò di geometria lineare) che fa sia da filtro che da SensorFusion.

Ciao lesto,

scusa ma quella che dici tu non si può usare solo per ricavare l'orientamento del sistema?
Nella misura dell'orientamento accelerometro e giroscopio vengono usati diversamente da come li vorrei usare io.
Infatti si integrano i dati del giroscopio per ottenere la posizione angolare e si usano questi sui tempi brevi.
Questo perchè sono dati che hanno subito un integrazione e sono inaffidabili sui lunghi periodi a causa di eventuali derive.
In quel momento entra in gioco l'accelerometro, il quale si usa per capire su quali assi è scomposto il vettore g e quindi l'orientamento.
Al contrario del giroscopio i dati dell'accelerometro, essendo molto rumorosi risultano utili solo sul lungo periodo.

Mentre quello che voglio fare io è un 3d-tracking.
Il giroscopio mi serve solo per capire dove si trova il vettore g e rimuoverlo dall'accelerazione misurata dall'accelerometro.
Questa misura di accelerazione la dovrò poi integrare 2 volte per ottenere la distanza e quindi la posizione del mio oggetto rispetto all'origine.

Sbaglio?

Saluti e grazie!

a priori il sistema che possiedi va per integrazioni. tu devi calcolare l'orientamento nel modo più preciso possibile (sensor fusions, quindi ontegrazione n.1 ma filtrata) e poi calcoli il vettore G secondo l'orientamento attuale, lo sottrai l'accelerometro e hai il vettore accelerazione. da qui doppia integrazione. come vedi, l'orientamento più preciso possibile èalla base. il resto va a ingigantire questo errore

Ti ringrazio molto per la delucidazione!

Il sistema praticamente deve fare così:

  • Individuare l'orientamento del vettore g sfruttando un algoritmo di sensor-fusion tra giroscopio ed accelerometro
  • Sottrarre il vettore g all'accelerazione misurata dall'accelerometro
  • Il resto è cinematica sull'accelerazione misurata.

Il problema a questo punto è la sottrazione di g dall'accelerometro, infatti i dati sono molto rumorosi!
Come dicevo nel primo post il rumore è 200 volte più grande del quanto.
Come faccio ad ottenere dati esenti di rumore dall'accelerometro?

Il rumore dell'accelerometro è completamente gaussiano?
In questo caso basta fare una media su tanti valori per eliminarlo completamente e quindi usare quel valore di accelerazione?

Grazie mille ancora :slight_smile:

se l'accelerometro è romoroso bai di kallman, però sono perplesso dai valori di errore che mi dici

edit:non so chè tipo di errore hai, ma sono sicuro che non ha solo una origine. se poi alla fine si comporta come una gaussiana non saprei

se vuoi un esempio di come integrare kalman con arduino e sensori in i2c puoi vedere il mio post
http://forum.arduino.cc/index.php?topic=168757.0

ammetto che non ho mai provato il tutto ma da quello che ho visto in giro molti lo usano e funziona bene (attento che qui sono impostati solo 2 assi).

come ha detto lesto una volta che sai come è inclinato il piano ti basta un coseno ed un seno solamente per capire quanto ogni asse dell'accelerometro è influenzato dalla gravità, quindi una volta pulito il segnale della gravità puoi sapere a che accelerazioni è sottoposto il sistema e valutando la variazione dell'angolo anche quali rotazioni (oppure puoi semplicemente riprendere il segnale del giroscopio che non è soggetto a problemi di gravità in quanto misura la velocità angolare).

ricorda ancora che tu vuoi ricavare uno spostamento ed una rotazione quindi essendo che i sensori restituiscono solo accelerazioni e velocità angolari la variante tempo sarà molto importante ma capirai che dopo un lungo utilizzo guadagnerai molti errori (dovrai tenere comunque il codice il più snello possibile ma la comunicazione col pc sarà molto lenta, usa seriale a 115200baud per comunicare col pc che è il massimo sopportato da arduino almeno per limitare la durata delle comunicazioni)

Ciao ragazzi,
grazie della collaborazione!

Allora vogliamo ottenere una misura di posizione abbastanza precisa, come ho detto nel mio primo post l'errore deve rimanere entro 0.5m dopo 60 secondi di utilizzo.
Per raggiungere questo grado di precisione ho pensato di usare un ADC a 16bit:

Sistema: MMA7361L + ADC 16-bit

Sensibilità accelerometro: 800 mV/g
Fondo scala accelerometro: +/- 1.65V (tutti positivi)
Risoluzione ADC: 2^10 = 65536 bit
Vref ADC: 3,3V
Peso di un digit in V: 3,3V/65536 = 5,0410^-5 V/digit
Peso di un digit in m/s^2 : 5,04
10^-5 / 0,8V * 9,81 = 6,1710^-4 m / s^2digit

Considerando questo sistema l'errore intrinseco massimo che deriva dalla quantizzazione equivale a:
a_err_max = 0.5 * 6,17*10^-4 * 60^2 = 1,11 m

Guardando il risultato dell'ADC a 10-bit si nota che il valore della bracciata massimo di bracciata è di circa 60 digit. Questo equivale a:
Accelerazione: 0,0395 * 60 = 2,37 m/s^2
Tensione: 3,22*10-3 * 60 = 0,1932 V

Notiamo quindi che è possibile amplificare il valore in uscita dall'accelerometro, siccome la massima tensione dovuta all'accelerazione raggiungibile in fase di bracciata è ben lontana dal fondo scala:
Fattore_di_scala_max: 512/60 = 8,53
A questo punto il nuovo errore massimo dovuto alla quantizzazione si riduce:
Sensibilità accelerometro amplificato: 0,88,53 = 6,82 V/g
Peso di un digit in m/s^2: 6,17
10^-4 / 6,82= 9,0510^-5 m / s^2digit
a_err_max = 0,5 * 9,05*10^-5 * 60^2 = 0,16 m

Qui avevo fatto un esempio con un accelerometro che avevo già a casa.
Da notare che il quanto è di 5.04*10^-5.

Ho fatto poi alcune considerazioni sul rumore di quell'accelerometro:

Negli accelerometri il rumore è influenzato da 2 parametri:
Densità di rumore, che è un parametro intrinseco dell'accelerometro
Bandwidth che viene determinata con i filtri che vengono posti in uscita agli assi dell'accelerometro.
La maggioranza degli accelerometri possiede integrata già la resistenza per realizzare il filtro passa-basso in uscita, così è sufficiente aggiungere solo il condensatore.
Per calcolare la Bandwidth è sufficiente usare la seguente formula:
BW = 1/(2 * ? * R * C)
Rappresenta la massima frequenza alla quale possiamo campionare valori affidabili dalle uscite. Siccome non dobbiamo misurare delle vibrazioni, sono sufficienti poche volte al secondo.

Conoscendo il valore della larghezza di banda e la densità di rumore, il valore del rumore è ricavabile da questa relazione:
Noise = Noise Density * sqrt(BW * 1.6)
(Trovata sul sito dell'ANALOG DEVICES: Accelerometer Specifications - Quick Definitions | Analog Devices

Quindi prima di poter effettuare un calcolo di rumore sarebbe necessario decidere la frequenza di campionamento delle accelerazioni.

A. MMA7361L
Noise density: +/-350ug/ sqrt(Hz) misurato tra 0.1Hz e 1kHz
Frequenza di variazione accelerazione: 10Hz
Resistenza interna filtro RC: 32Kohm
Sensibilità accelerometro: 800 mV/g

Calcolo il valore del condensatore per ottenere una Bandwidth di 10Hz:
15 = 1/(2 * ? * 32K * C) --> C = 1/(2 * ? * 32K * 10) = 5*10-7 = 500nF

Avendo scelto una BW = 10 HZ calcoliamo il rumore:
Noise_g = 35010^-6 * sqrt(101,6) = 1,410^-3 g
Noise_V = 1,4
10^-3 * 0,8 = 1,12*10^-3 V = 1,12mV

Se noi volessimo abbinare questo accelerometro ad un ADC a 16-bit avremmo un quanto di :
q = 3.3/2^16 = 5,0410^-5
Che risulta ben inferiore al valore del rumore e quindi le nostre misure sarebbero totalmente inaccurate. Infatti il rapporto Rumore/quanto è:
SNR = q / Noise_V = 1,12
10^-3 / 5,04*10^-5 = 22.2

La frequenza che richiede il calcolo del rumore è quella con la quale varia la frequenza vero?

Comunque avevo esagerato in un calcolo precedente, il rumore è 22 volte più grande del quanto. Ma di che rumore si tratta? E' gaussiano?

Supponendo che i miei calcoli siano esatti, come faccio ad ottenere una misura corretta dall'accelerometro?

Saluti e grazie.

Risoluzione ADC: 2^10 = 65536 bit

no, se l'ADC ha unarisoluzionedi 10 BIT vuol dire che leggrai valori da 0 a 1023 (2^10 = 1024 digit)

Peso di un digit in V: 3,3V/1024= 0.0032226525V = 3.22mV (l'adc puoi usarlo con reference esterno a 3,3v, in questo modo non perdi precisione leggendolo in 5v)

1 G in Digit: 800mV/g / 3.22mV/digit = 248digit/g

se 1G = 9.8m/s^2 allora 248/9.8 = 25digit/(m/s^2) da quì otteniamo una precisione di circa 1/25 m/s^2 = 0.04m/s^2

Da notare che il quanto è di 5.04*10^-5.

ovvero 0.0000504 m/s^2? ahahahhahahahhahahahha chiedi alla nasa se ti prestano qualche strumento (senza offesa, ma credo ci sia qualche problema nei calcoli)

Siccome non dobbiamo misurare delle vibrazioni, sono sufficienti poche volte al secondo

assolutamente no, se lo fai poche volte al secondo il deltaT cresce, e come dice superlol (anche se pare ignorare cosa sia un integrale)

la variante tempo sarà molto importante

però è anche vero che leggere troppo in fretta da problemi....

QUINDI

compra dei sensori SERI, MEMS, con ADC ad almeno 12bit(4096digit), meglio se 16bit (65536), che oltre ad avere filtri interni, essere tutto on-chip eliminando molto le influenze esterne (il valore non viaggia grezzo ma in i2c o in SPI, protocolli digitali), e li usi a circa metà della loro frequenza massima (e quindi in generale ci aggiriamo sui 250Hz ODR Output Data Rate, ovvero valori al secondo al netto di filtri etc..) per avere un buon compromesso velocità - errore ADC

Bandwidth di 10Hz:

cioè 10 letture al secondo.. ovvero 1 lettura ogni 100ms... hai idea di quanti movimenti puoi perdere? anche un essere umano ha riflessi migliori (siam lì in realtà)

Ciao lesto,

no, se l'ADC ha unarisoluzionedi 10 BIT vuol dire che leggrai valori da 0 a 1023 (2^10 = 1024 digit)

Come ti ho detto l'ADC che intendo usare ha 16 bit di risoluzione. Il "10" scritto lì è semplicemente un errore derivante dal fatto che ho prima eseguito i calcoli con l'adc a 10 bit e poi ho incollato modificando i valori per quello a 16.

ovvero 0.0000504 m/s^2? ahahahhahahahhahahahha chiedi alla nasa se ti prestano qualche strumento (senza offesa, ma credo ci sia qualche problema nei calcoli)

Con "quanto" ovviamente intendo la più piccola differenza di tensione apprezzabile dall' ADC. E non il minimo valore di accelerazione misurabile...
Mentre il peso di un digit in m/s^2 è:
Peso di un digit in m/s^2 : 5,0410^-5 / 0,8V * 9,81 = 6,1710^-4 m / s^2*digit

Bandwidth di 10Hz:
cioè 10 letture al secondo.. ovvero 1 lettura ogni 100ms... hai idea di quanti movimenti puoi perdere? anche un essere umano ha riflessi migliori (siam lì in realtà)

Questa larghezza di banda è quella della variazione dell'accelerazione. La frequenza di campionamento non c'entra nulla.

Mi dispiace ma ho incollato un testo vecchio infatti questo è sbagliato:

BW = 1/(2 * ? * R * C)
Rappresenta la massima frequenza alla quale possiamo campionare valori affidabili dalle uscite. Siccome non dobbiamo misurare delle vibrazioni, sono sufficienti poche volte al secondo.

La larghezza di banda indicata lì rappresenta la frequenza di variazione dell'accelerazione.

Quindi ritornando al mio problema:
Supponendo che io riesca ad usare un algoritmo di sensor-fusion e che riesca a ricavare un orientamente sufficientemente preciso sfruttando accelerometro e giroscopio.
Come posso poi sottrarre tale valore alle misure dell'accelerometro che sono piene di rumore?
E' gaussiano quel rumore?

Saluti e grazie.

ah, ho fatto caso adesso del 10 a 16

Con "quanto" [..]

ok, allora i conti tornano

La frequenza di campionamento non c'entra nulla.

Ora, quì entriamo in un campo che purtoopo non ho studiato e quindi vado "a naso"
La frequenza di campionamento deve essere inferiore inferiore o uguale alla Bandwidth in questo caso, altrimenti leggi N volte lo stesso valore.. di solito si usa un ODR doppio così da essere sicuri di non perdere letture e, se non erro, dimezzare l'errore della sincronizzazione deltaT in 2.

Questa larghezza di banda è quella della variazione dell'accelerazione

quì mi perdo, variazione dell'accelerazione ovvero di quanto può variare, ma non capisco perchè la espimi in Hz.. dovresti esprimerla in mV o in digit

La larghezza di banda indicata lì rappresenta la frequenza di variazione dell'accelerazione.

e quì invece ti seguio.. forse anche sopra intendi la stessa cosa?

Come posso poi sottrarre tale valore alle misure dell'accelerometro che sono piene di rumore?

pulendo quel rumore con un filtro kallmann, per esempio.

E' gaussiano quel rumore?

come ti ho detto ha varie fonti, e in base alla fonte è differente.
qui: Accelerometer Noise - Stack Overflow

Grazie le rapide risposte,

Ora, quì entriamo in un campo che purtoopo non ho studiato e quindi vado "a naso"
La frequenza di campionamento deve essere inferiore inferiore o uguale alla Bandwidth in questo caso, altrimenti leggi N volte lo stesso valore.. di solito si usa un ODR doppio così da essere sicuri di non perdere letture e, se non erro, dimezzare l'errore della sincronizzazione deltaT in 2.

Qui ti devo contraddire, infatti è proprio il contrario. Per il teorama del campionamento di Shannon se:

  • Lo spettro del segnale che viene campionato è a banda limitata (e nel nostro caso lo è perchè in uscita dall'accelerometro ho un filtro passa basso che mi seleziona la larghezza di banda)
  • La frequenza di campionamento è: fc >= 2*BW
  • Non c'è errore di aliasing
    Allora è idealmente possibile ricostruire il segnale perfettamente.
    Quindi la frequenza di campionamento deve certamente essere maggiore di 2*10Hz nell'esempio che ho considerato.

quì mi perdo, variazione dell'accelerazione ovvero di quanto può variare, ma non capisco perchè la espimi in Hz.. dovresti esprimerla in mV o in digit

E' detto un po' male, ma quello voglio intendere è quello scritto nel link che mi hai postato.

pulendo quel rumore con un filtro kallmann, per esempio.

Però quello che vorrei capire è come funziona Kalman in questo caso. Infatti, se il rumore non è gaussiano, come fa questo algoritmo a eliminarlo?

come ti ho detto ha varie fonti, e in base alla fonte è differente.

Cosa vuol dire che il rumore varia come 1/f?
Appurato che f sia la frequenza con la quale varia l'accelerazione, quale caratteristica del rumore ha quell'andamento?
Lo spettro?
L'ampiezza?
...o cos'altro?

Grazie ancora per la disponibilità.

Per il teorama del campionamento di Shannon

studio, però comunque 20Hz son pochi. vedi ADXL345, lavora tranquillamente sui 200Hz di capionamento, quindi 400Hz di ODR

il drift è un errore per cui lo 0 del sensore non è più lo 0; in pratica il sensore si "stara", e questo sfasamento varia in base a fari fattori come temperatura, disturbi esterni, etc.

Questo avveine anche sul valore di G (ampiezza);

Spesso questi sensori (quelli di fascia più alta, almeno) hanno un termometro interno molto preciso proprio per fornire all'utente la possibilità di compensare la temperatura, visto che il semplice fatto di essere accesi scalda il componente, in generale penso che il drift è un probelma che si risolve a "banco" con teabelle di compensazione

E' detto un po' male, ma quello voglio intendere è quello scritto nel link che mi hai postato.

ho capito, direi che 10Hz sono più che sufficienti.

Però quello che vorrei capire è come funziona Kalman in questo caso. Infatti, se il rumore non è gaussiano, come fa questo algoritmo a eliminarlo?

il kalman elimina la parte gaussiana, che nel tuo caso è il problema più grande. Considera che la maggior parte di algoritmi che trovi in giro sono per quadricotteri, che per loro natura hanno un sacco di vibrazioni ad alta frequenza.

Ciao Lesto,

io non riesco a capire:

il kalman elimina la parte gaussiana, che nel tuo caso è il problema più grande. Considera che la maggior parte di algoritmi che trovi in giro sono per quadricotteri, che per loro natura hanno un sacco di vibrazioni ad alta frequenza.

Se il rumore è gaussiano ha per definizione valor medio nullo, quindi perchè è necessario usare Kalman?

Saluti e grazie :slight_smile:

perchè ha valor medio nullo nel lungo periodo, ma nel frattempo ti sei schiantato :grin: