Dead Reckoning and Kalman Filter Design

for Trajectory Tracking of a Quadrotor DAV

Qing-Li Zhou, Youmin Zhang*, SMIEEE, Yao-Hong Qu and Camille-Alain Rabbath, SMIEEE

Abstract - The dynamic equations of a quadrotor balance the torque created by the spinning rotors. The
unmanned aerial vehicle (UA V), namely Qball-X4, are relative speed of the left and right rotors is varied to control
analyzed and built, trajectory tracking control design the roll rate of the UAV. Increasing the speed of the left
based on an inner/outer loop control structure is motor by the same amount that the speed of the right motor
proposed in this paper. The method of analytic is decreased will keep the total thrust provided by the four
geometry is used in dead-reckoning, discrete Kalman rotors approximately the same. In addition,the total torque
filter has been applied to trajectory tracking for created by these two rotors will remain constant. Similarly,
improving the accuracy of estimated location. the pitch rate is controlled by varying the relative speed of
Integrated online waypoints pre-planning and the front and rear rotors. The yaw rate is controlled by
trajectory tracking control have been carried out. varying the relative speed of the clockwise (right and left)
Simulation results demonstrate the effectiveness of the and counter-clockwise (front and rear) rotors. The
proposed approach. collective thrust is controlled by varying the speed of all the
rotors simultaneously. Quadrotor UAVs have advantages
I. INTRODUCTION over other rotary-wing UAVs. It is mechanically simple and
A quadrotor is a rotary-wing UAV that has been widely is controlled by only changing the speed of rotation for the
used as the test-bed for several recent research projects. four motors [2]. Since the yaw rate is controlled by
Small quadrotor UAVs have many exciting potential changing motor speed,a tail rotor is not required to control
applications including flying indoors and in dense urban yaw rate and all thrusts can be used to provide lift. A
areas. However, the development of effective navigation quadrotor UAV may also be able to fly closer to an obstacle
and trajectory tracking strategies can be very challenging. than conventional helicopter configurations that have a
A quadrotor UAV has four rotors and requires no cyclic large single rotor without fear of a rotor strike [2,3].
or collective pitch. It can be highly maneuverable,and has The paper is undertaken in a Qball-X4 quadrotor UAV
the potential to hover,take off,fly,and land in small areas, model, newly developed by Quanser Inc. for Concordia
and can have simple control mechanisms [1, 2]. However, University through an NSERC Strategic Project Grant [4].
because of its low rate damping, electronic stability See Figure 1 for the UAV configuration.
augmentation is required for stabilizing flight. The four
motors in a quadrotor UAV locate at the front, rear, left,
and right ends of a cross frame. The quadrotor is controlled
by changing the speed of rotation of each motor. The front
and rear rotors rotate in a clockwise direction while the left
and right rotors rotate in a counterclockwise direction to

This work is supported in part by the Natural Sciences and Engineering

Research Council of Canada (NSERC) through a Strategic Project Grant
(STPGP 350889-07) and a Discovery Project Grant.
Qing-Li Zhou is with the Mechanical and Industrial Engineering
Department, Concordia University, Montreal, Quebec, H3G 2WI, Canada
Figure 1. The Qball-X4 UAV structure
(qi_zho@encs.concordia.ca).
Youmin Zhang is with the Diagnosis, Flight Control and Simulation
Lab at the Dept. of Mechanical and Industrial Engineering, Concordia The Qball-X4 is a quadrotor helicopter propelled by four
University, Montreal, H3G 2WI, Canada (e-mail: motors fitted with 10-inch propellers. Since the entire
ymzhang@encs.concordia.ca).
quadrotor UAV is enclosed within a protective carbon fiber
Yao-Hong. Qu is with the School of Automation, Northwestern
Polytechnical University, Xi'an, 710072, P. R. China (e-mail: cage, it is named as Qball-X4. The Qball-X4's proprietary
qyh0809@nwpu.edu.cn). He is currently a Visiting Scholar at the Dept. of design ensures safe operation as well as opens the
Mechanical and Industrial Engineering, Concordia University, Montreal, possibilities for a variety of novel applications.
H3G 2Wl, Canada.
The paper is organized as follows. Section II presents the
Camille-Alain Rabbath is with Defence Research & Development
Canada, Valcartier, Quebec, G3J 1X5 Canada (Camille- detailed modeling process of the Qball-X4 UAV. Section
alain.rabbath@drdc-rddc.gc.ca). III introduces the navigation and trajectory tracking control.
Corresponding author phone: (514) 848-2424 ext. 5225.

