Unscented Kalman Filtering for Single Camera Based Motion and Shape Estimation

Abstract

Accurate estimation of the motion and shape of a moving object is a challenging task due to great variety of noises present from sources such as electronic components and the influence of the external environment, etc. To alleviate the noise, the filtering/estimation approach can be used to reduce it in streaming video to obtain better estimation accuracy in feature points on the moving objects. To deal with the filtering problem in the appropriate nonlinear system, the extended Kalman filter (EKF), which neglects higher-order derivatives in the linearization process, has been very popular. The unscented Kalman filter (UKF), which uses a deterministic sampling approach to capture the mean and covariance estimates with a minimal set of sample points, is able to achieve at least the second order accuracy without Jacobians’ computation involved. In this paper, the UKF is applied to the rigid body motion and shape dynamics to estimate feature points on moving objects. The performance evaluation is carried out through the numerical study. The results show that UKF demonstrates substantial improvement in accuracy estimation for implementing the estimation of motion and planar surface parameters of a single camera.

1. Introduction

The problem of estimating positions and velocities of moving features in space leads to the problem of estimating motion and shape parameters of moving features from their corresponding image data observed over time. This is important in various engineering applications, such as robotics and machine vision [1–6]. A dynamical systems approach to machine vision is introduced in [1], in which the problem of motion and shape parameter estimation is described as an inverse problem associated with a pair of coupled Riccati partial differential equations. Identification of the motion and shape parameters using the estimation technique was discussed in [5,6]. A CCD camera with a laser range finder mounted on a mobile robot for better motion and shape parameters identification can be seen in [5]. A sliding mode approach was proposed to estimate the motion of a moving body with the aid of a CCD camera [6]. For performance improvement, various estimation techniques have been adopted to reduce the noise. The algebraic methods yielded feasible results, but they were computationally unrealistic. The Kalman filter method is used in the estimation of the motion parameters to reduce the effect of the measurement errors which are inevitable in the real world. The Kalman filter (KF) method [7] for estimation of the 3D camera motion in imagine sequences for the applications to the video coding system is proposed by Kim et al. [8]. Kano et al. [9] performed a numerical study and compared an extended Kalman filter (EKF) based recursive algorithm with a non-recursive algebraic method for estimating motion and planar surface parameters.

The KF is theoretically attractive because it has been shown to be the one that minimizes the variance of the estimation mean square error (MSE). The nonlinear filter is used for nonlinear dynamics and/or nonlinear measurement relationships. The problem of estimating the state variables of the nonlinear systems may be solved using the nonlinear version of the Kalman filter. The most popular form is the EKF. The fact that EKF highly depends on a predefined dynamics model forms a major drawback. To achieve good filtering results, the designers are required to have the complete a priori knowledge on both the dynamic process and measurement models, in addition to the assumption that both the process and measurement are corrupted by zero-mean Gaussian white sequences. If the input data does not reflect the real model, the estimates may not be reliable.

Similar to the EKF, the unscented Kalman filter (UKF) [10–17] focuses on approximating the prediction probability characteristics and use the standard minimum mean square error estimator. The UKF has been developed in the context of state estimation of dynamic systems as a nonlinear distribution (or densities in the continuous case) approximation method. The UKF is superior to EKF not only in theory but also in many practical situations. The algorithm performs the prediction of the statistics with a set of carefully chosen sample points for capturing mean and covariance of the system. These sample points are sometimes referred to as the sigma points employed to propagate the probability of state distribution. The basic premise behind the UKF is it is easier to approximate a Gaussian distribution than it is to approximate an arbitrary nonlinear function. Instead of linearizing using Jacobian matrices as in the EKF and achieving first-order accuracy, the UKF can capture the states up to at least second order by using a deterministic sampling approach to capture the mean and covariance estimates with a minimal set of sample points. The deterministic sampling based UKF with applications on estimation of rigid body motion and shape dynamics are presented to estimate the feature points on the moving object. Results obtained shows that UKF is able to provide more accurate and reliable estimation accuracy of the object. Investigation of the UKF approach to the motion and shape estimation problem has not been seen in the literature.

This paper is organized as follows. In Section 2, preliminary background on rigid body motion is reviewed. The shape dynamics and optical flow dynamics are discussed in Sections 3 and 4, respectively. The unscented Kalman filter is introduced in Section 5. Results and Discussion are given in Section 6. Conclusions are given in Section 7.

