State Estimation for Autopilot Control of Small Unmanned Aerial Vehicles in Windy Conditions

Transcription

1 University of Colorado, Boulder CU Scholar Aerospace Engineering Sciences Graduate Theses & Dissertations Aerospace Engineering Sciences Summer State Estimation for Autopilot Control of Small Unmanned Aerial Vehicles in Windy Conditions David Paul Poorman University of Colorado Boulder, Follow this and additional works at: Part of the Aerospace Engineering Commons Recommended Citation Poorman, David Paul, "State Estimation for Autopilot Control of Small Unmanned Aerial Vehicles in Windy Conditions" (2014). Aerospace Engineering Sciences Graduate Theses & Dissertations This Thesis is brought to you for free and open access by Aerospace Engineering Sciences at CU Scholar. It has been accepted for inclusion in Aerospace Engineering Sciences Graduate Theses & Dissertations by an authorized administrator of CU Scholar. For more information, please contact

2 STATE ESTIMATION FOR AUTOPILOT CONTROL OF SMALL UNMANNED AERIAL VEHICLES IN WINDY CONDITIONS DAVID PAUL POORMAN B.S., University of Colorado at Boulder, 2010 A thesis submitted to the Faculty of the Graduate School of the University of Colorado in partial fulfillment of the requirement for the degree of Master of Science Department of Aerospace Engineering Sciences 2014

3 This thesis entitled: State Estimation for Autopilot Control of Small Unmanned Aerial Vehicles in Windy Conditions written by David Paul Poorman has been approved for the Department of Aerospace Engineering Sciences (Dale Lawrence) (Eric Frew) Date The final copy of this thesis has been examined by the signatories, and we Find that both the content and the form meet acceptable presentation standards Of scholarly work in the above mentioned discipline.

4 2014 David Poorman iii

5 Abstract Poorman, David Paul (M.S. Aerospace Engineering Sciences) State Estimation for Autopilot Control of Small Unmanned Aerial Vehicles in Windy Conditions Thesis directed by Professor Dale A. Lawrence The use of small unmanned aerial vehicles (UAVs) both in the military and civil realms is growing. This is largely due to the proliferation of inexpensive sensors and the increase in capability of small computers that has stemmed from the personal electronic device market. Methods for performing accurate state estimation for large scale aircraft have been well known and understood for decades, which usually involve a complex array of expensive high accuracy sensors. Performing accurate state estimation for small unmanned aircraft is a newer area of study and often involves adapting known state estimation methods to small UAVs. State estimation for small UAVs can be more difficult than state estimation for larger UAVs due to small UAVs employing limited sensor suites due to cost, and the fact that small UAVs are more susceptible to wind than large aircraft. The purpose of this research is to evaluate the ability of existing methods of state estimation for small UAVs to accurately capture the states of the aircraft that are necessary for autopilot control of the aircraft in a Dryden wind field. The research begins by showing which aircraft states are necessary for autopilot control in Dryden wind. Then two state estimation methods that employ only accelerometer, gyro, and GPS measurements are introduced. The first method uses assumptions on aircraft motion to directly solve for attitude information and smooth GPS data, while the second method integrates sensor data to propagate estimates between GPS measurements and then corrects those estimates with GPS information. The performance of both methods is analyzed with and without Dryden wind, in straight and level flight, in a coordinated turn, and in a wings level ascent. It is shown that in zero wind, the first method produces significant steady state attitude errors in both a coordinated turn and in a wings level ascent. In Dryden wind, it produces iv

6 large noise on the estimates for its attitude states, and has a non-zero mean error that increases when gyro bias is increased. The second method is shown to not exhibit any steady state error in the tested scenarios that is inherent to its design. The second method can correct for attitude errors that arise from both integration error and gyro bias states, but it suffers from lack of attitude error observability. The attitude errors are shown to be more observable in wind, but increased integration error in wind outweighs the increase in attitude corrections that such increased observability brings, resulting in larger attitude errors in wind. Overall, this work highlights many technical deficiencies of both of these methods of state estimation that could be improved upon in the future to enhance state estimation for small UAVs in windy conditions. v

7 This work is dedicated to Grandmary and Grandbob.

8 Contents 1 Introduction Nomenclature Background Information Reference Frames The Wind Triangle The Extended Kalman Filter Measurement Update Time Update Aircraft States Needed for Autopilots Scoping the Estimation Problem Overview of the Autopilot Simulation Setup Benchmark 1: Straight and Level Flight Benchmark 2: Coordinated Turn Benchmark 3: Wings Level Ascent The Dryden Wind Model vii

9 4.4 Sensors Demonstration of Autopilot Performance with Limited State Information Autopilot Performance under Benchmark Autopilot Performance under Benchmark Autopilot Performance under Benchmark Relative Importance of the Chosen States Autopilot Summary Method 1: IMU Attitude, Smoothed GPS, and Pressure Sensors Description The Transport Theorem Attitude Determination Extended Kalman Filter Extended Kalman Filter Method 1 Summary Detailed Simulation Results Zero Wind Dryden Wind viii

10 5.3 Method 1 Analysis Summary Method 2: IMU and GPS Sensor Fusion Description Direct IMU Numerical Integration Sensor Fusion through Kalman Filtering Method 2 Summary Detailed Simulation Results IMU Only Solution (No GPS/EKF3 Corrections) Zero Winds Dryden Wind Method 2 Analysis Summary Method Comparison and Future Work Direct Attitude Sensing Replace Integration Method in State Estimation Method Parallel Estimators Conclusion References ix

11 Figures Figure 3.1: Aircraft Reference Frames... 8 Figure 3.2: The Wind Triangle Figure 4.1: Straight and Level Flight Illustration Figure 4.2: Coordinated Turn Illustration Figure 4.3: Wings Level Ascent Illustration Figure 4.4: Example Dryden Wind Model Simulation Figure 4.5: Command Tracking Error for Benchmark 1 in Zero Wind Figure 4.6: Command Tracking Error for Benchmark 1 in Light Wind Figure 4.7: Command Tracking Error for Benchmark 2 in Zero Wind Figure 4.8: Command Tracking Error for Benchmark 2 in Light Wind Figure 4.9: Command Tracking Error for Benchmark 3 in Zero Wind Figure 4.10: Command Tracking Error for Benchmark 3 in Light Wind Figure 4.11: Effects of Bad Pitch Estimation Figure 5.1: Method 1 Flow Chart Figure 5.2: Method 1 State Estimation for Benchmark 1 in Zero Wind and Zero Gyro Bias Drift x

12 Figure 5.3: Method 1 State Estimation for Benchmark 1 in Zero Wind Figure 5.4: Estimated Yaw Angle for Benchmark 1 in Zero Wind Figure 5.5: Estimated Yaw Angle for Benchmark 1 in Zero Wind and Zero Gyro Bias Drift Figure 5.6: Method 1 Results for Benchmark 2 in Zero Wind Figure 5.7: Validity of Method 1 Assumptions in Benchmark 2 and Zero Wind Figure 5.8: Method 1 Results for Benchmark 2 in Zero Wind and Zero Gyro Bias Drift Figure 5.9: Method 1 Benchmark 3 Results in Zero Wind Figure 5.10: Error Statistics for Method 1 in Benchmark 3 and Zero Wind Figure 5.11: Method 1Assumption Validity for Benchmark 3 in Zero Wind Figure 5.12: Method 1 State Estimation with True Inertial Velocity Values Figure 5.13: Method 1 Results for Benchmark 1 in Dryden Wind Figure 5.14: Assumption Validity for Benchmark 1 in Dryden Wind Figure 5.15: Assumption (5.7) to Estimated Attitude Zoom Comparison Figure 5.16: Method 1 Results under Benchmark 2 and Dryden Wind Figure 5.17: Method 1 Assumption Validity under Benchmark 2 in Dryden Wind Figure 5.18: Method 1 Results for Benchmark 3 in Dryden Wind Figure 5.19: Method 1 Assumption Validity for Benchmark 3 in Dryden Wind xi

13 Figure 5.20: Method 1 Assumption Validity for Benchmark 3 in Dryden Wind and Delayed Climb Figure 6.1: Method 2 Flow Chart Figure 6.2: Airspeed Estimation for Method Figure 6.3: State Estimation Results of Method 2 without EFK3 in Benchmark 1 in Zero Wind Figure 6.4: Extra State Estimation Results of Method 2 without EFK3 in Benchmark 1 in Zero Wind Figure 6.5: State Estimation Results of Method 2 Benchmark 1 without EFK3 and with True Attitude Knowledge Figure 6.6: Extra State Estimation Results of Method 2 Benchmark 1 without EFK3 and with True Attitude Knowledge Figure 6.7: State Estimation Results for Method 2 without EKF3 under Benchmark 1 in Zero Wind Figure 6.8: Extra State Estimation Results for Method 2 without EKF3 under Benchmark 1 in Zero Wind Figure 6.9: Method 2 State Estimation for Benchmark 1 in Zero Wind and Zero Gyro Bias Drift Figure 6.10: Extra Estimated States from Method 2 for Benchmark 1 with Zero Wind and Zero Gyro Rate Bias Figure 6.11: Dependence of Difference between Accelerometer Measurement and Velocity Errror Growth on Yaw Growth Figure 6.12: Method 2 State Estimates for Benchmark 1 in Zero Wind xii

14 Figure 6.13: Extra State Estimates for Benchmark 1 in Zero Wind Figure 6.14: Bias Estimates for Method 2 in Benchmark 1 in Zero Wind Figure 6.15: State Estimation Results for Benchmark 2 in Zero Wind Figure 6.16: Bias Estimation for Benchmark 2 in Zero Wind Figure 6.17: Extra State Estimation Results for Benchmark 2 in Zero Wind Figure 6.18: Bias Estimates for Benchmark 2 with Time-Varying Jerk Vector Figure 6.19: State Estimation Results for Benchmark 3 in Zero Wind Figure 6.20: Extra Estimated States for Method 2 in Benchmark 3 and Zero Wind Figure 6.21: Estimated Bias States for Method 2 in Benchmark 3 and Zero Wind Figure 6.22: State Estimation Results for Method 2 Benchmark 1 in Dryden Wind and Zero Bias Drift Figure 6.23: Extra Estimated States for Method 2 Benchmark 1 in Wind with Zero Rate Bias Figure 6.24: Sensor Bias Estimates for Method 2 Benchmark 1 in Wind with Zero Rate Bias Figure 6.25: State Estimation Results for Method 2 in Benchmark 1 and Dryden Wind Figure 6.26: Extra State Estimates for Method 2 in Benchmark 1 and Dryden Wind Figure 6.27: Bias State Estimates for Method 2 in Benchmark 1 and Dryden Wind Figure 6.28: State Estimation Results for Method 2 in Benchmark 2 in Dryden Wind Figure 6.29: Gyro Bias Estimates for Benchmark 2 in Dryden Wind xiii

15 Figure 6.30: Extra State Estimation Results for Method 2 in Benchmark 2 and Dryden Wind Figure 6.31: State Estimation Results for Method 2 in Benchmark 3 and Dryden Wind Figure 6.32: Extra State Estimation Results for Method 2 in Benchmark 3 and Dryden Wind Figure 6.33: Bias Estimation Results for Method 2 in Benchmark 3 and Dryden Wind xiv

16 1 Introduction In all fields of aviation, from large commercial aircraft and military fighters to small scale Unmanned Aerial Vehicles (UAVs), accurate state estimation is a critical part of an aircraft s control system that is often taken for granted or simply overlooked. In large expensive aircraft like airliners or military fighters, the methods of obtaining accurate state information are well known and usually involve a complex array of numerous sensors to get the information [1]. However, today state estimation of small UAVs is at least as much of a topic of concern as the design of a controller. State estimation of small UAVs is a newer area of study and can in some ways be more difficult to implement than state estimation for large complex aircraft. This is due to two main aspects of small UAVs. First, large aircraft employ a sophisticated array of air data sensors to obtain detailed information about the airflow around the aircraft at high frequencies. They use this information combined with specialized, high-precision, and often expensive Inertial Measurement Units (IMUs), Global Positioning System (GPS) receivers, and other inertial sensors to accurately estimate vehicle states. Unfortunately, small UAVs can typically only support a limited sensor suite due to the size, weight, and cost of a large number of sophisticated sensors. Thus, state estimation has to be done with both fewer and lower cost sensors than is typically done on large aircraft. This means that the algorithms used for state estimation must be re-assessed before they are applied to small UAVs. Second, the effects of wind on the motion of a small UAV is much greater than on large aircraft. Reference [2] shows that as the scale of an aircraft shrinks, the aircraft is increasingly subject to disturbances caused by wind and turbulence. For example, it is not atypical for the wind speeds experienced by small aircraft to be 50% of their total airspeed. That is almost never the case on large aircraft. Many techniques for estimating small UAV states during zero or steady wind have been 1

17 developed and widely published, such as in [3], [4], [5], and [6]. These techniques generally work well for their designed purpose of zero or steady wind. However, in practice, if highly variable, high frequency gusty wind is encountered, these methods can produce significant amounts of error between the estimated value of the states and the true value of the states. If this error is large enough, depending on the design or the aircraft s controller, this can start unstable behavior in both the estimator and controller, and cause the aircraft to crash. The purpose of this research is to assess the previously unassessed effectiveness of existing methods for state estimation for small UAVs in highly variable windy conditions. There are two common methods of state estimation that are used today by small UAVs. This work simulates both of these methods in a MATLAB /Simulink aerodynamic model under different benchmark maneuvers in varying wind strengths to assess their performance with and without wind to find and analyze the reasons for the observed behavior of the state estimate errors in wind. The contributions of this work are detailed understandings of why these methods of state estimation undergo their observed behavior in wind so that these estimators can be improved upon in the future to improve the accuracy of state estimation for small UAVs in wind. The first method that is analyzed makes assumptions about the acceleration and velocity of the aircraft in order to measure attitude and then uses this attitude to smooth GPS data to get translational information [4]. This work addresses these assumptions with and without wind present in order to understand how wind affects this state estimation method. The second method that is analyzed does not make assumptions on the acceleration or velocity of the aircraft, and instead integrates upon a- priori knowledge of attitude and translation states using gyro and accelerometer sensor data to update the attitude and translation estimates [7]. It then corrects these integrated attitude and translation estimates with GPS provided position and velocity. The effectiveness of this method s ability to correct 2

