Position and Orientation Estimation

In the following, the previous localization problem is generalized in order to allow the estimation of the pose—position and orientation—of an extended object in 3D. Hence, the object state is given by

x,^ht^T t˙^T r^T ω^TiT

where t and ˙t are the position and velocity in 3D, respectively, andω is the angular velocity in 3D . The orientation vector r ∈R³is a so-called rotation vector with norm kr k = π. Alternatives for describing the orien-tation are quaternions [108] or Euler angles [130]. Quaternions however

170 5 Applications

are not minimal as they consist of four elements and the unit quaternions constraint can lead to inaccurate state estimates. Euler angles suffer from singularities and are not intuitive in usage. Rotation vectors instead are a minimal state representation. A further advantage of rotation vectors is that the dynamic behavior can be described by means of a nonlinear differential equation [22].

Dynamic Model

As the state now also comprises orientation related quantities, the dy-namic model consists of two separate motion models: one for the trans-lation and the second for the rotation. The transtrans-lation model coincides with (5.1) and (5.2), respectively. The temporal evolution of the rotation vector r_kis described by means of a nonlinear equation [17, 22]

r_k+1= r_k+ T ·¡I + 0.5·R¡r_k¢ + a¡r_k¢ ·R¡r_k¢ ·R¡r_k¢¢

The system model for the angular velocityω_kis assumed to be

ω_k+1= ω_k+ w^ω_k , (5.12)

where w^w_k is the process noise that affects the angular velocity. The pro-cess noise has zero mean and is Gaussian distributed with covariance C^ω. By combining (5.2), (5.11) and (5.12), the system model for the pose esti-mation scenario can be written as

x_k+1=

5.1 Range-based Localization 171

where the covariance of the process noise w_kcomprises the covariances of the process noises from the translation model C^t and the rotation model C^ωaccording to

C^w=





C^t 0 0

0 0 0

0 0 C^ω



.

Measurement Model

As depicted in Figure 5.1b, in range-based pose estimation the measured ranges depend on known landmarks located on the extended object and on known landmarks in a global coordinate system. Thus, the ranges depend on both the unknown translation and rotation of the object with respect to the global coordinate system.

Example 25: Telepresence

An example application where knowing the pose is of interest is large-scale telepresence [153]. Here, the pose of a human has to be tracked for steering a robot. For tracking the pose of the user or the pose of several body parts, several emitters are located at known positions in a global coordinate system. They are emitting signals that are received by several sensors attached to the user, e.g., at a head-mounted display (see Figure 5.3a) or at gloves (see Figure 5.3b).

Based on the emitted and received signals, ranges between emitters and sensors can be determined.

The relationship between measured ranges, translation, and rotation is given by

ρk,i j=

°

°

°Lj− D¡r_k¢ · M_i− t_k− v_{k,i j}

°

°

°₂ , (5.14)

which resembles the noise before non-linearity range measurement model as in (5.5). Here, L_jis the position of the j th landmark with respect to the

172 5 Applications

(a) Sensors at a head-mounted display.

(b) Sensors mounted at a glove.

Figure 5.3: Deployment of sensors (marked by blue circles) for tracking the pose of a user or of body parts in telepresence applications. Images taken from [111].

global coordinate system and M_iis the position of the i th landmark with respect to the object coordinate system. v_{k,i j} is the measurement noise between landmark L_jand landmark M_i. The termρk,i j is the measured range between these two landmarks, while d_kcomprises all possible mea-surements between global and object landmarks. The term D( · ) is the rotation matrix parametrized by the rotation vector r_k

D¡r_k¢ = I +sin¡° known as Rodrigues formula, with

R¡r_k¢ =

The system model (5.13) is conditionally linear, i.e., if the rotation vector is set to a fixed value, the system model becomes linear and the

predic-5.1 Range-based Localization 173

tion for each value can be performed by using the well-known Kalman predictor equation.

To facilitate an analytical solution of the measurement update in closed from, the measurement equation (5.14) is squared as in Section 5.1.1, which yields

d_{k,i j},^³ρk,i j

´2

=³

gi j¡r_k¢ − t_k− v_{k,i j}´T

·³

gi j¡r_k¢ − t_k− v_{k,i j}´

, (5.15) with

gi j¡r_k¢

,L_j− D¡r_k¢ · M_i. (5.16)

But in contrast to the previous section, squaring the measurement model alone is not sufficient due to the nonlinear term (5.16). By condition-ing on r , however, (5.16) becomes affine and the entire measurement model becomes quadratic. Thus, the measurement model is condition-ally integrable and the nonlinear-nonlinear decomposition proposed in Section 2.5.2 can be applied. The analytically integrable system state comprises x^T_a,^ht^T t˙^T w^T

i

, while the sampled state is x_s,r . For the latter, sampling via LRKFs can be employed. In doing so, for every fixed sample value of the rotation vector, the closed-form solutions in (5.10) for the moment integrals can be used with some minor modifications: the state dimension is now nine instead of six and the number of measured ranges is significantly higher as pair-wise measurements between object landmarks and global landmarks are incorporated.

Example 26: Two-dimensional Localization

A two-dimensional coordinate system is considered containing four sensors (global landmarks) and four emitters (object landmarks)

174 5 Applications

with respect to the object coordinate system and the global coor-dinate system, respectively. At different noise levels ranging from [0.000001, . . . ,0.3] m, 1000 random trajectories are generated, where the sampling time was T = 0.1s. The noise process is assumed as isotropic.

The proposed SAGF is compared to the UKF. For the UKF a decom-position into directly observed states t , r and indirectly observed states ˙t ,ω as proposed in Section 2.5.1 is used. Furthermore, due to the fact that the measurement noise is mapped through the non-linear transformation, it has to be approximated with samples as well. In total 71 sample points are required for the UKF. On the other hand, the proposed approach only has to approximate the rotation by sample points.

For the 2D case, the system equation (5.13) becomes linear and thus, the Kalman filter prediction can be using directly. The entries of the continuous process noise covariance are set to be C^t_c= diag ([0.1 0.1]) and C_c^ω= 0.1. The initial state has zero mean and its covariance matrix comprises C₀^t= diag ([10 10]), C0^˙t= diag ([10 10]), σ²r,0= 0.001, andσ²_ω,0= 0.0001.

The estimation performance in terms of the rmse of both estimators is almost identical. Regarding the computational effort, the proposed approach only has to determine sample points for one dimension, which can be implemented very efficiently. On the other hand, the UKF calculates a matrix square root of the covariance of the noise and the directly observed state. This operations is computationally