# Lateral Control Implementation for an Unmanned Aerial Vehicle

Authors: R. Samar; M. Zamurad Shah; M. Nzar
Journal: ISRN Aerospace Engineering
 (2013)

---

## Abstract

This paper presents practical aspects of guidance and control design for  UAV and its flight test results. The paper focuses on the lateral-directional control and guidance aspects. An introduction to the mission and guidance problem is given first. Waypoints for straight and turning flight paths are defined. Computation of various flight path parameters is discussed, including formulae for real-time calculation of down-range (distance travelled along the desired track), cross-track deviation, and heading error of the vehicle; these are then used in the lateral guidance algorithm. The same section also describes how to make various mission-related decisions online during flight, such as when to start turning and when a waypoint is achieved. The lateral guidance law is then presented, followed by the design of a robust multivariableH∞ controller for roll control and stability augmentation. The controller uses the ailerons and rudder for control of roll angle and stabilization of yaw rate of the vehicle. The reference roll angle is generated by the nonlinear guidance law. The sensors available on-board the vehicle do not measure yaw rate; hence, a practical method of its estimation is proposed. The entire guidance and control scheme is implemented on the flight control computer of the actual aerial vehicle and taken to flight. Flight test results for different mission profiles are presented and discussed.


---

## Body

## 1. Introduction

Successful control system design for high performance UAVs requires efficient and effective techniques for the design of guidance and control algorithms that ensure satisfactory operation in the face of system uncertainties. In this paper, we develop and present one such technique. The UAV under consideration is shown in Figure1. The cruising speed of said UAV is about 45 m/sec. The main tasks of the guidance and control system are to:  (i)
fly the vehicle on the desired mission path, with minimum cross-track deviation,(ii)
make decisions about various flight events, such as to start/stop turning or achievement of waypoints,(iii)
provide robust stabilization and control during flight.Figure 1
A photograph of the test vehicle.A lateral track control law for small UAVs has been discussed in [1]. This is based on a pure geometrical concept. The idea is to make the ratio of lateral deviation to lateral velocity equal to the ratio of longitudinal distance to longitudinal velocity. Simulation results indicate that the yaw rate command generated by the guidance law exhibits oscillations in the vehicle roll channel, this could be a problem for implementation on a real vehicle. A receding horizon based controller for UAV guidance is presented in [2]. The dynamics of the problem are linearized and simplified, and a quadratic cost function based on the model predictive control methodology is set up. The output prediction horizon and the control horizon need to be selected keeping in view the computational resources available, and the actuator and vehicle dynamical constraints have to be modelled carefully. The problem is challenging with regards to real-time implementation (specially for critical applications like high performance UAVs), and infeasibility conditions for solutions have to be guarded against. Mixed integer linear programming- (MILP-) based guidance for UAVs has also been considered [3, 4]; however, here the optimization program generates a sequence of waypoints (positions) and velocities for the vehicle to follow. In other words a mission plan is generated, and deviations from this plan need to be corrected through a lower level guidance algorithm.A conceptually different guidance scheme employing vector fields for curved path following has also been pursued, see, for example, [5, 6]. Here, a vector field of course (or heading) commands is generated which is a function of vehicle position relative to the desired track. The difference between the actual and commanded headings forms the heading error which is driven to zero using an appropriate control algorithm. The course vector field can in cases give rise to large and sudden heading changes which can tax the capability of the control system.The contribution of this paper is to present a practical and integrated guidance and control approach and demonstrate its effectiveness by experimental testing. Formulae for basic flight path computations are derived that provide the requisite input parameters for the guidance algorithm to function. The guidance algorithm is a computationally inexpensive algorithm based on the work presented in [7] with some improvements for practical considerations. The output of the guidance algorithm drives the control system, and design of a robust multivariable controller that tracks the guidance commands is presented. The overall system is integrated in a seamless fashion and implemented and test flown on an actual vehicle.This paper is organized as follows. Section2 describes the mission plan on which we intend the UAV to fly, including definition of straight, turn, and loiter waypoints. Here, we also outline the guidance and control problem in specific terms. Section 3 develops the basic formulae used for flight path computation. These form the basis for taking real-time decisions during flight and for implementing the guidance law. The nonlinear guidance law is discussed in Section 4. Section 5 describes the design of a robust multivariable H∞ controller and discusses robustness of the controller across the flight envelope. Flight test results are presented and discussed in Section 6; Section 7 concludes the paper.


## 2. The Mission Plan

The mission of the UAV consists of a number of waypoints which define the path the UAV is required to take. The mission may be planned on-line or off-line. Here, we assume that information regarding the waypoints through which the UAV is desired to navigate is available; our main objective here is to discuss the practical aspects of how to guide and control the aerial vehicle through the given mission. The entire mission can be represented by a series interconnection of straight line segments, arcs, and circles (for loiter).Figure2 shows on the left-hand side, part of a mission comprising of a straight line segment. In this case, the UAV is required to fly through the waypoint WP2. The right side of Figure 2 shows part of a mission with a turn. Here, the UAV is not required to go through the central waypoint (WP2) but rather fly on a circular arc AB close to WP2. Point A is where the turn starts, and B is where it ends. Similarly, loiter missions are also possible where the vehicle flies in a circular orbit around a given waypoint. Figure 3 shows part of a mission which involves loiter at WP2. The arcs AB and CD are used for transitioning from the straight flight segments to the circular loiter pattern. In this study, we shall assume that the mission data in terms of waypoints is available.Waypoints for straight and turning flight.
(a)
(b)Figure 3
Mission with loiter about WP2.We now formulate the problem in more specific terms. The task of the lateral guidance and control system is defined as to(i)
compute the down-range covered from one waypoint to the next and the cross-track deviation from the designated track/trajectory,(ii)
compute the error in heading angle of the vehicle,(iii)
determine if a waypoint is achieved,(iv)
compute “turn start” and “turn stop” decision flags for the autopilot,(v)
solve the guidance equations to calculate the roll reference command for steering the vehicle back onto the desired path,(vi)
stabilize and control the lateral-directional dynamics of the vehicle across the flight envelope in the presence of disturbances.All the above computations have to be done in real time. The available control inputs to the vehicle are the ailerons and the rudder. Note that the cross-track deviation and heading error are used in the guidance equations.


## 3. Flight Path Computations

Here, we present the basic flight path computations that are performed in real time to enable navigation of the vehicle through a sequence of straight and turning waypoints. Waypoints are defined as geographic positions in terms of, latitude and longitude. For any three consecutive waypoints WP1, WP2 and WP3, if the turn angle (defined as the difference of azimuths of WP1 and WP3 at WP2) at the central waypoint (WP2) is nearly 180∘, then the path from WP1 to WP2 will be considered a straight line, and WP2 will be referred to as a straight waypoint. On the other hand, if the turn angle is significant (say greater than 5∘), then WP2 will be referred to as a turning waypoint, and the path from WP1 to WP3 will consist of straight and curved parts (Figure 2). The flight path computations are worked out separately for straight and turning parts of the flight.The algorithm presented computes deviations from the reference track in the form of cross-track and heading errors both for straight and curved paths. The cross-track and heading errors are used in the guidance logic to generate appropriate roll commands for the autopilot so that the vehicle is steered back onto the desired path. In addition to cross-track and heading errors, “waypoint achieved,” “turn start,” and “turn stop” decision flags are also computed for the autopilot.LetWP1(ϕ1,λ1), WP2(ϕ2,λ2), and WP3(ϕ3,λ3) be three consecutive waypoints (ϕ and λ denote the latitude and longitude, resp.), and let M(ϕM,λM) be the current position of the vehicle. We define an Earth-Centred Earth-Fixed (ECEF) frame with its origin at the centre of mass of the earth, the z-axis directed northward along the polar axis, the x-axis in the equatorial plane and passing through the Greenwich Meridian, and the y-axis also in the equatorial plane and passing through 90° east longitude. The position vector of a point on the earth’s surface with latitude ϕ and longitude λ in ECEF frame is given by
(1)rx=acosϕrcosλ,ry=acosϕrsinλ,rz=bsinϕr,
where a is the earth’s equatorial radius, b is the polar radius, and ϕr=tan-1((b/a)tanϕ) is the reduced latitude of the point under consideration.


### 3.1. Straight Path Formulae

#### 3.1.1. Distance Covered

Here, we consider the straight path from WP1 to WP2 (Figure4). Let r1→ and r2→ be the position vectors of WP1 and WP2 in ECEF frame, respectively. The distance between WP1 and WP2 can be calculated as:
(2)r12=12(|r1→|+|r2→|)cos-1(r^1·r^2),
where r^ is a unit vector along r→.Figure 4
Straight flight path definitions.


#### 3.1.2. Cross-Track Deviation