18 both the translational states and the attitude states will be heavily scrutinized with and without wind present so that the change in the observability of the IMU error between zero wind and wind can be assessed. This detailed analysis of the change in performance of both of these methods when wind is added will provide the insights that are required to improve upon these existing methods in the future. 3

19 2 Nomenclature B B[A] N N[A] p n p e p d p B/N Designates the aircraft body frame. Always used as a subscript or superscript. Designates the vector A written in aircraft body B frame components Designates the inertial North-East-Down (N-E-D) earth-fixed frame. Earth-fixed is considered the inertial reference unless explicitly stated otherwise. Designates the vector A written in inertial N frame components. Inertial Earth position towards North, origin at simulation initial position Inertial Earth position towards East, origin at simulation initial position Negative Earth relative altitude. Zero altitude is ground level. Vector of the position of the aircraft body frame B relative to the inertial Earth N frame. h Height of the aircraft above ground level. Negative of p d. u v w V B/N φ θ ψ ψ χ p q r ω B/N α B/N V a Component of inertial velocity vector V B/N along the X axis of the aircraft. Component of inertial velocity vector V B/N along the Y axis of the aircraft. Component of inertial velocity vector V B/N along the Z axis of the aircraft. The inertial velocity vector. The velocity of the aircraft body frame B relative to the inertial Earth N frame. Roll angle in degrees. Third Euler angle. Pitch angle in degrees. Second Euler angle. Yaw angle in degrees. First Euler angle. Vector of Euler angle components, relating the N frame to the B frame: [φ θ ψ] T Course angle. Only the same as yaw angle in zero wind and sideslip. Roll rate Pitch rate Yaw rate Angular velocity of the aircraft body frame relative to Earth inertial frame. Angular acceleration of the aircraft body frame relative to the Earth inertial frame. Airspeed. Magnitude of the airspeed vector V a 4

20 V a α β a x a y a z a B/N w x w y w z w n w e w d V w Vector representing the speed of the air mass relative to the aircraft. Points into the direction the air mass is coming from. Angle of Attack Angle of Sideslip The component of the inertial acceleration of the aircraft in the direction of the body X axis The component of the inertial acceleration of the aircraft in the direction of the body Y axis The component of the inertial acceleration of the aircraft in the direction of the body Z axis Acceleration of body frame relative to Earth inertial frame. Component of the wind velocity V w in the direction of the aircraft body positive X axis. Component of the wind velocity V w in the direction of the aircraft body positive Y axis. Component of the wind velocity V w in the direction of the aircraft body positive Z axis. Component of the wind velocity V w in the direction from the north. Component of the wind velocity V w in the direction from the east. Component of the wind velocity V w in the downward direction. Wind speed velocity vector. Indicates which direction the wind is coming from. Φ EKFY State transition matrix. Maps the values of one state to the next state inside the time update section of a Kalman filter. P EKFY Covariance matrix of the states in extended Kalman filter Y. R EKFY Matrix of expected sensor noise that is used in extended Kalman filter Y. x EKFY The vector of states that is being estimated by extended Kalman filter Y. y EKFY The vector of measurements that are used in extended Kalman filter Y. u EKFY The vector of known truth inputs used by extended Kalman filter Y. F EKFY The matrix that maps the measurements and inputs of an extended Kalman filter Y to the time derivatives of the states of the extended Kalman filter Y H EKFY The matrix that maps the states and inputs of an extended Kalman filter Y to the measurements of the extended Kalman filter Y. ε η δ e δ a State noise vector. Measurement noise vector. Elevator deflection, as commanded by the autopilot. Aileron deflection, as commanded by the autopilot. 5

21 δ r δ t μ σ E min max d B dt x b gx b ax Rudder deflection, as commanded by the autopilot. Percentage of available thrust that is commanded by the autopilot, on a unit scale where δ t = 0 means use no thrust and δ t = 1 means use all available thrust. Thrust force is simulated as being directly proportional to δ t. Value that represents the mean of a set of data. Same as the first moment of a set of data. The expected value operator returns the mean. Value that represents the standard deviation of a set of data. The standard deviation of a dataset is the square root of the variance of the dataset. Operator that calculates the expected value of a set of data. The value returned is the mean of the set of data that was operated on. Represents the minimum value of a set of data. Represents the maximum value of a set of data. Time derivative performed in the B Frame. The hat symbol designates the output of the state estimator for the variable x The gyro bias about the body frame axis designated by x The accelerometer bias in the direction of the body frame axis designated by x 6

22 3 Background Information This section contains technical background information that is used throughout this document. 3.1 Reference Frames When expressing the components of vectors, there are two frames of reference that are used in this document. Those are the aircraft body fixed B frame and the earth-fixed inertial N frame. An earth fixed frame isn t truly inertial, as the frame rotates as the earth rotates, and the earth rotates about the Sun, and the Sun rotates around the galaxy, and so on. However, in this document, the earth-fixed N frame is considered the base inertial reference frame, unless it is otherwise explicitly stated. The one exception is in the covariance calculation in the sensor fusion of method 2. The body fixed B frame is a Cartesian frame whose origin is at the center of gravity of the aircraft. The X (1 st ) axis points outward from the nose of the aircraft, the Y (2 nd ) axis points out the right side of the aircraft, and the Z (3 rd ) axis points directly downward out of the aircraft. Knowing the exact geometric orientation of this frame with respect to aircraft parts is not necessary for this study. For the purposes of this study, all sensors will be assumed to be at the center of gravity of the aircraft. The inertial earth-fixed N frame is also referred to as the North-East-Down or N-E-D frame. The N frame is also a Cartesian frame, and its origin is at the surface of the Earth, at the position of the start of the simulations in this document. All distances measured in the N frame are with respect to the starting point of the simulation, not actual latitude and longitude coordinates. A visualization of these two frames is shown in Figure

23 Figure 3.1: Aircraft Reference Frames The relative orientation of these two frames is described in this paper using Euler angles. These Euler angles describe the orientation of the aircraft B frame relative to the inertial N frame through a series of three rotations. The B frame is obtained by rotating the first two axes of the N frame about the 3 rd axis by an amount known as the yaw angle ψ, then a rotation about the new transformed axes 2 nd axis by an amount known as the pitch angle θ, and then a last rotation about this new transformed axes 3 rd axis by an amount known as the roll angle φ. These three measures of rotation comprise the Euler angle collection in equation (3.1). φ ψ = [ θ] (3.1) ψ This collection of Euler angles is referred to as the aircraft s attitude. Using this definition, the attitude information can be used to map a vector written in body frame components into inertial frame components with equation (3.2). N B [A] = R NB [A] (3.2) 8

24 R NB in Equation (3.2) represents the direction cosine matrix (DCM) or rotation matrix that maps vectors from the B frame into the N frame. The definition of this DCM for Euler angles (3.1) is in equation (3.3), which is provided in [8]. cos ψ cos θ cos ψ sin θ sin φ cos φ sin ψ cos φ cos ψ sin θ + sin ψ sin φ R NB = [ sin ψ cos θ sin ψ sin θ sin φ + cos φ cos ψ cos φ sin ψ sin θ cos ψ sin φ] (3.3) sin θ cos θ sin φ cos θ cos φ A DCM is an orthogonal matrix such that its transpose performs an inverse mapping, such as in equations (3.4) and (3.5). T R BN = R NB (3.4) B N [A] = R BN [A] (3.5) Finally, it is important to realize that the time derivatives of Euler angles are not simply the rate of change of their values, but are instead the rate of change of their angles in their respective intermediate frames. Reference [9] provides this relationship, given in equation (3.6). φ 1 sin φ tan θ cos φ tan θ B [ θ ] = [ 0 cos φ sin φ ] [ω B/N ] ψ 0 sin φ sec θ cos φ sec θ (3.6) B [ω B/N ] is the angular velocity of the aircraft with respect to the inertial frame, written as aircraft body frame components of roll rate, yaw rate, and pitch rate, as given in equation (3.7). B [ω B/N ] p = [ q] (3.7) r Thus, the relationship between the yaw, pitch, and roll rates and the Euler angle rates is equation (3.8). 9

25 φ 1 sin φ tan θ cos φ tan θ p [ θ ] = [ 0 cos φ sin φ ] [ q] (3.8) ψ 0 sin φ sec θ cos φ sec θ r 3.2 The Wind Triangle The next piece of information that is used throughout this document is the concept of the wind triangle. The wind triangle relates the three measures of velocity that are used throughout this document. The wind triangle is shown in Figure 3.2. Figure 3.2: The Wind Triangle The vector V B/N represents the velocity of the aircraft with respect to the inertial Earth-fixed frame. In this paper, vector subscripts X/Y designate the vector quantity of the X frame relative to the Y frame. The way to read V B/N is the velocity of the body frame B with respect to the inertial frame N. The vector V a represents the velocity of the air over the aircraft, or the velocity of the air with respect to the body B frame. It points in the direction that the air mass is flowing towards. The vector V w is the velocity of the wind, or the velocity of the air with respect to the inertial N frame. It points in the 10

26 direction the wind is coming from. As illustrated by the wind triangle in Figure 3.2, these vectors are related through equation (3.9). V B/N = V a + V w (3.9) Now, the notion of air data is also used throughout this document. Air data is the coined term for the scalar values for airspeed V a, angle of attack α, and angle of sideslip β. When the airspeed vector is written in aircraft body coordinates, air data is related to the vector through the following relationship. cos α cos β B [V a ] = V a [ sin β sin α cos β ] (3.10) 3.3 The Extended Kalman Filter A Kalman filter is a software algorithm that determines a best guess estimate of a set of states given relationships of noisy sensor inputs to the states that are being estimated. This best guess is the values for the states that give the minimum variance of the errors between those estimated state values and the state values that a sensor model gives. This sensor model is a known or expected relationship from available sensor data to the states that are being estimated. To explain that in mathematical terms, an extended Kalman filter (EKF) filters the states that are provided by a nonlinear state-space model that describes the relationship of the states to sensors and the relationship of the states to state time derivatives. This set of relationships is described by the set of equations (3.11) and (3.12). x EKF,expected = F EKF (x EKF,expected, u EKF ) + ε (3.11) y EKF = H EKF (x EKF,expected, u EKF ) + η (3.12) 11

27 In this non-linear state space relationship, x EKF,expected is a vector of the states being estimated by the EKF, y EKF is a vector of the measurements, and u EKF is the vector of pre-determined inputs to the EKF that are to be taken as truth. As discussed later, not all values needed by a Kalman filter can be treated as measurements. Most often, some values need to be treated as true error-free measurements for the state-space system in (3.11) and (3.12) to be formed. A normal (not extended) Kalman filter is an extended Kalman filter whose state space relationships in (3.11) and (3.12) are linear. Now, the variance of the error in the estimate of the states is represented by the covariance matrix P EKF whose definition is in equation (3.13). P EKF = E [(x EKF x EKF,expected )(x EKF x EKF,expected ) T ] (3.13) x EKF Represents the estimated state vector that is the output of the EKF. x EKF,expected Represents the states that are computed via the expected relationship to the measurements that is given by equations (3.11) and (3.12). The operator E is the expected value, which returns the first moment of a dataset, the mean. The expected value operator determines the mean of all of the occurrences of the expression it operates on. When Kalman filters are used in real-time control systems like for small UAVs, equation (3.13) calculates the mean of all the error covariances of all the updates to the states that have occurred thus far. Next, the minimum error variance estimate is obtained by minimizing the covariance matrix P EKF. In an EKF, this is done in a two-step procedure; once each time an update for the measurements y is obtained, using relationship (3.12), and once each time a prediction can be made using relationship (3.11). 12

28 3.3.1 Measurement Update The procedure that is followed at each update of the measurement vector y EKF is called the measurement update. The measurement update creates a minimum variance estimate of the states x EFK,k and updates the covariance matrix P EKF,k for the current update k. These actions are performed by first calculating the Kalman gain matrix K EKF,k for the current update k as is outlined in equation (3.14). H EKF,k = H EKF (x EKF,k 1, u EKF,k ) T T K EKF,k = P EKF,k 1 H EKF,k (H EKF,k P EKF,k 1 H EKF,k + R EKF ) 1 (3.14) R EKF is a constant (unchanging) matrix representing the expected covariance of the measurements and the H EKF function is the matrix in the relationship in equation (3.12). The subscript k 1 on a tensor indicates using the values from the previous Kalman filter update of that tensor. The subscript k indicates the values for the current update of that tensor. Once it s computed in (3.14), the Kalman gain matrix K EKF,k for this measurement update is used to compute the updated state estimate x EKF,k and the updated covariance matrix P EKF,k, as in equations (3.15) and (3.16), respectively. x EKF,k = x EKF,k 1 + K EKF,k [y EKF H EKF x EKF x EKF,k 1 ] (3.15) P EKF,k = (I K EKF H EKF,k x EKF,k ) P EKF,k 1 (3.16) The matrix H EKF x EKF is the Jacobian of the non-linear measurement relationship H EKF in equation (3.12). At this point, both the updated minimum error variance estimate for the states x EKF,k and the updated error covariance matrix P EKF,k are saved off for the next (measurement or time) update. 13

29 3.3.2 Time Update The procedure that is followed each time a state prediction update can be made using relationship (3.11) is called the time update. While the measurements y EKF do not have to be available, the inputs u EKF do need to be available. Typically, inputs u EKF are not available continuously, but if equation (3.11) can be evaluated continuously, a time interval is chosen so that a balance between processor usage and estimator performance is achieved. In either case of discrete or continuous inputs, the time update is performed many times in between each measurement update. At each of these time updates, the state estimate is predicted from the measurement relationship in equation (3.11), using a simple first-order integration scheme as depicted by equation (3.17). x EKF,k = x EKF,k 1 + T s N F EKF(x k 1, u k ) (3.17) The index k 1 indicates to use the information from the most recent previous time or measurement update. If time and measurement updates occur at the same time, a time update is always performed first, and then the k indexed output of the time update becomes the k 1 information used in the measurement update. Note that with a nonlinear EKF, the above integration to estimate the state vector x EKF,k can be run multiple times to reduce errors due integration errors from nonlinearities in the state propagation. The variable N in equation (3.17) is the total number of iterations and T S is the time that has elapsed since the last measurement update. Inherently, there is trade space between processor consumption and performance of the time update. Now, to update the covariance matrix at each time update, a matrix is created that relates the states of the previous time step x EKF,k 1 to the states of the current time step x EKF.k. This matrix is called the state transition matrix Φ, and is derived using the relationship F EKF from equation (3.11) as depicted in equation (3.18). 14

