Filtro Kalman - Implementazione, parametri e messa a punto

Prima di tutto, questa è la prima volta che provo a creare un filtro Kalman.

In precedenza avevo pubblicato la seguente domanda Filtrare il rumore e le variazioni dai valori di velocità su StackOverflow che descrive lo sfondo di questo post. Questo è un tipico esempio di valori che sto cercando di filtrare. Non devono necessariamente diminuire, come è il caso qui. Ma il tasso di cambiamento è in genere così

X ------- Y
16 --- 233.75
24 --- 234.01
26 --- 234.33
32 --- 234.12
36 --- 233.85
39 --- 233.42
47 --- 233.69
52 --- 233.68
55 --- 233.76
60 --- 232.97
66 --- 233.31
72 --- 233.99

Ho implementato il mio filtro Kalman secondo questo tutorial: Kalman Filter for Dummies .

La mia implementazione è simile a questa (pseudocodice).

//Standard deviation is 0.05. Used in calculation of Kalman gain

void updateAngle(double lastAngle){
  if(firsTimeRunning==true)
     priorEstimate = 0;               //estimate is the old one here
     priorErrorVariance = 1.2;        //errorCovariance is the old one
  else
     priorEstimate = estimate;              //estimate is the old one here
     priorErrorVariance = errorCovariance;  //errorCovariance is the old one
  rawValue = lastAngle;          //lastAngle is the newest Y-value recieved
  kalmanGain = priorErrorVariance / (priorErrVariance + 0.05);
  estimate = priorEstimate + (kalmanGain * (rawValue - priorEstimate));
  errorCovariance = (1 - kalmanGain) * priorErrVariance;
  angle = estimate;              //angle is the variable I want to update
}                                //which will be lastAngle next time

Comincio con una stima precedente di 0. Questo sembra funzionare bene. Ma quello che noto è che kalmanGain diminuirà ogni volta che viene eseguito questo aggiornamento, il che significa che mi fido dei miei nuovi valori meno tempo fa il mio filtro è in esecuzione (?). Non lo voglio.

Sono passato dall'usare una media mobile (semplice ed esponenziale ponderata) all'utilizzo di questa. In questo momento non riesco nemmeno a ottenere buoni risultati.

La mia domanda è se questa è l'implementazione giusta e se la mia varianza di errore e la deviazione standard precedenti sembrano buone in base ai valori di esempio che ho pubblicato? I miei parametri sono in realtà scelti a caso per vedere se sono riuscito a ottenere dei buoni risultati. Ho provato diverse gamme diverse ma con scarsi risultati. Se hai qualche suggerimento per le modifiche che posso fare, sarebbe molto apprezzato. Mi dispiace se mancano alcune cose ovvie. Pubblicare la prima volta anche qui.

I filtri Kalman sono utili quando il segnale in ingresso è costituito da osservazioni rumorose di alcuni stati del sistema dinamico lineare. Data una serie di osservazioni sullo stato del sistema, il filtro Kalman mira a fornire in modo ricorsivo stime sempre migliori dello stato del sistema sottostante. Per applicarlo con successo, è necessario disporre di un modello per la dinamica del sistema di cui si sta valutando lo stato. Come descritto in dettaglio su Wikipedia , questo modello descrive come il sistema sottostante allo stato dovrebbe cambiare in una fase temporale, dato il suo stato precedente, eventuali input al sistema e un componente stocastico distribuito gaussiano chiamato rumore di processo.

Detto questo, dalla tua domanda non è chiaro se tu abbia un simile modello sottostante. Il post collegato indicava che stai consumando valori di velocità da un sensore. Questi possono essere modellati come osservazioni dirette dello stato di un sistema (dove lo stato è la sua velocità) o osservazioni indirette del suo stato (dove lo stato è la sua posizione, per esempio). Ma, al fine di utilizzare il framework Kalman, è necessario scegliere un modello per come si prevede che lo stato evolva col passare del tempo; queste informazioni aggiuntive vengono utilizzate per generare la stima ottimale. Un filtro Kalman non è una scatola nera magica che "ripulirà" un segnale che viene applicato ad esso.

