Bayesian Estimation and Tracking: A Practical Guide

19.4 Estimation Methods

Several estimation methods can be used for the recursive estimation of the state vector and its covariance matrix. In the sections below, we discuss the three specific methods that we applied to this photogrammetry tracking problem, including an NLLSQ solver, a Gaussian UKF, and an unscented combination particle filter that we call the unscented Gaussian particle filter (UGPF). Application of these three estimation methods are presented in this section and their performance will be compared in Section 19.6.

19.4.1 A Nonlinear Least Squares Estimation Method

The standard method used in the past for estimation of the position and orientation of a rigid body based on video image data has been the NLSSQ method that assumes a noise-free dynamic equation and produces an estimate that minimizes the mean squared difference between an actual measurement set and a prediction of that measurement set. In our implementation, the state vector is taken as only the pose of the rigid body at time t_n. We will drop all time indices because the pose is estimated anew for each time step, as will be shown below.

From (19.31), we define the NLSSQ state vector as

(19.55)

and we recall that the rigid body's orientation is represented by a combination of both a and the quaternion q that represents the prior (or initial) orientation.

The observation vector is shown in (19.54). The ...