30 x EKF,k = Φ EKF,(k,k 1) x EKF,k 1 (3.18) Φ EKF,(k,k 1) = F EKF(x EKF,k,u EKF,k ) x EKF Φ EKF,(k,k 1) (3.19) where Φ EKF,(k 1,k 1) = I (3.20) The subscripts (a, b) on the state transition matrix Φ EKF designates that the matrix transitions the state vector x EKF from update b to update a, like the transition matrix in (3.18) maps the previous k 1 update to the current update k. Equation (3.19) could be solved analytically or numerically. A numerical solution would require linearizing the state dynamics of F EKF in equation (3.11) about the updated state vector x EKF,k that was determined in equation (3.17). With a solution for the state transition matrix, like is obtained in state estimation method 2, the state error covariance matrix can be updated via equation (3.21). T P EKF,k = Φ EKF,(k,k 1) P EKF,k 1 Φ EKF,(k,k 1) + Q EKF (3.21) The matrix Q EKF is a tunable parameter that represents the expected covariance of the state derivatives as determined by relationship (3.11). However, solutions for the state transition matrix are often not easily obtainable. If the state transition matrix is not obtained, a linearized solution of (3.19) could be used to update the covariance matrix.. The first term of the Taylor series expansion of the solution to the differential equation (3.19) is derived in [4], and it results in the ability to update the covariance matrix as is described in equation (3.22). P EKF,k = P EKF,k 1 + T s N ([ F EKF(x EKF,k,u EKF,k ) ] P x EKF,k 1 [ F T EKF(x EKF,k,u EKF,k ) ] ) + Q EKF x EKF (3.22) EKF 15

31 It is important to note that this is a linearized solution of the nonlinear relationship between two states, as depicted by (3.18), and so like the time update for the state vector, updating the covariance matrix can also be iterated a number of times to improve the estimate. The variable N in equation (3.22) is the iteration number, and the matrix Q EKF is again the expected covariance of the error in relationship (3.11). Performing this calculation is essentially the same a performing a numerical integration to determine the state transition matrix that is needed for (3.21), but both forms of updating the covariance matrix (3.21) and (3.22) are used in this document. 16

32 4 Aircraft States Needed for Autopilots Before attacking any state estimation problem, it is critical to first know what states needs to be gathered. Ideally, a state estimator would be able to accurately reproduce every state of an aircraft.. However, full state estimation is often impossible given a finite set of sensors like the ones that are typically available on a small UAV. Further, estimating more states than are needed can be excessively costly and computationally intensive. This paper will focus on capturing only the aircraft states that are necessary for autopilot control of small UAVs. 4.1 Scoping the Estimation Problem True total information on the kinematic state of an object would involve a description of the continuous motion of an object, such that all orders of derivatives of the motion can be definitively described. However, a description of the continuous motion of an aircraft is not possible in a real world system, as the true motion of the aircraft can never be known ahead of time, and digital sensors that provide information are discrete. Thus, state estimation for aircraft is performed as determining the current states of an aircraft a given point in time. While it could be desired to obtain all of the time derivatives of the motion of an aircraft, second order information is all that is required for most UAV applications. In this research, the collection of states in (4.1) will be considered the complete set of states that could be desired to be gathered. x complete = {p B/N, V B/N, a B/N, ψ, ω B/N, α B/N, V w } (4.1) The vectors in x complete describe the inertial position, velocity, and acceleration of the aircraft, the angular position, angular velocity, and angular acceleration of the aircraft relative to the inertial frame, and the velocity of the airflow around the aircraft. It can be noted that any combination of two of the 17

33 velocity vectors V B/N, V a, and V w can be used in (4.1) to fully describe the velocities of the aircraft and the air, as the three are related through equation (3.10). Deeper information could be included in this list, such as the angular acceleration of the aircraft, the rate of change of wind velocity, or even the rotational motion of the air. However, deeper information is not necessary for autopilot control, as will be shown throughout this paper. Deeper information is also often very difficult, expensive, and impractical for small UAVS. While there are a number of different existing autopilot algorithms that each require a different states of states, (4.1) is considered a superset of the states that all common small UAV autopilots would use to control a small UAV. This is a superset because some of the states listed in (4.1) are not necessary for autopilots to perform acceptably well. Much work has been done in estimating the states that makeup the wind vector V w, such as in [10], [11], and [12]. However, full 3-dimensinoal measurements of the wind vector, even in windy conditions, are not necessary for an autopilot to function. They would be needed for an autopilot that employs an energy harvesting algorithm, or for a mission to gather wind information, but is not strictly necessary for autopilots to maintain basic stable flight, as will be shown shortly. While it is not important to recover full information about the wind vector, it is important to obtain a rough measure of the airspeed of the aircraft for the purpose of ensuring the aircraft doesn t travel too slow in large tailwinds and stall. With just inertial velocity information, airspeed has to be assumed close to the inertial velocity. Such an assumption can easily lead to stall because it is not uncommon for small aircraft to experience wind speeds close to the speed of their inertial velocity. Airspeed sensors that can remedy this problem are readily available today for under $50 USD. Nonetheless, the lack of a need for full wind vector V w stems from the aerodynamic behavior of small UAVs. Small UAVs are lightweight and thus naturally quickly align themselves into the airflow. This results in relatively small disturbances in angles of attack and angles of sideslip when compared to the 18

34 disturbances seen by large scale aircraft. Small UAVs are also typically designed to be aerodynamically stable, so rapid control authority is not necessary to maintain attitude. In contrast, some large modern maneuver aircraft, such as the Lockheed Martin F-22, are aerodynamically unstable and require significant and rapid control authority to maintain attitude. This behavior common to most small UAVs is often used to actually represent the wind vector in terms of other estimated states. By assuming that sideslip is zero and that the angle of attack is equivalent to the pitch angle, equation (3.10) for the airspeed vector V a can be simplified to equation (4.2). cos α cos β B [V a ] = V a [ sin β ] β=0,α=θ cos θ B [V a ] = V a [ 0 ] (4.2) sin α cos β sin θ This assumes that the airspeed magnitude V a is either directly measured or estimated. As discussed above, obtaining the airspeed magnitude is necessary to avoid stall and thus V a should always be available in some fashion. If the inertial velocities are also estimated, the airspeed vector in equation (3.9) can be substituted with equation (4.2) and rearranged to obtain equation (4.3), which is a rough estimate of the wind vector V a. B B [V w ] = [V B/N ] cos θ V a [ 0 sin θ ] (4.3) This simplification is the basis for showing that estimating the angle of attack and angle of sideslip are not necessary. Both the autopilot and the two state estimation methods described and simulated in this document do not estimate the full wind vector V w. Instead, they use an airspeed sensor and the assumption in (4.2) where air data is required. The validity of this assumption that is discussed in the prior section is 19

35 demonstrated here using a simulation of the RECUV Tempest small UAV, employing a total energy controller that processes altitude, course angle rate, and airspeed commands. 4.2 Overview of the Autopilot While the focus of this document is on state estimation, an autopilot is needed to control the flight of the aircraft in the simulations so that analysis of the state estimation methods can be done under different benchmark maneuvers. The autopilot that is used to perform this task is a total energy based controller, which is a common controller to use for small UAVs. The specific controller used here takes commanded airspeed V ac, altitude h c, and course angle rate χ c and commands thrust, aileron, rudder, and elevators. It does this by following the algorithm in [4] and [13]. Table 4.1: Total Energy Autopilot Algorithm Description Step Description 1 Calculate a commanded bank angle φ c using a Proportional, Integral, and Derivative (PID) controller that process the error between the commanded course angle rate and the course angle rate that is necessary for the desired turn, which is in equation (4.4). φ c = PID(χ c χ ) = PID (χ c g V g tan φ) (4.4) This equation assumes that the course angle rate is equivalent to g V g tan φ. This is only the case in zero wind. The effects of this will be seen in the simulation results that follow. 20

36 2 Calculate aileron deflection δ a to control the bank angle φ using a PID control that process the bank error, as in equation (4.5). δ a = PID(φ c φ) (4.5) 3 Determine commanded pitch angle θ c from a PID controller on the airspeed error, as in equation (4.6). θ c = PID(V ac V a ) (4.6) This essentially just increases pitch to decrease airspeed and decrease pitch to increase airspeed. This means that in order for the aircraft to climb, there must be excess airspeed. This observable is seen in all of the simulations in this document. 4 Calculate elevator deflection from commanded pitch using a PID controller, as in equation (4.7) δ e = PID(θ c θ) (4.7) 5a If the aircraft is within 10 meters of the ground, assume plane is in takeoff, and set the commanded thrust to a constant as in equation (4.8). δ t = 0.8 (4.8) Note that there is no landing mode for this autopilot. 5b If the aircraft is more than 10 meters off the ground, use a total energy approach to command thrust. This total energy calculation is uses a PID controller on the error between the 21

37 commanded and current sum of the kinetic and gravitational potential energies, as depicted in equation (4.9). δ t = PID (g(h c h) (V a c 2 V a 2 )) (4.9) 6 Calculate the rudder deflection that enforces zero sideslip. This could be done with a PID controller against estimated sideslip angle β, but this autopilot recognizes that β is often not available and assumed to be zero as in equation (4.2). So, the goal in this case is to instead zero the specific side force, which is the acceleration along the body Y axis a y. The sum of this acceleration component and the component of the gravity vector along the body Y axis is provided by the state estimator as the accelerometer measurement y ay. This yields equation (4.10). δ r = k p,side force a y = k p,side force (y ay g cos θ sin φ) (4.10) Equation (4.10) depicts how a proportional controller is used to zero the specific side force, or in other words zero the acceleration along the body Y axis. On a longitudinally symmetric aircraft, this also zeroes the sideslip angle β, even in wind. It can be noted that as discussed earlier, small UAVs typically quickly align themselves into the airflow. This autopilot employs this controller as an additional measure to ensure sideslip is zero and thus coordinated turns are forcibly maintained. Judging the necessity of a small UAV s autopilot to actively control sideslip is outside of the scope of this research. The gains for the PID controller in the algorithm were designed such that they achieve acceptable closed loop tracking of their states. 22

38 So in summary, this specific autopilot that is used throughout this document requires the inputs listed in equation (4.11). u autopilot = [h p q φ θ V a V g y ay ] (4.11) The states in Equation (4.11) are the states that this paper will focus on estimating, as they are what is needed for autopilot control. Note that this is still a subset of (4.1), as the airspeed is a function of the state vectors in (4.1), as depicted by (4.12), and the ground speed V g is also a function of the state vectors in (4.1), as depicted by (4.13). V a = V a = V B/N V w (4.12) V g = f(v B/N, ψ) = N VB/N [1 1 0] = R B NB VB/N [1 1 0] (4.13) Now, a distinction should be made here that these are the states needed for autopilot control and not overall navigation. A navigation algorithm is what takes user commands, such as destination, course angle, and/or altitude, and turns them into autopilot commands, which in this case are altitude, course angle rate, and airspeed. Analyzing accurate navigational layer control in windy conditions is outside of the scope of this research because state estimation algorithms inherently provide the information necessary for navigation. All that is really needed for navigation is inertial position and velocity estimates, and those can be obtained at a low rate directly from a GPS receiver. The only complexity of navigation that should be mentioned here is that the navigation layer needs a calibrated closed loop controller on course angle χ, as the autopilot takes course rate χ commands. As will be explained, method 1 estimates the course angle for this purpose, and if the first two inertial frame components of the inertial velocity vector are available, like in method 2, the course angle can be determined via equation (4.14). 23

39 N χ = tan 1 ( [V B/N][1 0 0] N ) (4.14) [V B/N ][0 1 0] Note that the equation (4.14) must be calculated using a quadrant-safe inverse tangent function. Course angle could be determined directly from GPS, but using a state estimator s output provides a higher sample rate that the navigation layer can take advantage of. 4.3 Simulation Setup Before focus can be turned to the state estimation methods, it must first be shown through simulation that this autopilot is able to effectively control a small UAV with the limited suite of state information listed in (4.11). To properly assess the autopilot s tracking of its commanded altitude, course angle rate, and airspeed, assessments must be made in flight regimes that small UAV autopilots and their state estimators are expected to be used in. To ensure each of these flight regimes is tested, the simulations throughout this document employ a number of benchmark maneuvers, which are described here Benchmark 1: Straight and Level Flight The first benchmark maneuver that is used in the simulations is straight and level flight, with initial conditions equal to the commanded course angle rate, altitude, and airspeed, as depicted in (4.15). h 0 = h c = 100 m, χ c = χ = 0, V a0 = V ac = 15 m/s (4.15) In all of the benchmark maneuvers in this document, the initial conditions in (4.16) are used. p 0 = q 0 = r 0 = φ 0 = θ 0 = ψ 0 = p e = p d = 0 (4.16) An illustration of the movement of the simulated aircraft is in Figure

40 Figure 4.1: Straight and Level Flight Illustration Benchmark 2: Coordinated Turn The second benchmark maneuver that is used in the simulations is a coordinated turn. A coordinated turn is a turning maneuver that involves elevator, aileron, and rudder deflections such that no acceleration is experienced in the lateral Y axis direction of the aircraft. Such a maneuver is illustrated in Figure

41 Figure 4.2: Coordinated Turn Illustration The simulations that are performed in this document for benchmark 2 are 90 seconds in duration, which is the time required for 2.5 complete circles at a constant course angle rate of 10 degrees per second. The autopilot enforces a coordinated turn through controlling the lateral specific force to zero, as in equation (4.10), commanding roll angle required for a coordinated turn, as in equation (4.4), and by commanding pitch through commanded airspeed, as in equation (4.6). It can be noted that the actual equation for required roll angle for a coordinated turn is equation (4.17). χ = g V g tan φ cos(χ ψ) (4.17) In the absence of wind, the course angle χ is equal to the yaw angle ψ, which transforms equation (4.17) into equation (4.18). χ = g V g tan φ (4.18) Since the autopilot still uses error in commanded airspeed instead of error in commanded course rate via (4.17) or even (4.18) in a coordinated turn, there is an observable lag in achieving a coordinated turn 26

