Technical documentation for AR.

Drone project
Inkyu, Hu and Van
Cyphy Lab.
Queensland University of Technology
January 14, 2013
Contents
1 Introduction 2
2 System Overview 2
2.1 AR.Drone platform . . . . . . . . . . . . . . . . . . . . . . . . 2
2.2 Initial ARDrone coordinates . . . . . . . . . . . . . . . . . . . 2
2.3 Navdata . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
2.4 ROS topic name . . . . . . . . . . . . . . . . . . . . . . . . . . 3
3 VICON comparison 4
4 Software building instruction 5
4.1 Building ardrone autonomy package . . . . . . . . . . . . . . . 5
4.2 Running ardrone autonomy driver . . . . . . . . . . . . . . . . 6
4.3 Building ethzasl ptam package . . . . . . . . . . . . . . . . . . 6
4.4 Running ethzasl ptam package . . . . . . . . . . . . . . . . . . 7
4.5 Building ardrone pose estimation package . . . . . . . . . . . 10
4.6 Running ardrone pose estimation package . . . . . . . . . . . 10
5 Kalman Filter for states estimation 12
5.1 States . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
5.2 Process model . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
5.3 Measurement model . . . . . . . . . . . . . . . . . . . . . . . . 12
5.4 Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
5.5 Update . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
6 Scale Estimation 13
6.1 Scale calibration . . . . . . . . . . . . . . . . . . . . . . . . . . 13
6.2 Online scale estimation . . . . . . . . . . . . . . . . . . . . . . 13
6.3 Experimental validation . . . . . . . . . . . . . . . . . . . . . 13
7 Control 13
7.1 System Identication . . . . . . . . . . . . . . . . . . . . . . . 13
7.2 Controller design . . . . . . . . . . . . . . . . . . . . . . . . . 14
7.3 Performance evaluation . . . . . . . . . . . . . . . . . . . . . . 14
8 Experimental results 14
1
1 Introduction
This document addresses the procedure how to develop an autonomous y
system based on o-the-shelf AR.Drone.
2 System Overview
2.1 AR.Drone platform
2.2 Initial ARDrone coordinates
This document describes our ardrone coordinate systems: a IMU and a cam-
era.
Figure 1: Close look. IMU and camera coordinate denitions. Red, green
and blue denote x,y,z respectively.
2.3 Navdata
navdata is a fundamental data structure which contains Euler angles, alti-
tude, accelerations, linear velocity
1
. This data can be obtained up to 100 Hz.
1
ROS Header message denition. http://www.ros.org/doc/api/roslib/html/msg/
Header.html
2
Table 1: navdata structure denition with SI unit.
navdata type Description Unit
batteryPercent oat32 0 to 100 %
rotX oat32 left/right tilt

rotY oat32 forward/backward tilt

rotZ oat32 orientation,yaw

altd oat32 estimated altitude m
vx oat32 linear x velocity m/s
vy oat32 linear y velocity m/s
vz oat32 linear z velocity m/s
accx oat32 body x acceleration m/s
2
accy oat32 body y acceleration m/s
2
accz oat32 body z acceleration m/s
2
gyrox oat32 angle rate about x axis

/s
gyroy oat32 angle rate about y axis

/s
gyroz oat32 angle rate about z axis

/s
tm oat32 Time stamp from ardrone sec
header Header ROS header
1
2.4 ROS topic name
For ROS, we have to dene topic names and namespace. Figure2 shows brief
description of ROS topic messaging map. More detail also can be found in
the repository named ardrone navi/src/ours.
3
Data
ardrone_brown
ARDrone
Parrot ardrone_pose_estimation
drone_teleop cyphy_kbd
Cmd
/ardrone/navdata
time stamp (sec)
pitch (deg)
roll (deg)
yaw (deg)
vx (m/s)
vy (m/s)
vz (m/s)
altitude (m)
Image topic
/ardrone/image_raw
/cmd_vel
linear.x
linear.y
linear.z
angular.z
/cmd_vel
/keycode
100Hz
15Hz
rviz
/tf
/world to /base_link
Figure 2: Blue boxes denote ROS nodes the orange box is a ardrone platform.
Gray boxes present both ROS topic name and data structure. Generally a
prex / stands for ROS topic.
0 10 20 30 40 50 60 70
0.8
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
sec
m
/
s


ardrone vx
ground truth vx
0 10 20 30 40 50 60 70
1.5
1
0.5
0
0.5
1
1.5
sec
m
/
s


