Nonlinear stochastic position and attitude filter on the Special Euclidean Group 3

Abstract

Listen

Translate article

This paper formulates the pose (attitude and position) estimation problem as nonlinear stochastic filter kinematics evolved directly on the Special Euclidean Group 3 (SE(3)). This work proposes an alternate way of potential function selection and handles the problem as a stochastic filtering problem. The problem is mapped from SE(3) to vector form, using the Rodriguez vector and the position vector, and then followed by the definition of the pose problem in the sense of Stratonovich. The proposed filter guarantees that the errors present in position and Rodriguez vector estimates are semi-globally uniformly ultimately bounded (SGUUB) in mean square, and that they converge to small neighborhood of the origin in probability. Simulation results show the robustness and effectiveness of the proposed filter in presence of high levels of noise and bias associated with the velocity vector as well as body-frame measurements.