Cross-track error is the instantaneous normal displacement of the vehicle from the desired track. To compute the cross-track error, we first define a vectorrN→ normal to the trajectory plane (plane containing WP1, WP2, and the centre of the earth) as:
(3)rN→=r^1×r^2.
If rM→ is the position vector of the vehicle in ECEF frame, then the cross-track error is
(4)Cross  track=-rM→·r^N.


#### 3.1.3. Heading Error

LetVE and VN denote the east and north velocity of the vehicle, respectively, then the velocity heading of the vehicle is given by ψG=tan-1(VE/VN). The heading error is the difference between the actual velocity heading ψG and the desired heading ψR; that is, ψE=ψG-ψR.IfP is the projection of the vehicle’s current position on the trajectory plane (Figure 4), then the desired heading angle (ψR) is defined as the angle between the trajectory plane and the meridian plane passing through the projected point P. The position vector rP→ of P in ECEF frame is given by
(5)rP→=rM→-(rM→·r^N)r^N.
The desired heading angle ψR works out to be
(6)ψR=tan-1[|(r^P×k^)×r^N|(r^P×k^)·r^N],
where k^ is a unit vector along the earth’s spin axis. The heading error ψE can now be readily computed.


#### 3.1.4. Waypoint Achieved Criterion

The straight waypoint WP2 is achieved when the straight path distance covered exceedsr12. The straight distance covered from WP1 to the current position of the vehicle is (1/2)(|r1→|+|rP→|)cos-1(r^1·r^P).


### 3.2. Curved Path Formulae

For turning paths (Figure5), we define another coordinate system (the East-North-Up or ENU frame) at the central waypoint WP2. The origin of the ENU frame is at WP2, the positive x-axis points towards east, the y-axis points towards north, and the z-axis points up along the local vertical. The position vector of a point on the earth’s surface with latitude ϕ and longitude λ can be expressed in the ENU frame as:
(7)x=RN(λ-λ2)cosϕ2,y=RM(ϕ-ϕ2),z=0,
where RN and RM are the normal and meridian radii of curvature (of the earth’s ellipsoidal model), respectively. These are given by
(8)RN=a(1-e2sinϕ2)1/2,RM=a(1-e2)(1-e2sinϕ2)3/2,
where e denotes the eccentricity of the ellipsoidal earth:
(9)e=a2-b2a.Figure 5
Flight path with turn.Letq1→ and q2→ be the position vectors of WP1 and WP3 in the ENU frame; that is,
(10)q1→=[RN(λ1-λ2)cosϕ2RM(ϕ1-ϕ2)0],q2→=[RN(λ3-λ2)cosϕ2RM(ϕ3-ϕ2)0].The turn angleβ (≤180∘) is given by
(11)β=cos-1(q^1·q^2).


#### 3.2.1. Turn Centre

The turn centre will be at a distance ofR/sin(β/2) from WP2 on the line bisecting the turn angle β, R being the turn radius. The vector q→ along this line in the ENU frame is given by q→=q^1+q^2. Thus, the position of the turn centre C in the ENU frame is
(12)C→=Rsin(β/2)q^.
The projection of the vehicle on the earth’s surface (ϕM,λM) in the ENU frame is given as:
(13)M→=[RN(λM-λ2)cosϕ2RM(ϕM-ϕ2)0],
and the position vector of the vehicle’s projection w.r.t. the turn centre C is SM→=M→-C→.


#### 3.2.2. Turn Start Criterion

The turn start point is at a distance ofR/tan(β/2) from WP2 on the line joining WP1 and WP2. Its position vector A→ in the ENU frame is A→=R/tan(β/2)q^1, and its position w.r.t. the turn centre C is SA→=A→-C→. Turning is started when the cross-product SM→×SA→ changes sign. During turn, the total distance (down-track) from WP1 to the current position is computed as:
(14)distance from WP1=r12-Rtan(β/2)+Rcos-1(S^A·S^M).


#### 3.2.3. Cross-Track Deviation

The cross track deviation during turning is given byR-|SM→|.


#### 3.2.4. Heading Error

During turn, the desired heading angle(ψR) is given by ψR=ψs+ψT, where ψs is the desired heading at the turn start point (computed using (6) at the turn start point) and ψT is the angular displacement from the turn start point to the current position:
(15)ψT=cos-1(S^A·S^M).


#### 3.2.5. Waypoint Achieved Criterion

The waypoint WP2 is said to be achieved when the vehicle crosses the bisector line; that is, when the vector cross-productSM→×C→ changes sign.


#### 3.2.6. Turn Stop Criterion

The turn stop point is at a distance ofR/tan(β/2) from WP2 on the line joining WP2 and WP3. Its position vector B→ in the ENU frame is B→=R/tan(β/2)q^2, and its position w.r.t. the turn centre C is SB→=B→-C→. Turning is stopped when SM→×SB→ changes sign.


## 3.1. Straight Path Formulae

### 3.1.1. Distance Covered

Here, we consider the straight path from WP1 to WP2 (Figure4). Let r1→ and r2→ be the position vectors of WP1 and WP2 in ECEF frame, respectively. The distance between WP1 and WP2 can be calculated as:
(2)r12=12(|r1→|+|r2→|)cos-1(r^1·r^2),
where r^ is a unit vector along r→.Figure 4
Straight flight path definitions.


### 3.1.2. Cross-Track Deviation

Cross-track error is the instantaneous normal displacement of the vehicle from the desired track. To compute the cross-track error, we first define a vectorrN→ normal to the trajectory plane (plane containing WP1, WP2, and the centre of the earth) as:
(3)rN→=r^1×r^2.
If rM→ is the position vector of the vehicle in ECEF frame, then the cross-track error is
(4)Cross  track=-rM→·r^N.


### 3.1.3. Heading Error

LetVE and VN denote the east and north velocity of the vehicle, respectively, then the velocity heading of the vehicle is given by ψG=tan-1(VE/VN). The heading error is the difference between the actual velocity heading ψG and the desired heading ψR; that is, ψE=ψG-ψR.IfP is the projection of the vehicle’s current position on the trajectory plane (Figure 4), then the desired heading angle (ψR) is defined as the angle between the trajectory plane and the meridian plane passing through the projected point P. The position vector rP→ of P in ECEF frame is given by
(5)rP→=rM→-(rM→·r^N)r^N.
The desired heading angle ψR works out to be
(6)ψR=tan-1[|(r^P×k^)×r^N|(r^P×k^)·r^N],
where k^ is a unit vector along the earth’s spin axis. The heading error ψE can now be readily computed.


### 3.1.4. Waypoint Achieved Criterion

The straight waypoint WP2 is achieved when the straight path distance covered exceedsr12. The straight distance covered from WP1 to the current position of the vehicle is (1/2)(|r1→|+|rP→|)cos-1(r^1·r^P).


## 3.1.1. Distance Covered

Here, we consider the straight path from WP1 to WP2 (Figure4). Let r1→ and r2→ be the position vectors of WP1 and WP2 in ECEF frame, respectively. The distance between WP1 and WP2 can be calculated as:
(2)r12=12(|r1→|+|r2→|)cos-1(r^1·r^2),
where r^ is a unit vector along r→.Figure 4
Straight flight path definitions.


## 3.1.2. Cross-Track Deviation

Cross-track error is the instantaneous normal displacement of the vehicle from the desired track. To compute the cross-track error, we first define a vectorrN→ normal to the trajectory plane (plane containing WP1, WP2, and the centre of the earth) as:
(3)rN→=r^1×r^2.
If rM→ is the position vector of the vehicle in ECEF frame, then the cross-track error is
(4)Cross  track=-rM→·r^N.


## 3.1.3. Heading Error

LetVE and VN denote the east and north velocity of the vehicle, respectively, then the velocity heading of the vehicle is given by ψG=tan-1(VE/VN). The heading error is the difference between the actual velocity heading ψG and the desired heading ψR; that is, ψE=ψG-ψR.IfP is the projection of the vehicle’s current position on the trajectory plane (Figure 4), then the desired heading angle (ψR) is defined as the angle between the trajectory plane and the meridian plane passing through the projected point P. The position vector rP→ of P in ECEF frame is given by
(5)rP→=rM→-(rM→·r^N)r^N.
The desired heading angle ψR works out to be
(6)ψR=tan-1[|(r^P×k^)×r^N|(r^P×k^)·r^N],
where k^ is a unit vector along the earth’s spin axis. The heading error ψE can now be readily computed.


## 3.1.4. Waypoint Achieved Criterion

The straight waypoint WP2 is achieved when the straight path distance covered exceedsr12. The straight distance covered from WP1 to the current position of the vehicle is (1/2)(|r1→|+|rP→|)cos-1(r^1·r^P).


## 3.2. Curved Path Formulae

