Multi-Camera Rigid Body Pose Estimation using Higher Order Dynamic Models

摘要

We describe a Bayesian filtering process that estimates the pose (3-D position and orientation) of a moving rigid body using multiple cameras. The estimator also produces an arbitrary number of pose derivatives. We first discuss various ways to represent 3-D orientation. Unfortunately all 3-parameter representations have areas of instability. Higher dimensional representations are stable but require unwieldy constraints. Our combination of an axis-angle vector with a unit quaternion represents orientation minimally while remaining stable under realistic circumstances. Our dynamic model of rigid body motion can include an arbitrary number of derivatives, and we explicitly develop it up to the third order. Our observation model takes a predicted pose and produces the 2-D locations in each camera's image plane of the visible features on the body's surface. We provide noise terms for both the dynamic and observation models. We describe how our models are used in extended and unscented Kalman filters, and also in a particle filter. As a baseline we also describe a non-linear least squares method that uses just our observation model. We construct a synthetic testing scenario, and use root-mean-square error analysis to grade the relative performance of each model/filter combination. We derive the Cramer-Rao lower bound that gives the best achievable performance for our particular scenario. Our results show that adding derivatives to the state vector significantly improves the accuracy of pose estimates, and we also show that an unscented Kalman filter with a second order dynamic model is best suited to the task.