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 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.

[1]  Miroslav Krstic,et al.  Stabilization of stochastic nonlinear systems driven by noise of unknown covariance , 2001, IEEE Trans. Autom. Control..

[2]  Hashim A. Hashim,et al.  Nonlinear Stochastic Attitude Filters on the Special Orthogonal Group 3: Ito and Stratonovich , 2018, IEEE Transactions on Systems, Man, and Cybernetics: Systems.

[3]  E. J. Lefferts,et al.  Kalman Filtering for Spacecraft Attitude Estimation , 1982 .

[4]  Jose Luis Blanco,et al.  A tutorial on SE(3) transformation parameterizations and on-manifold optimization , 2012 .

[5]  R. G. Medhurst,et al.  Topics in the Theory of Random Noise , 1969 .

[6]  Tor Arne Johansen,et al.  Attitude Estimation Using Biased Gyro and Vector Measurements With Time-Varying Reference Vectors , 2012, IEEE Transactions on Automatic Control.

[7]  Robert E. Mahony,et al.  A nonlinear observer for 6 DOF pose estimation from inertial and bearing measurements , 2009, 2009 IEEE International Conference on Robotics and Automation.

[8]  Hashim A. Hashim,et al.  Nonlinear Explicit Stochastic Attitude Filter on SO(3) , 2018, 2018 IEEE Conference on Decision and Control (CDC).

[9]  Ben Niu,et al.  A model with a solution algorithm for the operational aircraft maintenance routing problem , 2018, Comput. Ind. Eng..

[10]  Hai-Bo Ji,et al.  Adaptive output-feedback tracking of stochastic nonlinear systems , 2006, IEEE Trans. Autom. Control..

[11]  R. L. Stratonovich,et al.  Topics in the theory of random noise , 1967 .

[12]  Robert E. Mahony,et al.  Observer design on the Special Euclidean group SE(3) , 2011, IEEE Conference on Decision and Control and European Control Conference.

[13]  William G. Faris.  Lectures on Stochastic Processes , 2004 .

[14]  Robert E. Mahony,et al.  Nonlinear Complementary Filters on the Special Orthogonal Group , 2008, IEEE Transactions on Automatic Control.

[15]  Frank L. Lewis,et al.  Neuro-adaptive cooperative tracking control with prescribed performance of unknown higher-order nonlinear multi-agent systems , 2018, Int. J. Control.

[16]  R. Khasminskii.  Stochastic Stability of Differential Equations , 1980 .

[17]  Sami El-Ferik,et al.  Optimal Tuning of Fuzzy Feedback filter for L1 Adaptive Controller Using Multi-Objective Particle Swarm Optimization for Uncertain Nonlinear MIMO Systems , 2017, 1710.05423.

[18]  Rita Cunha,et al.  A nonlinear position and attitude observer on SE(3) using landmark measurements , 2010, Syst. Control. Lett..

[19]  H. Mohamed.  Improved Robust Adaptive Control of High-order Nonlinear Systems with Guaranteed Performance , 2018, 1807.00876.

[20]  F. Markley.  Attitude Error Representations for Kalman Filtering , 2003 .

[21]  R. Mahony,et al.  Complementary filter design on the Special Euclidean group SE(3) , 2007, 2007 European Control Conference (ECC).

[22]  Malcolm D. Shuster.  Survey of attitude representations , 1993 .

[23]  G. Wahba.  A Least Squares Estimate of Satellite Attitude , 1965 .

[24]  M. Krstić,et al.  Stochastic nonlinear stabilization—I: a backstepping design , 1997 .

[25]  Shuzhi Sam Ge,et al.  Adaptive neural control of uncertain MIMO nonlinear systems , 2004, IEEE Transactions on Neural Networks.

[26]  E. Wong,et al.  On the Convergence of Ordinary Integrals to Stochastic Integrals , 1965 .

[27]  Minh-Duc Hua,et al.  Riccati Observer Design for Pose, Linear Velocity and Gravity Direction Estimation Using Landmark Position and IMU Measurements , 2018, 2018 IEEE Conference on Control Technology and Applications (CCTA).

[28]  Guanghui Sun,et al.  Fuzzy logic based fault-tolerant attitude control for nonlinear flexible spacecraft with sampled-data input , 2017, J. Frankl. Inst..

[29]  John L. Crassidis,et al.  Survey of nonlinear attitude estimation methods , 2007 .

[30]  Sergio Dominguez,et al.  Simultaneous Recognition and Relative Pose Estimation of 3D Objects Using 4D Orthonormal Moments , 2017, Sensors.

[31]  Shi Qiang Liu,et al.  A Complementary Filter Based on Multi-Sample Rotation Vector for Attitude Estimation , 2018, IEEE Sensors Journal.

[32]  M. Shuster.  A survey of attitude representation , 1993 .

[33]  Florian Nadel,et al.  Stochastic Processes And Filtering Theory , 2016 .

[34]  Wei Liu,et al.  Vehicle state estimation based on Minimum Model Error criterion combining with Extended Kalman Filter , 2016, J. Frankl. Inst..

[35]  Yan Xiao,et al.  Robust output feedback attitude tracking control for rigid-flexible coupling spacecraft , 2018, J. Frankl. Inst..

[36]  F. Markley.  Attitude determination using vector observations and the singular value decomposition , 1988 .

[37]  Frank L. Lewis,et al.  Adaptive synchronisation of unknown nonlinear networked systems with prescribed performance , 2017, Int. J. Syst. Sci..

[38]  Yu Guo,et al.  Adaptive coordinated attitude control for spacecraft formation with saturating actuators and unknown inertia , 2019, J. Frankl. Inst..

[39]  Sai Ho Chung,et al.  Joint optimization using a leader-follower Stackelberg game for coordinated configuration of stochastic operational aircraft maintenance routing and maintenance staffing , 2018, Comput. Ind. Eng..

[40]  I. Bar-Itzhack,et al.  Novel quaternion Kalman filter , 2002, IEEE Transactions on Aerospace and Electronic Systems.

[41]  Axel Barrau,et al.  The Invariant Extended Kalman Filter as a Stable Observer , 2014, IEEE Transactions on Automatic Control.

[42]  Bijoy K. Ghosh,et al.  Pose estimation using line-based dynamic vision and inertial sensors , 2003, IEEE Trans. Autom. Control..