For turning paths (Figure5), we define another coordinate system (the East-North-Up or ENU frame) at the central waypoint WP2. The origin of the ENU frame is at WP2, the positive x-axis points towards east, the y-axis points towards north, and the z-axis points up along the local vertical. The position vector of a point on the earth’s surface with latitude ϕ and longitude λ can be expressed in the ENU frame as:
(7)x=RN(λ-λ2)cosϕ2,y=RM(ϕ-ϕ2),z=0,
where RN and RM are the normal and meridian radii of curvature (of the earth’s ellipsoidal model), respectively. These are given by
(8)RN=a(1-e2sinϕ2)1/2,RM=a(1-e2)(1-e2sinϕ2)3/2,
where e denotes the eccentricity of the ellipsoidal earth:
(9)e=a2-b2a.Figure 5
Flight path with turn.Letq1→ and q2→ be the position vectors of WP1 and WP3 in the ENU frame; that is,
(10)q1→=[RN(λ1-λ2)cosϕ2RM(ϕ1-ϕ2)0],q2→=[RN(λ3-λ2)cosϕ2RM(ϕ3-ϕ2)0].The turn angleβ (≤180∘) is given by
(11)β=cos-1(q^1·q^2).


### 3.2.1. Turn Centre

The turn centre will be at a distance ofR/sin(β/2) from WP2 on the line bisecting the turn angle β, R being the turn radius. The vector q→ along this line in the ENU frame is given by q→=q^1+q^2. Thus, the position of the turn centre C in the ENU frame is
(12)C→=Rsin(β/2)q^.
The projection of the vehicle on the earth’s surface (ϕM,λM) in the ENU frame is given as:
(13)M→=[RN(λM-λ2)cosϕ2RM(ϕM-ϕ2)0],
and the position vector of the vehicle’s projection w.r.t. the turn centre C is SM→=M→-C→.


### 3.2.2. Turn Start Criterion

The turn start point is at a distance ofR/tan(β/2) from WP2 on the line joining WP1 and WP2. Its position vector A→ in the ENU frame is A→=R/tan(β/2)q^1, and its position w.r.t. the turn centre C is SA→=A→-C→. Turning is started when the cross-product SM→×SA→ changes sign. During turn, the total distance (down-track) from WP1 to the current position is computed as:
(14)distance from WP1=r12-Rtan(β/2)+Rcos-1(S^A·S^M).


### 3.2.3. Cross-Track Deviation

The cross track deviation during turning is given byR-|SM→|.


### 3.2.4. Heading Error

During turn, the desired heading angle(ψR) is given by ψR=ψs+ψT, where ψs is the desired heading at the turn start point (computed using (6) at the turn start point) and ψT is the angular displacement from the turn start point to the current position:
(15)ψT=cos-1(S^A·S^M).


### 3.2.5. Waypoint Achieved Criterion

The waypoint WP2 is said to be achieved when the vehicle crosses the bisector line; that is, when the vector cross-productSM→×C→ changes sign.


### 3.2.6. Turn Stop Criterion

The turn stop point is at a distance ofR/tan(β/2) from WP2 on the line joining WP2 and WP3. Its position vector B→ in the ENU frame is B→=R/tan(β/2)q^2, and its position w.r.t. the turn centre C is SB→=B→-C→. Turning is stopped when SM→×SB→ changes sign.


## 3.2.1. Turn Centre

The turn centre will be at a distance ofR/sin(β/2) from WP2 on the line bisecting the turn angle β, R being the turn radius. The vector q→ along this line in the ENU frame is given by q→=q^1+q^2. Thus, the position of the turn centre C in the ENU frame is
(12)C→=Rsin(β/2)q^.
The projection of the vehicle on the earth’s surface (ϕM,λM) in the ENU frame is given as:
(13)M→=[RN(λM-λ2)cosϕ2RM(ϕM-ϕ2)0],
and the position vector of the vehicle’s projection w.r.t. the turn centre C is SM→=M→-C→.


## 3.2.2. Turn Start Criterion

The turn start point is at a distance ofR/tan(β/2) from WP2 on the line joining WP1 and WP2. Its position vector A→ in the ENU frame is A→=R/tan(β/2)q^1, and its position w.r.t. the turn centre C is SA→=A→-C→. Turning is started when the cross-product SM→×SA→ changes sign. During turn, the total distance (down-track) from WP1 to the current position is computed as:
(14)distance from WP1=r12-Rtan(β/2)+Rcos-1(S^A·S^M).


## 3.2.3. Cross-Track Deviation

The cross track deviation during turning is given byR-|SM→|.


## 3.2.4. Heading Error

During turn, the desired heading angle(ψR) is given by ψR=ψs+ψT, where ψs is the desired heading at the turn start point (computed using (6) at the turn start point) and ψT is the angular displacement from the turn start point to the current position:
(15)ψT=cos-1(S^A·S^M).


## 3.2.5. Waypoint Achieved Criterion

The waypoint WP2 is said to be achieved when the vehicle crosses the bisector line; that is, when the vector cross-productSM→×C→ changes sign.


## 3.2.6. Turn Stop Criterion

The turn stop point is at a distance ofR/tan(β/2) from WP2 on the line joining WP2 and WP3. Its position vector B→ in the ENU frame is B→=R/tan(β/2)q^2, and its position w.r.t. the turn centre C is SB→=B→-C→. Turning is stopped when SM→×SB→ changes sign.


## 4. Lateral Guidance Logic

The block diagram of the lateral-directional guidance and control system is shown in Figure6. The outputs of the UAV that are measured by on-board sensors include the roll, pitch, and heading attitude angles and the (GPS) position and velocity of the vehicle. The roll and pitch angles are measured by a vertical gyro, and the heading angle is provided by a magnetic sensor. The outer loop is the lateral guidance loop that looks at the measured position and velocity of the vehicle, compares it with the desired mission path, and generates a roll angle command that acts as a reference for the inner loop to track. The inner loop has roll angle and yaw rate feedbacks; the yaw-rate is computed from the attitude angles as discussed in Section 5.1 below.Figure 6
Block diagram of the overall guidance and control system.The lateral guidance scheme used is based on the work in [7]. The basic scheme is modified by introducing an adaptive tuning of the reference length to enhance the performance for large cross-track errors. Integral action is also added to improve tracking in the presence of disturbances. Here, we first introduce the basic scheme, followed by a discussion of the modifications.


### 4.1. The Basic Guidance Scheme

Figure7 illustrates the guidance scheme. Let AB be the desired flight path of the vehicle, and let the instantaneous position of the vehicle be denoted by C, at a perpendicular distance y from the line AB. We define a point D at a distance L1 from C such that the vehicle may follow a circular arc of radius R from C to the desired path at D. The centripetal acceleration aL required for the vehicle to fly along this circular arc is given by
(16)aL=2V2L1sinη,
where V is the velocity of the vehicle and η is the angle between the velocity vector and the line CD. Since we shall employ a bank-to-turn scheme, the lateral acceleration command needs to be converted into a roll reference command. When the vehicle is banked at an angle ϕ, the lift vector L can be resolved into two components Lcosϕ=mg and Lsinϕ=(2mV2/L1)sinη, from which, we have
(17)ϕ=tan-1(2V2gL1sinη).
From Figure 7, we have η=η1+η2, where the subangles are
(18)η1=sin-1(yL1),η2=ψE,
where ψE is the heading error. Here, y≤L1 because the argument of the arcsin function should be less than or equal to 1. Now, we may write (17) as follows:
(19)ϕc=tan-1[2V2gL1sin{sin-1(yL1)+ψE}],
where the subscript in ϕc stands for roll angle command. Equation (19) defines the nonlinear lateral guidance law; it gives the roll angle which the vehicle should fly to get the lateral acceleration required to follow a circular arc back to its desired flight path.Figure 7
Nonlinear guidance logic setup.


### 4.2. Selection ofL1 Using Linear Analysis

A guideline for selection of the lengthL1 is provided in [7] by linearizing (19) to yield a second-order system with damping and natural frequency given by
(20)ζ=12,ωn=2VL1,
where ζ and ωn refer to the damping ratio and natural frequency of the standard second order system. It is stressed, however, that this is only a guideline as a very strong assumption of no inner loop dynamics (aL=-y¨) is employed in its derivation.


### 4.3. Adaptive Adjustment of LengthL1