42 in the simulations when a change in commanded course angle is applied. This lag is characterized by an oscillation in pitch that is short in duration, which will be shown in simulation shortly. To illustrate this behavior, simulations are performed for coordinated turns by initially having the aircraft at the same straight and level initial conditions as benchmark 1 in (4.15) and (4.16), but with a commanded course angle rate χ c of 10 degrees per second Benchmark 3: Wings Level Ascent The third benchmark that is used in this document is a wings level ascent. For this benchmark, the same commanded altitude, airspeed, and course angle rate are the same as in the benchmark 1 (4.15). The initial airspeed and course angle rate are also the same as (4.15), but the initial altitude is taken to be zero. This setup is summarized in (4.19). h 0 = 0, h c = 100 m, χ 0 = 0, χ c = 0, V a0 = V ac = 15 m/s (4.15) This essentially simulates takeoff, but beginning at the full commanded airspeed so that the aircraft doesn t stall, as rolling on the ground is not part of this simulation. Simulations of this benchmark maneuver are stopped when the aircraft becomes within 1 meter of its commanded altitude. Figure 4.3 illustrates this maneuver. 27

43 Figure 4.3: Wings Level Ascent Illustration Simulation of all three pieces of the ascent takeoff, climb, and cruising altitude approach enables observations and insights that are discussed later The Dryden Wind Model This document simulates the wind necessary for analysis using the Dryden wind model. The Dryden wind model is used throughout department of defense projects as a standard by which aircraft systems should be designed to when considering wind. The Dryden wind model that is used in this document generates wind noise by summing sinusoidal signals with frequencies of relative power spectral densities that are given by military specification MIL-F-8785C. The equations for the spectral density take two parameters scale length and the wingspan of the aircraft that s being simulated. These two parameters provide how winds at high altitude should be scaled to those at low altitude, and the power of disturbances on the aircraft s angular rates, respectively. Note that for these simulations, high 28

44 altitudes, which are on the order of 2000 feet above ground level, are never reached, so the scale length has negligible impact on the wind speeds in our simulations. The simulations in this document use both outputs of the Dryden wind model wind speed disturbances and angular rate disturbances. The Dryden wind model uses a frozen turbulence field, so the wind speed output of the Dryden model must be added to the mean wind speed to obtain the actual wind speed value. This is depicted by (4.16a) below. w n [ w e ] = [ w d w n w e w d ]mean δw n + [ δw e δw d ]Dryden This wind velocity output is subtracted from the inertial velocity to obtain the airspeed that is used in the linearized aerodynamic model of the Tempest UAV that the simulation uses. This subtraction is in accordance with the wind vector convention in equation (3.9). Second, the rate disturbance output of the Dryden wind model is added to the angular rates of the aircraft after the aerodynamic forces and moments are used to calculate the rigid body dynamics of the aircraft. This is depicted in equation (4.16b). p p [ q] = [ q] r r model δp + [ δq] δr Dryden (4.16b) These wind-augmented angular rates are then used as the input to the rigid body dynamics equations in the simulations. The simulations throughout this document use a mean wind speed of 7 meters per second and a turbulence probability of exceedance of high-altitude intensity measurement set to 10^-5. That level of 29

45 turbulence is classified by the Dryden model as severe. Figure 4.4 below shows wind and rate disturbances that result from this wind. Dryden Wind Velocity Wind Speed (m/s) w n w e w d Time (seconds) Dryden Wind Distrubance on Angular Rates Angular Rate Disturbance (deg/s) Time (seconds) p q r Figure 4.4: Example Dryden Wind Model Simulation Statistics on the absolute value of the wind and angular rate disturbance values presented in Figure 4.4 are shown in Table 4.2. Table 4.2: Statistics of the Absolute Value of the Dryden Wind Components μ σ μ abs σ abs min abs max abs w n w e w d δp δq δr The benchmark maneuvers that were previously discussed all use a commanded airspeed V ac of 15 m/s. The wind speeds shown in Figure 4.4 become as large as about half of this value. Thus, as also previously discussed, it is imperative that the state estimators used here employ an airspeed sensor to ensure that the aircraft does not stall. Also, the Beaufort wind scale [14] classifies a wind speed of 7 m/s 30

46 as a Moderate Breeze at which tree branches begin to move. This is a wind speed that small UAVs will often encounter. It will be seen shortly that even this moderate amount of wind is sufficient to highlight the strengths and weaknesses of the two state estimation methods analyzed here. Moreover, extreme winds not a part of this simulation. The scope of this research is limited to the wind that UAVs might typically see outside of severe weather anomalies, as successfully controlling aircraft in such conditions involves more complex autopilots and navigational algorithms that are not used here. 4.4 Sensors All of the simulations in this document employ an identical suite of sensors. All simulations involve the use of a MEMS gyro and a MEMs accelerometer, which comprise the IMU sensor system. The gyros provide the angular rates of the body fame relative to the inertial frame, and the accelerometers provide measured N frame accelerations along the body frame components, plus gravity. Also included is a GPS sensor, which provides measures for the position in the inertial N frame and the velocity in the inertial N frame. The accuracy of the sensors modeled used in the simulations are determined from the spec sheets of actual off the shelf sensors that may be used in UAVs. The IMU sensor models used in the simulation are set to closely match the following sensors: o o Gyroscope: Analog Devices ADXRS450 Accelerometer: Analog Devices ADXL325 The GPS sensor model that is used in the simulations in this document have Gaussian noise of 0.21 meters in the north and east directions and.4 meters in the vertical direction, modelled as Gauss- Markov process with a time constant of The GPS sensor produces a non-time varying Gaussian error of 0.2 meters per second in the north, east, and down directions. 31

47 4.5 Demonstration of Autopilot Performance with Limited State Information Now that the setup of the simulation has been discussed, a simulation can be performed to validate the notion that the autopilot input states in (4.11) are at least a superset of the states that need to be estimated for an autopilot to successfully pilot an aircraft in both windy and non-windy conditions. That is, it needs to be shown that this autopilot can successfully control the aircraft with the sates in (4.11). This by no means rules out the possibility that an autopilot could control an aircraft successfully in wind with fewer states. But by illustrating that the states in (4.11) are sufficient for autopilot control of an aircraft in windy conditions, state estimation methods that obtain those state can be said to be sufficient to enable autopilot control of an aircraft in windy conditions. So, the performance of the autopilot discussed here will be measured by its error against its commanded inputs (airspeed, altitude, and course angle rate), under the three different benchmarks that were discussed. To demonstrate the effectiveness of the autopilot in these simulations, the autopilot uses the true values of the states in (4.11) rather than through a state estimation method. An analysis of the sensitivity of the autopilot s performance to errors in the different states is beyond the scope of this research, as the purpose of this research is solely to measure the performance of the state estimation methods sensitivity to wind, and not an autopilot s sensitivity to state errors. Further, an analysis of the sensitivity the autopilot to the states using the autopilot here would not necessarily be representative of autopilots as a whole. Nonetheless, a limited sensitivity analysis using constant state errors under benchmark 2 is performed in section Autopilot Performance under Benchmark 1 Benchmark 1 is straight and level flight. Figure 4.5 below shows the error of commanded course angle rate, χ χ c, the error in the commanded airspeed V a V ac, and error in the commanded altitude h 32

48 h c. This is the sign convention for error that is used throughout this paper the desired value subtracted from the actual value. This means that a positive error represents the actual value being higher than the desired value. For example, here, a positive airspeed error represents the actual airspeed being higher than the commanded airspeed. Figure 4.5: Command Tracking Error for Benchmark 1 in Zero Wind Statistics of the error shown in Figure 4.5 are provided in Table 4.3. The subscript abs in Table 4.3 indicates that the statistic is calculated on the absolute value of the corresponding state error. The absence of the subscript abs denotes that statistic is calculated on the raw state error, without first performing an absolute value operation. This convention will be used throughout this document. Table 4.3: Command Tracking Error Statistics for Benchmark 1 in Zero Wind μ σ μ abs σ abs min abs max abs 33

49 Course Angle Rate (deg/sec) Airspeed (meters/sec) Altitude (meters/sec) Figure 4.5 and Table 4.3 show that the autopilot closely tracks all three command inputs. Relating to the standard deviation of the raw error commands, the autopilot typically keeps the course angle rate within 0.1 deg/s, the airspeed with 0.08 m/s, and the altitude within 0.q5 m. It can be noticed that although the initial airspeed and altitude of the vehicle are set to the commanded airspeed and altitude, the autopilot induces a small deviation from the commanded airspeed and altitude for a short time. These deviations are small, roughly 0.5 m/s in airspeed and roughly 1 m in altitude, and are due to the fact that the initial pitch angle is set to zero rather than a slightly positive number, which is needed to maintain the commanded altitude. Also, the continual oscillations in course angle rate are small, and are due to the non-zero noisy input of the accelerometer that measures side force, y ay. The accelerometer that measures acceleration along the body Y axis is passed through a low pass filter, as described later, but its output is still noisy due to the high cutoff frequency that is used in the low pass filter. This measurement is used by the autopilot to command rudder deflection, artificially creating sideslip and thus a non-zero course angle rate. If y ay is taken to be zero, simulations not illustrated here have produced exactly zero course angle rate error. Overall, the autopilot performs remarkably well for straight and level flight with zero wind. Next, Figure 4.6 below is a plot of the autopilot s performance for straight and level flight under the light wind Dryden model illustrated in section

50 Figure 4.6: Command Tracking Error for Benchmark 1 in Light Wind When Figure 4.6 is compared to Figure 4.5, it can be seen that the wind induces a definite measurable error in the state tracking. Statistics for the error in the autopilot s tracking of the commanded states is quantified by Table 4.4 below. Table 4.4: Command Tracking Statistics for Benchmark 1 in Light Wind μ σ μ abs σ abs min abs max abs Course Angle Rate (deg/sec) Airspeed (meters/sec) Altitude (meters/sec) When the statistics for light wind in Table 4.4 are compared to the statistics for zero wind in Table 4.3, it can be seen that the command tracking error in zero wind is relatively negligible. The average values of the absolute values of the error in light wind for course angle rate, airspeed, and altitude, are much 35

51 greater than the errors in light wind. This, along with the oscillations seen in Figure 4.6, are caused by both the effects of the wind velocity on the aerodynamic forces and moments on the aircraft, and the angular rate disturbances that the Dryden wind model provides. This combination of perturbations causes changes in the course angle rate, airspeed, and altitude that the autopilot must correct for this. These statistics illustrate that the autopilot does a very good job of maintaining the commanded course rate, airspeed, and altitude, as all three values stay very close to their commanded values. Now, before moving on, it is important to note that the wind observed in the simulations is not exactly identical to those in Figure 4.4. The Dryden wind model used here uses seed values for its random noise generator, but the Dryden model varies its frequency spectrum based on the ground speed and orientation of the aircraft to simulate a consistent wind field that the aircraft can fly through in different manners. Thus, the wind speeds and angular rate disturbances as seen by the aircraft are not always exactly the same. They will only be the same if the aircraft maintains straight and level flight at a constant airspeed through the wind field. With an aircraft flying in wind, that is never the case, so the wind seen by the aircraft is slightly different in each simulation. To keep the discussion concise, plots and statistics of the exact wind field are only included when they become relevant enough to the discussion Autopilot Performance under Benchmark 2 Next, the aircraft s ability to track its states is measured under a coordinated turn maneuver. Figure 4.7 shows the error between the commanded course angle rate, airspeed, and altitude states during a coordinated turn at a commanded 10 deg/s course angle rate in zero wind. 36

52 Figure 4.7: Command Tracking Error for Benchmark 2 in Zero Wind Like in benchmark 1, it can be seen how an initial pitch angle of zero creates a small initial error in commanded airspeed and attitude. However, it also creates a small disturbance in the initial course angle rate. This simulation could be started once the aircraft is in full a coordinated turn, or the initial condition required for pitch to avoid these oscillations could be done, but it is not necessary to analyze the ability of the autopilot to maintain a coordinated turn. In fact, it also illustrates how the autopilot is also able to enter a coordinated turn. Table 4.5 below contains the statistics of the error in tracking the commanded states for this coordinated turn in zero wind. Table 4.5: Command Tracking Statistics for Benchmark 2 in Zero Wind μ σ μ abs σ abs min abs max abs 37

53 Course Angle Rate (deg/sec) Airspeed (meters/sec) Altitude (meters/sec) Both Figure 4.7 and Table 4.5 show that the error in tracking these states is very small and that the autopilot tracks the commanded states for a coordinated turn very well. Next, Figure 4.8 shows the error in the command tracking during a coordinated turn in Dryden wind. Figure 4.8: Command Tracking Error for Benchmark 2 in Light Wind Figure 4.8 shows that like benchmark 1 in wind, there is significant oscillation in the error of the course angle rate, airspeed, and altitude. Statistics on the magnitude of this error oscillation is in Table

54 Table 4.6: Command Tracking Statistics for Benchmark 2 in Light Wind μ σ μ abs σ abs min abs max abs Course Angle Rate (deg/sec) Airspeed (meters/sec) Altitude (meters/sec) The tracking results for airspeed and altitude for benchmark 2 in wind do not appear to differ much from the results for benchmark 1 in wind. However, the error in the course angle rate for benchmark 2 in light wind is about double the error in the course angle rate for benchmark 1 in light wind. While a coordinated turn is not always held during severe gusts, this increase in course angle rate error is best described by the course angle rate equation (4.17). Equation (4.17) shows that as course angle increases and the difference between course angle and yaw angle increases, the course angle rate becomes larger in a sinusoidal fashion and not a linear fashion. For example, a rate disturbance causing a 10 degree difference between the yaw angle and the course angle has a much larger effect on the course angle rate when the bank angle is large than when it is small. Regardless, even in wind, the autopilot manages to track its command course rate with a near zero mean error Autopilot Performance under Benchmark 3 The last benchmark test is a wings level ascent. This benchmark highlights some of the assumptions made in the state estimation methods that are described later. Figure 4.9 below shows the errors in the commanded states for a wings level ascent in zero wind. 39

55 Figure 4.9: Command Tracking Error for Benchmark 3 in Zero Wind The command error plots in Figure 4.9 show that the aircraft starts at zero altitude at the commanded airspeed and successfully achieves the commanded altitude of 100 meters and airspeed. It can be noticed that there is an excess in airspeed as the aircraft climbs. This is due to the total energy controller driving airspeed up through commanded thrust to account for the insufficient gravitational potential energy prior to the aircraft reaching its commanded height. It can also be seen that there is still a small oscillation in course angle rate. As previously discussed in the straight and level flight benchmark, this is due to the noisy inputs of the lateral accelerometer input y ay, which in turn creates small sideslip angles and subsequent non-zero course rate angles. Table 4.7 below contains statistics for these errors in commanded state tracking. 40

