the state 1 or 2 depending on the value of θ
tilt
as it is
presented in Figure 10 a) and 10 c). And ﬁnally, states
3 and 4 depend on the value of ψ
tilt
as it is shown in
Figure 10 b) and 10 c).
It is important to note that the system can change
between states with an error smaller than 1
◦
in the tilt
estimation as it is presented in Figure 10.
4.2 3D Positioning Estimation
Once the FSM is analysed, the next step is to check
the 3D positioning. To this end two experiments
were performed. In the ﬁrst one, the robot travels in
straight line with a speed of 10.3cm/s on a surface
with a elevation angle of ﬁfteen degrees (θ = 15
◦
).
In the second one, the robot drives a circular path
(speed of left wheel= 17.5cm/s and speed of right
wheel= 10.3cm/s) on a surface with bank angle of
minus twenty degrees (ψ = −20
◦
). The experiments
take 10 seconds in both cases. In Figure 11 some
snapshots of theses experiments are presented.
a)
b)
t=0s
t=0s
t=2s
t=10s
t=1,5s t=5s
Figure 11: Experiments for checking the 3D positioning es-
timation. a) Straight line with θ = 15
◦
and ψ = 0
◦
(state 1).
b) Circular path with θ = 0
◦
and ψ = −20
◦
(state 4).
To check the accuracy of the algorithm, the real
position (x,y,z) and orientation (ϕ,θ,ψ) of the robot
is obtained from the images of the video camera. The
estimated parameters ( ˆx, ˆy, ˆz,
ˆ
ϕ,
ˆ
θ,
ˆ
ψ) are calculated by
the proposed algorithm. The results of the ﬁrst exper-
iment is depicted in Figure 12 and in Figure 13 for the
second one. As it is shown in the Figures 12 and 13
the algorithm is a good 3D positioning estimator with
a low estimation error. The elevation and the bank an-
gles do not have accumulative errors because they are
obtained directly from the accelerometer. The rest of
the parameters, will have accumulative error over the
time because they are calculated by odometry.
4.3 The Heading Reference System
To analyse how the heading reference system im-
proves the 3D positioning estimation, a new exper-
0 1 2 3 4 5 6 7 8 9 10
0
50
100
0 2 4 6 8 10
-1
0
1
0 2 4 6 8 10
0
10
20
30
a)
b)
c)
Figure 12: 3D positioning estimation in a straight line. a)
Figure 12: 3D positioning estimation in a straight line. a)
Real position (x,y,z) (solid lines) and estimated position
( ˆx, ˆy, ˆz) (square line). b) Real heading ϕ (solid line) and
estimated heading
ˆ
ϕ (circle line). c) Real elevation θ (solid
line) and estimated elevation
ˆ
θ (circle line). The bank angle
ˆ
ψ is not depicted because is zero as it was deﬁned in (30).
0 1 2 3 4 5 6 7 8 9 10
-20
0
20
40
0 2 4 6 8 10
0
200
400
0 2 4 6 8 10
-40
-20
0
a)
b)
c)
Figure 13: 3D positioning estimation in a circular path. a)
Real position (x,y,z) (solid lines) and estimated position
( ˆx, ˆy, ˆz) (square line). b) Real heading ϕ (solid line) and es-
timated heading
ˆ
ϕ (circle line). c) Real bank ψ (solid line)
and estimated bank
ˆ
ψ (circle line). The elevation angle
ˆ
θ is
not shown due to is zero according to (38).
iment has been set up. In this case, the robot has
been programmed with a trajectory as it is shown in
the Figure 14. The real path of the robot has been
recorded by the video camera and two estimations
have been calculated in the same experiment. One
of them without the heading reference system and the
another one using this system. The real heading and
its estimations by the algorithm are presented in the
Figure 15. As it can be noticed from this ﬁgure, the
error in the heading estimation without the reference
system increases over time. However when the ref-
erence system is activated, the error in the heading
estimation does not increase.
Considering the position parameters, the results of
the experiment are depicted in the Figures 16 and 17.
As it is shown in these ﬁgures, when the position
is obtained with the reference system proposed in this
paper, the estimation error is reduced considerably. If
the real position vector is deﬁned as p = {x, y, z} and
the estimation position vector as
ˆ
p = { ˆx, ˆy, ˆz}, the po-
sitioning error can be deﬁned as P
error
= |p−
ˆ
p|. In
Figure 18, the positioning error is presented for this
ICINCO2015-12thInternationalConferenceonInformaticsinControl,AutomationandRobotics
12