Pose Estimation with The Invariant Extended Kalman Filter as a Stable Observer

Abstract

En typisk anvendelse av en IMU er å måle akselerasjonen og vinkelhastigheten ved hjelp av akselerometre og gyroskoper, og finnes ofte i lokaliseringsproblemer innen robotikkfeltet. I denne oppgaven er en utvidelse av det utvidede Kalman-filteret (EKF), kalt det invariante utvidede Kalman-filteret (IEKF), utledet fra [2] for å oppnå konvergens rundt enhver bane som er en ettertraktet egenskap for ikke-lineære observatører.

To tilfeller fra [2], enkel bilmodell og navigasjon på flat jord, er simulert for å undersøke IEKF som en stabil observatør i forhold til EKF. Simuleringene viser IEKFs overlegenhet ettersom den overgår EKF på hver simulering som utføres. Når det gjelder mer utfordrende tilfeller, ser man at EKF divergerer, mens IEKF ikke divergerer som et resultat av at logaritmen til feilen følger en lineær differensialligning, også referert til som en log-lineær egenskap, som gir lokal stabilitet rundt hvilken som helst bane.

A typical application of an IMU is to measure the acceleration and angular velocity using accelerometers and gyroscopes, and is often found in localization problems within the robotics field. In this thesis, an extension of the extended Kalman filter (EKF), termed the invariant extended Kalman filter (IEKF), is derived from [2] in order to achieve convergence around any trajectory which is a coveted property for nonlinear observers.

Two cases from [2], simple car model and navigation on flat earth, are simulated to investigate the IEKF as a stable observer in comparison to the EKF. The simulations displays the superiority of the IEKF as it outperforms the EKF on every simulation performed. When it comes to more challenging cases, the EKF is seen to diverge, whereas the IEKF does not diverge as a result of the logarithm of the error obeying a linear differential equation, also referred to as a log-linear property, which gives local stability around any trajectory.