56 Table 4.7: Command Tracking Statistics for Benchmark 3 in Zero Wind μ σ μ abs σ abs min abs max abs Course Angle Rate (deg/sec) Airspeed (meters/sec) Altitude (meters/sec) The statistics on airspeed and altitude are not very revealing. The error statistics for altitude and airspeed have large zero means, but in this benchmark, these large errors are expected to occur. The simulations for this benchmark are terminated when the aircraft reaches 1 meter of its commanded altitude, and due to the total energy controller as previously mentioned, airspeed is in excess until the commanded altitude is achieved. What really matters here is the fact that the controller successfully brings the aircraft to the commanded airspeed and altitude by the end of the simulation, which for zero wind is depicted by Figure 4.9. The only item of true interest in Table 4.7 is the statistics on the course angle rate. However, the average error of the course angle rate is very close to zero, and the standard deviation of the error about its mean is only 0.16 degrees. In conclusion, these results show that the autopilot is able to track the commanded air speed, altitude, and course angle rate well in zero wind. Figure 4.10 below shows the performance of the autopilot under Dryden wind. 41

57 Figure 4.10: Command Tracking Error for Benchmark 3 in Light Wind Figure 4.10 shows that the autopilot again successfully drives the initial airspeed and altitude command error to zero in Dryden wind. Table 4.8 below contains the statistics for the errors illustrated in Figure Table 4.8: Command Tracking Statistics for Benchmark 3 in Light Wind μ σ μ abs σ abs min abs max abs Course Angle Rate (deg/sec) Airspeed (meters/sec) Altitude (meters/sec) Table 4.8 shows that the course angle rate error is again small, with its mean being again essentially zero. The standard deviation of the course angle rate error is larger with wind, but that is to be 42

58 expected, and is the same order of magnitude of the previous wind simulations. This is still not concerning, because it is expected that the aircraft will be blow around by the wind, and these results show that the induced course rate error is still quickly driven to zero. In conclusion, the autopilot has been shown to perform well in Dryden wind, in that it consistently drives the wind-induced errors in commanded airspeed, altitude, and course angle rate to zero in all three benchmark maneuvers. This validates that the set of states identified in (4.11) are sufficient to achieve autopilot control of a small UAV in wind, so long as the states are estimated perfectly. 4.6 Relative Importance of the Chosen States Now, a short analysis of the sensitivity of the total energy controller autopilot s ability to maintain its commanded states is included here. This is not too detailed, as thoroughly analyzing a specific autopilot s sensitivity to state errors is not within the scope of this research. However, because state estimation and controller design are tied so close together, it is necessary to show which of the state estimates need to have the most stringent requirements so that the performance of different methods of state estimation can be properly assessed. This simple autopilot sensitivity analysis was performed by making constant errors in each of the states in (4.11), one at a time, keeping all other states equal to their true value. A more sophisticated analysis would involve randomly varying errors, but that is not performed here, as only high level conclusions on the effects of poor state estimation is desired. Table 4.9 below contains the results of such simulations, all performed using a benchmark 2 coordinated turn maneuver in light Dryden wind. Table 4.9: Sensitivity of the Autopilot s Command Tracking to Constant State Errors State Introduced Error Course Rate Error (deg/s) Airspeed Error (m/s) Altitude Error (m) μ σ μ σ μ σ 43

59 BASELINE h +5 m V a +3 m/s φ -10 deg θ -10 deg p +5 deg/s q +5 deg/s V g +5 m/s y ay +1 m/s Looking at Table 4.9 by commanded state individually, the autopilot s ability to track the desired course rate is most sensitive to the roll angle φ, as a 10 degree error in the roll angle estimate creates a steady state tracking error of about 7 deg/s. If the commanded course rate by the navigation layer controller cannot compensate for the autopilot not successfully tracking a desired course rate, this could cause a small UAV to perform indefinite circling maneuvers. Further, if this error is large enough, the aircraft could attempt to perform turns that are too sharp for it to maintain flight, and cause it to eventually crash. It should also be noted that the course angle rate is also sensitive to the ground speed estimate V g. This makes sense, as the coordinated turn equation (4.17) shows that ground speed is inversely proportional to the course angle rate. When an excess ground speed is estimated, the autopilot s roll hold equation (4.4) will think the actual course angle rate is smaller than it really is, resulting in a larger roll angle and faster turn. Next, the largest cause for error in airspeed tracking is the measurement of airspeed itself. The autopilot controls airspeed by directly measuring airspeed and commands correction to any error in what it measures with thrust. In most small UAV applications, maintaining airspeed is only critical to avoid stall. Thus, airspeed will be considered here to be one of the least important measures of state estimator performance, so long as the magnitude of the airspeed errors stay well below the difference between the flight speed and the stall speed. 44

60 Last, the largest error in altitude isn t due to error only in one state estimate. Errors in commanded altitude tracking are affected by errors in the estimates for altitude, roll, pitch, pitch rate, ground speed, and side specific force. This makes sense because the autopilot does not track altitude directly. The altitude command only appears in the equation for total energy, which outputs commanded thrust. Altitude is changed most directly by changing pitch, but commanded pitch is only a function of commanded airspeed. Thus, if pitch is off, the only effect is the aircraft performs a slow oscillation in altitude and subsequent airspeed. This notion is backed up by a simulation with an exaggerated pitch angle error of 30 degrees, the results of which are in Figure 4.11 below. Figure 4.11: Effects of Bad Pitch Estimation The effect of a pitch error is that the autopilot is commanding the appropriate pitch angle based on altitude, but instead tracks an offset in pitch angle. This causes the aircraft to pitch up or down, depending on the sign of the pitch angle error, and climb away or descend from the desired altitude, which in turn affects airspeed. Fortunately though, the increased airspeed that is accompanied with a decrease in pitch and flight path angle results in an increased pitch command, and thus the aircraft never crashes; it instead oscillates in altitude. Moreover, the autopilot also employs saturation on elevator deflection so that an extreme pitch angle is never reached. Thus, an error in pitch angle results in an undesired and interesting oscillation in altitude and subsequently airspeed. Fortunately though, an aircraft is still able to get to its destination this manner. 45

61 In conclusion, roll angle φ is the most important state to estimate correctly, as errors in it can cause the aircraft to indefinitely circle or in some extreme cases crash due to trying to undergo turns that are too sharp. Unfortunately, the state estimation methods used later in this document show that this is also the most difficult state to estimate. 4.7 Autopilot Summary This section has shown that a total energy controller autopilot is sufficient to control the RECUV Tempest small UAV in the zero wind, light wind, and heavy winds that are outlined in section 4.3.4, under the 3 benchmark maneuvers described in section 4.3. This proves that that the states listed in (4.11), h, p, q, φ, θ, V a, V g, and y ay, are sufficient information for an autopilot to control a small UAV in wind. The set of states in (4.11) are the set of states that this research will focus on estimating. Again, estimating air data states are not in the scope of this research. Air data states are more critical to autopilots for full scale aircraft, as illustrated by [15], and air data states may be necessary for particular small UAV applications, such as for wind energy harvesting. 46

62 5 Method 1: IMU Attitude, Smoothed GPS, and Pressure Sensors 5.1 Description Because of their required small size and low cost, small UAVs typically possess three types of sensors as input to their state estimators: micro-electro-mechanical systems (MEMS) gyros, MEMS accelerometers, and a GPS receiver. Thanks to the recent explosion of the demand for these devices in personal electronic devices, the cost of these sensors has dropped and supply has increased dramatically. Sometimes small UAVs also have other sensors, like three axis magnetometers (TAMs), horizon sensors, cameras [16], or range finders [17]. All of these additional sensors can present their own unique implementation issues, but more importantly, they are not inherently critical to state estimation for autopilot control, which will be shown shortly. Gyro and accelerometer sensors are often bundled together into a single unit which is often called an inertial measurement unit (IMU) or inertial navigation system (INS). Two main methods exist to estimate the state of an aircraft from an IMU and a GPS receiver. These two methods, which are described shortly, involve different approaches. Method 1 does not use a priori information at each time step, and instead simplifies the equations of motions to get estimated states. It uses two different EKFs to take care of sensor errors and errors induced by simplifying the equations of motion. Method 2 uses a priori information to numerically integrate its sensor inputs to propagate its position, velocity, and attitude estimates from the last time step, without needing to simplify the equations of motion. It then uses an EKF to smooth and filter the estimate. 47

63 5.1.1 The Transport Theorem Both of the state estimation methods in this paper rely on measurements of inertial acceleration, aircraft angular rates, and inertial (GPS) position and velocity measurements. To begin the description of the equations of motion used in these methods, an equation is needed to relate the states to these measurements. This is provided by [18] as the transport theorem in equation (5.1). a B/N = B d VB/N + ω dt B/N V B/N (5.1) Now, for the purposes of this document, it is simpler to write everything in body frame coordinates like in equation (5.2). B [a B/N ] = dtb B B [VB/N ] + [ω B/N ] [V B/N ] (5.2) B In body frame components, the inertial acceleration of the aircraft [a B/N ] can be related to the accelerometer measurements y ax, y ay, y az through the use of the gravity vector g. This is because the accelerometers measure acceleration with respect to the N frame plus the acceleration due to gravity. Because the N frame is fixed to a point on the Earth s surface, any point that is not changing position relative to the N frame experiences the same gravity. Thus, the acceleration due to gravity must be taken out of the accelerometer readings to obtain the acceleration of the aircraft relative to the N frame. The gravity vector is defined in inertial N frame components as the acceleration due to gravity g in the downward direction, as written in equation (5.3). 0 N [g] = [ 0] (5.3) g 48

64 The gravity vector can be rewritten in terms of aircraft B frame components through the use of the rotation matrix defined in equation (3.4). So, its relationship to the inertial acceleration as reported by the accelerometers is given in equation (5.4). B [a B/N ] = [ y a x y a y y a z 0 ] + R BN [ 0] (5.4) g Now, the transport theorem of equation (5.2) can be written entirely in body B frame component form, as in equation (5.5). [ y a x y a y y a z 0 u p u ] + R BN [ 0] = d [ v ] + [ q] [ v ] (5.5) dt g w r w Substituting equations (3.3) and (3.4) into (5.5) and solving yields equation (5.6). [ y a x y a y y a z u + qw rv + g sin θ ] = [ v + ru pw g cos θ sin φ ] (5.6) w + pv qu g cos θ cos φ This transport theorem based relationship (5.6) is the basis of the kinematics behind both methods of state estimation Attitude Determination The first method of state estimation that is analyzed in this paper is derived in [4], but is also used in [5]. The method first realizes that by making a few assumptions, the transport theorem relationship (5.6) can be used to solve for the attitude roll and pitch angles given accelerometer measurements and angular rates. These assumptions are: 49

65 1) The inertial velocity of the aircraft along its axes is constant, as described by equation (5.7). (u = v = w = 0) (5.7) This assumption is made simply because there is no means to directly measure these quantities. It should be noted though that even though the body frame components of the inertial velocity vector are assumed constant, that doesn t mean the inertial frame components of the velocity vector are not changing. For instance, in a perfect coordinated turn, the body frame components of the velocity vector do not change, while the inertial frame components do. This assumption will be readdressed later with simulation results. 2) The velocity of the air relative to the vehicle is solely due to the inertial velocity of the aircraft, which in other words means that the wind velocity vector V w is zero. This greatly simplifies the relationship in equation (3.9). Equation (5.8) is this simplified relationship written in body frame coordinates, substituting in equation (3.10) for V a. u [ v ] = V a [ w cos α cos β sin β sin α cos β ] (5.8) This has huge implications when the purpose of this estimator is perform well in wind. The effects of this assumption will be addressed in the simulation results. 3) Although it has already been assumed that there is zero wind, that doesn t mean that the sideslip angle is always zero in non-stalled conditions, even if the airframe is designed to maintain zero sideslip in normal flight. Although it is typically not done, the autopilot could induce a slipping turn where sideslip is used to turn the aircraft. In any case, in significantly varying wind, sideslip will arise. However, this method assumes sideslip to be zero because there is no direct measure for sideslip. This simplifies the above equation (5.8) further to equation (5.9). u cos α [ v ] = V a [ w 0 sin α ] (5.9) 4) The final assumption assumes that angle of attack α is close to the pitch angle. In both steady turns and significant wind, this is not the case. However, just like sideslip, without sophisticated air data 50

66 sensors, there is no direct measure of the angle of attack. Thus, equation (5.9) becomes equation (5.10). The validity of this will again be addressed in the simulation results. u [ v ] = V a [ w cos θ 0 sin θ ] (5.10) With these assumptions, the transport theorem relationship in equation (5.6) is simplified to equation (5.11) [ y a x y a y y a z qv a sin θ + g sin θ ] = [ rv a cos θ pv a sin θ g cos θ sin φ] (5.11) qv a sin θ g cos θ cos φ This gross simplification enables calculating the pitch and roll attitude angles from the accelerometers and angular rates. With the assumptions that were required to obtain this relationship, large errors can be expected in turning flight, accelerating flight, and in significant amounts of wind. This method attempts to combat these expected errors by first applying low pass filters to the gyro inputs. Performing low pass filters on the sensor inputs removes high frequency noise in the sensors and high frequency vehicle dynamics. Without sensing high frequency dynamics, equation (5.11) will not be as affected by high frequency angular rate dynamics. This avoids high frequency measurement error η in EKF 1 that is about to be discussed. However, the effects of such removal of high frequency dynamics on the actual attitude estimates is assessed in the simulation results. The angular rates needed in equation (5.11) are obtained through equation (5.12). p method1 [ q method1 r method1 ] = LPF (y gyrop ) LPF (y gyroq ) [ LPF (y gyror )] (5.12) 51

67 In this state estimation method, these angular rates are considered to be the final estimate, and are also used as inputs to later steps of this method s state estimation process. Second, the accelerometers inputs are used without filtering as in equation (5.13). [ y a x,method1 y a y,method1 y a z,method1 y ax ] = [ y ay ] (5.13) y az It is important to realize that the process of removing bias from both the gyro and accelerometer sensors is expected to be done in a manual calibration procedure outside of this state estimation process. For short duration flights, a manual calibration by leveling the aircraft on the ground can be sufficient, such as is mentioned in [4]. However, for longer flights this requires periodic manual in-flight calibration. This requires another inertial measurement system, such as on-board video to ensure straight and level flight, to perform the calibration procedure. Such a requirement, depending on the application, may not be allowable. Thus, the effects of drifting gyro biases, which is common for cheaper sensors, is also simulated in this document Extended Kalman Filter 1 Reference [4] demonstrates that a using a simple low pass filter on both accelerometer and gyro inputs for use in equation (5.11) is not enough to avoid the errors encountered from the assumptions used to derive equation (5.11). It shows that fast changes in attitude angles are sometimes completely missed, resulting in large errors in the attitude angles that last a few seconds. To overcome this, this method augments these low pass filters with an extended Kalman Filter (EKF). Reference [4] is not very clear on whether or not one should remove the low pass filters for both the gyros and accelerometer sensors prior for use by the Kalman filter, but it is chosen for this demonstration to keep the low pass filters on the gyro sensors and use the accelerometer sensors as is, since as is about to be discussed, the gyro 52