2. Rigid Body Motion

The mathematical description of a 3D point undergoing a rigid transformation about the camera axes is given as follows. Let ω_x, ω_y and ω_z represent the angle of rotation about the X, Y and Z axes, respectively. Figure 1 illustrates the world coordinate system. An arbitrary rotation R can be represented by successive rotations around the X, Y and Z axes, respectively, as:

$$R=\left[\begin{array}{ccc}1& 0& 0\\ 0& \text{cos}({\omega }_x)& -\text{sin}({\omega }_x)\\ 0& \text{sin}({\omega }_x)& \text{cos}({\omega }_x)\end{array}\right]\left[\begin{array}{ccc}\text{cos}({\omega }_y)& 0& \text{sin}({\omega }_y)\\ 0& 1& 0\\ -\text{sin}({\omega }_y)& 0& \text{cos}({\omega }_y)\end{array}\right]\left[\begin{array}{ccc}\text{cos}({\omega }_z)& -\text{sin}({\omega }_z)& 0\\ \text{sin}({\omega }_z)& \text{cos}({\omega }_z)& 0\\ 0& 0& 1\end{array}\right]$$

(1)

Assuming infinitesimal rotations, the zeroth order terms of the Taylor series expansion of the trigonometric functions and cos provide the following approximations:

$$\text{cos}(\theta )\approx 1,\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\text{sin}(\theta )\approx \theta$$

(2)

Using the approximations in Equation (2), R can be approximated in the skew-symmetric matrix form in terms of angular velocity:

$$R\approx \left[\begin{array}{ccc}1& 0& 0\\ 0& 1& -{\omega }_x\\ 0& {\omega }_x& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& {\omega }_y\\ 0& 1& 0\\ -{\omega }_y& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& -{\omega }_z& 0\\ {\omega }_z& 1& 0\\ 0& 0& 1\end{array}\right]\approx \left[\begin{array}{ccc}1& -{\omega }_z& {\omega }_y\\ {\omega }_z& 1& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 1\end{array}\right]$$

(3)

Let the vector T = [t_x t_y t_z]^T represent the translational velocity, where the elemental components t_x, t_y, and t_z represent the translational velocities in the X, Y and Z directions, respectively. The velocity vector V = [Ẋ Ẏ Ż]^T of a point in the world coordinates P = [X Y Z]^T with respect to camera coordinates undergoing rigid transformation is represented as:

$$\left[\begin{array}{c}\dot{X}\\ \dot{Y}\\ \dot{Z}\end{array}\right]=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]+\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\\ \end{array}\right]$$

(4)

which may be written in matrix form:

$$V=(R-I)P+T$$

where I represents a 3 × 3 identity matrix. By defining [−ω_z ω_y −ω_x]^T as [ω₁ ω₂ ω₃]^T ; and [t_x t_y t_z]^T as [b₁ b₂ b₃]^T, Equation (4) can be represented as:

$$V=\mathbf{\Omega }P+\mathbf{b}$$

(5)

where b = [b₁ b₂ b₃]^T and:

$$\mathbf{\Omega }\equiv \left[\begin{array}{ccc}0& {\omega }_1& {\omega }_2\\ -{\omega }_1& 0& {\omega }_3\\ -{\omega }_2& -{\omega }_3& 0\end{array}\right]$$

(6)

Consider the dynamical system Equation (5) where it is assumed that [ω₁ ω₂ ω₃]^T ≠ 0. If:

$$\mathbf{b}\in \text{Im}\mathbf{\Omega }$$

(7)

it is easily seen that ImΩ is a plane in ℜ³ given as:

$$\text{Im}\mathrm{\Omega }=\{\mathbf{x}\in R|{\mathbf{\omega }}^{\mathrm{T}}\mathbf{x}=0\}$$

(8)

where:

$$\mathbf{\omega }={[{\omega }_3-{\omega }_2\hspace{0.17em}{\omega }_1]}^{\mathrm{T}}$$

(9)

3. Shape Dynamics

The motion field describing the motion of individual points on the surface might undergo a change in shape. The dynamical system which describes the changing shape of the surface is called the shape dynamics. Let (X, Y, Z) be the world coordinate frame wherein we have a surface defined by:

$$Z=S(X,Y,t)$$

(10)

