Estimation of robot attitude using Kálmán filter

Abstract

This thesis addresses the problem of estimating the attitude (orientation) of a mobile robot us- ing potentially inaccurate data from an onboard gyroscope. Representing attitude as an element of the rotation group SO(3), the estimation is framed as a probabilistic filtering problem on this manifold. The work systematically explores Bayesian filtering techniques, starting with the founda- tional Recursive Bayes Filter and the standard Kalman Filter (KF) for linear systems. To handle the inherent nonlinearities of rigid body rotation, the Extended Kalman Filter (EKF) is derived via linearization. Further refinements are investigated, including the Error-State EKF (ES-EKF),which focuses on estimating the error relative to a nominal state. Recognizing the underlying geo- metric structure, the thesis introduces the mathematical framework of Lie groups and Lie algebras (specifically SO(3) and its algebra so(3)). This leads to the development of the Invariant Extended Kalman Filter (IEKF), which leverages group symmetries for improved consistency. Finally, the Right-Invariant EKF (RIEKF) formulation is applied specifically to the SO(3) attitude estimation problem, detailing its implementation for fusing gyroscope measurements with accelerometer data acting as an observation of the gravity vector. The derivations cover state prediction, covariance propagation, and measurement update steps for each filter variant considered.

Tato práce se zabývá problémem odhadu natočení (orientace) mobilního robota pomocí potenciálně nepřesných dat z gyroskopu umístěného na jeho palubě. Reprezentací natočení jako prvku rotační grupy SO(3) je odhad formulován jako problém pravděpodobnostního filtrování na této grupě. Práce systematicky zkoumá techniky Bayesovského filtrování, počínaje základním rekurzivním Bayesovým filtrem a standardním Kalmanovým filtrem (KF) pro lineární systémy. Pro zvládnutí inherentních nelinearit rotace tuhého tělesa je odvozen rozšířený Kalmanův filtr (EKF) pomocí linearizace. Jsou zkoumána další vylepšení, včetně chybového stavového EKF (ES-EKF), který se zaměřuje na od- had chyby vzhledem k nominálnímu stavu. S ohledem na podkladovou geometrickou strukturu práce představuje matematický rámec Lieových grup a Lieových algeber (konkrétně SO(3) a její algebry so(3)). To vede k vývoji invariantního rozšířeného Kalmanova filtru (IEKF), který využívá gru- pové symetrie pro zlepšení konzistence. Nakonec je formulace pravo-invariantního EKF (RIEKF) specificky aplikována na problém odhadu natočení v SO(3), přičemž je podrobně popsána jeho implementace pro fúzi měření z gyroskopu s daty z akcelerometru, která slouží jako pozorování gravitačního vektoru. Odvození pokrývají predikci stavu, propagaci kovariance a kroky aktualizace měření pro každou uvažovanou variantu filtru.