68 sensors are used as inputs to EKF1, and the accelerometers are the measurements. Note however though the cutoff frequency of the low pass filter was set such that optimal performance was obtained, which was about 10 Hz. Tweaking or removing this method of filter could affect the high frequency dynamics of the results, and further research could investigate this area. This EKF uses the angular rates from equation (5.12) and the airspeed from a pre-calibrated pitot-static sensor as the inputs u EKF1, as described in equation (5.14). u EKF1 = [p LPF q LPF r LPF V a,lpf ] T (5.14) While an airspeed sensor is not part of an IMU, cheap pitot-static tube based airspeed sensors are readily available for use on small UAVs, as discussed by [19]. However, before the airspeed sensor s input is treated as truth for this EKF, it is also passed through a low pass filter, as shown in the block diagram of this method in Figure 5.1 and written in equation (5.15). V a,lpf = LPF(y Va ) (5.15) Now, the purpose of this EKF is to estimate the attitude. Thus, the EKF uses the roll and pitch angles as the states it s estimating, as depicted by equation (5.16). x EKF1 = [ φ θ ] T (5.16) Note that the yaw angle ψ is not included here, and is found from EKF2, which is discussed shortly. Finally, the EKF uses the low pass filtered accelerometer sensors as the measurements: y EKF1 = [ y ax y ay y az ] LPF T (5.17) 53

69 The low pass filtered angular rates p, q, and r in equation (5.14) cannot be treated as measurements for this EKF because they are needed to define the time derivative of the roll and pitch angle states that are being estimated. This relationship to the state derivatives is written in equation (5.18). This relationship is the F EKF1 matrix that is needed to define part of the EFK state-space relationship in (3.11). Equation (5.18) comes from the definition of Euler angle rates that is provided in equation (3.8). p 1 sin φ tan θ cos φ tan θ LPF F EKF1 (x EKF1, u EKF1 ) = [ 0 cos φ sin φ ] [ q LPF ] (5.18) r LPF Multiply the two matrices in equation (5.18) yields equation (5.19). F EKF1 (x EKF1, u EKF1 ) = [ p LPF + q LPF sin φ tan θ + r LPF cos φ tan θ ] (5.19) q LPF cos φ r LPF sin φ Last, the Jacobian of F EKF1 with respect to the states is needed for the EKF time update via the linearized equation (3.22). Computing the Jacobian yields equation (5.20). F EKF1 (x EKF1,u EKF1 ) = [ q LPF cos φ tan θ sin φ tan θ qlpf sin φ+rlpf cos φ cos 2 θ ] (5.20) x EKF1 q LPF sin φ r LPF cos φ 0 Now, the measurement relationship H EKF1 needed for the EKF in equation (3.12) comes directly from the simplified transport theorem relationship in equation (5.11), but taking care to use EKF input low pass filtered angular rates and airspeed. This relationship is depicted in equation (5.21). q LPF V a,lpf sin θ + g sin θ H EKF1 (x EKF1, u EKF1 ) = [ r LPF V a,lpf cos θ p LPF V a,lpf sin θ g cos θ sin φ] (5.21) q LPF V a,lpf sin θ g cos θ cos φ Now, the Jacobian of the H EKF1 matrix is needed for the measurement update. This is directly computed from equation (5.21) to be equation (5.22). 54

70 0 q LPF V a,lpf cos θ + g cos θ H EKF1 = [ g cos φ cos θ r LPF V a,lpf sin θ p LPF V a,lpf cos θ + g sin φ sin θ] (5.22) x EKF1 g sin φ cos θ (q LPF V a,lpf + g cos φ) sin θ This is how EKF 1 obtains high frequency estimates of the roll and pitch angles. It is a piece of the Method 1 estimation algorithm, as depicted in the flow diagram in Figure Extended Kalman Filter 2 Now, another process is needed to determine the remaining states that are listed in (4.11), which are the altitude h and the ground speed V g. Note that it is also necessary to calculate the course angle χ so that a navigational layer algorithm can track a desired course. In [3], altitude comes from a low pass filter on a barometric pressure sensor to obtain frequently updated altitude information, as depicted by equation (5.23). h = LPF(y hstatic pressure ) (5.23) This removes the necessity for the altitude h, or position p d, to be part of the Kalman filter that is about to be discussed. Now, the determination of inertial velocities and position is fairly involved. This method uses the known attitude to smooth the GPS measurements to determine high frequency estimates for the states in (5.24) using a second EKF that for the purposes of this paper is called EKF 2. x EKF2 = [ p n p e V g χ w n w e ψ ] T (5.24) GPS updates for low cost receivers are typically on the order of 1-2 updates per second, so this method uses EKF 1 information to propagate the inertial velocities and positions between GPS updates. Such an act is necessary because updates on inertial position data are simply not frequent enough for autopilots 55

71 to counteract the high frequency dynamics small UAVs experience. See reference [2] for more information on the high frequency nature of small UAVs. It should be noted, however, that the method here is to smooth the existing GPS data given what data is available between GPS updates. The accelerometers have already been used to determine the attitude, and they are not reused here because they do not give direct mappings to all of the state derivatives, such as p n and p e. Such a relationship would involve numerically integrating accelerometer inputs (see Method 2). Instead, to accomplish this smoothing, EKF 2 uses the attitude output of EKF1 as well as the inputs to EKF1 as its truth inputs, which is depicted in equation (5.25). u EKF2 = [ φ EKF1 θ EKF1 p LPF q LPF r LPF V a,lpf ] T (5.25) EKF 2 uses only the GPS measurements, as depicted by (5.26). y EKF2 = y pn,gps y pe,gps y Vg,GPS y χgps 0 [ 0 ] (5.26) Equation (5.26) assumes that the GPS sensor provided a north position y pn,gps, east position y pe,gps, ground speed y Vg,GPS, and course angle y χgps. This is slightly different form the GPS measurements that are used in method 2, which are discussed later. The last two measurements listed in (5.26) are zero. These corresponds to two pseudo-measurements that this method uses to represent the measured wind in the north and east directions. Assuming that the flight path angle of the aircraft is zero and that the wind has no vertical component, these pseudo-measurements are related to components of the EKF states x and truth inputs u as follows: y 5EKF2 = 0 = V a cos ψ + w n V g cos χ (5.27) 56

72 y 6EKF2 = 0 = V a sin ψ + w e V g sin χ (5.28) This assumption of flat wind and level flight will be addressed in the simulation results. Realizing that the remainder of the GPS estimates match directly up with their state vector counterparts in equation (5.24), the measurement-to-state relationship H EKF2 in equation (3.12) can now be written as equation (5.29). H EKF2 (x EKF2, u EKF2 ) = p n pe V gχ V a cos ψ + w n V g cos χ [ V a sin ψ + w n V g sin χ ] (5.29) Though it is large, the Jacobian that is required for the measurement update in equations (3.15) and (3.16) is fairly straightforward to derive from equation (5.29). This measurement update Jacobian is in equation (5.30). H EKF2 (x EKF2,u EKF2 ) x EKF2 = cos χ V g sin χ 1 0 V a,lpf sin ψ [ 0 0 sin χ V g cos χ 0 1 V a,lpf cos ψ ] (5.30) Now, in order to relate the state derivatives to the states and inputs as is needed for EKF relationship (3.11), some additional assumptions had to be made. 1) An assumption is made that the flight path angle is zero. This yields the relationships (5.31) and (5.32). p n = V g cos χ (5.31) p e = V g cos χ (5.32) 57

73 2) Next, assumptions that wind and airspeed are constant had to be made to yield the following relationships (5.33) and (5.34). Detailed derivation of these are found in [4]. V g = (V a cos ψ+w n )( v a ψ sin ψ)+(v a sin ψ+w e )(V a ψ cos ψ) V g (5.33) sin φ cos φ ψ = q + r cos θ cos θ (5.34) Assumption 2 that was performed as part of the attitude determination process assumed that wind speed was zero, but it did not assume that airspeed was constant. Here, wind must be taken into account in order to for relationship (5.33) to somewhat properly measure ground speed at each time update of EKF2. Since the assumption of zero wind was used to determine the attitude, the estimated yaw rate could be expected to suffer from this, and thus affect the ground speed estimate. Using an assumption of constant airspeed just lets equation (5.33) neglect higher order terms of airspeed, for which there is no information. These assumptions will make themselves apparent and be discussed in the simulation results. Note that equation (5.34) itself does not involve any assumptions about the motion of the aircraft, and is just necessary for the calculation of (5.33). 3) Last, an assumption that the aircraft always experiences coordinated turns (even in wind) was used to derive the relationship (5.35). χ = g V g tan φ cos(χ ψ) (5.35) It should be noted that in the transport theorem simplification (5.7) that was used forekf1, it was assumed that the aircraft was not accelerating. While that assumption is not made here, errors in the roll angle and yaw angle could be expected to make their way through EKF1. The existence of such errors would have an effect on this coordinated turn relationship (5.35) and thus the estimated 58

74 course angle rate. One of the benchmarks is a coordinated turn, and this will be discussed with the results of that simulation. Placing these simplified relationships together yields the F EKF2 matrix that is needed by the EKF relationship (3.11). This relationship is written in equation (5.36). V g cos χ V g sin χ (V a,lpf cos ψ+w n )( V a,lpf ψ sin ψ)+(v a,lpf sin ψ+w e )(V a,lpf ψ cos ψ) F EKF2 (x EKF2, u EKF2 ) = [ V g g tan φ cos(χ ψ) V g 0 0 sin φ q cos θ + r cos φ cos θ ] (5.36) Now, the Jacobian of F EKF2 that is needed for the time update equation (3.22) can be derived from (5.36) to (5.37). F EKF2 (x EKF2,u EKF2 ) x EKF2 = 0 0 cos χ V g sin χ sin χ V g cos χ V g 0 0 χ V g V g 0 ψ V a,lpf sin ψ V g χ χ ψ V a,plf cos ψ [ ] V g 0 0 V g ψ χ ψ (5.37) Note the zeroes in the last three rows of the Jacobian for w n, w e, and ψ. This comes from the assumptions that wind speed is constant and the fact that yaw rate does not depend on any of the states of EKF2. This just means that the time update will not update the covariance matrix for wind speed or course angle. The equations for the partials used above are: 59

75 V g ψ = ψ V a,lpf (w n cos ψ+w e sin ψ) V g (5.38) χ V g = g V g 2 tan φ cos(χ ψ) (5.39) χ χ = g V g tan φ sin(χ ψ) (5.40) χ ψ = g V g tan φ sin(χ ψ) (5.41) This concludes the description for EKF2, and its place in Method 1 s algorithm can be seen in Figure Method 1 Summary Now that EKF 2 is in place, it can be noticed that nothing has actually estimated the inertial velocities u, v, w. While they are not needed for the minimum set of states (4.11), they can still be backed out from the output estimated states x EKF2 if the wind is zero. A zero wind assumption is necessary for this because the inertial velocities are related to the wind vector through (3.9) and (3.10) in body frame B component form as depicted by equations (5.42) and (5.43). B [V a ] = B B [V B/N ] [V w ] (5.42) cos α cos β u w x V a [ sin β ] = [ v ] [ w y ] = [ sin α cos β w w z u v w w n ] R BN [ w e w d ] (5.42) Moreover, in this estimator, angle of attack α and angle of sideslip β are not available. In EKF 1, sideslip was assumed be zero and angle of attack was assumed to be equivalent to the pitch angle. Further, it should be noted that many assumptions were made to come up with the EKF that estimates wind. Simulation of this filter even in zero wind show that the estimates for the wind are unacceptable. This is 60

76 likely due to many sensor and modelling errors manifesting themselves in the wind estimate. Thus, it has been observed through simulation that simply leaving these wind terms out of the equation produces far better estimates of the inertial velocities. Thus, equation (5.42) simplifies to equation (5.43). u cos θ [ v ] = V a [ 0 ] (5.43) w sin θ Again, inertial velocity information is not necessary for the autopilot input states listed in (4.11), but this simplification in (5.3) provides a comparison point between methods 1 and 2. So, in conclusion, method 1 produces high frequency information for the following states, listed in Table 5.1. State Used by Autopilot Noteworthy Comment p n N p e N h Y Directly from low pass filter of pressure sensor. p Y Directly from low pass filter of gyro. q Y Directly from low pass filter of gyro. r Y Directly from low pass filter of gyro. u Y Equal to V a cos θ, assumes zero wind. v Y Always zero w Y Equal to V a sin θ, assumes zero wind. V g N φ Y See section for assumptions. θ Y See section for assumptions. ψ N w e N Pseudo-estimate that contains other errors w d N Pseudo-estimate that contains other errors χ N Table 5.1: Summary of States Captured by Method 1 61

77 Note that some of the states not used in the autopilot system here could be used in a navigation layer controller. However, straight GPS measurements are usually sufficient for navigation layer control. Below in Figure 5.1 is a flow diagram that summarizes this method. Figure 5.1: Method 1 Flow Chart 5.2 Detailed Simulation Results Now that method 1 has been presented in detail, the effectiveness of this method can be assessed simulating the state estimation method. The simulation setup that is used here is the same that was described in section 4.3. To assess the effectiveness of the estimation method, the states that are needed by autopilot control, which are listed in (4.11), are deeply scrutinized. The results shown in this section contain plots of the state estimates that state estimation method 1 produces on the same plots 62

