Prediction and Update

The system state vector consists of three parameters describing translation of the target, another three for orientation and an additional six for the velocities:

$$\textbf{x}=\left[ X, Y, Z, \phi, \psi, \gamma, \dot{X}, \dot{Y}, \dot{Z}, \dot{\phi}, \dot{\psi}, \dot{\gamma} \right]$$ (1)

where $\phi$, $\psi$ and $\gamma$ represent roll, pitch and yaw angles [3]. The following piecewise constant white acceleration model is considered [1]:

$$\hspace{1cm}\textbf{x}_{k+1} =\textbf{F}\textbf{x}_k + \textbf{G}\textbf{v}_k , \hspace{.5cm} \textbf{z}_k = \textbf{H}\textbf{x}_k + \textbf{w}_k$$ (2)

where $\textbf{v}_k$ is a zero-mean white acceleration sequence, $\textbf{w}_k$ is the measurement noise and

$$\textbf{F} =\left[ \renewedcommand{arraycolsep}{4pt}\begin{smallm... ...{smallmatrix}\textbf{I}_{6\times6} & \vert& \textbf{0} \end{smallmatrix}\right]$$ (3)

For the prediction and update, the $\alpha-\beta$ filter is used:

$$\begin{split}& \hat{\textbf{x}}_{k+1\vert k}=\textbf{F}_{k}\h... ...{W}[\textbf{z}_{k+1}-\hat{\textbf{z}}_{k+1\vert k}] \end{split}$$ (4)

Here, the pose of the target is used as measurement rather than image features, as commonly used in the literature (see, for example, [4], [7]). An approach similar to the one presented here is taken in [18]. This approach simplifies the structure of the filter which facilitates a computationally more efficient implementation. In particular, the dimension of the matrix H does not depend on the number of matched features in each frame but it remains constant during the tracking sequence.

Figure: Training image used to estimate the initial pose (far left) followed by the intermediate images of the fitting step.