978-1-4244-7101-0/10/$26.00 2010 IEEE 119

Section IV presents numerical simulations in different flight where J = Jroll = Jpilch (4)
speeds and pre-planned trajectories. Based on the
are the rotational inertia of the device in roll and pitch axes.
simulation results,Section V gives the conclusion.
L is the distance between the propeller and the center of
gravity,and
II. QBALL-x4 UNMANNED UAV
(5)
This section describes the dynamic model of the Qball
represents the difference between the forces generated by
X4. The nonlinear model and a linearized model for the use
the motor 1 and motor 3.
in controller development are described. For the following
To facilitate the use of an integrator in the feedback
discussion,the axes of the Qball-X4 vehicle are denoted as
structure a state variable can be added to the state vector.
(x, y, z) and are defined with respect to the vehicle as
By combining the dynamics of motion for the roll/pitch axis
shown in Fig. 1. Roll, pitch, and yaw are defined as the and the actuator dynamics for each propeller,the following
angles of rotation about the x, y, and = axis, respectively. state space equations can be derived as:
The global workspace axes are denoted as (X,Y,Z) and

[ : i t [H
are defined with the same orientation as the Qball-X4
sitting upright on the ground. Fig. 2 shows the orientation
= Au (6)
and axes of the Qball-X4 UAV.
z. f.

Y'x o
CI?} where s = ()
Rotor
c. X YPosition Model
-

The motion of the Qball-X4 along the X and Y axes is

caused by the total thrust and by changing roll/pitch angles.
Assuming that the yaw angle is zero, the dynamics of
motion in X and Yaxes can be written as:

Rotor 3
MX =4Fsin(p)
Rotor 2
(7)
Figure 2. Axes and orientation of the Qball-X4 UAV
MY = -4Fsin(r)
Assuming that the roll and pitch angles are close to zero,
A. Actuator Dynamics the following linear state space equations can be derived for
The thrust generated by each propeller is modeled using X and Ypositions,
the following first-order system, o o
OJ
F=K u (I) 4K
--
o 0 P (8)
s+OJ M
where u is the PWM input to the actuator,W is the actuator o 0 -OJ
bandwidth and K is a positive gain. These parameters were
o o
calculated and verified through experimental studies. A
state variable, v, will be used to represent the actuator o
dynamics,which is defined as follows,
- 4K r
W
-
(9)
v= -- u (2) M
s+W -OJ

B. Roll/Pitch Model o
Assuming that rotations about the x and y axes are D. Yaw Model
decoupled,the motion in roll/pitch axis can be modeled,see
Fig. 2. As illustrated in Fig. 2,two propellers contribute to
The torque generated by each motor, r
is assumed to
the motion in each axis. The thrust generated by each motor have the following relationship with respect to the PWM
can be calculated from Eq. (1) and using its corresponding input u,
input. The rotation around the center of gravity is produced r=Ky u (10)
by the difference in the generated thrusts. The roll/pitch
where Ky is a positive gain. The motion in the yaw axis is
angle,() can be formulated using the following dynamics,
caused by the difference between the torques exerted by two
Je =M'L (3)
clockwise and two counterclockwise rotating propellers.

120
 The motion in the yaw axis can be modeled using the stability of the system, reducing the overshoot, and
following equation, improving the transient response.
J/jy =/).r (11) B. Dead Reckoning
In this equation, By is the yaw angle and Jy is the Dead Reckoning (DR) is the process of estimating one's
current position based upon a previously determined
rotational inertia about the = axis. The resultant torque of
position,and advancing that position based upon known or
the motors, /).r can be calculated from,
estimated speeds over elapsed time and course. It is based
/).r=r]-r2+r3-r4 (12) on UAV's heading,speed and flight path angle,and more to
The yaw axis dynamics can be rewritten in the state-space calculate the current position of an aircraft/UAV [5, 6].
form as, A disadvantage of dead reckoning is that since new
positions are calculated solely from previous positions, the
errors of the process are cumulative, so the error in the
position grows with time.
The formula of calculating position is as following:

X= Xo + r (V COS%COS r)dt
Y= Yo + r (V COS%sin r)dt
III. NAVIGA nON AND TRAJECTORY TRACKING CONTROL
(14)

z= Zo + r (V sin X)dt
A. Control Design
In this paper an inner/outer loop control structure are
designed and implemented. Inner loop is designed to
control the attitude of the system,outer loop is designed to
control to the location of the system (navigation) in X=fJ+1f!
and
accordance with pre-planning flight paths. Fig. 3 shows the y=a+()
attitude and navigation control block diagram.
where X is tracking azimuth, ris flight path angle,fJ is
If/c sideslip angle,If/" is yaw angle,a is angle of attack,B is
pitch angle. In this paper, a and fJ are small. They are
neglected,therefore X= If! and y=() .
As seen from the above equations, the DR system
calculates the location of the aircraft will deviate from the
actual flight path fast when the heading accuracy of the
information is poor. Therefore,DR system requests for the
Figure 3. Attitude and navigation control block diagram higher accuracy information.
Flight path is usually composed by a number of
In Fig. 3, If/c is commanded yaw attitude, If/g is the waypoints. Navigation computer stores the geographical
coordinates of these waypoints based on the planned flight
given yaw attitude, g is the given roll attitude, and the path,and orderly gives the track line of the next waypoint in