78 as the true states that the simulation produces. Again, the simulations here use a linearized model of the RECUV Tempest small UAV. Nonlinear effects of wind on the dynamics of the aircraft are not taken into account. The fact that these simulations do not involve a nonlinear aerodynamic model is discussed in section 0 on future studies. Furthermore, the autopilot previously discussed is used to maintain the benchmark maneuvers here. The autopilot is fed the true states that the simulation provides and not the estimated states that method 1 provides. The purpose of this is to assess the dynamics of the state estimation methods individually so that their strengths and weaknesses can be judged without the effects of closed loop control on the state estimation Zero Wind Benchmark 1: Straight and Level, without Gyro Bias Drift All simulations in this document, except the one in this immediate section, were performed with a linear rate gyro bias drift of 1 30 degrees degrees 2 on each gyro axis. This creates a total bias drift of 3 over the second second course of the 90 second simulations. The rate gyro bias will always begin at zero, as it is assumed that the gyro rate biases are calibrated out at the beginning of each simulation. However, the gyro bias is not included here to better illustrate the basic results of a benchmark 1 maneuver in zero wind. The plots below contain results of this simulation. The plots contain both the true value of the states listed in (4.11) and the estimated value of the states listed in (4.11) that method 1 generates. 63

79 Figure 5.2: Method 1 State Estimation for Benchmark 1 in Zero Wind and Zero Gyro Bias Drift This shows that method 1 was able to successfully converge on an estimate of the states in straight and level flight and zero wind. It is important to elaborate in detail on the results here, as this is used as the base results that are compared against in the rest of this section. To do this, Table 5.2 below contains statistics on the error between the estimated values of the each state and the true values of each state. The sign convention that is used for the non-absolute value terms in the state error statistics tables in this document is the estimated value subtracted from the true value. So, if an estimated value of a state is larger than the true value of the state, the error will be negative. Table 5.2 : Statistics of Method 1 State Error for Benchmark 1 in Zero Wind and Zero Gyro Bias μ σ μ abs σ abs min abs max abs 64

80 h (m) V a (m/s) -6.4E φ (deg) θ (deg) p (deg/s) q (deg/s) V g (m/s) y ay (m/s 2 ) The results here show that the mean of the errors are all essentially zero. State estimation method 1 made many assumptions of straight and level flight and zero wind, and this shows that there is in fact no bias in the estimates when flying in straight and level flight with zero wind. Now, although the mean of the errors is zero, the values are not always zero. The standard deviation of the errors and the mean of the absolute values of the errors both reflect how much error there typically is in the estimate for straight and level flight with zero wind. The errors in the ground speed V g, side specific force y ay, pitch angle θ, altitude h, and airspeed V a are very small. However, the errors in roll angle φ, roll rate p, and pitch rate q are not negligible. The roll angle estimate is shown to typically vary by about 2 degrees, and the pitch rate typically vary about 1 deg/s. While such a variance is not ideal, it is to be expected given the noisy nature small UAV sensors. Roll error of 2 degrees is within the tolerance of what a human pilot would fly a small UAV at, and the autopilot sensitivity analysis of section 4.6 show how only steady state errors of 10 degrees begin to seriously affect the autopilot s ability to track the states. Now, the source of this error can be strictly attributed to sensor noise. A simulation was performed with zero sensor noise, and the following estimation performance was achieved. Table 5.3: Statistics of Method 1 State Error for Benchmark 1 in Zero Wind, Zero Gyro Bias, and Zero Sensor Noise μ σ μ abs σ abs min abs max abs h (m)

81 V a (m/s) φ (deg) -4.5E θ (deg) p (deg/s) q (deg/s) -8E V g (m/s) y ay (m/s 2 ) All of the errors are essentially exactly zero. Thus, for method 1 in straight and level flight in zero wind, and without gyro bias drift, the errors are solely attributable to sensor noise and not the system model Benchmark 1: Straight and Level Next, the same simulation as the previous section was performed, but the gyro rate bias drift of 1 30 degrees second 2 on each of the 3 axes was added back into the simulation. Below are the results of this simulation. 66

82 Figure 5.3: Method 1 State Estimation for Benchmark 1 in Zero Wind Table 5.4: Error Statistics of Method 1 State Estimation for Benchmark 1 in Zero Wind μ σ μ abs σ abs min abs max abs h (m) V a (m/s) φ (deg) θ (deg) p (deg/s) q (deg/s) V g (m/s) y ay (m/s 2 )

83 These results show there are a number of differences in the performance of state estimation method 1 when compared with the zero bias drift simulation of the previous section. They are similar to the zero gyro bias drift simulation of the previous section, but with a few exceptions. First, the results show an accumulation of error in the estimated ground speed V g. This makes sense, as the system model s time update equation for ground speed is affected by the input angular rates. The time update equation (5.33) for V g calculates the change in ground speed from airspeed and yaw angle, as depicted in equation (5.33). The time update equation (5.34) for the yaw angle ψ is affected by the angular rates, and as depicted in Figure 5.4, EKF2 does sufficiently correct for yaw angle error over time. Figure 5.4: Estimated Yaw Angle for Benchmark 1 in Zero Wind IT makes sense that this unbounded yaw drift occurs, as EKF2 has no state or other mechanism that accounts for gyro bias. The EFK2 time update equation of (5.36) relates the yaw angle to the direct roll and yaw rate inputs, which are just low pass filtered and do not account for sensor bias. Then, the covariance matrix update equation of (5.37) defines the time rate of change of the yaw angle as zero. It should also be noted that this drift does not occur when gyro bias drift is not present. The yaw angle estimate of the previous section is shown in Figure 5.5. Figure 5.5: Estimated Yaw Angle for Benchmark 1 in Zero Wind and Zero Gyro Bias Drift 68

84 When gyro rate bias was zero, EKF2 was able to estimate the yaw angle within bounds of about 5 degrees. The degradation of the yaw angle when gyro bias drift is present directly feeds into the ground speed time update equation of (5.33). Corrections to this propagated error in the ground speed are performed by the measurement model in equation (5.29), which equates the GPS provided ground speed to the ground speed estimate. However, over time, inaccurate models in the time update cause the covariance matrix to place far less than 100% weight on the GPS measurements. Thus, the jagged behavior of substantial V g errors being partially corrected once a second in Figure 5.3 is observed. The reason the degradation in yaw angle estimate occurs is because the yaw angle is not really observable, and thus the Kalman filter gives very little weight to updating the estimate for ψ based on GPS inputs. Unfortunately, a traditional linear system observability analysis using the state-tomeasurement matrix in equation (5.30) and state derivative relationships in (5.37) does not yield much insight, as such an analysis yields a full rank observability matrix. The problem lies in the fact that the in the state-to-measurement relationship (5.30), the yaw angle is only affected by pseudomeasurements. As discussed in section and is shown in equations (5.27) and (5.28), these pseudo-functions are functions of inputs and states namely the estimated airspeed V a, ground speed V g, course angle χ, the estimated wind velocity components w n, and w e, and even the yaw angle ψ! If EFK2 was setup such that there were not fake measurements, the matrix in (5.30) would not have equations for the last three states - w n, w e, and ψ. Thus, the observability matrix would not be full rank. In conclusion, if an estimate for V g is desired that is not affected by angular rate errors, interpolating GPS measurements would be one better approach. This topic could be an area of future work. Second, the results in Figure 5.3 show a drift in the estimate of the angular rate p and q and the estimate for the bank angle φ. The fact that the angular rate bias drift shows in the attitude estimate is 69

85 because of the time and measurement update equations for EKF1. The attitude propagation equation (5.19), even in straight and level flight where φ = θ = 0, propagates the error in the angular rates p, q, r into both the bank and pitch angle estimates. Equation (5.44) below is equation (5.19) in straight and level flight conditions. F EKF1 (x EKF1, u EKF1 ) = [ p LPF q LPF ] (5.44) This would imply that both the pitch and bank angle estimates are affected by angular rate biases. However, it can be recalled that unlike the time update equations, the measurement update equations of EKF1 take gravity into account. At straight and level flight, the calibrated accelerometer measurements are equal to the opposite of the gravity vector: y accelx 0 [ y accely ] [ 0 ] (5.45) y accelz g Now, if the bank angle φ is assumed to be about 0, the measurement to state relationship becomes: y accelx (qv a,lpf + g) sin θ 0 H EKF1 (x EKF1, u EKF1 ) = [ rv a,lpf cos θ pv a,lpf sin θ] = [ 0 ] = [ y accely ] (5.55) ( qv a,lpf g) cos θ g y accelz The best solution to this system of equations is a pitch angle close to zero. The top equation of the above relationship is only satisfied when θ = 0. If θ=0, the second two relationships are also closely satisfied as long as the centripetal acceleration terms (rv a,lpf and qv a,lpf ) are small, which is indeed the case in straight and level flight. Thus, when EKF1 finds an optimal solution for the pitch angle, it is always very close to zero. Additional simulations that were performed that are not presented here show that if the centripetal acceleration terms are very large, such as with angular rate biases of 50 degrees per second, the pitch angle estimate can become off by 1-5 degrees. 70

86 Conversely, if the pitch angle θ is assumed to be zero, the measurement to state relationship becomes: y accelx 0 0 H EKF1 (x EKF1, u EKF1 ) = [ rv a,lpf g sin φ ] = [ 0 ] = [ y accely ] (5.56) qv a,lpf g cos φ g y accelz The best solution to this system of equations is a non-zero bank angle φ, as solving these equations for φ relates φ to only the angular rates and gravity. By finding a minimum error estimate to these equations, EFK1 continuously corrects the roll angle to this non-zero value that incorporates the incorrect angular rates. That is why the gyro bias drifts appear in the bank angle estimate but not the pitch angle estimate in this simulation. The time update equations add the angular rate errors to the estimate, but the measurement update equations corrects for it, resulting in different behavior for the two different Euler angles. Finally, it is important to note that this behavior is only the case when the estimated pitch and bank angle are close to zero. In other commanded maneuvers, φ and θ can t always assumed to be zero Benchmark 2: Coordinated Turn Next, below in Figure 5.6 are the results for simulating state estimation method 1 in zero wind. Just like the previous section, gyro rate bias drifts of 1 30 degrees second 2 on each axis were included. 71

87 Figure 5.6: Method 1 Results for Benchmark 2 in Zero Wind Table 5.5: Error Statistics of Method 1 State Estimation for Benchmark 2 in Zero Wind μ σ μ abs σ abs min abs max abs h (m) V a (m/s) φ (deg) θ (deg) p (deg/s) q (deg/s) V g (m/s) y ay (m/s 2 )

88 These results are very similar to the benchmark 1 results in Figure 5.3. Even the standard deviations of the error in attitude are very similar. However, it can be noticed that there is now a steady-state constant error in the pitch angle. This is due to the assumption that the aircraft is in straight and level unaccelearated flight. In a coordinated turn, there is an increase in the z-axis component of the measured acceleration. The measurement model assumes that an increase in vertical acceleration is due to a change in the orientation of the gravity vector and thus due to an increase in the pitch angle. In a coordinated turn, the vertical acceleration increases due to the turn, and not a change in the pitch angle. In mathematical terms, equation (5.10) assumes that the body vertical component of inertial velocity is a function of the airspeed and the sine of the pitch angle. In benchmark 2, the body vertical component of the velocity is non-zero while the pitch angle is zero, which breaks this assumption. This behavior is illustrated in Figure 5.7. Figure 5.7: Validity of Method 1 Assumptions in Benchmark 2 and Zero Wind 73

89 Figure 5.7 shows that the assumptions made in method 1 in order to back out attitude directly from the transport theorem equations without a-priori data are correct in this case, with the exception of the body frame inertial velocity component w. The fluctuations seen in the first second of the plot are initial fluctuations in angular rates that the autopilot induces at the beginning of the simulation, and can be ignored for now until these dynamics are later analyzed. The benchmark 1 simulations previously discussed had zero error on all of these assumptions, and more errors in these assumptions will be discussed later. Last, for comparison, Figure 5.8 below contains the visual results of a benchmark 2 simulation in zero wind and zero gyro bias drift. 74

90 Figure 5.8: Method 1 Results for Benchmark 2 in Zero Wind and Zero Gyro Bias Drift As expected, the drift in V g and φ disappears when gyro bias drift is removed, but the steady state error in the pitch angle remains Benchmark 3: Wings Level Ascent Now, the last benchmark maneuver that is simulated is a wings level ascent from zero altitude to the commanded altitude. Figure 5.9 below contains the state tracking results of estimation method 1 during this maneuver in zero wind. To show the effects of rate gyro bias drift, for just this benchmark, the drift rate was accelerated to 3 20 degrees second took for the aircraft to climb to its commanded altitude. 2 so that the gyro measurements drifted by 3 degrees over the time it 75

91 Figure 5.9: Method 1 Benchmark 3 Results in Zero Wind μ σ μ abs σ abs min abs max abs h (m) V a (m/s) φ (deg) θ (deg) p (deg/s) q (deg/s) V g (m/s) y ay (m/s 2 ) Figure 5.10: Error Statistics for Method 1 in Benchmark 3 and Zero Wind These results highlight a major inherent flaw in this state estimation method. In the attitude determination step via the simplified transport theorem, two main assumptions were made. Equation (5.7) presents the assumption that the aircraft was not accelerating along its body axes. This is not the 76

92 case during a climbing maneuver using the total energy controller, as the total energy controller increases airspeed during the first half of the climb, and then decreases it as it nears its commanded total energy. This is in direct violation of the assumption that u = 0. The second assumption of equation (5.10) assumes that the aircraft is close to level flight. By taking pitch to be the same as the angle of attack, it is assuming that the vector of the incoming air V a is parallel to the ground. This is not accurate in the climb scenario because in a climb, the angle of attack is not large, and the incoming air is still almost entirely opposite of the direction of travel (parallel to the body x frame). Equation (5.10) claims that in a climb w is large, where in reality, it is still near zero. Figure 5.11 compares the assumed values of the variables determined in equations (5.7) and (5.10) with their true values during this simulation. Figure 5.11: Method 1Assumption Validity for Benchmark 3 in Zero Wind The effect of the errors introduced by these two assumptions can be seen directly in the results in Figure 5.9. However, the effect of each assumption can be measured separately by artificially providing 77

93 method 1 the true values for u, v, and w. This can be done by replacing the measurement model of equation (5.30) with equation (5.57) below. [ y a x y a y y a z q w r v + g sin θ ] = [ r u p w g cos θ sin φ ] (5.57) p v q u g cos θ cos φ Note that all variables in equation (5.57) are method 1 s estimated values or inputs, and only u, v, and w are the true values. Also take note that this is the same as the full transport theorem equation (5.6), but still retaining the zero body frame acceleration assumption of equation (5.7). The results of a simulation of this modification are shown in Figure Figure 5.12: Method 1 State Estimation with True Inertial Velocity Values 78

