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

Two novel nonlinear pose (i.e., attitude and position) filters developed directly on the Special Euclidean Group SE(3) able to guarantee prescribed characteristics of transient and steady-state performance are proposed. The position error and normalized Euclidean distance of attitude error are trapped to arbitrarily start within a given large set and converge systematically and asymptotically to the origin from almost any initial condition. The transient error is guaranteed not to exceed a prescribed value while the steady-state error is bounded by a predefined small value. The first pose filter operates based on a set of vectorial measurements coupled with a group of velocity vectors and requires preliminary pose reconstruction. The second filter, on the contrary, is able to perform its function using a set of vectorial measurements and a group of velocity vectors directly. Both proposed filters provide reasonable pose estimates with superior convergence properties while being able to use measurements obtained from low-cost inertial measurement, landmark measurement, and velocity measurement units. The simulation results demonstrate the effectiveness and robustness of the proposed filters considering large error in initialization and high level of uncertainties in velocity vectors as well as in the set of vector measurements.