Assume that S is smooth enough so that its derivatives with respect to each of the variables are defined everywhere. The motion field is assumed to be described by:

$$\dot{X}=f\left(X,Y,Z\right),\hspace{0.17em}\dot{Y}=g\left(X,Y,Z\right),\hspace{0.17em}\dot{Z}=h\left(X,Y,Z\right)$$

(11)

How the surface as in Equation (10) moves as points on the surface move following the motion field as in Equation (11) can be described by the quasi-linear partial differential equation called the “shape dynamics”:

$$\frac{\partial S}{\partial t}+f\left(X,Y,Z\right)\frac{\partial S}{\partial X}+g\left(X,Y,Z\right)\frac{\partial S}{\partial Y}=h\left(X,Y,Z\right)$$

(12)

Consider the initial condition:

$$S\left(X,Y,0\right)=\varphi \left(X,Y\right)$$

(13)

The pair of Equations (12) and (13) constitutes an example of a Riccati partial differential equation. The surface defined by Equation (10) is assumed to be a plane described by:

$$Z=pX+qY+r$$

(14)

where p, q, r are shape parameters that are time varying as a result of the motion field. The vectors (X, Y, Z) and (p, q, r) are written in terms of homogeneous coordinates (X̄ Ȳ Z̄ W̄):

$$X=\frac{\overline{X}}{\overline{W}}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}Y=\frac{\overline{Y}}{\overline{W}}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}Z=\frac{\overline{Z}}{\overline{W}}$$

(15)

and:

$$p=\frac{\overline{p}}{\overline{s}}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}q=\frac{\overline{q}}{\overline{s}}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}r=\frac{\overline{r}}{\overline{s}}$$

(16)

where W̄ = (X² + Y² + Z²)^1/2. Equation (14) can then be rewritten as:

$$\overline{p}\overline{X}+\overline{q}\overline{Y}-\overline{s}\overline{Z}+\overline{r}\overline{W}=0$$

(17)

Using Equation (15), Equation (5) can be written as:

$$\frac{d}{\mathit{dt}}\left[\begin{array}{l}\overline{X}\hfill \\ \overline{Y}\hfill \\ \overline{Z}\hfill \\ \overline{W}\hfill \end{array}\right]=\left[\begin{array}{cc}\mathrm{\Omega }& \mathbf{b}\\ 0& 0\end{array}\right]\left[\begin{array}{l}\overline{X}\hfill \\ \overline{Y}\hfill \\ \overline{Z}\hfill \\ \overline{W}\hfill \end{array}\right]$$

(18)

The shape dynamics can be obtained by differentiating Equation (17) with respect to time t:

$$\frac{d}{\mathit{dt}}\left[\begin{array}{c}\overline{p}\\ \overline{q}\\ -\overline{s}\\ \overline{r}\end{array}\right]=\left[\begin{array}{cc}\mathrm{\Omega }& 0\\ -{\mathbf{b}}^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{c}\overline{p}\\ \overline{q}\\ -\overline{s}\\ \overline{r}\end{array}\right]$$

(19)

where p(t), q(t) and r(t) are the shape parameters to be discussed in Section 4.

4. Optical Flow Dynamics

As shown in Figure 2, the 3D vector P = (X, Y, Z) is assumed to be observed via perspective projection onto a plane parallel to the (x, y) axes and located at Z = 1 by defining the relationship between an image point u = (x, y) and a scene point (X, Y, Z) given by:

$$x=\frac{X}{Z},\hspace{0.17em}\hspace{0.17em}y=\frac{Y}{Z}$$

(20)

The focal length f = 1 is usually used without loss of generality. With the relation given by Equation (5), differentiating Equation (20) leads to the relations:

$$\dot{x}=\frac{Z\dot{X}-X\dot{Z}}{Z^2}=\frac{\dot{X}}{Z}-x\frac{\dot{Z}}{Z}=\left({\omega }_{1}y+{\omega }_2\right)-x\left(-{\omega }_{2}x-{\omega }_{3}y\right)+\frac{b_1}{Z}-x\frac{b_3}{Z}$$

(21)

and:

$$\dot{y}=\frac{Z\dot{Y}-Y\dot{Z}}{Z^2}=\frac{\dot{Y}}{Z}-y\frac{\dot{Z}}{Z}=\left(-{\omega }_{1}x+{\omega }_3\right)-y\left(-{\omega }_{2}x-{\omega }_{3}y\right)+\frac{b_2}{Z}-y\frac{b_3}{Z}$$