94 Figure 5.12 seems to indicate that at least some of the error in the pitch estimate is due to the non-zero u, as pitch angle errors only occur when u is non-zero. This makes sense, as the simplified transport theorem relationship (5.11) must account for excess acceleration in the x direction by adjusting pitch, as roll is not dependent on x axis acceleration. As shown in Figure 5.12, when the acceleration ceases about 3 seconds into the simulation, the estimate for the pitch angle begins to approach its true value. However, when airspeed begins to decrease as the aircraft approaches its target altitude, and before the pitch is decreased, the estimate for pitch becomes too low. Now, it can be noticed that the spike in estimated bank angle that appears 8 seconds into the simulation occurs at the same time as an abrupt period of non-zero pitch rate q. It can also be noticed that the spike does not appear in the true u, v, w simulation in Figure 5.12, and that at 8 seconds in the simulation, there is an error in the estimated u, v, and w. It makes sense that this error goes away when the true inertial velocity components are used to calculate the centripetal acceleration terms, as the only time those terms are non-zero is when the angular rates are non-zero. With excess magnitude on the right hand side of equation (5.11), the change must be accounted for in the estimate of the attitude. Because it is the pitch rate that is now non-zero, the first equation in (5.11) is used to indicate that pitch should be much larger. At the beginning of the simulation, the pitch rate is positive, and according to the first equation in (5.11), the subsequent pitch angle should be smaller. Interestingly, the change in estimated pitch angle due to the pitch rate is the opposite sign of the change in pitch due to inaccurate measurement of u both at both location in the simulation. This makes sense as both a non-zero X axis acceleration and a positive pitch rate are equivalent to an increase in pitch with respect to the gravity vector. When one of those equivalent increases in pitch are taken away, such as in Figure 5.12, the estimated pitch angle will be increased. 79

95 This behavior could be a key area for improving upon this state estimation method by future work. This analysis shows that the state estimates are very sensitive to maneuvers where the change in body frame velocity components are non-zero or there are significant angular rates that cause large centripetal acceleration terms in the transport theorem. This is a large part of the reason why this method performs so poorly in wind, which is about to be discussed Dryden Wind Now that the performance of method 1 has been characterized and the causes of error found, the same simulations will be performed in a Dryden wind field to see how these errors compound and new sources of error appear. For more information on the Dryden wind field that is used in these simulations, see section Benchmark 1: Straight and Level Figure 5.13 below contains the state estimation results of method 1 under straight and level flight conditions in Dryden wind. 80

96 Figure 5.13: Method 1 Results for Benchmark 1 in Dryden Wind Table 5.6: Error Statistics for Method 1 State Estimation for Benchmark 1 in Dryden Wind μ σ μ abs σ abs min abs max abs h (m) V a (m/s) φ (deg) θ (deg) p (deg/s) q (deg/s) V g (m/s) y ay (m/s 2 ) When comparing these results to the zero wind results for benchmark 1 in section , it should be noticed that state estimates become significantly noisier. The mean of the errors for states other than the ones previously shown to be affected by the gyro bias drifts (h, V a, θ, and y ay ) remain close to zero. 81

97 The mean of the error of the gyro bias drift affected states (p, q, V g, and φ) are similar to what was seen in the zero wind simulation for benchmark 1. These statistics show that in this case, the wind is only causing an increase in the standard deviation of the error. To investigate why this occurs, it can first be recalled that the Dryden wind model produces disturbances at a spectrum of frequencies to both the wind speed and angular rate disturbances on the aircraft. The wind speed oscillations create translational aerodynamic forces on the aircraft that in turn affect the accelerations measured by the accelerometers. The rate disturbances directly put large oscillations on the angular rates of the aircraft, which affect both the estimated angular rates and the accelerometers due to the centripetal acceleration terms in the rigid body dynamics equations. As Figure 5.14 shows, these disturbances on the aircraft s motion create errors in assumption (5.7) for zero change in body frame inertial velocity components and in assumption (5.10) for the body frame inertial velocity components being strictly relative to the air velocity. Figure 5.14: Assumption Validity for Benchmark 1 in Dryden Wind 82

98 Figure 5.14 indicates that the equation for w in (5.10) is actually fairly accurate. This makes sense because the wind experienced in the vertical direction is small, as shown in Figure 4.4, and because the aircraft is flying only in small disturbances about level flight. Because the changes in pitch due to the wind is small, and the dynamics of the aircraft point it into the wind, the pitch angle changes closely with the vertical wind component. However, Figure 5.14 indicates that the estimates for u and v are not accurate. This makes sense, because the aircraft is always pointed into the wind with zero sideslip, meaning that the aircraft is flying at an airspeed V a relative to the wind, and not the ground. The error seen on u is due to the aircraft not travelling at V a relative to the ground, and the constant error seen on v is due to the aircraft having a non-zero crab angle. If the aircraft were flying at a zero crab angle (directly into or away from the wind), then v would be close to its true value. Nonetheless, if the average values of the error are taken away from the u and v estimates, a highly variable component would remain. This is simply due to variations of the wind speed and its effect on the ground velocity of the aircraft. Finally, below are the results of using the true values of u, v, and w in method 1, as is depicted in equation (5.57), for benchmark 1 in Dryden wind. Table 5.7: Error Statistics for Benchmark 1 in Dryden Wind with True Inertial Velocity Values μ σ μ abs σ abs min abs max abs h (m) V a (m/s) φ (deg) θ (deg) p (deg/s) q (deg/s) V g (m/s) y ay (m/s 2 ) These results are almost identical to the results achieved with method 1 s original estimates for u, v, and w. The only distinguishable difference is there is actually a small increase in the standard deviation of the error in pitch and bank. This makes sense, because it should be recalled from equation (5.11) that 83

99 the pitch and bank angles are calculated assuming that the angle of the gravity vector and the centripetal acceleration terms are the only pieces of information that affect acceleration. When u, v, and w are not zero, as Figure 5.14 shows, the attitude estimates will suffer regardless of whether the centripetal acceleration terms are correct or not. The values of the centripetal acceleration terms determined by method 1 were smaller in magnitude than the actual estimates. When the true values were added back in, they only added more uncertainty into the relationship, resulting in a larger variance of the attitude estimates. To further back the notion that the acceleration measurement errors are the source of the error, the instantaneous values of acceleration can be directly mapped to individual fluctuations in the estimated pitch and bank angles. Figure 5.15 is a zoomed in version of the original method 1 simulation in Dryden wind. 84

100 Figure 5.15: Assumption (5.7) to Estimated Attitude Zoom Comparison Figure 5.15 highlights the direct correlation between the actual value for v and the bank angle φ. It also highlights the correlation between the combination of magnitude of acceleration in the body Z axis, w, and the magnitude of acceleration in the -X axis, u, with the pitch angle θ. In conclusion, this illustrates that reasoning that assumption (5.7) is the main source of the increased error seen in this windy scenario. As a tie in for method 2, and for the possibility of future work, this analysis shows that methods of finding information on the rate of change of the body frame velocity components would be very beneficial. Outside of this document, simple tweaks like artificially increasing expected accelerometer sensor noise and removing the low pass filter were tried, but neither had much effect on the quality of the estimates. Increasing the expected accelerometer noise only decreased the frequency of the 85

101 random deviations in the attitude errors, and did not modify the magnitude. Removing the low pass filters did not have any noticeable affect because, likely because the low pass filter cutoff frequencies used in this document were already tweaked by trial and error to attitude estimate with the least error. However, it was noticed that if the cutoff frequencies of the low pass filters were set very low, the error oscillations would decrease, but the error magnitude would increase Benchmark 2: Coordinated Turn Figure 5.16 below contains the results of simulating benchmark 2 for a coordinated turn in the Dryden wind field. Figure 5.16: Method 1 Results under Benchmark 2 and Dryden Wind 86

102 Table 5.8: Error Statistics for Method 1 under Benchmark 2 and Dryden Wind μ σ μ abs σ abs min abs max abs h (m) V a (m/s) φ (deg) θ (deg) p (deg/s) q (deg/s) V g (m/s) y ay (m/s 2 ) The first aspect of these results that should be noted is that the estimated bank angle φ and ground speed V g still exhibit the signature of degraded estimates from gyro bias drift. In fact, the mean error that is listed in Table 5.8 for benchmark 2 in wind is almost exactly the same as the mean error observed in benchmark 1 for straight and level flight in wind, which is in Table 5.6. This indicates that a coordinated turn in wind does not create any more steady state error for bank angle. The second thing that should be noticed from these results is the oscillation in the true bank angle. This oscillation in bank angle is due to the autopilot s reliance on the ground speed to determine the course angle rate as depicted in equation (4.4). Because the aircraft is kept at a constant airspeed, the ground speed varies as the aircraft performs its circular maneuver in the wind field. Because the ground speed is different at different locations in the turn, equation (4.4) dictates that in order to maintain a coordinated turn, the commanded bank angle must also different at different locations in the turn. This oscillation in bank angle is not perfectly sinusoidal because the autopilot will cause the aircraft to turn slower when the airspeed is lower. It can also be noted that the oscillation in pitch angle that was observed is due to the autopilot s inability to maintain altitude in a sharp turn where there is varying pitch rate. Studying in more detail and adjusting the autopilot s behavior in such a turn is outside of the scope of this research, and it also gives a variety of pitch angles to assess the performance of the state estimator at. 87

103 That being said, it can be noticed that steady state pitch error that was observed in benchmarks 2 in zero wind is not very detectable in these results. That error is still in these results, as just as in the zero wind condition in a coordinated turn, the acceleration measured opposite of the Z axis is larger than gravity. The measurement equation (5.11) captures this excess acceleration in the pitch angle because even if assumption (5.7) holds, the estimated vertical velocity w is inaccurate. This inaccuracy is depicted in Figure 5.17 below. Figure 5.17: Method 1 Assumption Validity under Benchmark 2 in Dryden Wind In addition to the assumption (5.10) error for w, Figure 5.17 shows how the true values for u and v change as the aircraft turns through the wind field. Just like discussed in the previous section, assumption (5.10) is only valid in the zero wind case. However, as also discussed in the previous section, the effects of these errors are miniscule in comparison to the errors in assumption (5.7). 88

104 The steady state error in pitch isn t very detectable here because of the large standard deviation of the error. The standard deviations of the attitude errors in benchmark 1 in wind is essentially identical to the standard deviations of the attitude here, but benchmark 1 did not have any steady state error in pitch, just like benchmark 1 in zero wind. Next, the magnitude of the estimation error for benchmark 2 in wind are nearly identical to the state estimation error for benchmark 1 in wind. Similarly, the magnitude of the estimation error for benchmark 2 in zero wind was essentially identical to the magnitude of the estimation error for benchmark 1 in zero wind. While the increase in error in wind for both benchmarks has been attributed to assumption (5.7), it could be interesting to see what affect removing the angular rate biases from the wind would have on the estimation performance. Table 5.9 below contains statistics of such a simulation. Table 5.9: Error Statistics for Method 1 in Benchmark 2 in Dryden Wind with No Angular Rate Disturbances μ σ μ abs σ abs min abs max abs h (m) V a (m/s) -2.7E φ (deg) θ (deg) p (deg/s) q (deg/s) V g (m/s) y ay (m/s 2 ) It should be noticed that these results are almost identical to the results for benchmark 2 in Dryden wind with the angular rate disturbances included. This makes sense, as it was previously proven that the accuracy of the centripetal acceleration terms of the transport theorem don t have much relative bearing on the accuracy of the solution when compared to 89

105 While the rate disturbances do affect the true values for u, v, and w in the rigid body dynamics of the aircraft, this leads one to wonder why the changes in those quantities are what they are. Part of those values is the wind, but part of those values is also the autopilot s reaction to the wind and its reaction to follow its commanded course. Unfortunately, it s not really possible to separate the state estimation from the closed loop behavior, as fixing the aircraft s motion to a certain path would violate the reality that the aircraft is perturbed by the wind and would negate any of those effects on the state estimation. However, it can be recognized the more abrupt changes are that the autopilot makes, the more state estimation method 1 would be affected due to the subsequent change in the rate of change of the body components of the inertial velocity vector. f x /m u + qw rv + g sin θ [ f_y /m] = [ v + ru pw g cos θ sin φ ] (5.58) f z /m w + pv qu g cos θ cos φ Equation (5.58) is the equation of motion for a rigid body aircraft. It shows that the rate of change of the body components of the inertial velocity vector, u, v, and w, are functions of the translational force the aircraft is experiencing, the angular rates of the aircraft, and the attitude of the aircraft. All control mechanisms on an aircraft affect these directly, and so the design of an autopilot may exacerbate or mitigate some of the behaviors observed here. However, the design of the autopilot to mitigate these issues is not the scope of this work. The goal of this work is to bring forward these insights so the designers of a closed loop system can be aware of these concerns Benchmark 3: Wings Level Ascent The final simulation of method 1 is for benchmark 3, a wings level ascent, in the same Dryden wind field as the previous simulations. 90

106 Figure 5.18: Method 1 Results for Benchmark 3 in Dryden Wind Table 5.10: Error Statistics for Method 1 State Estimation for Benchmark 3 in Dryden Wind μ σ μ abs σ abs min abs max abs h (m) V a (m/s) -2.7E φ (deg) θ (deg) p (deg/s) q (deg/s) V g (m/s) y ay (m/s 2 ) In simple terms, these results are very similar to the results obtained for benchmark 3 in zero wind; they are just noisier. This state estimation method still exhibits the large pitch angle errors that were found to be associated with non-level flight in zero wind. The mean of the bank angle error in Table 5.10 still 91

107 indicates a small magnitude drift of the bank angle estimate. The cause of this was determined previously to be the measurement update equations of EKF1. However, like the benchmark 3 simulation in zero wind, the estimate for the ground speed does not become too far off over the course of this short simulation. This makes sense, as over the course of this simulation, the yaw angle error only gets to about 45 degrees, and with GPS measurement updates once per second, the covariance matrix still places plenty of weight on the GPS ground speed measurement. Last, the standard deviation of the error of the attitude angles is in family with the standard deviations seen in the other two benchmark maneuvers in wind. Figure 5.19 below shows the error of the two assumptions in method 1. Figure 5.19: Method 1 Assumption Validity for Benchmark 3 in Dryden Wind The error shown in Figure 5.19 for assumption (5.10) matches the error seen in benchmark 3 in zero wind in Figure 5.11, but has some additional error on u and v. As explained earlier, the error on u and v is due to the aircraft pointing into the wind and having a non-zero crab angle. 92