Matlab Ufford Adam Thesis

Development and Implementation of Guidance, Navigation and Control Systems
for an Autonomous Air Vehicle

by
Adam Ufford, B.S.M.E.
A Thesis
In
MECHANICAL ENGINEERING
Submitted to the Graduate Faculty
of Texas Tech University in
Partial Fulfillment of the Requirements for the Degree of
MASTER OF SCIENCE
IN
MECHANICAL ENGINEERING
Approved
Dr. Jordan M. Berg
Committee Chair
Dr. Walt Oler
Dr. Seon Han
Fred Hartmeister
Dean of the Graduate School
August, 2009
Texas Tech University, Adam Ufford, August 2009
Contents
List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1
Introduction . . . . . . . . . . . . . . . . . . . . . . . . . .
2
Coordinate Frames and Nomenclature Preliminaries . . . .
2.1 Navigational Coordinate Frames . . . . . . . . . . . .
2.1.1 Earth Centered Earth Fixed Geoidal System . . .
2.1.2 Earth Centered Earth Fixed Rectangular System
2.1.3 Local Tangent Plane - enu Coordinate System . .
2.2 Aircraft Coordinate Frames . . . . . . . . . . . . . . .
2.2.1 Earth (Inertial) Axes . . . . . . . . . . . . . . . .
2.2.2 Vehicle Navigation Axes . . . . . . . . . . . . . .
2.2.3 Body Axes . . . . . . . . . . . . . . . . . . . . . .
2.2.4 Wind Axes . . . . . . . . . . . . . . . . . . . . . .
2.2.5 Path Axes . . . . . . . . . . . . . . . . . . . . . .
2.3 Nomenclature . . . . . . . . . . . . . . . . . . . . . .
3
Simulation Model . . . . . . . . . . . . . . . . . . . . . . .
3.1 State Equations . . . . . . . . . . . . . . . . . . . . .
3.2 Aerodynamic Forces and Moments . . . . . . . . . . .
3.2.1 Aerodynamic and Thrust Forces . . . . . . . . . .
3.2.2 Aerodynamic Moments . . . . . . . . . . . . . . .
3.2.3 Aerodynamic Coefficients . . . . . . . . . . . . . .
3.3 Inner Loop: Attitude Stabilization . . . . . . . . . . .
3.4 Outputs . . . . . . . . . . . . . . . . . . . . . . . . .
4
Attitude Stabilization System . . . . . . . . . . . . . . . . .
4.1 Infrared Horizon Sensing . . . . . . . . . . . . . . . .
4.2 FMA FS-8 Copilot . . . . . . . . . . . . . . . . . . .
4.3 Arbitrary Pitch and Bank Angle Stabilization . . . .
4.3.1 Calibration . . . . . . . . . . . . . . . . . . . . .
4.3.2 Stabilization . . . . . . . . . . . . . . . . . . . . .
5
Path Following Subsystem . . . . . . . . . . . . . . . . . .
ii
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
iv
v
1
4
4
4
5
6
6
7
7
8
8
9
9
11
12
12
13
14
14
14
15
17
18
19
19
19
20
23
5.1
Vector Field Path Following . . . . . . . . . . . . . . . . .

5.1.1 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . .
6
Autopilot Subsystem . . . . . . . . . . . . . . . . . . . . . . . .
6.1 Feedback Linearization Autopilot Algorithms . . . . . . . .
6.1.1 Ground Track Heading Hold . . . . . . . . . . . . . . .
6.1.2 Altitude Hold . . . . . . . . . . . . . . . . . . . . . . .
6.1.3 Existence of Solutions and Corresponding Control Laws
6.1.4 State Measurement and Estimation . . . . . . . . . . .
6.1.5 High-Gain Observer for Flight Path Angle . . . . . . .
6.2 Output Feedback Simulations . . . . . . . . . . . . . . . .
6.3 Alternative Autopilot Design and Comparison . . . . . . .
6.3.1 Altitude Hold: PI Controller . . . . . . . . . . . . . . .
6.3.2 Ground Track Heading Hold: P Controller . . . . . . .
6.3.3 Simulation Results . . . . . . . . . . . . . . . . . . . .
7
GNC System Implementation . . . . . . . . . . . . . . . . . . . .
7.1 GNC System Hardware . . . . . . . . . . . . . . . . . . . .
7.2 Radio Modem . . . . . . . . . . . . . . . . . . . . . . . . .
7.3 Remote Pilot Override . . . . . . . . . . . . . . . . . . . .
7.4 Hardware in the Loop Simulations . . . . . . . . . . . . . .
8
Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
A
Trajectory Model . . . . . . . . . . . . . . . . . . . . . . . . . .
B
Simulation Code . . . . . . . . . . . . . . . . . . . . . . . . . . .
C
C Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
iii
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
24
24
27
27
27
28
29
30
31
33
33
35
35
35
40
40
41
42
43
45
46
48
52
73
List of Tables
2.1
2.2
WGS-84 Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . .
Model Nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . .
5
10
5.1
Path Following Variables . . . . . . . . . . . . . . . . . . . . . . . . .
25
7.1
Telemetry Sentence Contents . . . . . . . . . . . . . . . . . . . . . . .
43
iv
List of Figures
1.1
1.2
Autonomous GNC System . . . . . . . . . . . . . . . . . . . . . . . .

Subsystem Partitioning . . . . . . . . . . . . . . . . . . . . . . . . . .
2
2
2.1
2.2
Earth Geoid Parameters . . . . . . . . . . . . . . . . . . . . . . . . .

Aircraft Coordinate Frames . . . . . . . . . . . . . . . . . . . . . . .
5
7
3.1
3.2
PID Controller: Roll to Aileron . . . . . . . . . . . . . . . . . . . . .

Simulation Results: PID Inner Loops . . . . . . . . . . . . . . . . . .
15
15
4.1
4.2
4.3
4.4
Infrared Horizon Sensing: Single Axis . . .

Copilot System: Single Axis . . . . . . . .
Voltage-Inclination Curve, Vmax =3.0 Volts
Modified Co-pilot System: Single Axis . .
.
.
.
.
19
20
21
21
5.1
5.2
Linear Path Segment Examples . . . . . . . . . . . . . . . . . . . . .

Waypoint Switching Look-Ahead, s . . . . . . . . . . . . . . . . . . .
23
26
6.1
6.2
6.3
6.4
6.5
6.6
6.7
6.8
6.9
Two Stage Estimation Scheme . . . . . . . . . . . . . . . . . . . . .

Angle of Attack Estimation: . . . . . . . . . . . . . . . . . . . . . .
High Gain Observer: = 10, h = 20 . . . . . . . . . . . . . . . .
Output Feedback Controller: a =0.25, k1 =0.18, k2 =0.7 . . . . . . .
Proportional Ground Track Heading Hold Autopilot, 15o Step Input
Proportional Ground Track Heading Hold Autopilot, P = 1.5 . . .
Proportional Ground Track Heading Hold Autopilot, P = 0.25 . .
Altitude Hold: Tuned for 3 Second Rise Time . . . . . . . . . . . .
Altitude Hold: (t) = sin t . . . . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
31
32
34
34
36
37
37
38
38
7.1
7.2
7.3
7.4
Hardware Prototype: P,PI Autopilot . . . . . .

DAC/Op-Amp Implementation: Single Channel
Telemetry Sentence Structure . . . . . . . . . .
Hardware in the Loop Simulation Results . . . .
.
.
.
.
41
42
42
44
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
Chapter 1
Introduction
Interest in unmanned and autonomous aerial vehicles in the Mechanical Engineering department at Texas Tech University began in 2006 with a team of undergraduate mechanical engineering students who constructed and flew an RC-sized electric,
flying wing aircraft via a standard RC remote pilot link. The following year, two
undergraduate mechanical engineering teams were tasked with implementing video
payload, autonomous navigation, and telemetry systems in hopes of entering an international university competition for undergraduate students. The system was desired
to autonomously follow a three dimensional path specified by a 2-D waypoints and a
desired altitude. The undergraduate teams purchased hardware and began algorithm
development with a simplified model. The scope of the project, however, proved too
broad for such a short term assignment, and the project ended with few deliverables
complete and limited success. This thesis details the development and implementation of the guidance, navigation, and control system components for an autonomous
system which satisfies the autonomous navigation requirements.
Guidance, Navigation and Control (GNC) system design can be naturally divided
(both conceptually and mathematically) in to several tractable sub-problems, the
solutions of which act together in a larger, nested structure. The output of the
higher level functions provides the input for the lower stages. A good example of this
layered architecture and its benefits are given in [9]. A autonomous GNC system
in this thesis is defined to be the system of systems which enables the vehicle to
follow an externally specified path and maintain altitude. This task is summarized
graphically in Figure 1.1. The available controls (A , E , R ) are the aileron, elevator
and rudder control surface deflections, respectively. In general, the GNC system
mission is to drive these controls in order to track the desired altitude and path.
Methods of autonomous path path planning, along with other higher-level functions
such as automatic target selection and cooperative formations are not considered
here, although [9] describes how the nested design structure is expandable for such
Figure 1.1 Autonomous GNC System

functions. We choose to partition the autonomous GNC system design into three
smaller systems as shown in Figure 1.2.
Figure 1.2 Subsystem Partitioning

Given a group of ordered waypoints and the current position of the aircraft, the
Path Following Subsystem generates a desired ground track heading angle which
will drive the aircraft to the desired path. This desired ground track and a desired
altitude are inputs to the Autopilot Subsystem which generates pitch and bank angle
commands. The bank and pitch angle commands are in turn fed into the Attitude
Stabilization Subsystem which drives the aircraft control surfaces to track the bank
and pitch angle commands. Each level of the system is designed assuming perfect
performance (either instantaneous actuation or first order error dynamics) of the
other levels.
The necessary definitions and additional nomenclature for discussion of the aircraft dynamics are covered in Chapter 2. The nonlinear, 6-DOF aircraft model
developed and used for simulation is detailed in Chapter 3 to illustrate the complexities inherent in control system design and motivate the following chapters. The
selected solution for the Path Following Subsystem is described in Chapter 4, while
the selected design for the Attitude Stabilization Subsystem is described in Chapter
2
5. The main theoretical contribution of this thesis is given in Chapter 6, where original algorithms for the ground-track heading and altitude hold autopilots are derived
and compared via simulation to more traditional autopilot designs. The hardware
and software implementations of the selected algorithms are described in Chapter 7.
A brief conclusion and suggestions for future work are included in Chapter 8.
Chapter 2
Coordinate Frames and
Nomenclature Preliminaries
An accurate discussion of aircraft navigation and dynamics involves several different coordinate systems and many variables. The coordinate systems and symbols
used throughout this thesis are defined in this chapter.
2.1
Navigational Coordinate Frames
The primary position information information is provided by GPS. Since the

source of this information consists of an array of satellites which orbit the earth, the
data is reported with respect to a Earth centered, non rectangular coordinate system.
This section defines that coordinate system and its relation to the local rectangular
system used for navigation. For more information on the navigational coordinate
frames, see [5, 4].
2.1.1
Earth Centered Earth Fixed Geoidal System
The three dimensional position data is reported by the GPS receiver in an Earth
Centered Earth Fixed, Geodetic (ECEF-g) coordinate system as latitude, longitude
and altitude above Mean Sea Level (MSL) (, , hM SL ) values. Application of
the ECEF-g requires a model for the Earths ellipsoid. The model used in GPS
measurements is the WGS-84 model, and its experimentally determined parameters
are listed in Table 2.1. The other quantities necessary to transform the ECEF-g
coordinates into the other coordinate systems can then be calculated. The ellipsoid
flatness, f , and eccentricity, e, are expressed in terms of the axis lengths.
f=
ab
a
= 3.3528107 103
e=
f (2 f )
4
(2.1)
Table 2.1 WGS-84 Parameters

Symbol
Quantity
Value
Units
a
Earth Semimajor Axis 6378137.0
m
b
Earth Semiminor Axis 6356752.3142 m
The normal distance, N , is the distance from the ellipsoid surface as shown in Figure
2.1 to the ellipsoid z-axis and is calculated as a function of latitude.
Figure 2.1 Earth Geoid Parameters
N () = q
2.1.2
a
1 e2 sin2 ()
(2.2)
Earth Centered Earth Fixed Rectangular System
A useful intermediate frame is the Earth Centered Earth Fixed, Rectangular

(ECEF-r) system. This system is an orthogonal right-handed (ORH) axes system
whose origin sits at the Earth center. The ECEF-r z-axis points up through the North
pole, with the x-axis perpendicular to z and passing through the Prime Meridian and
SL
the y-axis directed to complete the ORH system. For a point P = (P , P , hM
),
P
the ECEF-r coordinates of P are given by

SL
xP = hM
+ N (P ) cos P cos P
P
SL
yP = hM
+ N (P ) cos P sin P
P
SL
zP = hM
+ (1 e2 ) N (P ) sin P
P
(2.3)
2.1.3
Local Tangent Plane - enu Coordinate System
The Local Tangent Plane (LTP) is a plane parallel to the ellipsoid tangent at an
arbitrary reference point, x0 . The enu Coordinate System is centered at this point,
which is usually taken to be the origin of the flight. The 1-axis, denoted by e, points
in the true East direction, and the 2-axis, denoted by n, points in the true North
direction. The e n plane lies in the local tangent plane and the 3-axis, denoted by
u, points straight up, perpendicular to the LTP. The origin of the enu Coordinate
System can be expressed in ECEF-r coordinates.
xo =
x0
y0
z0
(2.4)
If the instantaneous position of the aircraft is reported as P = (P , P , hgP ), the

ECEF-r coordinates are given by

SL
+ N (P ) cos P cos P
xP = hM
P
SL
yP = hM
+ N (P ) cos P sin P
P
zP =
SL
hM
P
(2.5)
+ (1 e ) N (P ) sin P
and the point can be expressed in enu coordinates via one translation and two
rotations.
The coordinates of P in the local tangent plane (eP ,nP ,uP ) can then be given by
one translation and two rotations:
2.2
eP
nP
uP
1
0
=
0 sin P
0 cos P
0
sin P
cos P
cos P
sin P
0
cos P
sin P
0
0 xP x0
0
y y0
P
1
zP z0
(2.6)
Aircraft Coordinate Frames
Traditionally, a series of right-handed coordinate systems, all centered at a point

on the aircraft, are defined in various directions to ease the derivation of aircraft
equations of motion. For instance, the position of the aircrafts center of mass is
generally stated with respect to the origin of some inertial axes, the aerodynamic
forces are most easily described using the so-called wind axes, and on-board mea6
surements are easily referenced to the body axes directions. This section details the
pertinent coordinate axes systems for the analysis in the following chapters. For a
given frame, Q, we denote unit vectors in the directions of the xq , yq , and zq axes
as iq , jq , and kq , respectively. The five aircraft axes systems defined in this chapter
and used throughout this thesis are depicted graphically, along with the translations
or rotations that separate them, in Figure 2.2. For a more detailed derivation and
explanation of the aircraft coordinate systems, see [12, 3].
Figure 2.2 Aircraft Coordinate Frames
2.2.1
Earth (Inertial) Axes
The Earth axes frame, denoted E, is formed by pure rotation from the LTP, in
order to invert the z-axis. The unit vectors for this system are denoted (ie , je , ke )
and point in the true North, true East, and down directions, respectively. The Earth
frame is assumed to be an inertial frame. Consequently, the Earth axes frame will
often be referred to as simply the inertial frame.
2.2.2
Vehicle Navigation Axes
The Vehicle Navigation Frame is an orthogonal, right-handed (ORH) coordinate

system whose origin is located at the aircraft center of gravity. This frame is not
an inertial frame, since the origin translates with the aircraft center of gravity. The
7
coordinate axes of the vehicle navigation frame, however, remain aligned with the
inertial ie , je and ke directions, regardless of the vehicles orientation. The vehicle
navigation frame will also be referred to as simply the navigation frame.
2.2.3
Body Axes
The body frame is again an ORH system whose origin is located at the aircraft
c.g. Its coordinate axes, however, are defined as follows. The body frame 1-axis, xb ,
points directly out the nose of the aircraft. The 2-axis, yb , points out the right wing,
and the 3-axis, zb , completes the right-handed, orthogonal system by pointing out
the belly of the aircraft. As the body rotates, so do the body frame coordinate axes
- the xb zb plane remains in the plane of symmetry of the airframe.
The body frame axes can be obtained from the navigation frame axes by a 3-2-1,
body-fixed rotation which defines the Body Euler Angles, , , and .