(22)

From Equations (14) and (20), we have:

$$\frac{1}{Z}=\frac{1-px-qy}{r}$$

(23)

and hence the optical flow equations are given by:

$$\dot{x}={\omega }_2+{\omega }_{1}y+{\omega }_2{x}^2+{\omega }_{3}xy+\left(c_1-x{c}_3\right)\left(1-px-qy\right)$$

(24)

$$\dot{y}={\omega }_3-{\omega }_{1}x+{\omega }_{2}xy+{\omega }_3{y}^2+\left(c_2-y{c}_3\right)\left(1-px-qy\right)$$

(25)

where c_i = b_i/r, i = 1,2,3.

Equations (24) and (25) denote the optical flow equations. Even when the motion field is time invariant, the parameters of the optical flow equations could be time-varying due to the fact that the shape parameters are changing in time as a result of motion field described by Equation (11). As t → ∞, we have:

$$p(t)\to \frac{{\omega }_3}{{\omega }_1},\hspace{0.17em}\hspace{0.17em}q(t)\to -\frac{{\omega }_2}{{\omega }_1},\hspace{0.17em}\hspace{0.17em}r(t)\to \frac{\mathit{bt}}{{\omega }_1},\hspace{0.17em}\hspace{0.17em}{c}_i\to \frac{b_i{\omega }_1}{\mathit{bt}},\hspace{0.17em}\hspace{0.17em}i=1,2,3$$

(26)

In particular, if b ∈ ImΩ, the shape parameters p(t), q(t) and r(t) are periodic functions satisfying the Riccati Equation [2,9]:

$$\begin{array}{l}\dot{p}=-{\omega }_2(1+p^2)+{\omega }_{1}q-{\omega }_{3}pq\\ \dot{q}=-{\omega }_3(1+q^2)-{\omega }_{1}q-{\omega }_{2}pq\\ \dot{r}={\mathrm{b}}_3-{\mathrm{b}}_{1}p-{\mathrm{b}}_{2}q-r({\omega }_{3}q+{\omega }_{2}p)\end{array}$$

(27)

which is parameterized by a total of six motion parameters and three initial conditions on shape parameters. Therefore, a total of nine parameters need to be adopted for describing the shape dynamics. The Riccati equation propagates in time the relationship between coordinates X, Y, and Z expressed via the surface described by Equation (10).

Once the modeling of shape dynamics and optical flow dynamics is accomplished, the estimation algorithm can be employed for implementing the estimation of motion and planar surface parameters. In addition to the algebraic methods, the recursive estimation algorithms are applicable. The recursive algorithm presented by Kano et al. [9] is an EKF-based algorithm. The motivation of the paper is to carry out the UKF-based approach, which has not been seen in the literature for the motion and shape estimation problem. A brief introduction of the UKF is provided in Section 5.

5. The Unscented Kalman Filter

The UKF is a nonlinear filter which deals with the case governed by the nonlinear stochastic difference equations:

$${\mathbf{x}}_{k+1}={\mathbf{f}}_k({\mathbf{x}}_k)+{\mathbf{w}}_k$$

(28a)

$${\mathbf{z}}_k={\mathbf{h}}_k({\mathbf{x}}_k)+{\mathbf{v}}_k$$

(28b)

where the state vector x_k ∈ ℜⁿ, process noise vector w_k ∈ ℜⁿ, measurement vector z_k ∈ ℜ^m, and measurement noise vector v_k ∈ ℜ^m. In Equation (28), both the vectors w_k and v_k are zero mean Gaussian white sequence having zero crosscorrelation with each other:

$$E[{\mathbf{w}}_k{\mathbf{w}}_i{}^{\mathrm{T}}]=\{\begin{array}{c}{Q}_k,\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}i=k\\ 0,\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}i\ne k\end{array};\hspace{0.17em}\hspace{0.17em}E[{\mathbf{v}}_k{\mathbf{v}}_i{}^{\mathrm{T}}]=\{\begin{array}{c}{R}_k,\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}i=k\\ 0,\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}i\ne k\end{array};\hspace{0.17em}\hspace{0.17em}E[{\mathbf{w}}_k{\mathbf{v}}_k{}^{\mathrm{T}}]=0\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\mathit{for}\hspace{0.17em}\hspace{0.17em}\mathit{all}\hspace{0.17em}i\hspace{0.17em}\hspace{0.17em}\mathit{and}\hspace{0.17em}k$$

