A method, system, and device for rigid body pose estimation based on generalized correlation entropy geometric filtering.

Abstract

This invention belongs to the field of rigid body pose estimation technology, and discloses a rigid body pose estimation method, system, and device based on generalized correlation entropy geometric filtering. The method includes: acquiring target motion information and establishing a discrete nonlinear kinematic model; initializing the discrete nonlinear kinematic model; generating sigma points for the posterior state covariance matrix and the process noise covariance matrix, respectively, and propagating and updating the sigma points through a manifold fast unscented transformation; obtaining the predicted state mean, the prior state covariance matrix, and the square root of the prior state covariance matrix; updating the center parameters based on the information from historical moments; calculating the pseudo-measurement matrix and updating the gain; correcting the state estimate using the gain; determining whether convergence has occurred, and if not, continuing the iteration; otherwise, outputting the posterior state estimate as the final rigid body pose estimation result for that moment. This invention effectively solves the problems of robustness and accuracy degradation in rigid body pose estimation under non-zero mean and non-Gaussian noise environments.

Images