Kalman filter, Example 2 (falling body)

DEFINIZIONE

Consideriamo ora il problema della caduta di un grave. Lasciamo cadere un oggetto dalla sommità di una torre, abbiamo a disposizione uno strumento che ci ritorna l'altezza del corpo rispetto al suolo e vogliamo stimarne l'altezza stessa durante il percorso. In questo caso abbiamo la presenza di un termine forzante (la forza di gravità).

FIGURA 1: Rappresentazione del sistema studiato (caduta di un grave), cliccare sull'immagine per ingrandirla

MODELLO

La dinamica del corpo si può descrivere mediante le ben note formule fisiche:

Nel nostro caso il valore iniziale di altezza sarà quella della torre mentre la velocità iniziale sarà banalmente 0 (lasciamo cadere l’oggetto senza imprimere una forza).

Lo stato del sistema è definito dal vettore:

Passando al dominio tempo discreto possiamo riscrivere l'equazione di stato come segue:

Dove Δt è il periodo di campionamento (costante).

L'equazione di misura è banalmente:

Le matrici relative alle equazioni di cui sopra risultano essere:

Mentre le matrici relative agli errori (che non è restrittivo considerare incorrelati) saranno le seguenti:

SIMULAZIONE

Al solito il primo problema da affrontare è la determinazione dei valori iniziali che in questo caso è particolarmente semplice, sappiamo a priori l'altezza della torre ( nella simulazione 100 metri ) e la velocità iniziale è 0. Inoltre per quanto riguarda P possiamo tranquillamente inizializzarla come una matrice identità moltiplicata per un valore di varianza abbastanza elevato (ad esempio 10). Al solito ricordiamo l'importante proprietà di convergenza del filtro!

Per quanto rigurada invece le matrici Q ed R scegliamo (correttamente) che le varianze relative a Q siano basse (il modello è accurato!) mentre quelle relative ad R siano elevate (la misura è corrotta da molto rumore, lo strumento non è molto preciso).

Poche righe di codice (il progetto Octave lo trovate in allegato a questa pagina) e siamo pronti.

Due esempi di simulazione li trovate di seguito (sia la traiettoria che l'andamento della varianza dell'errore).

FIGURA 2: Simulazione della caduta di un grave. In rosso l'andamento ideale, in verde le misure molto rumorose ed in blu il valore di posizione restituito dal filtro di Kalman. Cliccare sulle immagini per ingrandirle.

FIGURA 3: Andamento della varianza dell'errore. Cliccare sulle immagini per ingrandirle.

In questo secondo esempio si capisce immediatamente l'importanza del modello e la bontà del filtro, è più che evidente quanto i risultati del filtro siano decisamente più vicine all'andamento reale dell'oggetto. Per quanto riguarda la proprietà di convergenza ritroviamo quanto visto con il primo esempio, ovvero la varianza dell'errore scende velocemente.