ie
ie
ib
cc
sc
s

cc + sss cs
je [Re2b ] je (2.7)
jb = sc + css
ke
ke
kb
ss + csc cs + ssc cc
Inside the rotation matrix, s denotes sin () and c denotes cos (). The former
compact notation will be used throughout the remaining pages unless the latter is
necessary for clarity. Given a vector expressed in Y coordinates, the rotation matrix
that transforms this vector into X axes coordinates will be written as Ry2x , as seen
in Equation 2.7.
2.2.4
Wind Axes
The wind frame is an ORH system whose origin is again located at the aircraft
c.g. The xw axis points in the direction of the relative velocity vector with respect
to the surrounding airmass. This relative velocity vector is denoted as Vcm/w . The
wind axes are obtained from the body axes by two body-fixed rotations. The first
is a positive 3-rotation by , and the second is a negative 2-rotation by . This
transformation is represented mathematically by the matrix operation below.

iw cc
= cs
j
w
s
kw
2.2.5
s
c
0

ib
ib
sc

ss
jb [Rb2w ] jb
kb
kb
c
(2.8)
Path Axes
The path axes frame is an ORH system whose origin is located at the aircrafts
c.g. The xp axis points in the direction of the aircrafts inertial velocity vector, Vcm/e .
The path axes can be obtained from the inertial axes by two successive body-fixed
rotations. The first is a 3-rotation by the instantaneous flight path heading angle, ,
and the second is a 2-rotation by the instantaneous flight path angle, . These two
rotations are expressed mathematically by the following rotation matrix.
ip cc
jp = s
kp
cs
sc
c
ss
s
n
0 e [Re2p ] e
d
d
c
(2.9)
The path axes frame is useful in describing the inertial velocity of the vehicle,
while the wind axes frame allows for easy quantification of the vehicles relative
velocity (with respect to the airmass). The velocity vector of the airmass with
respect to inertial is denoted Vw/e and is nonzero, in general. The following general
vector equation is a consequence of the definitions so far in this chapter.
Vcm/e = Vcm/w + Vw/e
2.3
(2.10)
Nomenclature
The 6-DOF simulation model, detailed in the following chapter, contains all the
quantities relevant to a discussion of aircraft dynamics. These quantities and their
symbols are summarized in Table 2.2. The same symbols are used in the discussion
of all algorithms and the trajectory model in Appendix A.
Table 2.2 Model Nomenclature

Symbol
Quantity
X
Aircraft COM Position Component (meters North)
Y
Aircraft COM Position Component (meters East)
Z
Aircraft COM Position Component (meters Down)
Roll Angle (radians)
Pitch Angle (radians)
Yaw Angle (radians)

P
Angular Rate about body x-axis (rad/s)
Q
Angular Rate about body y-axis (rad/s)
R
Angular Rate about body z-axis (rad/s)
U
Inertial Velocity along body x-axis (m/s)
V
Inertial Velocity along body y-axis (m/s)
W
Inertial Velocity along body z-axis (m/s)
Wx
Wind Component in North Direction (m/s)
Wy
Wind Component in East Direction (m/s)
Wz
Wind Component in Down Direction (m/s)
U0
Relative Velocity along body x-axis (m/s)
V0
Relative Velocity along body y-axis (m/s)
0
W
Relative Velocity along body z-axis (m/s)
VT
Magnitude of Relative Velocity (m/s)
Vin
Magnitude of Inertial Velocity (m/s)
Vertical Flight Path Angle (radians)
Horizontal Flight Path Angle (radians)
Angle of Attack (radians)
Sideslip Angle (radians)

T
Thrust Force (Newtons)
a
Aileron Deflection (radians)
e
Elevator Deflection (radians)
r
Rudder Deflection (radians)
Latitude
Longitude
hM SL
Altitude above Mean Sea Level
10
Comments
State
State
State
State
State
State
State
State
State
State
State
State
State
State
State
Output
Output
Output
Output
Output
Output
Output
Output
Output
Input
Input
Input
Input
GPS output
GPS output
GPS output
Chapter 3
Simulation Model
This chapter details the simulation model used to test and validate the GNC
algorithms derived in later chapters. The details of the model development are
included to better qualify the simulation results given in the following chapters and
to introduce the aircraft nomenclature in a more complete fashion. It is also hoped
that this chapter will serve as a brief tutorial for future users of the simulation model
code.
For simulations, a flat-earth, nonlinear 6-DOF aircraft model is used. This model
was implemented in MATLAB following [12] and includes the often neglected terms
for wind disturbances, sideslip and sideforce. For simplicity, this model uses the
standard 3-2-1, yaw-pitch-roll Euler angle sequence for vehicle attitude representation. It is well known that the kinematic differential equation for such an attitude
parameterization exhibits a singularity at some points. The singularity for a 3-2-1
sequence occurs as the pitch angle, , approaches 2 , as can be seen from Equation
3.2. However, vehicle attitudes in which the nose points straight up or straight down
are outside the space of desired attitudes for most path following problems. Large
positive pitch angles can have the tendency to stall the aircraft. Also, large angles
in pitch and roll can cause problems for the selected attitude measurement and stabilization system[14]. For these reasons, the commanded pitch and bank angles will
be confined to a reasonable range well within the limits imposed by this singularity.
The three Euler angles for the body axes, the position of the aircraft in a Local
Tangent Plane, the body rates, and the inertial body velocities form the 12 basic
states in 6-DOF state-space model. Also included as (unchanging) states are the
wind velocity components in the inertial axis directions (WN , WE , and WD ). The
inputs to the simulation model are a thrust force, T, and control surface deflections
for the ailerons, elevator, and rudder (a , e , and r , respectively). Other quantities
of interest (outputs) are calculated as function of the states after integration. These
include the relative body velocities with respect to the airmass (U 0 , V 0 , and W 0 ),
11
aerodynamic angles ( and ), and flight path angles ( and ). The nomenclature
used in the model and within the remainder of this paper is summarized in Table
2.2.
3.1
State Equations
The state equations for the 6-DOF model are as follows.

cc
sc
s 1 U
X

Y = sc + css
cc + sss cs
V

W
ss + csc cs + ssc cc
Z
(3.1)
1 tan sin tan cos

P

= 0
cos
sin Q

