1 Testo dell’esercizio
Si vuole stimare l’angolo \( \theta\) di un segmento corporeo usando una IMU composta da:
- un giroscopio, che misura la velocità angolare ma presenta un bias;
- un accelerometro, che fornisce una misura diretta dell’angolo rispetto alla gravità, affetta però da rumore.
Per rendere il modello più realistico, si sceglie come stato del sistema:
dove:
\( \theta_k\) è l’angolo al tempo \(k\);
\(b_k\) è il bias del giroscopio.
Il modello dinamico è:
\(x_k = F\,x_{k-1} + B\,u_k + w_k\)
Con
e il modello di misura è:
Sono assegnati i seguenti dati numerici:
Al passo \(k=1\) si hanno:
Si richiede di eseguire un ciclo completo del filtro di Kalman, calcolando:
- stato predetto;
- covarianza predetta;
- innovazione;
- covarianza dell’innovazione;
- guadagno di Kalman;
- stato aggiornato;
- covarianza aggiornata.
2 Teoria necessaria per risolvere l’esercizio
Il filtro di Kalman è un metodo ricorsivo che combina:
- un modello dinamico, utile per prevedere come evolve lo stato;
- una misura sperimentale, utile per correggere la previsione.
Nel caso delle IMU, il principio fisico è questo:
- il giroscopio descrive bene le variazioni rapide dell’angolo, ma tende a introdurre deriva nel tempo;
- l’accelerometro fornisce un riferimento assoluto rispetto alla gravità, ma è più rumoroso.
Per questo motivo, la stima migliore si ottiene fondendo le due informazioni.
2.1 Stato del sistema
Qui lo stato contiene due variabili:
L’idea è che il bias del giroscopio non sia nullo, ma vari lentamente. In questo modo il filtro non corregge solo l’angolo, ma impara anche il bias.
2.2 Equazione di evoluzione
Se il giroscopio misura:
allora in forma discreta l’angolo può essere aggiornato come:
mentre il bias viene approssimato come quasi costante:
Questo porta alla forma matriciale:
\( x_k = F\,x_{k-1} + B\,u_k + w_k \)
Dove \(W_k\) rappresenta il rumore di processo.
2.3 Equazione di misura
L’accelerometro fornisce una misura diretta dell’angolo:
quindi la matrice di osservazione è:
\( H= [1 \; \; 0 ] \)
Questo significa che il sensore misura l’angolo, ma non misura direttamente il bias.
1.1 Significato di P, Q,R
- \(P\) Matrice di covarianza dell’errore di stima;
- \(Q\) rumore di processo, cioè incertezza sul modello dinamico;
- \(R\) rumore di misura, cioè incertezza del sensore.
2.5 Equazioni principali del filtro di Kalman
2.5.1 Predizione dello stato
2.5.2 Predizione della covarianza
2.5.3 Innovazione
2.5.4 Covarianza dell’innovazione
2.5.5 Guadagno di Kalman
2.5.6 Aggiornamento dello stato
2.5.7 Aggiornamento della covarianza
\(P_{k|k} = (I – K_k H)\,P_{k|k-1}\)
3 Consigli di problem solving ed errori comuni
Quando si affronta un esercizio sul filtro di Kalman, conviene seguire sempre lo stesso schema:
- identificare chiaramente stato, ingresso e misura;
- costruire con attenzione le matrici F,B,H;
- eseguire prima la predizione e solo dopo la correzione;
- controllare sempre le dimensioni delle matrici.
3.1 Strategia pratica
Nel caso IMU, una strategia utile è questa:
- usare il giroscopio per prevedere l’evoluzione dell’angolo;
- usare l’accelerometro per correggere la stima;
- osservare come il filtro modifichi anche il bias, pur non essendo misurato direttamente.
Questo ultimo punto è fondamentale: se l’angolo e il bias diventano correlati nella covarianza, una misura sull’angolo può influenzare anche la stima del bias.
3.1 Errori frequenti
3.2.1 Segno sbagliato del bias nel modello dinamico
L’angolo si aggiorna con:
Non bisogna sommare il bias: va sottratto.
3.2.2 Confondere input e misura
La velocità angolare del giroscopio è un ingresso del modello, non la misura usata nell’update di Kalman.
La misura nel passo di correzione è qui l’angolo ottenuto dall’accelerometro.
3.2.3 Dimenticare il rumore di processo Q
Se non aggiunge \(Q\) il filtro rischia di diventare troppo sicuro del modello e di correggersi troppo poco.
3.2.4 Trattare il bias come perfettamente noto
Anche se il bias è quasi costante, non deve essere considerato privo di incertezza. Per questo compare una varianza iniziale e una componente di rumore in \(Q\).
3.2.5 Errori nei prodotti matriciali
Un errore molto comune è scambiare \(H\) con \(H^T\) oppure sbagliare il prodotto \( PH^T\). Conviene sempre verificare le dimensioni:
- \(P \) è \( 2 \times 2\)
- \(H\) è \( 1 \times 2\)
- \(H^T\) è \( 2 \times 1\)
3.2.6 Interpretare male il guadagno di Kalman
Se il guadagno è alto, il filtro dà più peso alla misura.
Se è basso, il filtro si fida di più della previsione del modello.
4 Soluzione svolta passo per passo
4.1 Impostazione delle matrici
Con \( \Delta t=0,1)\) s si ha:
Per continuare a visionare l’articolo clicca qui sotto:
Oltre a questo articolo ti consigliamo..
Di visitare il nostro negozio, dove puoi acquistare altri file in formato word per integrare i tuoi appunti. \( \rightarrow \) CLICCA QUI