ardrone vy
ground truth vy
Figure 3: Blue denotes ground truth velocity obtained from VICON and red
is AR.Drone estimated lateral x and y velocity respectively.
3 VICON comparison
In this section ground truthed comparison is presented. We compared lat-
eral body velocity estimation from AR.Drone to velocity obtained from sub-
millimetre accuracy VICON system. In addition roll-pitch-yaw angle estima-
tion comparison is presented.
4
0 10 20 30 40 50 60 70
10
5
0
5
10
15
sec
D
e
g


ardrone roll (degree)
ground truth roll
0 10 20 30 40 50 60 70
20
15
10
5
0
5
10
sec
D
e
g


ardrone pitch (degree)
ground truth pitch
0 10 20 30 40 50 60 70
10
5
0
5
10
sec
D
e
g


ardrone yaw (degree)
ground truth yaw
Figure 4: Blue denotes ground truth angles obtained from VICON and red
is AR.Drone estimated angles, roll, pitch and yaw respectively.
4 Software building instruction
In this section, how to build ardrone brown, ardrone pose estimation and
ethzasl ptam packages will be described step by step. Before getting started,
we dene the repository sync local folder as:
1 /home/ enddl 22 /Workspace/ a r d r i ne s i d e p r o j e c t
and ROS working space path is
1 /home/ enddl 22 /Workspace/ f ue r t e
Note that this path should be dierent for each machine.
4.1 Building ardrone autonomy package
Compared to previous ardrone brown package, ardrone autonomy package
extended the driver compatible with AR.Drone 2.0 and also increase the
image sampling rates.
In order to build ardrone autonomy package on your local machine, you
should checkout the package from our repository located in https://enddl22@
bitbucket.org/enddl22/ardrone_side_project.git. Once you are under
the folder named ardrone autonomy, the driver should be built with following
commands.
1 . / bui l d s dk . sh
5
2 mkdir bui l d
3 cd bui l d
4 cmake . .
5 make
Now you should see the ardrone driver appearing under subfolder /bin.
4.2 Running ardrone autonomy driver
By running this package we are able to access all data of AR.Drone. Open a
new console and type
1 r os c or e
Searching SSID ardrone 015526 from your WiFi searching lists and this
SSID should be dierent for each ardrone. After establishing connection,
type
1 i f c o n f i g
You should able to see 192.168.1.x for your WiFi adaptor. Go to ardrone brown/bin
and run ros ardrone driver node.
1 cd /home/ enddl 22 /Workspace/ f ue r t e /ardrone brown/ bi n
2 . / ar dr one dr i ve r
Checking all data is being published using ros topic command.
1 r o s t opi c l i s t v
You should be able to see /ardrone/navdata on the publishing lists.
1 r o s t opi c echo / ardrone /navdata
You should be able to see data, rotX, rotY, rotZ, accX etc on the console.
Further if you want to plot all data from AR.Drone, type
1 r os l aunch ardrone brown r xpl ot dat a . l aunch
You should be able to see Fig. 5.
4.3 Building ethzasl ptam package
To avoid corruption the local repository, lets make a working copy to workspace.
1 cp a . / a r dr o ne s i de pr o j e c t / s r c / ours / et hzas l pt am /
home/ enddl 22 /Workspace/ f ue r t e /
2 cd /home/ enddl 22 /Workspace/ f ue r t e / et hzas l pt am
3 rosmake et hzas l pt am
6
Figure 5: Plotting all data using rxplot tool.
You should be able to see the following log message
. . .
[ rosmake ] Bui l t 25 packages wi th 0 f a i l u r e s .
[ rosmake ] Summary output to di r e c t or y
Now we are at
1 pwd /home/ enddl 22 /Workspace/ f ue r t e / et hzas l pt am
Lets check all executable binaries are generated correctly.
1 cd ptam/ bi n
You should be able to see build, bin folders and also can see executable les
from bin folder inside.
4.4 Running ethzasl ptam package
We need open 4 terminals for roscore, ardrone brown ros ardrone driver,
image proc to covert RGB image into gray scale and ptam. Running the
following commands for each terminal window.
7
1 r os c or e
2 ar dr one br one ( di r path=ardrone brown/ bi n )
3 ROS NAMESPACE=ardrone r osr un i mage proc i mage proc ( di r
path=i mage pi pe l i ne / i mage proc / bi n )
4 r os l aunch . / ptam. l aunch ( di r path et hzas l pt am/ptam/
l aunch )
You should be able to see Fig.6.
8
(a) Image overlaying window with tracking features and the reference frame.
(b) 3D Map and 6 degree of freedom camera position estimation visualisation
view
Figure 6: PTAM
9
4.5 Building ardrone pose estimation package
As mentioned before, lets make a working copy to workspace.
1 cp a . / a r dr o ne s i de pr o j e c t / s r c / ours /
ar dr one pos e e s t i mat i on /home/ enddl 22 /Workspace/
f ue r t e /
2 cd /home/ enddl 22 /Workspace/ f ue r t e /
ar dr one pos e e s t i mat i on
3 rosmake ar dr one pos e e s t i mat i on
You should be able to see the following log message
. . .
[ rosmake ] Res ul t s :
[ rosmake ] Cl eaned 20 packages .
[ rosmake ] Bui l t 20 packages wi th 0 f a i l u r e s .
[ rosmake ] Summary output to di r e c t or y
You should be able to see build, bin folders and also can see executable les
from bin folder inside.
4.6 Running ardrone pose estimation package
Make sure ardrone brown driver running and publishing navdata topic by
typing
1 r o s t opi c l i s t v
You should be able to see the following message
Publ i shed t opi c s :
/ ardrone /navdata [ ardrone brown/Navdata ] 1 publ i s he r
/ ardrone /i mage raw [ s ens or ms gs /Image ] 1 publ i s he r
/ r os out agg [ rosgraph msgs /Log ] 1 publ i s he r
/ r os out [ rosgraph msgs /Log ] 1 publ i s he r
/ c l oc k [ rosgraph msgs /Cl ock ] 1 publ i s he r
Launching the ardrone pose estimation node.
1 r os l aunch ar dr one pos e e s t i mat i on a r dr o ne na vi t r j .
l aunch
Note that if you are publishing navdata through rosbag le then you need
to modify source code. Since ardrone pose estimation will be started position
estimation 4 second later after receiving taking o command. This 4 second
was picked experimentally and is the time when AR.Drone goes to hovering
state.
10
Figure 7: Position estimation visualisation using rviz.
Modify ardrone pose estimation/src/ardrone pose estimation.cpp le line
number 338
i f ( now. toSec ( ) t a k e o f f s t a r t . toSec ( ) >4 &&
pi c k t a ke o f f t i me )
to
i f ( 1)
then rebuild the package
1 rosmake ar dr one pos e e s t i mat i on
Then you should be able to see Fig. 7. Note that rviz is installed properly
on your system otherwise need to build rviz by typing
1 rosmake r vi z
You should be able to see the following message.
[ rosmake 1] Fi ni s hed <<< r vi z [ PASS] [ 413. 15 seconds ]
WARNING: 857 compi l er warni ngs
[ rosmake ] Res ul t s :
[ rosmake ] Cl eaned 33 packages .
[ rosmake ] Bui l t 33 packages wi th 0 f a i l u r e s .
11
[ rosmake ] Summary output to di r e c t or y
5 Kalman Filter for states estimation
In this section, we will describe only states, process model and measurement
model. More detail and well-documented Kalman lter can be found from
wikipedia.
2
5.1 States
States are expressed w.r.t world coordinate.
X
k
=