sin
cos
0
R
cos
cos
(3.2)
2
Jxz (Jx Jy +Jz )
P Q Jz (Jz Jy )+Jxz QR + Jz l + Jxz n
x)
Q = (JzJJ
P R JJxzy (P 2 R2 ) + J1y m
y
2
(Jx Jy )Jx +Jxz
y +Jz )
P Q Jxz (Jx J
QR + Jxz l + Jx n
(3.3)
P =
R =
U
T
D
0
U
i
h
1
= (1/m) 0 + [Rb2w ] C + [Re2b ] 0 B/E V

W
0
L
g
(3.4)
The cross-product matrix, B/E , corresponds to the body fixed angular velocity
vector, B/E = [P Q R]T , and is shown below.
3.2
B/E
0
R Q
= R
0 P
Q P
0
(3.5)
Aerodynamic Forces and Moments
Given initial conditions on the state, x, the aerodynamic forces and moments
must be calculated to be inserted into the right hand side of the state equations.
x = [X, Y, Z, , , , P, Q, R, U, V, W ]T
12
(3.6)
3.2.1
Aerodynamic and Thrust Forces
The aerodynamic forces are, by definition, applied in the wind frame and, consequently, are naturally written in wind axes coordinates:
w
b
FA =
C = [Rb2w ] FA
L
(3.7)
The body axes components are given by
b
1
FA = [Rb2w ] C
(3.8)
The aerodynamic force components are modeled via the component build-up method
[12], with any dependence on neglected. The lift force is approximated as
CL = CL0
L = qSCL
+ CL + CLe e +
c
2VT
CLQ Q
(3.9)
and the drag force as

D = qSCD
CD = CD0 +
(CL CL0 )2
ARe
+ CDe e + CDa a + CDr r
(3.10)
in which the first two terms will typically dominate. A simple model for the sideforce,
C is given by
C = qSCY
CY = CY + CYr r + CYa a +
b
2VT
[CYP P + CYR R]
(3.11)
The thrust force, T , is directed along the longitudinal body axis, xb .

T = Tib
13
(3.12)
3.2.2
Aerodynamic Moments
The aerodynamic moments are modeled similarly, using the component build-up
method.
l
bCl
(3.13)
MbA [Rb2w ]1
m = [Rb2w ] qS cCm
bCn
n
Rolling Moment
The rolling moment coefficient is modeled as
Cl = Cl + Cla a + Clr r +
b
[ClP P + ClR R]
2VT
(3.14)
Pitching Moment
The pitching moment coefficient is modeled with no dependence on as
Cm = Cm (, Tc ) + Cme e +
i
c h
Cm q Q
2VT
(3.15)
Yawing Moment
Similarly, the yawing moment coefficient is modeled as
Cn = Cn + Cnr r + Cna a
3.2.3
(3.16)
Aerodynamic Coefficients
All simulations were performed using aerodynamic coefficients of the Aerosonde

UAV[6] as summarized in the Aerosim toolset[15], distributed by Unmanned Dynamics.
3.3
Inner Loop: Attitude Stabilization
In order to mimic the assumed inner loop attitude stabilization and actuation
system, PID controllers are implemented for roll and pitch angle holds in the simulations, as depicted for the roll axis in Figure 3.1. These controllers are tuned for
reasonable settling times as evidenced by simulation for static inputs of desired pitch
14
and roll angles. Typical simulation results for a step response in desired pitch and
roll angles are shown in Figure 3.2.
Desired Roll +
Roll Error
PID Controller
Aileron
Deflection
UAV Dynamics
Actual Roll
Angle
Figure 3.1 PID Controller: Roll to Aileron
Figure 3.2 Simulation Results: PID Inner Loops
3.4
Outputs
The output quantities are functions of the states and are computed at each
timestep after integration. Given the inertial velocity components in the body axes
directions, the wind velocity components in the inertial axis directions, and the rotation matrix that transforms vectors between the body and inertial axes systems,
15
we can calculate the relative velocity of the vehicle with respect to the surrounding
airmass. U 0 , V 0 and W 0 are the components of the relative velocity in the body axes
directions.
0
U
WN
U
0
V = V [Re2b ] WE
(3.17)
W0
W
WD
The magnitude of the relative velocity, VT , and the dynamic pressure, q, can then
be calculated.
q
VT = (U 0 )2 + (V 0 )2 + (W 0 )2
(3.18)
q = 21 VT2
The aerodynamic and flight path angles can also be calculated from the inertial and
relative velocity components.
0
)
= tan1 ( W
U0
0
= sin1 ( VVT )
Y
= tan1 ( X
)
Z
)
= tan1 (
2 2
X +Y
16
(3.19)
Chapter 4
Attitude Stabilization System
The lowest level of the GNC system is the attitude stabilization system. The attitude stabilization system drives the aileron and elevator control surfaces in order to
stabilize the aircraft orientation at desired bank and pitch angles, as commanded by
the higher level systems. Control for automatic attitude stabilization is a well-known
problem which has been solved numerous times for full-size aircraft and spacecraft.
The main complicating factor for small UAVs, which typically have only a fraction
of the size, weight and power of their full-size aircraft counterparts, is the method of
measuring or estimating the attitude for feedback in the control system design.
A common solution for full-size aircraft attitude estimation involves expensive
and often large inertial measurement units (IMU), which use gyroscopic measurements to estimate the aircrafts orientation with respect to the inertial frame. With
the miniaturization provided by recent advances in MEMS accelerometer and gyro
technologies, similar approaches have been successfully applied to small UAVs. These
solutions, however, involve multiple measurements from relatively expensive hardware and complex nonlinear filtering methods[1, 13].
Another attitude estimation technique of recent interest for small UAVs is horizon sensing via image processing. In this technique, an on-board camera generates
images of the views along the pitch and roll axes (yb and xb ). The images are
then processed digitally to estimate the orientation of the horizon with respect to
the aircraft body axes and the pitch and roll angles can be estimated. Although
methods with reduced computational loads have been proposed, the algorithms and
software/hardware implementations are often complex.
Another method of horizon sensing, however, offers a cheaper, simpler solution
to the attitude estimation problem. Infrared (IR) horizon sensors have been used
successfully in spacecraft attitude control systems for over 40 years and their use
has more recently been extended to small UAVs[14]. The relatively low size, weight
and cost of thermopile sensors, along with the commercially available stabilization
17
technologies described later in thi schapter make an IR attitude stabilization system

a natural choice for a first effort in UAV development. For these reasons, we choose
to modify an existing commercial IR stabilization system for our purposes. It should
be noted that the higher level GNC systems, however, are independent of the chosen
method of attitude stabilization, provided that the performance is acceptable.
4.1
Infrared Horizon Sensing
Human pilots can determine aircraft attitude in visual meteorological conditions

(VMC) with a quick glance out the cockpit by referencing the visible horizon. Digital
image processing allows an automatic control system to accurately mimic this behavior, but at significant computational cost. A simple method of stabilizing an RC-sized
aircraft in straight and level attitude (zero degrees in both pitch and roll) is marketed to beginner RC pilots in the Futaba Pilot Assist FA-2 Copilot devices. These
devices use light-sensitive sensors to estimate the midpoint between the dark ground
and the light sky, but are especially sensitive to clouds or very bright sunshine[14].
Egan and Taylor[2] show that these sensitivities can be minimized by using sensors
with bandwidths in the infrared portion of the spectrum. Since the ground and
sky have different apparent infrared temperatures, an infrared sensor with a large
field of view, say 100o , can be thought of as giving an average temperature of the
sky/ground combination in view of the sensor as depicted in Figure 4.1. Arranged
in a differential pair, two sensors facing 180o apart can be placed along the pitch or
roll axis to output voltages related directly to the inclination of the body about a
perpendicular axis.
Testing by Egan and Taylor[14] showed that the output of a differential pair
approximates a sinusoidal function of the inclination angle for modest deviations
(+/- 45o ) about zero. As expected, the output curve flattens out as each sensors
field of view becomes filled with all sky or all ground. Since we prefer our system
to maintain modest pitch and bank angles, the sinusoidal approximation suits our
systems needs.
18
Figure 4.1 Infrared Horizon Sensing: Single Axis
4.2
FMA FS-8 Copilot
Like the Futaba PA-2 Pilot Assist device, the FS-8 Co-pilot, manufactured and
distributed by FMA Direct, is an off-the-shelf device designed to assist beginner-level
RC pilots restore straight and level flight from precarious attitudes. The FS-8 Copilot, however, consists of a standard RC receiver coupled with a PIC microcontroller
and three orthogonal axes of amplified and offset differential thermopiles. The offset
voltage for the FS-8 is 1.6 Volts, but we will refer to it in general as Vof f set . After
proper calibration, the PIC samples the output from the differential pairs along
the pitch and roll axes, then determines the control surface deflections necessary to
drive the differential readings to the neutral offset value. This system is depicted
graphically in Figure 4.2. While the FS-8 Co-pilot is able to stabilize the aircraft at
zero pitch and roll angles, this device is unable to stabilize the aircraft at arbitrary
attitudes, as required by the higher level control loops, without further modification.
4.3
4.3.1
Arbitrary Pitch and Bank Angle Stabilization

Calibration
In order to modify the commercial FS-8 device for our needs, we employ an
electrical bias technique as suggested by [14]. The Co-pilot device is mounted with
19
Figure 4.2 Copilot System: Single Axis

the two horizontal sensor axes aligned with the body pitch and roll axes. The third
vertical sensor axis is aligned with the aircraft yaw axis. Immediately following the
short calibration procedure described in the FS-8 Users Manual, the vertical axis
sensor pair is sampled several times and averaged by our flight controller. Since the
aircraft is still in the horizontal attitude dictated by the FS-8 calibration procedure,
the vertical sensors see all ground or all sky and the differential voltage measured
gives the maximum value of the Voltage-Inclination relationship, which we denote
with the symbol Vmax . By assuming that the thermopiles exhibit identical responses
to the same stimuli, we know that differential output will reach the neutral offset
value, Vof f set when the inclination of its axis reaches 0o and 180o . Together with the
Vmax calibration reading at an inclination of 90o , this allows us to fully parameterize
the sinusoidal Voltage-Inclination curve with the relationship in Equation 4.1. This
relationship is shown graphically in Figure 4.3 for Vmax = 3.0 Volts.
VIR = (Vof f set Vmax ) sin(x) + Vof f set
(4.1)
where x is taken to be either or depending on the axis of interest.
4.3.2
Stabilization
The FS-8 Co-pilot is designed to stabilize the aircraft about the IR reading given
when x=0, or VIR = Vof f set . We desire however, to stabilize about arbitrary values
of x. Rather than feeding the direct differential voltage VIR to the FS-8 controller,
we utilize digital to analog converters (DACs) and an additonal summing op-amp.
One axis of the modified Co-pilot system is shown in Figure 4.4. By adding (or
20
Figure 4.3 Voltage-Inclination Curve, Vmax =3.0 Volts
Figure 4.4 Modified Co-pilot System: Single Axis

subtracting) some bias voltage, VDAC , to the differential IR voltage VIR we aim to
trick the FS-8 controller into thinking it is flying level when, in fact, it is flying at
some arbitrarily selected inclination angle. The voltage at the end of the modified
op-amp scheme, which is fed into the FS-8 is denoted VF S8 , and its value is given by
Equation 4.2.
VF S8 = VIR + VDAC
(4.2)
Given a desired inclination angle, xd , we first calculate the output of the differ-
21
ential IR pair exhibited at this inclination, using 4.1.

VIR,d = (Vof f set Vmax ) sin(xd ) + Vof f set
(4.3)
Knowing that we wish VF S8 to be equal to Vof f set at xd , we then solve Equations 4.2
and 4.3 for the desired VDAC .
VDAC,d = Vof f set VIR,d = (Vmax Vof f set ) sin(xd )
22
(4.4)
Chapter 5
Path Following Subsystem
Given a set of ordered waypoints specifying a desired flight path, a higher level
algorithm is required to generate desired flight trajectories to drive the vehicle to its
path. This chapter details the Path Following Subsystem design, which solves this
guidance problem.
Many useful paths can be formed by using linear segments alone. Search patterns, rectangular perimeter orbits, and simple point-to-point paths are some of the
simplest examples, as shown in Figure 5.1. More advanced examples of linear paths
Figure 5.1 Linear Path Segment Examples

might include following streets in urban neighborhoods. The paths considered in
this thesis are limited to those which can be constructed by linear segments. These
paths are defined by an ordered series of two-dimensional (X,Y) waypoints in a local
tangent plane coordinate system. Our system also considers an altitude hold system independently of the lateral guidance algorithms, rather than following linear
trajectories in three dimensions.
With the path specified by an array of ordered waypoints, there are two general
approaches to the 2-D tracking problem. The first approach, commonly referred to
as trajectory tracking, drives the vehicle to follow a reference point which travels
along the path at some velocity. This implicitly requires that the vehicle be in a
certain position at a certain time, which can cause problems for small UAVs, where
wind speeds can approach 50 or 60 percent of the aircrafts airspeed. For example,
23
if the aircraft is traveling into the wind with constant thrust or constant airspeed,
and the reference points velocity is not reduced accordingly, the position error for
the trajectory tracking system will continually increase.
The second approach, which is used in this system, is a path following approach.
The objective in path following is that the vehicle be on the path, rather than in a
certain place at a certain time. The path following algorithms used are adapted from
the work in [11].
5.1
Vector Field Path Following
The path following algorithms used in this system are based on a vector field construction that represents the desired ground track heading of the aircraft. Given the
current position and the waypoint path, both specified in the planar coordinates, the
vector field is essentially painted on the ground around the path. This field specifies
the desired ground track headings as a function of vehicle position, groundspeed, and
ground track. By directly specifying a desired ground-track heading (rather than a
body-fixed heading), the algorithms in this subsystem guarantee path convergence in
the presence of constant wind disturbances, provided that the aircraft can maintain
a positive groundspeed. The algorithms for the vector field construction for linear
paths are summarized here.
5.1.1
Algorithm
A list of variables used for the vector field path following algorithm is given in
Table 5.1. The design parameters, , e , k, and s are tunable, and change the
convergence characteristics of the cross-track error.
First, the course between the two currently selected waypoints is calculated.
f
= tan
w2Y w1Y
w2X w1X
(5.1)
Then the position of the aircraft in the inertial coordinates is transformed into coordinates along and orthogonal to the path (s ,) and the side of the path which the
24
Table 5.1 Path Following Variables

Symbol
i
f
s

s
wi , wiX , wiY
z = (X, Y )T
c
e
k
Vg
Description
Current waypoint index (initialized to 0)
Heading from waypoint i to waypoint i + 1
Aircraft progress along path, s [0,1]
Lateral tracking error
Waypoint switching look-ahead distance (0 < s 1)
Transition region boundary distance
ith waypoint, its north and east components
The side of the path the aircraft is on, having a value of 1
Current location of the aircraft
Commanded heading
Entry heading angle (0 < e < 2 )
Transition gain, k > 1
Current aircraft groundspeed
Current aircraft ground-track heading
aircraft is on, , is determined.

(zw1 )T (w2 w1 )
|w2 w1 |2
(s (w2 w1 ) +
s =
= kz
w1 )k
= sign [(w2 w1 ) (z w1 )]
(5.2)
Once the position of the aircraft relative to the current waypoints (w1 , w2 ) is determined, we must determine whether it is time to switch to the next set of waypoints.
If s > s , then we switch to the next waypoint. w2 becomes w1 and the next waypoint in the ordered array becomes w2 . We use s = 1 in order to drive our aircraft
as close to w2 as possible, but smaller values can be used to switch waypoints sooner
and reduce overshoot in tight turns, as shown in Figure 5.2. The cross-track error,
, is then given a signed value.
=
(5.3)
If the magnitude of the cross track error is greater than the threshold value (|| > ),
then the commanded heading to the autopilot is set using the entry angle.
c = f e
25
(5.4)
Figure 5.2 Waypoint Switching Look-Ahead, s

Otherwise, an intermediate vector field heading, d , is calculated and the heading
commanded to the autopilot, c , is compensated for the instantaneous groundspeed
and ground track heading by Equation 5.6.
d = f (e )
k
ke Vg k1
sin
=
k
(5.5)
(5.6)
This algorithm is repeated each time a new position measurement is acquired and
the commanded ground-track heading, given either by Equation 5.4 or 5.6 is passed
on to the autopilot subsystem, which is described in the next chapter.
26
Chapter 6
Autopilot Subsystem
With the commanded ground-track heading angle given by the subsystem described in Chapter 5, a desired altitude specified, and the ability to actuate pitch
and bank angles via the subsystem described in Chapter 4, we then require algorithms which map the desired ground-track and altitude quantities into pitch and
bank commands. This task is accomplished with the autopilot subsystem described
in this chapter. The autopilot system is further subdivided into two controllers: a
ground track heading-hold autopilot which commands to drive , and an altitude
hold autopilot which commands to drive h.
When first considered with a simple model, the autopilot controller problems
lent themselves naturally to a feedback linearization design process. The model naturally took an affine-in-control structure with globally invertible nonlinearities and
a feedback linearization control law was found to be quite successful in early simulations. Similar controllers were derived based on the full 6-DOF model and found to
maintain the performance seen in the simpler model. These feedback linearization
controllers were then compared to a linear autopilot formulation and it was found
that comparable performance can be attained in the full GNC system with simpler,
easier to implement P and PI autopilot algorithms. This chapter details the derivation of the feedback linearization autopilot algorithms, the formulation of the linear
autopilot, and simulation results comparing performance of both.
6.1
6.1.1
Feedback Linearization Autopilot Algorithms

Ground Track Heading Hold
As seen in Chapter 3, the 6-DOF simulation model contains numerous nonlinear

coupling between states and a non-intuitive mapping between inputs and outputs.
While linearization and decoupling is certainly a design option, we instead choose
27
to reformulate the exact nonlinear equations of motion into a somewhat simpler

form we call the Trajectory Model. The derivation of these equations is covered
in Appendix A, and the final results given there depend only on an assumption of
zero sideslip. Another simplifying assumption can be made if the wind speeds are
assumed to be low, as compared to the inertial velocity of the aircraft. Under these
two assumptions, then , and the dynamics from Equation A.14 simplify
greatly.
=
L
sin
mVin cos
(6.1)
Intuitively, we wish to determine the necessary control, , which will exponentially drive the vehicle to the desired ground track angle, d , as commanded by the
path tracking algorithm. That is, we desire:
(t) = d exp(a t)
(6.2)
for some positive time constant, a . This is equivalent to desiring that the dynamics
take the form
= a ( d )
(6.3)
Simply equating the desired in Equation 6.3 to the actual from Equation 6.1 yields
a trigonometric equation in .
L
sin = a ( d )
mVin cos
6.1.2
(6.4)
Altitude Hold
An altitude-hold component is also needed to complete the trajectory tracking

subsystem. The control selected to maintain altitude during maneuvers and make
small altitude changes is the commanded pitch angle, d . We again choose a nonlinear
approach based on the trajectory model and a feedback linearization technique to
design the longitudinal controller.
We begin by defining the output we wish to stabilize. Let hd be the desired
altitude.
z1 = h hd
(6.5)
We differentiate, assuming that the desired altitude hd is constant. The aircraft
28
is given via geometry of the flight path trajectory in the vertical plane.
climb rate, h,
z1 = Vin sin
(6.6)
Since the control has not appeared, we differentiate again using the chain rule and
substituting from Equation A.12.
z1 = V in sin + V
in cos
= (V in ) sin + (Vin )(
cos )
(LfLx +DfDx +T fTx mg sin ) sin +(LfLz +DfDz +T fTz +mg cos )( cos )
=
m
(6.7)
Substitution from Equation A.13 allows for several cancellations and yields
z1 =
T Dc + Ls
Ds + Lc
sin +
cos cos g
m
m
(6.8)
We wish to use the control to reshape the z1 dynamics into a linear system,
z1 = z2
(6.9)
z2 = k1 z1 k2 z2 v(z1 , z2 )
(6.10)
which is a globally asymptotically stable system for positive k1 , k2 . We must then

solve the nonlinear equation
Ds + Lc
T Dc + Ls
sin +
cos cos g = v(z1 , z1 )
(6.11)
m
m
for . The autopilot task is then to simultaneously solve (or approximate a solution
to) equations (6.4) and (6.11).
6.1.3
Existence of Solutions and Corresponding Control Laws
The system of equations (6.4, 6.11) provided the available aerodynamic force
maintains a certain balance with the desired response characteristics. Mathematically, this condition is described by the following inequalities:
q
(T Dc + Ls)2 + (Ds + Lc)2 c2 |m(v + g)|

mV Lcos
in
|a ( d )|
29
(6.12)
This shows mathematically that the desired speed of the altitude response must be
balanced by the available aerodynamic forces. The direct algebraic solution, given
by (6.13) is used when equations (6.12) are satisfied for a complete inversion.
1
= sin
m(v+g)
(T Dc+Ls)2 +(Ds+Lc)2 c2

sin1 a (dL)mVin cos
(6.13)
Here, 0 is given by an identity for linear combinations of trigonometric functions.

0 = sin1
(Ds+Lc)c
(T Dc+Ls)2 +((Ds+Lc)c)2
0 ; T Dc + Ls 0
+
; T Dc + Ls < 0
(6.14)
Otherwise, we instead solve a first-order approximation of the system (6.4,6.11) about

( = 0, = 0).
Regardless of whether the (, ) commands are determined by equations (6.13)
or their linearized equivalents, both commands are passed through a saturation filter
before relay to the attitude stabilization system. The bank commands are saturated
at 45o while the pitch commands are saturated at a more modest 20o .
6.1.4
State Measurement and Estimation
The lateral and longitudinal control laws given above assume full state feedback.
For implementation, we must either directly measure or estimate the ground-track
heading and vertical flight path angles (, ), the inertial velocity (Vin ), altitude (h),
and the angle of attack (). The instantaneous values of the thrust, lift, and drag
forces must also be estimated.
It is assumed that the thrust force, T , is known. Common static or wind tunnel
testing techniques are available to generate models for the thrust force as a function
of motor speed and airspeed. It is further assumed that the lift and drag forces are
known functions of airspeed and the angle of attack. The quantities which require
further estimation are and . We choose a two-stage estimation scheme in which
an estimate of angle of attack is generated based on the available measurements, and
the estimate of the vertical flight path angle depends on these measurements and the
estimate. This sequenced approach is depicted graphically in Figure 6.1.
30
Figure 6.1 Two Stage Estimation Scheme

Angle of Attack Estimation
Consider the relationship between relative and absolute velocities given by Equation 2.10. Writing the ze component of this vector equation, assuming no vertical
wind component (WD =0), we have
h = Vrel (scc cs)
(6.15)
By differentiating the altitude measurement given by the altimeter (after a low-pass

filter to reduce high frequency noise), we can have a direct reading for the climb rate,
Since and are directly measured, we can directly solve Equation 6.15 for .
h.
1
= sin
h
2 2
Vrel s s + s2
!
1
sin
s
2 2
c c + s2
(6.16)
Simulation results for a highly maneuvering aircraft ((t) = 45o sin t , (t) = 12o cos t)
the estimator performance is shown in Figure 6.2. Although the estimator performance is hardly asymptotic, it works acceptably well in the closed loop system as
evidenced by the simulation results that follow.
6.1.5
High-Gain Observer for Flight Path Angle
If we again make the approximation = , the flight path angle and altitude
dynamics simplify.
[Lcs(T D)s]+[Lcc+(T D)smg]
(mVin )
[Lcs(T D)s]
D)smg]
+
(sin ) + [Lcc+(T
(mVin )
(mVin )
h = Vin + Vin (sin )

31
(cos 1)
(6.17)
(6.18)
15
Actual
Estimated
Angle of Attack (deg.)
10
10
15
10
Time (s)
15
20
Figure 6.2 Angle of Attack Estimation:

These dynamics, which have a nearly linear form as the flight path angle approaches
zero, are purposely separated into the linear portions and perturbation terms for
nonzero . For compactness we denote the perturbation terms as x .
=
[Lcs(T D)s]
mVin
D)smg]
(sin ) + [Lcc+(T
(cos 1)
mVin
h = Vin (sin )
(6.19)
In level flight, the flight path angle, , is zero. Since the longitudinal autopilot
uses pitch angle as a control and is designed only to hold altitude and make small
corrections, the resulting flight path angles during normal operation should be small.
Since both the and h terms will approach zero as does, we neglect these terms
formulate the observer as
=
[Lcs+(T D)s]
[Lcc+(T D)smg]
mVin
= Vin + h h h
h

+ h h
(6.20)
(6.21)
or in a more standard LPV system form

x = A
x + Bu
32
(6.22)
with
"
x = ,
h
"
#
1 0
B=
,
0 1
" Lcs(T D)s
mVin
A=
Vin
h
#
" Lcc+(T D)smg
+ h
mVin
u=
h h
(6.23)
The estimation error is given by

"
e

xe e =
h
hh
(6.24)
and satisfies the equations

e =
[Lcs+(T D)s]
e
mVin
e +
h
e = (V ) h
e +
h
in
h
h
(6.25)
In the high-gain observer formulation, and h are design parameters which

control the convergence of the observer to the true state by driving the poles of the
error dynamics (6.25) further into the left half plane. The high gain observer suffers
from the so-called peaking phenomenon, which is known to cause instability in the
closed loop feedback system. Our control laws, however, include saturation, which
reduces the risk of instability[8]. Simulation results for the high-gain observer are
shown in Figure 6.3.
6.2
Output Feedback Simulations
The closed loop system uses direct measurements and estimates based on these
measurements to generate the feedback terms. Output feedback simulation results
in response to simultaneous step inputs in both desired altitude and desired ground
track are shown in Figure 6.4.
6.3
Alternative Autopilot Design and Comparison
In this chapter, more traditional autopilot control laws are presented. While the
autopilot laws derived via feedback linearization work well in simulation, their implementation requires an impressive sensor suite and a two-stage estimation scheme
33
Flight Path Angle (deg.)
15
Actual
Estimated
10
5
0
5
10
15
10
Time (s)
15
20
Altitude (m)
220
Actual
Estimated
200
180
160
140
10
Time (s)
15
20
Ground Track Heading (deg.)
Figure 6.3 High Gain Observer: = 10, h = 20

40
30
20
10
Desired
Actual
0
0
10
15
20
Time (s)
25
30
35
40
Altitude (m)
215
210
205
Desired
Actual
200
195
10
15
20
Time (s)
25
30
35
40
Figure 6.4 Output Feedback Controller: a =0.25, k1 =0.18, k2 =0.7

which makes for a complex software implementation. The question is whether the
practically feasible performance improvements warrant such a complicated controller
implementation.
Several commercial and academic UAV autopilots implement relatively simple P,
34
PI, or PID controllers for the heading-hold and altitude-hold functions. Encouraged
by their successes and our own experiences in simulation, we choose to formulate a
PI loop for altitude hold and a simpler proportional controller for heading hold.
6.3.1
Altitude Hold: PI Controller
Given a desired altitude, hd , we can calculate an altitude error signal from our
altitude measurement.
eh = h hd
(6.26)
The control law for the altitude hold autopilot is implemented as
= P eh + I
6.3.2
Z t
0
eh dt
(6.27)
Ground Track Heading Hold: P Controller
The ground track heading hold autopilot is configured with a simple proportional
controller. With a desired ground track specified as d and a measurement of the
current ground track heading, , the lateral error signal can be calculated.
e = d
(6.28)
The proportional controller is then easily formulated.

= P e
(6.29)
Additionally, both the ground track and altitude hold controllers include saturation blocks ( 45o in roll, 20o in pitch) to eliminate illogical commands in cases
of large error signals.
6.3.3
Simulation Results
The simpler, linear autopilot designs were tuned and tested using the 6-DOF
simulation model for a variety of conditions. The proportional ground track heading
hold autopilot was successful, as expected, for small step inputs in desired ground
track commands. Figure 6.5 shows typical response in ground track and bank angle
for a 15o change in desired ground track. The ground track heading hold autopilot is
35
Figure 6.5 Proportional Ground Track Heading Hold Autopilot, 15o Step Input
required to drive the ground track dynamics to a form approximating Equation 6.3,
since this is assumed by the Path Following algorithms. Moreover, the first order time
constant a should be quantifiable and constant over a wide range of operation. The
proportional controller is able to maintain the same response characteristics until the
right hand side of Equation 6.29 is large enough to saturate the bank angle command
at 45o . Performance, as measured by rise time, degrades for larger step inputs, but
stability is maintained. This effect is shown in Figure 6.6 where the time response of
the ground track heading dynamics, normalized by the magnitude of the step input,
are shown for several step input sizes and a constant proportional gain, P . The
transient response curves are tightly grouped for step input magnitudes between 15
and 45 degrees, but performance degrades for higher values. This phenomenon is
alleviated for smaller gains, as seen in Figure 6.7, at the cost of a constant but larger
time constant, a .
A comparison between the feedback linearization and PI altitude hold autopilots
also demonstrates the relatively small difference in performance between the two
designs. Both the feedback linearization (FL) and proportional plus integral (PI)
altitude hold controllers can be tuned for well-damped responses to a step input in
desired altitude as seen in Figure 6.8. This tuning is done with wings-level flight
( = 0). For a maneuvering vehicle, however, 6= 0, and the performance of both
36
Figure 6.6 Proportional Ground Track Heading Hold Autopilot, P = 1.5
Figure 6.7 Proportional Ground Track Heading Hold Autopilot, P = 0.25

FL and PI altitude hold controllers exhibit small altitude losses (and gains). This
behavior can be seen a maneuvering aircraft with (t) = 45o sin t in Figure 6.9.
The oscillations in the feedback linearization controller response are damped by the
explicit inclusion of in the control law of Equation 6.13, but the approximations
made in the FL controller derivation (namely, = 0) are not well satisfied during
aggressive maneuvers. Even without a derivative term or a lateral coupling term, the
PI controller is able to maintain performance comparable to that of the FL controller
37
Figure 6.8 Altitude Hold: Tuned for 3 Second Rise Time

in simulation. The magnitude of the altitude oscillations for both controller cases
are less than three feet. 6.8.
Figure 6.9 Altitude Hold: (t) = sin t

After evaluation comparison of numerous simulation studies, all with results comparable to those presented in this chapter, it was determined that the feedback linearization autopilot algorithms demonstrated only slightly improved performance
over the (P, PI) autopilot design in only a few situations. Accordingly,the increased
hardware complexity, i.e. the requirements for additional sensors for airspeed and
38
body-fixed heading, are not warranted and the simpler proportional, proportionalintegral controllers are used for the autopilot subsystem in implementation.
39
Chapter 7
GNC System Implementation
Hardware and software implementations of the selected path following, autopilot
and attitude stabilization systems are presented in this chapter. The hardware developed to accomplish the basic GNC functions includes a flight computer, sensors
and the DAC/op-amp scheme designed to complete the modified Co-pilot attitude
stabilization system. Additional developments detailed in this chapter are a wireless
telemetry downlink and a microcontroller-based pilot override system.
7.1
GNC System Hardware
The flight computer is composed of an MSP430F149 microcontroller, manufactured by Texas Instruments. The MSP430 family is used in several courses in Texas
Tech Universitys Mechanical and Electrical Engineering Departments. The F149
variant offers two hardware USART ports, an 8 channel, 12-bit Analog to Digital
Converter (ADC) and is programmable in C. For the GNC system designed in the
previous chapters, using the simpler P, PI autopilot formulation, the only required
sensor is a GPS receiver which gives measurements of 3-D position, groundspeed and
ground-track heading. Four 12-bit digital to analog converters (DAC) are combined
with two summing operational amplifiers to provide the attitude stabilization bias
voltages, VDAC , for the two pitch and roll channels, as described in Chapter 4. The
hardware prototype for this GNC system is depicted graphically in Figure 7.1.
The Garmin 15L GPS receiver is configured to output the NMEA 0183 standard
GPGGA and GPVTG sentences at 9600 baud once a second. The flight computer
receives these sentences on UART0, which is configured for asynchronous serial communication (USART mode). The GPGGA sentence is parsed for position (latitude,
longitude, height aMSL) and the GPVTG sentence is parsed for ground-track heading
(course) and groundspeed. Additionally, the un-biased infrared readings, VIR , from
both the pitch and bank channels are sampled at a user-defined rate for transmission
40
Figure 7.1 Hardware Prototype: P,PI Autopilot

to the ground station.
The position, ground-track, and groundspeed measurements are used to evaluate
the Path Following and Autopilot algorithms to generate the pitch and bank angle
commands. These commands are converted to a desired DAC voltage (either positive
or negative) using equation 4.4. This voltage is actuated by the flight computer via
a custom SPI library written to interface with the DAC7512 chip, manufactured by
Texas Instruments. Both the pitch and roll channels utilize two DAC modules a
piece, for a total of four modules. One of the channels DAC modules represents an
addition voltage, while the other represents a subtraction voltage, as shown for a
single channel (either pitch or roll) in Figure 7.2.
If the result of Equation 4.4 is greater than (or equal to) zero, then VDAC+ is set
to this result and VDAC is set to zero. If the result of Equation 4.4 is negative, then
VDAC+ is set to zero and VDAC is set to this result. In this way, the full 12 bits of
each DAC is devoted to the positive voltage range [0,3.3V], effectively doubling the
resolution and eliminating the need for negative voltage references.
7.2
Radio Modem
A 900 MHz radio link provides a wireless telemetry downlink to the ground station
software. The Maxstream 9XTend module is operated at a regulated 5.0 Volts with a
9600 baud rate using the Tx channel of UART0 on the flight computer. The 9XTend
module has a maximum transmit power of 1 Watt with software selectable power
ranges of 1, 10, 100, 500, or 1,000 mW. The current implementation uses 500 mW
41
Figure 7.2 DAC/Op-Amp Implementation: Single Channel

for transmission to alleviate interference problems with the 72 MHz on-board RC
receiver. After parsing a set of valid GPS sentences and setting the DAC array to
actuate the commanded pitch and bank angles, the Flight Computer transmits an
ASCII sentence to the ground station for flight visualization and debugging purposes.
The structure and contents of the telemetry sentence are shown in Figure 7.3 and
Table 7.1.
Figure 7.3 Telemetry Sentence Structure
7.3
Remote Pilot Override
A safety pilot override is required before any flight testing can begin. If the
aircraft adopts an unstable attitude or an undesirable flight path, the safety pilot
must be able to regain control and bring the vehicle home to ensure the integrity of
the electronics payload and the safety of any observers.
The pilot override consists of a second MSP430 microcontroller. This microcontroller is an F2012 variant, which has a smaller pinout, less I/O capabilities and is
cheaper. The F2012 monitors a PWM signal from the on-board 72 MHz RC receiver. This channel is configured to a switch on the RC pilots controls. When
42
Table 7.1 Telemetry Sentence Contents

Symbol Format Quantity
J
N/A
Denotes Beginning of Telemetry Sentence
<1>
xx
Latitude Degrees North
<2>
xx.xxxx Latitude Minutes North
<3>
xxx
Longitude Degrees West
<4>
xx.xxxx Longitude Minutes West
<5>
xxxxx.x GPS height in Meters Above/Below Mean Sea Level
<6>
xxx.xx Pressure Airspeed (m/s) Not Currently Implemented
<7>
xxx
GPS Ground Track Heading: 000-359 degrees CW from North
<8>
xxxx.x GPS Ground Speed: km/h
<9>
xxxx
IR ADC Reading, Roll Axis: 0-4095 corr. to 0-3.3 Volts
< 10 >
xxxx
IR ADC Reading, Pitch Axis: 0-4095 corr. to 0-3.3 Volts
< 11 >
xxxx
IR ADC Reading, Yaw Axis: 0-4095 corr. to 0-3.3 Volts
< 12 > xxxxxx GPS Time of Fix: hhmmss Format
< 13 >
xxx.x
Calculated Pitch Angle Command (degrees)
< 14 >
xxx.x
Calculated Roll Angle Command (degrees)
xx
Denotes End of Telemetry Sentence
< CR >
x
Carriage Return character
< NL >
x
New Line character
the pulse width on the PWM signal raises above a certain threshold (corresponding
to an ON control switch), the F2012 raises an digital signal high. This high signal
is then read by the Flight Computer and autonomous operation is enabled. If the
pulse width drops below the threshold (corresponding to an OFF control switch),
the F2012 drops the digital signal low. This triggers an interrupt on the Flight Computer which disables the telemetry downlink and sets all DAC bias voltages to zero
for remote pilot flight.
7.4
Hardware in the Loop Simulations
In the absence of a reliable aircraft system, a hardware-in-the-loop (HIL) simulation environment was developed to test, debug and validate the hardware/software
implementations of the GNC system design. In these simulations, the nonlinear,
6-DOF equations of motion are integrated in software. The output position and
velocity quantities are then converted to the form of expected GPS measurements:
43
longitude, latitude, altitude, ground-track and groundspeed. These converted values

are used to construct simulated GPS readings using the NMEA 0183 ASCII format
and passed to the Flight Computer through an RS-232 interface at a 1 Hz frequency
to simulate the actual GPS environment. The simulated pitch and bank angles are
converted to simulated infrared voltages and passed through two analog outputs to
the Flight Computers ADC.
The Flight Computer is then able to perform the same functions as it would if
it were on an actual flying aircraft. The Flight Computer runs the navigation and
actuation routines, and the DAC bias voltages are read by the simulation computer
as commanded pitch and bank angles. The simulation computer then uses these as
inputs to integrate the equations of motion and the HIL simulation cycle continues.
The Flight Computer also sends the telemetry data through the wireless modem to
a laptop running the ground station software.
Figure 7.4 shows typical Hardware in the Loop simulation results from the simulation computer.
Figure 7.4 Hardware in the Loop Simulation Results
44
Chapter 8
Conclusion
This thesis presents a complete Guidance, Navigation and Control system design
for an autonomous air vehicle. Flight tested algorithms for air vehicle path following
are summarized. These algorithms are combined with a modified commercial attitude
stabilization system based on horizon sensing using infrared thermopiles. An original autopilot design using feedback linearization is also presented alongside a more
traditional proportional and proportional plus integral autopilot design. Simulations
show that the qualitative performance of both autopilot designs is comparable, and
it is concluded that the current complexity of the physical implementation of the
feedback linearization control strategy does not warrant further investigation at this
time and a simpler P, PI autopilot is selected for implementation. The path following, autopilot, and attitude stabilization system designs demonstrate acceptable
performance in simulation and these three subsystem designs are implemented in
hardware and tested using Hardware in the Loop simulations.
Future work to experimentally validate the entire GNC system design is required.
Although the basic hardware implementations have been validating using HIL simulation, these simulations assume satisfactory performance of the modified commercial
attitude stabilization system, which has yet to be demonstrated for the custom design. Additionally, while the simple P, PI autopilot designs require fewer sensors
and less software to implement, their gains have proved cumbersome to tune in
simulation. An investigation of the actual tuning requirements of a P,PI autopilot
implementation compare to that of a feedback linearization implementation.
45
Bibliography
[1] Markley, F.L., Yang, C., and Crassidis, J.L. Nonlinear Attitude Filtering Methods Proceedings of the AIAA Guidance, Navigation, and Control Conference
and Exhibit, San Francisco, 15-18 August 2005.
[2] Egan, G. and Taylor, B. Characterisation of Infrared Sensors for Absolute Unmanned Aerial Vehicle Attitude Determination, Technical Report MECSE-22007, Monash University, 2007.
[3] Etkin, B. Dynamics of Atmospheric Flight. New York: John Wiley and Sons,
1972.
[4] Farrell, J.A. and Barth, M. The Global Positioning System and Inertial Navigation. New York: McGraw-Hill, 1999.
[5] Farrell, J.A. Aided Navigation: GPS with High Rate Sensors. New York: McGraw Hill, 2008.
[6] Holland, G. The Aerosonde Robotic Aircraft: A New Paradigm for Environmental Observations, Bulletin of the American Meteorological Society, vol. 82
part 5, pp. 889-902, 2001.
[7] Khalil, H.K., High-gain observers in nonlinear feedback control, Proceedings
of the International Conference on Control, Automation and Systems, 14-17
October 2008.
[8] Khalil, H.K. Nonlinear Systems. Upper Saddle River, NJ: Prentice Hall, 2002.
[9] Kingston, D. Implementation Issues of Real-Time Trajectory Generation on
Small UAVs. Masters thesis, Brigham Young University, April 2004.
[10] Miele, A. Flight Mechanics, Volume One: Theory of Flight Paths. Reading, MA:
Addison-Wesley, 1962.
[11] Nelson, D.R., Barber, D.B., McLain, T.W., and Beard, R.W. Vector Field
Path Following for Small Unmanned Air Vehicles, Proceedings of the American
Control Conference, Minneapolis, 14-16 June 2006.
[12] Stevens, B.L. and Lewis, F.L. Aircraft Control and Simulation. Hoboken, NJ:
John Wiley and Sons, 2003.
46
[13] Tayebi, A., McGilvray, S., Roberts, A., and Moallem, M. Attitude Estimation
and Stabilization of a Rigid Body Using Low-Cost Sensors, Proceedings of the
46th IEEE Conference on Decision and Control, New Orleans, 12-14 December
2007.
[14] Taylor, B., Bil, C., Watkins, S., and Egan, G. Horizon Sensing Attitude Stabilisation: A VMC Autopilot, Proceedings of the 18th International UAV Systems
Conference, Bristol, UK, 2003.
[15] Aerosim Blockset, Unmanned Dynamics. [Online]. Available: http://www.udynamics.com/aerosim/
47
Appendix A
Trajectory Model
The 6-DOF, body axes equations of motion involve complicated aerodynamic
moments, nonlinear coupling between the angular rates and other undesirable mathematical structures for control system design. The trajectory tracking system, described in Chapter 5, however, maps desired the desired aircraft kinematics (groundtrack heading and altitude) into desired pitch and roll angles, and must therefore
consider the effects of the relevant forces acting on the aircraft. This chapter presents
a more intuitive model of the aircrafts flight path that is used in Chapter 5 to formulate a nonlinear solution to the trajectory tracking problem.
To simplify control design, we first assume that the full orientation of the aircraft
is measurable and/or controllable at any given time. The quantities , , and
are assumed to be time varying, but known. The angles and are assumed to
be controllable, via some relatively tight attitude stabilization loop. We can then
model the aircraft as a particle and apply Newtons law to generate an alternative
but equivalent state space model, with a reduced number of state equations.
Newtons Law for the Aircraft

Newtons law for the aircraft center of mass (COM) is given as
T + A + mg = ma0
(A.1)
where T, A, and a0 are the thrust force, aerodynamic force and total acceleration
vectors, respectively.
The path axes frame, as defined in Chapter 2, is centered at the aircraft COM,
but can rotate with respect to the inertial frame. The total acceleration, a0 , term
must account for this angular rate, which is nonzero in general, using a correction
term for a rotating frame.
48
d
d
Vcm/e = p Vcm/e +
p/e Vcm/e
(A.2)
dt
dt
Since the xp axis points in the direction of the inertial velocity vector, Vcm/e can
easily be expressed in path coordinates.
a0 = e
p
Vcm/e
= Vinip
(A.3)
p
The scalar quantity Vin is the magnitude of Vcm/e
. The angular velocity vector of
the path axes with respect to inertial axes is
p/e = ( sin ) ip + ()
jp + ( cos ) k
(A.4)
The total acceleration (A.2) can then be expressed in path axes coordinates.

p
in cos ) jp (V
in ) k
a0 = V in ip + (V
(A.5)
Newtons law then takes the following form.
Tp + Ap + mgp = m
V in
V
in cos
V
in
(A.6)
The right superscripts denote the vector quantities written in path axes coordinates.
In order to complete the three scalar equations of motion, we must express the thrust,
aerodynamic, and gravitational forces in path axes coordinates.
Forces
Gravity
The gravitational force resolved into path axes is given by
mgp = [Re2p ]
0
0
mg
49
mg sin
0
mg cos
(A.7)
Thrust Force
We assume the thrust force, T, is directed along the bodys xb axis and resolve
this force into the path axes directions using rotation matrices.
Tp = [Re2p ] [Rb2e ]
T
0
0
(A.8)
Expanded, the thrust force in the path axes coordinate directions is given by
sin sin + sin sin cos cos + cos cos cos cos
T
cos sin cos sin cos cos
sin sin cos sin + cos cos cos sin sin cos
(A.9)
where T is the magnitude of the thrust force generated by the motor-prop combination.
Aerodynamic Forces
The aerodynamic forces (lift, drag, and sideforce) are applied in the wind frame.
Aw =
C
L
(A.10)
The sideforce, C, is primarily caused by sideslip and rudder deflection. For an aircraft
with fixed rudder and a suitable vertical tail fin, sideslip is induced only when the
aircraft banks and is quickly damped by the moment generated by the vertical tail.
Although sideforce is modeled in simulation, we neglect this force in the Trajectory
Model, which is used only for control design. With C = 0, the aerodynamic forces
can then be expressed in the path axes directions.
p
p
p
A = L + D = [Re2p ] [Rb2e ] [Rw2b ] 0
(A.11)
Ignoring sideslip (setting =0 in Rw2b ), the total external force, expressed in path
axes coordinates, can be expressed as the sum of the gravitational, thrust and aero-
50
dynamic forces.
LfLx + DfDx + T fTx mg sin
p
p
p
T + A + mg =
LfLy + DfDy + T fTy
LfLz + DfDz + T fTz + mg cos
(A.12)
In equation A.12, L, D, and T are the magnitudes of the lift, drag, and thrust forces,
and the fQi are the direction cosines of the Q force in the path axes i direction. The
direction cosines are shown below as a function of the aerodynamic, attitude, and
flight path angles.
fLx = (ss + ccc) s + (sc csc) c cos( ) + (csc) sin( )
fDx = (scc cs) s (ssc + cc)c cos( ) + (ssc) sin( )
fLy = (csc sc) sin( ) + cs cos( )
fDy = (scs + cc) sin( ) + ss cos( )
fLz = (sc csc) s cos( ) + css sin( ) (ss + ccc) c
fDz = (cc + scs) s cos( ) + sss sin( ) + (cs scc) c
fTx = cc cos( ) + ss
fTy = c sin( )
fTz = cs cos( ) sc
(A.13)
Substituting A.12 into the LHS of Equation A.6 yields scalar state equations for Vin ,
, and .
mV in = LfLx + DfDx + T fTx mg sin
mV
in cos = LfLy + DfDy + T fTy
mV
in = LfLz + DfDz + T fTz + mg cos
(A.14)
These equations form a basis for the derivation the nonlinear autopilot designs
and are referred to collectively as the trajectory model. Together with the assumption
that vehicle orientation can be instantaneously measured (, , , and are known
inputs), zero sideslip, and a model of lift and drag forces, these three equations
describe the same path dynamics as the full 6-DOF body axes equations given in
Chapter 3.
51
Appendix B
Simulation Code
This appendix contains the MATLAB files used to simulate the GNC system. The
main script (mainScript.m) sets initial conditions, runs the simulation, and interprets
and plots the output. The RHS (rhs6DOFcontrolled.m) function generates the state
equations at each timestep. Each of the three GNC subsystems is contained in its
own MATLAB function (pathFollowing.m, autopilot.m and attitudeStabilization.m).
Additional subroutines are used for canned functions such as generating waypoint
data, rotation matrices, and output calculations. These files must all be included in
a common directory.
Main Script
The main script (mainScript.m) sets initial conditions, runs the simulation, and
interprets and plots the output.
% Main Simulation Script
% mainScript.m
clear all; close all; clc
waypointgen; % Run waypoint generation script (waypointgen.m)
WN0=0; WE0=0; WD0=0; VcI = 27.88; GcI = 0; ChiCi = 0;
gamHatInitial = 15*pi/180; % Observer
hHatInitial = 205; % Observer
tspan = [0,130]; % Time Span
IC = [0,0,-200,0,.00682,0,27.88,0,0,0,0,0,WN0,WE0,WD0,...
.006820,0,27.88,200,0,0,0,0,gamHatInitial,hHatInitial,...
VcI, ChiCi,GcI];
count=0;
fprintf(Simulating...\n)
[t,state] = ode45(@rhs6DOFPIDinnerLoops,tspan,IC,[],count);
52
fprintf(Simulation Complete.\n)
%keyboard
[X,Y,Z,phi,theta,psi,U,V,W,P,Q,R,W_N,W_E,W_D,gammaCalc1,VCalc1,...
h2,x2,y2...,gamHat,hHat,chiCalc,gamCalc,V_T,q,aoa,bet,gam,...
chi,V_in,Lift,Drag,Sideforce,A,B,C,v,phi_d,theta_d,aoaEst,...
LiftEst,DragEst,chi_d]= calcOutputs(t,state);
% Calculate outputs
%%%% Generate Plots: Commment out if desired

% State Estimation
subplot(3,1,1)
h=-Z;
plot(t,h,t,hHat)
xlabel(t (seconds))
ylabel(Altitude (m))
subplot(3,1,2)
plot(t,gam*180/pi,t,gamHat*180/pi)
ylim([-40,40]);
xlabel(t (seconds))
ylabel(Flight Path Angle (deg))
subplot(3,1,3)
plot(t,(aoa)*180/pi)
title(AOA (deg))
% Autopilot
figure;
subplot(3,1,1)
plot(t,chi*180/pi)
title(Flight Path Heading Angle)
subplot(3,1,2)
plot(t,phi*180/pi)
title(Bank Angle)
subplot(3,1,3)
plot(t,theta*180/pi)
title(Pitch Angle)
% Aerodynamic Forces
plotAeroForces;
% Control Time Series
plotControls;
53
%%%%
% AOA, Lift/Drag Force Estimation

figure;
subplot(3,1,1)
plot(t,aoa*180/pi,t,aoaEst*180/pi)
legend(Actual,Est.)
ylabel(AOA (deg))
subplot(3,1,2)
plot(t,Lift,t,LiftEst);
legend(Actual,Est.)
ylabel(Lift (N))
subplot(3,1,3)
plot(t,Drag,t,DragEst);
ylabel(Drag (N))
legend(Actual,Est.)
% Birds-eye View
figure;
plot(Y,X,b,y1,x1,ro)
RHS Function
The right hand side (RHS) function generates the state equations at each timestep.
This file must be included in the local directory.
function stateDots = rhs6DOFcontrolled(t,state,co)
% RHS of 6DOF model
% t - current time
% state - state vector: [ X, Y, Z, phi, theta, psi, U, V, W,...
%
P, Q, R ,... W_N , W_E , W_D,...
%
augmented states... ]
% Give the states nicer names
X = state(1); Y = state(2); Z = state(3);
phi = state(4); theta = state(5);
psi = state(6);
U = state(7); V = state(8); W = state(9); P = state(10); Q = state(11);
R = state(12); W_N = state(13); W_E = state(14); W_D = state(15);
gammaCalc=state(16); chiCalc=state(17); VCalc = state(18);
gamHat = state(24); hHat = state(25); Vcalc = state(26);
chiCalc = state(27); gamCalc = state(28);
h=-Z;
g=9.81;
54
%%%%%%%%%%%%%%%% Airframe Properties %%%%%%%%%%%%%%%%%%%%%%%%%

% Wing span
b = 2.8956; % m
% Wing area
S = 0.55; % m^2
AR = b^2/S;
c = .189941;
% Gross Moments of Inertia kg*m^2
Jx = .8244;
Jy = 1.135;
Jz = 1.759;
Jxz = .1204;
% ALL aerodynamics derivatives are per radian:
%%% Lift coefficient %%%
% Zero-alpha lift
C_L0 = 0.23;
% alpha derivative
C_Lalpha = 5.6106;
% Pitch control (elevator) derivative
C_LdeltaE = 0.13;
% alpha-dot derivative
C_Lalphadot = 1.9724;
% Pitch rate derivative
C_LQ = 7.9543;
%%% Drag coefficient %%%
% Lift at minimum drag
CLmind = 0.23;
% Minimum drag
C_D0 = 0.0434;
C_DdeltaE = 0.0135;
% Roll control (aileron) derivative
C_DdeltaA = 0.0302;
% Yaw control (rudder) derivative
C_DdeltaR = 0.0303;
% Oswalds coefficient
e = 0.75;
%%% Side force coefficient %%%
% Sideslip derivative
C_Ybeta = -0.83;
55
% Roll control derivative

C_YdeltaA = -0.075;
% Yaw control derivative
C_YdeltaR = 0.1914;
% Roll rate derivative
C_YP = 0;
% Yaw rate derivative
C_YR = 0;
%%% Pitch moment coefficient %%%
% Zero-alpha pitch
C_m0 = 0.135;
% alpha derivative
C_malpha = -2.7397;
% Pitch control derivative
C_mdeltaE = -0.9918;
% alpha_dot derivative
%C_malphaDot = -10.3796;
C_mQ = -38.2067;
%%% Roll moment coefficient %%%
C_lbeta = -0.13;
C_ldeltaA = -0.1695;
C_ldeltaR = 0.0024;
C_lP = -0.5051;
C_lR = 0.2519;
%%% Yaw moment coefficient %%%
C_nbeta = 0.0726;
C_ndeltaA = 0.0108;
C_ndeltaR = -0.0693;
C_nP = -0.069;
56
C_nR = -0.0946;
mass = 13.5; % kg (my estimate)
Weight=mass*g;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Thrust = 10;
% Trim 21.4066
airframeProps = [mass*9.81,S,C_D0,e,AR,C_Lalpha,C_L0,Thrust];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Model for air density
rho = 1.121;
% Inertia Matrix
InertiaMatrix = [Jx , 0 , -Jxz; 0, Jy , 0; -Jxz , 0 , Jz];
% Rotation from tangent plane to body frame
C_bn = [ cos(psi)*cos(theta) , sin(psi)*cos(theta)...
, -sin(theta); -sin(psi)*cos(phi)+cos(psi)*...
sin(theta)*sin(phi) , cos(psi)*cos(phi)...
+sin(psi)*sin(theta)*sin(phi) , cos(theta)*sin(phi) ;...
sin(psi)*sin(phi) + cos(psi)*sin(theta)*cos(phi), ...
-cos(psi)*sin(phi) + sin(psi)*sin(theta)*cos(phi) , ...
cos(theta)*cos(phi) ];
% Rotation from body frame to tangent plane
C_nb = C_bn;
% Cross Product Matrix for omega x ...
OmegaCrossProdMatrix = [0 , -R , Q; R , 0 , -P ; -Q, P, 0];
% Calculate relative velocities (w.r.t moving atmosphere (wind) )
[c1,Rb2e,c2] = calcRotationMatrices(psi,theta,phi,0,0,0,0);
primes = [U ; V ; W] - Rb2e*[W_N ; W_E ; W_D];
Uprime = primes(1);
Vprime = primes(2);
Wprime = primes(3);
57
% Calculate Airspeed, dynamic pressure, aerodynamic angles

V_T = norm([Uprime; Vprime ; Wprime]);
q = .5*rho*(V_T)^2;
positionDots = Rb2e*[U ; V ; W ];
Xdot = positionDots(1);
Ydot = positionDots(2);
Zdot = positionDots(3);
V_in = sqrt(Xdot^2+Ydot^2+Zdot^2);
gam = atan2(-Zdot,sqrt(Xdot^2+Ydot^2));
chi = atan2(Ydot,Xdot);
aoa = atan2(Wprime,Uprime);
bet = asin(Vprime/V_T);
[Re2p,Rb2e,Rw2b] = calcRotationMatrices(psi,theta,...
phi,bet,aoa,chi,gam);
% Calculate Aerodynamic Forces
C_L = C_L0 + C_Lalpha*aoa;
Lift = q*S*C_L;
% Better model later...
C_D = C_D0 + (C_L-C_L0)^2/(pi*AR*e); % Better model later...

Drag = q*S*C_D;
%% Beard, et al. Vector Field Path Following

[chi_d] = pathFollowing(X,Y,chi,V_T,S);
%% My Controller
[phi_desired,theta_desired,h_d,k1,k2] = ...
controllerPIDinnerLoops(t,state,airframeProps,Lift,Drag,chi_d);
%% PID CONTROLLER FOR INNER LOOPS %%%%%%%%%%%%

pitchError = theta-theta_desired;
pitchErrorDerivative = cos(phi)*Q-sin(phi)*R;
pitchErrorIntegral = state(22);
pitchErrorIntegralDot = pitchError;
bankError = phi-phi_desired;
bankErrorDerivative = P+tan(theta)*sin(phi)*Q+tan(theta)*cos(phi)*R;
58
bankErrorIntegral = state(23);
bankErrorIntegralDot = bankError;
% Simulate PID inner loops for att. stabilization
[deltaA,deltaE,deltaR] = attitudeStabilization(t,state,...
bankError,bankErrorDerivative,bankErrorIntegral,pitchError,...
pitchErrorDerivative,pitchErrorIntegral);
C_Y = C_Ybeta*bet + C_YdeltaR*deltaR + C_YdeltaA*deltaA +...
b/(2*V_T)*(C_YP*P + C_YR*R);
Sideforce = q*S*C_Y;
% Calculate Aerodynamic Moments
% Rolling Moment
C_l = C_lbeta*bet + C_ldeltaA*deltaA + C_ldeltaR*deltaR +...
b/(2*V_T)*(C_lP*P+C_lR*R);
l = q*S*b*C_l;
% Pitching Moment
C_m = C_m0 + C_malpha*aoa + C_mdeltaE*deltaE+ ...
c/(2*V_T)*(C_mQ*Q); % C_malphaDot*alphaDot
m = q*S*c*C_m;
% Yawing Moment
C_n = C_nbeta*bet + C_ndeltaA*deltaA + C_ndeltaR*deltaR +...
b/(2*V_T)*(C_nR*R+C_nP*P);
n = q*S*b*C_n;
% Begin State Equations:
positionDots = Rb2e*[U ; V ; W ];
% phidot ; thetadot ; psidot
eulerdots = [1 , tan(theta)*sin(phi) , tan(theta)*cos(phi) ;...
0, cos(phi) , -sin(phi);...
0, sin(phi)/cos(theta) , cos(phi)/cos(theta)]*[P;Q;R];
psidot = eulerdots(3);
thetadot = eulerdots(2);
phidot = eulerdots(1);
59
%Udot ; Vdot ; Wdot

velocitydots = (1/mass)*(Rw2b*[-Drag;-Sideforce;-Lift] +...
[Thrust ;0 ;0]) + Rb2e*[0;0;9.81]...
- OmegaCrossProdMatrix*[U;V;W];
% Pdot ; Qdot ; Rdot
angularRatedots = inv(InertiaMatrix)*...
(Rw2b*[ l ; m ; n ]-OmegaCrossProdMatrix*...
InertiaMatrix*[P ; Q ; R]);
%keyboard;
Pdot = angularRatedots(1);
Qdot = angularRatedots(2);
Rdot = angularRatedots(3);
% Path calculations for debugging
Drag_pathAxes = Re2p*Rb2e*Rw2b*[-Drag;0;0];
Lift_pathAxes = Re2p*Rb2e*Rw2b*[0;0;-Lift];
Sideforce_pathAxes = Re2p*Rb2e*Rw2b*[0;-Sideforce;0];
Thrust_pathAxes = Re2p*Rb2e*[Thrust;0;0];
F_pathAxes = Drag_pathAxes+...
Lift_pathAxes+Thrust_pathAxes+Sideforce_pathAxes;
mg_pathAxes=Re2p*[0;0;mass*g];
gamDot = -1/(mass*V_in)*(F_pathAxes(3)+mg_pathAxes(3));
Vdot=g*(Thrust/(mass*g) -Drag/(mass*g) -sin(gam));
chiDot = 1/(mass*V_in*cos(gam))*...
(F_pathAxes(2)+mg_pathAxes(2));
% Observer Formulation
epsilon1 = 0.05;
epsilonGamma = 1/epsilon1;
epsilonH = 1/epsilon1;
gamHatDot = -((-Lift*cos(phi)*sin(theta) +...
(Thrust-Drag)*sin(theta))*gamHat - ...
(Lift*cos(phi)*cos(theta) +...
(Thrust-Drag)*sin(phi)-mass*g ))/(mass*V_in) ...
+ epsilonGamma*(h-hHat);
hHatDot = V_in*gamHat + epsilonH*(h-hHat);
stateDots = [Xdot ; Ydot ; Zdot ; phidot; thetadot ; psidot ;...
velocitydots ; Pdot ; Qdot ; Rdot ; 0 ; 0 ; 0 ; gamDot; ...
chiDot;Vdot; h2Dot;x2Dot; y2Dot; pitchErrorIntegralDot; ...
bankErrorIntegralDot ; gamHatDot ; hHatDot; Vdot; ...
chiDot; gamDot];
60
Subroutines
These subroutines perform repeated tasks and must also be included in the local
simulation directory.
function [chiC] = pathFollowing(x,y,chi,V_T,S)
% Path Following subroutine
% %Design parameters%
tau = 35;
% set transition boundary distance
% on either side of path
xe=pi/5;
% set entry angle heading
k=2;
% Transition Gain
deltaS = 1.0; % Waypoint Switching look-ahead
a = 1;
g = 9.81;
cnt=0;
% initialize iterative counting index
z = [x;y];
d = getpts;
% grabs waypoints from data file
% Check to see if current position is past any waypoints
sprime=5;
% Make first condition check true
while sprime>deltaS
cnt=cnt+1;
sprime = ( ((z-d(1:2,cnt)))*( d(1:2,(cnt+1))-...
d(1:2,cnt)))./ ((norm( d(1:2,(cnt+1))-d(1:2,cnt)))^2);
end
sprime = ( ((z-d(1:2,cnt)))*( d(1:2,(cnt+1))-d(1:2,cnt)))...
./ ((norm( d(1:2,(cnt+1))-d(1:2,cnt)))^2);
% Calculate current waypoint ground track heading
xf = atan2( (d(2,cnt+1)-d(2,cnt)),(d(1,cnt+1)-d(1,cnt)));
% Calculate error from path
epsilon = norm(z - ((sprime*( d(1:2,(cnt+1))-d(1:2,cnt)))+...
d(1:2,cnt)));
% Assign variable names to the currently applicable waypoints
% w1 and w2 are 3 dimensional vectors to use cross function
w1= [d(1:2,cnt);0];
w2= [d(1:2,cnt+1);0];
znow = [z;0]; % add third dimension to current position
61
% Check to see which side of path the vehicle is on

% p is either +1 or -1
p = sign ( cross( (w2-w1), (znow-w1) ) );
p = p(3,1);
% Give error a sign
epsilon = p*epsilon;
% Calculate Commanded Heading Angle
if abs(epsilon)>tau % Position is far away
xd = xf -(p*xe);
xc = xd;
else
% Position is inside transition zone
xd = xf - p*xe*((epsilon/tau)^k);
xc = xd - (k*xe*S/(a*tau^k))*(epsilon^(k-1))*sin(chi);
end
chiC = xc;
% Remove waypoints already passed, write new waypoint file
% not removing most recently passed waypoint
dnew = d(1:2,cnt:length(d));
writepts(dnew);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [deltaA,deltaE,deltaR] = ...
attitudeStabilization(t,state,bankError,...
bankErrorDerivative,bankErrorIntegral,pitchError,...
pitchErrorDerivative,pitchErrorIntegral)
% Simulates PID loops for attitude stabilization
psi = state(6);
theta = state(5);
phi = state(4);
K_thetaP
K_thetaI
K_thetaD
K_phiP =
K_phiI =
K_phiD =
= 10;
= 1;
= 1;
1;
.5;
0;
62
deltaA = K_phiP*bankError + K_phiI*bankErrorIntegral + ...

K_phiD*bankErrorDerivative;
deltaE = (K_thetaP*pitchError+K_thetaI*pitchErrorIntegral+...
K_thetaD*pitchErrorDerivative);%Trim -3deg @.00682,,27.88
deltaR = 0; % Fixed Rudder
% Saturation
if deltaE > 60*pi/180
deltaE = 60*pi/180;
% fprintf(Sat Elev. Pos.\n)
elseif deltaE < -60*pi/180
deltaE = -60*pi/180;
% fprintf(Sat Elev. Neg.\n)
end
if deltaA > 45*pi/180
deltaA = 45*pi/180;
%fprintf(Sat Ail. Pos.\n)
elseif deltaA < -45*pi/180
deltaA = -45*pi/180;
%fprintf(Sat Ail. Neg.\n)
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [bank,pitch,h_d,k1,k2] = ...
autopilot(t,state,airframeProps,Lift,Drag,chi_d)
% Feedback Linearization Autopilot Algorithms
%% Nice names for states, etc.
X = state(1); Y = state(2); Z = state(3); phi = state(4);
theta=state(5);psi = state(6); U = state(7); V = state(8);
W = state(9); P = state(10); Q = state(11); R = state(12);
W_N = state(13); W_E = state(14); W_D = state(15); g=9.81;
rho = 1.121; h = 0-Z;Weight = airframeProps(1);
S = airframeProps(2); CD_0 = airframeProps(3);
e = airframeProps(4); AR= airframeProps(5);
CL_alpha = airframeProps(6);CL_0= airframeProps(7);
Thrust = airframeProps(8); mass=Weight/g;
h_d = 200; % Altitude Setpoint
63
% Design parameters
aChi=.465;
k1=10;
k2=10;
% Calculate Airspeed, dynamic pressure, aerodynamic angles
C_bn = [ cos(psi)*cos(theta) , sin(psi)*cos(theta) , ...
-sin(theta); -sin(psi)*cos(phi)+cos(psi)...
*sin(theta)*sin(phi) , ...
cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi) , ...
cos(theta)*sin(phi) ; sin(psi)*sin(phi) + ...
cos(psi)*sin(theta)*cos(phi), -cos(psi)*sin(phi) +...
sin(psi)*sin(theta)*cos(phi) , cos(theta)*cos(phi) ];
C_nb = C_bn;
primes = [U ; V ; W] - C_bn*[W_N ; W_E ; W_D];
Uprime = primes(1);Vprime = primes(2);Wprime = primes(3);
V_T = norm([Uprime; Vprime ; Wprime]);
q = .5*rho*(V_T)^2;
aoa = atan2(Wprime,Uprime);
bet = asin(Vprime/V_T);
% Calculate Gamma: (or grab estimate)
positionDots = C_nb*[U ; V ; W ];
%Xdot = positionDots(1); Ydot = positionDots(2);
%Zdot = positionDots(3);
%V_in = sqrt(Xdot^2+Ydot^2+Zdot^2);
%gam = atan2(-Zdot,sqrt(Xdot^2+Ydot^2));
gam = state(24); % Estimated
chi = atan2(Ydot,Xdot); % Measurement
%% Measured Airspeed
V_a = Uprime; % w.r.t. rel. wind. & measured along body x axis
x1 = h-h_d;
x2=V_in*sin(gam);
v = -k1*x1-k2*x2;
% Wrap the difference mod. 2pi
dif = (chi-chi_d);
if dif < -pi
dif = dif + 2*pi;
else if dif > pi
dif = dif-2*pi;
end
64
end
Thrust_est = Thrust;
Drag_est = Drag;
Lift_est = Lift;
% Exact solution if it exists

bank = (asin( (-aChi*(chi-chi_d)*mass*V_in*cos(gam))...
/Lift_est));
% Saturate
if bank > maxPhi
bank = maxPhi;
elseif bank<-maxPhi
bank = -maxPhi;
end
A1 = Thrust_est - Drag_est*cos(aoa)+Lift_est*sin(aoa);
B1 = Drag_est*sin(aoa)*cos(phi)+Lift_est*cos(aoa)*cos(phi);
% Linearization Solution:
pitch = (mass*v+mass*g-B1)/A1;
% Find exact solution if it exists
if (sqrt(A1^2+B1^2)>=abs( mass*v + mass*g))
theta0 = asin(B1/sqrt(A1^2+B1^2));
if A1<0
theta0=theta0+pi;
end
tcom = asin((mass*v +mass*g)/sqrt(A1^2+B1^2))-theta0;
tquad = (-A1 + sqrt(A1^2+2*B1*(B1-mass*(v+g))))/-B1;
pitch = tcom;
else
end
if pitch>pi/14
pitch = pi/14;
elseif pitch <-pi/14
pitch = -pi/14;
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
65
function [X,Y,Z,phi,theta,psi,U,V,W,P,Q,R,W_N,W_E,W_D,...
gammaCalc,VCalc,h2,x2,y2,gamHat,hHat,chiCalc,...
gamCalc,V_T,q,aoa,bet,gam,chi,V_in,...
Lift,Drag,Sideforce,A,B,C,v,phi_desired,...
theta_desired,aoaEst,...
LiftEst,DragEst,chi_d]= calcOutputs(t,state)
% Calculate Ouputs
% Nice Names again:
X = state(:,1); Y = state(:,2); Z = state(:,3);
phi = state(:,4); theta = state(:,5);psi = state(:,6);
U = state(:,7); V = state(:,8); W = state(:,9);
P = state(:,10); Q = state(:,11); R = state(:,12);
W_N = state(:,13); W_E = state(:,14); W_D = state(:,15);
gammaCalc=state(:,16); chiCalc=state(:,17);
VCalc = state(:,18);h2 = state(:,19); x2 = state(:,20);
y2=state(:,21);gamHat = state(:,24);hHat = state(:,25);
Vcalc = state(:,26); chiCalc = state(:,27);
gamCalc = state(:,28);pitchErrorIntegral = state(:,22);
bankErrorIntegral = state(:,23);
h = -Z;
%Initialize for calcuation
V_T = zeros(size(X)); q = zeros(size(X));
bet = zeros(size(X)); gam = zeros(size(X));
V_in = zeros(size(X)); rho = 1.121;
Lift = zeros(size(X)); Drag = zeros(size(X));
phi_desired = zeros(size(X)); theta_desired = zeros(size(X));
deltaA = zeros(size(X)); deltaE = zeros(size(X));
aoa = zeros(size(X)); chi = zeros(size(X));
Sideforce = zeros(size(X));deltaR = zeros(size(X));
% Airframe Props
% Wing span
b = 2.8956; % m
% Wing area
S = 0.55; % m^2
AR = b^2/S;
c = .189941;
% Gross Moments of Inertia kg*m^2
Jx = .8244;
Jy = 1.135;
Jz = 1.759;
Jxz = .1204;
66
% ALL aerodynamics derivatives are per radian:

%%% Lift coefficient %%%
% Zero-alpha lift
C_L0 = 0.23;
% alpha derivative
C_Lalpha = 5.6106;
C_LdeltaE = 0.13;
% alpha-dot derivative
C_Lalphadot = 1.9724;
C_LQ = 7.9543;
%%% Drag coefficient %%%
% Lift at minimum drag
CLmind = 0.23;
% Minimum drag
C_D0 = 0.0434;
C_DdeltaE = 0.0135;
% Roll control (aileron) derivative
C_DdeltaA = 0.0302;
% Yaw control (rudder) derivative
C_DdeltaR = 0.0303;
% Oswalds coefficient
e = 0.75;
%%% Side force coefficient %%%
C_Ybeta = -0.83;
C_YdeltaA = -0.075;
C_YdeltaR = 0.1914;
C_YP = 0;
C_YR = 0;
%%% Pitch moment coefficient %%%
% Zero-alpha pitch
67
C_m0 = 0.135;
% alpha derivative
C_malpha = -2.7397;
% Pitch control derivative
C_mdeltaE = -0.9918;
% alpha_dot derivative
%C_malphaDot = -10.3796;
C_mQ = -38.2067;
%%% Roll moment coefficient %%%
C_lbeta = -0.13;
C_ldeltaA = -0.1695;
C_ldeltaR = 0.0024;
C_lP = -0.5051;
C_lR = 0.2519;
%%% Yaw moment coefficient %%%
C_nbeta = 0.0726;
C_ndeltaA = 0.0108;
C_ndeltaR = -0.0693;
C_nP = -0.069;
C_nR = -0.0946;
g = 9.81;
mass = 13.5; % kg (my estimate)
Weight=mass*g;
Thrust = 10;
% Trim 21.4066
[numRows,numCols] = size(state);
airframeProps = [mass*9.81,S,C_D0,e,AR,C_Lalpha,C_L0,Thrust];
A = zeros(size(X)); B = zeros(size(X)); h_d = zeros(size(X));
v=zeros(size(X));
Thrust = 21.4066*ones(size(X));
aoaEst=zeros(size(X)); LiftEst=zeros(size(X)); ...
68
DragEst = zeros(size(X));
chi_d=zeros(size(X));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
counter1=0;
fprintf(Calculating Outputs: )
waypointgen;
for i = 1:length(X)
C_bn = [ cos(psi(i))*cos(theta(i)) , sin(psi(i))...
*cos(theta(i)) ,-sin(theta(i)); -sin(psi(i))*...
cos(phi(i))+cos(psi(i))*sin(theta(i))...
*sin(phi(i)) , cos(psi(i))*cos(phi(i))+...
sin(psi(i))*sin(theta(i))*sin(phi(i)) ,...
cos(theta(i))*sin(phi(i)) ; sin(psi(i))...
*sin(phi(i)) + cos(psi(i))*sin(theta(i))...
*cos(phi(i)), -cos(psi(i))*sin(phi(i))...
+ sin(psi(i))*sin(theta(i))*cos(phi(i)) ,...
cos(theta(i))*cos(phi(i)) ];
C_nb = C_bn;
primes = [U(i) ; V(i) ; W(i)] - C_bn*[W_N(i) ...
; W_E(i) ; W_D(i)];
Uprime = primes(1);
Vprime = primes(2);
Wprime = primes(3);
V_T(i) = norm([Uprime; Vprime ; Wprime]);
q(i) = .5*rho*(V_T(i))^2;
aoa(i) = atan2(Wprime,Uprime);
bet(i) = asin(Vprime/V_T(i));
positionDots = C_nb*[U(i) ; V(i) ; W(i) ];
gam(i) = atan2(-Zdot,sqrt(Xdot^2+Ydot^2));
chi(i) = atan2(Ydot,Xdot);
V_in(i) = sqrt(Zdot^2+Xdot^2+Ydot^2);
C_L = C_L0 + C_Lalpha*aoa(i);
Lift(i) = q(i)*S*C_L;
C_D = C_D0 + (C_L-C_L0)^2/(pi*AR*e);
Drag(i) = q(i)*S*C_D;
chi_d(i)=trajectoryGenerator(X(i),Y(i),chi(i),V_T(i),S);
[phi_desired(i),theta_desired(i),h_d(i),k1,k2] = ...
autopilot(t(i),state(i,:),...
airframeProps,Lift(i),Drag(i),chi_d(i));
pitchError = theta(i)-theta_desired(i);
69
pitchErrorDerivative = cos(phi(i))...
*Q(i)-sin(phi(i))*R(i);
bankError = phi(i)-phi_desired(i);
bankErrorDerivative = P(i)+...
tan(theta(i))*sin(phi(i))*Q(i)...
+tan(theta(i))*cos(phi(i))*R(i);
[deltaA(i),deltaE(i),deltaR(i)] = ...
attitudeStabilization(t(i),state(i,:),bankError,...
bankErrorDerivative,bankErrorIntegral(i),...
pitchError, pitchErrorDerivative,...
pitchErrorIntegral(i));
C_Y = C_Ybeta*bet(i) + C_YdeltaR*deltaR(i) +...
C_YdeltaA*deltaA(i)...
+ b/(2*V_T(i))*(C_YP*P(i) + C_YR*R(i));
Sideforce(i) = q(i)*S*C_Y;
A(i) = Thrust(i) - Drag(i)*cos(aoa(i))+Lift(i)*sin(aoa(i));
B(i) = Drag(i)*sin(aoa(i))*cos(phi(i))+...
Lift(i)*cos(aoa(i))*cos(phi(i));
v(i) = -k1*(h(i)-h_d(i)) - k2*(V_in(i)*sin(gam(i)));
C(i) = mass*v(i)+mass*g;
aoaEst(i) = asin(-V_in(i)*sin(gam(i))/...
(V_T(i)*sqrt(cos(phi(i))^2*...
cos(theta(i))^2+sin(theta(i))^2))-...
asin(-sin(theta(i)/sqrt(cos(phi(i))^2....
*cos(theta(i))^2+sin(theta(i))))));
C_LEst = C_L0 + C_Lalpha*aoaEst(i);
LiftEst(i) = q(i)*S*C_LEst;
C_DEst = C_D0 + (C_LEst-C_L0)^2/(pi*AR*e);
DragEst(i) = q(i)*S*C_DEst;
if ((i>numRows/10)&(counter1<1))
fprintf(10%%..)
counter1 = 1;
elseif ((i>numRows/5)&(counter1<2))
fprintf(20%%..)
counter1 = 2;
elseif ((i>numRows/2)&(counter1<5))
fprintf(50%%..)
counter1 = 5;
elseif((i>numRows*.8)&(counter1<8))
fprintf(80%%..\n)
70
counter1 = 8;
end
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Re2p,Rb2e,Rw2b] = ...
calcRotationMatrices(psi,theta,phi,bet,aoa,chi,gam)
% Calcuate Rotation Matrices
Rpsi = rotate3(psi);
Rtheta=rotate2(theta);
Rphi=rotate1(phi);
Re2b=Rphi*Rtheta*Rpsi;
Rb2e = Re2b;
Rbet=rotate3(bet);
Raoa=rotate2(aoa);
Rb2w = Raoa*Rbet;
Rb2w = [ cos(aoa)*cos(bet), sin(bet), sin(aoa)*cos(bet); ...
-cos(aoa)*sin(bet),cos(bet),-sin(aoa)*sin(bet);...
-sin(aoa),0,cos(aoa)];
Rw2b = Rb2w;
Rchi = rotate3(chi);
Rgam = rotate2(gam);
Re2p = Rgam*Rchi;
end
function rotMat3=rotate3(ang)
rotMat3 = [cos(ang),sin(ang),0; -sin(ang),cos(ang),0;0,0,1];
end
function rotMat2=rotate2(ang)
rotMat2 = [cos(ang),0,-sin(ang);0,1,0;sin(ang),0,cos(ang)];
end
function rotMat1 = rotate1(ang)
rotMat1 = [1,0,0;0,cos(ang),sin(ang);0,-sin(ang),cos(ang)];
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function genpts(x,y)
% Waypoint File Generation
if size(y)~=size(x)
error(size of x and y differ);
end
fout = fopen(waypoints.dat,w);
71
for k=1:length(x);
fprintf(fout, %f %f\n,x(k),y(k));
end
fclose(fout);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function d = getpts
% Waypoint file grabbing
fin = fopen(waypoints.dat,r);
d = fscanf(fin,%f);
fclose(fin);
l = length(d);
d = reshape(d,[2 (l/2)]);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function writepts(d)
% Write to waypoint file
fout = fopen(waypoints.dat,w);
ld=length(d);
for k=1:ld;
fprintf(fout, %f %f\n,d(1,k),d(2,k));
end
fclose(fout);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Waypoint Generation Script:
x1=4*[0 200
400 400 200 200];
y1=4*[0 0 0 200 200 0]
genpts(x1,y1);
fprintf(Waypoint Pairs Generated: %i.\n,length(x1));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
72
Appendix C
C Code
This appendix contains the C code used to program the flight computer for the
Hardware in the Loop simulations. The code was compiled and loaded on the the
MSP430F149 using the IAR Embedded Workbench IDE.
// HIL Simulation Code
#include<msp430x14x.h>
#include<stdlib.h>
#include<string.h>
#include<stdio.h>
#include<math.h>
#define SIZECHARS 6
// 6 for at least one decimal digit, including neg. numbers
// UDF Prototypes:
void configUART(void);
void configSPI1(void);
void resetSlave(void);
void sendOutput2Ground(void);
void parseGPS(void);
void constructOutput(void);
double deg2rad(double);
double rad2deg(double);
void addADCtoOutput(void);
void itoa(unsigned int n, char s[]);
void clearStr(char st[]);
void configADCwithTimerA(void);
void sendSlaveAnalogPacket(void);
void constructAnalogPacket(void);
void convertFloat2String(float num, char s[]);
void addCommandstoOutput(void);
void controller(void);
float saturate(float num, float max);
void voltageOut(float, int);
73
void actuatePitch(float);
void actuateBank(float);
void delay(int);
void geodecCurrent2currentECEFr(void);
void currentECEFr2localENU(void);
void geodecOrigin2originECEFr(void);
float vectorField(void);
void initWaypoints(void);
int numWypts;
// Global Variables
unsigned int q;
int i;
char c;
int firstRun;
char output[98]; //92;
char chPitchDeg[SIZECHARS];
char chRollDeg[SIZECHARS];
/////// GPS Variables ////////
int GPSreceive;
char sentenceType[5];
char check = F;
char GGAsentence[68];
char VTGsentence[68];
int count = 0;
int it;
// ADC Variables //////
int rollADC=15678;
int pitchADC=333;
int zAxisADC=1;
int pressureAlt;
char s[5];
int jk=0;
int analogReady;
int calibrationZaxis;
int calibrationZarray[5];
// Struct Type Definitions
struct gpsChars
{
char chLatDeg[3];
char chLongDeg[4];
char chLongMin[8];
74
char chLatMin[8];
char chTime[7];
char latDir;
char longDir;
char chTrueCourse[4];
char chGrdSpeed[7];
char chHeightAMSL[8]; //meters above MSL
};
struct gpsData
{
int latitudeDeg;
int longitudeDeg;
float latitudeMin;
float longitudeMin;
float latitude;
float longitude;
float trueCourse;
float grdSpeed;
float heightAMSL;
unsigned long int gpsTime;
};
struct geodecticCoord
{
float altitude; //meters
float lambda;
// RADIANS !!!
float phi;
// RADIANS!!!
};
struct ECEFr
{
float Norm;
float x;
float y;
float z;
};
struct enu
{
float e;
float n;
float u;
};
struct attCommand
{
75
float pitch;
float roll;
};
struct northEastWaypoint
{
float north;
float east;
};
//// More Globals
static char airspeedTest[]={"12.345"};
struct gpsChars c_chars;
struct gpsData current;
struct northEastWaypoint NEwaypoint[15];
int currWpt=0;
struct geodecticCoord geodecOrigin;
struct geodecticCoord geodecCurrent;
struct attCommand toFMA;
struct ECEFr originECEFr;
struct ECEFr currentECEFr;
struct enu localENU;
float PI=3.1415;
float altErrorIntegral=0.0;
float lastAltError=0.0;
float altError;
void main(void)
{
WDTCTL = WDTPW + WDTHOLD; // Stop watchdog
volatile unsigned int i;
geodecOrigin.altitude = 0.0;
geodecOrigin.lambda = 33.5647*PI/180.0; // Hardcode
geodecOrigin.phi = 101.878*PI/180.0;
// Hardcode
initWaypoints();
geodecOrigin2originECEFr();
firstRun=1;
//enable first run boolean
GPSreceive = 0;
analogReady=0;
P1OUT = 0x03;
// P1.0 setup for LED output,
P1DIR |= 0x03;
// Enable P1.0, P1.1 high
76
P1OUT = 0x01;
// Drop P1.1 low for slave reset
for (i = 0xAFF; i > 0; i--);
P1OUT = 0x03;
// Raise P1.1 back high to enable slave
configUART();
// Setup UART for debugging purposes
configADCwithTimerA();
_EINT();
IE1 |= URXIE0; // enable GPS receiving
configSPI1();
while(1)
{
if (GPSreceive==1)
{
sendOutput2Ground();//TransmitGPS sentence
GPSreceive =0; // reset boolean
}
}
}
#pragma vector=UART0RX_VECTOR // UART Rx interrupt (GPS)

