Nonlinear Stochastic Estimators on the Special Euclidean Group SE(3) Using Uncertain IMU and Vision Measurements

Two novel robust nonlinear stochastic full pose (i.e., attitude and position) estimators on the Special Euclidean Group
$\mathbb {SE}(3)$
are proposed using the available uncertain measurements. The resulting estimators utilize the basic structure of the deterministic pose estimators adopting it to the stochastic sense. The proposed estimators for six degrees of freedom (DOF) pose estimations consider the group velocity vectors to be contaminated with constant bias and Gaussian random noise, unlike nonlinear deterministic pose estimators which disregard the noise component in the estimator derivations. The proposed estimators ensure that the closed-loop error signals are semi-globally uniformly ultimately bounded in mean square. The efficiency and robustness of the proposed estimators are demonstrated by the numerical results which test the estimators against high levels of noise and bias associated with the group velocity and body-frame measurements and large initialization error.