IEEE Robotics & Automation Magazine - June 2015 - 29

0.5
0
-0.5

0

5

10

15
20
25
Time (s)
(a)

30

Autonomous Navigation
In this experiment, the system is set to follow a trajectory in 3-D
space. In this way, the dynamic properties of the system can be
fully tested. The results confirm the localization properties of the
presented architecture and fully validate the proposed approach
for autonomous navigation in an unknown environment. The trajectory is generated according to previous works [24], [26]. Since
the input M is an algebraic function of the fourth derivative of the
position (snap), it is convenient to plan smooth trajectories that
minimize the snap of the trajectory using the cost functional,
min

Vy (m/s)

2
0

5

10

15
20
25
Time (s)
(b)

30

1
0

5

10

15
20
25
Time (s)
(c)

30

35

Figure 7. The Cartesian positions of the vehicle during the hovering
phase (blue: Vicon, red: Tango estimation, and green: the estimator).
Some jumps are noticeable in the plots (orange circles). They represent
an external Cartesian perturbations made by the user to show the
stability of the whole system: (a) the x Cartesian component, (b) the y
Cartesian component, and (c) the z Cartesian component.

#t

tf
0

nd

d 4 x d (t)
dt 4

2

dt,

Vicon
Fused

1
0
-1

0

5

10

15

20
25
Time (s)
(a)

30

35

0

5

10

15
20
25
Time (s)
(b)

30

35

0

5

10

15
20
25
Time (s)
(c)

30

35

2
0
-2

35

2
0

where R is the vehicle's rotation and R d the desired one. As
shown in Table 2, the proposed filtering technique is able to
keep the errors at the same order of magnitude as the Tango device, but with the additional benefit of increased estimation rate,
since the pose and the velocity are updated at 100 Hz. In Table 3,
we summarize the velocity RMSE for the three Cartesian components with respect to the Vicon motion-capture system. The
error is small, with an average value of 0.06 m/s.

35

3

1

W ^R, R d h = 1 tr ^I - R Td Rh,
2

Vx (m/s)

Vicon
Tango
Fused

components are suitable for robotic tasks. The orientation
error is evaluated according to [25] as

Vz (m/s)

z Position (m)

y Position (m)

x Position (m)

System Stabilization
In this experiment, the system is set to hover in a defined 3-D
position in space. To show the effectiveness of the proposed
controller and estimation pipeline, perturbations are applied to
the vehicle. The vehicle promptly returns to the set point,
showing the stability of the controller presented. This behavior
is shown in Figure 7, where after the taking-off phase (the first
5 s), the vehicle reaches the specified altitude and the goal position is set to p = 60.2 2.2 1.2@T m, while b 1d = 61 0 0@T .
Then, three consecutive perturbations of around 0.7 m along
the z-axis, y-axis, and x-axis are noticeable (see orange circles
in Figure 7). The corresponding velocity values are shown in
Figure 8. After each perturbation (highlighted by the orange
dashed ellipse), the vehicle controller commands changes in
velocity to allow the system to return to its original position.
The peak velocities are around 2 m/s, and, in each case, the
controller takes less than 3 s to stabilize, suggesting a closedloop control bandwidth of around 0.25-0.5 Hz. During each
disturbance, the estimator is able to identify the pose and velocity of the system (this is noticeable just comparing the estimation with respect to the Vicon data in Figures 7-9) and
reacts promptly, once released, increasing the spatial velocity to
return to its hovering position. The performance is compared
with the motion capture system, and root mean square error
(RMSE) values are reported in Table 2. These results can be
considered a first benchmark for this new device. It exhibits
strong localization performances, and, thus, the COTS

2
0
-2

Figure 8. The Cartesian velocities of the vehicle during the hovering
phase (blue: Vicon, green: the estimator). Some jumps are noticeable
in the plots (orange circles). They represent the effects of external
Cartesian perturbations made by the user to show the stability of the
whole system: (a) the x Cartesian component, (b) the y Cartesian
component, and (c) the z Cartesian component.

June 2015

*

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

29
Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - June 2015
