Factorized Kalman filter implementation for aided inertial navigation systems

Abstract

For å undersøke om NTNU UAVlabs navigasjonssystemsprogramvare for simulering av treghetsnavigasjon kunne forbedres i tilfeller der målevektoren er stor, så utførte vi et eksperiment der vi sammenlignet tre variasjoner av et error-state Kalman filter. I denne testen sammenlignet vi kjøretid, feiltilstandens kvadratisk gjennomsnittsfeil og den akkumulerte tilstandsvariansen ved endt simulering.

De tre filterimplementasjonene som ble test var den vanlige implementasjonen av filteretm som oftest blir presentert i lærebøker, og som er basert på matriseinvertering under beregningen av Kalman-forsterkingen. Denne variasjonen av filteret ble også brukt som referansegrunnlag for å sammenligne de to andre. Den andre variasjonen var det sekvensielle Kalman-filteret der måleoppdateringen blir utført sekvensielt for å unngå matriseinverteringen under beregningen av Kalman-forsterkningen. Det siste, og tredje filteret var en faktorisert variant av filteret basert på en strategi der kovariansmatrisen blir faktorisert gjennom UDU-faktorisering i både tids- og måleoppdateringen.

For å teste hvilket av de tre filterene som var best i forhold til testparameterene, impleterte vi en 16 tilstanders simuleringsmodell med simulerte bevegelses- og hjelpemålinger. Her ble m antall simulerte sendetårn spredd rundt den simulerte banen til fartøyet i modellen. Hver filtervariasjon ble oppdelt i tre tidskategorier separert i total kjøretid, kjøretid som tidsoppdateringen bruker av total kjøretid, samt kjøretid som måleoppdateringen bruker av total kjøretid. Tidsmålingen ble utført ved å kjøre simuleringen N = 200 ganger for et gitt sett med simuleringskjøretid. Av de 200 målte dataene, ble det beregnet en gjennomsnittlig kjøretid for de tre kategoriene som ble brukt for å sammenligne de tre variasjonene. I tillegg ble filterenes kvadratiske gjennomsnittsfeil og varians målt og sammenlignet mot hverandre. Dette eksperimentet ble gjort med henholdsvis m = 15 og 30 hjelpemålinger hver for seg.

Denne oppgaven resulterte i at den sekvensielle filterstrategien gav en forbedring i kjøretid for måleoppdateringen med en relativ forbedring til referansen på 4.25% for m = 15 hjelpemålinger, og 81.65% forbedring for m = 30. I tillegg så økte referansens kjøretid med en nær kubisk rate for denne økningen av m. Den UD-faktoriserte variasjonen viste en 137% nedgang i relativ kjøretid for tidsoppdateringen og 1161% nedgang i måleoppdateringen relativ til referansen for m = 15, samt en 146% og 200% nedgang for m = 30 i de respektive modulene.

Den sekvensielle variasjonen viste en numerisk identisk presisjon og ytelse når det kommer til den kvadratiske gjennomsnittsfeilen og den endelige estimerte kovariansen for den lengste simuleringstiden utført i dette eksperimenten. UD-variasjonen viste en lignendes kvadratisk gjennomsnittsfeil relativt til referansen og en liten forbedring i tallstørrelsen til kovariansen.

To examine if the NTNU UAVlabs navigation system toolbox for offline inertial navigation could yield a better run time performance for cases with large measurement vectors, we tested three variations of the Error-State Kalman Filter. The performance metrics measured were run time, state root square mean error and state covariance.

The three implementations we tested were a batch wise implementation of the error-state Kalman filter which were used as the reference filter. The second variation were the sequential Kalman filter where the measurement update is done sequentially to avoid a matrix inversion when computing the Kalman gain. Lastly, a third filter variation which uses a strategy involving UDU-factorization of the covariance matrix in both the time-update and measurement-update step were chosen as a factorized filter variation.

To test the hypothesis we use a 16 state simulation model with simulated IMU- and aiding measurements, where m generic ranges were generated between beacons spread around the simulated trajectory and the simulated vessel. Each filter variation were separated into three timing categories: total-, time update- and measurement update run time. The timing were performed by running the simulation n = 200 times for a set of simulation durations before the average of the n measured run times were used to compare the variations. This experiment were done with m = 15 and 30 measurement separately.

This thesis showed that the sequential filter strategy yielded a improvement in measurement update run time with a speed up increase of 4.25% for m= 15 measurements and 81.65% for m = 30. Additionally, the reference measurement update run time increased at a near cubic rate when increasing m from 15 to 30. The UD-factorized variation were 137% slower in the time update and 1161% slower in the measurement update relative to the reference for m = 15, and 146% and 200% for m = 30 in the respective update modules.

The sequential variation displayed a numerical identical accuracy and performance in terms of RMSE and final estimated covariance for the longest simulation duration in this experiment. The UD-variation displayed a similar RMSE as the reference, and a small improvement in terms of magnitude in the final covariance estimation compared to the reference.