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 tn. 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
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 ...