For system stability, we must havey<L1 and |η|<π/2 [7]. For UAVs however, there is a real possibility of large cross-track errors developing during flight. This can be due to the runway not being in the direction of the first waypoint so that a cross-track deviation is necessarily generated in the initial phase of the flight or the GPS signal becoming masked for extended periods during flight. Furthermore, since lateral control is usually engaged sometime after take-off (to avoid large roll angles during take-off), the cross-track may build to a large value initially. In such cases, the condition y<L1 may not be satisfied making ϕc undefined. Table 1 gives values of L1 computed using (20) for different speeds typical of modern UAVs. The table shows that for small-to-medium UAVs, L1 can vary from 200 m to 850 m depending upon the desired response time of the guidance algorithm (or ωn). If a fast response is desired (high ωn), a small L1 will be selected, leading to large roll angles to bring the vehicle back on the desired path quickly, but with possibly high overshoots. A larger L1 will make the system slower and reduce overshoot. This is discussed in [8] with reference to gain selection for a conventional proportional-derivative guidance law where the gains are scheduled with the magnitude of the cross-track error. It may be noted here that to cater for large cross-track errors (which are possible as discussed above), L1 will have to be kept unduly large which may yield poor performance otherwise. On the other hand, if a relatively small L1 is selected, the resulting roll angle might become too large and exceed the set limit. Table 2 shows the commanded roll angle ϕc for different cross-track errors for a fixed speed and L1 chosen to yield an ωn of 0.2 rad/sec. The last row of the table gives a ϕc of 55∘, which may be too large for most vehicles, and thereafter for cross tracks larger than L1, ϕc becomes undefined. A solution to this is to introduce an adaptive adjustment of L1 with cross-track error [9]. We introduce a compensated reference length L^1 to be used in (19) instead of a fixed L1:
(21)L^1=L1+k1|y|,
where k1>0 is a gain to be tuned (a value of 1.5 for our application gave good results). Roll angle commands are computed using L1 and L^1 and compared (for V=50 m/s and ωn=0.2 rad/s) in Table 3. The table shows that for large cross-track errors, the roll angle command computed using L^1 remains reasonably small and can be designed to lie within a desired range.Table 1
L
1 (m) for different flight speeds and ωn.
Speed (m/s)
ω
n
=
0.1 rad/s
ω
n
=
0.2 rad/s
ω
n
=
0.3 rad/s
40
565
280
190
50
705
350
235
60
850
425
285Table 2
ϕ
c for different cross-track errors (ωn=0.2 rad/s).
Cross-track deviation (% ofL1)
ϕ
c (deg)
12.5
23
25
31
50
43
100
55Table 3
ϕ
c computed using L1 and L^1.
Cross-track deviation (% of L1)
ϕ
c (deg) using fixed L1
ϕ
c (deg) using L^1
12.5
23
19
25
31
21
50
43
21
100
55
19
200
Undefined
14


### 4.4. Addition of Integral Action