Detto questo, il fenomeno a cui hai accennato, in cui il filtro Kalman diventerà sempre più fiducioso nel proprio output fino al punto in cui le osservazioni di input vengono progressivamente ignorate, avviene in pratica. Ciò può essere mitigato aumentando manualmente i valori nella matrice di covarianza del rumore di processo. Quindi, qualitativamente, il modello per la transizione di stato del sistema contiene una componente stocastica più ampia, quindi la capacità dello stimatore di prevedere con precisione lo stato successivo dato lo stato corrente è ridotta. Ciò ridurrà la sua dipendenza dalla sua attuale stima dello stato del sistema e aumenterà la sua dipendenza da osservazioni successive, impedendo l'effetto "ignorando l'input".

Se l'ho capito correttamente, hai qualcosa che si muove e puoi osservare la velocità e questa velocità è rumorosa. Dalle tue misurazioni, osservi 2 tipi di variazioni. \

1. Variazioni causate dal rumore
2. Variazioni perché l'oggetto sta veramente cambiando la velocità (es. Svolta)

La ragione per cui il tuo guadagno di Kalman va a zero è che hai implicitamente supposto che la velocità dell'oggetto sia costante e tutto ciò che devi fare è stimare questa vera velocità.

" Ehi, ho un oggetto che si muove a velocità costante e voglio stimare questa velocità costante "

$x_k$$k$$y_k$

$$x_k = x_{k-1}$$ $$y_k = x_k + q_k$$

Ma il tuo oggetto non si muove in quel modo. La sua velocità sta cambiando e non sai come e quando cambierà.

Quello che hai da dire invece è:

" Ehi, ho un oggetto che si sta muovendo ad una velocità ma non sono sicuro di come sta cambiando la sua velocità "

Ci sono molti modi in cui puoi farlo: il modo più semplice è aggiungere incertezza al tuo stato.

$$x_{k} = x_{k-1} + \underbrace{v_{k-1}}_{\text{you add uncertainty}}$$ $$y_k = x_k + q_k$$ $q_k$$v_k$

Le equazioni del tuo filtro Kalman saranno così:

$$\hat y_{k|k-1} = \hat x_{k|k-1}$$ $$K_{k} = \frac{P_{k|k-1}}{P_{k|k-1} + Q^o}$$ $$\hat x_{k|k}= \hat x_{k|k-1} + K_k(y_k - \hat y_{k|k-1})$$ $$P_{k|k} = P_{k|k-1} - K_kP_{k|k-1}$$ $$P_{k+1|k} =P_{k|k} + Q^s$$

0.05$Q^o$$Q^s$

Nel tuo codice la leggera modifica sarebbe:

stateVariance = 0.5

errorCovariance = (1 - kalmanGain) * priorErrVariance + stateVariance;

stateVariance$Q^s$

Questo stateVariancevalore può essere quello che vuoi. Si basa sulla tua sicurezza su quanto cambierà effettivamente la velocità. Se pensi che la velocità rimarrà abbastanza costante, impostala su un numero piccolo.

In questo modo il tuo guadagno di Kalman non andrà a zero.

È necessario un sistema dinamico per utilizzare un filtro Kalman.

suggerirei

$$y = \sum\limits_{i =0}^n a_i\, x^i$$

$$a[k+1] = a[k] + w$$ $$cov(w) = Q$$ $$z = \sum\limits_{i =0}^n a_i\, x^i = y$$

$x$$a$

Penso che potresti usare alcune idee della teoria del controllo classico, ad esempio il controller PID .

Il segnale Y può essere il setpoint del controller u (t). L'impianto di processo è solo 1 e y (t) verrà filtrato in uscita. Tutto quello che dovrai fare è impostare i parametri (accordare) P, I e D per ottenere ciò che desideri.

L'uscita y (t) proverà a "seguire" l'ingresso u (t), ma i parametri controllano come sarà questo tracking.

Il guadagno differenziale D renderà la tua risposta sensibile alle rapide modifiche degli errori. Nel tuo caso, penso che D dovrebbe essere piccolo. Non vuoi che y (t) cambi se u (t) cambia bruscamente.

Il guadagno integrale "I" renderà la tua risposta sensibile agli errori accumulati. Dovresti mettere un valore elevato lì. Se u (t) cambia livello e lo mantiene lì, l'errore si accumula e quindi si desidera che y (t) faccia lo stesso.

Il guadagno P può dare una buona melodia. Ad ogni modo, prova a giocare con i parametri e vedi cosa ottieni.

Esistono metodi di ottimizzazione complicati, tuttavia, ma non credo che ne avrai bisogno.

In bocca al lupo.

Ecco un'implementazione semplice e chiara di Kalman Filter che segue le notazioni come nella pagina di Wikipedia. https://github.com/zziz/kalman-filter