(29)

where Q_k is the process noise covariance matrix, R_k is the measurement noise covariance matrix.

5.1. The Unscented Transformation

The first step in the UKF is to sample the prior state distribution, i.e., generate the sigma points through the unscented transformation (UT). Figure 3 illustrates the true means and covariances as compared to those obtained by the mapping of the UKF versus that of the EKF. The dot-line ellipse represents the true covariance. The UKF is implemented through the transformation of the nonlinear function f(·), shown as the solid-line ellipse on the top portion of the figure; the EKF is accomplished through the Jacobian F = ∂f/∂x, shown as the solid-line ellipse at the bottom portion of the figure. The UKF approach estimates are expected to be closer to the true values than the EKF approach.

Several UT’s are available. One of the popular approaches is the scaled unscented transformation [15–17]. Consider an n dimensional random variable x, having the mean x̂ and covariance P, and suppose that it propagates through an arbitrary nonlinear function f. The unscented transform creates 2n +1 sigma vectors X (a capital letter) and weighted points, given by:

$$X^{(0)}=\widehat{\mathbf{x}}$$

(30a)

$$X^{(i)}=\widehat{\mathbf{x}}+(\sqrt{(n+\lambda )P{)}_i^T},\hspace{0.17em}i=1,...,n$$

(30b)

$$X^{(i+n)}=\widehat{\mathbf{x}}-(\sqrt{(n+\lambda )P{)}_i^T},\hspace{0.17em}i=1,...,n$$

(30c)

where $(\sqrt{(n+\lambda )P{)}_i}$ is the i th row of the matrix square root. $\sqrt{(n+\lambda )P}$ can be obtained from the lower-triangular matrix of the Cholesky factorization; λ = α²(n + κ) − n is a scaling parameter; α determines the spread of the sigma points around x̑ and is usually set to a small positive (e.g., 1e − 4 ≤ α ≤ 1); κ is a secondly scaling parameter (usually set as 0); β is used to incorporate prior knowledge of the distribution of x̄ (when x is normally distributed, β = 2 is an optimal value); $W_i^{(m)}$ is the weight for the mean associated with the ith point; and $W_i^{(c)}$ is the weigh for the covariance associated with the ith point:

$$W_0^{(m)}=\frac{\lambda }{(n+\lambda )}$$

(31a)

$$W_0^{(c)}=W_0^{(m)}+(1-{\alpha }^2+\beta )$$

(31b)

$$W_i^{(m)}=W_i^{(c)}=\frac{1}{2(n+\lambda )},\hspace{0.17em}\hspace{0.17em}i=1,...,2n$$

(31c)

The sigma vectors are propagated through the nonlinear function to yield a set of transformed sigma points:

$${\mathbf{y}}_i=f(X^{(i)}),\hspace{0.17em}\hspace{0.17em}i=0,...,2n$$

(32)

The mean and covariance of y_i are approximated by a weighted average mean and covariance of the transformed sigma points as follows:

$${\overline{\mathbf{y}}}_u=\sum _{i=0}^{2n}{W}_i^{(m)}{\mathbf{y}}_i$$

(33)

$${\overline{P}}_u=\sum _{i=0}^{2n}{W}_i^{(c)}({\mathbf{y}}_i-{\overline{\mathbf{y}}}_u){(y_i-{\overline{y}}_u)}^T$$

(34)

5.2. The Unscented Kalman Filter

A high level of operation of the unscented Kalman filter is shown in Figure 4. To look at the detailed algorithm of the UKF, firstly, the set of sigma points are created by Equation (30). After the sigma points are generated, the time update (prediction step) of the UKF involves the following steps:

$${({\zeta }_k^{-})}_i=f(X_{k-1}^{(i)}),\hspace{0.17em}\hspace{0.17em}i=0,...,2n$$

(35)

$${\widehat{\mathbf{x}}}_k^{-}=\sum _{i=0}^{2n}{W}_i^{(m)}{({\zeta }_k^{-})}_i$$

(36)