The roll angle command as given in (19) is modified by addition of an integrator for strict path tracking in the presence of constant (wind) disturbances. Addition of an integrator in the loop can pose problems such as wind-up leading to control surface saturation, for which the following actions are taken:  (i)
integral action is activated only when the cross-range error becomes smaller than a set thresholdyth,(ii)
the integrator state is limited to a predefined value; the integrator is switched off if its state exceeds the set limit.With the addition of integral action and substitution ofL^1 in place of L1, (19) becomes
(22)ϕc=tan-1[2V2gL^1sin{sin-1(yL^1)+ψE}]+k2∫‍y^dt,
where the integral gain k2 is tuned by simulation and
(23)y^={y,ify≤yth,0,ify>yth.


## 4.1. The Basic Guidance Scheme

Figure7 illustrates the guidance scheme. Let AB be the desired flight path of the vehicle, and let the instantaneous position of the vehicle be denoted by C, at a perpendicular distance y from the line AB. We define a point D at a distance L1 from C such that the vehicle may follow a circular arc of radius R from C to the desired path at D. The centripetal acceleration aL required for the vehicle to fly along this circular arc is given by
(16)aL=2V2L1sinη,
where V is the velocity of the vehicle and η is the angle between the velocity vector and the line CD. Since we shall employ a bank-to-turn scheme, the lateral acceleration command needs to be converted into a roll reference command. When the vehicle is banked at an angle ϕ, the lift vector L can be resolved into two components Lcosϕ=mg and Lsinϕ=(2mV2/L1)sinη, from which, we have
(17)ϕ=tan-1(2V2gL1sinη).
From Figure 7, we have η=η1+η2, where the subangles are
(18)η1=sin-1(yL1),η2=ψE,
where ψE is the heading error. Here, y≤L1 because the argument of the arcsin function should be less than or equal to 1. Now, we may write (17) as follows:
(19)ϕc=tan-1[2V2gL1sin{sin-1(yL1)+ψE}],
where the subscript in ϕc stands for roll angle command. Equation (19) defines the nonlinear lateral guidance law; it gives the roll angle which the vehicle should fly to get the lateral acceleration required to follow a circular arc back to its desired flight path.Figure 7
Nonlinear guidance logic setup.


## 4.2. Selection ofL1 Using Linear Analysis

A guideline for selection of the lengthL1 is provided in [7] by linearizing (19) to yield a second-order system with damping and natural frequency given by
(20)ζ=12,ωn=2VL1,
where ζ and ωn refer to the damping ratio and natural frequency of the standard second order system. It is stressed, however, that this is only a guideline as a very strong assumption of no inner loop dynamics (aL=-y¨) is employed in its derivation.


## 4.3. Adaptive Adjustment of LengthL1

For system stability, we must havey<L1 and |η|<π/2 [7]. For UAVs however, there is a real possibility of large cross-track errors developing during flight. This can be due to the runway not being in the direction of the first waypoint so that a cross-track deviation is necessarily generated in the initial phase of the flight or the GPS signal becoming masked for extended periods during flight. Furthermore, since lateral control is usually engaged sometime after take-off (to avoid large roll angles during take-off), the cross-track may build to a large value initially. In such cases, the condition y<L1 may not be satisfied making ϕc undefined. Table 1 gives values of L1 computed using (20) for different speeds typical of modern UAVs. The table shows that for small-to-medium UAVs, L1 can vary from 200 m to 850 m depending upon the desired response time of the guidance algorithm (or ωn). If a fast response is desired (high ωn), a small L1 will be selected, leading to large roll angles to bring the vehicle back on the desired path quickly, but with possibly high overshoots. A larger L1 will make the system slower and reduce overshoot. This is discussed in [8] with reference to gain selection for a conventional proportional-derivative guidance law where the gains are scheduled with the magnitude of the cross-track error. It may be noted here that to cater for large cross-track errors (which are possible as discussed above), L1 will have to be kept unduly large which may yield poor performance otherwise. On the other hand, if a relatively small L1 is selected, the resulting roll angle might become too large and exceed the set limit. Table 2 shows the commanded roll angle ϕc for different cross-track errors for a fixed speed and L1 chosen to yield an ωn of 0.2 rad/sec. The last row of the table gives a ϕc of 55∘, which may be too large for most vehicles, and thereafter for cross tracks larger than L1, ϕc becomes undefined. A solution to this is to introduce an adaptive adjustment of L1 with cross-track error [9]. We introduce a compensated reference length L^1 to be used in (19) instead of a fixed L1:
(21)L^1=L1+k1|y|,
where k1>0 is a gain to be tuned (a value of 1.5 for our application gave good results). Roll angle commands are computed using L1 and L^1 and compared (for V=50 m/s and ωn=0.2 rad/s) in Table 3. The table shows that for large cross-track errors, the roll angle command computed using L^1 remains reasonably small and can be designed to lie within a desired range.Table 1
L
1 (m) for different flight speeds and ωn.
Speed (m/s)
ω
n
=
0.1 rad/s
ω
n
=
0.2 rad/s
ω
n
=
0.3 rad/s
40
565
280
190
50
705
350
235
60
850
425
285Table 2
ϕ
c for different cross-track errors (ωn=0.2 rad/s).
Cross-track deviation (% ofL1)
ϕ
c (deg)
12.5
23
25
31
50
43
100
55Table 3
ϕ
c computed using L1 and L^1.
Cross-track deviation (% of L1)
ϕ
c (deg) using fixed L1
ϕ
c (deg) using L^1
12.5
23
19
25
31
21
50
43
21
100
55
19
200
Undefined
14


## 4.4. Addition of Integral Action

The roll angle command as given in (19) is modified by addition of an integrator for strict path tracking in the presence of constant (wind) disturbances. Addition of an integrator in the loop can pose problems such as wind-up leading to control surface saturation, for which the following actions are taken:  (i)
integral action is activated only when the cross-range error becomes smaller than a set thresholdyth,(ii)
the integrator state is limited to a predefined value; the integrator is switched off if its state exceeds the set limit.With the addition of integral action and substitution ofL^1 in place of L1, (19) becomes
(22)ϕc=tan-1[2V2gL^1sin{sin-1(yL^1)+ψE}]+k2∫‍y^dt,
where the integral gain k2 is tuned by simulation and
(23)y^={y,ify≤yth,0,ify>yth.


## 5. Robust Multivariable Controller Design

### 5.1. The Plant Model

The lateral-directional control system is a multivariable control system as shown in Figure6. The roll and pitch angles are sensed by the vertical gyro which has very fast dynamics as compared to the vehicle, and so this sensor is modelled as a simple gain in the feedback loop. The magnetic sensor (which senses the heading angle) dynamics are also neglected. The model of the UAV for controller design is taken as a linear approximation obtained at a cruise altitude of 2000 m and a speed of 42 m/sec. However, the flight envelope consists of an altitude range from 10 to 5000 m and a speed bracket from 35 to 60 m/sec. A number of linear models are available across the flight envelope to test the robustness of the control system at different operating conditions. Note that the plant has 2 inputs and 2 outputs. The inputs are the aileron and rudder deflection commands to the actuators. The outputs are the roll angle and the yaw-rate of the vehicle. The computation of the yaw-rate and its filtering is discussed below, but first, we look at computation of the heading angle.


#### 5.1.1. Computation of the Heading Angle

The magnetic sensor installed on-board the vehicle measures the three components of the magnetic field of the earth in the body axes. This magnetic field vector is resolved into a local level frame using the pitch and roll measurements from the vertical gyro. We define the local level axis system(XlYlZl) in which the Zl axis is upwards along the local vertical and the Xl and Yl axes are in the local horizontal plane, with the Yl axis directed along the projection of the longitudinal axis of the vehicle in the local level plane. The pitch and roll rotations θ and ϕ occur about the Xl and Yl′ axes (the Yl′ axis is obtained by rotating the Yl axis by the pitch angle θ), respectively. The transformation matrix from the local level to the body frame is given by
(24)Tlb=[cosϕsinϕsinθ-sinϕcosθ0cosθsinθsinϕ-sinθcosϕcosϕcosθ].
The magnetic field vector is transformed from the body axes to the local level axes by Ml=TlbTMb, where Mb and Ml represent the magnetic field vector in the body and local level axes, respectively. The magnetic heading is now determined as tan-1(Mlx/Mly), where Mlx and Mly are the first two components of Ml. Note that this gives the magnetic heading of the vehicle, the true heading ψ (w.r.t. the true north) can be found by applying the declination correction to the magnetic heading [10].


#### 5.1.2. Yaw-Rate Computation

Feedback of the yaw-rate is considered useful for providing damping to the Dutch-roll mode of the vehicle. The vertical gyro however does not provide measurement of angular rates; it only measures roll and pitch angles. The magnetic heading sensor measures the heading angle. Body angular rates are computed from attitude angles as follows.We define a navigation axis system(XnYnZn) in which the Zn axis is upwards along the local vertical, and the Xn and Yn axes are in the local horizontal plane directed eastward and northward, respectively. The (true) heading, pitch, and roll angles are denoted by ψ, θ, and ϕ, respectively, and these rotations occur in this specific order to align the navigation axes with the body axes. The heading rotation occurs first about the Zn axis, followed by the pitch rotation about the Xn′ axis, finally followed by the roll rotation about the Yn′′ axis (note that the primed axes are the intermediate axes obtained while going from the navigation to the body frame through the ψ, θ, and ϕ rotation angles). Resolving the Euler angle rates ψ˙, θ,˙ and ϕ˙ into body axes, we can solve for the body axes rates. Denoting the roll, pitch, and yaw rates in the body axes by P, Q, and R, respectively, we have
(25)P=ψ˙sinθ+ϕ˙,Q=θ˙cosϕ-ψ˙cosθsinϕ,R=θ˙sinϕ+ψ˙cosθcosϕ.It may be noted that the attitude sensors measure the body angles and not their rates. The derivatives of the attitude angles are computed by fitting a least squares line ton consecutive attitude measurements. We have chosen n=4 here as we have seen it to yield good results; the derivative, thus, computed has enough noise smoothing and an acceptably small time delay. However, other choices for n can be made depending on the application and on the sampling time of the attitude sensor measurements. If, for example, the pitch angle measurements from the vertical gyro are denoted by θ0, θ1, θ2, and θ3, where θ0 corresponds to the current measurement and θ3 corresponds to the measurement taken three samples previously, then the slope of the least squares line for these points approximates the derivative [11]:
(26)θ˙≈3θ0+θ1-θ2-3θ310Δt.Here,Δt is the time interval at which the angle θ is sampled (20 msec for our case). This approximation of the derivative by slope of a 4-point least squares line gives a good compromise between sensitivity to measurement noise and the delay introduced into the estimation of θ˙. Figure 8 shows the delay between the actual and computed derivatives for a pure sinusoid of frequency 10 rad/sec, which is about 25 msec. This method of estimating body rates and using them for feedback control is demonstrated to work well for normal flight maneuvers. A test flight was carried out in which a rate sensor was specially installed on the vehicle for validation purposes only; this sensor is not available for feedback control otherwise. The comparison between the measured and computed yaw-rates shown in Figure 9 indicates the validity of the approximation. It is, thus, concluded that the approximation (26) can be used for Euler angle rate estimation from attitude measurements, and thereafter (25) can be employed for transforming the Euler angle rates into body-axes angular rates which are used for feedback control.Figure 8
Delay introduced because of the 4-point derivative approximation.Figure 9
Measured and computed yaw rates during test flight.


#### 5.1.3. The Washout Filter

Yaw-rate feedback is typically useful during transients when the yaw-rate fluctuates or exhibits appreciable changes with time. The rudder is normally employed to suppress the yaw-rate oscillations to provide damping to the Dutch-roll mode of the vehicle. Thus, during steady turns when there is a nearly constant yaw-rate, it is not desirable for the rudder to act to oppose the turning motion. The washout filter is a high-pass filter that is used to feedback the yaw-rate at higher frequencies, while effectively breaking the feedback at dc to enable steady turning of the vehicle. The washout filter is chosen asWw=s/(s+1); it may be thought of as a frequency-dependent weight on the yaw-rate feedback, with a small gain at low frequencies  and unity gain at high frequencies. The 3 dB point occurs at 1 rad/sec, so that frequencies above 1 rad/sec will be available for feedback.


### 5.2.H∞ Loop-Shaping Design Procedure

For controller design, we shall use theH∞ loop-shaping design procedure proposed in [12]. The procedure is intuitive in that it is based on the multivariable generalization of classical loop-shaping ideas. The open-loop plant, once given the desired loop-shape, is robustly stabilized against coprime factor uncertainty. The resulting controller has been shown to enjoy some favourable properties, such as no pole-zero cancellation occurs in the closed-loop system (except for a certain special class of plants), see [13]. In addition, the controllers, thus, designed have been successful in various applications; examples are those described in [14–17].In practical design applications, the performance specifications are first translated into the frequency domain, and the open-loop plant’s singular value frequency response is given the desired shape. This is achieved by augmentation of the nominal plant modelG by pre- and/or postcompensators (or weighting functions) W1 and W2, respectively. The shaped plant Gs=W2GW1 is then robustly stabilized against coprime factor uncertainty, and the controller K, thus, obtained is cascaded with the weights to obtain the final controller W1KW2. It can be shown that the controller does not significantly alter the specified loop-shape provided a sufficiently small value of the cost γ is achieved; for details, refer to [17].We now outline a design procedure for designing robust controllers based on open-loop shaping and robust stabilization of the normalized coprime factors of the plant. The procedure consists of the following main steps.(1)
Plot the singular value frequency response of the open-loop plantG(s). Based on this, select a precompensator W^1 and/or a postcompensator W2 to give the plant a desired open-loop shape. Form the product W2(s)G(s)W^1(s).(2)
Align the singular values ofW2GW^1 at the desired bandwidth. The align gain Ka is the approximate real inverse of the system at the specified frequency. The cross-over (and hence the bandwidth) is thus adjusted to approximately the align frequency. An additional constant diagonal matrix Kg may sometimes be used in front of the align gain to exercise control over actuator usage. It is chosen so that the various actuator rate limits are not exceeded whilst following references or rejecting disturbances. The precompensator can now be written as W1=W^1KaKg; see Figure 10. Build the shaped plant Gs(s)=W2(s)G(s)W1(s), and calculate the optimal cost γopt [12]. A high value (typically > 10) of γopt indicates that the specified loop-shape is inconsistent with robust stability; in such a case, the weights W1 and W2 should be modified.(3)
Use a slightly suboptimal value ofγ, and compute the corresponding controller.(4)
Cascade the controller with the weightsW1 and W2. Controller order reduction may be performed if desired.(5)
Form the closed-loop, and check the appropriate performance and robustness measures against the given specification.Figure 10
The shaped plant and the controller.


### 5.3. Controller Design

Here, we will describe the design of the lateral controller, using the design procedure listed in Section5.2. The block diagram of the closed-loop system is shown in Figure 11. (1)
Singular values of the open-loop plantG(s) are plotted, and these indicate the need for boosting the low frequency gain for good tracking and disturbance rejection for the roll control channel at these frequencies. The low frequency gain is boosted by introducing integral action in the roll control loop; we choose to place a pole at s=-0.01 instead of using a pure integrator (because of the wind-up problems associated with integrators, and also because we do not want to introduce an unstable pole into the shaped plant Gs). W^1 in Figure 10 is chosen as [28(s+2.5)/(s+0.01)0040] and W2 as [14(s+2)/(s+70)0050/(s+50)]. Since we are aiming for a closed-loop bandwidth of approximately 10–15 rad/sec, therefore zeros are introduced at −2.5 and −2 in W^1 and W2 to reduce the roll-off at the cross-over frequencies. The poles at −70 and −50 in W2 are placed to provide adequate roll-off at higher frequencies. Since W^11 (the upper left element of W^1) approximates an integrator, it will be implemented in its conditioned form (see [18, 19]). Gains are selected in W^1 and W2 to adjust the cross-over frequency to around 10 rad/sec.(2)
The align matrixKa is chosen to be the identity matrix for robustness reasons (aligning the nominal plant and designing the controller based on this may not hold good with other models of the plant across the flight envelope) and since both singular values are close enough at cross-over. The matrix Kg is also kept as the identity matrix I2. The precompensator is therefore W1=W^1KaKg=W^1. The shaped plant Gs=W2GW1 is now formed, and its singular values are shown in Figure 12. The low frequency gain in the roll channel is boosted, as indicated by the larger singular value. The washout filter in the yaw-rate channel means that the low frequency gain in this channel remains low, as indicated by the smaller singular value of the shaped plant.(3)
An optimalγ of 1.925 is computed; the controller is obtained for a slightly suboptimal γ of 2.02.(4)
The controller is reduced using optimal Hankel-norm approximation and cascaded with the weightsW1 and W2 to form the complete controller W1KW2. This reduced controller is discretized using the bilinear (Tustin’s) approximation for implementation.Figure 11
Plant, controller, and weighting functions.Figure 12
Singular values of the original and shaped plants.The singular values of the sensitivity function are shown in Figure13. One of the singular values, the one that remains flat at low frequencies, corresponds to yaw-rate and indicates no disturbance rejection on that output. The other singular value shows adequate disturbance rejection on the roll angle channel for frequencies less than 5 rad/sec. A number of linearized models are taken along the periphery of the flight envelope. Step responses for these plant models in the envelope using the designed controller are given in Figure 14. The controller performs acceptably well throughout the envelope, and the robustness of the design is illustrated.Figure 13
Sensitivity function(I+GW1KW2)-1.Figure 14
Step responses for different plant models across the flight envelope.


### 5.4. Robustness Analysis

A more formal robustness analysis of the controller is now performed. Note that the controller design is based on a normalized coprime factor representation of the plant:Gs=W2GW1=M~-1N~, where the transfer functions M~ and N~ represent the normalized coprime factors of the (shaped) plant. The controller is designed to maximize robust stability in the face of coprime factor perturbations. The perturbations (or uncertainties) in the plant are represented as perturbations on the normalized coprime factors of the plant. The perturbed plant GsΔ is given by
(27)GsΔ=(M~+ΔM~)-1(N~+ΔN~),
where ΔM~, ΔN~ are stable unknown transfer functions representing uncertainty in the plant model. Robust stability to coprime factor perturbations is maximized by a controller K which stabilizes the nominal plant Gs and minimizes
(28)γ=∥[KI](I-GsK)-1M~-1∥∞.
The perturbed closed-loop system is then guaranteed to remain stable in the face of all ΔM~, ΔN~ such that [20]
(29)∥[ΔM~ΔN~]∥∞<γ-1.
Robust stability across the flight envelope is tested by obtaining normalized coprime factorizations of all plants in the envelope and then taking their differences from the nominal plant to get uncertainty transfer functions [ΔM~iΔN~i], where the subscript i refers to the i"th"  plant in the envelope. Singular values of these uncertainty transfer functions are plotted in Figure 15; the maximum of these across all frequencies equals 0.42. The cost γ for our controller is 2.02, which indicates that (29) is satisfied, thus ensuring robust stability for the family of plants in the envelope; that is,
(30)∥[ΔM~iΔN~i]∥∞=0.42<2.02-1.
It is seen that a single controller is robust enough to stabilize all plants in the flight envelope; also robust performance is checked for and is found to be adequate by performing step response analysis of the family of plants (Figure 14).Figure 15
Singular values of(ΔM~ΔN~) for different plants in the envelope.


## 5.1. The Plant Model

The lateral-directional control system is a multivariable control system as shown in Figure6. The roll and pitch angles are sensed by the vertical gyro which has very fast dynamics as compared to the vehicle, and so this sensor is modelled as a simple gain in the feedback loop. The magnetic sensor (which senses the heading angle) dynamics are also neglected. The model of the UAV for controller design is taken as a linear approximation obtained at a cruise altitude of 2000 m and a speed of 42 m/sec. However, the flight envelope consists of an altitude range from 10 to 5000 m and a speed bracket from 35 to 60 m/sec. A number of linear models are available across the flight envelope to test the robustness of the control system at different operating conditions. Note that the plant has 2 inputs and 2 outputs. The inputs are the aileron and rudder deflection commands to the actuators. The outputs are the roll angle and the yaw-rate of the vehicle. The computation of the yaw-rate and its filtering is discussed below, but first, we look at computation of the heading angle.


### 5.1.1. Computation of the Heading Angle

The magnetic sensor installed on-board the vehicle measures the three components of the magnetic field of the earth in the body axes. This magnetic field vector is resolved into a local level frame using the pitch and roll measurements from the vertical gyro. We define the local level axis system(XlYlZl) in which the Zl axis is upwards along the local vertical and the Xl and Yl axes are in the local horizontal plane, with the Yl axis directed along the projection of the longitudinal axis of the vehicle in the local level plane. The pitch and roll rotations θ and ϕ occur about the Xl and Yl′ axes (the Yl′ axis is obtained by rotating the Yl axis by the pitch angle θ), respectively. The transformation matrix from the local level to the body frame is given by
(24)Tlb=[cosϕsinϕsinθ-sinϕcosθ0cosθsinθsinϕ-sinθcosϕcosϕcosθ].
The magnetic field vector is transformed from the body axes to the local level axes by Ml=TlbTMb, where Mb and Ml represent the magnetic field vector in the body and local level axes, respectively. The magnetic heading is now determined as tan-1(Mlx/Mly), where Mlx and Mly are the first two components of Ml. Note that this gives the magnetic heading of the vehicle, the true heading ψ (w.r.t. the true north) can be found by applying the declination correction to the magnetic heading [10].


### 5.1.2. Yaw-Rate Computation

Feedback of the yaw-rate is considered useful for providing damping to the Dutch-roll mode of the vehicle. The vertical gyro however does not provide measurement of angular rates; it only measures roll and pitch angles. The magnetic heading sensor measures the heading angle. Body angular rates are computed from attitude angles as follows.We define a navigation axis system(XnYnZn) in which the Zn axis is upwards along the local vertical, and the Xn and Yn axes are in the local horizontal plane directed eastward and northward, respectively. The (true) heading, pitch, and roll angles are denoted by ψ, θ, and ϕ, respectively, and these rotations occur in this specific order to align the navigation axes with the body axes. The heading rotation occurs first about the Zn axis, followed by the pitch rotation about the Xn′ axis, finally followed by the roll rotation about the Yn′′ axis (note that the primed axes are the intermediate axes obtained while going from the navigation to the body frame through the ψ, θ, and ϕ rotation angles). Resolving the Euler angle rates ψ˙, θ,˙ and ϕ˙ into body axes, we can solve for the body axes rates. Denoting the roll, pitch, and yaw rates in the body axes by P, Q, and R, respectively, we have
(25)P=ψ˙sinθ+ϕ˙,Q=θ˙cosϕ-ψ˙cosθsinϕ,R=θ˙sinϕ+ψ˙cosθcosϕ.It may be noted that the attitude sensors measure the body angles and not their rates. The derivatives of the attitude angles are computed by fitting a least squares line ton consecutive attitude measurements. We have chosen n=4 here as we have seen it to yield good results; the derivative, thus, computed has enough noise smoothing and an acceptably small time delay. However, other choices for n can be made depending on the application and on the sampling time of the attitude sensor measurements. If, for example, the pitch angle measurements from the vertical gyro are denoted by θ0, θ1, θ2, and θ3, where θ0 corresponds to the current measurement and θ3 corresponds to the measurement taken three samples previously, then the slope of the least squares line for these points approximates the derivative [11]:
(26)θ˙≈3θ0+θ1-θ2-3θ310Δt.Here,Δt is the time interval at which the angle θ is sampled (20 msec for our case). This approximation of the derivative by slope of a 4-point least squares line gives a good compromise between sensitivity to measurement noise and the delay introduced into the estimation of θ˙. Figure 8 shows the delay between the actual and computed derivatives for a pure sinusoid of frequency 10 rad/sec, which is about 25 msec. This method of estimating body rates and using them for feedback control is demonstrated to work well for normal flight maneuvers. A test flight was carried out in which a rate sensor was specially installed on the vehicle for validation purposes only; this sensor is not available for feedback control otherwise. The comparison between the measured and computed yaw-rates shown in Figure 9 indicates the validity of the approximation. It is, thus, concluded that the approximation (26) can be used for Euler angle rate estimation from attitude measurements, and thereafter (25) can be employed for transforming the Euler angle rates into body-axes angular rates which are used for feedback control.Figure 8
Delay introduced because of the 4-point derivative approximation.Figure 9
Measured and computed yaw rates during test flight.


### 5.1.3. The Washout Filter

Yaw-rate feedback is typically useful during transients when the yaw-rate fluctuates or exhibits appreciable changes with time. The rudder is normally employed to suppress the yaw-rate oscillations to provide damping to the Dutch-roll mode of the vehicle. Thus, during steady turns when there is a nearly constant yaw-rate, it is not desirable for the rudder to act to oppose the turning motion. The washout filter is a high-pass filter that is used to feedback the yaw-rate at higher frequencies, while effectively breaking the feedback at dc to enable steady turning of the vehicle. The washout filter is chosen asWw=s/(s+1); it may be thought of as a frequency-dependent weight on the yaw-rate feedback, with a small gain at low frequencies  and unity gain at high frequencies. The 3 dB point occurs at 1 rad/sec, so that frequencies above 1 rad/sec will be available for feedback.


## 5.1.1. Computation of the Heading Angle

The magnetic sensor installed on-board the vehicle measures the three components of the magnetic field of the earth in the body axes. This magnetic field vector is resolved into a local level frame using the pitch and roll measurements from the vertical gyro. We define the local level axis system(XlYlZl) in which the Zl axis is upwards along the local vertical and the Xl and Yl axes are in the local horizontal plane, with the Yl axis directed along the projection of the longitudinal axis of the vehicle in the local level plane. The pitch and roll rotations θ and ϕ occur about the Xl and Yl′ axes (the Yl′ axis is obtained by rotating the Yl axis by the pitch angle θ), respectively. The transformation matrix from the local level to the body frame is given by
(24)Tlb=[cosϕsinϕsinθ-sinϕcosθ0cosθsinθsinϕ-sinθcosϕcosϕcosθ].
The magnetic field vector is transformed from the body axes to the local level axes by Ml=TlbTMb, where Mb and Ml represent the magnetic field vector in the body and local level axes, respectively. The magnetic heading is now determined as tan-1(Mlx/Mly), where Mlx and Mly are the first two components of Ml. Note that this gives the magnetic heading of the vehicle, the true heading ψ (w.r.t. the true north) can be found by applying the declination correction to the magnetic heading [10].


## 5.1.2. Yaw-Rate Computation

Feedback of the yaw-rate is considered useful for providing damping to the Dutch-roll mode of the vehicle. The vertical gyro however does not provide measurement of angular rates; it only measures roll and pitch angles. The magnetic heading sensor measures the heading angle. Body angular rates are computed from attitude angles as follows.We define a navigation axis system(XnYnZn) in which the Zn axis is upwards along the local vertical, and the Xn and Yn axes are in the local horizontal plane directed eastward and northward, respectively. The (true) heading, pitch, and roll angles are denoted by ψ, θ, and ϕ, respectively, and these rotations occur in this specific order to align the navigation axes with the body axes. The heading rotation occurs first about the Zn axis, followed by the pitch rotation about the Xn′ axis, finally followed by the roll rotation about the Yn′′ axis (note that the primed axes are the intermediate axes obtained while going from the navigation to the body frame through the ψ, θ, and ϕ rotation angles). Resolving the Euler angle rates ψ˙, θ,˙ and ϕ˙ into body axes, we can solve for the body axes rates. Denoting the roll, pitch, and yaw rates in the body axes by P, Q, and R, respectively, we have
(25)P=ψ˙sinθ+ϕ˙,Q=θ˙cosϕ-ψ˙cosθsinϕ,R=θ˙sinϕ+ψ˙cosθcosϕ.It may be noted that the attitude sensors measure the body angles and not their rates. The derivatives of the attitude angles are computed by fitting a least squares line ton consecutive attitude measurements. We have chosen n=4 here as we have seen it to yield good results; the derivative, thus, computed has enough noise smoothing and an acceptably small time delay. However, other choices for n can be made depending on the application and on the sampling time of the attitude sensor measurements. If, for example, the pitch angle measurements from the vertical gyro are denoted by θ0, θ1, θ2, and θ3, where θ0 corresponds to the current measurement and θ3 corresponds to the measurement taken three samples previously, then the slope of the least squares line for these points approximates the derivative [11]:
(26)θ˙≈3θ0+θ1-θ2-3θ310Δt.Here,Δt is the time interval at which the angle θ is sampled (20 msec for our case). This approximation of the derivative by slope of a 4-point least squares line gives a good compromise between sensitivity to measurement noise and the delay introduced into the estimation of θ˙. Figure 8 shows the delay between the actual and computed derivatives for a pure sinusoid of frequency 10 rad/sec, which is about 25 msec. This method of estimating body rates and using them for feedback control is demonstrated to work well for normal flight maneuvers. A test flight was carried out in which a rate sensor was specially installed on the vehicle for validation purposes only; this sensor is not available for feedback control otherwise. The comparison between the measured and computed yaw-rates shown in Figure 9 indicates the validity of the approximation. It is, thus, concluded that the approximation (26) can be used for Euler angle rate estimation from attitude measurements, and thereafter (25) can be employed for transforming the Euler angle rates into body-axes angular rates which are used for feedback control.Figure 8
Delay introduced because of the 4-point derivative approximation.Figure 9
Measured and computed yaw rates during test flight.


## 5.1.3. The Washout Filter

Yaw-rate feedback is typically useful during transients when the yaw-rate fluctuates or exhibits appreciable changes with time. The rudder is normally employed to suppress the yaw-rate oscillations to provide damping to the Dutch-roll mode of the vehicle. Thus, during steady turns when there is a nearly constant yaw-rate, it is not desirable for the rudder to act to oppose the turning motion. The washout filter is a high-pass filter that is used to feedback the yaw-rate at higher frequencies, while effectively breaking the feedback at dc to enable steady turning of the vehicle. The washout filter is chosen asWw=s/(s+1); it may be thought of as a frequency-dependent weight on the yaw-rate feedback, with a small gain at low frequencies  and unity gain at high frequencies. The 3 dB point occurs at 1 rad/sec, so that frequencies above 1 rad/sec will be available for feedback.


## 5.2.H∞ Loop-Shaping Design Procedure

For controller design, we shall use theH∞ loop-shaping design procedure proposed in [12]. The procedure is intuitive in that it is based on the multivariable generalization of classical loop-shaping ideas. The open-loop plant, once given the desired loop-shape, is robustly stabilized against coprime factor uncertainty. The resulting controller has been shown to enjoy some favourable properties, such as no pole-zero cancellation occurs in the closed-loop system (except for a certain special class of plants), see [13]. In addition, the controllers, thus, designed have been successful in various applications; examples are those described in [14–17].In practical design applications, the performance specifications are first translated into the frequency domain, and the open-loop plant’s singular value frequency response is given the desired shape. This is achieved by augmentation of the nominal plant modelG by pre- and/or postcompensators (or weighting functions) W1 and W2, respectively. The shaped plant Gs=W2GW1 is then robustly stabilized against coprime factor uncertainty, and the controller K, thus, obtained is cascaded with the weights to obtain the final controller W1KW2. It can be shown that the controller does not significantly alter the specified loop-shape provided a sufficiently small value of the cost γ is achieved; for details, refer to [17].We now outline a design procedure for designing robust controllers based on open-loop shaping and robust stabilization of the normalized coprime factors of the plant. The procedure consists of the following main steps.(1)
Plot the singular value frequency response of the open-loop plantG(s). Based on this, select a precompensator W^1 and/or a postcompensator W2 to give the plant a desired open-loop shape. Form the product W2(s)G(s)W^1(s).(2)
Align the singular values ofW2GW^1 at the desired bandwidth. The align gain Ka is the approximate real inverse of the system at the specified frequency. The cross-over (and hence the bandwidth) is thus adjusted to approximately the align frequency. An additional constant diagonal matrix Kg may sometimes be used in front of the align gain to exercise control over actuator usage. It is chosen so that the various actuator rate limits are not exceeded whilst following references or rejecting disturbances. The precompensator can now be written as W1=W^1KaKg; see Figure 10. Build the shaped plant Gs(s)=W2(s)G(s)W1(s), and calculate the optimal cost γopt [12]. A high value (typically > 10) of γopt indicates that the specified loop-shape is inconsistent with robust stability; in such a case, the weights W1 and W2 should be modified.(3)
Use a slightly suboptimal value ofγ, and compute the corresponding controller.(4)
Cascade the controller with the weightsW1 and W2. Controller order reduction may be performed if desired.(5)
Form the closed-loop, and check the appropriate performance and robustness measures against the given specification.Figure 10
The shaped plant and the controller.


## 5.3. Controller Design

Here, we will describe the design of the lateral controller, using the design procedure listed in Section5.2. The block diagram of the closed-loop system is shown in Figure 11. (1)
Singular values of the open-loop plantG(s) are plotted, and these indicate the need for boosting the low frequency gain for good tracking and disturbance rejection for the roll control channel at these frequencies. The low frequency gain is boosted by introducing integral action in the roll control loop; we choose to place a pole at s=-0.01 instead of using a pure integrator (because of the wind-up problems associated with integrators, and also because we do not want to introduce an unstable pole into the shaped plant Gs). W^1 in Figure 10 is chosen as [28(s+2.5)/(s+0.01)0040] and W2 as [14(s+2)/(s+70)0050/(s+50)]. Since we are aiming for a closed-loop bandwidth of approximately 10–15 rad/sec, therefore zeros are introduced at −2.5 and −2 in W^1 and W2 to reduce the roll-off at the cross-over frequencies. The poles at −70 and −50 in W2 are placed to provide adequate roll-off at higher frequencies. Since W^11 (the upper left element of W^1) approximates an integrator, it will be implemented in its conditioned form (see [18, 19]). Gains are selected in W^1 and W2 to adjust the cross-over frequency to around 10 rad/sec.(2)
The align matrixKa is chosen to be the identity matrix for robustness reasons (aligning the nominal plant and designing the controller based on this may not hold good with other models of the plant across the flight envelope) and since both singular values are close enough at cross-over. The matrix Kg is also kept as the identity matrix I2. The precompensator is therefore W1=W^1KaKg=W^1. The shaped plant Gs=W2GW1 is now formed, and its singular values are shown in Figure 12. The low frequency gain in the roll channel is boosted, as indicated by the larger singular value. The washout filter in the yaw-rate channel means that the low frequency gain in this channel remains low, as indicated by the smaller singular value of the shaped plant.(3)
An optimalγ of 1.925 is computed; the controller is obtained for a slightly suboptimal γ of 2.02.(4)
The controller is reduced using optimal Hankel-norm approximation and cascaded with the weightsW1 and W2 to form the complete controller W1KW2. This reduced controller is discretized using the bilinear (Tustin’s) approximation for implementation.Figure 11
Plant, controller, and weighting functions.Figure 12
Singular values of the original and shaped plants.The singular values of the sensitivity function are shown in Figure13. One of the singular values, the one that remains flat at low frequencies, corresponds to yaw-rate and indicates no disturbance rejection on that output. The other singular value shows adequate disturbance rejection on the roll angle channel for frequencies less than 5 rad/sec. A number of linearized models are taken along the periphery of the flight envelope. Step responses for these plant models in the envelope using the designed controller are given in Figure 14. The controller performs acceptably well throughout the envelope, and the robustness of the design is illustrated.Figure 13
Sensitivity function(I+GW1KW2)-1.Figure 14
Step responses for different plant models across the flight envelope.


## 5.4. Robustness Analysis

A more formal robustness analysis of the controller is now performed. Note that the controller design is based on a normalized coprime factor representation of the plant:Gs=W2GW1=M~-1N~, where the transfer functions M~ and N~ represent the normalized coprime factors of the (shaped) plant. The controller is designed to maximize robust stability in the face of coprime factor perturbations. The perturbations (or uncertainties) in the plant are represented as perturbations on the normalized coprime factors of the plant. The perturbed plant GsΔ is given by
(27)GsΔ=(M~+ΔM~)-1(N~+ΔN~),
where ΔM~, ΔN~ are stable unknown transfer functions representing uncertainty in the plant model. Robust stability to coprime factor perturbations is maximized by a controller K which stabilizes the nominal plant Gs and minimizes
(28)γ=∥[KI](I-GsK)-1M~-1∥∞.
The perturbed closed-loop system is then guaranteed to remain stable in the face of all ΔM~, ΔN~ such that [20]
(29)∥[ΔM~ΔN~]∥∞<γ-1.
Robust stability across the flight envelope is tested by obtaining normalized coprime factorizations of all plants in the envelope and then taking their differences from the nominal plant to get uncertainty transfer functions [ΔM~iΔN~i], where the subscript i refers to the i"th"  plant in the envelope. Singular values of these uncertainty transfer functions are plotted in Figure 15; the maximum of these across all frequencies equals 0.42. The cost γ for our controller is 2.02, which indicates that (29) is satisfied, thus ensuring robust stability for the family of plants in the envelope; that is,
(30)∥[ΔM~iΔN~i]∥∞=0.42<2.02-1.
It is seen that a single controller is robust enough to stabilize all plants in the flight envelope; also robust performance is checked for and is found to be adequate by performing step response analysis of the family of plants (Figure 14).Figure 15
Singular values of(ΔM~ΔN~) for different plants in the envelope.


## 6. Flight Test Results

The complete guidance and control scheme is implemented on a real-time embedded computer and flown on the actual UAV for various missions. Part of the two typical missions and their flight results are presented here. Figure16 shows part of the first mission in which waypoints are indicated by small squares, and the actual path flown by the vehicle is shown in solid line. The thick portions of the line indicate that the vehicle is in a state of turn; that is, the turn start criterion is satisfied (see Section 3.2). As soon as the turn stop criterion is met, the line thickness is changed back to normal. The figure shows very good path following by the vehicle during flight. Figure 17 shows the performance of the multivariable roll/yaw-rate controller during flight. In the top part of the figure the commanded and actual roll angles are plotted in dotted and solid lines, respectively. The two lines are almost on top of one another, thus demonstrating the tracking performance of the roll controller. The lower part of the figure shows the roll error which peaks to a maximum value of around 3 degrees at times when there is a sudden change in the roll reference; the error in the steady-state is almost zero. Figure 18 shows the actuator (aileron and rudder) deflections recorded during the same flight. The figure shows a 100 second zoomed view of the flight data. The deflections are small, and the aileron is deflected to about 5 degrees, whereas the rudder deflection stays within 2 degrees during a turn. The multivariable controller engages both actuators to provide good roll angle tracking and stability augmentation. Figure 19 shows the cross-track deviation of the vehicle from the desired path and thus illustrates the tracking performance of the guidance and control system. The performance is excellent with the off-track deviation controlled to within a few meters. The larger 10–20 m errors arise only when a turn is initiated or when one ends, since the reference path to be followed suddenly changes at these instants. Figure 20 shows a zoomed view of different variables of interest related to the turn performance of the vehicle. The bank-to-turn scheme is illustrated as the rolling of the vehicle causes the heading angle to change continuously. The yaw-rate of the vehicle is also shown (which is fed back to the controller through the washout filter). The washout filter blocks the yaw-rate feedback during steady turns and only feeds through the higher frequency transients for stabilization of the Dutch-roll mode of the vehicle.Figure 16
Part of Mission 1 and the path flown by the vehicle.The roll angle and the roll angle error.
(a)
(b)The actuator (aileron and rudder) deflections.
(a)
(b)Figure 19
Cross-track deviation of the vehicle.The turn performance.
(a)
(b)
(c)
(d)Figure21 shows part of the second mission with the waypoints indicated by small boxes and also the actual path flown by the vehicle. The line width is increased to indicate turnings. The top part of Figure 22 shows the commanded and actual roll angles, the commanded angle being generated by the guidance (22). The roll angle tracking provided by the multivariable controller is quite good. The bottom half of the figure shows the roll error which again is seen to be very small and peaks only during transients. The cross-track deviation is shown in Figure 23.Figure 21
Part of Mission 2 and the path flown by the vehicle.The roll angle and the roll angle error for Mission 2.
(a)
(b)Figure 23
Cross-track deviation of the vehicle for Mission 2.


## 7. Conclusion

A practical methodology for the design of lateral-directional guidance and control systems for high performance UAVs is presented. The methodology has been developed keeping in view the practical aspects of the problem and implementation on a real-time platform. Flight path formulae are derived both for straight and turning flight. A nonlinear lateral guidance law is presented that uses the angular error and the cross-track deviation of the vehicle and generates roll angle commands to guide the vehicle back onto the desired path. An adaptive adjustment in the basic law is suggested to cater for large deviations from the nominal track as may arise in case of GPS outages. A robust multivariable controller is designed for the lateral-directional control of the vehicle. The controller uses measurements of the roll angle and yaw-rate of the vehicle and optimally actuates the ailerons and the rudder to provide roll control and Dutch-roll damping. A practical method of computing the heading angle from the magnetometer and then estimating the yaw-rate is presented. The robustness analysis of the controller is performed, and it is seen to perform well throughout the flight envelope of the vehicle. Finally, the flight test results are presented and discussed; these demonstrate the overall performance of the guidance and control system.



---

*Source: 905865-2013-06-11.xml*