ABSTRACT
HAZARD, MATTHEW WESLEY. Unscented Kalman Filtering for Real-Time Atmospheric Thermal Tracking. (Under the direction of Dr. C. E. Hall.)
The increasing use of unmanned air vehicles in military and civilian applications has been accompanied by a growing demand for improved endurance and range. These demands have been largely met by advances in aerodynamic and structural efficiency, improved battery tech- nology, and the ongoing miniaturization of onboard computing and payload systems. Recently, more attention has been paid to the extraction of energy from the atmosphere. Aircraft can make use of atmospheric updrafts, or thermals, to gain altitude without expenditure of onboard fuel stores. By intelligently tracking thermals, an unmanned aircraft can extend its range or loiter time without carrying additional fuel or specialized sensors.
Prior research has focused on the ‘big picture’ concepts associated with autonomous soaring - determining when to stop and soar in a thermal, what speed to fly, when to return to the desired course, and so on. Finding and tracking thermals is only a single component of the complete soaring system. However, because the high-level decision making tasks rely on estimates of the thermal parameters, the accuracy and computational cost of the thermal tracking algorithm set the upper performance limit of the entire system.
So, this research reformulated batch regression thermal finding algorithms used in past ef- forts into an efficient Unscented Kalman Filter. Open-loop simulation results showed the filter was capable of accurately estimating thermal position, strength, and size with low computa- tional cost for a variety of realistic flight paths. Closed-loop simulation reaffirmed this statement in the presence of realistic aircraft, sensor, and thermal dynamics. Further, the algorithm was embedded into the ALOFT soaring platform (a 4.3 m wingspan unmanned glider) for flight testing, which demonstrated its ability to track real-world thermals during cross-country flights exceeding 5 hours flight time over a 70 mile course. c Copyright 2010 by Matthew Wesley Hazard
All Rights Reserved Unscented Kalman Filtering for Real-Time Atmospheric Thermal Tracking
by Matthew Wesley Hazard
A thesis submitted to the Graduate Faculty of North Carolina State University in partial fulfillment of the requirements for the Degree of Master of Science
Aerospace Engineering
Raleigh, North Carolina
2010
APPROVED BY:
Dr. A. Gopalarathnam Dr. F. Wu
Dr. C. E. Hall Chair of Advisory Committee DEDICATION
For my grandfathers, who inspired me to become an engineer.
ii ACKNOWLEDGEMENTS
I’d like to thank my committee, friends, and family for their support in the completion of this research. Specific thanks go to Dan, who got me started on this project, and Dave, who convinced me to stop working and start writing. My dad, Donald, lent an editor’s eye to my
final draft - he is my grammar guru. My mom, Stephanie, deserves special credit for worrying about my health when I haven’t had time.
iii TABLE OF CONTENTS
List of Tables ...... vi
List of Figures ...... vii
Chapter 1 Introduction ...... 1 1.1 Autonomous Soaring ...... 2 1.1.1 Mission Management ...... 3 1.1.2 Autopilot and Airframe ...... 4 1.2 ALOFT Autonomous Soaring Framework ...... 5 1.2.1 Autopilot and Airframe ...... 7 1.2.2 Soaring Controller and Mission Management ...... 8
Chapter 2 Thermal Theory ...... 10 2.1 Thermal Modeling ...... 10 2.2 Thermal Sensing ...... 14
Chapter 3 Thermal Tracking with a Kalman Filter ...... 18 3.1 Kalman Filtering Basics ...... 18 3.2 The Extended Kalman Filter ...... 20 3.3 The Unscented Transform ...... 21 3.4 The Unscented Kalman Filter ...... 25 3.5 Thermal Models in the UKF Framework ...... 28
Chapter 4 Implementation and Simulation ...... 31 4.1 Open-loop Thermal Tracking Simulation ...... 31 4.2 Computational Performance Comparison ...... 37 4.3 Integration with the ALOFT Framework ...... 40 4.4 Closed Loop Simulation ...... 40 4.4.1 Closed-loop tracking using a Wharington thermal model ...... 42 4.4.2 Closed-loop tracking using the Gedeon thermal model ...... 48 4.5 Simulation Summary ...... 52
Chapter 5 Flight testing ...... 53 5.1 Flight testing benefits and caveats ...... 53 5.2 Flight Testing Results ...... 54
References ...... 58
Appendix ...... 59
iv Appendix A MATLAB UKF Implementation ...... 60 A.1 Thermal UKF Closed-Loop Simulation Interface ...... 60 A.2 Thermal UKF ...... 66 A.2.1 Closed-Loop Simulation / Flight Testing Version ...... 66 A.2.2 Sigma Point Selection ...... 69 A.3 Quaternion Algebra ...... 70 A.3.1 Reconstruction of a Quaternion from Euler Angles ...... 70 A.3.2 Rotation of a Vector by a Quaternion ...... 70
v LIST OF TABLES
Table 4.1 Open-Loop Simulation Parameters ...... 33 Table 4.2 Open-Loop Simulation Results ...... 35 Table 4.3 Closed Loop Simulation Parameters ...... 43 Table 4.4 Closed Loop UKF Configuration Parameters ...... 44
vi LIST OF FIGURES
Figure 1.1 Autonomous Soaring Subsystems ...... 3 Figure 1.2 Loosely Coupled Outer and Inner Loop Controllers ...... 6 Figure 1.3 Ground-based and airborne subsystems ...... 6 Figure 1.4 ALOFT and its creator, Dan Edwards ...... 7 Figure 1.5 Cloud Cap Technology Piccolo II ...... 8 Figure 1.6 ALOFT Electronics Installation ...... 9
Figure 2.1 Schematic View of a Thermal ...... 11 Figure 2.2 Wharington thermal model, viewed in 3D ...... 12 Figure 2.3 Gedeon thermal model, viewed in 3D ...... 13 Figure 2.4 Comparison of Wharington and Gedeon thermal models ...... 14
Figure 3.1 Kalman Filter Flow Diagram ...... 19 Figure 3.2 Unscented Transform versus Monte Carlo Simulation ...... 24 Figure 3.3 Unscented Kalman Filter Flow Diagram ...... 25
Figure 4.4 Execution time per iteration for batch processing and Thermal UKF. . . . 37 Figure 4.5 Mean Squared Error for batch processing and Thermal UKF ...... 38 Figure 4.6 Thermal position estimates for batch processing and Thermal UKF . . . . 39 Figure 4.7 Energy Rate History in Closed-Loop Simulation ...... 44 Figure 4.8 Overview of Closed-Loop Simulation using the Wharington Model . . . . 45 Figure 4.9 Altitude Time History in Closed-Loop Simulation ...... 45 Figure 4.10 Closed-Loop Tracking performance for Thermal 3 ...... 46 Figure 4.11 Closed-Loop Tracking performance for Thermal 1 ...... 46 Figure 4.12 Closed-Loop Tracking performance for Thermal 2 ...... 47 Figure 4.13 Overview of Closed-Loop Simulation using the Gedeon Model ...... 49 Figure 4.14 Closed-Loop Tracking performance for Thermal 2 ...... 49 Figure 4.15 Closed-Loop Tracking performance for Thermal 3 ...... 50 Figure 4.16 Closed-Loop Tracking performance for Thermal 1 ...... 50 Figure 4.17 Altitude Time History in Closed-Loop Simulation ...... 51
Figure 5.1 Altitude Trace for Cross Country Flight Test ...... 55 Figure 5.2 Flight Path for Cross Country Flight Test ...... 56 Figure 5.3 Altitude Trace for Cross Country Flight Test ...... 56 Figure 5.4 Flight Path for Cross Country Flight Test ...... 57
vii Chapter 1
Introduction
The use of unmanned aerial vehicles (UAVs) in military and civilian applications is growing rapidly. In many unmanned aerial vehicle applications, endurance, range, and payload capacity are the factors which define the capabilities of the system (in contrast to manned aircraft, where speed, maneuverability, and stealth characteristics play a larger role). Increasing endurance and range by simply adding fuel or battery weight results in loss of payload capacity, so increased efficiency or energy density is needed. Although many advances have been made in structural, propulsion, and aerodynamic efficiency, recent research efforts have turned instead to extracting energy from the atmosphere. The most promising source of atmospheric energy is the thermal, or updraft. By autonomously tracking thermals, unmanned aerial vehicles can augment their internal energy stores, improving their endurance and range. This thesis presents a novel technique for efficiently tracking atmospheric thermals using an Unscented Kalman Filter.
In the remainder of this chapter, we present background information on autonomous soaring, to show how this contribution fits within a larger framework. Chapter 2 continues with details of how thermals are modeled and how we can sense their effect on an aircraft. Next, Chapter 3 shows how an Unscented Kalman Filter (UKF) can be used to efficiently fuse information from thermal models and aircraft sensors. The remainder of the thesis details the implementation and testing of the thermal tracking UKF. Chapter 4 shows how the algorithm was exercised
1 in open-loop and closed-loop simulations. In Chapter 5, we document two flight tests which demonstrate the performance of the algorithm under favorable and adverse conditions. Finally, in the hope that this research may prove of benefit to others (just as the ALOFT framework, detailed in section 1.2, was invaluable to this work), the MATLAB implementation is presented in Appendix A.
1.1 Autonomous Soaring
Atmospheric thermals, or convective updrafts, are naturally occurring sources of energy created by the interaction between solar heating, local terrain variations, and ambient meteorological conditions. An aircraft flying through a thermal can extract energy from the atmosphere. This energy gain can be used to offset energy losses due to drag, fuel expenditure, or maneuvers.
Autonomous soaring involves actively tracking and engaging thermals to maximize energy gains, while achieving a mission. Because UAVs are already equipped with a suite of sensors and computers, adding soaring intelligence may simply require a software upgrade. In aircraft that are designed with soaring in mind from the outset (especially solar-powered or hybrid systems), fuel weight might be traded for increased payload capacity or simply omitted for weight savings.
For instance, the algorithm developed in this work was implemented on a small glider, where all the energy expended by the system (except for the initial winch launch) was first extracted from the atmosphere. For existing UAV designs not specially designed for soaring, the energy gains may be more modest, however even slight improvements in endurance and range are of tactical utility to unmanned aircraft operators.
The amount of energy to be gained from autonomous soaring depends on both the ability of the system to detect and track thermals, as well as the types of maneuvers the system is permitted to perform to engage the thermal. For example, an aircraft that is restricted to flying in a straight line may benefit only slightly from thermals that are encountered along the flight path. On the other hand, an aircraft that (due to relaxed mission requirements and/or a more sophisticated soaring controller or autopilot) can actively turn to fly in an area of rising air
2 Autopilot
Aircraft
Autonomous Soaring
Thermal ID
Mission Management
Figure 1.1: Autonomous Soaring Subsystems will be able to extract more energy from each thermal it passes. It is apparent that the overall performance of a soaring system depends equally on three subsystems: thermal detection and identification, mission management, and the lumped airframe and autopilot. While the first component is the subject of this work and will be discussed in the rest of the document, the other two subsystems are discussed in brief below.
1.1.1 Mission Management
Even at a basic level, there is a tradeoff between the energy gain to be derived from autonomous soaring and the cost in terms of mission performance. If the goal is simply to fly as long as possible (for academic research, for instance), the system designer may elect to always try to
’thermal’, or fly in areas of rising air, when the cost of maneuvering the vehicle to stay in the thermal is outweighed by the energy extracted from the thermal. On the other hand, most
UAV missions require the aircraft to follow a pre-determined route in space and time. In these
3 real-world situations, a compromise must be drawn between the desired mission profile and the maximum-energy-gain approach.
A good deal of literature and approaches to solving this problem can be inherited from the manned sailplane community, for instance MacCready ‘Speed To Fly’ theory and final glide computations. However, because a UAV is a computer controlled system, the optimal balance between soaring and flying a predetermined route might instead be determined using a weighted cost function, where the cost of executing soaring maneuvers depends on the predicted energy gains, aircraft loadout, meteorological information, aircraft drag polars in level and turning
flight, flight regime, and so on.
In many ways the development of this cost function is straightforward. However, the weights for each component vary with the intended mission, and their selection should not be a burden on the UAV operator. Future work might develop mission planning algorithms in which the
UAV completes a series of tasks, and is free to select the best method to achieve the task.
Waypoint sequences could be replaced with goals e.g., arrive at destination x1 by t1, loiter as long as possible (but at least t2), then return home with at least f percent fuel remaining. While these techniques might reduce workload on the operator and ease the development of cost functions for optimizing energy gains from soaring, significant challenges such as monitoring, failsafing, and computational cost remain.
1.1.2 Autopilot and Airframe
The autopilot provides an interface between the actual airframe and the soaring controller.
It provides a set of standard maneuvers which may be selected by the soaring controller in order to effect the desired flight. Generally the minimal set of autopilot features required for autonomous soaring are the selection of a target airspeed and/or altitude, the ability to ‘orbit’ or fly around a point, and the ability to fly a sequence of waypoints. For unpowered aircraft, altitude tracking is impossible; instead, airspeed is varied to maintain an efficient glide slope.
For powered aircraft, altitude tracking may be relaxed to ensure the aircraft does not descend
4 below a critical altitude but can still gain energy from thermals. Accurate airspeed tracking is still essential to ensure efficiency of the aircraft and its propulsion system.
The role of the autopilot in the soaring framework is analogous to the brain stem in the human nervous system. The brain handles higher-level tasks such as thinking and planning, while the brain stem is the primary interface to the body’s motor and sensory functions. Ex- ecution of higher-level tasks depends on proper functionality of the brain stem. Similarly, the autopilot translates high-level guidance commands into low-level aircraft actuator commands which achieve the desired response without exceeding the limits of the aircraft. In the human body, the brain stem prevents important basic functions (e.g., breathing, temperature regula- tion) from being forgotten while the brain works on other more interesting tasks. Similarly, the ‘separation of powers’ in the soaring framework prevents an experimental soaring controller from causing a system failure. However, lacking the sophistication of the human nervous sys- tem, achieving a high level of performance with a loosely coupled autonomous soaring system is challenging. Tighter integration of the soaring controller and autopilot will likely result in higher system performance (due to the reduction in system weight, communication latencies, and so on) at the possible expense of system safety.
Due to the loosely coupled nature of our prototype autonomous soaring system, we can lump together the autopilot and airframe as a single system. Just as the body ceases to function without the brain stem (and vice-versa), it is difficult to separate the performance or limitations of the aircraft from the abilities of the autopilot. If a different host vehicle is selected, a new autopilot (or at least a new autopilot configuration) will be needed to fly it.
1.2 ALOFT Autonomous Soaring Framework
So far we have discussed autonomous soaring on a conceptual basis. Now let’s look at a specific example of an autonomous soaring system, ALOFT. ALOFT, or Autonomous Locater
OF Thermals, is an unmanned aircraft system developed by another NCSU researcher, Dan
Edwards, in partnership with the US Naval Research Laboratory. It can be seen as a practical
5 Inner Loop Aircraft Specific Controller u y Autopilot Aircraft Sensors −
Thermal ID
Generate Commands Is Good Lift?
Speed To Fly Outer Loop Soaring Controller
Figure 1.2: Loosely Coupled Outer and Inner Loop Controllers
Current ALOFT System Future ALOFT System
Airborne Airborne
Aircraft Aircraft
Autopilot Autopilot
Sensors Sensors
Thermal ID High Bandwidth Reduced Computational Cost Is Good Lift? Thermal ID Speed To Fly Is Good Lift?
Speed To Fly Low Bandwidth
Supervision Supervision
Ground Ground
Figure 1.3: Ground-based and airborne subsystems
6 Figure 1.4: ALOFT and its creator, Dan Edwards
first-pass implementation of the ideal soaring framework discussed above, and encompasses autonomous soaring, mission management, and autopilot subsystems. In this work, we will often refer to ALOFT as a soaring framework, simply to remind the reader that the system is more than just an airplane and an autopilot. The details of the hardware are less important to this research than its general structure. As shown in Fig. 1.3, ALOFT is still a work in progress. In the current ALOFT implementation, all the soaring-related computations are done on the ground, but as the computational efficiency of the soaring algorithms is increased, these subsystems will be moved onboard, eliminating communications latency as a limiting factor on soaring performance. Key features of the system will be listed below; for additional details the reader can consult [1].
1.2.1 Autopilot and Airframe
ALOFT is built upon a successful commercially available remote control sailplane called the
SBXC. It has a wingspan of 4.3m and weighs about 12 pounds. The unpowered glider can be launched using a winch to an altitude of approximately 300 feet. The SBXC is reasonably
7 Figure 1.5: Cloud Cap Technology Piccolo II efficient for a small glider, boasting a max L/D of 24:1. A Cloud Cap Technology Piccolo II autopilot is integrated with the airframe, satisfying the basic requirements mentioned in the subsystem section above.
Aircraft telemetry is transmitted to a ground station at a nominal rate of 20Hz, although communications latency due to range and antenna orientation can affect the actual data rate.
Basic failsafe and envelope-limiting functionality is incorporated into the autopilot; the autopilot can automatically return to a predesignated rally point if communication is lost or other errors occur. A remote-control override is provided to allow a ground-based observer to take control of the aircraft in case of malfunction, as well as to launch and recover the aircraft.
1.2.2 Soaring Controller and Mission Management
The soaring subsystem is implemented on a PC connected to the autopilot ground station.
Aircraft telemetry information is decoded from the live serial stream and handed to a thermal identification module. Briefly, the thermal identification module uses the telemetry information
8 Figure 1.6: ALOFT Electronics Installation to predict the location, strength, and size of the thermal. More details will be presented in the following chapters. A separate mission management routine computes an average rate of energy gain required to stop and circle in a thermal. The threshold is computed based on the current altitude of the aircraft (stopping the aircraft for smaller thermals if closer to the ground), the expected average strength of thermals, and MacCready theory. An ad-hoc aggression factor is used to manually adjust the threshold setting during flight testing, as the system cannot adjust the average thermal strength (which largely depends on weather and location) automatically.
9 Chapter 2
Thermal Theory
2.1 Thermal Modeling
In order to find and track thermals, we first need to know what we’re looking for. Thermals are generated by differences in air temperature at ground level. Just like the ‘usual’ horizontal wind, these vertical updrafts come and go depending on environmental conditions, terrain, the type of vegetation, weather, and season. However, vertical air movement also induces a local horizontal flow and vorticity around the edges, as shown in Fig. 2.1. With so many variables that affect thermal properties, it’s clear that some simplifying assumptions need to be made in order to make the modeling problem tractable. In general, for a single isolated thermal, the vertical wind strength at a given location varies with the distance from the center of the thermal, and the height above the ground. Additional constraints on the distribution of vertical wind speed in a thermal can improve the fidelity of the simulation at the expense of computational effort and complexity.
A variety of ad-hoc thermal models have been developed that describe the vertical wind profile of a single thermal as a simple, low-order model. At the other end of the complexity spectrum, there are sophisticated thermal models based on meteorological data and a more rigorous application of the laws of conservation of mass and momentum, although simplify-
10 Figure 2.1: Schematic View of a Thermal
ing assumptions are still used. For example, Allen used balloon measurements to identify the statistical variation of key underlying meteorological parameters which control the shape and distribution of thermals [2]. These parameters were used to create a comprehensive vertical wind model that varied with distance from the core in the horizontal plane and with altitude.
However, the relative sophistication of the Allen model also comes with computational complex- ity; it is more suitable for use in simulation than for use in a real-time thermal tracker. In this work, only ad-hoc thermal models were embedded in the thermal tracking algorithm. But sim- ulation and flight test data confirm that the ad-hoc models used are sufficient approximations to give useful results in higher-fidelity simulations and reality.
The ad-hoc thermal models used in this work make several common assumptions: thermals are radially symmetric around a center point, do not create significant horizontal wind gradients, are time-invariant, and do not change with altitude. Categorically, the distribution of vertical wind speeds varies with the distance r away from the center of the thermal. The size and shape of the thermal is controlled by two key constants, the maximum thermal strength W and its characteristic radius R. Generically, the form of a low-order thermal model is given by
w = f (r, W, R) . (2.1)
11 Figure 2.2: Wharington thermal model, viewed in 3D
Considering the thermal to be located at some absolute position r0 = (x0, y0), the thermal strength at the sampling position r1 = (x1, y1) would then be given by substituting the Eu- clidean distance in the XY plane (recall that the models neglect variation in altitude) for r, giving
p 2 2 w = f (x1 − x0) + (y1 − y0) , W, R . (2.2)
One of the simplest ad-hoc thermal models was used by John Wharington in his dissertation on autonomous soaring. It represents the distribution of vertical wind speed as a Gaussian function in radius, given in (2.3). The thermal strength decays from W down to zero far away from the center. A 3D view of the Wharington thermal model is given in Fig 2.2.
r 2 w = W e−( /R) (2.3)
The Wharington model does not predict sinking air (as long as the strength term is positive), but conservation of mass and momentum require air to descend as often as it ascends. Perhaps
12 Figure 2.3: Gedeon thermal model, viewed in 3D
this explains the shape of another thermal model, which predates Wharington’s work by several decades. We will refer to it as the Gedeon model, as presented by Langelaan [3]. It takes the form of a Gaussian weighted by a quadratic function,
r 2 r 2 w = 1 − W e−( /R) , (2.4) R which results in a trough of sinking air around the core of rising air. The result is shown in Fig.
2.3. The Gedeon model was developed on the basis of manned sailplane flights in the 1970s.
Although it is a low order model, it represents the basic structure of a thermal fairly well. It has recently seen use in Langelaan’s gust soaring research.
Both of these low-order models were used in simulation and flight testing, although the
Wharington model was preferred. Due to the fundamental difference in shape, thermals created using different models cannot be easily compared, even though their characteristic parameters may be the same. However, two thermals created with each of the models are presented for qualitative comparison in Fig. 2.4.
13 Thermal model comparison, W = 1m/s, R = 100m 1.0 Wharington Model Gedeon Model 0.8
0.6
0.4
0.2 vertical wind speed, m/s
0.0
0.2 0 50 100 150 200 250 300 distance from center, m
Figure 2.4: Comparison of Wharington and Gedeon thermal models
2.2 Thermal Sensing
The goal of autonomous soaring is to add energy to the entire unmanned vehicle system and reduce the amount of energy expended by the propulsion system. Achieving this goal requires actively monitoring the energy state of the vehicle. Including the most significant terms, we can reduce the system energy to gravitational potential energy and translational kinetic energy:
mV 2 E = mgh + i , (2.5) 2
where Vi is the aircraft velocity magnitude in the inertial (i.e., NED) reference frame. An aircraft (especially an efficient glider) can exchange potential energy for kinetic energy with minimal loss. Therefore we can relate the total energy of the vehicle to an equivalent altitude which could be gained if all the kinetic energy was converted exactly into potential energy:
E V 2 h ≡ = h + i , (2.6) e mg 2g
14 which is the same energy-height or specific energy used in aircraft performance computations.
Differentiating, we get the rate of change of specific energy, or specific power,
dh dh V dV P ≡ e = + i i , (2.7) s dt dt g dt where the left hand side is the specific power being added or removed from the system, and the right hand side is its effect on the aircraft kinematics. For powered aircraft, Ps is usually taken to be excess propulsion power, neglecting other contributions. For gliders, on the other hand, it is clear that Ps must come from the atmosphere. Without loss of generality, we can divide
Ps into separate contributions from the propulsion system and atmosphere,
Ps = Ps,propulsive + Ps,atmospheric, (2.8)
where Ps,propulsive is the usual excess power[4]:
T − qC S P = V D . (2.9) s,propulsive a W
The excess power term includes the total aircraft drag coefficient, CD. The drag state of the aircraft in steady level flight depends on the current airspeed and load factor, as well as aircraft geometry. The drag polar is typically established using flight test data, although estimated values could be used when a better value is not available. The specific power for an electrically powered aircraft could be estimated by applying wind-tunnel measurements of motor and propeller efficiencies to the instantaneous electrical power consumed by the propulsion system.
Observing that the atmospheric contribution to specific power can be represented directly as a change in energy-height over time, or equivalent vertical wind speed, we can express it using similar notation to the thermal models presented earlier:
wˆ ≡ Ps,atmospheric. (2.10)
15 Having accounted for the most important sources of specific power, we can solve for the instan- taneous vertical wind speed based on an observed response in aircraft kinematics and known throttle input:
wˆ = Ps − Ps,propulsive (2.11) dh V dV qC S − T = + i i + V D . dt g dt a W
At this point we have developed a relationship which gives the vertical wind speed in terms of kinematic state variables. We can confirm that this manipulation makes sense by considering
dVi a glider in unaccelerated flight in still air. With T =w ˆ = dT = 0 and L u W , (2.11) can be rearranged to give dh/ L dt = − . (2.12) V D
Or in other words, the aircraft must be descending at a sink rate related to the current aircraft lift-to-drag ratio, which makes intuitive sense. By similar manipulations we can show that ambient vertical wind will directly offset sink rate and that an unpowered plane may climb only by decelerating i.e., in a zoom climb.
If we allow the velocity to be a 3-dimensional vector in the inertial frame (as measured by
GPS), we can use the output from the autopilot’s 3-axis accelerometer to avoid taking finite differences. The accelerometer measures the specific force experienced by the aircraft in the body frame, so we need to first rotate the accelerometer measurement by the current attitude quaternion and subtract gravity to get the kinematic acceleration in the inertial frame:
−1 T ai = qaˆ bqˆ + [0 0 9.8] , (2.13) where qˆ is the estimated attitude quaternion obtained from autopilot telemetry and qˆ−1 repre- sents the quaternion conjugate (or inverse), in which the complex (vector) components of the quaternion change signs. Finally, (2.11) must also be modified to accommodate vector terms
16 instead of scalars. Replacing scalar multiplication with the vector dot product gives
dh V • a qC S − T wˆ = + i i + V D . (2.14) dt g a W
In summary, we have developed a measure of the energy contributions from the atmosphere that is compensated for propulsion effects and local acceleration and is based on parameters readily available in the autopilot telemetry stream. Using this information, the atmospheric energy-rate signal can be reconstructed in real time. Now we have two sources of information: models which predict the specific power the aircraft should absorb based on thermal parameters, and a measurement of the actual specific power absorbed by the vehicle. These two sources of information can be fused to estimate the thermal parameters, and ultimately to guide the aircraft to increase the energy gains. This task will fall to an Unscented Kalman Filter, which is the topic of the next chapter.
17 Chapter 3
Thermal Tracking with a Kalman Filter
As mentioned in the previous chapter, we need to develop a system for efficiently tracking thermals in real-time. By implementing a recursive estimator in place of the batch regression used by the basic ALOFT framework, we reduce the data storage and computational cost requirements while improving the performance of the overall autonomous soaring system. This will allow autonomous soaring algorithms to be embedded into a UAV’s onboard flight control system rather than running on a ground station computer.
3.1 Kalman Filtering Basics
The Kalman Filter (KF) is the optimal recursive estimator for linear systems. The recursive nature of the filter means that only the previous state and covariance estimates are required to extrapolate a new state estimate, and only the most current measurement is needed to correct the extrapolation errors. A flow diagram of the Kalman Filter is shown in Fig. 3.1.
As shown in the diagram, the filter algorithm can be divided into two distinct phases. In the
18 zk
xˆ+ − + k−1 xˆk xˆk Φk−1 H K k − k
Pˆ+ ˆ− ˆ+ k−1 Pk Pk Φk−1 −
Qk−1 HkKk
Prediction Observation
Figure 3.1: Kalman Filter Flow Diagram prediction phase, the state and covariance estimates are extrapolated forward in time. In the observation phase, the state estimate is used to estimate the output and a covariance matrix for the output prediction. The covariance of the estimate of the output is used to compute the Kalman Gain and fuse the predictions and measurements to yield the optimal estimate.
The prediction phase is typically repeated at a constant frame rate while the observation phase is executed asynchronously as measurements arrive. Because the Kalman Filter has been well known for several decades, we will only present a summary of its basic structure and properties.
The interested reader can refer to [5] for a complete derivation.
The classic Kalman filter is suitable for systems that can be modeled in discrete-time with linear process and measurement dynamics subject to Gaussian noise, or
xk = Φk−1xk−1 + wk−1, wk∼N(0, Qk) (3.1)
zk = Hkxk + vk, vk∼N(0, Rk) (3.2)
19 At each time step, the estimator predicts the next state based on the current state. The estimate of the error covariance is likewise predicted forward in time.
− xˆk = Φk−1xk−1 (3.3) ˆ− ˆ T Pk = Φk−1Pk−1Φk−1 + Qk−1 (3.4)
Between measurements, the filter ‘coasts.’ The accuracy of state estimates while coasting depends on the fidelity of the model; if the process model is not well known, and the process noise is large, the filter’s estimate of the state will become uncertain very quickly, and intermediate prediction steps may not be beneficial. On the other hand, if the process model is reliably known (and accompanied by an equivalently small process noise), the filter may be expected to give confident estimates for extended periods of time between observations. The process noise is therefore an important ‘knob’ the designer can tune to account for modeling errors, usually on an ad-hoc basis. When a measurement becomes available, the Kalman Gain matrix is computed to weight the statistical value of data from the measurement source and the prediction. The two sources are combined to produce an updated state and covariance estimate.
ˆ− T ˆ− T −1 Kk = Pk Hk [HPk Hk + Rk] (3.5)
− − xˆk = xˆk + Kk[zk − Hkxˆk ] (3.6) ˆ ˆ− Pk = [I − KkHk]Pk (3.7)
3.2 The Extended Kalman Filter
Due to the nonlinear nature of our low-order thermal models, we cannot apply the classic
Kalman filter directly. A number of alternate formulations for nonlinear systems have been developed, primarily for inertial navigation applications. The most common is the Extended
Kalman Filter (EKF). In the EKF, the mean is propagated using a nonlinear function of the prior mean. The covariance is separately propagated through a linearized model of the system
20 dynamics. In other words, the prediction and measurement functions are nonlinear, but do not take into account the prior covariance distribution, and the covariance propagation is only a
first order approximation [6]. For many applications, the relative ‘sub-optimality’ of the EKF is insignificant when estimation errors are scored next to modeling errors - without infinite computing capability, certain simplifications of higher order terms and sources of error are not only necessary but prudent [5].
Therefore, the EKF has found widespread use, but its tendency to diverge under poor initial conditions has been well documented [7]. This tendency results from the inability of linearized dynamics to accurately model the propagation of covariance information through nonlinear functions (a violation of the assumption of local linearity) [8]. In many cases, the robustness of the EKF can be improved by adding artificial process noise, but this can also inhibit the estimation accuracy. Additionally, the computation of Jacobian matrices for linearizing the system can be a source of errors in its own right, and presents a significant roadblock to rapid development of an estimation algorithm.
3.3 The Unscented Transform
The deficiencies of the Extended Kalman Filter have been largely resolved by the introduction of the Unscented Kalman Filter, which eliminates the linearization step altogether, replacing it with the ‘Unscented Transform’. The complete derivation of the Unscented Kalman Filter can be found in other works, for example,[6] and [7], and will not be repeated here. Instead, we will present a survey of the salient features of the UKF as required to implement this work; the interested reader can refer to these sources for more details.
The Unscented Transform (UT) generates a minimal set of sample points that completely capture the mean and covariance of a random variable. The first sigma point is selected to be the expected value of the distribution,while the rest of the sigma points are chosen to lie symmetrically around the mean, distributed along the basis vectors of the covariance matrix.
A scaling parameter is used to control the spread of the sigma points along these axes. In other
21 words, the set of sigma points is given by
X 0 = E[x] (3.8a) p X i = E[x] + γ( Px)i i = 1,...,N (3.8b) p X i = E[x] − γ( Px)i−N i = N + 1,..., 2N, (3.8c)
√ where ( Px)i is the ith column of the matrix square root (i.e., Cholesky decomposition) of the covariance matrix and
λ = α2(N + κ) − N √ (3.9) γ = N + λ.
The sigma points are then propagated through the nonlinear function, giving the transformed set of sigma points Y:
Yi = g (Xi) (3.10)
The posterior statistics of the random variable are then recovered using a weighted sample mean and covariance algorithm:
2N X y ≈ wi,meanYi (3.11) i=0 2N X T Py ≈ wi,cov(Yi − y)(Yi − y) (3.12) i=0 2N X T Pxy ≈ wi,cov(X i − x)(Yi − y) (3.13) i=0
The weights are selected to match the scale factors used to generate the sigma points. The weight for the zeroth sigma points is slightly different for the mean and covariance, but are the
22 same for the rest of the sigma points:
λ λ w = w = , i = 1,..., 2N (3.14a) 0,mean γ2 i,mean 2γ2 λ λ w = + (1 − α2 + β) w = , i = 1,..., 2N. (3.14b) 0,cov γ2 i,cov 2γ2
Although the algorithm represents Monte Carlo Sampling in the propagation and poste- rior recovery of statistics, the selection of sigma points is not random. The two techniques are compared in Fig. 3.2. The strategic selection of sigma points means that although the transformation accurately propagates mean and covariance to the second order (third order for
Gaussian inputs), it does so at relatively low computational cost.
23 (a) Monte Carlo Points (b) Sigma Points
(c) Transformed Monte Carlo Points (d) Transformed Sigma Points
Figure 3.2: Unscented Transform versus Monte Carlo Simulation
24 Rv zk Rn
xˆ− + k + xˆk−1 xˆk UKF Prediction UKF Observation Pˆ+ Pˆ+ xk−1 xk Pˆ− xk
+ ∗ − X k−1 X k X k Zk
xk = f(xk−1) zk = h(xk)
Process Model Observation Model
Figure 3.3: Unscented Kalman Filter Flow Diagram
3.4 The Unscented Kalman Filter
The Unscented Transform can be used to replace the linear state and covariance propagation equations to accommodate nonlinear process and measurement models within the framework of the Kalman Filter . The result is known as the Unscented Kalman Filter. The proceeding derivation follows from [7] under the assumption of additive noise. We have already detailed how the UT is used to compute the posterior statistics of a random variable propagated through a nonlinear function. We can see that the UT will need to be used once in the prediction phase and again in the observation phase.
In the prediction phase, it will be used to extrapolate the state and covariance estimates forward in time. Recall that in the Kalman Filter, the process model was given as a linear function of the previous state (3.3). In the UKF, the system model is allowed to be an arbitrary nonlinear function,
xk = f(xk−1, wk−1), wk∼N(0, Qk). (3.15)
The UT is used to replace the matrix formulation for predictions of the prior mean and covari-
25 ance. A set of sigma points is selected around the previous state estimate (per 3.8),
" r r # + + + ˆ+ + ˆ+ X k−1 = xˆk−1 xˆk−1 + γ Pxk−1 xˆk−1 − γ Pxk−1 . (3.16) i i
The sigma points are individually propagated through the nonlinear state transition function to obtain the prior set of sigma points,
∗ + X k,i = f(X k−1,i), (3.17) from which the prior mean and covariance are computed, using the weighted sums described in
(3.11), giving:
2N X xˆ− = w X ∗ (3.18) k i,mean ki i=0 2N X ∗ − ∗ − T Pˆ − = w (X − xˆ )(X − xˆ ) + R . (3.19) i,cov ki k ki k v xk i=0
In the observation phase, the UT is used to predict the measurement and its covariance, which will then be used to compute the Kalman gain and fuse the residuals with the state estimate. As in the prediction phase, the measurement model in the UKF is allowed to be a nonlinear function of the state:
z = h (xk, vk) , vk∼N(0, Rk) (3.20)
The observation phase begins with a new set of sigma points, drawn around the prior mean and covariance,
" r r # − − − ˆ− − ˆ− X k = xˆk xˆk + γ Pxk xˆk − γ Pxk , (3.21) i i which is the same formulation given before, except the prior (extrapolated) estimates are used
26 rather than those from the previous time step, which would not include the addition of process noise. Once the sigma points are selected, they are propagated through the nonlinear output function h,
Z = h(X − ), (3.22) k ki from which the best prediction of the output mean and covariance can be derived, again using the weighted sums from (3.11),
2N X zˆ = w Z− (3.23) k i,mean ki i=0 2N X P = w (Z− − zˆ−)(Z− − zˆ−)T + R (3.24) zk i,cov ki k ki k n i=0 2N X P = w (X − − xˆ−)(Z− − zˆ−)T , (3.25) xkzk i,cov ki k ki k i=0 noting that in the observation phase we also need to compute the cross-covariance term. Now, we can compute the Kalman Gain, which can be considered to be the ‘ratio’ between the uncertainty in the state estimate and the uncertainty in the measurement:
K = P P −1. (3.26) k xkzk zk
Now all that remains is to use the Kalman Gain to compute the posterior state and covariance estimates,
+ − − xˆk = xˆk + Kk zk − zˆk (3.27)
P + = P − − K P KT , (3.28) xk xk k zk k which is the prior covariance minus the predicted measurement covariance, weighted by the
Kalman Gain.
The use of the Unscented Transform for mean and covariance propagation means arbitrarily
27 nonlinear prediction and measurement functions can be embedded in a Kalman Filter frame- work in purely functional form. As seen in Fig. 3.3, this use permits the Unscented Kalman
Filter to become a ‘black box’ in the sense that different measurement and prediction functions can be immediately interchanged without altering the fundamental structure of the filter. The
UKF is an obvious improvement upon the Extended Kalman Filter, which would require the computation of Jacobians, a laborious task. In a research environment, the generic nature of the Unscented Transform permits the interchange and development of a standard Unscented
Kalman Filter algorithm, upon which complete estimators can be constructed with the imple- mentation of only the appropriate state transition and measurement functions.
3.5 Thermal Models in the UKF Framework
We already have our thermal models in functional form, so integrating them with the UKF framework is almost trivial. In order to generate orbit commands for the autopilot, we need to know the position of the thermal, and knowledge of the thermal strength and radius will help us plan our flight path and make mission management decisions. So the state vector we wish to estimate can be written as W R x ≡ (3.29) rx ry
As stated in Section 2.1, the low-order models assume that thermals are time-invariant, therefore the thermal has zero dynamics and our state transition function can be written as:
− + xk = xk−1. (3.30)
This allows us to skip the prediction step for propagating the mean, but we still need to deal with the covariance. Although the stationary dynamics of the system will not alter the covariance,
28 the process noise (which accounts for the inaccuracy of this prediction) must still be added.
Adding process noise has the effect of converting stationary dynamics into a random walk. So we can rewrite the covariance update equation given in (3.19) for this specialized case as
P − = P + + R , (3.31) xk xk−1 v
where Rv is a diagonal matrix of process noise values. Because the process noise corresponds to unmodeled states, the values are tuned on an ad-hoc basis instead of appealing to a theoretical foundation. The typical value used in this work is given by
0.005 2 √ Rv ≈ dt (3.32) 4 4
Now we turn our attention to the measurement phase. We start by assuming we have a sensor that can measure the specific power of the aircraft,which could be a synthetic sensor reading obtained by reconstructing the aircraft specific power state from autopilot telemetry informa- tion, as given in (2.11). Our thermal models give us what the sensor should read based on the current state estimate, and the current position of the plane. We have two choices for measurement function z = h (x) , depending on which thermal model we choose, either
−d/x2 zw = x0e 1 or (3.33) d zg = 1 − 2 zw, with (3.34) x1 2 2 d = (rz,x − x2) + (rz,y − x3) , (3.35)
where zw gives the Wharington model output, zg gives the Gedeon model output, and r0 and r1 are the position of the aircraft along the local east and north axes, respectively.
29 We must also specify the measurement noise matrix, Rn, which is in this case reduced to a scalar. For our tests aboard the ALOFT platform, where the sensor reading was reconstructed from telemetry from the Piccolo II autopilot, a value of
Rn ≈ 0.1 (3.36) was found to be suitable.
30 Chapter 4
Implementation and Simulation
Once the algorithm for tracking thermals was prepared, a prototype implementation was de- veloped and exercised in a variety of simulation environments. Along with giving an indication of the performance of the algorithm, the simulation environments helped reduce the cost of
flight testing by exposing programming errors before being used in flight critical applications.
The specialized UKF routines derived in section 3.5 were implemented as MATLAB functions, which can be found in Appendix A. Two variants were created, one for simulation and devel- opment and one for flight testing. For the simulation implementation, the primary interest is evaluating the accuracy of the algorithm and the time-history of the estimate, so extensive logging is performed. In the flight testing implementation, these extra features are stripped away to yield the highest performance on an onboard computer within the ALOFT framework.
4.1 Open-loop Thermal Tracking Simulation
The first stage of algorithm testing evaluated the performance of the algorithm at tracking a single thermal given an arbitrary open-loop flight path. Representative flight-paths that were tested included straight lines, sinusoidal flight paths of varying frequency and amplitude, and circling flight paths. In this initial stage of testing, aircraft and sensor dynamics were not modeled. Instead the specific power measurement for each sampling position along the nominal
31 path was taken from the Wharington thermal model given in 2.3 and corrupted with white,
m zero-mean Gaussian noise with a standard deviation of 0.1 /s. The four flight paths used for open-loop thermal tracking simulation are given in Figure 4.1. For each flight path, the colored track indicates the thermal vertical wind speed for the Wharington thermal model given in (2.3).
The thermal tracking algorithm was configured identically for all test runs in order to give a good assessment of performance with various flight path profiles. The filter was initialized at the first data point of the flight path, assuming no a priori knowledge of the thermal’s position.
The configuration parameters used in simulation are summarized in Table 4.1.
Once the simulated flight paths were created, the points were passed to the thermal UKF in a sequential manner. At each time update, a new GPS position and energy measurement were provided. The prediction step was run at the same frame-rate as the measurement step; because the state variables are assumed to have zero dynamics, running the prediction step at a faster rate gives no new information. The system state and estimated covariance was logged at each timestep, providing insight into the performance of the algorithm over time. The results are summarized in Table 4.2. Figure 4.2 shows the time-history of the aircraft position, the estimated thermal position, and the final covariance ellipse for the thermal position tracking states. Figure 4.3 shows the time-history of the diagonal elements of the covariance matrix.
The results show good convergence to the true thermal position in all cases except for the straight line flight case, shown in Figure 4.2d, where the filter estimate seems to have converged in the x-axis but not in the y-axis. This apparent divergence is further explained by examining the covariance estimate for the same flight path, given in Figure 4.3d. It is clear that the filter’s estimated covariance in the x axis has dropped from the initialization point, indicating new information was received and incorporated into that state variable. For the y axis, however, the covariance element continues to grow (by the process noise added on each cycle) indicating no new information has been received. In fact, the flight path of the vehicle has given adequate energy gradient measurements to locate the thermal along the direction of flight, but not along the axis perpendicular to the vehicle trajectory. This leads to the general conclusion that in
32 Table 4.1: Open-Loop Simulation Parameters.
Parameter Value Units
Thermal Parameters
m Peak Strength, W 3.0 /s Radius, R 150.0∗ m
Position, (x0, y0) (500, 500) m
UKF Parameters
Process Noise, W 0.0025
Process Noise, R 0.25
Process Noise, (x, y) 2.0
Specific Power Noise,w ˆ 0.1
UKF Initialization Parameters
Initial Covariance, W 1.0
Initial Covariance, R 100.0
Initial Covariance, (x, y) 1000.0
m Initial State, W 2.5 /s Initial State, R 100.0 m
Initial State, (x, y) First Sample Location ∗ Except for the small sinusoidal flight path, which used a thermal radius of 75m.
33 750 3.0 3.0 520 700 2.8 2.8 2.5 500 2.5 650 2.3 2.3 600 480 2.0 2.0 550 1.8 460 1.8 500 1.5 1.5 440 450 1.3 1.3 y−coordinate, m y−coordinate, m 1.0 1.0
400 thermal strength, m/s 420 thermal strength, m/s 0.8 0.8 350 0.5 400 0.5 300 0.3 0.3 380 250 0.0 0.0 200 300 400 500 600 700 800 350 400 450 500 550 600 650 x−coordinate, m x−coordinate, m (a) Large Sinusoidal Flight Path (b) Small Sinusoidal Flight Path
700 3.0 451 3.0 2.8 650 450.8 2.8 2.5 2.5 600 450.6 2.3 2.3 450.4 550 2.0 2.0 1.8 450.2 500 1.8 1.5 450 1.5 450 1.3 449.8 1.3 y−coordinate, m y−coordinate, m 400 1.0 1.0 thermal strength, m/s
449.6 thermal strength, m/s 0.8 350 0.8 449.4 0.5 0.5 300 0.3 449.2 0.3 250 0.0 449 0.0 200 300 400 500 600 700 800 100 200 300 400 500 600 700 800 x−coordinate, m x−coordinate, m (c) Circling Flight Path (d) Straight Line Flight Path
Figure 4.1: Simulated Open-Loop Flight Paths order to fully constrain the thermal tracking state, the vehicle flight path must contain turning
flight. So the apparent divergence of the filter is not true divergence, which would indicate an instability in the tracking algorithm, but instead a product of the choice of flight path. This is promising for closed-loop soaring, where once a thermal is detected the vehicle will be instructed to orbit (circle) in the updraft. Regardless of whether the aircraft turns the ‘correct’ direction to capture the thermal, the turn will establish the energy gradient in the off-flight-path direction, promoting convergence to the true thermal location.
34 Table 4.2: Open-Loop Simulation Results.
Parameter Flight Path
Large Sines Small Sines Circling Straight Line
Number of samples used 600 300 600 600
Initial position error 357 m 167 m 173 m 403 m
Final position error 14 m 2 m 1.8 m 50 m
m m m m Final strength error -0.11 /s 0.26 /s 0.24 /s 1.0 /s Final radius error -2.2 m 0.4 m -8.3 m -12.0 m
true thermal position true thermal position estimated thermal position estimated thermal position last estimate last estimate est covariance est covariance 800 3.0 520 3.0 2.8 2.8 700 2.5 500 2.5 2.3 2.3 600 2.0 480 2.0 1.8 1.8 500 1.5 460 1.5 1.3 1.3
y−coordinate, m 400 1.0 y−coordinate, m 440 1.0 0.8 0.8 thermal strength, m/s thermal strength, m/s 300 0.5 420 0.5 0.3 0.3 200 0.0 400 0.0 100 200 300 400 500 600 700 800 300 350 400 450 500 550 600 650 x−coordinate, m x−coordinate, m (a) Large Sinusoidal Flight Path (b) Small Sinusoidal Flight Path
true thermal position true thermal position estimated thermal position estimated thermal position last estimate last estimate est covariance est covariance 700 3.0 500 3.0 2.8 2.8
600 2.5 480 2.5 2.3 2.3 2.0 2.0 500 1.8 460 1.8 1.5 1.5 400 1.3 440 1.3
y−coordinate, m 1.0 y−coordinate, m 1.0 0.8 0.8
300 thermal strength, m/s 420 thermal strength, m/s 0.5 0.5 0.3 0.3 200 0.0 400 0.0 350 400 450 500 550 600 650 700 0 100 200 300 400 500 600 700 x−coordinate, m x−coordinate, m (c) Circling Flight Path (d) Straight Line Flight Path
Figure 4.2: Open-Loop Tracking Overview
35 1200 σW 1200 σW σR σR σXt σXt 1000 1000 σYt σYt
800 800
600 600
400 400 Covariance diagonal term Covariance diagonal term
200 200
0 0 0 100 200 300 400 500 600 0 50 100 150 200 250 300 sample number sample number (a) Large Sinusoidal Flight Path (b) Small Sinusoidal Flight Path
1000 σW 1600 σR 900 σXt 1400 800 σYt 1200 700 1000 600 σW σR 500 800 σXt 400 σ 600 Yt 300
Covariance diagonal term Covariance diagonal term 400 200 200 100
0 0 0 100 200 300 400 500 600 0 100 200 300 400 500 600 sample number sample number (c) Circling Flight Path (d) Straight Line Flight Path
Figure 4.3: Open-Loop Tracking Covariance History
36 4.2 Computational Performance Comparison
An important feature of the recursive algorithm developed in this work is the reduction in computational complexity when compared to batch processing. The algorithm developed for this work was compared against the state of the art batch processing algorithm presented in
[1]. A history of timing information and state estimates was recorded for each iteration of the two estimators, allowing a side-by-side comparison of both computational cost and accuracy.
The batch estimator was configured to use a maximum queue size of 90 samples, corresponding to 45 seconds at 2 Hz. The UKF implementation was configured to run at 20 Hz just like the previous tests, using the initialization parameters given in Table 4.1. The test was performed in
MATLAB R2008a, in single-threaded mode, on an Intel Core 2 Duo 2.13 GHz CPU. Figure 4.4 shows the recorded time per iteration for the batch processing method and the Thermal UKF.
Note that while the UKF is using each of the 1800 samples, the batch processing algorithm only uses every tenth data point.
2 10 Batch Processing Thermal UKF realtime threshold 1 10
0 10
−1 10
−2 10 processing time per iteration, s
−3 10
−4 10 0 200 400 600 800 1000 1200 1400 1600 1800 sample number
Figure 4.4: Execution time per iteration for batch processing and Thermal UKF.
The execution time for the batch processing algorithm increases with the size of the recorded
37 queue of data, and levels off at the maximum queue size. On the other hand, the recursive nature of the Thermal UKF results in an approximately constant execution time (the jitter in the plot can be attributed to system timing granularity in Windows XP). The UKF is approximately one order of magnitude faster than batch processing when the queue is empty, and almost two orders of magnitude faster when compared to the full 45 second queue of data. The mean execution time for the batch processing routine (with the queue filled) was about 80ms. The mean execution time for the Thermal UKF over 1800 samples was only 1.25ms. Both batch processing and the UKF appear to be sufficiently fast for real-time operation, but the improved performance of the UKF indicates it may still give reasonable execution rates on less powerful computers.
The next step in comparing the two algorithms is to evaluate their estimation accuracy. This allows us to weigh the perceived computational benefits against any loss of thermal estimation performance. So, Figure 4.5 shows the mean squared error (MSE) of both algorithms. The time-history of the thermal position estimates generated by each algorithm is given in Figure
4.6.
6 10
5 10
4 10
3 Batch Processing 10 Thermal UKF MSE error
2 10
1 10
0 10 0 200 400 600 800 1000 1200 1400 1600 1800 sample number
Figure 4.5: Mean Squared Error for batch processing and Thermal UKF
38 600 Aircraft Position Batch Processing 500 Thermal UKF Actual Thermal Location y estimate, m 400 0 200 400 600 800 1000 1200 x estimate, m
Figure 4.6: Thermal position estimates for batch processing and Thermal UKF
Both algorithms converge to the thermal’s true position. As expected, the batch processing algorithm seems to outperform the UKF as long as enough thermal data is present in the queue.
This result can be attributed to the limited queue size of the batch processing algorithm. As the aircraft leaves the area where the thermal is located, new data displaces the ‘useful’ data near the core of the thermal, resulting in a poorer fit. On the other hand, the UKF estimate remains near to the site of the true thermal even as the aircraft exits the thermal field. The simulation results indicate both estimation algorithms are sufficiently accurate for closed loop soaring.
However, when the accuracy results are viewed in the context of the relative computational cost, the UKF appears to be superior. The batch algorithm may still find use as a way to initialize the UKF state and covariance values, from which point the UKF can take over and update those parameters in an efficient way.
39 4.3 Integration with the ALOFT Framework
Once the performance benefits of using the UKF formulation instead of batch processing were identified, the algorithm was implemented in the ALOFT Framework to allow testing in closed- loop simulation and flight testing. The older batch processing methods require queues of flight data to be recorded, while the UKF implementation only requires the latest sample. To maintain backwards compatibility, the queues were not completely removed; the UKF simply uses the top element in the stack. The specific power computation routines were updated to match the derivation provided in 2.2. The key difference between the implementations was the choice of reference frames for the computation of kinetic energy.
Additional modifications had to be made to the UKF algorithm to better support real-world testing and higher-fidelity simulation. The most immediate difference between the ‘real world’ and the simple simulation results previously presented is the presence of multiple thermals. In a cross country glider flight, new thermals are encountered constantly along the flight path. The thermal model we developed earlier only accounted for a single thermal, which is probably still a valid assumption in a small region, but not over an entire flight. So an ad-hoc reinitialization procedure was added to ‘reboot’ the filter after leaving a thermal. The reinitialization criteria is based on the aircraft position relative to the estimated position of the thermal. Specifically, we reset the filter if the aircraft is 3R away from the thermal. For the Wharington thermal, which is a Gaussian function, this is analogous to 3σ away from the mean for a normal distribution.
Since we don’t expect to experience any detectable influence from the previous thermal beyond this threshold, resetting the filter enables convergence to the next updraft.
4.4 Closed Loop Simulation
Once the thermal UKF was implemented into the ALOFT framework, it was tested using a simulator provided by the autopilot manufacturer, Cloud Cap Technology. This simulator includes the aircraft and autopilot closed-loop dynamics, sensor dynamics, and a thermal model.
40 Thermals are simulated according to the Allen model- the thermal strength and size vary nonlinearly with the height above the ground. Therefore this simulator environment presented a more realistic test of the thermal sensing algorithm derived in Section 2.2, but additionally challenged the thermal UKF to track thermals that did not match its internal abstracted model.
Because the thermal model as tracked by the filter is not the same as the actual thermal, it is impossible to directly compare the tracked thermal parameters W and R, which have meaning only within the context of a specific thermal model. Instead, the analysis of the closed-loop simulation was focused on the ability of the filter to track the thermal’s position in the horizontal plane.
To limit the effect of the mission management algorithms and focus the test on the estimation routines, a simplified autonomous soaring controller was created. When soaring was enabled, the autopilot was sent to an orbit waypoint centered at the estimated position of the thermal with a radius related to the estimated characteristic radius of the thermal. More details on the orbit radius selection will follow. The strength output from the filter was logged, but not used in the control loop.
A thermal field was configured in the simulator, consisting of three identical thermals spaced at intervals near a closed flight path. The flight path consisted of four waypoints in a quadrilat- eral. The arrangement of thermals and waypoints allowed the testing of three primary cases: a thermal at a waypoint, a thermal along a flight segment, and a thermal near but not quite along the flight path. The tracking algorithm was tested using both the Wharington and Gedeon ther- mal models. For the Wharington thermal, the characteristic radius R is located in an area of positive lift, and was used as the orbit radius for the commanded waypoint when soaring was enabled. For the Gedeon thermal, the characteristic radius R marks the radial distance of the zero-lift contour, and is not a suitable soaring orbit diameter. So when the Gedeon thermal
R was used, the orbit radius was commanded to /2. Similar modifications were made to the initialization and reset routines. Note that these parameters were selected on an ad-hoc basis for testing purposes; a more sophisticated implementation might set the orbit radius as a func-
41 tion of thermal and vehicle properties in order to maximize energy extraction. The simulator configuration details are given in Table 4.3. The UKF configuration parameters were for the most part the same as used for the open loop simulation (see Table 4.1); those that changed are given in Table 4.4.
After initializing the simulator, the aircraft was instructed to fly between the prepro- grammed waypoints. The soaring controller was enabled and disabled manually using a GUI interface built into the autopilot software. When soaring was enabled, the aircraft would leave the nominal flight path and begin to fly an orbit around the estimated thermal position. When soaring was disabled again, the aircraft would resume its normal flight path.
4.4.1 Closed-loop tracking using a Wharington thermal model
The first test tracked the simulated thermals using the Wharington thermal model. A typical closed-loop flight path is shown in figure 4.7, where the color of the trace indicates the computed power contributions from the atmosphere at each point along the flight path. In this test the vehicle started near thermal 1, soared briefly, then left in the direction of thermal 2, where it again soared briefly, and finally departs for thermal 3, where it remains in soaring mode until the conclusion of the test. The tracking algorithm was allowed to run continuously; enabling or disabling soaring mode simply closed or opened the waypoint control loop, respectively. The vehicle was allowed to fly through the area of thermal activity before soaring mode was engaged, so the covariance structure would be similar to the worst-case straight line example shown in
figure 4.2d. The history of estimated thermal positions are shown in figure 4.8. The altitude trace of the aircraft over the simulation time interval is given in figure 4.9. As expected, the aircraft gains altitude when circling in a thermal. The closed-loop tracking performance of the thermal UKF is shown in figures 4.10, 4.11, and 4.12, which show the convergence and tracking performance for each thermal, while soaring mode is engaged. The data show that the filter typically tracks the thermal position with less than 20 meters of error, once converged.
42 Table 4.3: Closed Loop Simulation Parameters.
Thermal Parameters†
m Nominal Strength 3.0 /s Radius Gain 0.5
Updraft Shape Trapezoid
Environmental Sink On
Autopilot Sim Configuration
Aircraft Model ALOFT AVL model
Flight Speed 30 kts
Output Data Rate 1 Hz
Thermal Positions Local Easting, m Local Northing, m
Thermal 1 -344.2244 355.1339
Thermal 2 357.4084 315.9495
Thermal 3 -69.9738 15.2755
Waypoint Positions Local Easting, m Local Northing, m
Waypoint 0 -95.6021 593.5803
Waypoint 1 358.1381 315.8381
Waypoint 2 387.5058 -108.6231
Waypoint 3 -451.4804 128.1535
† The thermals in the Cloud Cap Technology simulator use the Allen model, given in [2]. In this model, the thermal strength and size vary with altitude.
43 Table 4.4: Closed Loop UKF Configuration Parameters.
UKF Configuration, Wharington Thermal
m Initial Radius 100 /s Reset Distance max(R,ˆ 300)
Orbit Radius Rˆ
m Allowed Strength Range W ∈ [0.75, 8.0] /s Allowed Radius Range R ∈ [50, 200] m
UKF Configuration, Gedeon Thermal
m Initial Radius 200 /s Reset Distance max(0.ˆ5R, 300)
Orbit Radius 0.5Rˆ
m Allowed Strength Range W ∈ [0.75, 8.0] /s Allowed Radius Range R ∈ [50, 300] m
600 5.0 Thermal Locations Nominal Flight Path 4.4 500 3.8
400 3.3
2.7 300 2.1
200 1.5
0.9 100 Energy Rate, m/s local north coordinate, m 0.3
0 −0.3
−0.8 −100 −1.4
−200 −2.0 −500 −400 −300 −200 −100 0 100 200 300 400 500 local east coordinate, m
Figure 4.7: Energy Rate History in Closed-Loop Simulation
44 600
Nominal Flight Path 500 Actual Flight Path, Soaring On Actual Flight Path, Soaring Off Estimated Thermal Position, Soaring On 400 Actual Thermal Locations
300
200
100 local north coordinate, m
0
−100
−200 −500 −400 −300 −200 −100 0 100 200 300 400 500 local east coordinate, m
Figure 4.8: Overview of Closed-Loop Simulation using the Wharington Model
850 Soaring On 800 Soaring Off
750
700
650
600 AGL altitude, m
550
500
450
400 0 200 400 600 800 1000 1200 1400 time, seconds
Figure 4.9: Altitude Time History in Closed-Loop Simulation
45 250
200
Aircraft distance to thermal Estimation Error 150
distance, m 100
50
0 600 700 800 900 1000 1100 1200 1300 1400 time, seconds
Figure 4.10: Closed-Loop Tracking performance for Thermal 3
250
200 Aircraft distance to thermal Estimation Error
150
distance, m 100
50
0 0 50 100 150 200 250 300 350 time, seconds
Figure 4.11: Closed-Loop Tracking performance for Thermal 1
46 160
140 Aircraft distance to thermal 120 Estimation Error
100
80 distance, m 60
40
20
0 380 400 420 440 460 480 500 520 540 560 time, seconds
Figure 4.12: Closed-Loop Tracking performance for Thermal 2
47 4.4.2 Closed-loop tracking using the Gedeon thermal model
A similar test was conducted using the Gedeon thermal model, The same thermal and simulation parameters were used as in the previous test, except for the addition of environmental sink.
The flight path and thermal position estimates are shown in figure 4.13. Thermal tracking performance was very good for the first two thermals the aircraft encountered, again achieving better than 20m accuracy, as shown in figures 4.15 and 4.14. The tracking performance for the
final thermal, shown in figure 4.14, was initially good, but then began to deviate. The altitude trace in figure 4.17 shows part of the story; the thermal itself starts to get weaker at high altitudes (according to the parameters of the Allen model), so even though tracking performance may be adequate, less energy is being extracted from the thermal. As the thermal dissipates, the signal to noise ratio of the thermal sensing computations is proportionally reduced. The total tracking time for this test was also much longer than for the Wharington thermal (the aircraft and autopilot tracked the final thermal with some degree of accuracy for about thirty minutes).
48 700
Nominal Flight Path 600 Actual Flight Path, Soaring On Actual Flight Path, Soaring Off 500 Estimated Thermal Position, Soaring On Actual Thermal Locations
400
300
200
local north coordinate, m 100
0
−100
−200 −500 −400 −300 −200 −100 0 100 200 300 400 500 local east coordinate, m
Figure 4.13: Overview of Closed-Loop Simulation using the Gedeon Model
140
120 Aircraft distance to thermal Estimation Error 100
80
distance, m 60
40
20
0 0 50 100 150 200 250 300 350 400 time, seconds
Figure 4.14: Closed-Loop Tracking performance for Thermal 2
49 140
120 Aircraft distance to thermal Estimation Error 100
80
distance, m 60
40
20
0 400 450 500 550 600 650 700 time, seconds
Figure 4.15: Closed-Loop Tracking performance for Thermal 3
150 Aircraft distance to thermal Estimation Error
100 distance, m
50
0 600 800 1000 1200 1400 1600 1800 2000 2200 2400 2600 time, seconds
Figure 4.16: Closed-Loop Tracking performance for Thermal 1
50 1400
1200
1000
800
600 AGL altitude, m
400
200 Soaring On Soaring Off 0 0 500 1000 1500 2000 2500 time, seconds
Figure 4.17: Altitude Time History in Closed-Loop Simulation
51 4.5 Simulation Summary
An open-loop simulation was used to test the ability of the thermal UKF to track thermals that matched its internal model, using a fictional specific power sensor and representative flight paths. The open-loop simulation results showed that the algorithm is both computationally efficient and accurate, especially when compared to the state of the art batch processing meth- ods. The aircraft flight path (and hence, spatial distribution of energy rate samples) was shown to have an influence on the filter’s ability to track thermals, but the closed-loop results showed that even a simple autonomous soaring controller was able to track thermals with reasonable accuracy. Additionally, the closed-loop simulation added realistic aircraft, autopilot, and sensor dynamics. These factors provided a challenging test of the robustness of the thermal sensing algorithm derived in (2.14).The tracking algorithm was further challenged by using a higher- order updraft model in the simulator, testing the ability of the algorithm to cope with thermals that do not precisely match the internal model. Despite the presence of unmodeled higher- order terms, the algorithm proved capable of tracking thermals to within 20 meters and gave satisfactory closed-loop soaring performance with a simple controller.
52 Chapter 5
Flight testing
Following the open-loop simulation efforts (and preceding the inclusion of thermals in the autopilot simulator), the algorithm was flight tested in a cross-country soaring scenario. Cross- country soaring typically involves a ‘distance task,’ in which the goal is to travel as far as possible, or a speed task, in which the goal is to maintain the highest average cross-country speed over a set period of time. Key variations to both tasks include flying around a set course or out-and-back flights, both of which can be complicated by environmental conditions. Satisfying the soaring task is a challenge for an autonomous system which does not have local knowledge of typical thermal hotspots. More information on cross-country autonomous soaring can be found in [9]. For the purpose of this research, we will intentionally overlook many of these details, which fall into the mission management subsystem, rather than thermal identification.
5.1 Flight testing benefits and caveats
Flight testing is an essential process for evaluating the performance of aircraft control systems.
Many assumptions made in the software simulator do not hold in reality. For instance, the influence of background wind on thermals is not well modeled. Neither are the external effects of terrain and weather. But, without truth data to compare with the thermal tracker’s estimate, there is no way to determine which thermals were accurately identified and which were missed.
53 So flight testing a thermal tracking algorithm is a more significant test of the robustness of the algorithm than its absolute accuracy.
Also, the decoupled nature of the ALOFT framework makes it difficult to assess the perfor- mance of individual components. For instance, we have shown in section 4.1 that the flight path has a great deal of influence on thermal tracking performance. The thermal tracking algorithm, autonomous soaring logic, autopilot, communications links, and airframe must all work in con- cert, or the performance of one element will be inhibited by another. In the simulator, these components can be individually disabled or ‘faked’ to isolate portions of the system. This is not possible in flight testing. So quantitative assessments of this algorithm and soaring perfor- mance in general would be better performed in closed-loop simulation. With these disclaimers in place, flight test results from a typical West Coast cross-country flight are summarized here, to document the apparent real-world success of the algorithm.
5.2 Flight Testing Results
Let’s begin by looking at an example of a very successful test flight. Figure 5.1 shows the altitude profile of the flight over time, which lasted some 4 hours and 51 minutes. The manually piloted takeoff and landing are visible as dotted lines at the start and end of the data. For this flight, which covered a distance of about 128 km, the soaring controller performed very well. When the altitude is displayed as a function of distance along the course, as in figure 5.2, the thermals appear as nearly vertical rising columns of air, just as they are modeled in the simulator. One large thermal was tracked for more than 25 minutes, for a total altitude gain of about 1200 meters (0.75 miles).
The next day, a second flight was attempted at the same location. This time, the weather and thermal activity was notably different. The time-history of altitude (figure 5.3) shows multiple manually piloted takeoffs and landings over the course of several hours, while the total autonomous flight time is very brief. The difference in thermal behavior can be seen in figure
5.4, which shows the altitude plotted against distance along course. In this perspective, it is
54 Figure 5.1: Altitude Trace for Cross Country Flight Test clear that the thermal centers are drifting downwind over time. A compensation routine was used to ’push’ the thermal center position downwind with the estimated wind speed provided by the autopilot. In this flight, it appears that this method results in overcompensation; the thermal center is pushed downwind too fast and the soaring routines lose track of the thermal.
Including the effects of wind both in simulation and in the tracking algorithm remains an open research challenge.
In summary, the flight tests demonstrated the algorithm’s capabilities when meteorological conditions were favorable. Incorporating weather effects into a simulator would likely lead to improved thermal tracking algorithms, but that remains beyond the scope of this research. Al- though the simulation frameworks are invaluable tools for analyzing the performance of thermal tracking software, flight testing will remain the most comprehensive test of the entire system.
55 Figure 5.2: Flight Path for Cross Country Flight Test
Figure 5.3: Altitude Trace for Cross Country Flight Test
56 Figure 5.4: Flight Path for Cross Country Flight Test
57 REFERENCES
[1] Edwards, Daniel J. (2008) Implementation Details and Flight Test Results of an Au- tonomous Soaring Controller. 2008 AIAA Guidance, Navigation, and Control Conference and Exhibit. AIAA-2008-7244. [2] Allen, Michael (2006) Updraft Model for Development of Autonomous Soaring Uninhabited Air Vehicles. 44th AIAA Aerospace Sciences Meeting and Exhibit. AIAA-2006-1510. [3] Langelaan, Jack W. and G¨otzBramesfeld (2008) Gust Energy Extraction for Mini- and Micro- Uninhabited Aerial Vehicles. 2008 AIAA Aerospace Sciences Conference. AIAA- 2008-0223. [4] Saarlas, Maido (2007) Aircraft Performance. Wiley P. New York. [5] Gelb, Arthur (1974) Applied Optimal Estimation. MIT P., Cambridge. [6] Haykin, Simon., ed. (2001) Kalman Filtering and Neural Networks. Wiley P. New York. [7] van der Merwe, Rudolph (2004) Sigma-Point Kalman Filters for Probabalistic Inference in Dynamic State-Space Models. PhD Dissertation. OGI School of Science and Engineering, Oregon Health and Science University. [8] Julier, Simon J. (2004) Unscented Filtering and Nonlinear Estimation. Proceedings of the IEEE, Vol. 92.3 [9] Edwards, Daniel J. and Larry M. Silverberg. (2010) Autonomous Soaring: The Montague Cross Country Challenge Accepted for publication in the AIAA Journal of Aircraft.
58 APPENDIX
59 Appendix A
MATLAB UKF Implementation
A.1 Thermal UKF Closed-Loop Simulation Interface
% Matt Hazard % [email protected] % North Carolina State University % February 2010 % Test Script for Thermal UKF % Meant to be run as an alternate to ALOFT code, for development& % debugging of the UKF. Must turn ona Piccolo telemetry data source. % % credit to Dan Edwards for elements of the ALOFT drag estimation code and % Aaron Kahn for development ofa MATLAB link to the Piccolo autopilot
clear all ; clc ;
%%%%% Establish the connection %%%%% % Close the SDK if it was open(if fails, we don’t care) CloseSDK() ;
% Load the SDK s t a t u s = 0 ; while (status == 0) s t a t u s = LoadSDK ( ) ;%load the SDK again. end
60 % Set up the source for the data status = ComsNetwork( ’localhost’ , 2001) ;
%Get the geographic coordinates at start−up %these will be used as the local origin for conversion toX −Y coords home gps = GetGps(); while home gps . Lat == 0 home gps = GetGps();%prime the pump to make sure we’re getting good data end
%enable to get fast(up to 20Hz) telemetry, otherwise1Hz % SendBandwidthMode(0,0);
stats = GetAutopilotStatus(); old wpt = stats .WptIdxTo;
%%%%% Define Constants& Pre −Define Arrays %%%%% d e f i n e v e h i c l e ;% for sink polar d e f i n e e a r t h ;% for lat/lon to NED coordinates m = MASS; W = m ∗ g ; num iters = 10000;
u k f s t a t e = zeros ( 4 , 1 ) ; u k f c o v = zeros ( 4 , 1 ) ;
%log data in this big array(preallocate for speed) s u p e r l o g = zeros ( num iters , 16) ;
%%%%% Main Loop %%%%% w hat = 0 ; l a s t w h a t = 0 ; l a s t v a l i d = 0 ; k = 0 ; r e s e t f l a g = 0 ; while ( k < num iters )
61 % Get the data from the Piccolo a = GetAirdata(); gps = GetGps(); c = GetCoreTelem() ; nav = GetNavDiag() ; t h r u s t = 0 ;
% Extract time and roll angle from the core telemetry t = c.SysTime; roll = c.Euler(1);
% Compute the aircraft acceleration in the inertial frame %first, reconstruct the attitude quaternion from core telemetry attitude = quat f r o m euler(c.Euler); b o d y accel = c.Axyz; i f (nav.AccelBiasGood == 1) b o d y a c c e l = b o d y a c c e l − nav.AccelBias; end %rotate the accel vector from body to inertial frame i n e r t i a l accel = rotate a v e c t o r b y a quat(attitude , body a c c e l ) ; %gravity compensation(must be applied in the inertial frame) i n e r t i a l accel = inertial a c c e l − [ 0 , 0 , − 9 . 8 ] ;
% Gps measured change in altitude over time= Vd dhdt = −gps . Vd ;
%x andy are along NED axes... usinga utility function from ALOFT %could be replaced with UTM/libproj to avoid doing the projection ourselves % but so long as we only care about ∗ r e l a t i v e ∗ coordinates this is fine. [northing easting ] = MakeXYfromLatLon(home gps . Lat∗r2d , home gps . Lon∗r2d, gps.Lat∗ r2d, gps.Lon∗ r2d ) ;
% Inertial velocity for kinetic energy Vivec = [gps.Vn, gps.Ve, gps.Vd]; Vi = norm( Vivec ) ; Va = a.TrueAirSpd;
% Sink rate computation borrowed from Estimate Energy Queue(ALOFT) % Needs to be computed using airspeed, not inertial velocity
62 % Compute aircraft sink rate from sink polar data polynomial fit sinkrate = Va.ˆ2 ∗ polys(1) + Va. ∗ polys(2) + polys(3); % bank angle modification sinkrate = sinkrate ∗ 1/ cos (roll).ˆ(3/2);
i f ( l a s t v a l i d ) i f t == l a s t t pause ( 0 . 0 5 ) ; continue end k = k+1;
% Energy Estimation %todo: use accelerometer reading(it will need to be %rotated into inertial frame using current euler angles) dt = t − l a s t t ; %[k dt] dVidt = ( Vi − l a s t V i ) / dt ; dVidtvec = (Vivec − l a s t V i v e c ) / dt ;
%Compute kinetic power term, based on gps VidVidti = dot (Vivec, dVidtvec);
%Compute kinetic power term, based on accelerometer VidVidta = dot (Vivec, inertial a c c e l ) ;
%make sure to keep inertial velocities and airspeed %quantities distinct. Engine power and drag power should be % computed based on airspeed %However, acceleration should be measured in the earth frame. w hat = dhdt + VidVidta / g − s i n k r a t e − Va∗ t h r u s t ;%accelerometer %w hat= dhdt+ VidVidti − s i n k r a t e − Va∗ t h r u s t;%gps based
i f ( k > 2) % use dt to geta rough one second time −constant w hat = l a s t w h a t + ( w hat − l a s t w h a t ) ∗ dt ; else % hard reset on the UKF; it’s never been run before u s e r l o g ( [ ’full reset to:<’ num2str( e a s t i n g ) ’,’ num2str( northing ) ’>’ ]);
63 [ u k f s t a t e , u k f c o v ] = u k f a b s o l u t e fn(easting , northing, w hat , dt , 2) ; end
% Compare euclidean distance of thermal center to3 ∗ r a d i u s fora reset % or more than 300m from the estimate of the center position(likelya bad e s t i m a t e) d i s t a n c e = norm ([easting; northing] − u k f state(3:4)); i f ( d i s t a n c e > max(3∗ u k f state(2), 300)) u s e r l o g ( [ ’soft reset to:<’ num2str( e a s t i n g ) ’,’ num2str( northing ) ’>’ ]); [ u k f s t a t e , u k f c o v ] = u k f a b s o l u t e fn(easting , northing, w hat , dt , 1) ; else [ u k f s t a t e , u k f c o v ] = u k f a b s o l u t e fn(easting , northing, w hat , dt , 0) ; end
u k f s t a t e ( 1 ) = max( u k f state(1), 0.75); u k f s t a t e ( 1 ) = min( u k f state(1), 8.0); u k f s t a t e ( 2 ) = max( u k f state(2), 50); u k f s t a t e ( 2 ) = min( u k f state(2), 200);
l a s t w h a t = w hat ; end
%senda waypoint update to the autopilot %this way we can watch the update in PCC without having to do plotting in MATLAB.... % Coordinate shift back to include initialization bias(Make XY from lat/lon) %don’t forget to swap northing and easting... [ lat lon] = MakeLatLonfromXY(home gps . Lat∗r2d , home gps . Lon∗r2d , u k f s t a t e ( 4 ) , u k f s t a t e ( 3 ) ) ; %make the orbit size equal to the state estimate of radius... l a s t stats = stats; stats = GetAutopilotStatus(); %always orbit to the right %seta minimum orbit radius of 50m waypoint = build waypoint(lat , lon, max( u k f state(2), 50), 91, 90, 1); SendWaypoint(waypoint) ;%91 waypoint = build waypoint(home gps . Lat∗r2d , home gps . Lon∗r2d, 0, 90, 91, 0); SendWaypoint(waypoint) ;%90
%latch based on autopilot Brakes enabled in the status message
64 %soaring enabled only ona rising edge(this way we can ignore an old %button press) i f ((stats .Brakes − l a s t stats .Brakes) > 1) SendToWaypoint(91 ,1) ; old wpt = stats .WptIdxTo; u s e r l o g ( ’soaring enabled, tracking wpt 91’ ) e l s e i f ((stats .Brakes − l a s t stats .Brakes) < 0)%soaring disabled SendToWaypoint(old wpt , 1) ;%send back to an old waypoint s t r = sprintf ( ’soaring disabled, tracking wpt%d’ , old wpt ) ; userlog(str) end
i f ( k > 0) superlog(k,:) = [t, ...%1 system time stats.Brakes, ...%2 latch status l a t , . . .%3 thermal latitude lon , . . .%4 thermal longitude u k f state(1), ...%5 thermal strength u k f state(2), ...%6 thermal radius w hat , . . .%7 observed specific power from atmosphere u k f c o v ( 4 ) , . . .%8 thermal estimate cov(northing) u k f c o v ( 3 ) , . . .%9 thermal estimate cov(easting) u k f c o v ( 1 ) , . . .%10 thermal estimate cov(strength) u k f c o v ( 2 ) , . . .%11 thermal estimate cov(radius) sum( u k f c o v ) , . . .%12 trace of covariance matrix gps.Alt84, ...%13 aircraft altitude a.TrueAirSpd, ...%14 aircraft airspeed gps . Lat ∗ r2d , . . .%15 aircraft latitude gps . Lon ∗ r2d ] ;%16 aircraft longitude end l a s t V i = Vi ; l a s t Vivec = Vivec; l a s t v a l i d = 1 ; l a s t t = t ; end
CloseSDK ( )
65 A.2 Thermal UKF
A.2.1 Closed-Loop Simulation / Flight Testing Version
% Unscented Kalman Filter based ThermalID % Matthew Hazard % North Carolina State University % 21 Nov 2008 − 27 February 2009 − 14 February 2010
% This version uses the absolute position coordinates % and either the Wharington(Gaussian) or Gedeon models
% Recursive version based on Dan Edwards’ original batch thermal fit
% Additive UKF based on work by Rudolph van der Merwe in Ph.D. thesis: % Sigma−Point Kalman Filters for Probabilistic Inference in Dynamic % State−Space Models, OHSU, 2004
function [state , state c o v d i a g ] = u k f a b s o l u t e fn(easting , northing, w hat , dt , reset )
%These things should only be set once... persistent x X Rn N M alpha beta kappa lambda... Rv r e l a t i v e distance model r1 e d o t p r e d i c t ;
i f ( reset >= 1)%just reset state and covariance %set to the current position x = [3; 100; easting; northing]; %set toa safe covariance X = diag ([1, 100, 1000, 1000]);
i f ( reset == 2)%full reset x y p r o c e s s n o i s e = 4 ∗ sqrt ( dt ) ; r p r o c e s s n o i s e = . 5 ∗ sqrt ( dt ) ; w p r o c e s s noise = 0.005 ∗ sqrt ( dt ) ;
%sensor noise characteristics Rn = 0 . 0 1 ;%from Dan Edwards’ SBXC Piccolo data
N = 4 ;%4 state variables
66 % thermal strengthW % thermal radiusR % relative thermal position[r x,r y]’
M = 1 ;%2 measurements % Energy rate at aircraft1 % Energy rate at aircraft2
%these control the distribution of the sigma points(defaults used) alpha = 0.001; beta = 2 ; kappa = 0 . 0 ; %composite scaling parameter to reduce computations lambda = alpha ∗ alpha ∗(N + kappa ) − N;
Rv = diag ( [ w p r o c e s s n o i s e . . . r p r o c e s s n o i s e . . . x y p r o c e s s n o i s e . . . x y p r o c e s s n o i s e ] ) ;%arbitrary guess
%measurement model defined using anonymous functions %givena state vectorx, calculate the expected measurement output %first compute the relative distance to the thermal r e l a t i v e distance = @(x, xy) norm( x ( 3 : 4 ) − xy ) ; %Choosea thermal model:
%Gaussian/ Wharington model = @(r,W,R) W ∗ exp(−( r /R) ˆ2) ; % −−− or −−−−− %gedeon’modified gaussian’ model(includes sink ring) %model=@(r,W,R)W ∗ exp( −(r/R)ˆ2) ∗(1 −(r/R)ˆ2);
%then compute the expected energy rate e d o t predict = @(x, xy) model(relative distance(x,xy), x(1), x(2)); end end
%∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ PREDICTION STEP ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ %the prediction step has zero dynamics, therefore we only need to update
67 %the covariance with the process noise. X = X + Rv ;
%∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗MEASUREMENT UPDATE(OBSERVATION) ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ %generate sigma points centered around the current state, distributed % according to the covariance matrix sigma = sigma points(x,X,N,lambda) ; %we have one measurement: the current atmospheric specific power z = w hat ;
%predict the output by propagating each of the sigma points % through our thermal model for k = 1 : 2∗N+1 sigma output(:,k) = e d o t predict(sigma(:,k), [easting; northing]); end
%compute the mean output from all the weighted sigma points y sum = (lambda / (N+lambda)) ∗ sigma output(: ,1); for k = 2 : 2 ∗N+1 y sum = y sum + (0.5 / (N+lambda)) ∗ sigma output(: ,k); end y = y sum ; % compute the output covariance Y sum = ((lambda / (N+lambda)) + (1− alpha ∗ alpha + beta )) ∗ ( sigma output(: ,1)−y ) ∗( sigma output(: ,1) − y ) ’ ; for k = 2 : 2 ∗N+1 Y sum = Y sum + (0.5 / (N+lambda)) ∗ ( sigma output(: ,k) − y ) ∗ ( sigma output(: ,k) − y ) ’ ; end Y = Y sum + Rn ; %compute the output cross−c o v a r i a n c e XY sum = ((lambda / (N+lambda)) + (1− alpha ∗ alpha + beta )) ∗ (sigma(: ,1)−x ) ∗( sigma output(: ,1) − y ) ’ ; for k = 2 : 2 ∗N+1 XY sum = XY sum + (0.5 / (N+lambda)) ∗ (sigma(: ,k) − x ) ∗ ( sigma output(: ,k) − y ) ’; end XY = XY sum ;
68 %compute the Kalman gain K = XY∗inv (Y); %update the mean and covariance based on the actual measurement x = x + K∗( z − y ) ; X = X − K∗Y∗K’; s t a t e = x ;
%return the diagonal elements for diagnostics s t a t e c o v d i a g = diag (X); end
A.2.2 Sigma Point Selection
% Select’sigma points’ based on the current state, state covariance, and % scaling factors. %x − mean(center) sigma point %X − c o v a r i a n c e of estimate(controls distribution of sigma points) %N − s i z e of state (2N+1 sigma points returned) % lambda − combined scaling factor per van der Merwe % see reference: % Sigma−Point Kalman Filters for Probabilistic Inference in Dynamic % State−Space Models, OHSU, 2004
function sigmas = sigma points(x, X, N, lambda)
%create2 ∗N+1 sigma points that correspond to the %uncertainty distribution of the covariance... sigmas = zeros (N, 2∗N+1) ; sigmas(:,1) = x;%row vector!
%take the matrix square root ofX and scale covariance [X sqrt ,resnorm] = sqrtm ((N+lambda) ∗X);
for k = 1 :N %addi’th column of the matrix square root sigmas(:,k+1) = x + X sqrt ( : , k ) ; end
%offset added in assignment to get(N:2N+1) (+1 for matlab, too)
69 for k = 1 :N %subtracti’th column of the matrix square root sigmas(: ,k+N+1) = x − X sqrt ( : , k ) ; end end
A.3 Quaternion Algebra
A.3.1 Reconstruction of a Quaternion from Euler Angles
%function quat= quat f r o m e u l e r(rpy) % reconstructs an attitude quaternion froma vector % of roll, pitch, and yaw angles(in radians) % uses GNC conventions: %q= {q0, q1, q2, q3 } (scalar first) % and rotation order 321(yaw, pitch, roll) % sourced from http://www. resonancepub.com/quaterni.htm % with quaternion order altered(orig. {q1, q2, q3, q0 }) function quat = q u a t f r o m e u l e r ( rpy ) quat = zeros ( 4 , 1 ) ; rpy = rpy / 2.0; s1 = sin ( rpy ( 1 ) ) ; s2 = sin ( rpy ( 2 ) ) ; s3 = sin ( rpy ( 3 ) ) ; c1 = cos ( rpy ( 1 ) ) ; c2 = cos ( rpy ( 2 ) ) ; c3 = cos ( rpy ( 3 ) ) ;
quat ( 1 ) = c3 ∗ c2 ∗ c1 + s3 ∗ s2 ∗ s1 ; quat ( 2 ) = c3 ∗ c2 ∗ s1 − s3 ∗ s2 ∗ c1 ; quat ( 3 ) = c3 ∗ s2 ∗ c1 + s3 ∗ c2 ∗ s1 ; quat ( 4 ) = s3 ∗ c2 ∗ c1 − c3 ∗ s2 ∗ s1 ; end
A.3.2 Rotation of a Vector by a Quaternion
% function rvec= rotate a v e c t o r b y a q u a t(q, vec) % rotatea vector by the given quaternion % optimized version uses temporaries to avoid repeating
70 % multiplication steps. % sourced from pseudo code at: % http://en.wikipedia.org/wiki/Quaternions a n d s p a t i a l r o t a t i o n function rvec = rotate a v e c t o r b y a quat(q, vec) %maintain row or column vector shape of input vector: rvec = zeros ( size ( vec ) ) ; % build temporaries t2 = q ( 1 ) ∗ q ( 2 ) ; t3 = q ( 1 ) ∗ q ( 3 ) ; t4 = q ( 1 ) ∗ q ( 4 ) ; t5 = −q ( 2 ) ∗ q ( 2 ) ; t6 = −q ( 2 ) ∗ q ( 3 ) ; t7 = q ( 2 ) ∗ q ( 4 ) ; t8 = −q ( 3 ) ∗ q ( 3 ) ; t9 = q ( 3 ) ∗ q ( 4 ) ; t10 = −q ( 4 ) ∗ q ( 4 ) ; %assemble output vector rvec ( 1 ) = 2 ∗ ( ( t8 + t10 ) ∗ vec ( 1 ) + ( t6 − t4 ) ∗ vec(2) + (t3 + t7) ∗ vec(3)) + vec(1); rvec ( 2 ) = 2 ∗ ( ( t4 + t6 ) ∗ vec ( 1 ) + ( t5 − t10 ) ∗ vec ( 2 ) + ( t9 − t2 ) ∗ vec(3)) + vec(2); rvec ( 3 ) = 2 ∗ ( ( t7 − t3 ) ∗ vec(1) + (t2 + t9) ∗ vec(2) + (t5 + t8) ∗ vec(3)) + vec(3);
71