$$P_k^{-}=\sum _{i=0}^{2n}{W}_i^{(c)}[{({\zeta }_k^{-})}_i-{\widehat{\mathbf{x}}}_k^{-}]{[{({\zeta }_k^{-})}_i-{\widehat{\mathbf{x}}}_k^{-}]}^T+Q_{k-1}$$

(37)

$${(Z_k^{-})}_i=h({({\zeta }_k^{-})}_i)$$

(38)

$${\widehat{z}}_k^{-}=\sum _{i=0}^{2n}{W}_i^{(m)}{(Z_k^{-})}_i$$

(39)

The measurement update (correction) step of the UKF involves the following steps:

$$P_{\mathit{zz}}=\sum _{i=0}^{2n}{W}_i^{(c)}[{(Z_k^{-})}_i-{\widehat{z}}_k^{-}]{[{(Z_k^{-})}_i-{\widehat{z}}_k^{-}]}^T+R_k$$

(40)

$$P_{\mathit{xz}}=\sum _{i=0}^{2n}{W}_i^{(c)}[{({\zeta }_k^{-})}_i-{\widehat{x}}_k^{-}]{[{(Z_k^{-})}_i-{\widehat{z}}_k^{-}]}^T$$

(41)

$$K_k=P_{\mathit{xz}}{P}_{\mathit{zz}}^{-1}$$

(42)

$${\widehat{x}}_k={\widehat{x}}_k^{-}+K_k(z_k-{\widehat{z}}_k^{-})$$

(43)

$$P_k=P_k^{-}-K_k{P}_{\mathit{zz}}{K}_k^T$$

(44)

The samples are propagated through true nonlinear equations, which can capture the states up to at least second order.

6. Results and Discussion

Simulation experiments have been carried out to evaluate the performance of the UKF approach in comparison with the conventional EKF approach for estimating motion and shape parameters. Computer code was developed using MATLAB. It is assumed that, at each time instant k, a set of three feature points (x_i,k, y_i,k), i = 1, 2, 3 are observed. The state vector x_k is given by:

$$x_k={\left[{\omega }_{1,k}\hspace{0.17em}\hspace{0.17em}{\omega }_{2,k}\hspace{0.17em}\hspace{0.17em}{\omega }_{3,k}\hspace{0.17em}\hspace{0.17em}{c}_{1,k}\hspace{0.17em}\hspace{0.17em}{c}_{2,k}\hspace{0.17em}\hspace{0.17em}{c}_{3,k}\hspace{0.17em}\hspace{0.17em}{p}_k\hspace{0.17em}\hspace{0.17em}{q}_k\hspace{0.17em}\hspace{0.17em}{x}_{1,k}\hspace{0.17em}\hspace{0.17em}{y}_{1.k}\hspace{0.17em}\hspace{0.17em}{x}_{2,k}\hspace{0.17em}\hspace{0.17em}{y}_{2,k}\hspace{0.17em}\hspace{0.17em}{x}_{3,k}\hspace{0.17em}\hspace{0.17em}{y}_{3,k}\right]}^T$$

(45)

The associated state equation in discretized form is given by:

$$x_{\mathrm{k}+1}=f(x_{\mathrm{k}})$$

(46)

which is constituted from the set of Equations (24,25) and (27), as follows:

$$\begin{array}{l}{p}_{k+1}=({\omega }_{1,k}{q}_k-{\omega }_{2,k}-{\omega }_{2,k}{p}_k^2-{\omega }_{3,k}{p}_k{q}_k)\mathrm{\Delta }t+p_k\\ q_{k+1}=(-{\omega }_{1,k}{p}_k-{\omega }_{3,k}-{\omega }_{2,k}{p}_k{q}_k-{\omega }_3{q}_k^2)\mathrm{\Delta }t+q_k\\ r_{k+1}=({\mathrm{b}}_{3,k}-{\mathrm{b}}_{1,\mathrm{k}}{p}_k-{\mathrm{b}}_{2,k}{q}_k-r_k({\omega }_{3,k}{q}_k+{\omega }_{2,k}{p}_k))\mathrm{\Delta }t+r_k\end{array}$$

(47)

