Concatenare i filtri di Kalman

Il mio team sta costruendo un robot per navigare autonomamente in un ambiente esterno. Recentemente abbiamo ottenuto un nuovo sensore IMU / GPS integrato che a quanto pare esegue alcuni filtri Kalman estesi su chip. Fornisce velocità di beccheggio, rollio e imbardata, nord, est e giù, latitudine e longitudine.

Tuttavia, abbiamo anche alcuni encoder collegati alle nostre ruote, che forniscono velocità lineari e angolari. Prima di ottenere questo nuovo sensore IMU / GPS, abbiamo creato il nostro EKF per stimare il nostro stato utilizzando gli encoder e alcuni altri sensori a basso costo. Vogliamo utilizzare il filtro on-chip di questo nuovo sensore, ma anche incorporare i nostri encoder nel mix.

C'è qualche problema con il concatenamento dei filtri? Voglio dire, useremmo l'output dell'EKF on-chip del sensore IMU / GPS come aggiornamento del nostro EKF, così come usiamo i dati letti dagli encoder come aggiornamento del nostro EKF. Mi sembra ragionevole, ma mi chiedevo cosa si dovesse fare di solito in questo caso.

È possibile utilizzare INS / GPS come aggiornamenti all'output del primo EKF. In realtà, questo non è concatenare, ma semplicemente condizionare la stima sulla base delle informazioni aggiunte dall'INS / GPS.

Supponiamo di avere le seguenti funzioni:

, P t + 1 | t = EKF_PREDICT ( x t , P t , u t ), per input come stato x , covarianza P e input di controllo (stimati dall'odometria) u t .$x_{t+1|t}$$P_{t+1|t}$$x_t$$P_t$$u_t$$x$$P$$u_t$

e

, P t + 1 | t + 1 = EKF_UPDATE ( x t + 1 | t , P t + 1 | t , x t + 1 ). $x_{t+1|t+1}$$P_{t+1|t+1}$$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}$

Le stime sensori sono x t + 1 . Abbiamo cose come:$\hat{x}_{t+1}$

$\hat{x}^{gps}_{t+1} = f(GPS)$

$\hat{x}^{map}_{t+1} = f(map)$

$\hat{x}^{ins}_{t+1} = f(INS)$

ecc. per tutti gli altri modi di stimare lo stato del robot. Quindi eseguire la funzione EKF_UPDATE per tutti quei sensori è abbastanza buono.

Il tuo loop sarà qualcosa del genere:

per tutto il tempo $t$

• Let tramite l'odometriche corrente / stima cinematica di posa, e R u tramite il rumore tale stima.$u_t$$R_u$

• , P t + 1 | t = EKF_PREDICT ( x t , P t , u t , R u )$x_{t+1|t}$$P_{t+1|t}$$x_t$$P_t$$u_t$$R_u$

• per tutti i sensori ,$S$

  • Let x tramite la stima della posa da tale sensore, e R S tramite il rumore che stima$\hat{x}^{S}_{t+1}$$R_{S}$

  • , P t + 1 | t + 1 = EKF_UPDATE ( x t + 1 | t , P t + 1 | t , x t + 1 , R S ). $x_{t+1|t+1}$$P_{t+1|t+1}$$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}, R_S$

  • finali per

• finali per

Alcuni avvertimenti sono:

• Poiché stiamo utilizzando l'EKF, non vi è alcuna garanzia che il preventivo sia indipendente dall'ordinamento degli aggiornamenti. Cioè, se si esegue INS quindi GPS, la stima risultante potrebbe essere diversa rispetto a se si aggiorna con GPS quindi INS. Solitamente questo non è un grosso problema, ma il filtro richiederà molta più messa a punto.

• Si prega di essere consapevoli del fatto che il tuo INS ha una propensione e una deriva, che potrebbero influire sulla tua affidabilità a lungo termine. Il GPS può aiutarti molto qui. La maggior parte della letteratura stima contemporaneamente la distorsione e la deriva nell'INS.