Guaranteed Performance of Nonlinear Pose Filter on SE(3)

Abstract

This paper presents a novel nonlinear pose filter evolved directly on the Special Euclidean Group SE (3) with guaranteed characteristics of transient and steady-state performance. The above-mention characteristics can be achieved by trapping the position error and the error of the normalized Euclidean distance of the attitude in a given large set and guiding them to converge systematically to a small given set. The error vector is proven to approach the origin asymptotically from almost any initial condition. The proposed filter is able to provide a reliable pose estimate with remarkable convergence properties such that it can be fitted with measurements obtained from low-cost measurement units. Simulation results demonstrate high convergence capabilities and robustness considering large error in initialization and high level of uncertainties in measurements.

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

“…where the two sets in (15) include the normalized vectors in (14) for all υ I(R) , υ B(R) ∈ R 3×NR . The position of the moving body can be extracted if its attitude R has already been determined and there exist N L known landmarks (feature points) obtained, for example, by a vision system.…”

“…Assumption 1. (Rigid-body pose observability) The pose of a rigid-body in 3D space can be extracted given the availability of at least two non-collinear vectors from the sets in (15) (N R ≥ 2) and at least one feature point from the sets in (17) with N L ≥ 1. In the case when N R = 2, the third vector can be obtained by the means of cross multiplication: υ…”

“…The first proposed filter is named a semi-direct pose filter with prescribed performance and the second one is termed a direct pose filter with prescribed performance. The difference between the two lies in the fact that while the semi-direct filter requires both attitude and position to be reconstructed through a set of vectorial measurements given in (15) and (17) combined with the measurement of the group velocity vector as described in ( 22) and ( 23), the direct filter only utilizes the abovementioned measurements in the filter design. The structure of the proposed pose filters described in the two subsequent subsections is nonlinear on the Lie group of SE (3) and is given by…”

“…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.…”

“…Consider the pose dynamics in(21), the group of noise-free velocity measurements in(22) and (23) such that Ω m = Ω + b Ω and V m = V + b V , in addition to other vector measurements given in(15) and(17)coupled with the filter kinematics in (43), (44), (45), (46), (47), and (48). Let Assumption 1 hold.…”