__interrupt void usart0_rx (void)
{
if(RXBUF0 == $)
{
check = T;
count = 0;
it=0;
}
else if(check == T)
{
if(it<5)
// Finish filling in sentenceType
{
sentenceType[it]=RXBUF0;
it = it+1;
}
else if((sentenceType[2]==G)|(sentenceType[3]==G)...
|(sentenceType[4]==A))
{ GGAsentence[5+count++] = RXBUF0;
if(RXBUF0 == *)
{
77
check = F;
_BIS_SR(LPM3_EXIT);
}
}
else if((sentenceType[2]==V)|(sentenceType[3]==T)...
|(sentenceType[4]==G))
{
VTGsentence[5+count++] = RXBUF0;
if(RXBUF0==*)
{
it = 0;
P1OUT=0x00; // Turn LED on
parseGPS();
constructOutput();
addADCtoOutput();
controller();
GPSreceive=1;
P1OUT=0x01; // Turn LED off
}
}
}
}
void addADCtoOutput(void)
{
char o;
itoa(rollADC,s);
for(o = 0; o < 5; o++)
{ output[55-o]=s[o];}
itoa(pitchADC,s);
for(o = 0; o < 5; o++)
itoa(zAxisADC,s);
for(o = 0; o < 5; o++)
itoa(pressureAlt,s);
for(o = 0; o < 5; o++)
78
void configUART(void)
{
UCTL0 |= SWRST;
P3SEL |= 0x30;
ME1 |= URXE0 + UTXE0;
UCTL0 |= CHAR;
UTCTL0 |= SSEL0;
UBR00 = 0x03;
UBR10 = 0x00;
UMCTL0 = 0x4A;
UCTL0 &= ~SWRST;
}
// = USART0 TXD/RXD
//Enable USART1 RXD/TXD
//8-bit character
// UCLK = ACLK
// 32k/9600 - 3.41
//
// Modulation
// Set
void resetSlave(void)
{
P1OUT &= ~0x02;
//for (i = 0xFF; i > 0; i--);
P1OUT |=0x02;
}
void constructOutput(void)
{
int o;
// Construct Output for GPS data TX to Grd Station
for(o = 0; o < 2; o++)
{ output[o+2]=c_chars.chLatDeg[o]; }
for(o = 0; o < 7; o++)
{ output[o+5]=c_chars.chLatMin[o]; }
for(o = 0; o < 3; o++)
{ output[o+13]=c_chars.chLongDeg[o];}
for(o = 0; o < 7; o++)
{ output[17+o]=c_chars.chLongMin[o];}
for(o = 0; o < 7; o++)
{output[25+o]=c_chars.chHeightAMSL[o];}
for(o = 0; o < 6; o++)
{ output[33+o]=airspeedTest[o]; }
for(o = 0; o < 3; o++)
{ output[40+o]=c_chars.chTrueCourse[o];}
79
for(o = 0; o < 6; o++)

{ output[44+o]=c_chars.chGrdSpeed[o];}
for(o=0; o < 6; o++)
{ output[69+o]=c_chars.chTime[o];}
}
void parseGPS(void)
{
int j;
char ch1;
for(i = 0; i < 3; i++)
{
c_chars.chLatDeg[i] = GGAsentence[13+i];
}
for(i = 0; i < 4; i++)
{
c_chars.chLongDeg[i] = GGAsentence[25+i];
}
for(i = 0; i < 8; i++)
{
c_chars.chLongMin[i] = GGAsentence[28+i];
c_chars.chLatMin[i] = GGAsentence[15+i];
}
for (i =0; i<6; i++)
{
c_chars.chTime[i] = GGAsentence[6+i];
}
i=43; //Beginning of dilution of precision
ch1 = GGAsentence[i];
while (ch1!=,) // Go through Dilution of Precision
{
ch1 = GGAsentence[i++];
}
ch1 = GGAsentence[i++];
j=0;
while(ch1!=,) // Collect True Course
{
c_chars.chHeightAMSL[j] = ch1;
ch1=GGAsentence[i++];
j = j+1;
}
ch1=VTGsentence[i++]; //Skip Comma
80
i = 0;
j = 0;
ch1= VTGsentence[i];
////////
while(ch1!=,) // Go through $GPVTG
{
ch1=VTGsentence[i++]; // Go to next character
}
j=0;
///////
while(ch1!=,) // Collect True Course
{
c_chars.chTrueCourse[j] = ch1;
ch1=VTGsentence[i++];
j = j+1;
}
j=0;
///////
while(ch1!=,) // Go through T
{
}
j=0;
/////
while(ch1!=,) // Collect Magnetic Heading
{
//c_chars.magneticHeading = ch1;
}
j=0;
///////
while(ch1!=,) // Go through M
{
}
j=0;
//////
81
while(ch1!=,) // Collect groundspeed (knots)

{
//Collect here
j = j+1;
}
j=0;
/////
{
}
j=0;
/////
while(ch1!=,) // Collect groundspeed (km/h)
{
c_chars.chGrdSpeed[j]=ch1;
j = j+1;
}
/////
// Add \0 char to signal end of string
c_chars.chLatDeg[2] = \0;
c_chars.chLongDeg[3] = \0;
c_chars.chLatMin[7] = \0;
c_chars.chLongMin[7] = \0;
c_chars.chTime[6] = \0;
c_chars.chTrueCourse[3] = \0;
c_chars.chGrdSpeed[6] = \0;
c_chars.chHeightAMSL[7]=\0;
current.latitudeDeg = atoi(c_chars.chLatDeg);
current.longitudeDeg = atoi(c_chars.chLongDeg);
current.latitudeMin = atof(c_chars.chLatMin);
current.longitudeMin = atof(c_chars.chLongMin);
current.gpsTime = atol(c_chars.chTime);
current.trueCourse = atof(c_chars.chTrueCourse);
current.grdSpeed = atof(c_chars.chGrdSpeed);
current.heightAMSL = atof(c_chars.chHeightAMSL);
current.latitude = current.latitudeDeg + ...
82
(current.latitudeMin/60.0);
current.longitude = current.longitudeDeg + ...
(current.longitudeMin/60.0);
//geodecCurrent.altitude = current.altitude;
geodecCurrent.lambda = deg2rad(current.latitude);
geodecCurrent.phi
= deg2rad(current.longitude);
}
void sendOutput2Ground(void)
{
int outCounter=0;
while (outCounter<sizeof output)
{
while (!(IFG1 & UTXIFG0));
for (i = 2; i > 0; i--); //Short Delay
TXBUF0=output[outCounter];
outCounter=outCounter+1;
}
}
double deg2rad(double degrees)
{
// In Progress...
// Will convert deg to radians
double radians;
radians = degrees*PI/180;
return radians;
}
double rad2deg(double radians)
{
return radians*180/PI;
}
void itoa(unsigned int n, char st[])
{
// Convert int to string for Tx
clearStr(st);
int i;
i = 0;
do {
/* generate digits in reverse order */
st[i++] = n % 10 + 0;
/* get next digit */
83
} while ((n /= 10) > 0);
/* delete it */
}
void clearStr(char st[])
{
int i;
for(i=0;i<5;i++)
st[i]= ;
}
void convertFloat2String(float value, char s[])
{
// Convert float to string
int result;
result = snprintf(s, SIZECHARS, "%f", value);
}
#pragma vector=ADC12_VECTOR
__interrupt void ADC12ISR (void)
{
// ADC Interrupt Service Routine
if(firstRun==1)
{
calibrationZarray[jk]=ADC12MEM3;
jk=jk+1;
if(jk==4)
{
calibrationZaxis = ...
(calibrationZarray[0]+calibrationZarray[1]+...
calibrationZarray[2]+calibrationZarray[3])/4;
itoa(calibrationZaxis,s);
// Tx Cal. packet to Ground:
TXBUF0=C;
TXBUF0=,;
TXBUF0=s[4];
TXBUF0=s[3];
TXBUF0=s[2];
84
TXBUF0=s[1];
TXBUF0=s[0];
TXBUF0=*;
TXBUF0=\r;
TXBUF0=\n;
jk=0; // reset counter
firstRun=0;
// disable calibration run
}
ADC12CTL0 &= (~ENC); // Toggle ENC bit to allow for next
ADC12CTL0 |= ENC;
// sample (see Users Manual)
}
else
{
pressureAlt = ADC12MEM0;
rollADC = ADC12MEM1;
pitchADC = ADC12MEM2;
zAxisADC = ADC12MEM3;
jk = jk+1;
if(jk == 10)
{
//P1OUT^=0x01;
// For debugging, to check frequency
jk=0;
}
ADC12CTL0 &= (~ENC); // Toggle ENC bit to allow for next

ADC12CTL0 |= ENC;
// sample (see Users Manual)
}
}
void configADCwithTimerA(void)
{
P6SEL |=0xF;
ADC12CTL1 = SHS_1 + SHP + CONSEQ_1;
// TA trig., single seq. conv.
ADC12MCTL0 = SREF_1 + INCH_0 ;
// Channel A0, Vref+
ADC12MCTL1 = INCH_1 ;
85
// Channel A1, 3.3V pos. ref

ADC12MCTL2 = INCH_2 ;
// Channel A2, 3.3V pos. ref
ADC12MCTL3 = INCH_3 + EOS;
// Channel A3, 3.3V pos. ref, end sequence
ADC12IE = 0x08;
// Enable ADC12IFG.3
ADC12CTL0 = SHT0_8 + REF2_5V + REFON + ...
ADC12ON + MSC + ENC; // Config ADC12
TACCTL1 = OUTMOD_4;
// Toggle on EQU1 (TAR = 0)
TACCR0 = 1638;
// 1/2 of sampling rate (in ACLK ticks)
TACTL = TASSEL_1 + MC_1;
// ACLK, up-mode
}
void controller(void)
{
float kProll = 3*0.5;
float kPpitch = 0.025;//0.005;
float kIpitch = 0.003;
float headingError;
int maxBank = 45;
int maxPitch = 10;
geodecCurrent2currentECEFr();
currentECEFr2localENU();
int h_d = 992;
current.trueCourse = deg2rad(current.trueCourse);
float chi_d = vectorField();
headingError = chi_d-current.trueCourse;
if (headingError <-PI)
headingError = headingError +2*PI;
else if (headingError > PI)
headingError = headingError-2*PI;
altError = h_d-current.heightAMSL;
altErrorIntegral += lastAltError;
altErrorIntegral += ((altError-lastAltError)*0.5);
// Save last value in global lastAltError
86
//for next integral approximation

lastAltError = altError;
toFMA.roll = kProll*headingError; // Radians
toFMA.pitch = kPpitch*altError + ...
kIpitch*altErrorIntegral; // Radians
toFMA.roll = saturate(toFMA.roll,deg2rad(maxBank));
toFMA.pitch = saturate(toFMA.pitch,deg2rad(maxPitch));
actuateBank(toFMA.roll);
actuatePitch(toFMA.pitch);
convertFloat2String(rad2deg(toFMA.pitch),chPitchDeg);
convertFloat2String(rad2deg(toFMA.roll),chRollDeg);
addCommandstoOutput();
}
float saturate(float num, float max)
{
if(num>max)
num=max;
else if(num<-max)
num=-max;
return(num);
}
void addCommandstoOutput(void)
{
int o;
for(o = 0; o < 6; o++)
{ output[76+o]=chPitchDeg[o];}
for(o = 0; o < 6; o++)
{ output[83+o]=chRollDeg[o];}
}
void configSPI1(void)
{
P5SEL |= 0x0E;
// P5.1,2,3 SPI option select
P5OUT |= 0xF0;
87
P5DIR |= 0xF0;
//Hook pin 5.4 to SSelect
U1CTL = CHAR + SYNC + MM + SWRST;
// 8-bit, SPI, Master
U1TCTL = CKPL + SSEL1 + STC; // Polarity, SMCLK, 3-wire
U1BR0 = 0x02;
// SPICLK = SMCLK/2
U1BR1 = 0x00;
U1MCTL = 0x00;
ME2 |= USPIE1;
// Module enable
U1CTL &= ~SWRST; // SPI enable
}
void voltageOut(float q, int channel)

{
// Acutate DAC for voltage out
P5OUT=~0x00;
if(channel == 4)
P5OUT = ~0x10;
else if(channel ==5)
P5OUT = ~0x20;
P5OUT = ~0x40;
P5OUT = ~(0x80);
q=q+.02;
int vOut;
int MSBout;
vOut = q/3.3*4095;
MSBout = vOut>>8;
TXBUF1 = MSBout;
//BUF1 = 0xFFF-(MSBout<<8);
delay(8);
TXBUF1 = vOut;
delay(4);
P5OUT = ~0x00;
}
void delay(int q)
{
for (q = 3; q > 0; q--);
}
88
void actuatePitch(float pitch)

{ // Pitch: Positive-4, Negative - 6
float Vmax=3.0;
float Voffset = 1.65;
float VDAC = (Vmax-Voffset)*sin(pitch);
if(VDAC<0)
{
voltageOut(0,4);
voltageOut(-VDAC,6);
}
else
{
voltageOut(0,6);
voltageOut(VDAC,4);
}
}
void actuateBank(float bank)
{
float Vmax=3.0;
float Voffset = 1.65;
// Bank: Positive-5, Negative - 7
float VDAC = (Vmax-Voffset)*sin(bank);
if(VDAC<0)
{
voltageOut(0,5);
voltageOut(-VDAC,7);
}
else
{
voltageOut(0,7);
voltageOut(VDAC,5);
}
}
void geodecCurrent2currentECEFr(void)
{
float a = 6378137.0; // earth semimajor axis in meters
float f = 1/298.257223563; // reciprocal flattening
float e2 = 2*f -f*f; // eccentricity squared
float chi = sqrt(1-e2*(sin(geodecCurrent.lambda))...
*(sin(geodecCurrent.lambda)));
89
currentECEFr.x = (a/chi +current.heightAMSL)*...

cos(geodecCurrent.lambda)*cos(-geodecCurrent.phi);
currentECEFr.y = (a/chi +current.heightAMSL)...
*cos(geodecCurrent.lambda)*sin(-geodecCurrent.phi);
currentECEFr.z = (a*(1-e2)/chi + ...
current.heightAMSL)*sin(geodecCurrent.lambda);
}
void currentECEFr2localENU(void)
{
float phiP = atan2(originECEFr.z,...
sqrt(originECEFr.x*originECEFr.x ...
+ originECEFr.y*originECEFr.y));
float lambda = atan2(originECEFr.y,originECEFr.x);
localENU.e = -sin(lambda)*(currentECEFr.x-originECEFr.x)...
+ cos(lambda)*(currentECEFr.y-originECEFr.y);
localENU.n = -sin(phiP)*cos(lambda)...
*(currentECEFr.x-originECEFr.x)- sin(phiP)...
*sin(lambda)*(currentECEFr.y-originECEFr.y)...
+ cos(phiP)*(currentECEFr.z-originECEFr.z);
localENU.u = cos(phiP)*cos(lambda)*...
(currentECEFr.x-originECEFr.x)+ cos(phiP)*sin(lambda)...
*(currentECEFr.y-originECEFr.y)...
+ sin(phiP)*(currentECEFr.z-originECEFr.z);
}
void geodecOrigin2originECEFr(void)
{
float a = 6378137.0; // earth semimajor axis in meters
float f = 1/298.257223563; // reciprocal flattening
float e2 = 2*f -f*f; // eccentricity squared
float chi = sqrt(1-e2*(sin(geodecOrigin.lambda))...
*(sin(geodecOrigin.lambda)));
originECEFr.x = (a/chi +geodecOrigin.altitude)...
*cos(geodecOrigin.lambda)*cos(-geodecOrigin.phi);
originECEFr.y = (a/chi +geodecOrigin.altitude)...
*cos(geodecOrigin.lambda)*sin(-geodecOrigin.phi);
originECEFr.z = (a*(1-e2)/chi + geodecOrigin.altitude)...
*sin(geodecOrigin.lambda);
}
float vectorField(void)
90
{
// Path Following Subroutine
float p;
float sprime;
float w1x;
float w2x;
float w1y;
float w2y;
float xf;
float epsilon;
float dif;
float xd;
float crossProduct;
int tau = 20;
float xe = 45*PI/180;
int kp = 5;
w1x = NEwaypoint[currWpt].north;
w1y = NEwaypoint[currWpt].east;
w2x = NEwaypoint[currWpt+1].north;
w2y = NEwaypoint[currWpt+1].east;
// Check to see if waypoint passed
sprime = ((localENU.n-w1x)*(w2x-w1x)+(localENU.e-w1y)...
*(w2y-w1y))/((w2x-w1x)*...
(w2x-w1x)+(w2y-w1y)*(w2y-w1y));
if(sprime>0.8)
{
//while(sprime>1.0)
//{
currWpt = currWpt+1;
w1x = NEwaypoint[currWpt].north;
w1y = NEwaypoint[currWpt].east;
w2x = NEwaypoint[currWpt+1].north;
w2y = NEwaypoint[currWpt+1].east;
sprime = ((localENU.n-w1x)*(w2x-w1x)+...
(localENU.e-w1y)*(w2y-w1y))/((w2x-w1x)...
*(w2x-w1x)+(w2y-w1y)*(w2y-w1y));
//}
}
xf = atan2 ( (w2y-w1y),(w2x-w1x) );
epsilon = sqrt ( (localENU.n-sprime*(w2x-w1x)-w1x)*...
(localENU.n-sprime*(w2x-w1x)-w1x)+...
91
(localENU.e-sprime*(w2y-w1y)-w1y)...
*(localENU.e-sprime*(w2y-w1y)-w1y) );
////////////////////////////
// Obtain sign of cross product
crossProduct = (w2x-w1x)*(localENU.e-w1y)-...
(w2y-w1y)*(localENU.n-w1x);
if (crossProduct >= 0.01)
p=1.0;
else
p=-1.0;
///////////////////////////
if ( (epsilon>tau) || (epsilon<-tau))
// Outside Transition Zone
{
xd = xf-p*xe;
dif = xd-current.trueCourse;
if (dif <-PI)
dif = dif +2*PI;
else if (dif > PI)
dif = dif-2*PI;
}
else // Inside transition zone
{
xd = xf-p*xe*(pow((epsilon/tau),kp));
dif = xd-current.trueCourse;
if (dif < -PI)
dif = dif + 2*PI;
if (dif > PI)
dif = dif-2*PI;
}
return xd;
}
void initWaypoints(void)
{
// Square Pattern
NEwaypoint[0].north = 0.0;
NEwaypoint[0].east = 0.0;
92
numWypts = 3;
}
93
PERMISSION TO COPY
In presenting this thesis in partial fulfillment of the requirements for a masters
degree at Texas Tech University or Texas Tech University Health Sciences Center,
I agree that the Library and my major department shall make it freely available
for research purposes. Permission to copy this thesis for scholarly purposes may
be granted by the Director of the Library or my major professor. It is understood
that any copying or publication of this thesis for financial gain shall not be allowed
without my further written permission and that any user may be liable for copyright
infringement.
Agree (Permission is granted.)
Adam Ufford
Student Signature
5/25/2009
Date
Disagree (Permission is not granted.)
Student Signature
Date

Matlab Ufford Adam Thesis

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Matlab Ufford Adam Thesis

Uploaded by

Copyright:

Available Formats

Development and Implementation of Guidance, Navigation and Control Systems

for an Autonomous Air Vehicle

Texas Tech University, Adam Ufford, August 2009

Texas Tech University, Adam Ufford, August 2009

Vector Field Path Following . . . . . . . . . . . . . . . . .

Texas Tech University, Adam Ufford, August 2009

Path Following Variables . . . . . . . . . . . . . . . . . . . . . . . . .

Telemetry Sentence Contents . . . . . . . . . . . . . . . . . . . . . . .

Texas Tech University, Adam Ufford, August 2009

Autonomous GNC System . . . . . . . . . . . . . . . . . . . . . . . .

Earth Geoid Parameters . . . . . . . . . . . . . . . . . . . . . . . . .

PID Controller: Roll to Aileron . . . . . . . . . . . . . . . . . . . . .

Infrared Horizon Sensing: Single Axis . . .

Linear Path Segment Examples . . . . . . . . . . . . . . . . . . . . .

Two Stage Estimation Scheme . . . . . . . . . . . . . . . . . . . . .

Hardware Prototype: P,PI Autopilot . . . . . .

Texas Tech University, Adam Ufford, August 2009

Texas Tech University, Adam Ufford, August 2009

Figure 1.1 Autonomous GNC System

Figure 1.2 Subsystem Partitioning

Texas Tech University, Adam Ufford, August 2009

Texas Tech University, Adam Ufford, August 2009

Navigational Coordinate Frames

The primary position information information is provided by GPS. Since the

Earth Centered Earth Fixed Geoidal System

Texas Tech University, Adam Ufford, August 2009

Table 2.1 WGS-84 Parameters

Figure 2.1 Earth Geoid Parameters

Earth Centered Earth Fixed Rectangular System

A useful intermediate frame is the Earth Centered Earth Fixed, Rectangular

Texas Tech University, Adam Ufford, August 2009

Local Tangent Plane - enu Coordinate System

If the instantaneous position of the aircraft is reported as P = (P , P , hgP ), the

Aircraft Coordinate Frames

Traditionally, a series of right-handed coordinate systems, all centered at a point

Texas Tech University, Adam Ufford, August 2009

Figure 2.2 Aircraft Coordinate Frames

Earth (Inertial) Axes

Vehicle Navigation Axes

The Vehicle Navigation Frame is an orthogonal, right-handed (ORH) coordinate

Texas Tech University, Adam Ufford, August 2009

Texas Tech University, Adam Ufford, August 2009

transformation is represented mathematically by the matrix operation below.

Texas Tech University, Adam Ufford, August 2009

Table 2.2 Model Nomenclature

Roll Angle (radians)

Pitch Angle (radians)

Yaw Angle (radians)

Vertical Flight Path Angle (radians)

Horizontal Flight Path Angle (radians)

Angle of Attack (radians)

Sideslip Angle (radians)

Texas Tech University, Adam Ufford, August 2009

Texas Tech University, Adam Ufford, August 2009

The state equations for the 6-DOF model are as follows.

1 tan sin tan cos

Aerodynamic Forces and Moments

Texas Tech University, Adam Ufford, August 2009

Aerodynamic and Thrust Forces

The body axes components are given by

and the drag force as

+ CDe e + CDa a + CDr r

The thrust force, T , is directed along the longitudinal body axis, xb .

Texas Tech University, Adam Ufford, August 2009