Pc = (xc,Yc,zJT is the commanded position. The

accordance with the location of the aircraft.
vector During lateral track control,the lateral deviation from the
control scheme consists of inner attitude stabilization loop planning line I1Z
is the main control signals. A definition
and outer trajectory tracking loop controller. A PID of D.Z for the left-deviation is positive; the right-deviation
(Proportional-Integral-Derivative) controller is used in is negative. The calculation can use the method of analytic
inner/outer controller. A PID controller calculates an geometry. Pre-planning waypoints are A( xI'YI) and
"error" value as the difference between a measured process
variable and a desired set point. The controller attempts to , x
B(x2,yJ from trajectory tracking path P( o,Y o ) is the
minimize the error by adjusting the process control inputs. position of Qball-X4. The distance formula of point P to the
The PID parameters used in the calculation must be tuned trajectory tracking path, II1ZI can be calculated as:
according to the nature of the system. A proportional
controller (Kp) has the effect of reducing the rise time and llJ 1(Y2 -YI)XO -(X2 -x1)Yo +YIX2 -Y2XI I
(15)
will reduce but never eliminate the steady-state error. An I =
2
integral control (K;) has the effect of eliminating the steady (y2 _ YI) +(X2 -XY
state error,but it may make the transient response worse. A /).Z direction can be determined by vector determination
derivative control (Kd) has the effect of increasing the

121
method. In R3 space, using the North-East-Ground
coordinate system,constructed three vectors,
AP = {xo -xPYo -YpO}.
AB={x2 -xpYz YpO}. -
k={O,O,l}
f=(AP AB) e k=(-'1> -oXi)(Yz -Y,)- (-X2 -oXi)(Yo -Y,)
The following conclusions are given:
If/>O,point P is located in planning routes left, I1Z > ;
If/=O,point P is located in planning routes, I1Z = 0;
If/<O,point P is located in planning routes right, I1Z < 0.
C. Waypoints Switch
When an UAV pre-planned path is composed of several
waypoints and when UAV almost arrives at routine point of
this segment,there will inevitably exist a problem when and
how to fly to the next track segment. There are two
switching control methods to choose,as shown in Fig. 4.
c X(k+11k)
A
Figure 4. Waypoints switch mode diagram
1) UAV does not fly waypoint of this segment when
UAV flies near the waypoint. It completes the route
waypoint of the conversion,and gives the next fly waypoint
(or target point) control signal automatically;
2) UAV flies off points after the UAV arrives at the end
of this segment, then UAV converts automatically to the
waypoint (or target point) of next route.
Obviously the second approach cannot meet the two flight
path angles changing in most cases. The control process
will produce a large overshoot when angle is large.
D. Discrete Kalman Filter
In 1960, R. E. Kalman published his famous paper (7]
describing a recursive solution to the discrete-data linear
filtering problem. Since that time, due in large part to
advances in digital computation,the Kalman filter has been
the subject of extensive researches and applications,
particularly in the area of autonomous or assisted
navigation. A very "friendly" introduction to the general
idea of the Kalman filter can be found in Chapter 1 of [8],
while a more complete introductory discussion can be found
122
in [9], which also contains some interesting historical
narrative. More extensive references can be found in [10,
11, 13, 14]. The Kalman filter is a set of mathematical
equations that provides an efficient computational
(recursive) means to estimate the state of a process, in a
way that minimizes the mean of the squared error. The filter
is very powerful in several aspects: it supports estimations
of past,present,and even future states and it can do so even
when the precise nature of the modeled system is unknown.
The Kalman filter is the filtering method that is based on
an estimate of the last state and the current state of
measurement launch an estimate of the current state. It is to
estimate using the state equation and recursive methods,
thus Kalman filter does not require the signals processing
being stationary or time-invariant. Kalman filter is an
"optimal recursive data processing algorithm [12].
The Kalman filter estimates a process by using a form of
feedback control: the filter estimates the process state at
some time and then obtains feedback in the form of noisy
measurements. As such, the equations of the Kalman filter
fall into two groups: time-update equations and
measurement-update equations. The time-update equations
are responsible for projecting forward (in time) the current
state and error covariance estimates to obtain the a priori
estimates for the next time step. The measurement-update
equations are responsible for the feedback. The time-update
equations can also be thought of as predictor equations,
while the measurement-update equations can be thought of
as corrector equations. A complete picture of the operation
of the discrete-time Kalman filter is showed in Fig. 5.
= FX(k)
P(k+1/ k) = FP(k)FT +Q
K(k+1) = P(k+1/ k)HT[HP(k+11 k)HT + Rf'
X(k+1) = X(k+1/ k)+ K(k+1)[Z(k+1)- HX(k+1/ k)]
P(k+1) = [I - K(k+ I)H]P(k+1/ k)
Figure 5. A schematic of operation of the Kalman filter
In this paper the sampling time T is chosen as 0.02
second, assuming the Qball-X4 to do the uniform linear
motion. A linear dynamical system is a partially observed
stochastic process with linear dynamics and linear
observations, both subject to Gaussian white noise. It can
be defined as follows,where X(k) is the state at time t=kT,
and =(k) is the observation.
X(k) = FX(k-1)+ w(k-1)
(16)
Z(k) = HX(k)+v(k)
where WE N(O,Q),X(O)E N(X(O),v(O andVE N(O,R ).
In practice, H. Q and R matrices might change with each
time step or measurement. However here we assume that
they are constant. So a partial moving can be considered in
the plane at constant velocity subject to random
perturbations in its trajectory. The new position (xe, Xn) is

Xve'C(kk)) 1 1 1 veC(kk---l1l))
. _._I
. te:vo:ll:

the old position plus the velocity (ve' vn) multiplies sample -_1'ft-11.2
-21.1
-ll.'

w.
--tnoq
time plus noise The discrete-time function can be written

x[vnnCCkk))1 [ 1Wxvnn,CCkk-l))1
as:
T 0 0
I I I I I
0 0 o w
------ ------------1----

_ v, II II II II II
+ (17) ----- -4----4----1-- -1----
0 0 T 0 I I I I I

0 0 0 W VII -,
I
---------d

x eCk)
where WVe and WVn are zero mean Gaussian noises of the
system. Figure 6. Tracking curve by dead-reckoning without Gaussian
Assuming only the position of the Qball-X4 is observed, white noise when flying speed is 75m/s

following function can be obtained:

[XCk)]=[l
yCk)
vllCk)
l ::i: +[::l
o o -,., 'o:.a I bIo:<'
_.'I'-bIoS
(18) -J."'_ltJI.l

0 o --------- -+-
\ ---------
-P.MIh;_t

: :
1 -- --
- - - -1- - - - .., -/- : \ :r
-''\1 :
- - - -1- - --
T

fZB ____:____ l ___ 1_ - - i ____:___ _

where Vx and Vy are zero mean Gaussian noises of the 1. : I : 1\, :

------ 1----T---- ------

I
observations in x andy direction respectively. --- --
I

:
7 I I
-1- - - - + - - - - .. :
- - --- I

---

S - - -
I

1
__ ____ .:. ____ _ _:____
,1 I I I
I
I
IV. NUMERICAL SIMULATION
tt --. :
I
-- r- -
I I
---

In order to verify the effectiveness of the navigation and

waypoints tracking scheme proposed in this paper,
numerical simulation is performed on Matlab/Simulink Figure 7. Tracking curve by dead-reckoning without Gaussian
environment. The Qball-X4 model used in the simulations white noise when flying speed is 100m/s
is a common kind of small-size helicopter with autopilot
B. Results Analysis using Kalman Filter
installed. The simulation can be considered in a 3D space
representation. In this paper a 20 space representation is Fig. 8 shows Qball-X4 tracking response with Gaussian
presented since it can facilitate the visualization of the white noise is added into X and Y position,respectively.
Qball-X4 trajectories in a clearer manner when altitude is
chosen at a constant.
In this section,pre-planned three waypoints are A (0,0),B
(4000,2000),C (0,4000),where A is the first waypoint,B
is the second waypoint and C is the third waypoint. Starting
point is at (0, -1000). Unit is meter and sampling time is
0.02 second. I I I I I I
___ I ___ I ___ I ___ I ___ I _ __ ___
L L

___ I ___ I __ _ I ___ I ___ I __ _ II ___

I I I I I
t
A. Results Analysis using Dead Reckoning
L J L

Fig. 6 and Fig. 7 show that Qball-X4 tracks the path that is I I I I I I
formed by three pre-planned waypoints in different flying
speeds. The flying speed is 75m/s in Fig. 6. The flying
speed in Fig. 7 is lOOmis. As can be seen, the model
Figure 8. Tracking curve with Kalman filter
follows the desired path curve with no error under the lower
In order to facilitate analysis of graphics, part of the
speed except near the waypoints. The model follows the
enlarged view of the tracking curve is shown in Fig. 9.
desired path curve with some error under the higher speed,
especially near the waypoints. Qball-X4 needs more time to
adjust waypoint switch following the flying speed.

123
 trajectory tracking and the desired path; there is a smaller
, position error between estimated trajectory for tracking
1i'ot.: ,
- -- --r---
control and the desired path.

V. CONCLUSION

Dynamic equations of a newly developed quadrotor UAV

, test-bed (Qball-X4) has been developed in this paper based
....S2
.J -:;_;:::T __ ---!--- on analysis X!Y position, altitude, pitch/roll and yaw
,
___

it!-:i - - - +- - - + - - - --- --- models. A navigation and trajectory tracking control design
, I

'"
I I I

..
I
approach based on an inner/outer-loop control structure is
then proposed in this paper. The inner loop control
Figure 9. Part of enlarged view of curve with Kalman filter
stabilizes the attitude of UAV; the outer loop control
enables UAV navigation in accordance with pre-planned
In Fig. 8 and Fig. 9, blue dash line is the measured
flight paths. Dead-reckoning and discrete Kalman filter
trajectory disturbed with Gaussian white noise; black solid
have been applied to the navigation and trajectory tracking
line is the estimated trajectory with Kalman filter; red dot
control. The effective switching of different waypoint
line is the real trajectory of Qball-X4; green solid line is the
ensures the validity of simulation results.
desired path. Fig. 9 shows that the estimated trajectory with
Kalman filter tracks the desired trajectory of Qball-X4 well,
REFERENCES
reducing and smoothing the impact of noise on the system.
[I] E. AItug, J. P. Ostrowski, R. Mahony, "Control of a Quadrotor
In order to more clearly explain the simulation results,
Helicopter Using Visual Feedback, " In Proceedings of the IEEE
trajectory tracking error is shown in Fig. 10. Part of the International Conference, pp. 72-7, 2002.
enlarged view is shown in Fig. I I. [2] P. Pounds, R. Mahony, J. Gresham, P. Corke, and L. Roberts,
'Towards Dynamically- Favourable Quad-Rotor Aerial Robots, " In
Proceedings of the 2004 Australasian Conference on Robotics and
I I
(" ('"
I
("
I
- .-
I
-
- - - - - - r - - - - - - - - - - - - - - - - - - - - - - - - - - -
Automation,2004.
[3] P. McKerrow, "Modeling the Draganflyer Four-rotor Helicopter, " In
Proceedings of the 2004 IEEE international Conference on
Robotics and Automation, 2004.
[4] "Quanser Qball-X4, User Manual," Quanser Inc., Toronto, Canada,
2010.
[5] I. Kamal, (2008, April (5), WFR, A Dead Reckoning Robot-A
Practical Application to Understand the Theory, [Online].
Available: http://www.ikalogic.com.
[6] Y. H. Qu, and F. Liang, "A Method of Wind Field Estimation and
Trajectory Correction Online in DRlGPS/RP Integrated Navigation
of UAV, " in Proceedings of the IEEE International Conference on
Automation and Logistics, pp. 1402-1407, 2008.
Figure 10. Trajectory tracking error with Kalman filter
[7] R. E. Kalman, "A New Approach to Linear Filtering and Prediction
Problems, " Transaction of the ASME-Journal of Basic Engineering,
pp. 35-45, March 1960.
[8] P. S. Maybeck, "Stochastic Models, Estimation, and Control,"
Academic Press, Inc., vol. I, 1979.
[9] H. W. Sorenson, "Least-Squares Estimation: From Gauss to Kalman, "
IEEE Spectrum, vol. 7, pp. 63-88, July 1970.
[10] A. Gelb, "Applied Optimal Estimation," MIT Press, Cambridge,
MA, 1974.
[II] M. S. Grewal, and P. A. Angus, "Kalman Filtering Theory and
Practice," Upper Saddle River, NJ USA, Prentice Hall, 1993.
[12] G., WeIch, and G. Bishop, "An Introduction to the Kalman Filter, "
UNC-Chapel Hill, TR 95-041, July 24, 2006.
[13] O. L. R. Jacobs, Introduction to Control Theory, 2nd ed., Oxford
University Press, 1993.
[14] R. G. Brown, and P. Y. C. Hwang, "Introduction to Random Signals
and Applied Kalman Filtering," 2nd ed., John Wiley and Sons Inc.

Figure 1 1. Part of enlarged view of error with Kalman filter

In Fig. 10 and Fig. 11, blue line represents the position

error with added Gaussian white noise; black line is the
position error with Kalman filter. As can be seen from Fig.
11, there is large position error between the measured

124