Nonlinear Stochastic Position and Attitude Filter on the Special Euclidean Group 3

Abstract: This paper formulates the pose estimation problem as nonlinear stochastic filter kinematics evolved directly on the Special Euclidean Group SE(3). 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. Keywords: Pose estimator, pose observer, attitude estimate, control, estimator, observer, Nonlinear stochastic pose filter, stochastic differential equations, Brownian motion process, Ito, Stratonovich, Wong Zakai, unit-quaternion, special orthogonal group, homogeneous transformation matrix, complimentary filter, Euler angles, Angle-axis, mapping, Parameterization, Representation, Robust, Multiplicative Extended Kalman Filter, Unscented Kalman Filter, Particle filter, KF, EKF, IEKF, UKF, MEKF, partial derivative, small, dynamics, equilibrium, asymptotic, covariance, expected value, zero, unknown, time-varying, global, semi-global, stable, stability, uncertain, Gaussian, colored, white, noise, vectorial measurement, vector measurement, translational velocity, angular velocity, singular value decomposition, rotational matrix, identity, deterministic, comparison, inertial frame, rigid body, three dimensional, 3D, space, adjoint, Lie group, projection, landmark, feature, Gyroscope, micro electromechanical systems, Inertial measurement units, sensor, IMUs, Fixed, moving, orientation, Roll, Pitch, Yaw, SVD, UAVs, QUAV, unmanned, underwater vehicle, robot, Robotic System, Spacecraft, quadrotor, quadcopter, integral, advantage, disadvantage, Comparative study, Review, Overview, Survey, autonomous, xyz, axis, SO(3), SE(3).