x
k
y
k
x
k
y
k

T
(1)
5.2 Process model
Linear constant-velocity process model is
F =

1 0 0
0 1 0
0 0 1 0
0 0 0 1

(2)
where is sample rate of Kalman Filter, 160 Hz.
5.3 Measurement model
H =

1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1

(3)
z
k
= HX
k
(4)
where z
k
is 4 1 position and velocity measurement vector. Position is
obtained from PTAM with scale estimation at 15 Hz and velocity comes from
AR.Drone onboard at 160Hz. We choose Q and R matrix as
Q =diag

0.1 m 0.1 m 0.3 m/s 0.3 m/s

2
R =diag

0.1 m 0.1 m 0.05 m/s 0.05 m/s

2
2
wikipedia Kalman Filter. http://en.wikipedia.org/wiki/Kalman_filter
12
5.4 Prediction
States prediction may be represented

k
= F

X
k1
(5)
Covariance prediction may be represented

k
=F

P
k1
F
T
+Q (6)

k
is predicted covariance and

P
k1
is estimated covariance at k 1.
5.5 Update
Compute Kalman gain.
S =H

k
H
T
+R
K =

k
HS
1
Compute innovation matrix.
N =z H

k
States and covariance update

X
k
=

k
+KN

P
k
=

k
KH

k
6 Scale Estimation
6.1 Scale calibration
6.2 Online scale estimation
6.3 Experimental validation
7 Control
7.1 System Identication
For x,y,z,yaw models.
13
7.2 Controller design
Something about PIDs.
7.3 Performance evaluation
Simulated model output VS real model output.
8 Experimental results
Hovering and way points following.
14