$$\begin{array}{l}{x}_{k+1}=({\omega }_{2,k}+{\omega }_{1,k}{y}_k+{\omega }_{2,k}{x}_k^2+{\omega }_{3,k}{x}_k{y}_k+(c_{1,k}-x_k{c}_{3,k})(1-p_k{x}_k-q_k{y}_k))\mathrm{\Delta }t+x_k\\ y_{k+1}=({\omega }_{3,k}-{\omega }_{1,k}{x}_k+{\omega }_{2,k}{x}_k{y}_k+{\omega }_{3,k}{y}_k^2+(c_{2,k}-y_k{c}_{3,k})(1-p_k{x}_k-q_k{y}_k))\mathrm{\Delta }t+y_k\end{array}$$

(48)

$$c_{i,k}=b_{i,k}/r_k$$

(49)

along with the random walk models for the angular velocities ω_i,k+1 = ω_i,k1, i = 1, 2, 3. Let z_k ∈ ℜ⁶ represent the observation vector, the observation equation can then be written as:

$$z_k={Hx}_k+v_k$$

(50)

where v_k is assumed to be a zero mean Gaussian white sequences noise with covariance R = σ²I₆(σ = 0.01). The elements of the measurement model H_2n×14 is defined by:

$$H=[0_{2n\times 8}\hspace{0.17em}\hspace{0.17em}{I}_{2n}],\hspace{0.17em}\hspace{0.17em}n=3$$

The process noise covariance matrix is given by Q = 10⁻¹⁰ × I₁₄ and the parameters utilized in the UKF are given as follows: α = 1e − 4, β = 2, κ = 0. The sigma points capture the same mean and covariance irrespective of the choice of matrix square root which is used. The numerical efficient and stable method such as the Cholesky factorization has been used in obtaining the sigma points.

Experiment was conducted on a simulated three feature points: (2.5,2.5), (1.5,0.5) and (1.0,1.5) locations. The motion and shape parameters Ω is set as (ω₁, ω₂, ω₃) = (−0.2, 0.1, −0.1), with initial values for shape parameters are (p₀, q₀, r₀) = (0.1, 0.1, 2.0). For comparative purposes, the following two cases are considered [2].

$$\begin{array}{l}\text{Case}1:b={(\begin{array}{ccc}0.1& 0.1& -0.1\end{array})}^T\in \text{Im}\mathrm{\Omega }\\ \text{Case}2:b={(\begin{array}{ccc}0.1& 0.1& 0.1\end{array})}^T\notin \text{Im}\mathrm{\Omega }\end{array}$$

Figures 5–12 show the results for the numerical experiments. Figure 5 shows the sample trajectories for the three feature points on image plane for the case b ∈ ImΩ. It can be seen that the motions are circular and periodic around the axis of rotation. Figures 6–8 are the results for the case when b ∈ ImΩ. Estimation results for parameters c₁, c₂ and c₃ for the three feature points are given in Figure 6. Three curves, shown in red, green, and black colors, denote the true, EKF-based, and UKF-based estimated values, respectively. For better clarity, the estimation errors for the three feature points are shown in Figure 7, which illustrates the advantage of UKF. Figure 8 gives the estimation accuracy for the shape parameters p_k, q_k and r_k. The EKF is not working very well whereas the UKF demonstrates its good capability for capturing the true trajectories. Comparison of RMSE (in the unit of mm) of the three feature points on the image plane using the EKF and UKF is summarized in

Table 1. Comparison of RMSE (in the unit of mm) of the three feature points on the image plane using the EKF and UKF (b ∈ ImΩ).

	EKF			UKF
	x	y	x-y	x	y	x-y
Feature point 1 (x1,y1)	0.0051	0.0057	0.0077	0.0011	0.0011	0.0016
Feature point 2 (x2,y2)	0.0037	0.0037	0.0052	0.0009	0.0008	0.0012
Feature point 3 (x3,y3)	0.0058	0.0043	0.0072	0.0008	0.001	0.0013

. Comparison of RMSE (in the unit of mm) for the feature points of the scene point using the EKF and UKF is summarized in

Table 2. Comparison of RMSE (in the unit of mm) for the feature points of the scene point using the EKF and UKF (b ∈ ImΩ).

	EKF				UKF
	X	Y	Z	X-Y-Z	X	Y	Z	X-Y-Z
Feature point 1 (x1,y1)	5.35	5.6286	5.4815	9.5053	1.5783	1.7815	0.7082	2.4832
Feature point 2 (x2,y2)	2.4956	1.0329	1.7686	3.2284	0.621	0.2423	0.3968	0.7757
Feature point 3 (x3,y3)	1.6892	2.546	1.9205	3.6088	0.5264	0.8431	0.5515	1.1367

