IEEE Robotics & Automation Magazine - June 2015 - 30

1
0

0.2
0
-0.2

5

10

15
20
25
Time (s)
(a)

30

35

0.8
0.6
0.4

0

5

5

10

10

15
20
25
Time (s)
(b)

30

35

0
-0.5

15
20
25
Time (s)
(c)

30

15
20
25
Time (s)
(d)

30

35

5

10

35

Position

Tango Estimation (m) UKF Estimation (m)

x

0.0165

0.0195

y

0.0320

0.0296

z

0.0274

0.0299

Orientation

Tango Estimation

UKF Estimation

0.0627

0.091

W ^ R, R d h

Table 3. The velocity RMSE of the estimation filtering
technique compared to Vicon.
Component

UKF Estimation (m/s)

x

0.0651

y

0.0533

z

0.0788

where n d is a constant value, and t 0 and t f the inital and final
time, respectively. This minimization problem can be formulatIEEE ROBOTICS & AUTOMATION MAGAZINE

*

June 2015

0.5

1

1.5

Figure 10. A view in the x - y plane of the Cartesian 3-D position of
the vehicle, with the desired trajectory (blue), Tango estimates (red),
and UKF estimates (green).

Table 4. The position RMSE of the Tango estimation
and of the estimation filtering technique with
respect to the desired values.

0.9
0.8
0.7
0

-1.5
-2.5 -2 -1.5 -1 -0.5
0
x Position (m)

Table 2. The position and orientation RMSE of the
Tango estimation and of the estimation filtering
technique compared to Vicon.

*

0.5

-1
0

Figure 9. The orientation of the vehicle during the hovering phase
(blue Vicon, red Tango estimation, and in green the estimator). Some
disturbances are observable on the rotational values. Those are the
effects of the vehicle recovering the initial position. (a) x Cartesian
component, (b) y Cartesian component, (c) z Cartesian component,
and (d) scalar.

30

Desired
Tango
Fused
Waypoints

1.5

y Position (m)

q1 Orientation
q2 Orientation
q3 Orientation
q0 Orientation

0.2
0
-0.2

Vicon
Tango
Fused

Position

Tango Estimation (m) UKF Estimation (m)

x

0.0593

0.0565

y

0.0719

0.0657

z

0.0122

0.0125

Table 5. The velocity RMSE of the estimation filtering
technique with respect to the desired values.
Component

UKF Estimation (m/s)

x

0.0626

y

0.0833

z

0.0214

ed as a quadratic program [24]. Furthermore, equality constraints can be enforced and can be determined by desired robot
positions (or velocities). A trajectory of 9 s, presenting an oval
shape (see Figure 10), is generated passing through four points
in space (P1 = 6-1 -1.5 -1.2@T , P2 = 60 -0.5 -1.2@T ,
P3 = 60 0.5 -1.2@T , P4 = 6-1 -1.5 -1.2@T , for the x, y,
and z components, respectively) according to the mentioned
technique. The trajectory's sampling frequency is chosen to be
the same of the feedback control signal. The velocity reaches values of 1 m/s.
In Tables 4 and 5, the RMSE values of the Cartesian position and velocity (shown in Figures 11 and 12, respectively), with respect to the planned trajectory, are reported. The
error is relatively small, with maximum values of around 6
cm, and similar to the error obtained during the stabilization task. This confirms the feasibility of using this new device and the presented approach for real robotic tasks. In
Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - June 2015
