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

Abstract

This paper formulates the pose (attitude and position) estimation problem as nonlinear stochastic filter kinematics evolved directly on the Special Euclidean Group 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.

Nonlinear Pose Filters on the Special Euclidean Group SE(3) With Guaranteed Transient and Steady-State Performance

“…Define T y = R y P y 0 3 1 as a reconstructed homogeneous transformation matrix of the true T . R y corrupted by uncertain measurements can be reconstructed as in [1,2] or for simplicity visit the Appendix in [3,14]. From (18) and (19) P y is reconstructed in the following manner…”

“…. This step is followed by the normalization of v , respectively, for i = 1, 2, 3 as given in (14). Thus, Assumption 1 holds.…”

“…The pose estimation problem relies on filters evolved on the Special Euclidean Group SE (3) which require a measurement derived from a group velocity vector, vectorial measurements that could be provided by IMU, landmark measurements collected, for example, by a vision system and an estimate of the bias associated with velocity measurements. Pose estimation commonly involves a computer vision system with a monocular camera and IMU [12][13][14][15]. The pose filter described in [13] was developed directly on SE (3) and its performance has been proven to be exponentially stable.…”

“…2) The proposed filters guarantee systematic convergence of the error controlled by the dynamic reducing boundaries forcing the error to start within a predefined large set and decrease systematically and smoothly to a residual small set, unlike [12][13][14][16][17][18].…”

Guaranteed Performance of Nonlinear Pose Filter on SE(3)

“…In order to satisfy Assumption 1, the third vector is obtained using v , respectively, for j = 1, 2, 3 as given in (8). R y is obtained by SVD (visit the appendix in [3] or [16]) withR =RR y . The initialization of the true and the estimated pose is given by Fig 3 and 4 show impressive tracking performance with fast convergence of the Euler angles (φ, θ, ψ) and xyz-coordinates in 3D space, respectively.…”