.

Figure 9 shows the sample trajectories for the three feature points on image plane for the case b ∉ ImΩ. The performance comparison for EKF and UKF in the case of b ∉ ImΩ is basically similar to the results obtained for b ∈ ImΩ. The motions are receding to infinitely linearly in time. Figures 10–12 provide the results for the case when b ∉ ImΩ. Figure 10 shows the estimation results for parameters c₁, c₂ and c₃and for the three feature points; Figure 11 provides the estimation errors for the three feature points; and Figure 12 gives the estimation accuracy for the shape parameters p_k, q_k and r_k. Comparison of RMSE (in the unit of mm) of the three feature points on the image plane using the EKF and UKF is summarized in

Table 3. Comparison of RMSE (in the unit of mm) of the three feature points on the image plane using the EKF and UKF (b ∉ ImΩ).

	EKF			UKF
	x	y	x-y	x	y	x-y
Feature point 1(x1,y1)	0.0056	0.0073	0.0092	0.001	0.0011	0.0015
Feature point 2 (x2,y2)	0.0033	0.0029	0.0044	0.0009	0.0008	0.0011
Feature point 3 (x3,y3)	0.0028	0.0025	0.0037	0.0008	0.001	0.0013

. Comparison of RMSE (in the unit of mm) for the feature points of the scene point using the EKF and UKF is summarized in

Table 4. Comparison of RMSE (in the unit of mm) for the feature points of the scene point using the EKF and UKF (b ∉ ImΩ).

	EKF				UKF
	X	Y	Z	X-Y-Z	X	Y	Z	X-Y-Z
Feature point 1 (x1,y1)	2.9116	2.557	5.1281	6.4276	0.476	1.4102	0.7018	1.6455
Feature point 2 (x2,y2)	0.8623	1.0275	1.6439	2.1217	0.0938	0.0894	0.1173	0.1748
Feature point 3 (x3,y3)	0.8424	1.1548	1.9177	2.3918	0.0562	0.087	0.1245	0.1619

.

In general, utilization of an adequate nonlinear model with suitable filter makes it possible to achieve improved estimation performance. For the problem of single camera based motion and shape estimation, the UKF-based recursive estimation algorithm is a good alternative adopted for implementing the estimation of motion and planar surface parameters. The states and the dynamic process are related nonlinearly. The nonlinearity/uncertainty in the state estimate is suitably taken care of in the UKF, which has therefore demonstrated substantial state estimation accuracy improvement as compared to the EKF based approach. When compared with EKF, the UKF method exhibits superior performance since the series approximations in the EKF algorithm can lead to poor representations of the nonlinear functions and probability distributions of interest.

7. Conclusions

Various engineering applications, such as robotics and machine vision, require the estimation of positions and velocities of moving features in space, leading to the problem of estimating motion and shape parameters of moving features from their corresponding image data observed over time. To alleviate the noise and obtain better estimation accuracy in feature points on the moving objects, the filtering/estimation approach can be used. A variety of estimation techniques have been adopted to reduce the noise. One of the most important ones is the extended Kalman filter (EKF).

As compared to the EKF’s linear approximation, the unscented transformation is accurate to the second order for any nonlinear function. In light of unscented Kalman filter’s superiority over the extended Kalman filter, this paper has presented a deterministic sampling of UKF approach for estimating motion and shape parameters of feature points on the moving object. Such a motion is described by the nonlinear model with skew symmetric matrix Ω, which is widely used in the theory of machine vision. The reason is due to the fact that the UKF is able to deal with the nonlinear formulation, which will ensure better accurate parameter estimation. For the nonlinear estimation problem, alternatives for the classical model-based extended Kalman filter (EKF) can be employed. The UKF is a nonlinear distribution approximation method, which uses a finite number of sigma points to propagate the probability of state distribution through the nonlinear dynamics of system. The UKF exhibits superior performance when compared with conventional EKF since the series approximations in the EKF algorithm can lead to poor representations of the nonlinear functions and probability distributions of interest.

The analyses were confirmed by simulation studies. For both cases, b ∈ ImΩ and b ∉ ImΩ, motion and shape parameters were recovered successfully with remarkable accuracy improvement. The results obtained shows that the proposed UKF method have been compared to the EKF and have demonstrated substantial improvement in obtaining the motion and shape of moving objects.