multi rate sensors based inertial navigation for micro

MULTI RATE SENSORS BASED INERTIAL NAVIGATION FOR MICRO AERIAL VEHICLES A Thesis Submitted in partial fulfilment of the requirements for the award of the degree of MASTER OF TECHNOLOGY in Engineering of Flight Vehicles by

Sanketh Ailneni Roll No- AcSIR/MTECH/21917

Submitted to NATIONAL AEROSPACE LABORATORIES

(Council of Scientific and Industrial Research)

BANGALORE August, 2012

CERTIFICATE

I hereby certify that the work being presented in this thesis entitled “Multi rate sensors based inertial navigation for micro aerial vehicles” in partial fulfilment of requirements for the award of the degree of M.Tech. (Engineering of Flight Vehicles) at the National Aerospace Laboratories under the Academy of Scientific and Innovative Research is an authentic record of my own work carried out during a period from August 2011 to August 2012 under the supervision of Dr. Venkatesh K. Madyastha. The material presented in this thesis has not been submitted by me in any other University / Institute for the award of M.Tech.

_________________ Sanketh Ailneni Roll No. AcSIR/MTECH/21917

This is to certify that the above statement made by the candidate is correct to the best of my/our knowledge and belief.

_______________________ Dr. Venkatesh K. Madyastha

____________________ Dr. Girija Gopalrathnam

Scientist, FMCD-NAL

Acting Head of Division, FMCD-NAL

The M.Tech Viva –Voce Examination of (Sanketh Ailneni) has been held on _3rd August 2012 and accepted.

_________________________ Dr. V.Y. Mudkavi Coordinator PGRPE AcSIR, CSIR-NAL

Acknowledgments This work would not have been completed without help and support of many individuals. I would like to thank everyone who has helped me along the way. Particularly Dr. Venkatesh K. Madyastha for providing me an opportunity to conduct my master’s research under him and for his guidance and support over the course of it; it has been an honour to work with him. Special thanks to Dr. Girija Gopalrathnam for giving me opportunity to pursue my work at Flight Mechanics and Control Division (FMCD). I would like to thank my mates at National Aerospace Laboratories (NAL) for all the memorable times. Lastly, my family without whose support none of this would have been possible.

i

ABSTRACT In this thesis, the design of an extended Kalman filter (EKF) is addressed for the purpose of estimating the position, velocity, attitude and heading of a micro aerial vehicle (MAV). While, estimating the attitude and heading of an MAV is typically addressed via an Attitude Heading Reference System (AHRS), estimating the position, velocity along with the attitude and heading of the MAV is conventionally studied as an inertial navigation solution (INS). The design, development and simulation studies of the EKF addressing both these areas are derived in this thesis. The objective is to develop a fully integrated system using Micro Electro Mechanical Systems (MEMS) inertial sensors combined with low-update rate Global Positioning System (GPS) measurements. The approach uses tri-axial accelerometers, triaxial rate gyroscopes & tri-axial magnetometers and GPS measurements, as an external reference, to aid the EKF algorithm. While MEMS based sensors are low cost, low power consuming, low weight devices, they are not without bias errors, some of which vary with time and hence cannot be simply calibrated off-line. Therefore, it becomes critical to at least estimate the time varying bias errors and correct the MEMS sensors with the bias estimates online. The issue of modeling the time varying bias sources in rate gyroscopes as well as accelerometers as a part of the model for the EKF has been addressed in detail in this thesis. An EKF solution for attitude and heading estimation (AHRS) was implemented with GPS measurements aiding the Inertial Measurement Unit (IMU). The estimation was carried out assuming GPS update rates of 1, 4, 5 and 10 Hz while the IMU update rate was assumed to be 40 Hz. GPS outage durations of 20 s and 15 s were considered for a total simulation duration of 500 s. Typically, an EKF has a prediction phase (process model update - IMU) and a correction phase (measurement update - GPS, magnetometer). During the outage, two ways of implementing the EKF correction phase was proposed and simulations were carried out for attitude and heading estimation using these correction techniques. In an AHRS problem, it is typical to model the rate gyroscope bias states as a part of the EKF state vector while not including the accelerometer bias states as a part of the EKF state vector. This thesis has shown, theoretically, the reason for not including accelerometer bias states in the model of the EKF. An EKF solution for inertial navigation was implemented in four ways namely: • Nine state EKF (3 position states, 3 velocity states, 2 attitude states and 1 heading state. Accelerometer and rate gyroscope bias was not considered in the model.) • Twelve state EKF (3 position states, 3 velocity states, 2 attitude states, 1 heading state and 3 rate gyroscope bias states. Accelerometer bias states not considered in the model.) ii

• Fifteen state EKF (3 position states, 3 velocity states, 2 attitude states, 1 heading state, 3 rate gyroscope bias states and 3 accelerometer bias states.) • Split architecture (six state EKF (2 attitude states and 1 heading state and 3 rate gyroscope bias) + nine state EKF (3 position states, 3 velocity states and 3 accelerometer bias states) implemented). In the above filter designs, the measurement set consisted of 3 GPS position, 3 GPS velocity and heading from a tri-axial magnetometer. Furthermore, the measurements obtained from the IMU (accelerometer and rate gyroscope) are assumed to be corrupted with noise and bias errors. With this measurement set (7 measurements), it is shown that a conventional 15 state EKF is not observable (rank of the observability matrix < 15) thus not guaranteeing estimation performance of all the 15 states. However, with the split architecture, the observability of each individual model is guaranteed thereby ensuring that all the 15 states are observable via this architecture (rank of observability matrix for six state filter is 6; rank of observability matrix for nine state filter is 9). Thus, it was shown that not only does the developed split architecture (EKF-EKF) overcome the problem of lack of observability compared to the 15 state EKF but also better estimates the MAV position, velocity, attitude and heading. EKF performance was evaluated by comparing the state estimates against the simulated truth data. Simulation studies of the AHRS as well as the INS problems involved generating 1000 Monte Carlo simulations, each of 500 s, with and without GPS outages. For the AHRS problem, the impact of GPS update rates of 1, 4, 5 and 10 Hz is studied while for the INS problem the impact of GPS update rates of 4, 5 and 10 Hz is studied and the respective root mean square error plots are shown for all the states modeled in the respective filter. For both sets of problems, single run sample plots are also shown for all the states modeled in the respective filter.

iii

Contents Acknowledgments

i

Contents

i

List of Tables

v

List of Figures

vi

Chapter 1: Introduction 1.1 Inertial Navigation System . . . . . . . . . . 1.1.1 Historical Development of INS . . . . 1.2 Strapdown INS . . . . . . . . . . . . . . . . 1.2.1 INS errors . . . . . . . . . . . . . . . 1.3 Attitude Heading Reference System (AHRS) 1.4 Micro Aerial Vehicles . . . . . . . . . . . . . 1.4.1 Sensors for Micro air vehicles . . . . 1.5 The Structure of this Thesis . . . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

1 1 1 2 3 3 4 4 7

Chapter 2: Mathematical Preliminaries 2.1 Useful Definitions . . . . . . . . . . . . 2.2 Taylor Series Expansion . . . . . . . . 2.2.1 Trace Identities . . . . . . . . . 2.2.2 Trace Derivatives . . . . . . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

8 8 10 11 12

Chapter 3: Random Variables and Processes 3.1 Basic definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2 Various random processes . . . . . . . . . . . . . . . . . . . . . . . . . . . .

16 16 21

Chapter 4: Extended Kalman Filter 4.1 Nonlinear System: Discrete Process Dynamics & Discrete Measurement . . 4.1.1 EKF Design: Notations & Assumptions . . . . . . . . . . . . . . . . 4.1.2 EKF Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1.3 EKF Correction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Nonlinear System: Continuous Process Dynamics & Discrete Measurement

. . . . .

23 24 25 26 27 30

GPS Based Attitude and Heading Estimation for a Micro Aerial Vehicle: Model Formulation

32

Chapter 5:

i

. . . .

. . . .

. . . .

5.1

5.2

5.3 5.4 5.5

Strapdown Kinematic Model: Plant . . . . . . . . . . . . . . . . . . . . . . . 5.1.1 Attitude Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1.2 Rate Gyroscope Model . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1.3 Accelerometer Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1.4 AHRS Process Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1.5 AHRS Measurement Model . . . . . . . . . . . . . . . . . . . . . . . EKF Based AHRS Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2.1 Model & EKF State Vector, Nonlinear Dynamics Map and Jacobian: ˆ, f , F . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . x, x 5.2.2 Process Noise Vector and Continuous Process Noise Scaling Matrix: w, Γc . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2.3 Process & Measurement Noise Intensity Matrices: Q, Rk . . . . . . . 5.2.4 Initial State Error Covariance Matrix: P (0) . . . . . . . . . . . . . . ˆ − , hk , Hk 5.2.5 Predicted & Actual Measurement and Measurement Matrix: h k EKF Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3.1 Alternatives Ways of Obtaining the Pitch and Roll Measurements . . Consolidated Noise Table . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4.1 EKF Pseudo Code with Sensors of Varying Rates: IMU, GPS, MAG Attitude and Heading Estimation for a Micro Aerial Vehicle: Model Formulation Using a Differential Pressure Sensor and No GPS . . . . . . . . . . . . 5.5.1 Differential Pressure Sensor Model . . . . . . . . . . . . . . . . . . . 5.5.2 Attitude Measurements: Pitch (θm ) & Roll (ϕm ) . . . . . . . . . . . .

42 43 43 43 44 44 45 46 47 49 50 50

Chapter 6: 6.1 6.2

6.3

6.4 6.5

6.6

GPS Based Attitude and Heading Estimation for a Micro Aerial Vehicle: Simulation Results Simulated MAV Trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . Simulation Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2.1 Initial Conditions: Model & Filter . . . . . . . . . . . . . . . . . . . . 6.2.2 Sensor Noise Parameters . . . . . . . . . . . . . . . . . . . . . . . . . 6.2.3 EKF Parameters: Q, Rk , P (0) . . . . . . . . . . . . . . . . . . . . . . Simulation Scenario: Modeled Sensor Bias - NO GPS Outage . . . . . . . . . 6.3.1 Case 1: MAV Attitude & Heading Estimation without Sensor Bias . . 6.3.2 Case 2: MAV Attitude & Heading Estimation with Sensor Bias but not accounting for it in the EKF . . . . . . . . . . . . . . . . . . . . 6.3.3 Case 3: MAV Attitude & Heading Estimation with Sensor Bias and accounting for it in the EKF . . . . . . . . . . . . . . . . . . . . . . . Simulation Scenario: Modeled+Unmodeled Sensor Bias - NO GPS Outage . 6.4.1 Modeled+Unmodeled Sensor Bias (Other Maneuvers - RMSE Plots) . Simulation Scenario: Modeled+Unmodeled Sensor Bias - Intermittent GPS Outage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.5.1 EKF Correction During GPS Outage: Eq. (5.82) . . . . . . . . . . . 6.5.2 EKF Correction During GPS Outage: Eqs. (5.83) ∼ (5.87) . . . . . . Attitude and Heading Estimation for a Micro Aerial Vehicle: Simulation results Using a Differential Pressure Sensor and No GPS . . . . . . . . . . . . 6.6.1 Attitude and Heading Estimation results using DPS : . . . . . . . . .

32 33 33 35 35 36 41

ii

52 52 53 54 54 55 55 60 62 64 68 74 75 76 78 80 81

Chapter 7: 7.1 7.2 7.3 7.4

7.5

GPS Based Inertial Navigation for a Micro Aerial Vehicle: Model Formulation Rate Gyroscope and Accelerometer Model . . . . . . . . . . . . . . . . . . . Model for Inertial Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . EKF Based INS/GPS Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.3.1 Full State EKF Model . . . . . . . . . . . . . . . . . . . . . . . . . . EKF Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.4.1 Nine State Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.4.2 Twelve State Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.4.3 Fifteen State Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.4.4 Split Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ˆ, f , F Model & EKF State Vector, Nonlinear Dynamics Map and Jacobian: x, x 7.5.1 Jacobian F: Split Architecture . . . . . . . . . . . . . . . . . . . . . . 7.5.2 Process Noise Vector and Continuous Process Noise Scaling Matrix: w, Γc . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.5.3 Process & Measurement Noise Intensity Matrices: Q, Rk . . . . . . . ˆ − , hk , Hk 7.5.4 Predicted & Actual Measurement and Measurement Matrix: h k

95 96 96

Chapter 8: 8.1

8.2

8.3 8.4

GPS Based Inertial Navigation for a Micro Aerial Vehicle: Simulation Results Simulated MAV Trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.1.1 Initial Conditions: Model & Filter . . . . . . . . . . . . . . . . . . . . 8.1.2 Sensor Noise Parameters . . . . . . . . . . . . . . . . . . . . . . . . . 8.1.3 EKF Parameters: Q, Rk , P (0) . . . . . . . . . . . . . . . . . . . . . . Simulation Scenario - NO GPS Outage . . . . . . . . . . . . . . . . . . . . . 8.2.1 Case 1: Estimation using Nine State Filter . . . . . . . . . . . . . . . 8.2.2 Case 2: Estimation using Twelve State Filter . . . . . . . . . . . . . 8.2.3 Case 3: Estimation using Fifteen State Filter . . . . . . . . . . . . . 8.2.4 Case 1: Estimation using Split Architecture . . . . . . . . . . . . . . 8.2.5 Case 1: Monte Carlo simulations for Split Architecture - Intermittent GPS outage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Other Maneuvers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Simulation Scenario: Intermittent GPS outage . . . . . . . . . . . . . . . . . 8.4.1 Case 1: Monte Carlo simulations for Split-Architecture - Intermittent GPS outage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

82 82 84 85 86 87 87 87 87 88 88 92

Chapter A: Optimal Kalman Gain A.1 Deriving the Optimal Kalman Gain, Kk

98 98 100 100 101 101 102 105 108 111 114 119 121 121

128 . . . . . . . . . . . . . . . . . . . . 128

Chapter B: Derivation 130 B.1 Deriving Eqs. (4.32) and (4.33) for the System in Section 4.2 . . . . . . . . . 130 Chapter C: Basics 132 C.1 MAV Heading, Track, Course . . . . . . . . . . . . . . . . . . . . . . . . . . 132 C.2 Earth North-South Pole: True, Magnetic . . . . . . . . . . . . . . . . . . . . 133 iii

Chapter D: Frames of Reference D.1 Frames of Reference . . . . . . . . . . . . . . . . . . D.1.1 Inertial Frame: FI . . . . . . . . . . . . . . D.1.2 Earth Fixed Frame: FEF . . . . . . . . . . . D.1.3 Earth Centered Earth Fixed Frame: FECEF D.1.4 Body Fixed Frame: FB . . . . . . . . . . . . D.1.5 Inertial Navigation Unit Frame: FIN U . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

136 136 136 136 137 137 139

Chapter E:

Relation between MSE, bias and variance of an estimator

140

Chapter F:

The Matrix Inversion Lemma

143

Chapter G:

The Exponential Function and its Identities

144

Chapter H: Solution of linear systems 147 H.1 Solution of Linear, Time Invariant Systems . . . . . . . . . . . . . . . . . . . 147 H.2 Solution of Linear, Time Varying Systems . . . . . . . . . . . . . . . . . . . 149 Chapter I: Observability I.1 Observability of Linear, Time Invariant Systems . . . . . . . . . . I.2 Observability of Nonlinear, Time Invariant Systems . . . . . . . . I.3 Nonlinear Observability Analysis: AHRS, X-axis Accelerometer Gyro Bias, 3 Measurements . . . . . . . . . . . . . . . . . . . . .

152 . . . . . . 152 . . . . . . 154 Bias, No . . . . . . 157

Bibliography

160

Vita

163

iv

List of Tables 5.1

Magnetic Variation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

38

5.2

Rate Gyroscope noise & EKF standard deviations.

. . . . . . . . . . . . . .

46

6.1

Sensor noise standard deviations (Std. Dev.). . . . . . . . . . . . . . . . . .

54

8.1

Sensor noise standard deviations (Std. Dev.). . . . . . . . . . . . . . . . . . 100

v

List of Figures 1.1

Strapdown navigation system [1] . . . . . . . . . . . . . . . . . . . . . . . . .

2

1.2

Rate Gyroscopes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

5

1.3

Accelerometers. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

6

3.1

Ensemble of sample realizations of a random signal. . . . . . . . . . . . . . .

19

3.2

Simulating colored noise. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

22

3.3

Simulating wiener process. . . . . . . . . . . . . . . . . . . . . . . . . . . . .

22

4.1

Prediction (Time Update) and Correction (Measurement Update) philosophy

25

4.2

Prediction (Time Update) and Correction (Measurement Update) in an EKF

30

5.1

Roll, Pitch and Yaw angles of an MAV. . . . . . . . . . . . . . . . . . . . . .

32

5.2

Magnetometer Mounted Onboard an MAV. . . . . . . . . . . . . . . . . . . .

37

5.3

Pitot Static tube. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

49

6.1

Simulated MAV Trajectories. . . . . . . . . . . . . . . . . . . . . . . . . . .

53

6.2

Simulating Colored Noise (1st Order Gauss-Markov Model). . . . . . . . . .

54

6.3

Accelerometer & Rate Gyroscope: Truth (Solid); Measurement (Dots). . . .

56

6.4

Rate Gyroscope Bias Plots: Rate Random Walk & Colored Noise. . . . . . .

56

6.5

Rate Gyroscope Bias Model Bode Plots. . . . . . . . . . . . . . . . . . . . .

57

6.6

Rate Gyroscope Noise Plots: Signals & Histogram. . . . . . . . . . . . . . .

57

6.7

Accelerometer Noise Plots: Signals & Histogram. . . . . . . . . . . . . . . .

58

6.8

GPS Velocity Noise Plots: Signals & Histogram. . . . . . . . . . . . . . . . .

58

6.9

AHRS Measurements in degree: Truth (Solid); Measurement (Dots). . . . . .

59

vi

6.10 EKF Performance in Estimating Euler Angles (deg) - MAV Trajectory in Figure 6.1(a). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

60

6.11 EKF Performance in Estimating Euler Angles (deg) - MAV Trajectory in Figure 6.1(b). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

61


62


63


64

6.15 EKF Performance in Estimating Bias, b1p , b1q , b1r , (deg/sec) - MAV Trajectory in Figure 6.1(a). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

65


66

6.17 EKF Performance in Estimating Bias, b1p , b1q , b1r , (deg/sec) - MAV Trajectory in Figure 6.1(b). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

67


68

6.19 EKF Performance in Estimating Bias, b1p , b1q , b1r , (deg/sec) - MAV Trajectory in Figure 6.1(a). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

69

6.20 Euler Angles RMSE (deg) - MAV Trajectory in Figure 6.1(a). . . . . . . . .

70


71

6.22 EKF Performance in Estimating Bias, b1p , b1q , b1r , (deg/sec) - MAV Trajectory in Figure 6.1(b). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

72

6.23 Euler Angles RMSE (deg) - MAV Trajectory in Figure 6.1(b). . . . . . . . .

73

6.24 Simulated MAV Trajectories. . . . . . . . . . . . . . . . . . . . . . . . . . .

74

vii

6.25 Showing the IMU and GPS Update Rates Along with GPS Outage Periods of 250 ∼ 270 secs and 390 ∼ 405 secs. . . . . . . . . . . . . . . . . . . . . . .

75


76


77


78


79

6.30 Simulated MAV Trajectories. . . . . . . . . . . . . . . . . . . . . . . . . . .

80

6.31 Euler angle estimates using DPS in the measurement suit. . . . . . . . . . .

81

8.1

Simulated MAV Trajectories. . . . . . . . . . . . . . . . . . . . . . . . . . .

99

8.2

Nine State EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(a). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102

8.3


8.4

Nine State EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(b). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104

8.5


8.6

Observability index Twelve State Filter . . . . . . . . . . . . . . . . . . . . . 106

8.7

Twelve State EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(a). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107

8.8

Twelve State EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(b). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108

8.9

Fifteen state EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(a). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109

8.10 Fifteen state EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(b). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110

viii

8.11 Fifteen state EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(b). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111 8.12 Split Architecture EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(a). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112 8.13 Split Architecture EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(b). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113 8.14 EKF Performance in Estimating Position - MAV Trajectory in Figure 8.1(a). 114 8.15 EKF Performance in Estimating Velocities - MAV Trajectory in Figure 8.1(a). 115 8.16 EKF Performance in Estimating Euler angles - MAV Trajectory in Figure 8.1(a). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116 8.17 EKF Performance in Estimating Position - MAV Trajectory in Figure 8.1(b). 117 8.18 EKF Performance in Estimating Velocities - MAV Trajectory in Figure 8.1(b). 118 8.19 EKF Performance in Estimating Euler angles - MAV Trajectory in Figure 8.1(b). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 8.20 Simulated MAV Trajectories. . . . . . . . . . . . . . . . . . . . . . . . . . . 120 8.21 EKF Performance in Estimating Position - MAV Trajectory in Figure 8.20(a). 121 8.22 EKF Performance in Estimating Velocity (m/sec) - MAV Trajectory in Figure 8.20(a). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122 8.23 EKF Performance in Estimating Euler angles - MAV Trajectory in Figure 8.20(a). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123 8.24 EKF Performance in Estimating Position - MAV Trajectory in Figure 8.20(b). 124 8.25 EKF Performance in Estimating Velocity (m/sec) - MAV Trajectory in Figure 8.20(b). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125 8.26 EKF Performance in Estimating Euler angles - MAV Trajectory in Figure 8.20(b). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 C.1 MAV Heading, Track and Course. . . . . . . . . . . . . . . . . . . . . . . . . 134 C.2 Showing True North and Magnetic North. . . . . . . . . . . . . . . . . . . . 135 ix

D.1 Earth Fixed Reference Frame, FEF . . . . . . . . . . . . . . . . . . . . . . . . 137 D.2 Earth Centered Earth Fixed Reference Frame, FECEF . . . . . . . . . . . . . 138 D.3 Body Fixed Reference Frame, FB . . . . . . . . . . . . . . . . . . . . . . . . . 138 D.4 Inertial Navigation Unit Reference Frame, FIN U . . . . . . . . . . . . . . . . . 139 E.1 Accuracy and Precision . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141

x

Chapter 1 Introduction Navigation is a skill or an art which has become a complex science. No matter how far we go back in time it has always been critical to know where one is positioned. It is essentially about travelling and finding the way from one place to another. There are a variety of means by which this can be achieved. Perhaps one of the simplest forms of navigation is following the directions and instructions. An extension of this process is navigation by following a map. As an alternative method the navigator may chose to observe other objects or naturally occurring phenomena to determine his or her position. An alternative approach is to use the phenomenon of dead reckoning by which one’s present position may be calculated from the knowledge of initial position and measurements of speed and direction. An equivalent process may be conducted using inertial sensors such as accelerometers, gyroscopes and magnetometers to sense translational and rotational motion with respect to an inertial reference frame. This is known as inertial navigation [1]. 1.1

Inertial Navigation System

An inertial navigation system (INS) is a navigation system which depends entirely on inertial measurements for navigation. An INS consists of accelerometers which measure the translatory acceleration, rate gyroscopes which measure the angular rotation and magnetometers which measure the direction of the magnetic field. This sensor array is called an inertial measurement unit (IMU). Using the measurements from the IMU, the INS can calculate the current attitude, velocity and position of the system starting from some known initial point. This means that INS does not depend on any third party applications, like GPS, and thus will always work regardless of external influences. 1.1.1

Historical Development of INS

The first applications using inertial measurements for navigation purposes appeared at the end of the 19th century. They were simple gyro compasses which were able to determine the direction of true north. Under WW2 the development of the gimballed INS was refined, and the Germans used a gimballed INS to navigate their V2 rockets. In gimballed INS, the IMU is isolated from vehicle rotations by the use of a gimbal. Further development of the gyros lead to even more precise INS during the 1950s. Until the 1970s only the gimbaled systems had been investigated but in the late 1970s the development of the strapdown INS 1

(SINS) began. In a SINS, the sensors are rigidly mounted to the body of the vehicle, hence the name strapdown. The development of the SINS is primarily due to the introduction of the Ring Laser Gyro (RLG) in the 1960s and the Fibre Optic Gyro (FOG) in the 1970s. These gyros eventually enabled strapdown INS to obtain a degree of accuracy comparable to low-end gimballed systems but with a lower price tag. This made INS solutions applicable to military aircraft and the first commercial aircraft Boeing 757. The advantages of a nonmechanic system with low price and low weight was the source of this development. The lack of computer processing power postponed the introduction of SINS system until the 1980s. The gimbaled system still achieved better precision but the SINS had an precision which made it applicable in lower-cost applications. 1.2

Strapdown INS

The strapdown implementation offers numerous advantages over its gimballed counterpart, and has made it the preferred type of inertial navigation systems today. By disposing of bulky, expensive and error-prone moving mechanical parts, the reliability of the system is increased and the cost and size is reduced. The problem with strapdown inertial navigation systems is, that the sensor assembly is not mechanically isolated from the rotations of the body, and thus, the gyros must be able to capture these rotations. This requires the strapdown gyros to have a much higher dynamic range than the gyros employed in gimballed systems. A typical block diagram for Strapdown INS is given below:

Figure 1.1: Strapdown navigation system [1] The diagram displays the main functions to be implemented within a strapdown navigation system such as [1]: 2

1. The processing of the rate measurements to generate body attitude (AHRS). 2. The resolution of the specific force measurements into the inertial reference frame. 3. Gravity correction and the integration of the resulting acceleration estimates to determine velocity and position (Navigation solution). 1.2.1

INS errors

INS suffers from integration drift since this form of navigation uses integration to get the orientation and position of the vehicle. Thus, any error, no matter how small, in the accelerometer and gyroscope readings, when integrated result in larger errors in velocity and orientation. When velocity is further integrated to get position, greater errors are seen. Since the new position is calculated from the previous calculated position, the measured acceleration and the measured angular velocity, these errors are cumulative and increase at a rate proportional to the time since the initial position was taken. Therefore the position must be frequently corrected by a different form of navigation system. The inaccuracy of a goodquality navigational system is normally less than 0.6 nautical miles per hour (.3645f t/hr) in position and about 0.1deg/hr in orientation [2]. 1.3

Attitude Heading Reference System (AHRS)

An Attitude Heading Reference System (AHRS) consists of sensors on three axes that provide heading and attitude information for an aircraft. A typical AHRS consist of either solidstate or MEMS gyroscopes, accelerometers and magnetometers on all the three axes. The attitude information generated via an AHRS is used in many navigation guidance and control applications such as pilot-in-the-loop control of manned aircraft [3]. Some differences between an AHRS and an IMU are [2]: 1. AHRS systems measure only attitude (roll, pitch and yaw/heading) contrary to IMU systems that measure attitude, position and velocity. 2. AHRS requires magnetometer, gyroscope and accelerometer while an INS system would require only accelerometer and gyroscope. 3. AHRS estimates attitude using Kalman filtering, while IMU estimates attitude, position and velocity via dead reckoning. 4. AHRS errors will not necessarily build up in time, while IMU errors will necessarily accrue in time because IMU simply integrates acceleration to get velocity and further integrates velocity or rate to get position or attitude given initial estimate of position or attitude. Thus initial errors in position or attitude will build up via integration. 5. AHRS directly solves for the attitude, while IMU gives the position and orientation estimate along with speed and angular rate to a dead reckoning system to solve for the states.

3

1.4

Micro Aerial Vehicles

The concept of micro-sized unmanned aerial vehicles (UAVs) or micro aerial vehicles (MAVs) has gained increasing interest over the past few years, with the principal aim of carrying out surveillance missions. The primary payload of these tiny aircraft ( 15 centimeters or 6 inches wingspan) is usually a miniature image sensor. Operating in an approximate radius of 600 meters from the launch point, MAVs are used to acquire real-time visual information for a wide range of applications. According to DARPA (Defense Advanced Research project Agency) [4], MAVs are “six-degree-of-freedom aerial robots, whose mobility can deploy a useful micro payload to a remote or otherwise hazardous location where it may perform any of a variety of missions, including reconnaissance and surveillance, targeting, tagging and bio-chemical sensing”. The small size of an MAV creates several unique stability and control challenges. Since the mass moment of inertia scales as the fifth power of the characteristic dimension [5], small vehicles tend to have high natural frequencies of rotational oscillation. Obtaining a stable video image requires an actively stabilized camera mount or an actively stabilized aircraft. Therefore high oscillation frequencies require a control system with fast processors and fast actuators to stabilize the camera or the entire MAV. Since wing loading decreases with decreasing size, small air vehicles are quite susceptible to gusts. Even small birds (with highly evolved active control systems) have trouble maintaining steady flight in extremely turbulent conditions.One of the objectives for the MAV is to achieve autonomous flight so that the vehicle can be easily operated by an unskilled operator. The first step toward autonomous flight is to sense the state of the vehicle and pass the data to the flight computer. For this reason, an MAV has a two-axis magnetometer to sense compass heading, accelerometers to sense specific forces and a piezoelectric gyro to sense the turn rate. The vehicle also uses GPS to measure ground speed of the vehicle. The small size of an MAV demands for small size sensors with less weight which calls for the usage of Micro electro mechanical systems (MEMS). 1.4.1

Sensors for Micro air vehicles

Sensors are essential components of automotive electronic control systems. Sensors are defined as [6] “devices that transform (or transduce) physical quantities such as pressure or acceleration (called measurands) into output signals (usually electrical) that serve as inputs for control system”. Important automotive sensor technology developments are micro machining and micro electro mechanical systems (MEMS). MEMS manufacturing of automotive sensors began in 1981 with pressure sensors for engine control, continued in the early 1990s with accelerometers to detect crash events for air bag safety systems and in recent years has further developed with angular-rate inertial sensors for vehicle-stability chassis systems. MEMS makes high-performance sensors available for automotive applications, at the same cost as the traditional types of limited-function sensors. The MEMS technology allows the realization of new miniaturized low cost sensors. Over the last few years many MEMS systems have been developed with growing performances, very small size and light weight. These sensors, matched with miniaturized GPS receivers, are the basis of complete low cost and low weight navigation systems. 4

The results obtained by such sensors are generally less accurate than the ones typically associated with tactical-grade inertial sensors. However, by means of high quality integration algorithms, it is possible to obtain significant positioning accuracy improvements with respect to GPS systems, when there is limited satellite availability. The IMU measurements are independent of the GPS signal outages and the frequencies of acquisition are very high. The IMU measures the angular velocities and linear accelerations. The matrix rotation is obtained using the angular rates by gyroscopes and the measured forces are rotated in the required frame using the rotation matrix. The velocities and positions of the body are obtained by integration of the accelerations and angular rates. Unfortunately the integration process is very sensible to the systematic errors of the IMU. The acceleration bias introduces an error in the velocity proportional to the time t, and an error in the position proportional to t2 . Still worse the gyro bias introduces a quadratic error in the velocity and a cubic error in the position. The position and velocity drift depends on bias, scale factor, misalignment and sensor noise. Rate Gyroscope Schematic view of various rate gyroscopes are shown in Figure 1.2

(a) Tuning fork gyroscope

(b) Vibrating wheel gyroscope

(c) Ring laser gyroscope

Figure 1.2: Rate Gyroscopes 5

A MEMS based rate gyroscope sensor is usually modeled as: ωm = ω + ωb + Sω + N ω + vω

(1.1)

where ωm is the measured angular rate, ω is the true angular rate, ωb is the gyroscope instrument bias, S is the linear scale factor matrix, N is the non-orthogonality(Misalignment) matrix and vω is the sensor noise. Accelerometer Schematic view an accelerometer is shown in Figure 1.3

(a) Basic accelerometer

(b) Schematic view of accelerometer

Figure 1.3: Accelerometers. In the case of the accelerometers generally the following equation is used: fm = f + fb + S1 f + S2 f 2 + N f + δg + vf

(1.2)

where fm is the measured acceleration, f is the true specific force, bf is the accelerometer instrument bias, S1 and S2 are the linear and non linear scale factor matrices, N is the non-orthogonality(Misalignment) matrix, δg is the deviation from theoretical gravity and vf is the accelerometer noise.

6

1.5

The Structure of this Thesis

This thesis does not have one central literature review. Rather, the literature relevant to a particular topic will be cited at the beginning of each chapter. Chapter 2 presents few of the required mathematical basics in understanding and implementing an Extended Kalman Filter (EKF). Chapter 3 addresses about Random variables and their process. Basic definitions of random process and their mathematical description in probabilistic terms is presented in this chapter. Chapter 4 addresses about EKF algorithm. The EKF equations for nonlinear systems for both discrete and continuous time domain are derived in this chapter. Chapter 5 presents the mathematical model formulation for an attitude and heading estimation problem. Detailed description about process model and measurement model required to implement EKF for AHRS are presented. A pseudo code is presented for implementing EKF with different GPS update rates. Chapter 6 presents few simulation results of Attitude and heading estimation using the proposed EKF algorithms. Root mean square error (RMSE) plots are presented for various MAV maneuvers and EKF performance is evaluated. Chapter 7 addresses about the mathematical model formulation for INS/GPS. Detailed description of process and measurement models are presented an implementation of various filter algorithms are described. Chapter 8 presents few simulation results of INS/GPS. Position, velocity and attitude estimates are presented along with the estimates of sensor bias. RMSE plots are presented for various MAV trajectories and EKF performance is evaluated.

7

Chapter 2 Mathematical Preliminaries In this chapter, some definitions, theorems and facts are presented which are necessary to understand some of the Kalman filter derivations and the estimation problem formulations in the thesis. 2.1

Useful Definitions

[ ]T Definition. Given a vector v = v1 v2 . . . vn ∈ Rn and a scalar valued real function f (·) : v ∈ Rn → R, the partial derivative of f with respect to v is defined as [ ∂f (v) = ∂v

∂f (v) ∂v1

∂f (v) ∂v2

...

∂f (v) ∂vn

]

∈ R1×n

(2.1)

and the partial derivative of f (v) with respect to vT defined as     ∂f (v)  =  ∂vT  ( =

∂f (v) ∂v1 ∂f (v) ∂v2

.. .

∂f (v) ∂vn

∂f (v) ∂v

     )T ∈ Rn

(2.2)

Remark 2.1. The notation ∇f (v) is commonly used to represent ∂f∂v(v) . [ ]T Definition. Given a vector v = v1 v2 . . . vn ∈ Rn and a vector valued real function [ ]T ∈ Rm , the partial derivative of f with respect to v is defined as f = f1 f2 . . . fm   ∂f (v)  =  ∂v 

∂f1 (v) ∂v1 ∂f2 (v) ∂v1

∂f1 (v) ∂v2 ∂f2 (v) ∂v2

...

...

∂fm (v) ∂v1

∂fm (v) ∂v2

8

... ... ... ...

∂f1 (v) ∂vn ∂f2 (v) ∂vn

... ∂fm (v) ∂vn

    ∈ Rm×n 

(2.3)

and the partial derivative of f with respect to vT is defined as  ∂f (v) ∂f (v)  ∂fm (v) 1 2 . . . ∂v1 ∂v2 ∂vn ( )T  ∂f1 (v) ∂f2 (v) ∂fm (v)  ∂f (v) ∂f (v) . . .  ∂v1  ∂v ∂v n 2 =  ∈ Rm×n = ∂vT ∂v  ... ... ... ...  ∂f1 (v) ∂f2 (v) m (v) . . . ∂f∂v ∂v1 ∂v2 n

(2.4)

[ ]T [ ]T Definition. Given a matrix V = V1 V2 . . . Vm ∈ Rn×m , Vi = v1i v2i . . . vni ∈ Rn and a scalar valued real function f (·) : V ∈ Rn×m → R, the partial derivative of f with respect to V is defined as  ∂f (V )   ∂f (V ) ∂f (V )  ∂f (V ) . . . ∂v ∂v ∂vn1  ∂f∂V(V1 )   ∂f (V11 ) ∂f (V21 ) ∂f (V )  ∂f (V ) . . .  ∂V2   ∂v12 ∂v22 ∂vn2  (2.5) =  =  ∈ Rm×n ∂V  ...   ... ... ... ...  ∂f (V ) ∂f (V ) (V ) ∂f (V ) . . . ∂f ∂v1m ∂v2m ∂vnm ∂Vm Corollary 2.2. From Definition 2.1, we obtain

∂f (V ) ∂V T

( =

∂f (V ) ∂V

)T

∈ Rm×n .

Proof. : From Definition 2.1, rewrite V as 

V

=

[

V1 V2 . . . Vm

v11 ]  v21 =  ... vn1

v12 v22 ... vn2

 . . . v1m . . . v2m   ... ...  . . . vnm

(2.6)

Taking the transpose of Eq. (2.6), yields 

VT =

=

[ [ |

V1T U1

 v11 v21 . . . vn1 ]T  v12 v22 . . . vn2   V2T . . . VmT =  ... ... ... ...  v1m v2m . . . vnm [ ]T ] U2 . . . Um , Ui = vi1 vi2 . . . vim ∈ Rm {z }

(2.7)

U

Thus, taking the partial derivative of f (V ) with respect to V T is equivalent to taking the partial derivative of f (V ) with respect to U . This yields  ∂f (V )   ∂f (V ) ∂f (V )  (V ) . . . ∂f ∂v11 ∂v12 ∂v1m ∂U1 ( )T  ∂f (V ) ∂f (V ) ∂f (V )  ∂f (V )  ∂f (V ) ∂f (V )  ∂f (V ) . . .  ∂U2   ∂v21  ∂v22 ∂v2m  = = = ∈ Rm×n (2.8) = ∂V T ∂U ∂V  ...   ... ... ... ...  (V ) ∂f (V ) ∂f (V ) ∂f (V ) . . . ∂f ∂vn1 ∂vn2 ∂vnm ∂Um This completes the proof.

9

2.2

Taylor Series Expansion

Theorem 2.3 presents the Taylor series expansion for a scalar valued function of 1 variable. Theorem 2.3. [7] Let k ≥ 1 be an integer and let the function f : R → R be k times differentiable at the point a ∈ R. Then there exists a function hk : R → R such that: ∂f 1 ∂ 2 f 1 ∂ k f 2 f (x) = f (a) + (x − a) + (x − a) + . . . + (x − a)k 2 k ∂x x=a 2! ∂x x=a k! ∂x x=a +hk (x) (x − a)k

(2.9)

where, limx→a hk (x) = 0. Theorem 2.4 presents the Taylor series expansion for a scalar valued function of multiple variables. Theorem 2.4. [7] Let k ≥ 1 be an integer and let the function f : Rn → R be k times differentiable at the vector a ∈ Rn . Then there exists a function H2 : Rn → Rn×n such that: f (x) = f (a) + ∇f (x)|x=a (x − a) +

1 (x − a)T ∇2 f (x) x=a (x − a) 2!

+(x − a)T H2 (x)(x − a)

(2.10)

where, limx→a H2 (x) = 0 and ∇2 f (x) = ∇ (∇f (x)). Remark 2.5. The Taylor series expansion for a vector valued function of a single variable   f1 (x)  f2 (x)    f (x) =  (2.11)  , f (·) : R → Rm ..   . fm (x) can be obtained by expanding each individual entry of the vector f via Theorem 2.3. Remark 2.6. The Taylor series expansion for a vector valued function of multiple variables     f1 (x) x1  f2 (x)   x2      (2.12) f (x) =   , x =  ..  , f (·) : Rn → Rm ..   .   . xn fm (x) can be obtained by expanding each individual entry of the vector f via Theorem 2.4. The following identities are useful for the computation of the derivative of the trace of a matrix with respect to a vector or another matrix. This will be especially helpful while deriving an expression for the Kalman gain matrix.

10

2.2.1

Trace Identities

This sub-section provides necessary identities to further the understanding on trace of a matrix or the trace of a product of matrices. Fact 2.7. T r(AB) = T r(BA), where A ∈ Rn×m , B ∈ Rm×n . Proof. : The matrices A and B can be expressed as [ ] [ ] A1 A2 . . . Am , Ai = a1i a2i . . . ani , Ai ∈ Rn×1 A = [ T ]T [ ]T T B1 B2T . . . Bm B = , BiT = b1i b2i . . . bni , Bi ∈ R1×n

(2.13)

Thus multiplying the matrices A and B yields [

] [

T B1T B2T . . . Bm m ∑ = A1 B1 + A2 B2 + . . . + Am Bm = Ai Bi

AB =

A1 A2 . . . Am

]T (2.14)

i=1

Taking the trace of Eq. (2.14) results in T r (AB) = T r (A1 B1 + A2 B2 + . . . + Am Bm ) ( m ) m m ∑ n ∑ ∑ ∑ = Tr Ai Bi = T r (Ai Bi ) = aji bij i=1

i=1

i=1 j=1

| {z } T r(Ai Bi )

=

m n ∑ ∑ i=1

bij aji

=

j=1

m ∑

( T r (Bi Ai ) = T r

i=1

| {z }

m ∑

)

Bi Ai

i=1

Bi Ai =T r(Bi Ai )

= T r (BA)

(2.15)

This proves Fact 2.7. Fact 2.8. T r(AB) = T r(AT B T ), where A ∈ Rn×m , B ∈ Rm×n . Proof. : From Eq. (2.15), we have T r (AB) =

m ∑

T r (Ai Bi ) =

i=1

m ∑ n ∑

aji bij

(2.16)

i=1 j=1

Also notice that m m m ∑ n ∑ ∑ ( ) ( ) ∑ T r AT B T = T r ATi BiT = ATi BiT = aji bij i=1

i=1

i=1 j=1

Comparing Eq. (2.16) and Eq. (2.17) completes the proof of Fact 2.8. 11

(2.17)

( ) Fact 2.9. T r (AB) = T r B T AT , where A ∈ Rn×m , B ∈ Rm×n . ) ( ) ( Proof. : From Fact 2.8 T r (AB) = T r AT B T . Thus T r B T AT = T r (BA). Furthermore from ( Fact )2.7 T r (AB) = T r (BA). Thus combining Fact 2.7 and Fact 2.8, we obtain T r B T AT = T r (AB). This completes the proof of Fact 2.9. ( ) T Fact 2.10. T r (AB) = T r (AB) , where A ∈ Rn×m , B ∈ Rm×n . Proof. : Follows trivially from Fact 2.9 since B T AT = (AB)T . In summary, Facts 2.7 through 2.10 simply state that the trace operator is commutative and ( ) ( ) invariant to matrix transpose. Thus T r (AB) = T r (BA) = T r AT B T = T r B T AT . 2.2.2

Trace Derivatives

This sub-section provides necessary trace derivative identities to help understand the trace derivative identities used while arriving at the optimal Kalman gain matrix. Fact 2.11.

∂ {T r (AB)} ∂B

= AT , where A ∈ Rn×m , B ∈ Rm×n and T r (AB) is a scalar.

Proof. : Notice from Definition 2.1, that [ ∂ {T r (AB)} = ∂B

∂ {T r (AB)} ∂B1

∂ {T r (AB)} ∂B2

...

∂ {T r (AB)} ∂Bm

]T

(2.18)

Notice that from Fact 2.8 we have T r (AB) = =

m ∑ n ∑

aji bij

i=1 j=1 m ∑

(a1i bi1 + . . . + ani bin )

i=1

= =

m ∑ ( i=1 m ∑

bi1 bi2 . . . bin

)(

a1i a2i . . . ani

Bi Ai

)T

(2.19)

i=1

Furthermore Eq. (2.19) further reduces to T r (AB) = B1 A1 + B2 A2 + . . . + Bm Am

12

(2.20)

Taking the partial derivative of Eq. (2.20) with respect to B and using Eq. (2.18) further yields [ ∂ ∂ ∂ {T r (AB)} ∂B {T r (AB)} . . . {T r (AB)} = ∂B1 2 ∂B [ ]T A1 A2 . . . Am = ∂ ⇒ {T r (AB)} = AT ∂B

∂ {T r (AB)} ∂Bm

]T

(2.21)

This completes the proof. ( ) ( T T) ∂ ∂ ∂ ∂ Corollary 2.12. ∂B {T r (AB)} = ∂B {T r (BA)} = ∂B {T r AT B T( } = ∂B {T r ) (B A )} = AT , where A ∈ Rn×m , B ∈ Rm×n and T r (AB) , T r (BA) , T r AT B T , T r B T AT are scalars. Proof. : The proof follows trivially from Facts 2.7 through 2.11. Fact 2.13.

∂ {T r (BA)} ∂A

= B T , where A ∈ Rn×m , B ∈ Rm×n and T r (BA) is a scalar.

∂ {T r (AB)} = Proof. : From Fact 2.7 T r (AB) = T r (BA). Furthermore from Corollary 2.12 ∂A ∂ T {T r (BA)} = B , which completes the proof. ∂A ( ) ( ) ∂ ∂ ∂ ∂ Corollary 2.14. ∂A {T r (AB)} = ∂A {T r (BA)} = ∂A {T r AT B T( } = ∂A {T r( B T AT) } = ) B T , where A ∈ Rn×m , B ∈ Rm×n and T r (AB) , T r (BA) , T r AT B T , T r B T AT are scalars.

Proof. : The proof follows trivially from Facts 2.7 through 2.10 and Fact 2.13. {T r (AB)} = A, where A ∈ Rn×m , B ∈ Rm×n and T r (AB) is a scalar. (∂ )T ( )T Proof. : From Corollary 2.2 and Fact 2.11 ∂B∂ T {T r (AB)} = ∂B {T r (AB)} = AT = A. Fact 2.15.

∂ ∂B T

{T r (AB)} = B, where A ∈ Rn×m , B ∈ Rm×n and T r (AB) is a scalar. (∂ )T ( )T Proof. : From Corollary 2.2 and Fact 2.13 ∂A∂ T {T r (AB)} = ∂A {T r (AB)} = B T = B. Fact 2.16.

Fact 2.17. is a scalar.

∂ ∂AT

∂ {T r (ABC)} ∂A

= C T B T , where A ∈ Rn×m , B ∈ Rm×p , C ∈ Rp×n and T r (ABC)

Proof. : The proof follows trivially from Fact 2.13 where B is replaced by BC. Fact 2.18. is a scalar.

∂ {T r (ABC)} ∂B

= AT C T , where A ∈ Rn×m , B ∈ Rm×p , C ∈ Rp×n and T r (ABC)

Proof. : The proof follows trivially from Facts 2.7 and 2.13. ( ) ( ) ∂ Fact 2.19. ∂B {T r AB T C } = CA, where A ∈ Rn×m , B ∈ Rp×m , C ∈ Rp×n and T r AB T C is a scalar. 13

) ( ) ( )T ∂ AB T C } = ∂B {T r BAT C T } = AT C T = CA. ) ( ) ( ( ) ∂ Fact 2.20. ∂B {T r AT BA } = B + B T A, where A ∈ Rn×n , B ∈ Rn×n and T r AT BA is a scalar. Proof. :

∂ {T r ∂B

(

Proof. : Notice that ( ) ∂ {T r AT BA } = ∂B

(

∂ ∂B

{

(

)}

T Tr A | {zB} A

∂ + ∂B

{

(

T r AT |{z} BA X Y ( ) ( T ) ∂ ∂ = {T r (XA)} + {T r A Y } ∂B ∂B

)})

(2.22)

( ) ∂ ∂ Now, from Corollary 2.12 or 2.14, observe that ∂B {T r (XA)} = X T and ∂B {T r AT Y } = Y . Substituting this into( Eq. (2.22) along with the ( notations ) ) (of X and ) Y (in the first ) equation ∂ ∂ of Eq. (2.22), yields ∂B {T r (XA)} + ∂B {T r AT Y } = X T + Y = B T + B A. ( ) ∂ Fact 2.21. ∂X {T r (AXBX)} = AT X T B T + B T X T AT , where A ∈ Rn×m , B ∈ Rn×m , X ∈ Rm×n and T r (AXBX) is a scalar. Proof. : Notice that           ∂ ∂ ∂ T T T T      {T r (AXBX)} = T r AXB | {z } X  + ∂X T r X B | X {z A }  ∂X ∂X  Λ1 Λ2 ( ) ( ) ∂ ∂ = {T r (Λ1 X)} + {T r X T Λ2 } (2.23) ∂X ∂X ( ) ∂ ∂ Now, from Corollary 2.12 or 2.14, observe that ∂X {T r (Λ1 X)} = ΛT1 , ∂X {T r X T Λ2 } = Λ2T . Substituting this into Eq. (2.23) ( ∂ along with the∂ notations ( T of ) )Λ1 and ( TΛ2 inT )the of Eq. )(2.23), yields ∂X {T r (Λ1 X)} + ∂X {T r X Λ2 } = Λ1 + Λ2 = (firstT equation B X T AT + AT X T B T . ( ) ( T ) ∂ T T Fact 2.22. ∂X {T r AXBX } = A XB + AXB , where A ∈ Rn×n , B ∈ Rn×n , X ∈ ( ) n×n T R and T r AXBX is a scalar. 

Proof. : Notice that           ) ( ∂ ∂ ∂ T X T AT XB T   X + {T r AXBX T } =  T r AXB T r | {z } | {z }   ∂X  ∂X ∂X  Λ1 Λ2 ( ) ) ( ) ( ∂ ∂ = (2.24) {T r Λ1 X T } + {T r X T Λ2 } ∂X ∂X ( ∂ ) ) ) ( ( ∂ Now, from Corollary 2.12 or 2.14, observe that ∂X {T r Λ1 X T } + ∂X {T r X T Λ2 } = (Λ1 + Λ2 ) = AXB + AT XB T . ) ( ∂ T Fact 2.23. } = 2AXB, where A ∈ Rn×n , B ∈ Rn×n , X ∈ Rn×n and {T r AXBX ∂X ) ( T T r AXBX is a scalar. 

14

Summary of a list of useful trace identities: ( ) ( ) 1. T r (AB) = T r (BA) = T r AT B T = T r B T AT , where A ∈ Rn×n , B ∈ Rn×n . ( ) ( ) 2. T r (ABC) = T r (CAB) = T r C T B T AT = T r B T AT C T , where A ∈ Rn×m , B ∈ Rm×p , B ∈ Rp×n . ( ) ( ) 3. T r (ABC) = T r (BCA) = T r AT C T B T = T r C T B T AT , where A ∈ Rn×m , B ∈ Rm×p , B ∈ Rp×n . Summary of a list of useful trace derivative identities: ( ) ∂ ∂ ∂ 1. ∂B {T r (AB)} = ∂B {T r (BA)} = ∂B {T r AT B T } = A ∈ Rn×m , B ∈ Rm×n . ( ) ∂ ∂ ∂ 2. ∂A {T r (AB)} = ∂A {T r (BA)} = ∂A {T r AT B T } = A ∈ Rn×m , B ∈ Rm×n .

∂ {T r ∂B

∂ {T r ∂A

( (

) B T AT } = AT , where

) B T AT } = B T , where

3.

∂ ∂B T

{T r (AB)} = A, where A ∈ Rn×m , B ∈ Rm×n .

4.

∂ ∂AT

{T r (AB)} = B, where A ∈ Rn×m , B ∈ Rm×n .

5.

∂ {T r (ABC)} ∂A

= C T B T , where A ∈ Rn×m , B ∈ Rm×p , C ∈ Rp×n .

6.

∂ {T r (ABC)} ∂B

= AT C T , where A ∈ Rn×m , B ∈ Rm×p , C ∈ Rp×n .

( ) AB T C } = CA, where A ∈ Rn×m , B ∈ Rp×m , C ∈ Rp×n . ( ) ( ) ∂ 8. ∂A {T r AT BA } = B + B T A, where A ∈ Rn×n , B ∈ Rn×n . ( ) ( ) ∂ 9. ∂X {T r AXBX T } = AT XB T + AXB A, where A ∈ Rn×n , B ∈ Rn×n , X ∈ Rn×n . 7.

∂ {T r ∂B

10.

∂ {T r (BB)} ∂B

= 2B T , where B ∈ Rn×n .

11.

∂ {T r ∂B

12.

∂ {T r (B −1 )} ∂B

13.

∂ {T r (AB −1 C)} ∂B

( ) BB T } = 2B, where B ∈ Rn×n . = − (B −2 ) , where B ∈ Rn×n and det (B) ̸= 0. T

= − (B −1 CAB −1 ) , where A, B, C ∈ Rn×n and det (B) ̸= 0. T

15

Chapter 3 Random Variables and Processes In probability theory, a stochastic process or random process is a collection of random variables. This is often used to represent the evolution of some random value, or system, over time. In discrete time, a stochastic process amounts to a sequence of random variables known as a time series. Another basic type of a stochastic process is a random field, whose domain is a region of space, in other words, a random function whose arguments are drawn from a range of continuously changing values. In this chapter, basic definitions of random process and their mathematical description in probabilistic terms is presented. 3.1

Basic definitions

Definition. [8] (Sample space) The set of all possible outcomes of a conceptual chance experiment or random trail is called as sample space. It is often denote by S. Definition. [8] (Event) A subset of the sample space is usually called an event. Definition. [8] (Mutually exclusive events) Any two events are mutually exclusive if they cannot occur at the same time or two mutually exclusive events are events that logically cannot be true at the same time. Another term for mutually exclusive is “disjoint”. Definition. [8] (Random variable) Given a conceptual chance experiment, which has a suitable sample space, an appropriate set of events, and a probability assignment for the set of events, a random variable is a function that maps every event in the sample space on to the real line. Definition. [8] (Conditional probability) The conditional probability of an event A given B is the probability of event A if event B is known to occur. The conditional probability of A given B is defined as ∩ P (A B) (3.1) P (A|B) = P (B) Similarly, the conditional probability of B given A is ∩ P (B A) P (B|A) = P (A)

16

(3.2)

Definition. [8] (Bayes theorem) If A and B are two events in the sample space with their probabilities P(A) and P(B), Bayes theorem states that P (A|B) =

P (B|A) P (A) P (B)

(3.3)

where, P(A|B) and P(B|A) are conditional probabilities. Definition. (Total law of probability) If countably infinite events of a sample space are given by Bn : n = 1, 2, 3..... and each event in Bn is measurable then for any event A the law of total probability states: ∑ P (A|Bn )P (Bn ) (3.4) P (A) = n

The law of total probability can also be stated for conditional probabilities. Taking the Bn as above, and assuming X is not mutually exclusive with A or any of the Bn : ∑ ( ∩ ) P (A|X) = P A|X Bn P (Bn |X) (3.5) n

Definition. [8] (Cumulative distribution function) The cumulative distribution function, FX (cdf ), or distribution function, describes the probability that a real-valued random variable X with a given probability distribution will be found at a value less than or equal to x. The cdf, FX (x), has following properties: 1. FX (x) → 0,

as x → −∞

2. FX (x) → 1,

as x → ∞

3. FX is a non decreasing function of x. where x is a parameter representing a realization of random variable X. Definition. [8] (Probability density function) The information contained in the distribution function FX may also be presented in derivative form. fX (x) =

d FX (x) dx

(3.6)

where, the function fX (x) is known as the probability density function (pdf ) associated with the random variable X. The pdf, fX (x), has following properties: 1. fX (x) is a nonnegative function. ∫∞ 2. −∞ fX (x) dx = 1 Definition. [8] (Expectation) In the study of random variables we consider the conceptual average that would occur for an infinite number of trials. This hypothetical average is called 17

expected value. In a random variable with n possible realizations x1 , x2 ,...., xn , an n corresponding probabilities p1 , p2 ,...., pn , for N trials, the sample average is ¯ = (p1 N )x1 + (p2 N )x2 + ... + (pn N )xn X N

(3.7)

The expected value for the discrete probability case is E [X] = Σni=1 pi xi Similarly, for a continuous random variable X, we have ∫ ∞ E [X] = xfX (x) dx

(3.8)

(3.9)

−∞

Definition. [8] (Variance) The variance of a random variable X is defined as [ ] V ar X = E (X − E (X))2

(3.10)

In a qualitative sense, the variance of X is a measure of the dispersion of X about its mean. The expression for variance in Eq. (3.10) can be reduced to a more convenient computational form by by expanding the quantity within the brackets to obtain: [ ] V ar X = E X 2 − 2XE (X) + (E (X))2 ( ) = E X 2 − (E (X))2 (3.11) Definition. [8] (Standard deviation) The standard deviation of a random variable X is defined as the square root of its variance Definition. (Normal or Gaussian random variable) The random variable X is called normal or Gaussian if its pdf is 2 1 1 fX (x) = √ e[− 2σ2 (x−µx ) ] (3.12) 2πσ where µx and σ 2 represent random variable’s mean and variance respectively. The pdf of a Gaussian distributed random variable is completely specified by assigning numerical values to the mean and variance. Thus, a short hand notation to describe a Gaussian distributed random variable is given as X ∼ N (µx , σ 2 ) (3.13) Definition. (Bernoulli random variable) A Bernoulli random variable X is one that takes on the values 0 or 1 according to { p, if j = 1; P (X = j) = (3.14) q = 1 − p, if j = 0. Definition. (Markov property) A stochastic process has the Markov property if the conditional probability distribution of future states of the process (conditional on both past and present values) depends only upon the present state; that is, given the present, the future 18

does not depend on the past. A process with this property is said to be Markovian or a Markov process. Definition. [8] (Random process) A random or a stochastic process is a representation of the evolution of a single random variable or a collection of random variables over time. The words “random” and “stochastic” are used interchangeably.

Figure 3.1: Ensemble of sample realizations of a random signal. Figure 3.1 shows an ensemble for a random signal X(t). Definition. [8] (Stationary process) A stationary process is a stochastic process whose joint probability distribution does not change when shifted in time. Definition. [8] (Strict sense stationary process) A random process X(t) is said to be strict sense stationary (SSS) if all its finite order distributions are time invariant i.e., the joint cdf of X(t1 ), X(t2 ), ...., X(tk ) is same as for X(t1 + α), X(t2 + α), ...., X(tk + α), for all k, t and time shifts α. Definition. [8] (Wide sense stationary process) A random process X(t) is said to be wide sense stationary (WSS) if its mean and autocorrelation functions are time invariant i.e., E(X(t)) = µ independent of t and RX (t2 − t1 ) is dependent only on t2 − t1 . Definition. [8] (Independence) The random variables X and Y with cumulative distribution functions FX (x) and FY (y), and probability densities X (x) and Y (y), are independent if the combined random variable (X, Y) has a joint cumulative distribution function and density function FX,Y (x, y) = FX (x)FY (y) fX,Y (x, y) = fX (x)fY (y) 19

(3.15)

Definition. [8] (Ergodicty) A random process is said to be ergodic if time averaging is equivalent to ensemble averaging. Definition. [8] (Auto correlation) The autocorrelation of a random process describes the correlation between values of the process at different points in time, as a function of the two times or of the time difference. Let Xt and Xs are realizations of a random processes with mean µt , µs with variances σt2 , σs2 then the auto correlation function RX (t, s) is given by RX (t, s) =

E [(Xt − µt ) (Xs − µs )] σt σs

(3.16)

Definition. [8] (Cross correlation) The cross correlation describes the correlation between multiple random processes at any particular instance of time. Let X and Y be two random 2 processes with mean µX , µY and with variances σX , σY2 at time t. Then the cross correlation function is given by E [(X − µX ) (Y − µY )] R (X, Y ) = (3.17) σX σY Definition. [8] (Auto covariance) For a random process X(t), the auto covariance is the covariance of the variable against a time shifted version of itself. If the process has a mean µt , then the auto covariance is given by Cov (t, s) = E [(Xt − µt ) (Xs − µs )]

(3.18)

Definition. [8] (Cross covariance) For a random processes X(t) and Y(t), the cross covariance is the covariance of X and Y. If the processes have a mean µX and µY , then the cross covariance is given by Cov(X, Y ) = E [(X − µX ) (Y − µY )] (3.19) Remark 3.1. Consider a set of n random variables X1 , X2 ,....., Xn . We define a vector random variable X as [ ]T X = X1 X2 . . . Xn (3.20) In general, the components of X may be correlated and have nonzero means. We denote the respective means as µ1 , µ2 ,....., µn , and thus we define a mean vector m as µ =

[

µ1 µ2 . . . µn

]T

(3.21)

Now we can define a matrix that describes the variances and correlation structure of the variants. The covariance matrix for X is defined as [ ]   E (X1 − µ1 )2 E [(X1 − µ1 ) (X2 − µ2 )] . . . .   E [(X2 − µ2 ) (X1 − µ1 )] . . . . .     . . . . . . C =    . . . . . . [ 2] . . . . . E (Xn − µn ) 20

The terms along the major diagonal of C are seen to be the variances of the variants, and the off diagonal terms are the covariances. Definition. [8] (Power spectral density) The power spectral density (PSD), or energy spectral density (ESD), is a positive real function of a frequency variable associated with a stationary stochastic process, or a deterministic function of time, which has dimensions of power per hertz (Hz), or energy per hertz. It is often called simply the spectrum of the signal. Intuitively, the spectral density measures the frequency content of a stochastic process and helps identify periodicities. If RX is the autocorrelation function of a random process X, the PSD of X is given by ∫ SX (jω) = F [RX (τ )] =

∞

−∞

RX (τ )e−jωτ dτ

(3.22)

where, F indicates Fourier transform and ω is frequency in hertz. SX is called the power spectral density function of the random process X. Definition. [8] (Wiener Khinchin Theorem) The WienerKhinchin theorem states that the power spectral density of a wide-sense-stationary random process is the Fourier transform of the corresponding autocorrelation function. 3.2

Various random processes

Definition. (White noise) White noise is defined to be a stationary random process having a constant spectral density function over the entire frequency spectrum. In other words, a pure white noise process is one which has all frequencies and, furthermore, equal power at all frequencies. Since the power spectrum of a process is the summation of the power of the process at all frequencies, a white noise process possess infinite power. However, in reality, no process can exhibit this kind of behavior and hence it is always advisable to define white noise from the perspective of being band-limited in nature and hence a limited number of frequencies and not the entire frequency spectrum. This is called as a ”band-limited” white noise process. Mathematically, a process {w (t) : t ∈ R+ → R} is white if and only if its mean and autocorrelation function satisfy the following: E [w (t)] = 0 E [w (t1 ) w (t2 )] = v0 δ (t1 − t2 ) , δ (t1 − t2 ) = 1, t1 = t2 δ (t1 − t2 ) = 0, t1 = ̸ t2

(3.23)

where v0 < ∞ is the variance of w (t) and δ (·) is the Dirac delta function. Thus a white noise process is a serially uncorrelated, zero-mean, constant and finite variance process. From the second equation of Eq. (3.23) (autocorrelation), one can compute the power spectrum of w (t) by taking the Fourier transform of E [w (t1 ) w (t2 )]. This results in a power spectrum of v0 at all frequencies, which when added up yields infinite power. Hence only band-limited white noise is realizable process and not pure white noise. As the name suggests, band-limited white noise is a random process whose power spectral density is constant (flat) over a certain range of frequencies and 0 outside this range. It should be noted here that for a white noise process such as w (t) its auto covariance and autocorrelation are identical. Furthermore, an 21

i.i.d process (independent and identically distributed process), with finite variance is a white noise process but the converse is not necessarily true. Definition. (Colored noise) When first order transfer function is exited with band limited white noise, the resulting random process is called as colored noise. The simulation block diagram is shown below:

Figure 3.2: Simulating colored noise. Definition. (Wiener process) The random process obtained by integrating bandlimited white noise is called as wiener process.

Figure 3.3: Simulating wiener process. The Wiener process and First order Gauss markov process are used for modeling bias in IMU measurements. The detailed description of these processes along with bode plot and histogram are presented in Chapter 6.

22

Chapter 4 Extended Kalman Filter The availability of direct measurements for all state variables is a rare occasion in practice. In physical systems, some components of the state vector are inaccessible internal variables, which either cannot be measured or the measurements require the use of very costly measurement devices. Therefore, it is either not feasible or it is very expensive to measure all the state variables. Hence, in most practical scenarios there is a true need to construct estimates of the unknown state variables via known measurements. For the case of linear dynamical systems with deterministic disturbances, the Luenberger observer offers a complete and comprehensive answer to the problem of state estimation [9]. In the case of systems that are linear in the process and measurement, and which are exogenously corrupted by white process noise and measurement noise, some of the early methods for observer design, which date as far back as the early 1940’s, involved the Wiener-Hopf filter, which is a linear system [10, 11] and the Kalman-Bucy filter [12, 13, 8], which is a model-based filtering approach. The major contribution of the Wiener filter to observer theory was in the derivation of the steady state optimal filter and predictor for stochastic stationary scalar processes using spectral factorization techniques. On the other hand, in the development of the Kalman filtering theory, the input-output signal Wiener model was replaced by a state-space model, and a recursive solution of the optimal state estimator was obtained for stationary and non stationary cases. Furthermore, the Kalman filter offers a recursive solution in the sense of minimizing the trace of the error covariance of the system states. In other words, the Kalman filter can also be called as a minimum variance estimator. The Wiener and Kalman filters are optimal filters in the least squares sense and are equivalent to each other in steady state [14] and offer optimum state estimates for linear systems. However, in the case of systems which are inherently nonlinear in either the process dynamics or the measurement or both, straightforward implementation of the linear KF is not guaranteed to yield optimum results in the sense of minimizing the root mean square of the estimation error. Thus the need to be able to effectively reconstruct the unknown states of a nonlinear system has promoted research in nonlinear filtering theory. The design of nonlinear filters is a very challenging problem and has received a considerable amount of attention in the literature over the past few decades [15, 16]. Of the numerous attempts being made for the development of nonlinear filters, the extended Kalman filter (EKF) is the most popular approach1 . The design of the EKF is typically based on a 1

EKF is a class of approaches that approximates the problem and then applies linear Kalman filtering techniques [16].

23

first order local linearization of the system around a reference trajectory at each time step [8, 17, 18, 19]. Thus the partial derivative of the nonlinear system dynamics is computed with respect to the system states and evaluated at the state estimate for each time step. The EKF is an estimator that handles nonlinearities in the dynamical processes as well as measurements and furthermore, akin to a KF, treats systems with the process and measurement dynamics corrupted by white Gaussian noise. The EKF [18, 20, 21, 22] was conceptualized as an engineering approximation to a difficult theoretical problem. The issue was about estimating the state of a nonlinear system from available measurements in the presence of disturbances. Since their inception, EKFs have been successfully applied for the purpose of state estimation for nonlinear stochastic systems [23, 24, 25]. In the case of systems that have absolutely no noise in either the process or the measurement, i.e., perfect model and noise free sensor(s), the EKF can be designed to function as a state estimator for a deterministic nonlinear system [26]. 4.1

Nonlinear System: Discrete Process Dynamics & Discrete Measurement

In this section, the EKF prediction and correction steps are derived for a typical discrete nonlinear system. Consider the following observable, discrete, nonlinear dynamical system: xk = f (xk−1 ) + Γk−1 wk−1 , zk = h (xk ) + vk

k≥0

(4.1) (4.2)

where, xk ∈ Dx ⊂ Rn denotes the n-dimensional state vector of the system, f (·) : Dx → Rn is a finite nonlinear mapping of the system states and system input, zk ∈ Dz ⊂ Rp denotes the p-dimensional system measurement, h (·) : Dx → Rp is a nonlinear mapping of the system states to the output, Γk−1 ∈ Rn×w denotes the process noise scaling matrix, wk ∈ Rw denotes the w-dimensional random process noise vector, vk ∈ Rv denotes the v-dimensional random measurement noise vector and k is discrete index. The process and measurement noise vectors are assumed to be zero mean, band-limited, additive, white Gaussian noise (AWGN) processes given by: { [ ] Qk , k = j T E wk wj = Qk δkj = (4.3) 0, k ̸= j { [ ] Rk , k = j T E vk vj = Rk δkj = (4.4) 0, k ̸= j ] [ ] [ (4.5) E wk vjT = E [wk ] E vjT = 0 where Qk , Rk are process and measurement noise intensities, δkj is the kronecker delta function and Eq. (4.5) is due to the fact that wk and vk are uncorrelated for all k, j. The random variables wk and vk are commonly denoted as wk ∼ N (0, Qk ) and vk ∼ N (0, Rk ) respectively. The initial state of the system is assumed to be a Gaussian random vector with ¯ 0 and covariance P0 . mean x Remark 4.1. The matrices Qk and Rk are symmetric, i.e. Qk = QTk and Rk = RkT . 24

Remark 4.2. The goal of the EKF is to obtain an estimate of the state vector given by xk in Eq. (4.1) using only the set of available measurements given by zk in Eq. (4.2). Remark 4.3. The design of the EKF is accomplished in two stages: 1. Prediction (Time Update) 2. Update/Correction (Measurement Update) In the prediction stage, the EKF predicts the estimate of the state vector via the nonlinear mapping in Eq. (4.1) based on the previous estimate of the state vector. In the update stage, the filter corrects the predicted estimate of the state obtained in the prediction stage with available measurement at the current time instant. A typical 1-step prediction-update scheme is shown in Figure 4.1.

Figure 4.1: Prediction (Time Update) and Correction (Measurement Update) philosophy 4.1.1

EKF Design: Notations & Assumptions

Denote the following predicted variables at the discrete time index k: ˆ− 1. Predicted State Estimate: x k ˆ− 2. Predicted State Error Estimate: e− k k = xk − x − 3. Predicted State Error Covariance Matrix: Pk− = E[e− k ek ] T

and the following corrected variables at the discrete time index k as: ˆk 1. Corrected State Estimate: x 25

ˆk 2. Corrected State Error Estimate: ek = xk − x 3. Corrected State Error Covariance Matrix: Pk = E[ek eTk ] Assumption 4.4. The corrected state [ error ] ek and [ the ] process noise vector wk are uncorT T related signals and hence satisfy: E ek wk = E wk ek = 0. Assumption 4.5. The predicted state error e− and the noise process vk are [ measurement ] [ − Tk] −T uncorrelated signals and hence satisfy: E ek vk = E vk ek = 0. 4.1.2

EKF Prediction

Consider a first order Taylor series approximation of f (xk−1 ) in Eq. (4.1) about the corrected ˆ k−1 at k − 1 as state estimate x ˆ k−1 + x ˆ ) f (xk−1 ) = f (xk−1 − x k−1 ∂f ˆ ) + H.O.T = f (ˆ xk−1 ) + (xk−1 − x {z k−1} ∂xk−1 xk−1 =ˆxk−1 | ek−1 | {z } Fk−1 ∂f ˆ ) (xk−1 − x ≈ f (ˆ xk−1 ) + {z k−1} ∂xk−1 xk−1 =ˆxk−1 | ek−1 {z } | Fk−1

⇒ f (xk−1 ) − f (ˆ xk−1 ) ≈ Fk−1 ek−1

(4.6)

where, H.O.T denotes higher order terms, Fk−1 denotes the Jacobian of the nonlinear system ˆ k−1 . The EKF state prediction is given as dynamics in Eq. (4.1) evaluated at x ˆ− x = f (ˆ x ), k ( k−1 ) − ˆ− zˆk = h x k

ˆ− ¯0 x 0 = x

(4.7) (4.8)

The difference between Eqs. (4.1) and (4.7) yields ˆ− e− = xk − x k k = f (xk−1 ) − f (ˆ xk−1 ) + Γk−1 wk−1

(4.9)

Furthermore, substituting Eq. (4.6) in Eq. (4.9) yields = Fk−1 ek−1 + Γk−1 wk−1 e− k

26

(4.10)

Using Eq. (4.10), the predicted state error covariance matrix, Pk− can be obtained as Pk− = = = =

−T E[e− k ek ] E[(Fk−1 ek−1 + Γk−1 wk−1 )(Fk−1 ek−1 + Γk−1 wk−1 )T ] T T E[(Fk−1 ek−1 + Γk−1 wk−1 )eTk−1 Fk−1 + wk−1 ΓTk−1 ] T T E[Fk−1 ek−1 eTk−1 Fk−1 + Fk−1 ek−1 wk−1 ΓTk−1 T T +Γk−1 wk−1 eTk−1 eTk−1 Fk−1 + Γk−1 wk−1 wk−1 ΓTk−1 ] T T = E[Fk−1 ek−1 eTk−1 Fk−1 ] + E[Fk−1 ek−1 wk−1 ΓTk−1 ] T T ΓTk−1 ] ] + E[Γk−1 wk−1 wk−1 +E[Γk−1 wk−1 eTk−1 Fk−1 T T = Fk−1 E[ek−1 eTk−1 ] Fk−1 + Fk−1 E[ek−1 wk−1 ] ΓTk−1 | {z } | {z } =0 from Assumption 4.4

Pk−1

+Γk−1

E[wk−1 eTk−1 ] |

{z

}

T Fk−1

T + Γk−1 E[wk−1 wk−1 ] ΓTk−1 | {z }

=0 from Assumption 4.4

⇒ 4.1.3

Pk−

=

T Fk−1 Pk−1 Fk−1

+

Qk−1

Γk−1 Qk−1 ΓTk−1

(4.11)

EKF Correction

Consider a first order Taylor series approximation of h (xk ) in Eq. (4.2) about the predicted ˆ− state estimate x k at k as ( ) ˆ− ˆ− h (xk ) = h xk − x k +x k ( −) ( ) ∂h − ˆk + ˆ = h x x − x + H.O.T k k ∂xk xk =ˆx− | {z } | {z k} e− k Hk ( ) ( −) ∂h − ˆ ˆk + x − x ≈ h x k ∂xk xk =ˆx− | {z k } | {z k} e− k Hk ( −) ˆk ⇒ h (xk ) − h x = Hk e − (4.12) k where, Hk denotes the Jacobian of the nonlinear measurement map in Eq. (4.2) evaluated ˆ− at x k . The EKF correction/update equations are given as ) ( ˆk = ˆ− + Kk zk − zˆ− x x k k |{z} | {z } due to prediction, i.e., Eq.

4.7

( ( − )) ˆ ˆ− = x k + Kk h (xk ) + vk − h x ( ( −) k ) − ˆ k + vk ˆ k + Kk h (xk ) − h x = x

27

due to correction

(4.13)

The difference between Eqs. (4.1) and (4.13) yields ( ( −) ) ˆ− ˆ ek = xk − x −K h (x ) − h x + v k k k k | {z k} e− k

( ( −) ) ˆ k + vk = e− k − Kk h (xk ) − h x

(4.14)

Furthermore, substituting Eq. (4.12) in Eq. (4.14) yields ) ( − ek = e− k − Kk Hk ek + vk = (I − Kk Hk ) e− − Kk vk | {z } k =

Ψk − Ψk ek −

Kk vk

(4.15)

where, I − Kk Hk is denoted as Ψk . Using Eq. (4.15), the corrected state error covariance matrix, Pk can be obtained as Pk = = = = =

E[ek eTk ] −T T T T E[(Ψk e− k − Kk vk )(ek Ψk − vk Kk )] −T T −T T − T T T T E[Ψk e− k ek Ψk − Kk vk ek Ψk − Ψk ek vk Kk + Kk vk vk Kk ] −T T −T T − T T T T E[Ψk e− k ek Ψk ] − E[Kk vk ek Ψk ] − E[Ψk ek vk Kk ] + E[Kk vk vk Kk ] Ψk E[e− e−T ] ΨT − Kk ΨTk E[vk e−T ] | k{z k } k | {z k } Pk−

=0 from Assumption 4.5 T E[e− k vk ] | {z }

−Ψk

KkT +

Kk E[vk vkT ] KkT | {z } Rk

=0 from Assumption 4.5

=

Ψk |{z} I−Kk Hk

⇒ Pk

Pk−

+Kk Rk KkT

ΨTk

|{z} I−HkT KkT

= (I − Kk Hk ) Pk− (I − Kk Hk )T + Kk Rk KkT = Pk− − Kk Hk Pk− − Pk− HkT KkT + Kk Hk Pk− HkT KkT + Kk Rk KkT

(4.16)

The optimal Kalman gain matrix Kk is obtained by minimizing the trace of Pk in Eq. (4.16), with respect to KkT to yield2 )−1 ( Kk = Pk− HkT Hk Pk− HkT + Rk

(4.17)

Substituting Eq. (4.17) in the last equation of Eq. (4.16) and simplifying yields Pk = (I − Kk Hk ) Pk−

(4.18)

) ( To see the result in Eq. (4.18), post multiply Eq. (4.17) by Hk Pk− HkT + Rk to obtain ) ( (4.19) Pk− HkT = Kk Hk Pk− HkT + Rk = Kk Hk Pk− HkT + Kk Rk 2

For a derivation of the Kalman gain refer to Appendix A.

28

Substituting Eq. (4.19) into Eq. (4.16) yields Pk = Pk− − Kk Hk Pk− − Pk− HkT KkT + Kk Hk Pk− HkT KkT + Kk Rk KkT ( ) = Pk− − Kk Hk Pk− − Pk− HkT KkT + Kk Hk Pk− HkT + Kk Rk KkT | {z } =Pk− HkT from Eq.(4.19)

⇒ Pk

= Pk− − Kk Hk Pk− − Pk− HkT KkT + Pk− HkT KkT = (I − Kk Hk ) Pk−

(4.20)

Thus the prediction and update EKF equations can be summarized as follows: Prediction Equations ˆ− x k zˆ− k Pk− Fk−1

= f (ˆ x ) ( k−1 ) ˆ− = h x k

(4.21) (4.22)

T = Fk−1 Pk−1 Fk−1 + Γk−1 Qk−1 ΓTk−1 ∂f = ∂xk−1 xk−1 =ˆxk−1

(4.23)

Kk = ˆk = x Pk = Hk =

Update Equations ( )−1 Pk− HkT Hk Pk− HkT + Rk ˆ− ˆ− x k + Kk (zk − z k) − (I − Kk Hk ) Pk ∂h ∂xk xk =ˆx−

(4.24)

(4.25) (4.26) (4.27) (4.28)

k

A schematic view of the EKF philosophy is shown in Figure 4.2, where it can be seen that in order to start the filtering process, the EKF needs to be initialized with respect to the ˆ 0 and the initial state error covariance matrix, P0 . initial state estimate, x

29

Figure 4.2: Prediction (Time Update) and Correction (Measurement Update) in an EKF 4.2

Nonlinear System: Continuous Process Dynamics & Discrete Measurement

Consider an observable, nonlinear dynamical system where the process dynamics is continuous and the measurement is discrete, as x˙ (t) = f (x (t)) + Γc w (t) , zk = h (xk ) + vk ,

x (0) = x0 xk = x (k∆t) , k ≥ 0

(4.29)

where, as before, x ∈ Dx ⊂ Rn denotes the n−dimensional state vector of the system, f (·) : Dx → Rn is a finite nonlinear mapping of the system states and system input, zk ∈ Dz ⊂ Rp denotes the p−dimensional system measurement, h (·) : Dx¯ ⊂ Rn → Rp is a nonlinear mapping of the system states to the output, Γc ∈ Rn×w denotes the continuous process noise scaling matrix, w ∈ Dw ⊂ Rw denotes the w−dimensional random process noise, vk ∈ Dv ⊂ Rv denotes the v−dimensional random measurement noise and ∆t denotes the sample time. The space Dx¯ denotes the sampled version of the continuous state vector which evolves on Dx . The process and measurement noise are assumed to be zero mean,

30

band-limited, AWGN processes given by { [ ] T E w(t)w(t − τ ) = Qδ(t − τ ) = { [ ] Rk , T E vk vj = Rk δkj = 0,

Q, t = τ 0, t = ̸ τ k=j k ̸= j

(4.30)

where, Q is the continuous process noise covariance, Rk is the discrete measurement noise covariance, δ (·) is the Dirac delta function and δkj is the Kronecker delta function. The random variables w and vk are uncorrelated and commonly denoted as w ∼ N (0, Q) and vk ∼ N (0, Rk ) respectively. The initial state of the system in Eq. (4.29) is assumed to be a ¯ 0 and covariance P¯0 . Let k be the discrete index. Then Gaussian random vector with mean x the form of the EKF is given as: EKF Prediction Equations ˆ (t) = x ˆ k−1 , P (t) = Pk−1 x ˙x ˆ (t) = f (ˆ x(t)) P˙ (t) = F (t)P (t) + P (t)F (t) + T

Kk ˆk x Pk

Γc QΓTc ,

∂f F (t) = ∂x(t) x(t)=ˆx(t)

EKF Update Equations ( )−1 = Pk− HkT Hk Pk− HkT + Rk , Pk− = P (t) + P˙ (t) · ∆t ( ) ( −) − − ˙ (t) · ∆t, zˆ− = h x ˆ− ˆ ˆ ˆ ˆ ˆk = x + K z − z , x = x (t) + x k k k k k k ∂h = (I − Kk Hk ) Pk− , Hk = ∂xk xk =ˆx−k

(4.31) (4.32) (4.33)

(4.34) (4.35) (4.36)

where, the initialized state estimate and the initialized state error covariance matrix [ ] are T ˆ (0) = x ˆ 0 = E [x (0 · ∆t)] = E [x0 ] and P (0) = P0 = E (x0 − x ˆ 0 ) (x0 − x ˆ0) . given as x

31

Chapter 5 GPS Based Attitude and Heading Estimation for a Micro Aerial Vehicle: Model Formulation In Chapter 4, the prediction and correction equations for an EKF were derived. In this chapter, the attitude and heading estimation model is derived. An Attitude Heading Reference System (AHRS) consists of sensors on three axes that provide heading and attitude information for a MAV. A typical AHRS consist of either solid-state or MEMS gyroscopes, accelerometers and magnetometers on all the three axes. The attitude information generated via an AHRS is used in many navigation guidance and control applications. 5.1

Strapdown Kinematic Model: Plant

Consider an MAV as shown in Figure 5.1 where the attitude and heading angles are as shown and denoted by ϕ (roll/bank), θ (pitch) and ψ (yaw/heading). In Figure 5.1, Xb , Yb , Zb represent the body axis frame while X, Y, Z represent the inertial axis frame.

Figure 5.1: Roll, Pitch and Yaw angles of an MAV. 32

5.1.1

Attitude Kinematics

The system of nonlinear differential equations which can be used to describe the attitude of an MAV parameterized via Euler angles (ϕ, θ and ψ) are written as: ( ) ( ) s cϕ ϕ + rm (t) ψ˙ (t) = qm (t) cθ cθ θ˙ (t) = qm (t) cϕ − rm (t) sϕ ϕ˙ (t) = pm (t) + qm (t) sϕ tθ + rm (t) cϕ tθ (5.1) with the notations sin ϕ ≡ sϕ ,

sin θ ≡ sθ ,

cos ϕ ≡ cϕ ,

cos θ ≡ cθ ,

tan θ ≡ tθ

(5.2)

where, ψ, θ and ϕ are the Euler angles (yaw, pitch and roll) that describe the MAV attitude, ˙ θ˙ and ϕ˙ are Euler angle rates and pm , qm and rm are angular rate measurements in body ψ, axis from a triaxial rate gyroscope strapped to the body of the MAV and whose sensing axes are aligned along the Xb , Yb , Zb directions respectively. The rate gyroscope and the accelerometer models are described in the sections below. 5.1.2

Rate Gyroscope Model

The rate gyroscope measurements pm (t) , qm (t) , rm (t) are assumed to be modeled as [27, 28]:   ( )  pm (t) = p(t) + b0p + b1p (t) + b2p (t)  +wp (5.3) | {z } |

{z

bp (t)

¯bp (t)



} 

( )  qm (t) = q(t) + b0q + b1q (t) + b2q (t)  +wq {z } | |

{z

bq (t)

¯bq (t)



} 

  rm (t) = r(t) + b0r + (b1r (t) + b2r (t)) +wr | {z } {z

|

br (t)

¯br (t)

(5.4)

(5.5)

}

where, p(t), q(t), r(t) are the true values of the angular velocity; b0p , b0q , b0r are the constant null-shift bias terms, b1p (t) , b1q (t) , b1r (t) are the rate random walk bias components, b2p (t) , b2q (t) , b2r (t) are the correlated (colored noise) bias components and wp , wq , wr denote the error due to sampling noise which are typically modeled as zero mean, bandlimited, additive, white Gaussian noise (AWGN) processes of specified covariances denoted 33

by σp2 , σq2 , σr2 , respectively. Remark 5.1. The effects due to constant bias in the rate gyroscope output are modeled as Weiner processes1 given by: b˙ 0p = 0 b˙ 0q = 0 b˙ 0r = 0

(5.6) (5.7) (5.8)

Remark 5.2. [27] The constant bias components, b0p , b0q , b0r , can be estimated off-line and removed from the output of the rate gyroscope. Hence, the effects of b0p , b0q , b0r on the output of the rate gyroscope are neglected. Remark 5.3. The effects due to rate random walk bias in the rate gyroscope output are modeled as Weiner processes given by: b˙ 1p = w1p b˙ 1q = w1q b˙ 1r = w1r

(5.9) (5.10) (5.11)

where, w1p , w1q , w1r are zero mean, band-limited, AWGN processes with specified covariances denoted by σ12p , σ12q , σ12r , respectively. Remark 5.4. The effects due to correlated noise in the rate gyroscope output are modeled as first order Gauss-Markov processes [29] given by: b2 w2 b˙ 2p = − p + p τp τp w2 b2 b˙ 2q = − q + q τq τq b2 w2 b˙ 2r = − r + r τr τr

(5.12) (5.13) (5.14)

where, w2p , w2q , w2r represent the driving process noise components which are assumed to be zero mean, band-limited, AWGN processes with specified covariances denoted by σ22p , σ22q , σ22r , respectively and τp , τq , τr denote the time constants of the modeled correlated noise processes. Remark 5.5. In this thesis, the effects due to b2p , b2q , b2r are not captured in the model for the EKF, i.e. b2p , b2q , b2r are treated as unmodeled bias components, while the effects due to b1p , b1q , b1r are captured in the EKF model as Weiner process components with however a different standard deviation for each of these components which alters the individual entries in the Q matrix. 1

A Weiner process is obtained by integrating a white noise process [29].

34

5.1.3

Accelerometer Model

A triad of accelerometers measures the proper acceleration (acceleration relative to free-fall), in terms of g-force. These accelerometer readings are denoted as axm , aym , azm and are given by: axm = ax + wax aym = ay + way azm = az + waz

(5.15) (5.16) (5.17)

where, ax , ay , az are the true forces and wax , way , waz denote the additive sensor noise processes, respectively, for the x, y, z axes and are modeled as zero mean, band-limited AWGN processes with specified covariances denoted by σa2x , σa2y , σa2z , respectively. Remark 5.6. Accelerometer bias is not considered since it is not observable in an AHRS formulation. For a detailed explanation refer to Appendix I. In case the accelerometer biases are significant then the full INS-GPS filter (15 ∼ 21 states) needs to be designed [1]. 5.1.4

AHRS Process Model

Recall that the system of differential equations which describe the MAV attitude parameterized via Euler angles is given as: [30] ( ) ( ) sϕ cϕ ˙ ψ (t) = qm (t) + rm (t) cθ cθ θ˙ (t) = qm (t) cϕ − rm (t) sϕ ϕ˙ (t) = pm (t) + qm (t) sϕ tθ + rm (t) cϕ tθ (5.18) Substituting Eqs. (5.3), (5.4) and (5.5) into Eq. (5.18), dropping the argument ’t’ and rearranging, yields ( ) ( ) ( ) ( ) ( ) ( ) sϕ cϕ sϕ sϕ cϕ cϕ ˙ ψ = q +r + b1q + b1r + wq + wr c cθ cθ cθ cθ cθ | θ {z } | {z } w1 f1 ( ) ( ) sϕ cϕ + b2r + b2q cθ cθ | {z } f1u

θ˙ = qcϕ − rsϕ + b1q cϕ − b1r sϕ + wq cϕ − wr sϕ + b2q cϕ − b2r sϕ {z } | | {z } | {z } w2

f2

f2u

ϕ˙ = p + qsϕ tθ + rcϕ tθ + b1p + b1q sϕ tθ + b1r cϕ tθ + wp + wq sϕ tθ + wr cϕ tθ {z } | {z } | w3

f3

+ b2p + b2q sϕ tθ + b2r cϕ tθ | {z }

(5.19)

f3u

35

where, f1 , f2 , f3 denote the modeled part of the process dynamics, f1u , f2u , f3u denote the unmodeled part of the process dynamics and w1 , w2 , w3 denote the coupling between the states (Euler angles) and the rate gyroscope noise components wp , wq , wr . 5.1.5

AHRS Measurement Model

To derive the measurement model for an AHRS framework, consider the force equations for a 6 degree-of-freedom (DOF) aircraft as [30]:    E      axm u˙ qm wE − rm v E sθ  aym  =  v˙ E  +  rm uE − pm wE  + g  −cθ sϕ  (5.20) azm w˙ E pm v E − q m u E −cθ cϕ Y Z where axm = X , a ym = m , azm = m , and X, Y, Z are the (components of m ) the resultant aeroE E E dynamic force acting on the MAV in the body axis frame; u , v , w are ( the components ) of the GPS obtained aircraft ground speed expressed in the body frame; u˙ E , v˙ E , w˙ E are the MAV linear accelerations expressed in the body frame; m is the mass of the MAV, and g is the acceleration due to gravity. The terms axm , aym , azm are the accelerometer readings in the body frame and are given by Eqs. (5.15) ∼ (5.17).

Attitude Measurements: Pitch (θm ) & Roll (ϕm ) From simple trigonometric relations and using Eq. (5.20), the pitch and roll measurements can be obtained from the triaxial accelerometer, triaxial rate gyroscope and GPS as: ( ) axm − u˙ E + rm v E − qm wE −1 θm = sin (5.21) g ) ( aym − v˙ E + pm wE − rm uE −1 (5.22) ϕm = tan azm − w˙ E + qm uE − pm v E where, uE , v E , wE are obtained as explained in Remark 5.7. Remark 5.7. The terms uE , v E , wE and as   E  cθ cψ u  v E  =  sϕ sθ cψ − cϕ sψ cϕ sθ cψ + sϕ sψ wE |

u˙ E , v˙ E , w˙ E in Eqs. (5.21) and (5.22) are obtained   cθ sψ −sθ x˙ gps + wxgps sϕ sθ sψ + cϕ cψ sϕ cθ   y˙ gps + wygps  cϕ sθ sψ − sϕ cψ cϕ cθ z˙gps + wzgps {z }

(5.23)

RB I 2 where, RB I denotes the direction cosine matrix that takes a vector from the inertial frame to the body frame, x˙ gps , y˙gps , z˙gps denote the aircraft velocity components in the inertial frame provided by an on-board GPS [31] receiver at an update rate of fu Hz and wxgps , wygps , wzgps denote zero mean, band-limited, AWGN processes of specified covariances denoted by σx2gps , σy2gps , σz2gps , 2

RB I is obtained using the current online attitude estimates.

36

respectively. Furthermore, u˙ E , v˙ E , w˙ E can be obtained by a finite differencing of uE , v E , wE , given the GPS update rate.3 Heading Measurement: Yaw (ψmag ) A magnetometer4 is used to generate the MAV heading measurement. Magnetometers are frequently used for spacecrafts that explore other celestial bodies such as planets. Magnetometers can be divided into two basic types: 1. Scalar magnetometers measure the total strength of the magnetic field to which they are subjected. 2. Vector magnetometers measure the component of the magnetic field in a particular direction, relative to the spatial orientation of the device. The use of three orthogonal vector magnetometers allows the magnetic field strength, inclination and declination to be uniquely defined. A typical triaxial magnetometer setup (axes − → direction only) is shown in Figure 5.2, where M N is the direction of magnetic north and MXB , MYB , MZB are the body axes magnetic field components, measured in units of Tesla5 , from the 3 orthogonally mounted single-axis magnetometers. Then the magnetic heading, ψmh , can be calculated, from Figure 5.2(b), as MXh = MXB cos (θ) + MYB sin (ϕ) sin (θ) + MZB cos (ϕ) sin (θ) MYh = MYB cos (ϕ) + MZB sin (ϕ) ( ) MYh −1 ψmh = − tan MXh

(5.24) (5.25) (5.26)

which is the MAV magnetic heading in the horizontal plane.

Figure 5.2: Magnetometer Mounted Onboard an MAV. 3

If the GPS obtained velocity is prohibitively noisy, then their low pass filtered (or smoothed) outputs can be used for the finite differencing instead. 4 A magnetometer measures the strength and direction of the magnetic field in its vicinity. 5 The tesla (symbol T) is the SI derived unit of magnetic flux density, commonly denoted as B, (which is also known as ”magnetic field”). 1 T = 1 Volt sec m−2 = 1 N Ampere−1 m−1 = 1 Weber m−2 = 1 kg Coulomb−1 s−1 = 1 kg Ampere−1 sec−2 = 1 N sec Coulomb−1 m−1 .

37

Remark 5.8. In the wings level condition, i.e. θ = 0, ϕ = 0, Eq. (5.26) reduces to ( ) MYB −1 ψmh = − tan (5.27) MXB Furthermore, the magnetic declination or magnetic variation at the location should be added to the magnetic heading to obtain the MAV true heading. Thus the MAV true heading can then be calculated as ψmag = ψmh + mv

(5.28)

where, mv denotes the magnetic variation. The following relationship is referred to obtain the heading:  ( ) −1 MYh  π − tan if MXh < 0  MXh       ( )   −1 MYh  2π − tan if MXh > 0, MYh > 0  MX h     ) ( (5.29) ψmh = −1 MYh if MXh > 0, MYh < 0 − tan  M  X h       π  if MXh = 0, MYh < 0  2       3π if MXh = 0, MYh > 0 2 A description of heading, track, course and magnetic variation is given in Appendix C. The following table provides mv for a list of locations in India according to the International Geomagnetic Reference Field (IGRF) model as on June 6th , 2012: Sl. No. 1. 2. 3. 4.

Place Bengaluru Mumbai Chennai Delhi

Latitude 12.9747 N 19.0769 N 13.0619 N 28.6667 N

Longitude 77.5878 E 72.8261 E 80.2481 E 77.2167 E

Magnetic Variation (deg) -1.61202 -0.57759 -1.58069 0.87478

Table 5.1: Magnetic Variation. The source for the numbers in Table 5.1 can be found in http : //www.ngdc.noaa.gov/geomag− web/#declination.

38

AHRS Measurement Model in Terms of States Pitch Measurement θm : Recall the pitch attitude measurement in Eq. (5.21) as ( θm = sin |

−1

) axm − u˙ E + rm v E − qm wE g {z }

(5.30)

fθ

Consider a first order Taylor series approximation of Eq. (5.30) about ax , r, q as ( ) (axm − ax + ax ) − u˙ E + (rm − r + r) v E − (qm − q + q) wE −1 θm = sin g ) ( E E E ax − u˙ + rv − qw ≈ sin−1 g | {z } θ ∂fθ ∂fθ ∂fθ (axm − ax ) + (rm − r) + (qm − q) + ∂axm ax ,r,q ∂rm ax ,r,q ∂qm ax ,r,q

(5.31)

Furthermore, from Eqs. (5.4), (5.5) and (5.15), we have a x m − a x = wa x rm − r = br + wr qm − q = b q + wq

(5.32) (5.33) (5.34)

where, bq , br do not include b0q , b0r respectively. Substituting Eqs. (5.32), (5.33) and (5.34) into Eq. (5.31) yields ∂fθ ∂fθ ∂fθ θm = θ + wa + (br + wr ) + (bq + wq ) ∂axm ax ,r,q x ∂rm ax ,r,q ∂qm ax ,r,q | {z } | {z } | {z } α1

α2

α3

⇒ θm = θ + α1 wax + α2 (br + wr ) + α3 (bq + wq ) (5.35) ∂fθ ∂fθ Notice that the terms ∂a∂fxθ , ∂r and once evaluated at their respective ∂qm m m ax ,r,q

ax ,r,q

ax ,r,q

variables are constants and hence do not alter the distribution of the noise process. Finally, assume that br , bq are such that Eq. (5.35) can be approximated as θ m = θ + vθ where, vθ is a zero-mean, bandlimited AWGN process of covariance denoted by σθ2 .

39

(5.36)

Roll Measurement ϕm : Recall the roll attitude measurement in Eq. (5.22) as ( −1

ϕm = tan |

) aym − v˙ E + pm wE − rm uE azm − w˙ E + qm uE − pm v E {z }

(5.37)

fϕ

Consider a first order Taylor series approximation of Eq. (5.37) about ay , az , p, r, q as ( ) (aym − ay + ay ) − v˙ E + (pm − p + p) wE − (rm − r + r) uE −1 ϕm = tan (az − az + az ) − w˙ E + (qm − q + q) uE − (pm − p + p) v E ) ( m E ay − v˙ + pwE − ruE −1 ≈ tan az − w˙ E + quE − pv E | {z } ϕ ∂fϕ ∂fϕ (ay − ay ) + (az − az ) + ∂aym ay ,az ,p,r,q m ∂azm ay ,az ,p,r,q m ∂fϕ ∂fϕ ∂fϕ + (pm − p) + (rm − r) + (qm − q)(5.38) ∂pm ∂rm ∂qm ay ,az ,p,r,q

ay ,az ,p,r,q

ay ,az ,p,r,q

Furthermore, from Eqs. (5.3), (5.4), (5.5), (5.16) and (5.17), we have aym − ay azm − az pm − p rm − r qm − q

= = = = =

way waz b p + wp b r + wr b q + wq

(5.39) (5.40) (5.41) (5.42) (5.43)

where, bp , bq , br do not include b0p , b0q , b0r respectively. Substituting Eqs. (5.39) through (5.43) into Eq. (5.38) yields ∂fϕ ∂fϕ ϕm = ϕ + wa + wa ∂aym ay ,az ,p,r,q y ∂azm ay ,az ,p,r,q z {z } | {z } | β2 β1 ∂fϕ ∂fϕ ∂fϕ + (bp + wp ) + (br + wr ) + (bq + wq ) ∂pm ay ,az ,p,r,q ∂rm ay ,az ,p,r,q ∂qm ay ,az ,p,r,q | | | {z } {z } {z } β3

β4

β5

⇒ ϕm = ϕ + β1 way + β2 waz + β3 (bp + wp ) + β4 (br + wr ) + β5 (bq + wq )

(5.44)

Notice that the terms β1 , β2 , β3 , β4 and β5 once evaluated at their respective variables are constants and hence do not alter the distribution of the noise process. Finally, assume that bp , br , bq are such that Eq. (5.44) can be approximated as ϕm = ϕ + vϕ 40

(5.45)

where, vϕ is a zero-mean, bandlimited AWGN process of covariance denoted by σϕ2 . Similarly, for ψ we have ψm = ψ + vψ

(5.46)

where, vψ is a zero-mean, bandlimited AWGN process of covariance denoted by σψ2 . 5.2

EKF Based AHRS Model

The final model for the design of an EKF for obtaining the attitude and heading of an MAV via an AHRS formulation is obtained by formulating the EKF state vector as an augmentation of the 3 Euler angle states (ψ, θ, ϕ) with the constant rate gyroscope bias terms (b1p , b1q , b1r ). The measurement model comprises the pitch and roll measurements obtained from Eqs. (5.21) and (5.22) and heading derived from a triaxial magnetometer on board the MAV. Remark 5.9. In this thesis, the effects of the bias components due to colored noise process (Remark 5.4) are not explicitly modeled in the filter model but rather treated as unmodeled bias components. This is because augmenting all bias sources as states in the filter model without additional measurements can lead to loss of observability and adversely affect the filter stability. Thus b1p , b1q , b1r are included as states in the filter model while b2p , b2q , b2r are treated as unmodeled states, i.e. the effects due to b2p , b2q , b2r are seen in the output of the rate gyroscope but are not captured in the filter process model. With Remark 5.9 and using the rate gyroscope measurements for the values of p, q, r in Eq. (5.19), the EKF process and measurement model can be written as (unmodeled ˙ θ, ˙ ϕ˙ via pm , qm , rm and is not explicitly written): dynamics affects ψ, EKF-AHRS Continuous Process Model ( ) ( ) ( ) ( ) ( ) ( ) s c s c s cϕ ϕ ϕ ϕ ϕ ϕ ψ˙ = qm + rm + b1q + b1r + wq + wr cθ cθ cθ cθ cθ cθ θ˙ = qm cϕ − rm sϕ + b1q cϕ − b1r sϕ + wq cϕ − wr sϕ

(5.47)

ϕ˙ = pm + qm sϕ tθ + rm cϕ tθ + b1p + b1q sϕ tθ + b1r cϕ tθ + wp + wq sϕ tθ + wr cϕ tθ

(5.49)

b˙ 1p = w1p b˙ 1q = w1q b˙ 1r = w1r zk1m zk2m zk3m

(5.48) (5.50) (5.51) (5.52)

EKF-AHRS Discrete Measurement Model = ψmk = ψk + vψk = θmk = θk + vθk = ϕmk = ϕk + vϕk

where, ψk , θk , ϕk represent the discrete counterparts of ψ, θ, ϕ.

41

(5.53) (5.54) (5.55)

5.2.1

Model & EKF State Vector, Nonlinear Dynamics Map and Jacobian: ˆ, f , F x, x

The state vector and the nonlinear dynamics map for the EKF based AHRS formulation are ( ) ( ) ( ) ( )       cϕ sϕ c sϕ ˆ ψ ψ qm cθ + rm cθ + b1q cθ + b1r cϕθ   θˆ    θ      qm cϕ − rm sϕ + b1q cϕ − b1r sϕ       ˆ   ϕ  ϕ    p + q s t + r c t + b + b s t + b c t   m m ϕ θ m ϕ θ 1 1 ϕ θ 1 ϕ θ p q r ˆ =  ˆ , f =  x =  , x  (5.56)    b1p   0  b1p       b1q    ˆb1q   0 b1r ˆb1r 0 where, the hatted quantities denote the estimates of the corresponding state variables. The ∂f nonzero entries of the Jacobian matrix F = ∂x x=ˆx in Eq. (4.33) is given by

F [1, 2]

F [1, 3] F [1, 5] F [1, 6]

Jacobian: First Row ( ) ) ) s ˆ (( = 2θ qm + ˆb1q sϕˆ + rm + ˆb1r cϕˆ cˆ (θ ) ( ) qm + ˆb1q cϕˆ − rm + ˆb1r sϕˆ = cθˆ sϕˆ = cθˆ cϕˆ = cθˆ

Jacobian: Second Row (( ) ( ) ) F [2, 3] = − qm + ˆb1q sϕˆ + rm + ˆb1r cϕˆ F [2, 5] = cϕˆ F [2, 6] = −sϕˆ Jacobian: Third Row ( ) ( ) ˆ ˆ qm + b1q s ˆ + rm + b1r c ˆ ϕ

F [3, 2] = (( F [3, 3] = tθˆ

ϕ

c2θˆ

) ( ) ) ˆ ˆ qm + b1q cϕˆ − rm + b1r sϕˆ

F [3, 4] = 1 F [3, 5] = sϕˆtθ F [3, 6] = cϕˆtθ ˆ where, sϕˆ ⇒ sin ϕ,

ˆ sθˆ ⇒ sin θ,

(5.57)

ˆ cϕˆ ⇒ cos ϕ,

42

ˆ cθˆ ⇒ cos θˆ and tθˆ ⇒ tan θ.

5.2.2

Process Noise Vector and Continuous Process Noise Scaling Matrix: w, Γc

The process noise vector in Eq. (4.29) and the continuous process scaling matrix in Eq. (4.33) are given by   cϕˆ sϕˆ   0 0 0 0 wp cθˆ cθˆ    wq   0 cϕˆ −sϕˆ 0 0 0       wr   1 sϕˆtθˆ cϕˆtθˆ 0 0 0    w = (5.58)   , Γc =    0 0 0 1 0 0  w1 p     w1q   0 0 0 0 1 0  w1 r 0 0 0 0 0 1 5.2.3

Process & Measurement Noise Intensity Matrices: Q, Rk

The process and measurement noise intensity matrices are  2  σ ˆp 0 0 0 0 0  0 σ  2  ˆq2 0 0 0 0    σ ˆ 0 0 2 ψ  0 0 σ ˆr 0 0 0   , Rk =  0 σ ˆθ2 0  Q = 2  0 0 0 σ  ˆ 0 0 1p   0 0 σ ˆϕ2  0 0 0 0 σ ˆ12q 0  0 0 0 0 0 σ ˆ12r

(5.59)

where, σ ˆψ , σ ˆθ and σ ˆϕ denote estimates of the standard deviations of ψm , θm , ϕm respectively, (ˆ σp , σ ˆq , σ ˆr ) and (ˆ σ1p , σ ˆ1q , σ ˆ1r ) denote the estimates of the standard deviations of the rate gyro bias models which are explained below Eq. (5.5) and in Remark 5.3 respectively. 5.2.4

Initial State Error Covariance Matrix: P (0)

The initial state error covariance matrix is given by [ ] ˆ ) (x − x ˆ )T P (0) = E (x − x )2  ( ˆ ψ−ψ 0 0 0  ( ) 2   0 θ − θˆ 0 0  ( )2   0 0 ϕ − ϕˆ 0  ( )2 ≈ E  ˆ 0 0 0 b − b 1p 1p   (  0 0 0 0 b1q   0 0 0 0 ˆ are given in Eq. (5.56). where, the representations for x and x

43

 0

0

0

0

0

0

0 − ˆb1q 0

0

)2 (

0 b1r − ˆb1r

        (5.60)      )2 

ˆ − , hk , Hk Predicted & Actual Measurement and Measurement Matrix: h k

5.2.5

The AHRS predicted and actual measurement vectors are respectively given as  ˆ−    ψ ψ m k k ( −) ˆ k =  θˆk−  h x and h (xk ) =  θmk  − ϕmk ϕˆk | {z } | {z }

(5.61)

actual measurement vector

predicted measurement vector

where, k denotes the discrete time index. Since the measurements for the AHRS problem are linear in the state vector (Eqs. (5.53), (5.54) and (5.55)), the Jacobian matrix Hk is a constant (independent of the predicted state) given by   1 0 0 0 0 0 Hk =  0 1 0 0 0 0  (5.62) 0 0 1 0 0 0 5.3

EKF Implementation

The attitude measurements in Eqs. (5.21) and (5.22) are implemented as follows: ( ) ( )   E E axmk + rmk−1 + ˆb1rk−1 vˆk−1 − qmk−1 + ˆb1qk−1 w ˆk−1  θmk = sin−1  g ( ) ( )   E aymk + pmk−1 + ˆb1pk−1 wˆk−1 − rmk−1 + ˆb1rk−1 uÊ k−1  ( ) ( ) ϕmk = tan−1  E ˆ azmk + qmk−1 + ˆb1qk−1 uÊ − p + b v ˆ mk−1 1pk−1 k−1 k−1

(5.63)

(5.64)

E E where, ˆb1pk−1 , ˆb1qk−1 , ˆb1rk−1 denote the online estimates of the rate gyroscopic bias and uÊ ˆk−1 ,w ˆk−1 k−1 , v are obtained as described in Remark 5.7 as follows:  E    x˙ gpsk−1 + wxgpsk−1 uˆk−1 E ˆB  vˆk−1  = R  y˙ gpsk−1 + wygps  (5.65) Ik−1 k−1 E wˆk−1 z˙gpsk−1 + wzgpsk−1

ˆ B is computed as follows: where, R Ik−1 

ˆB R Ik−1

 cθˆk−1 cψˆk−1 cθˆk−1 sψˆk−1 −sθˆk−1 =  sϕˆk−1 sθˆk−1 cψˆk−1 − cϕˆk−1 sψˆk−1 sϕˆk−1 sθˆk−1 sψˆk−1 + cϕˆk−1 cψˆk−1 sϕˆk−1 cθˆk−1  (5.66) cϕˆk−1 sθˆk−1 cψˆk−1 + sϕˆk−1 sψˆk−1 cϕˆk−1 sθˆk−1 sψˆk−1 − sϕˆk−1 cψˆk−1 cϕˆk−1 cθˆk−1

Furthermore, the linear acceleration terms u˙ E , v˙ E , w˙ E are ignored, i.e., the MAV center of gravity is assumed completely unaccelerated with respect to the inertial frame [32]. The tan−1 function in Eq. (5.64) is implemented in MATLAB via the atan2 command. 44

Remark 5.10. Notice in Eqs. (5.63) and (5.64), the pitch and roll measurements at time index k are obtained by using the GPS velocities in the body frame and the rate gyroscope sensor outputs at time index k −1 which are corrected via the estimated bias, ˆb1pk−1 , ˆb1qk−1 , ˆb1rk−1 , at k − 1, while the accelerometer outputs at time index k are used. Typically, the pitch and roll measurements at k should be generated by all quantities at k. However, since we are interested in correcting the rate gyroscope outputs with the estimated bias (even in the update stage) and since at time index k the bias estimates will not yet be available, we rely on the rate gyroscope measurements at k − 1 corrected by the bias estimates at k − 1. Furthermore, E E are obtained as described in Remark 5.7 which require ,w ˆk−1 ˆk−1 since the quantities uÊ k−1 , v the estimates of ϕ, θ, ψ, which at k will not yet be available, we rely on the estimates of velocities at k − 1. 5.3.1

Alternatives Ways of Obtaining the Pitch and Roll Measurements

Remark 5.11. In order to avoid the time indexing issue mentioned in Remark 5.10, even though one could generate the pitch and roll measurements at time index k without correcting the rate gyroscope outputs for bias at k in the update stage, the velocities still can only be computed using the previous estimates of the Euler angles. This procedure will lead to the pitch and roll measurements being computed as follows: ( ) E E a + r v − q w x m m m k−1 k k−1 k k θmk = sin−1 (5.67) g ( ) E E a + p w − r u y m m m k−1 k k k−1 k ϕmk = tan−1 (5.68) E azmk + qmk uE − p mk vk−1 k−1 Remark 5.12. Instead of using the previous estimates of bias (at k−1) and velocity estimates at k − 1, it is possible to use the predicted estimates of bias and velocity and obtain the pitch and roll measurements as follows: ) E ) E ( (  − ˆb− w ˆk− axmk + rmk + ˆb− v ˆ − q + m 1qk 1rk k k −1   θmk = sin (5.69) g ) ) E ( (   − ˆb− uˆ−E w ˆ − r + aymk + pmk + ˆb− m 1rk 1pk k k k  ) ) ( ( ϕmk = tan−1  (5.70) E − − − −E ˆ ˆ azmk + qmk + b1q uˆk − pmk + b1p vˆk k

where, uˆ− ˆk− , wˆk− are obtained as: k ,v     E x˙ gpsk−1 + wxgpsk−1 uˆ− k  −E  ˆ −B  y˙gps + wygps   vˆk  = R k−1 Ik−1 k−1 −E z˙gpsk−1 + wzgpsk−1 w ˆk E

E

k

E

ˆ −B is based on the predicted estimates ψˆ− , θˆ− , ϕˆ− . where, R Ik−1 k k k 45

(5.71)

Remark 5.13. An alternative to Remarks 5.10 and 5.11 is to perform a smoothing at every time index k in the following manner: 1. Obtain the the pitch and roll measurements at time index k via accelerometer and rate gyroscope at k and velocities at k − 1. [ ]T ˆ k = ψˆk θˆk ϕˆk ˆb1pk ˆb1qk ˆb1rk 2. Compute the updated state estimate vector, x , at k. 3. With the computed state estimate vector at k repeat Step 1 with the following pitch and roll measurements: ( ) ( )   E ˆ ˆ ˆkE axmk + rmk + b1rk vˆk − qmk + b1qk w  θmk = sin−1  (5.72) g ( ) ( )   E ˆ ˆ aymk + pmk + b1pk wˆk − rmk + b1rk uÊ k −1  )  ( ) ( (5.73) ϕmk = tan ˆb1p vÊ − p + azmk + qmk + ˆb1qk uÊ m k k k k 4. With the measurements obtained as in Eqs. (5.72) and (5.73), again compute the [ ]T ˆ ˆ ˆ ˆ ˆ ˆ ˆ k = ψk θk ϕk b1pk b1qk b1rk updated state estimates at k, x . Remark 5.14. In this thesis, primary focus is given to implementing the EKF as described in Remark 5.10. This means that the pitch and roll measurements for the EKF update stage are based on Eqs. (5.63), (5.64), (5.65) and (5.66). 5.4

Consolidated Noise Table

The true and estimated (for the EKF) standard deviations for the rate gyroscope sensor model are shown in Table 5.2. Rate Gyro Model σ of AWGN (sampling noise) σ of Weiner process (Remark 5.3) σ of 1st order Gauss-Markov (Remark 5.4) τ of 1st order Gauss-Markov (Remark 5.4)

p-gyro σp σ1p σ2p τp

Truth q-gyro σq σ1q σ2q τq

r-gyro σr σ1r σ2r τr

p-gyro σ ˆp σ ˆ 1p NA NA

EKF q-gyro σ ˆq σ ˆ 1q NA NA

r-gyro σ ˆr σ ˆ 1r NA NA

Table 5.2: Rate Gyroscope noise & EKF standard deviations.

Remark 5.15. It can be seen that the values of the standard deviations for the design of the EKF (Q matrix) are deliberately assigned values different from those of the true standard deviations. This is done for 2 reasons: 46

1. The true standard deviations are obtained from the data sheet of the rate gyro manufacturer which need not be the same when these sensors are mounted on the processor board that eventually goes onboard the MAV. 2. It is important to check the robustness of the EKF with respect to such mismatches in order to capture the behavior seen in the real world. 5.4.1

EKF Pseudo Code with Sensors of Varying Rates: IMU, GPS, MAG

Let us assume that the update rate of the IMU and MAG (magnetometer) are given by fIM U = fM AG = fp Hz and the update rate of the GPS is fGP S Hz such that fGP S < fp . Thus the update time for the IMU and MAG is tp = f1p secs, while the GPS update time is 1 secs, where tGP S > tp . Let us also assume that the time of prediction (tpred ) tGP S = fGP S happens at a rate equal to the update rate of the IMU & MAG, i.e., tpred = tp secs. Denote the end of simulation time as Tf secs. Furthermore, we will assume that at the initial time instant all sensors are functioning. Then the EKF implementation for the AHRS problem can be shown, as a pseudo code, via the following steps: 1. Initialize the state vector and the state error covariance matrix as   ψˆ (tk−1 = 0)   θˆ (t k−1 = 0)     ˆ  ϕ (tk−1 = 0)  ˆ ˆ0 x (tk−1 = 0) =  ˆ =x  bp (tk−1 = 0)     ˆbq (tk−1 = 0)  ˆbr (tk−1 = 0) P (tk−1 = 0) = P0

(5.74)

(5.75)

2. At t = tk obtain an estimate of the predicted state vector as ( ˙x ˆ (tk−1 ) = f pˆm (tk−1 ) , qˆm (tk−1 ) , rˆm (tk−1 ) , ψˆ (tk−1 ) , θˆ (tk−1 ) , ϕˆ (tk−1 ) , ) ˆbp (tk−1 ) , ˆbq (tk−1 ) , ˆbr (tk−1 ) ˆ − (tk ) = x ˆ (tk−1 ) + x ˆ˙ (tk−1 ) ∆t, ⇒x

∆t = tp

(5.76)

where, pˆm (tk−1 ) = pm (tk−1 ) + ˆbp (tk−1 ), qˆm (tk−1 ) = qm (tk−1 ) + ˆbq (tk−1 ), rˆm (tk−1 ) = rm (tk−1 ) + ˆbr (tk−1 ), f is obtained from Eq. (5.56) and ∆t is the simulation time step. 3. At t = tk obtain an estimate of the predicted state error covariance matrix as P˙ (tk−1 ) = F (tk−1 )P (tk−1 ) + P (tk−1 )F T (tk−1 ) + Γc (tk−1 ) QΓTc (tk−1 ) ⇒ P − (tk ) = P (tk−1 ) + P˙ (tk−1 ) ∆t (5.77) where, F (tk−1 ), Γc (tk−1 ) are respectively obtained by evaluating Eqs. (7.63) and (5.58) at pˆm (tk−1 ) , qˆm (tk−1 ) , rˆm (tk−1 ), ψˆ (tk−1 ) , θˆ (tk−1 ) , ϕˆ (tk−1 ), ˆbp (tk−1 ) , ˆbq (tk−1 ) , ˆbr (tk−1 ) and Q is given by the first quantity of Eq. (5.59). 47

4. An ”if” loop to process measurements: IF rem (t, tGP S ) = 0 (if a GPS packet has arrived) ( )−1 Kk = P − (tk )HkT Hk P − (tk )HkT + Rk     ˆ− ψ (tk ) ψm (tk ) ˆ − (tk ) = x ˆk = x ˆ − (tk ) + Kk  θm (tk )  −  θˆ− (tk )  x ϕm (tk ) ϕˆ− (tk )

(5.78) (5.79)

P (tk ) = (I − Kk Hk ) P − (tk )   1 0 0 0 0 0 Hk =  0 1 0 0 0 0  0 0 1 0 0 0

(5.80) (5.81)

ELSE (use MAG reading to correct the estimate in ψˆ− (tk )) ( ) ¯ k −1 , Kk = P − (tk )HkT Hk P − (tk )HkT + R



1 2 σ ˆψ

¯ −1 =  0 R k 0

0 0



ϵ 0  0 ϵ

(5.82)

Solve Eqs. (5.79) ∼ (5.81) OR (Alternate approach when new GPS packet has NOT arrived) ( ) ¯ k = P − (tk )H ¯T H ¯ k P − (tk )H ¯ T + Rk (1, 1) −1 K k k [ ] ¯ k (1, 1) 0 0 0 0 0 T K Kk = ( ) − − − ˆ ˆ (tk ) = x ˆk = x ˆ (tk ) + Kk ψm (tk ) − ψ (tk ) x ( ) ¯ k P − (tk ) P (tk ) = I − Kk H [ ] ¯k = 1 0 0 0 0 0 H

(5.83) (5.84) (5.85) (5.86) (5.87)

END where, ψm (tk ) = ψmk , θm (tk ) = θmk , ϕm (tk ) = ϕmk and ϵ in Eq. (5.82) denotes a very small number so that the value of its inverse is an extremely large number so as to represent the fact that the measurements θm , ϕm that depend on GPS are in fact not to be trusted during the period when a new GPS packet is not available. Furthermore, θm (tk ) , ϕm (tk ) are based on Remark 5.14. The rem (·, ·) operator just above Eq. (5.78) is an operation which generates the remainder when tGP S divides t. Hence only if the remainder is 0 ⇒ a new GPS packet has arrived6 . 5. Store the state vector in a counter for display of graphs later on. 6

The term ”GPS packet” in simulation really implies whether the apriori determined GPS update time divides without remainder the simulation time. In reality, the arrival of a GPS packet is based on the value of a GPS flag.

48

6. Go to step 2 and repeat steps 2 through 5 for the next time instant and continue on till the end of simulation time, Tf . 5.5

Attitude and Heading Estimation for a Micro Aerial Vehicle: Model Formulation Using a Differential Pressure Sensor and No GPS

Thus far, we have looked at the AHRS estimation problem from a perspective of augmenting the INS sensor suite with a GPS receiver onboard the MAV. While such an augmentation approach is useful since it exploits the accuracy of the GPS in correcting the errors accumulated due to MEMS based INS sensors, the approach is disadvantaged due to the occurrence of intermittent GPS outages. In this section, we consider an AHRS augmenting approach where we do not rely on GPS to augment the existing INS sensor package, rather rely on a differential pressure sensor to provide a sense of the forward speed of the MAV.

Figure 5.3: Pitot Static tube. A differential pressure sensor setup is shown in a pitot-static tube in Figure 5.3 which are also called Prandtl tubes. The actual tube on a MAV is around 10 centimeters long with 1 centimeter diameter. Several small holes are drilled around the outside of the tube and a center hole is drilled down the axis of the tube. The outside holes are connected to one side of a device called a pressure transducer. The center hole in the tube is kept separate from the outside holes and is connected to the other side of the transducer. The transducer measures the difference in pressure in the two groups of tubes by measuring the strain in a thin element using an electronic strain gauge. Since the outside holes are perpendicular to the direction of flow, these tubes are pressurized by the local random component of the air velocity. The pressure in these tubes is the static pressure (ps ). The center tube, however, is pointed in the direction of travel and is pressurized by incoming air velocity and the pressure in this tube is the total pressure (pt ). The pressure transducer measures the difference in total and static pressure which is the dynamic pressure (q). 49

With the difference in pressures measured and knowing the local value of air density from pressure and temperature measurements, we can use Bernoulli’s equation to give us the velocity. Bernoulli’s equation states that the static pressure plus total pressure is equal to the total pressure. 1 2 pt = ps + ρVtas 2 2 (p − ps ) t 2 Vtas = ρ √ 2 (pt − ps ) Vtas = ρ

(5.88)

(5.89)

where ρ and Vtas represent air density and true air speed of a MAV. 5.5.1

Differential Pressure Sensor Model

Here Vtas is treated as the output of a differential pressure sensor, thus the ifferential pressure sensor measurement Vtas is assumed to be modeled as: Vtasm (t) = Vtas (t) + wv

(5.90)

where Vtas (t) is the true value of the true air speed; wv denote the error due to sampling noise which are typically modeled as zero mean, bandlimited, additive, white Gaussian noise (AWGN) processes of specified covariance denoted by σv2 . 5.5.2

Attitude Measurements: Pitch (θm ) & Roll (ϕm )

Here the AHRS measurement model is similar to the one presented in Section 5.1.5; but the lateral component velocities are assumed to be zero as there is no measurement source. And the true air speed (Vtas ) obtained form differential pressure sensor is treated as the forward speed of MAV. Thus the terms in Eq. (5.20) (v E , wE ) are assumed to be zero and uE = Vtas . From simple trigonometric relations and using Eq. (5.20), the pitch and roll measurements can be obtained from the triaxial accelerometer, triaxial rate gyroscope and differential pressure sensor as: ( ) aym − rm Vtas −1 ϕm = tan (5.91) azm + qm Vtas ( )  ˙ sin ϕ axm − V  θm = tan−1  (5.92) −aym + rm Vtas

50

Remark 5.16. V˙ can be obtained by a finite differencing of Vtas , given the DPS update rate.7 Heading Measurement: Yaw (ψmag ) The heading measurement is modeled in similar way as presented in Section 5.1.5. The MAV true heading can then be calculated as ψmag = ψmh + mv

(5.93)

where, mv denotes the magnetic variation. A description of heading, track, course and magnetic variation is given in Appendix 3. The process model for attitude and heading estimation using differential pressure sensor is similar to the model presented in Section 5.1.4. The state vector and measurement vector are similar to the model presented in Section 4.2; but the measurement model is given by the Eqs. 5.91 ∼ 5.93.

7

If the DPS obtained velocity is prohibitively noisy, then their low pass filtered (or smoothed) outputs can be used for the finite differencing instead.

51

Chapter 6 GPS Based Attitude and Heading Estimation for a Micro Aerial Vehicle: Simulation Results In Chapter 5, a mathematical model for the estimation of attitude and heading of a micro aerial vehicle (MAV) was developed along with the appropriate sensor models (rate gyroscope and accelerometer). In the case of rate gyroscopes, bias errors (constant & time varying) and zero mean, bandlimited, additive, white Gaussian noise (AWGN) processes were considered while the accelerometer output was assumed to corrupted by zero mean, bandlimited AWGN processes. In this chapter, open-loop simulation results for the model developed in Chapter 5 for MAV attitude and heading estimation are shown for a suite of MAV maneuvers. Furthermore, the impact of unmodeled sensor bias (rate gyroscope) on the performance of the extended Kalman filter (EKF) is studied in addition to the case with only modeled sensor bias (rate gyroscope). 6.1

Simulated MAV Trajectories

A 6 degree-of-freedom (DOF) model adapted from the open-source Flight Dynamics and Control toolbox (FDC) [33] is used for the simulation study. Turbulence is introduced to the model via the Dryden model [34]. The trajectories shown in Figure 6.1 are the trajectories that are considered for MAV attitude estimation. The following cases are studied which are presented in the form of 2 remarks given below (Remarks 6.1 and 6.2). Remark 6.1. )Modeled Sensor Bias: The output of the rate gyroscope is corrupted by (wp , wq , wr ) ( & b1p , b1q , b1r . Remark 6.2. Modeled(& Unmodeled Bias:) The output of the rate gyroscope is cor) Sensor ( rupted by (wp , wq , wr ), b1p , b1q , b1r & b2p , b2q , b2r . Remark 6.3. It is to be noted that in either of the above cases, modeled and modeled+unmodeled gyro bias, only b1p , b1q , b1r (rate gyro bias effect due to rate random walk process) are modeled as states in the EKF.

52

Trajectory Start (o); Trajectory Finish (x)

205

Alt (m)

200

195

190

185 200 0

1500 1000

−200

Y (m)

500

−400

0 −600

−500

X (m)

(a) Steady Level Flight


205

Alt (m)

200

195

190

185 200 0

1500 1000

−200

Y (m)

500

−400

0 −600

−500

X (m)

(b) Steady Level Flight: Turbulence

Figure 6.1: Simulated MAV Trajectories. 6.2

Simulation Parameters

This section presents the true & EKF initial conditions, the sensor noise parameters and the EKF tuning matrix parameters, process noise intensity matrix, measurement noise intensity matrix and initial state error covariance matrix, (Q, Rk , P (0)). 53

6.2.1

Initial Conditions: Model & Filter

The model and filter initial conditions (for Figures 6.1(a) & 6.1(b)) are        ψˆ (0) ψ (0) 0  θˆ (0)    θ (0)   3.62◦          ˆ   ϕ (0)   0   ϕ (0)   = , ˆ x (0) =  x (0) = ˆ =  b1p (0)   0   b1p (0)           b1q (0)   0   ˆb1q (0)   b1r (0) 0 ˆb1r (0) {z } | {z | Model Initial Condition

6.2.2

0 0 0 0 0 0

       

(6.1)

}

EKF Initial Condition

Sensor Noise Parameters

The true sensor noise parameters for the accelerometer, rate gyroscope and the GPS are given in Table 6.1 below: Sensor Type Rate Gyroscope Rate Gyroscope Rate Gyroscope Rate Gyroscope Accelerometer Magnetometer GPS

Noise Type Sampling Noise (Std. Dev.) Weiner Process (Std. Dev.) Colored Noise (Std. Dev.) Colored Noise (Time constant) Sampling Noise (Std. Dev.) Sampling Noise (Std. Dev.) Velocity Sampling Noise (Std. Dev.)

Notation σp , σq , σr σ1p , σ1q , σ1r σ2p , σ2q , σ2r τp , τq , τr σax , σay , σaz vψ σxgps , σygps , σzgps

Value 0.05◦ /s 0.05◦ /s 0.05◦ /s 100/s 0.1 m/s2 0.5◦ /s 0.1 m/s

Table 6.1: Sensor noise standard deviations (Std. Dev.).

Figure 6.2: Simulating Colored Noise (1st Order Gauss-Markov Model). 54

6.2.3

EKF Parameters: Q, Rk , P (0)

In keeping with Remark 5.15, the EKF estimates of the standard deviations below Eq. (5.5) and in Remark 5.3 are given as σ ˆp = σ ˆq = σ ˆr = 0.001◦ /s ˆ1q = σ ˆ1r = 0.001◦ /s σ ˆ 1p = σ

(6.2) (6.3)

Furthermore, the estimates for the standard deviations of the derived measurements of ψ, θ, ϕ (Eqs. (5.53) to (5.55)) are chosen as σ ˆψ = σ ˆθ = σ ˆϕ = 0.5◦ /s

(6.4)

From Eqs. (6.2), (6.3) and (6.4), the Q and Rk matrices in Eq. (5.59) can be written as Q = 0.0012 · I6×6 ,

Rk = 0.52 · I3×3

(6.5)

where, I6×6 and I3×3 denote identity matrices of dimension 6 × 6 and 3 × 3 respectively. Finally, the EKF estimate of the initial state error covariance matrix, P (0), is given by   (5◦ )2 0 0 0 0 0  0  (5◦ )2 0 0 0 0     0 (5◦ )2 0 0 0  0  P (0) = E  (6.6)  2 ◦  0  0 0 (0.5 /s) 0 0    0  0 0 0 (0.5◦ /s)2 0 2 ◦ 0 0 0 0 0 (0.5 /s) Remark 6.4. For the cases studied in Remarks 6.1 and 6.2, the filter parameters (P (0), Q, R, x(0)) remain unchanged. 6.3

Simulation Scenario: Modeled Sensor Bias - NO GPS Outage

For the MAV maneuvers shown in Figures 6.1(a) and 6.1(b), the following cases are studied: 1. No sensor bias in the output of the rate gyroscope. 2. Sensor bias in the output of the rate gyroscope but not accounting for it in the EKF. 3. Sensor bias in the output of the rate gyroscope and accounting for it in the EKF. The IMU measurements into EKF and their corresponding true data in presented in Figure 6.3. Figures 6.3(a) and 6.3(b) show the measurement and truth corresponding to MAV trajectories shown in Figures 6.1(a) and 6.1(b).

55

0

0

400

0

400

0

r (deg/s)

2

−10

0

200

400

200 400 Time (secs)

0

(a) (ax , ay , az ), (p, q, r) for Figure 6.1(a)

200

400

0

200

400

0

200 400 Time (secs)

0.01 0

p (deg/s)

−0.01 0

200

400

−10 −11

200 400 Time (secs)

0

0.02

−9

0.5 0 −0.5 −1 −1.5

0 −0.01

400

0 −0.5

az (m/s2)

200

200

y

y

0.5 −0.5

0

0

0.5 a (m/s2)

q (deg/s)

2

a (m/s )

200

1

−9 az (m/s )

0.5

q (deg/s)

400

0.01

r (deg/s)

200

0

−11

0 −0.5

0

0.5

−0.5

ax (m/s2)

p (deg/s)

0.5 0

1

0.5

2

ax (m/s )

1

0

200 400 Time (secs)

0.01 0 −0.01 −0.02 −0.03

(b) (ax , ay , az ), (p, q, r) for Figure 6.1(b)

Figure 6.3: Accelerometer & Rate Gyroscope: Truth (Solid); Measurement (Dots).

Rate Gyroscope: Colored Noise Bias

−3

b2 (deg/s)

5

x 10

0

p

0

p

b (deg/s) 1

Rate Gyroscope: Rate Random Walk Bias 0.2

−0.2

0

100

200

300

400

500

−5

0

100

200

300

400

500

100

200

300

400

500

100

200

300

400

500

b2 (deg/s)

0.2

2

x 10

0

q

0

q

b (deg/s) 1

−3

−0.2

0

100

200

300

400

500

−2

0

b2 (deg/s)

0.2

1

x 10

0

r

0

r

b (deg/s) 1

−3

−0.2

0

100

200

300

400

500

Time (secs)

−1

0

Time (secs)

(a) (b1p , b1q , b1r )

(b) (b2p , b2q , b2r )

Figure 6.4: Rate Gyroscope Bias Plots: Rate Random Walk & Colored Noise. The rate gyroscope bias (b1p , b1q , b1r ) and (b2p , b2q , b2r ), which are modeled as wiener process and first order Guass-Markov process are simulated and shown in Figure 6.4(a) and 6.4(b) respectively. The Magnitude and phase plots for wiener process and first-order Guass-Markov process are shown in Figure 6.5.

56

Magnitude (dB)

Bode Diagram 100 80 60 40 20 0 −20 −40 −60 −80 −4 10

First Order Gauss−Markov Model Rate Random Walk (Integrating White Noise)

−3

−2

10

−1

10

0

10

1

10

10

Phase (deg)

10 0

−45

−90 −4

−3

10

−2

10

−1

0

10 10 Frequency (rad/sec)

1

10

10

Figure 6.5: Rate Gyroscope Bias Model Bode Plots.

Sampling noise (AWGN)

wp (deg/s)

0.2

wp (deg/s)

400 0

−0.2

200

0

100

200

300

400

0 −0.2

500

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2

wq (deg/s)

0.2

wq (deg/s)

400 0

−0.2

200

0

100

200

300

400

0 −0.2

500

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2

wr (deg/s)

0.2

w (deg/s)

400 0

−0.2

r

200

0

100

200

300

400

0 −0.2

500

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

Time (secs)

(a) Rate Gyroscope Noise: Signals)

(b) Rate Gyroscope Noise: Histogram

Figure 6.6: Rate Gyroscope Noise Plots: Signals & Histogram.

57

0.2

Accelerometer Noise (AWGN)

wa (m/s2)

0.5

w (m/s2) a

400

x

0

x

200

−0.5

0

100

200

300

400

0

500

−0.2

−0.1

0

0.1

0.2

0.3

wa (m/s )

0.5 2

2

400 200

y

−0.5

w (m/s ) a y

0

0

100

200

300

400

0

500

−0.2

−0.1

0

0.1

0.2

0.3

wa (m/s2)

400

z

0

200

z

w (m/s2) a

0.5

−0.5

0

100

200

300

400

0

500

−0.2

−0.1

0

0.1

0.2

0.3

Time (secs)

(a) Accelerometer Noise: Signals

(b) Accelerometer Noise: Histogram

Figure 6.7: Accelerometer Noise Plots: Signals & Histogram.

GPS Velocity (Inertial) Noise (AWGN)

(m/s)

0.5

w

xGPS

(m/s)

200

wx

GPS

400 0

−0.5

0

100

200

300

400

0 −0.4

500

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

(m/s)

0.5

wyGPS (m/s)

400 200

w

y

GPS

0 −0.5

0

100

200

300

400

0 −0.4

500

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

(m/s)

0.5

wzGPS (m/s)

200

wz

GPS

400 0

−0.5

0

100

200

300

400

0 −0.4

500

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

Time (secs)

(a) GPS Velocity Noise: Signals

(b) GPS Velocity Noise: Histogram

Figure 6.8: GPS Velocity Noise Plots: Signals & Histogram. The noise signals and Histogram plots for rate gyroscope, accelerometers and GPS are shown in Figure 6.6, 6.7 and 6.8 respectively. The standard deviation for these noise signals is shown in the respective histogram’s.

58

Truth (Solid); AHRS Measurement (Dotted)

Truth (Solid); AHRS Measurement (Dotted) 400

ψ (deg)

ψ (deg)

400 200 0

0

100

200

300

400

0

500

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

6

θ (deg)

θ (deg)

6 4 2 0

100

200

300

400

0 −5 −10 −15 0

100

200

300

400

4 2

500

φ (deg)

φ (deg)

200

500

Time (secs)

0 −5 −10 −15

Time (secs)

(a) ψ, θ, ϕ for Figure 6.1(a)

(b) ψ, θ, ϕ for Figure 6.1(b)

Figure 6.9: AHRS Measurements in degree: Truth (Solid); Measurement (Dots).

59

6.3.1

Case 1: MAV Attitude & Heading Estimation without Sensor Bias

Here Rate gyroscopes are assumed to be unbiased and are only corrupted with white Gaussian noise. Thus there is no need for estimating bias in gyroscopes, which leads to estimating only 3 − states with 3 − measurements (ψ, θ, ϕ). Thus the state vector for EKF is given as:   ψ x =  θ  (6.7) ϕ In each of the figures shown in Figures 6.10(a), 6.10(b), 6.10(c) and 6.10(d), the dashed lines correspond to ±0.1◦ .

θ (deg)

100

200

300

400

500

−0.2

4

0.2

3.5

0

3

0

100

200

300

400

500

−0.2

ψ (deg)

0

0

Truth (Solid); Estimate (Dotted)

0

100

200

300

400

0

100

200

300

400

100

200

300

400

500

−0.2

0

100

Time (secs)

200

300

400

0 100

200

300

400

0.2 0 0

100

200

300

400

500

0

−0.2

0

100

200

300

400

500

φ (deg)

100

200

300

400

100

200

300

400

0

3

−0.2

0

100

200

300

400

500

100

200

300

400

500

0 0

100

200

300

400

500

−0.2

0

100

Time (secs)

0

100

200

300

400

500

−0.2

0

100

200

300

400

500

Time (secs)

200

300

400

500

0 −0.2

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0.2

3.5

0

3

−0.2

0

100

200

300

400

500

0.2

0 −5 −10 −15

0 0

Time (secs)

Estimation Errors

0

4

0

500

0.2

0

0.2

0 −5 −10 −15

500

200

500

0.2

3.5

400


θ (deg)

θ (deg)

4

300

0

400

0

200

(b) GPS 4 Hz

ψ (deg)

0

100

Time (secs)

φ (deg)

ψ (deg)

200

−0.2

0

0.2

0

Estimation Errors 0.2

500

0 −5 −10 −15

(a) GPS 1 Hz Truth (Solid); Estimate (Dotted)

−0.2

4

Time (secs)

400

500

3.5 3

0 0

200 0

0.2

0 −5 −10 −15

0.2

500

500

Estimation Errors

400

0

θ (deg)

200 0

φ (deg)


φ (deg)

ψ (deg)

Truth (Solid); Estimate (Dotted) 400

100

200

300

400

500

−0.2

Time (secs)

(c) GPS 5 Hz

Time (secs)

(d) GPS 10 Hz

Figure 6.10: EKF Performance in Estimating Euler Angles (deg) - MAV Trajectory in Figure 6.1(a).

60

At GPS update rates (1Hz, 4Hz, 5Hz, 10Hz) and without any sensor bias in rate gyroscopes it can be seen from Figures 6.10(a), 6.10(b), 6.10(c) and 6.10(d) that the estimates of position, velocity and Euler angles clearly track the respective truth data. And the estimation errors are observed to lie with in ±0.1◦ .

0 0

100

200

300

400

500

200

0

100

200

300

400

500

0

0.2

4

0

100

200

300

400

500

−0.2

0

100

200

300

400

0 0

100

200

300

400

500

−0.2

200

300

400

0

100

Time (secs)

200

300

400

0

100

200

300

400

−0.2

0

100

200

300

400

500

φ (deg)

200

300

400

0

100

200

300

400

500

−0.2

100

200

300

400

0

100

100

200

300

400

500

0

100

Time (secs)

−0.2

100

200

300

400

500

0

100

200

300

400

500

Time (secs)

200

300

400

500

200

300

400

500

0 −0.2

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0.2

4

0

3 0

100

200

300

400

500

−0.2

0.2

0 −5 −10 −15

0 0

Time (secs)

Estimation Errors

0

2

0 −0.2

0

0.2

0

0.2

0 −5 −10 −15

500

5

0

−0.2

200

500

0

3 2

100

0.2

4

500


θ (deg)

θ (deg)

5

400

0

400

0

300

(b) GPS 4 Hz

ψ (deg)

0

0

200

Time (secs)

φ (deg)

ψ (deg)

200

100

0.2

0


500

0 −5 −10 −15


0

0

3

500

−0.2

0.2

Time (secs)

400

500

4 2

500

0.2

0 −5 −10 −15

100

5

0

3 2

0.2

0 0

Estimation Errors

400

θ (deg)

θ (deg)

5

−0.2

Truth (Solid); Estimate (Dotted) ψ (deg)

200 0

φ (deg)


φ (deg)

ψ (deg)


100

200

300

400

500

−0.2

Time (secs)

(c) GPS 5 Hz

Time (secs)

(d) GPS 10 Hz

Figure 6.11: EKF Performance in Estimating Euler Angles (deg) - MAV Trajectory in Figure 6.1(b). For the MAV trajectory shown in Figure 6.1(b), the Euler angle estimates when there is no bias in rate gyroscopes with various GPS update rates are tracking the truth data which implies the proposed EKF algorithm is robust even if turbulence occurs in a maneuver.

61

Case 2: MAV Attitude & Heading Estimation with Sensor Bias but not accounting for it in the EKF ( ) Here the output of the rate gyroscope is corrupted by (wp , wq , wr ) & b1p , b1q , b1r , but the sensor bias is not modeled in EKF, which leads to estimating 3 − states with 3 − measurements while the measurements are biased. 6.3.2

0

0

−20

100

200

300

400

500

0

100

200

300

400

0

0

−20

−20

0

100

200

300

400

500

50 0

0 −20

0

100

200

300

400

500

−20

100

200

300

400

0

100

200

300

400

500

0

100

200

300

400

500

0

0

−20

−20

0

100

200

300

400

0

0 −20

0

100

200

300

400

0

100

200

300

400

500

ψ (deg) 100

200

300

400

0

0

−10

−20

0

100

200

300

400

500

0

0 −20

0

100

100

200

300

400

500

0 −20

100

200

300

400

500

0

200

300

400

500

0

100

200

300

400

500

200

300

400

500

100

200

300

400

500

Time (secs)

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

20

0

0

−20

−20

0

100

200

300

400

500

50

Time (secs)

100

Estimation Errors

0 0

20

−20

0

20

20

0

500

Time (secs)

200

500

20

20

φ (deg)

0

400


θ (deg)

θ (deg)

10

500

400

φ (deg)

ψ (deg)

0

300

(b) GPS 4 Hz Estimation Errors

−20

200

Time (secs)

20

0

500

−20

(a) GPS 1 Hz

200

100

20

Time (secs)


0

20

20

Time (secs)

400

500

20

20

−50

0

0 0

20

Estimation Errors 20

200

500

θ (deg)

θ (deg)

20


ψ (deg)

200

0

φ (deg)


φ (deg)

ψ (deg)


20

0

0

−50

−20

0

100

200

300

400

500

Time (secs)

(c) GPS 5 Hz

Time (secs)

(d) GPS 10 Hz

Figure 6.12: EKF Performance in Estimating Euler Angles (deg) - MAV Trajectory in Figure 6.1(a). Figure 6.12 shows the Euler angle estimates for the MAV trajectory shown in Figure 6.1(a). As the measurements are biased and as the rate gyroscope bias is not accounted in the EKF implementation, the Euler angle estimates are observed to be drifting away with respect to the truth data. It is seen from the Figures 6.12(a), 6.12(b), 6.12(c), and 6.12(d) that EKF is able to reject the noise completely but the estimates are biased as they are not modeled in the filter implementation. 62

0

0

−20

100

200

300

400

500

0

100

200

300

400

0

0

−20

500

0

20

0

0

−50

−20

0

100

200

300

400

500

50 0

0 −20

0

100

200

300

400

500

200

300

400

100

200

300

400

500

0

100

200

300

400

500

0

0 −20

0

100

200

300

400

0

0 −20

0

100

200

300

400

−20

0

100

200

300

400

500

0

0

−20

−20

0

100

200

300

400

500

100

200

300

400

0

0 −20

0

100

100

0 −20

0

200

300

400

500

100

200

300

400

500

0

200

300

400

500

0

100

200

300

400

500

200

300

400

500

100

200

300

400

500

Time (secs)

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

20

0

0

−5

−20

0

100

200

300

400

500

20

Time (secs)

100

Estimation Errors

0 500

20

−50

0

20

5

0

500

Time (secs)

200

20

50

φ (deg)

0

400


θ (deg)

θ (deg)

20

500

400

ψ (deg)

0

300

(b) GPS 4 Hz

φ (deg)

ψ (deg)

0

200

Time (secs)

Estimation Errors

200

500

−50

(a) GPS 1 Hz 20

100

20

Time (secs)


0

20

50

Time (secs)

400

500

−10

20

−50

100

10

0


200

θ (deg)

θ (deg)

50


ψ (deg)

200

0

φ (deg)


φ (deg)

ψ (deg)


20

0

0

−20

−20

0

100

200

300

400

500

Time (secs)

(c) GPS 5 Hz

Time (secs)

(d) GPS 10 Hz

Figure 6.13: EKF Performance in Estimating Euler Angles (deg) - MAV Trajectory in Figure 6.1(b). Figure 6.13 shows the Euler angle estimates for the MAV trajectory shown in Figure 6.1(b). It is seen from the Figures 6.13(a), 6.13(b), 6.13(c), and 6.13(d) that the Euler angle estimates are drifting away with respect to the truth data. The Euler angle estimates are observed to be biased because the sensor bias is not modeled and corrected in the EKF implementation.

63

Case 3: MAV Attitude & Heading Estimation with Sensor Bias and accounting for it in the EKF ( ) Here the output (of the rate) gyroscope is corrupted by (wp , wq , wr ) & b1p , b1q , b1r , and the ( sensor bias b)1p , b1q , b1r is modeled in EKF, which leads to estimating 6 − states ψ, θ, ϕ, b1p , b1q , b1r with 3 − measurements (ψm , θm , ϕm ). 6.3.3

Estimation Errors

200

0

0

−2

0

100

200

300

400

500

θ (deg)

5

0

100

200

300

400

0

0

−2

500

0

0

100

200

300

400

500

−2

100

200

300

400

500

5

0

3

0

100

200

300

400

500

0

100

200

300

400

500

0

100

Time (secs)

0

100

200

300

400

500


200

300

400

500

0

−20

−2

0

100

200

300

400

0

100

200

300

400

500

θ (deg)

5

2

ψ (deg) 0

100

200

300

400

0

100

200

300

400

500

−2

0 −2

100

200

300

400

500

5

0

100

200

300

400

500

0

100

200

300

400

500

500

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

3 0

100

200

300

400

500

−2

2

φ (deg)

φ (deg)

400

2

4 2

0 −2

300

Estimation Errors

0

2

−20

200

2

0

0 −10

100

Time (secs)

200

500

0

3

0


2

4

500

400

θ (deg)

ψ (deg)

0

500


−2

400

Time (secs)

2

0

−2

−10

(a) GPS 1 Hz

200

300

0

Time (secs)

400

200

2

φ (deg)

φ (deg)

0 −2

100

0

3

2

−20

0

2

4 2

0 −10

Estimation Errors 2

200

2

4 2


ψ (deg)

2

θ (deg)

ψ (deg)


0

100

Time (secs)

200

300

400

0

−20

500

Time (secs)

0

−10 0

100

200

300

400

500

−2

Time (secs)

(c) GPS 5 Hz

Time (secs)

(d) GPS 10 Hz

Figure 6.14: EKF Performance in Estimating Euler Angles (deg) - MAV Trajectory in Figure 6.1(a). Figure 6.14 shows the Euler angle estimates for the MAV trajectory shown in Figure 6.1(a). As the rate gyroscope bias is modeled in the EKF, this allows to estimate the bias and correct the rate gyroscope measurement online. Thus it is seen from the Figures 6.14(a), 6.14(b), 6.14(c), and 6.14(d) that the errors between the estimates and truth of the MAV attitude and heading are within a bound of ±0.5◦ . 64

0 100 200 300 400 500

−0.2

0

0

0

−0.2 0

100 200 300 400 500

−0.2

0

0 0

100 200 300 400 500

−0.2

0

Time (secs)

100 200 300 400 500

0 100 200 300 400 500

0 0

100 200 300 400 500

b1p (deg/sec) 0

0

0

−0.2 0

100 200 300 400 500

−0.2

0

0 0

100 200 300 400 500

−0.2

0

Time (secs)

100 200 300 400 500

100 200 300 400 500

−0.2

0

100 200 300 400 500

0

100 200 300 400 500

0

100 200 300 400 500

0.2

0.2 0

0

−0.2 100 200 300 400 500

−0.2

0.2

0.2 0

0

−0.2 0

Time (secs)

Estimation Errors 0

0

0

−0.2

0 −0.2

100 200 300 400 500

0.2

0.2

100 200 300 400 500

0.2

0

0.2

0.2

0

Time (secs)

0.2

100 200 300 400 500

b1q (deg/sec)

b1q (deg/sec)

100 200 300 400 500

−0.2


0 0

b1r (deg/sec)

Estimation Errors

−0.2

100 200 300 400 500

(b) GPS 4 Hz

b1r (deg/sec)

b1p (deg/sec)

0

0

Time (secs)

0.2

−0.2

−0.2

0

−0.2


100 200 300 400 500

0.2

0.2

Time (secs)

0.2

0

0

−0.2

100 200 300 400 500

−0.2

0.2

0

0

−0.2

0

0.2

100 200 300 400 500

0.2

0.2

0 −0.2 0

0.2

0.2


0.2

100 200 300 400 500

b1q (deg/sec)

b1q (deg/sec)

Truth (Solid); Estimate (Dotted) b1p (deg/sec)

0 −0.2 0

b1r (deg/sec)


b1r (deg/sec)

b1p (deg/sec)

Truth (Solid); Estimate (Dotted) 0.2

100 200 300 400 500

−0.2

Time (secs)

(c) GPS 5 Hz

Time (secs)

(d) GPS 10 Hz

Figure 6.15: EKF Performance in Estimating Bias, b1p , b1q , b1r , (deg/sec) - MAV Trajectory in Figure 6.1(a). ( ) Figure 6.15 shows the sensor bias estimates b1p , b1q , b1r for the MAV trajectory shown in Figure 6.1(a). As the rate gyroscope bias is modeled in the EKF the estimates of sensor bias for GPS update rates (1Hz, 4Hz, 5Hz, 10Hz) are seen from the Figures 6.15(a), 6.15(b), 6.15(c), and 6.15(d) respectively.

65

Estimation Errors

200

0

0

−2

0

100

200

300

400

500

θ (deg)

5

0

100

200

300

400

0

0

−2

500

0

0

100

200

300

400

500

−2

100

200

300

400

500

5

0

3

0

100

200

300

400

500

0

100

200

300

400

500

φ (deg)

φ (deg) 0

100

Time (secs)

0

100

200

300

400

500

200

300

400

−20

500

0

100

200

300

400

−2

0

100

200

300

400

500

θ (deg)

5

2

0

100

200

300

400

0

100

200

300

400

500

−2

0 −2

100

200

300

400

500

0

100

200

300

400

500

100

200

300

400

500

500

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

3 0

100

200

300

400

500

−2

2

φ (deg)

φ (deg)

400

2

4 2

0 −2

300

Estimation Errors

0

2

−20

200

2

0

0 −10

100

Time (secs)

5

0

−2

200

500

0

3

0


2

4

500

400

ψ (deg)

0

−2


0

500

Time (secs)

θ (deg)

ψ (deg)


400

0

−10

(a) GPS 1 Hz 2

300

0

Time (secs)

400

200

2

0 −2

100

0

3

2

−20

0

2

4 2

0 −10

Estimation Errors 2

200

2

4 2


ψ (deg)

2

θ (deg)

ψ (deg)


0

100

Time (secs)

200

300

400

0

−20

500

Time (secs)

0

−10 0

100

200

300

400

500

−2

Time (secs)

(c) GPS 5 Hz

Time (secs)

(d) GPS 10 Hz

Figure 6.16: EKF Performance in Estimating Euler Angles (deg) - MAV Trajectory in Figure 6.1(b). Figure 6.16 shows the Euler angle estimates for the MAV trajectory shown in Figure 6.1(b). As the rate gyroscope bias is modeled in the EKF, this allows to estimate the bias and correct the rate gyroscope measurement online. Thus it is seen from the Figures 6.16(a), 6.16(b), 6.16(c), and 6.16(d) that the errors between the estimates and truth of the MAV attitude and heading are within a bound of ±0.5◦ .

66

0 100 200 300 400 500

−0.2

0

0

0

−0.2 0

100 200 300 400 500

−0.2

0

0 0

100 200 300 400 500

−0.2

0

Time (secs)

100 200 300 400 500

0 100 200 300 400 500

0 0

100 200 300 400 500

b1p (deg/sec) 0

0

0

−0.2 0

100 200 300 400 500

−0.2

0

0 0

100 200 300 400 500

−0.2

0

Time (secs)

100 200 300 400 500

100 200 300 400 500

−0.2

0

100 200 300 400 500

0

100 200 300 400 500

0

100 200 300 400 500

0.2

0.2 0

0

−0.2 100 200 300 400 500

−0.2

0.2

0.2 0

0

−0.2 0

Time (secs)

Estimation Errors 0

0

0

−0.2

0 −0.2

100 200 300 400 500

0.2

0.2

100 200 300 400 500

0.2

0

0.2

0.2

0

Time (secs)

0.2

100 200 300 400 500

b1q (deg/sec)

b1q (deg/sec)

100 200 300 400 500

−0.2


0 0

b1r (deg/sec)

Estimation Errors

−0.2

100 200 300 400 500

(b) GPS 4 Hz

b1r (deg/sec)

b1p (deg/sec)

0

0

Time (secs)

0.2

−0.2

−0.2

0

−0.2


100 200 300 400 500

0.2

0.2

Time (secs)

0.2

0

0

−0.2

100 200 300 400 500

−0.2

0.2

0

0

−0.2

0

0.2

100 200 300 400 500

0.2

0.2

0 −0.2 0

0.2

0.2


0.2

100 200 300 400 500

b1q (deg/sec)

b1q (deg/sec)


0 −0.2 0

b1r (deg/sec)


b1r (deg/sec)

b1p (deg/sec)


100 200 300 400 500

−0.2

Time (secs)

(c) GPS 5 Hz

Time (secs)

(d) GPS 10 Hz

Figure 6.17: EKF Performance in Estimating Bias, b1p , b1q , b1r , (deg/sec) - MAV Trajectory in Figure 6.1(b). ( ) Figure 6.17 shows the sensor bias estimates b1p , b1q , b1r for the MAV trajectory shown in Figure 6.1(b). As the rate gyroscope bias is modeled in the EKF the estimates of sensor bias for GPS update rates (1Hz, 4Hz, 5Hz, 10Hz) are seen from the Figures 6.17(a), 6.17(b), 6.17(c), and 6.17(d) respectively.

67

6.4

Simulation Scenario: Modeled+Unmodeled Sensor Bias - NO GPS Outage ( ) ( ) Here the output of the rate gyroscope is corrupted by (w , w , w ), b , b , b & b , b , b p q r 1 1 1 2 2 2 p q r p q r , ( ) ( ) and the bias b1p , b1q , b1r is modeled( in EKF while b1p), b1q , b1r is treated as unmodeled bias. which leads to estimating 6−states ψ, θ, ϕ, b1p , b1q , b1r with 3−measurements (ψm , θm , ϕm ).

200

0

0 0

100

200

300

400

500

θ (deg)

5

2

200

0

0 0

100

200

300

400

500

0

3 0

100

200

300

400

500

−2

100

200

300

400

500

5

0 0

100

200

300

400

100

200

300

400

500

−2

φ (deg)

φ (deg) 0

100

Time (secs)

0

100

200

300

400

500

200

300

400

−20

500

0

100

200

300

400

500

θ (deg)

5

0

100

200

300

400

500

−2

0

100

200

300

400

500

ψ (deg) 0

100

200

300

400

0

500

0

100

200

300

400

100

200

300

400

500

100

200

300

400

500

−2

400

500

0

100

200

300

400

500

−2

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

3 0

100

200

300

400

500

−2

2

φ (deg)

φ (deg)

300

2

4 2

500

0 0

200

Estimation Errors

200

5

0

−2

2

2

−20

100

Time (secs)

0

0 −10

0


0

3

−2

400

2

4 2

−2

500


0

0

400

Time (secs)

θ (deg)

ψ (deg)


300

0

−10

(a) GPS 1 Hz 2

200

0

Time (secs)

400

100

2

0 0

0

0

3

2

−20

−2

2

4 2

500

0 −10

Estimation Errors

400

2

4 2

−2

Truth (Solid); Estimate (Dotted) ψ (deg)

Estimation Errors 2

θ (deg)

ψ (deg)


0

100

Time (secs)

200

300

400

0

−20

500

Time (secs)

0

−10 0

100

200

300

400

500

−2

Time (secs)

(c) GPS 5 Hz

Time (secs)

(d) GPS 10 Hz

Figure 6.18: EKF Performance in Estimating Euler Angles (deg) - MAV Trajectory in Figure 6.1(a). Figure 6.18 shows the Euler angle ( estimates) for the MAV trajectory shown in Figure 6.1(a). Even though the sensor bias b2p , b2q , b2r is not accounted in EKF implementation it is observed that the Euler angle estimates are not biased, which illustrates the robustness of the EKF even if the bias is not accounted in the filter design. Furthermore, the errors between the estimates and truth of the MAV attitude and heading are within a bound of ±0.5◦ . 68

0 100 200 300 400 500

−0.2

0

0

0

−0.2 0

100 200 300 400 500

−0.2

0

0 0

100 200 300 400 500

−0.2

0

Time (secs)

100 200 300 400 500

0 100 200 300 400 500

0 0

100 200 300 400 500

b1p (deg/sec) 0

0

0

−0.2 0

100 200 300 400 500

−0.2

0

0 0

100 200 300 400 500

−0.2

0

Time (secs)

100 200 300 400 500

100 200 300 400 500

−0.2

0

100 200 300 400 500

0

100 200 300 400 500

0

100 200 300 400 500

0.2

0.2 0

0

−0.2 100 200 300 400 500

−0.2

0.2

0.2 0

0

−0.2 0

Time (secs)

Estimation Errors 0

0

0

−0.2

0 −0.2

100 200 300 400 500

0.2

0.2

100 200 300 400 500

0.2

0

0.2

0.2

0

Time (secs)

0.2

100 200 300 400 500

b1q (deg/sec)

b1q (deg/sec)

100 200 300 400 500

−0.2


0 0

b1r (deg/sec)

Estimation Errors

−0.2

100 200 300 400 500

(b) GPS 4 Hz

b1r (deg/sec)

b1p (deg/sec)

0

0

Time (secs)

0.2

−0.2

−0.2

0

−0.2


100 200 300 400 500

0.2

0.2

Time (secs)

0.2

0

0

−0.2

100 200 300 400 500

−0.2

0.2

0

0

−0.2

0

0.2

100 200 300 400 500

0.2

0.2

0 −0.2 0

0.2

0.2


0.2

100 200 300 400 500

b1q (deg/sec)

b1q (deg/sec)


0 −0.2 0

b1r (deg/sec)


b1r (deg/sec)

b1p (deg/sec)


100 200 300 400 500

−0.2

Time (secs)

(c) GPS 5 Hz

Time (secs)

(d) GPS 10 Hz

Figure 6.19: EKF Performance in Estimating Bias, b1p , b1q , b1r , (deg/sec) - MAV Trajectory in Figure 6.1(a). ( ) Figure 6.19 shows the sensor bias estimates b1p , b1q , b1r for the MAV trajectory shown in Figure 6.1(a). As the rate gyroscope bias is modeled in the EKF the estimates of sensor bias for GPS update rates (1Hz, 4Hz, 5Hz, 10Hz) are seen from the Figures 6.19(a), 6.19(b), 6.19(c), and 6.19(d) respectively.

69

1000 Monte Carlo simulations were performed on the proposed EKF algorithm for the MAV trajectory show in Figure 6.1(a). The dotted lines in the Figure 6.20 correspond to ±0.5◦ . RMSE: Euler Angles (deg)

RMSE: Euler Angles (deg) 1

ψrmse

ψrmse

1 0 −1

0

100

200

300

400

−1

500

0

0

100

200

300

400

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

1

φrmse

φrmse

100

0 −1

500

1 0 −1

0

1

θrmse

θrmse

1

−1

0

0

100

200

300

400

0 −1

500

Time (secs)

Time (secs)

(a) GPS 1 Hz

(b) GPS 4 Hz

RMSE: Euler Angles (deg)


ψrmse

ψrmse

1 0 −1

0

100

200

300

400

−1

500

0

0

100

200

300

400

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

1

φrmse

φrmse

100

0 −1

500

1 0 −1

0

1

θrmse

θrmse

1

−1

0

0

100

200

300

400

0 −1

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 6.20: Euler Angles RMSE (deg) - MAV Trajectory in Figure 6.1(a). Figure 6.20 shows the root mean square error (RMSE)1 of Euler angle estimates for 1000 Monte Carlo simulations. Figures 6.20(a), 6.20(b), 6.20(c) and 6.20(d) show the RMSE plots for Euler angle estimates (ψ, θ, ϕ) with GPS update rates of 1 Hz, 4 Hz, 5 Hz an 10 Hz respectively. It is observed that as the GPS update rate increases the RMSE of Euler angles decreases. Throughout the simulations the RMSE of Euler angles is observed to be with in ±0.5◦ . 1

MSE derivation is shown in Appendix E.

70

Estimation Errors

200

0

0

−2

0

100

200

300

400

500

θ (deg)

5

0

100

200

300

400

0

0

−2

500

0

0

100

200

300

400

500

−2

100

200

300

400

500

5

0

3

0

100

200

300

400

500

0

100

200

300

400

500

φ (deg)

φ (deg) 0

100

Time (secs)

0

100

200

300

400

500

200

300

400

−20

500

0

100

200

300

400

−2

0

100

200

300

400

500

θ (deg)

5

2

0

100

200

300

400

0

100

200

300

400

500

−2

0 −2

100

200

300

400

500

0

100

200

300

400

500

100

200

300

400

500

500

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

3 0

100

200

300

400

500

−2

2

φ (deg)

φ (deg)

400

2

4 2

0 −2

300

Estimation Errors

0

2

−20

200

2

0

0 −10

100

Time (secs)

5

0

−2

200

500

0

3

0


2

4

500

400

ψ (deg)

0

−2


0

500

Time (secs)

θ (deg)

ψ (deg)


400

0

−10

(a) GPS 1 Hz 2

300

0

Time (secs)

400

200

2

0 −2

100

0

3

2

−20

0

2

4 2

0 −10

Estimation Errors 2

200

2

4 2


ψ (deg)

2

θ (deg)

ψ (deg)


0

100

Time (secs)

200

300

400

0

−20

500

Time (secs)

0

−10 0

100

200

300

400

500

−2

Time (secs)

(c) GPS 5 Hz

Time (secs)

(d) GPS 10 Hz

Figure 6.21: EKF Performance in Estimating Euler Angles (deg) - MAV Trajectory in Figure 6.1(b). Figure 6.21 shows the Euler angle ( estimates) for the MAV trajectory shown in Figure 6.1(b). Even though the sensor bias b2p , b2q , b2r is not accounted in EKF implementation it is observed that the Euler angle estimates are not biased, which illustrates the robustness of the EKF even if the bias is not accounted in the filter design. Furthermore, the errors between the estimates and truth of the MAV attitude and heading are within a bound of ±0.5◦ .

71

0 100 200 300 400 500

−0.2

0

0

0

−0.2 0

100 200 300 400 500

−0.2

0

0 0

100 200 300 400 500

−0.2

0

Time (secs)

100 200 300 400 500

0 100 200 300 400 500

0 0

100 200 300 400 500

b1p (deg/sec) 0

0

0

−0.2 0

100 200 300 400 500

−0.2

0

0 0

100 200 300 400 500

−0.2

0

Time (secs)

100 200 300 400 500

100 200 300 400 500

−0.2

0

100 200 300 400 500

0

100 200 300 400 500

0

100 200 300 400 500

0.2

0.2 0

0

−0.2 100 200 300 400 500

−0.2

0.2

0.2 0

0

−0.2 0

Time (secs)

Estimation Errors 0

0

0

−0.2

0 −0.2

100 200 300 400 500

0.2

0.2

100 200 300 400 500

0.2

0

0.2

0.2

0

Time (secs)

0.2

100 200 300 400 500

b1q (deg/sec)

b1q (deg/sec)

100 200 300 400 500

−0.2


0 0

b1r (deg/sec)

Estimation Errors

−0.2

100 200 300 400 500

(b) GPS 4 Hz

b1r (deg/sec)

b1p (deg/sec)

0

0

Time (secs)

0.2

−0.2

−0.2

0

−0.2


100 200 300 400 500

0.2

0.2

Time (secs)

0.2

0

0

−0.2

100 200 300 400 500

−0.2

0.2

0

0

−0.2

0

0.2

100 200 300 400 500

0.2

0.2

0 −0.2 0

0.2

0.2


0.2

100 200 300 400 500

b1q (deg/sec)

b1q (deg/sec)


0 −0.2 0

b1r (deg/sec)


b1r (deg/sec)

b1p (deg/sec)


100 200 300 400 500

−0.2

Time (secs)

(c) GPS 5 Hz

Time (secs)

(d) GPS 10 Hz

Figure 6.22: EKF Performance in Estimating Bias, b1p , b1q , b1r , (deg/sec) - MAV Trajectory in Figure 6.1(b). ( ) Figure 6.22 shows the sensor bias estimates b1p , b1q , b1r for the MAV trajectory shown in Figure 6.1(b). As the rate gyroscope bias is modeled in the EKF the estimates of sensor bias for GPS update rates (1Hz, 4Hz, 5Hz, 10Hz) are seen from the Figures 6.22(a), 6.22(b), 6.22(c), and 6.22(d) respectively.

72



ψrmse

ψrmse

1 0 −1

0

100

200

300

400

−1

500

0

0

100

200

300

400

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

1

φrmse

φrmse

100

0 −1

500

1 0 −1

0

1

θrmse

θrmse

1

−1

0

0

100

200

300

400

0 −1

500

Time (secs)

Time (secs)

(a) GPS 1 Hz

(b) GPS 4 Hz



ψrmse

ψrmse

1 0 −1

0

100

200

300

400

−1

500

0

0

100

200

300

400

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

1

φrmse

φrmse

100

0 −1

500

1 0 −1

0

1

θrmse

θrmse

1

−1

0

0

100

200

300

400

0 −1

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 6.23: Euler Angles RMSE (deg) - MAV Trajectory in Figure 6.1(b). Figure 6.23 shows the RMSE of Euler angle estimates for 1000 Monte Carlo simulations. Figures 6.23(a), 6.23(b), 6.23(c) and 6.23(d) show the RMSE plots for Euler angle estimates (ψ, θ, ϕ) with GPS update rates of 1 Hz, 4 Hz, 5 Hz an 10 Hz respectively. It is observed that as the GPS update rate increases the RMSE of Euler angles decreases. Furthermore, the errors between the estimates and truth of the MAV attitude and heading are within a bound of ±0.5◦ .

73

6.4.1

Modeled+Unmodeled Sensor Bias (Other Maneuvers - RMSE Plots)

Figures 8.20(a) and 8.20(b) shows the MAV trajectories considered for 1000 Monte Carlo simulations. Trajectory Start (o); Trajectory Finish (x)

205

Alt (m)

200

195

190

185 200 100

100

50

Y (m)

0

0

X (m)

−50 −100

−100

(a) Coordinated Turn (Approximately Constant Altitude)


205

Alt (m)

200

195

190

185 200 100

100

Y (m)

50 0

0 −50 −100

−100

X (m)

(b) Coordinated Turn (Approximately Constant Altitude): Turbulence

Figure 6.24: Simulated MAV Trajectories.

74

Simulation Scenario: Modeled+Unmodeled Sensor Bias - Intermittent GPS Outage ( ) ( ) Here the output of the rate gyroscope is corrupted by (w , w , w ), b , b , b & b , b , b , p q r 1 1 1 2 2 2 p q r p q r ( ) ( ) and the bias b1p , b1q , b1r is modeled( in EKF while b1p), b1q , b1r is treated as unmodeled bias. which leads to estimating 6−states ψ, θ, ϕ, b1p , b1q , b1r with 3−measurements (ψm , θm , ϕm ). The GPS measurement scenario is shown in Figure 6.25. 50

50

45

45

Sensor Update Rates (Hz)


6.5

40 35 30

IMU = 40 Hz

25 20 15

GPS = 1 Hz

10

GPS Outage Period

5 0 −5

40 35 30

IMU = 40 Hz

25 20

10 5 0

0

100

200

300

400

−5

500

0

100

Simulation Time (secs)

45


50

45 40 35

IMU = 40 Hz

25 20

GPS Outage Period

GPS = 5 Hz

15 10 5

30

400

500

GPS = 10 Hz

20 15

GPS Outage Period

10 5 0

300

IMU = 40 Hz

25

−5

200

500

35

−5

100

400

40

0 0

300

(b) GPS 4 Hz

50

30

200


(a) GPS 1 Hz


GPS Outage Period

GPS = 4 Hz

15

0

100

200

300

400

500



(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 6.25: Showing the IMU and GPS Update Rates Along with GPS Outage Periods of 250 ∼ 270 secs and 390 ∼ 405 secs. An intermittent GPS Outage Periods of 250 ∼ 270 secs and 390 ∼ 405 secs is considered in the GPS measurements. Figures 6.25(a), 6.25(b), 6.25(c) and 6.25(d) show the IMU and GPS update rates with red colored flags indicating outage period.

75

6.5.1

EKF Correction During GPS Outage: Eq. (5.82)

A pseudo-code has been presented in Chapter 4, where two ways of implementing EKF correction phase has been presented when there is a GPS outage such as: Eq. (5.82) and Eqs. (5.83) ∼ (5.87). In this section we present few simulation results using this correction techniques. RMSE: Euler Angles (deg)

0

−2

0

100

200

300

400

θrmse

θrmse

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

−2 0

100

200

300

400

500

2

φrmse

2

0

φ

rmse

0

2

0

−2

0

−2

500

2

−2


2

ψrmse

ψrmse

2

0

−2 0

100

200

300

400

500

Time (secs)

Time (secs)

(a) GPS 1 Hz RMSE: Euler Angles (deg)

ψ

0

−2

0

100

200

300

400

θrmse

rmse

θ

0

100

200

300

400

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

−2

500

2

2

φrmse

φrmse

0

2

0

0

−2

0

−2

500

2

−2


2

ψrmse

2

rmse

(b) GPS 4 Hz

0

100

200

300

400

0

−2

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 6.26: Euler Angles RMSE (deg) - MAV Trajectory in Figure 6.1(a). Figure 6.26 shows the Euler angle estimation errors for 1000 Monte Carlo simulations. The EKF correction was carried out using Eq. (5.82). The Figures 6.26(a), 6.26(b), 6.26(c) and 6.26(d) show that when there is a GPS outage the estimation error increases until there is a GPS packet available. For the GPS outage duration of 20 secs and 15 secs it has been observed that the RMSE of Euler angles through out the simulations are within a bound of ±1◦ .

76


0

−2

100

200

300

400

500

θrmse

rmse

θ

0

0

100

200

300

400

2

φrmse

φrmse

100

200

300

400

500

0

100

200

300

400

500

0

0

100

200

300

400

0

100

200

300

400

500

0

−2

500

2

−2

0

2

2

−2

0

−2 0


2

ψrmse

ψrmse

2

0

−2

500

Time (secs)

Time (secs)


0

−2

100

200

300

400

500

θrmse

θrmse

0

100

200

300

400

500

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2

φrmse

φrmse

100

0

−2 0

2

0

−2

0

2

2

−2

0

−2 0


2

ψrmse

2

ψrmse

(b) GPS 4 Hz

0

−2 0

100

200

300

400

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 6.27: Euler Angles RMSE (deg) - MAV Trajectory in Figure 6.1(b). Figure 6.26 shows the Euler angle estimation errors for 1000 Monte Carlo simulations for MAV trajectory shown in Figure 6.1(b). The EKF correction was carried out using Eq. (5.82). For the GPS outage duration of 20 secs and 15 secs it has been observed that the RMSE of Euler angles through out the simulations are within a bound of ±1◦ .

77

EKF Correction During GPS Outage: Eqs. (5.83) ∼ (5.87)

6.5.2


ψrmse

0

−2

0

100

200

300

400

θrmse

rmse

θ

0

100

200

300

400

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

−2

500

2

φrmse

2

0

φ

rmse

0

2

0

−2

0

−2

500

2

−2


2

ψ

rmse

2

0

100

200

300

400

0

−2

500

Time (secs)

Time (secs)


0

−2

0

100

200

300

400

θrmse

θrmse 0

100

200

300

400

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

−2

500

2

φrmse

2

φrmse

0

2

0

0

−2

0

−2

500

2

−2


2

ψrmse

2

ψrmse

(b) GPS 4 Hz

0

100

200

300

400

0

−2

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 6.28: Euler Angles RMSE (deg) - MAV Trajectory in Figure 6.1(a). Figure 6.28 shows the Euler angle estimation errors for 1000 Monte Carlo simulations. The EKF correction was carried out using Eqs. (5.83) ∼ (5.87). The Figures 6.28(a), 6.28(b), 6.28(c) and 6.28(d) show that when there is a GPS outage the estimation error increases until there is a GPS packet available. For the GPS outage duration of 20 secs and 15 secs it has been observed that the RMSE of Euler angles through out the simulations are within a bound of ±1◦ .

78


2

θrmse

ψrmse

0

−2

100

200

300

400

500

0

100

200

300

400

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2

φrmse

φrmse

100

0

−2

500

2

0

−2

0

2

0

−2

0

−2 0


2

θrmse

ψrmse

2

0

100

200

300

400

0

−2

500

Time (secs)

Time (secs)


0

−2

0

100

200

300

400

200

300

400

500

θrmse 100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2

φrmse

θrmse φrmse

100

0

−2 0

0

−2

0

2

0

2

0

−2

500

2

−2


2

ψrmse

2

ψrmse

(b) GPS 4 Hz

0

0

100

200

300

400

−2

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 6.29: Euler Angles RMSE (deg) - MAV Trajectory in Figure 6.1(b). Figure 6.29 shows the Euler angle estimation errors for 1000 Monte Carlo simulations for MAV trajectory shown in Figure 6.1(b). The EKF correction was carried out using Eqs. (5.83) ∼ (5.87). For the GPS outage duration of 20 secs and 15 secs it has been observed that the RMSE of Euler angles through out the simulations are within a bound of ±1◦ .

79

6.6

Attitude and Heading Estimation for a Micro Aerial Vehicle: Simulation results Using a Differential Pressure Sensor and No GPS

Thus far, we have looked at the attitude and heading estimation simulation results from a perspective of augmenting the INS sensor suite with a GPS receiver onboard the MAV. In this section, we consider an AHRS augmenting approach where we do not rely on GPS to augment the existing INS sensor package, rather rely on a differential pressure sensor to provide a sense of the forward speed of the MAV. Various maneuvers were simulated using a 6 degree-of-freedom (DOF) model adapted from the open-source Flight Dynamics and Control toolbox (FDC) [33], turbulence is introduced to the model via the Dryden model [34]. The trajectories shown in Figure 6.30 are the trajectories that are considered for MAV Attitude and Heading estimation. Trajectory Start (o); Trajectory Finish (x)

205

205

200

200

Alt (m)

Alt (m)


195

195

190

190

185 200

185 200 0

0

1500 1000

−200

Y (m)

−400

0 −600

Y (m)

X (m)

−500

200

200

Alt (m)

Alt (m)

205

195

X (m)

195

190

190

185 200

185 200 100

100

100

100

50 0 −50

0 −500


205

−100

−400



0

500 −600


Y (m)

1500 1000

−200

500

Y (m)

X (m)

50 0

0 −50 −100

−100

−100

X (m)

(c) Coordinated Turn (Approximately Constant Alti- (d) Coordinated Turn (Approximately Constant Altude) titude): Turbulence


80

6.6.1

Attitude and Heading Estimation results using DPS : RMSE: Euler Angles (deg)


ψrmse

ψ

rmse

2 0 −2

0

100

200

300

400

−2

500

0 2

rmse

0

θ

θ

rmse

2

−2

0

100

200

300

400

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

rmse

2

0

φ

rmse

φ

100

0

−2

500

2

−2

0

0

−2 0

100

200

300

400

500

Time (secs)

Time (secs)




RMSE: Euler Angles (deg) rmse

2

0 −2

ψ

ψrmse

2

−2 0

100

200

300

400

500

rmse

0

0

100

200

300

400

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

rmse

2

0

φ

φrmse

100

0 −2

500

2

−2

0

2

θ

θrmse

2

−2

0

0 −2

0

100

200

300

400

500

Time (secs)

Time (secs)

(c) Coordinated Turn

(d) Coordinated Turn: Turbulence

Figure 6.31: Euler angle estimates using DPS in the measurement suit. Figure 6.31 show the RMSE of Euler angle estimates for the MAV trajectories shown in Figure 6.30. It is observed that using DPS in the measurement suit is an effective way of forming a measurement model for EKF. It is seen from Figures 6.31(a), 6.31(b), 6.31(c) and 6.31(d) that the EKF performance is observed to degrade with implementing it for various MAV trajectories. This performance degradation is because of assumptions made in Section 5.5.2. The assumption (uE = Vtas , v E = 0, wE = 0) violates for other trajectories except for steady level flight. Thus we can state that implementing EKF for attitude and heading estimation using DPS in the measurement suit is not robust for various MAV trajectories.

81

Chapter 7 GPS Based Inertial Navigation for a Micro Aerial Vehicle: Model Formulation In Chapter 5, the attitude and heading estimation model was derived, while in Chapter 6, open-loop simulation results for the model developed in Chapter 5 for MAV attitude and heading estimation were shown for a suite of MAV maneuvers. In this chapter, a model for GPS based inertial navigation estimation is developed. We first recall the rate gyroscope and the accelerometer models used. 7.1

Rate Gyroscope and Accelerometer Model

Remark 7.1. The rate gyroscope model used is similar to the model used in Eqs. (5.3), (5.4) and (5.5), with the bias terms modeled as shown in Remarks 5.3 and 5.4. We rewrite these remarks and the rate gyroscope model used in this chapter for the ease of the reader. The rate gyroscope measurements pm (t) , qm (t) , rm (t) are assumed to be modeled as: ( ( )) pm (t) = p(t) + b0p + b1p (t) + b2p (t) + wp (7.1) ( ( )) qm (t) = q(t) + b0q + b1q (t) + b2q (t) + wq (7.2) rm (t) = r(t) + (b0r + (b1r (t) + b2r (t))) + wr (7.3) where, p(t), q(t), r(t) are the true values of the angular velocity; b0p , b0q , b0r are the constant null-shift bias terms, b1p (t) , b1q (t) , b1r (t) are the rate random walk bias components, b2p (t) , b2q (t) , b2r (t) are the correlated (colored noise) bias components and wp , wq , wr denote the error due to sampling noise which are typically modeled as zero mean, bandlimited, additive, white Gaussian noise (AWGN) processes of specified covariances denoted by σp2 , σq2 , σr2 , respectively. Remark 7.2. The effects due to rate random walk bias in the rate gyroscope output are modeled as Weiner processes given by: b˙ 1p = w1p b˙ 1q = w1q b˙ 1r = w1r 82

(7.4) (7.5) (7.6)

where, w1p , w1q , w1r are zero mean, band-limited, AWGN processes with specified covariances denoted by σ12p , σ12q , σ12r , respectively. Remark 7.3. The effects due to correlated noise in the rate gyroscope output are modeled as first order Gauss-Markov processes [29] given by: b2 w2 b˙ 2p = − p + p τp τp b2 w2 b˙ 2q = − q + q τq τq b2 w2 b˙ 2r = − r + r τr τr

(7.7) (7.8) (7.9)

where, w2p , w2q , w2r represent the driving process noise components which are assumed to be zero mean, band-limited, AWGN processes with specified covariances denoted by σ22p , σ22q , σ22r , respectively and τp , τq , τr denote the time constants of the modeled correlated noise processes. Remark 7.4. The constant bias components, b0p , b0q , b0r , can be estimated off-line and removed from the output of the rate gyroscope. Hence, the effects of b0p , b0q , b0r on the output of the rate gyroscope are neglected. Remark 7.5. The accelerometer model for the INS/GPS problem is given as: axm = ax + bax + wax aym = ay + bay + way azm = az + baz + waz

(7.10) (7.11) (7.12)

where, bax , bay , baz , are constant bias offsets, ax , ay , az are the true forces and wax , way , waz denote the additive sensor noise processes, respectively, for the x, y, z axes and are modeled as zero mean, band-limited AWGN processes with covariances denoted by σa2x , σa2y , σa2z , respectively. Remark 7.6. The GPS model for the INS/GPS problem is given as: λm µm hm vnm v em v dm

= = = = = =

λ + vλ µ + vµ h + vh vn + vvn ve + vve vd + vvd

(7.13) (7.14) (7.15) (7.16) (7.17) (7.18)

where, vλ , vµ , vh , vvn , vve , vvd denote zero mean, bandlimited, AWGN processes of covariances respectively denoted by σvλ , σvµ , σvvn , σvve , σvvd . Thus the final state vector includes 15 states which are as follows: 1. Position (3 states): λ, µ, h (Latitude, Longitude, Altitude). 83

2. Velocity (3 states): vn , ve , vd . 3. Attitude (3 states): ψ, θ, ϕ (Heading or Yaw, Pitch, Bank or Roll). 4. Rate Gyroscopic Bias (3 states): b1p , b1q , b1r (Modeled as a Weiner Process). 5. Accelerometer Bias (3 states): bax , bay , baz (Modeled as a Weiner Process). 7.2

Model for Inertial Navigation

Neglecting the effect of Earth’s rotation rate, the INS/GPS model in the local North-EastDown (NED) frame1 is [1]: 

 λ˙  µ˙  = h˙

  |

[ =

vn RM +h ve (RN +h)cλ

 

(7.19)

−vd {z } ]T f1 f2 f3

     v˙ n cθ cψ sϕ sθ cψ − cϕ sψ cϕ sθ cψ + sϕ sψ ax + b ax  v˙ e  =  cθ sψ sϕ sθ sψ + cϕ cψ cϕ sθ sψ − sϕ cψ   ay + bay  +   v˙ d −sθ sϕ cθ cϕ cθ az + baz | {z [ ]T = f4 f5 f6    cθ cψ sϕ sθ cψ − cϕ sψ cϕ sθ cψ + sϕ sψ wa x +  cθ sψ sϕ sθ sψ + cϕ cψ cϕ sθ sψ − sϕ cψ   way  −sθ sϕ cθ cϕ cθ waz | {z } 

2 vn vd λ − RvNe t+h RM +h ve (vd +vn tλ ) RN +h 2 −vd2 − RNve+h + RM +h

   g

}

(7.20)

RIB

  ˙       sϕ sϕ cϕ cϕ 0 0 ψ p + b1p b2p cθ cθ cθ cθ  θ˙  =  0 cϕ −sϕ   q + b1q  +  0 cϕ −sϕ   b2q  r + b1r b2r 1 sϕ tθ cϕ tθ 1 sϕ tθ cϕ tθ ϕ˙ {z } | [ ]T = f7 f8 f9    cϕ sϕ 0 w p cθ cθ +  0 cϕ −sϕ   wq  wr 1 sϕ tθ cϕ tθ    ˙  b1p w1p  b˙ 1q  =  w1q  w1r b˙ 1r    ˙  b ax wbax  b˙ ay  =  wbay  wbaz b˙ az 1

Locally, the North East Down (NED) frame is assumed to be inertial.

84

(7.21)

(7.22)

(7.23)

where, λ, µ, h denote position (latitude, longitude and altitude), vn , ve , vd denote velocity, ψ, θ, ϕ denote heading, pitch & roll, wbax , wbay , wbaz are zero mean, bandlimited, AWGN processes of specified covariances, RM , RN respectively denote the Earth’s meridional and normal radius of curvature, R0 ≈ 6378137m, e = 0.01671123 denote the Earth’s radius ( )T and eccentricity respectively, g denotes the acceleration due to gravity and RIB = RB I is the rotation matrix that takes a vector from the body frame to the inertial frame. The expressions for RM and RN are given as: ( ) ( )1 RM = R0 1 − e2 / 1 − e2 s2λ 3 √ RN = R0 / 1 − e2 s2λ

(7.24) (7.25)

The measurement model consists of GPS position, velocity and heading derived from a magnetometer and can be written as: λm µm hm vnm v em v dm ψm

= = = = = = =

λ + vλ µ + vµ h + vh vn + vvn ve + vve vd + vvd ψ + vψ

(7.26) (7.27) (7.28) (7.29) (7.30) (7.31) (7.32)

where, vλ , vµ , vh , vvn , vve , vvd , vψ denote zero mean, bandlimited, AWGN processes of covariances respectively denoted by σvλ , σvµ , σvvn , σvve , σvvd , σvψ . 7.3

EKF Based INS/GPS Model

The final model for the design of an EKF for obtaining the inertial solution via an INS/GPS formulation is obtained by formulating the EKF state vector as an augmentation of the 3 position states, the 3 velocity states, the 3 Euler angle states (ψ, θ, ϕ) with the rate gyroscope bias terms (b1p , b1q , b1r ) and the bias accelerometer terms (bax , bay , baz ). The measurement model comprises the heading derived from a triaxial magnetometer on board the MAV and position and velocity from an onboard GPS receiver. Remark 7.7. The effects of the rate gyroscopic bias components due to colored noise process (Remark 7.3) are not explicitly modeled in the filter model but rather treated as unmodeled bias components. This is because augmenting all bias sources as states in the filter model without additional measurements can lead to loss of observability and adversely affect the filter stability. Thus b1p , b1q , b1r are included as states in the filter model while b2p , b2q , b2r are treated as unmodeled states, i.e. the effects due to b2p , b2q , b2r are seen in the output of the rate gyroscope but are not captured in the filter process model.

85

7.3.1

Full State EKF Model

With Remark 7.7 and using the rate gyroscope measurements for the values of p, q, r and the accelerometer measurements for the values of ax , ay , az in Eqs. (7.21) and (7.20) respectively, the EKF process and measurement model can be written as (unmodeled dynamics affects ˙ θ, ˙ ϕ˙ via pm , qm , rm and is not explicitly written): ψ, 



EKF-AHRS Continuous Process Model   vn

λ˙ RM +h ve   µ˙  =  (7.33) (RN +h)cλ ˙h −vd  2       vn vd λ − RvNe t+h v˙ n cθ cψ sϕ sθ cψ − cϕ sψ cϕ sθ cψ + sϕ sψ ax + b ax RM +h  ve (vd +vn tλ )  v˙ e  =  cθ sψ sϕ sθ sψ + cϕ cψ cϕ sθ sψ − sϕ cψ   ay + bay  +    RN +h −vd2 ve2 v˙ d −sθ sϕ cθ cϕ cθ az + baz − RN +h + g RM +h    cθ cψ sϕ sθ cψ − cϕ sψ cϕ sθ cψ + sϕ sψ wa x    way  + cθ sψ sϕ sθ sψ + cϕ cψ cϕ sθ sψ − sϕ cψ (7.34) −sθ sϕ cθ cϕ cθ waz        ˙  cϕ sϕ cϕ sϕ 0 0 ψ p + b1p wp cθ cθ cθ cθ  θ˙  =  0 cϕ (7.35) −sϕ   q + b1q  +  0 cϕ −sϕ   wq  ˙ wr r + b1r 1 sϕ tθ cϕ tθ 1 sϕ tθ cϕ tθ ϕ  ˙    b1p w1p  b˙ 1q  =  w1q  (7.36) ˙b1r w1r  ˙    b ax wbax  b˙ ay  =  wbay  (7.37) ˙baz wbaz EKF-AHRS Discrete Measurement Model zk1m = λm = λk + vλ (7.38) zk2m = µm = µk + vµ (7.39) zk3m = hm = hk + vh (7.40) zk4m = vnm = vnk + vvn (7.41) (7.42) zk5m = vem = vek + vve (7.43) zk6m = vdm = vdk + vvd zk7m = ψm = ψk + vψ (7.44) where, λk , µk , hk , vnk , vek , vdk , ψk represent the discrete counterparts of λ, µ, h, vn , ve , vd , ψ.

86

7.4

EKF Implementation

EKF for INS/GPS will be implemented in four ways: 1. Nine State Filter. 2. Twelve State Filter. 3. Fifteen State Filter. 4. Split Architecture. Each of these filters comprises to a particular MEMS based sensor configuration. For all the filters to be implemented the measurements from GPS and magnetometer are modeled as shown in (Eqs. (7.38) ∼ (7.44)), but measurements from accelerometers and rate gyroscopes are different for each case. 7.4.1

Nine State Filter

The accelerometers and rate gyroscopes are assumed to be unbiased and are only corrupted with white Gaussian noise. Thus there is no need for estimating bias in accelerometer and gyroscopes, which leads to estimating only nine states with seven measurements (λ, µ, h, vn , ve , vd , ψ). Thus the state vector for Nine State Filter: x = 7.4.2

[

λ µ h v n ve vd ψ θ ϕ

]T

(7.45)

Twelve State Filter

Here only accelerometers are assumed to be unbiased. While rate gyroscopes are modeled as shown in (Eqs. (7.1) ∼ (7.3)). Thus there is no need for estimating bias in accelerometer, which leads to estimating twelve states with seven measurements (λ, µ, h, vn , ve , vd andψ). Thus the state vector for Twelve State Filter: [ ]T λ µ h vn ve vd ψ θ ϕ b1p b1q b1r x = (7.46) 7.4.3

Fifteen State Filter

Full 15 − statef ilter will be implemented with biased measurements from accelerometers and gyroscopes. Accelerometer bias and rate gyro bias is modeled in EKF as wiener process. Fifteen states are estimated with only seven measurements (λ, µ, h, vn , ve , vd , ψ). Thus the state vector for Fifteen State Filter: [ ]T λ µ h vn ve vd ψ θ ϕ b1p b1q b1r bax bay baz x = (7.47)

87

7.4.4

Split Architecture

Two EKFs will be implemented: First to estimate attitude (ψ, θ, ϕ) and gyroscope bias (b1p , b1p , b1r ) with three measurements as discussed in Chapter 4. The state and measurement vectors for this EKF are given as: [

ψ θ ϕ b1p b1q b1r [ ]T ψm θm ϕm z =

x =

]T (7.48)

Secondly, the estimated attitude is fed to the second EKF to estimate the position, velocity and accelerometer bias of the MAV with six measurements. The state and measurement vectors for this EKF are given as: [

λ µ h vn ve vd bax bay baz [ ]T λm µm hm vnm vem vdm z =

x =

7.5

]T (7.49)

ˆ, f , F Model & EKF State Vector, Nonlinear Dynamics Map and Jacobian: x, x

The state vector and the nonlinear dynamics map for the Fifteen state EKF based INS/GPS formulation are: [ ]T λ µ h vn ve vd ψ θ ϕ b1p b1q b1r bax bay baz x = (7.50) [ ]T ˆ µ ˆ vˆn vê vˆd ψˆ θˆ ϕˆ ˆb1p ˆb1q ˆb1r ˆbax ˆbay ˆbaz ˆ = x (7.51) λ ˆ h   vn RM +h

ve  (RN +h)cλ   −vd  2  I I λ d − RvNe t+h  RB(1,1) (axm ) + RB(1,2) (aym ) + RIB(1,3) (azm ) + RvMn v+h  n tλ )  RIB(2,1) (axm ) + RIB(2,2) (aym ) + RIB(2,3) (azm ) + ve (vRdN+v  +h  I −vd2 ve2 I I  RB − +g (a ) + R (a ) + R (a ) + x y z m m m B B RM (3,1)  ( ) (3,3)( ) (+h) RN +h ( )(3,2)  cϕ sϕ cϕ sϕ qm cθ + rm cθ + b1q cθ + b1r cθ  f =   qm cϕ − rm sϕ + b1q cϕ − b1r sϕ   pm + qm sϕ tθ + rm cϕ tθ + b1p + b1q sϕ tθ + b1r cϕ tθ   0   0   0   0   0

0

88

                           

(7.52)

where, the hatted quantities denote the estimates of the corresponding state variables. The ∂f nonzero entries of the Jacobian matrix F = ∂x x=ˆx in Eq. (4.33) are given by: Jacobian: First Row −ˆ v n RMλˆ F [1, 1] = ( )2 ˆ RM + h F [1, 3] = (

−ˆ vn RM

F [1, 4] = (

)2 ˆ +h

1 RM

) ˆ +h

(7.53)

Jacobian: Second Row ( ) vê sλˆ − RNλˆ F [2, 1] = ( )2 ˆ c2 RN + h ˆ λ F [2, 3] = ( F [2, 5] = (

−ˆ ve

)2 ˆ RN + h cλˆ 1

) ˆ RN + h cλˆ

Jacobian: F [3, 6] = −1

(7.54)

Third Row (7.55)

Jacobian: Fourth Row −ˆ vn vˆd RNλˆ vê2 ) − tλˆ RNλˆ F [4, 1] = ( )2 − ( ˆ ˆ R + h c2λˆ N RM + h F [4, 3] = (

−ˆ vn vˆd RM

F [4, 4] = ( F [4, 5] = ( F [4, 6] = (

vê2 tλˆ )2 + ( )2 ˆ ˆ +h RN + h

vˆd

) ˆ RM + h −2ˆ ve tλˆ ) ˆ RN + h vˆn

) ˆ RM + h (7.56) 89

(

) ( )( ) ( )( ) ˆ ˆ ˆ F [4, 7] = −cθˆsϕˆ ax + bax − sϕˆsθˆsψˆ + cϕˆcψˆ ay + bay + −cϕˆsθˆsψˆ + sϕˆcψˆ az + baz ( ) ( )( ) ( )( ) F [4, 8] = −sθˆcϕˆ ax + ˆbax + sϕˆcθˆcψˆ − cϕˆsψˆ ay + ˆbay + cϕˆcθˆcψˆ + sϕˆsψˆ az + ˆbaz ( )( ) ( )( ) F [4, 9] = cϕˆsθˆcψˆ + cϕˆcψˆ ay + ˆbay + −sϕˆsθˆcψˆ + cϕˆsψˆ az + ˆbaz F [4, 13] = cθˆcψˆ F [4, 14] = sϕˆsθˆcψˆ − cϕˆsψˆ F [4, 15] = cϕˆsθˆcψˆ + sϕˆsψˆ

(7.57)

Jacobian: Fifth Row −ˆ vn vˆd RNλˆ vê vˆn ) − tλˆ RNλˆ F [5, 1] = ( )2 + ( ˆ ˆ R + h c2λˆ N RN + h F [5, 3] =

−ˆ ve (ˆ vd + vˆn tλˆ ) ( )2 ˆ RN + h

F [5, 4] = (

vê tλˆ

) ˆ RN + h

vˆd + vˆn tλˆ ) F [5, 5] = ( ˆ RN + h F [5, 6] = (

vê

) ˆ RN + h ( ) ( )( ) ( )( ) F [5, 7] = cθˆcϕˆ ax + ˆbax + sϕˆsθˆcψˆ − cϕˆsψˆ ay + ˆbay + cϕˆsθˆcψˆ + sϕˆsψˆ az + ˆbaz ( ) ( )( ) ( )( ) ˆ ˆ ˆ F [5, 8] = −sθˆsϕˆ ax + bax + sϕˆcθˆsψˆ + cϕˆcψˆ ay + bay + cϕˆcθˆsψˆ − sϕˆcψˆ az + baz ( )( ) ( )( ) F [5, 9] = cϕˆsθˆsψˆ − sϕˆcψˆ ay + ˆbay + −sϕˆsθˆsψˆ − cϕˆcψˆ az + ˆbaz F [5, 13] = cθˆsψˆ F [5, 14] = sϕˆsθˆsψˆ + cϕˆcψˆ F [5, 15] = cϕˆsθˆsψˆ − sϕˆcψˆ Jacobian: Sixth Row vê2 RNλˆ vˆd2 RMλˆ ( ) F [6, 1] = ( )2 + ˆ ˆ RN + h RM + h F [6, 3] = ( F [6, 5] = (

−ˆ vn vˆd

(7.58)

vê2 tλˆ

)2 + ( )2 ˆ ˆ RM + h RN + h −2ˆ ve

) ˆ RN + h 90

F [6, 5] = (

−2ˆ v

) ˆ RM + h )( ) )( ) ( ( ) ( F [6, 8] = −cθˆ ax + ˆbax − sϕˆsθˆ ay + ˆbay − cϕˆsθˆ az + ˆbaz ( )( ) ( )( ) F [6, 9] = cϕˆcθˆ ay + ˆbay − sϕˆcθˆ az + ˆbaz F [6, 13] = −sθˆ F [6, 14] = sϕˆcθˆ F [6, 15] = cϕˆcθˆ

F [7, 8]

F [7, 9] F [7, 11] F [7, 12]

(7.59) Jacobian: Seventh Row ( ) ) ) sθˆ (( ˆ ˆ = 2 qm + b1q sϕˆ + rm + b1r cϕˆ cθˆ ( ) ( ) ˆ ˆ qm + b1q cϕˆ − rm + b1r sϕˆ = cθˆ sϕˆ = cθˆ cϕˆ = cθˆ

(7.60)

Jacobian: Eighth Row (( ) ( ) ) F [8, 9] = − qm + ˆb1q sϕˆ + rm + ˆb1r cϕˆ F [8, 11] = cϕˆ F [8, 12] = −sϕˆ

(7.61)

Jacobian: Ninth Row ( ) ( ) ˆ ˆ qm + b1q s ˆ + rm + b1r c ˆ ϕ

F [9, 8] = (( F [9, 9] = tθˆ

ϕ

c2θˆ


F [9, 10] = 1 F [9, 11] = sϕˆtθ F [9, 12] = cϕˆtθ

(7.62)

ˆ s ˆ ⇒ sin θ, ˆ s ˆ ⇒ cos ϕ, ˆ c ˆ ⇒ cos ψ, ˆ where, sψˆ ⇒ sin ψ, θ ϕ ψ ˆ tˆ ⇒ tan λ, ˆ RM ⇒ ∂RM ˆ and RN ⇒ ∂RN ˆ . tan θ, ˆ ˆ λ ∂λ λ ∂λ λ λ λ

91

ˆ cθˆ ⇒ cos θ,


tθˆ ⇒

7.5.1

Jacobian F: Split Architecture

The state and measurement vectors for Split Architecture EKF s shown in Eqs. 7.48 and ∂f 7.49. The nonzero entries of the Jacobian matrix F = ∂x is given by: x=ˆ x Attitude and Heading Estimation: Jacobian elements

F [1, 2]

F [1, 3] F [1, 5] F [1, 6]

Jacobian: First Row ( ) ) ) s ˆ (( = 2θ qm + ˆb1q sϕˆ + rm + ˆb1r cϕˆ cˆ ( ) (θ ) qm + ˆb1q cϕˆ − rm + ˆb1r sϕˆ = cθˆ sϕˆ = cθˆ cϕˆ = cθˆ

Jacobian: Second Row (( ) ( ) ) F [2, 3] = − qm + ˆb1q sϕˆ + rm + ˆb1r cϕˆ F [2, 5] = cϕˆ F [2, 6] = −sϕˆ Jacobian: Third Row ( ) ( ) ˆ ˆ qm + b1q s ˆ + rm + b1r c ˆ ϕ

F [3, 2] = (( F [3, 3] = tθˆ

ϕ

c2θˆ


F [3, 4] = 1 F [3, 5] = sϕˆtθ F [3, 6] = cϕˆtθ ˆ where, sϕˆ ⇒ sin ϕ,

ˆ sθˆ ⇒ sin θ,

(7.63)


92

ˆ cθˆ ⇒ cos θˆ and tθˆ ⇒ tan θ.

Navigation Solution: Jacobian elements

Jacobian: First Row −ˆ v n RMλˆ F [1, 1] = ( )2 ˆ RM + h F [1, 3] = (

−ˆ vn RM

)2 ˆ +h

1

F [1, 4] = (

RM

) ˆ +h

(7.64)

Jacobian: Second Row ( ) vê sλˆ − RNλˆ F [2, 1] = ( )2 ˆ c2 RN + h ˆ λ F [2, 3] = ( F [2, 5] = (

−ˆ ve

)2 ˆ RN + h cλˆ 1

) ˆ RN + h cλˆ

Jacobian: F [3, 6] = −1

(7.65)

Third Row (7.66)

Jacobian: Fourth Row −ˆ vn vˆd RNλˆ vê2 ) − tλˆ RNλˆ F [4, 1] = ( )2 − ( ˆ ˆ R + h c2λˆ N RM + h F [4, 3] = (

−ˆ vn vˆd RM

vˆd

F [4, 4] = (

RM F [4, 5] = ( F [4, 6] = (

vê2 tλˆ )2 + ( )2 ˆ ˆ +h RN + h ) ˆ +h

−2ˆ ve tλˆ ) ˆ RN + h vˆn

) ˆ RM + h

F [4, 7] = cθˆcψˆ 93

(7.67)

F [4, 8] = sϕˆsθˆcψˆ − cϕˆsψˆ F [4, 9] = cϕˆsθˆcψˆ + sϕˆsψˆ

(7.68)

Jacobian: Fifth Row −ˆ vn vˆd RNλˆ vê vˆn ) − tλˆ RNλˆ F [5, 1] = ( )2 + ( ˆ ˆ R + h c2λˆ N RN + h F [5, 3] =

−ˆ ve (ˆ vd + vˆn tλˆ ) ( )2 ˆ RN + h

F [5, 4] = (

vê tλˆ

) ˆ RN + h

vˆd + vˆn tλˆ ) F [5, 5] = ( ˆ RN + h F [5, 6] = (

vê

) ˆ RN + h

F [5, 7] = cθˆsψˆ F [5, 8] = sϕˆsθˆsψˆ + cϕˆcψˆ F [5, 9] = cϕˆsθˆsψˆ − sϕˆcψˆ

(7.69)

Jacobian: Sixth Row vˆd2 RMλˆ vê2 RNλˆ ) F [6, 1] = ( )2 + ( ˆ ˆ R + h N RM + h F [6, 3] = (

−ˆ vn vˆd RM

F [6, 5] = ( F [6, 5] = (

vê2 tλˆ )2 + ( )2 ˆ ˆ +h RN + h

−2ˆ ve

) ˆ RN + h −2ˆ v

) ˆ RM + h

F [6, 7] = −sθˆ F [6, 8] = sϕˆcθˆ F [6, 9] = cϕˆcθˆ

(7.70)

ˆ s ˆ ⇒ sin θ, ˆ s ˆ ⇒ cos ϕ, ˆ c ˆ ⇒ cos ψ, ˆ c ˆ ⇒ cos θ, ˆ where, sψˆ ⇒ sin ψ, θ ϕ ψ θ ∂R ∂R ˆ t ˆ ⇒ tan θ, ˆ tˆ ⇒ tan λ, ˆ RM ⇒ M ˆ and RN ⇒ N ˆ . cos ϕ, ˆ ˆ θ λ ∂λ λ ∂λ λ λ λ 94

cϕˆ ⇒

7.5.2

Process Noise Vector and Continuous Process Noise Scaling Matrix: w, Γc

The process noise vector in Eq. (4.29) and the continuous process scaling matrix in Eq. (4.33) for Fifteen state EKF are given by w =

[

wax way waz wp wq wr w1p w1q w1r wbax wbax wbax

]T (7.71)



Γc

0 0 0

0 0 0

0 0 0

    I I  RI  B(1,1) RB(1,2) RB(1,3)  I  RB(2,1) RIB(2,2) RIB(2,3)  I  RB(3,1) RIB(3,2) RIB(3,3)   0 0 0  =  0 0 0    0 0 0   0 0 0   0 0 0   0 0 0   0 0 0   0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 sϕˆ cϕˆ 0 cˆ cθˆ θ 0 cϕˆ −sϕˆ 1 sϕˆtθˆ cϕˆtθˆ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0

95

0 0 0 0 0 0 0 0 0 1 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 1 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 0 0 0 0 0 0 1

                           

(7.72)

7.5.3

Process & Measurement Noise Intensity Matrices: Q, Rk

The process and measurement noise  2 σ ˆ ax 0 0 0 2  0 σ ˆ ay 0 0  2  0 0 σ ˆ az 0   0 0 0 σ ˆp2   0 0 0 0   0 0 0 0  Q = 0 0 0 0   0 0 0 0   0 0 0  0   0 0 0 0   0 0 0 0 0 0 0 0 

Rk

     =    

σ ˆv2λ 0 0 0 0 0 0

intensity matrices for fifteen state EKF are:  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0   0 0 0 0 0 0 0 0   0 0 0 0 0 0 0 0   σ ˆq2 0 0 0 0 0 0 0   0 σ ˆr2 0 0 0 0 0 0   0 0 0 0 0  0 0 σ ˆb21p  0 0 0 σ ˆb21q 0 0 0 0    0 0 0 0 σ ˆb21r 0 0 0   0 0 0 0 0 σ ˆb2ax 0 0   0 0 0 0 0 0 σ ˆb2ay 0  0 0 0 0 0 0 0 σ ˆb2az

0 0 0 0 0 0 2 σ ˆ vµ 0 0 0 0 0 0 σ ˆv2h 0 0 0 0 0 0 σ ˆv2vn 0 0 0 2 0 0 0 σ ˆvve 0 0 2 0 0 0 0 σ ˆvv 0 d 0 0 0 0 0 σ ˆv2ψ

          

(7.73)

(7.74)

where, σ ˆv2λ , σ ˆv2µ , σ ˆv2h , σ ˆv2vn , σ ˆv2ve , σ ˆv2v and σ ˆvψ denote estimates of the standard deviations of λ, d µ,h,vn ,ve ,vd and ψm respectively. (ˆ σb1p , σ ˆb1q , σ ˆb1r ) and (ˆ σbax , σ ˆbay , σ ˆbaz ) denote the estimates of the standard deviations of the accelerometer bias models,rate gyro bias models which are explained below Eq. (7.3) and in Remark 7.2 respectively. 7.5.4

ˆ − , hk , Hk Predicted & Actual Measurement and Measurement Matrix: h k

The INS/GPS predicted and actual measurement vectors for Fifteen state EKF are given as     − ˆ λk  λmk   µ −     ˆk   µmk   ˆ−    h   hmk  ( −)   −k   ˆ k =  vˆnk  (7.75) h x and h (xk ) =   vnm   −  k    vêk   v em   −   k   vˆdk   v dm  k ψˆk− ψ m k | {z } | {z } predicted measurement vector actual measurement vector

96

where, k denotes the discrete time index. Since the measurements for the INS/GPS problem are linear in the state vector (Eqs. (7.38), (7.39), (7.40), (7.41), (7.42), (7.43) and (7.44), the Jacobian matrix Hk is a constant (independent of the predicted state) given by   1 0 0 0 0 0 0  0 1 0 0 0 0 0     0 0 1 0 0 0 0     0 0 0 1 0 0 0 Hk =  (7.76)    0 0 0 0 1 0 0     0 0 0 0 0 1 0  0 0 0 0 0 0 1 For split architecture the continuous process noise scaling matrix, Γ, the process and measurement covariance intensity matrices Q, Rk and the measurement matrix, H, are similarly formulated as presented for the fifteen state EKF in Sections 7.5.2, 7.5.3 and 7.5.4.

97

Chapter 8 GPS Based Inertial Navigation for a Micro Aerial Vehicle: Simulation Results In Chapter 7, a mathematical model for inertial navigation of a micro aerial vehicle (MAV) was developed along with the appropriate sensor models (rate gyroscope and accelerometer). In the case of rate gyroscopes, bias errors (constant & time varying) and zero mean, bandlimited, additive, white Gaussian noise (AWGN) processes were considered while the accelerometer bias error (constant) and zero mean, bandlimited, additive, white Gaussian noise (AWGN) processes. In this chapter, open-loop simulation results for the model developed in Chapter 7 for MAV inertial navigation are shown for a suite of MAV trajectories. The estimation was carried out in four ways: 1. Nine State Filter. 2. Twelve State Filter. 3. Fifteen State Filter. 4. Split Architecture. The observability rank condition is studied for the above four cases. 8.1

Simulated MAV Trajectories

A 6 degree-of-freedom (DOF) model adapted from the open-source Flight Dynamics and Control toolbox (FDC) [33] is used for the simulation study. Turbulence is introduced to the model via the Dryden model [34]. The trajectories shown in Figure 8.1 are the trajectories that are considered for MAV attitude estimation. The following cases are studied which are presented in the form of 4 remarks given below (Remarks 8.1 ∼ 8.4). Remark 8.1. Nine State Filter: The output of the rate gyroscope and accelerometer are unbiased and is assumed to be corrupted by zero mean, bandlimited AWGN processes. State) Filter: The output of the rate gyroscope is corrupted by (wp , wq , wr ), Remark 8.2. ( ) Twelve ( b1p , b1q , b1r & b2p , b2q , b2r , while the accelerometer output was assumed to corrupted by zero mean, bandlimited AWGN processes. Here only b1p , b1q , b1r (rate gyro bias effect due to rate random walk process) are modeled as states in the EKF. 98

Remark 8.3. Fifteen State Filter: The output of the rate gyroscope is corrupted by (wp , wq , wr ), ( ) ( ) b1p , b1q , b1(r & b2p , b2)q , b2r . And the accelerometer output was assumed to corrupted by constant bias bax , bay , baz and zero mean, bandlimited AWGN processes. Here b1p , b1q , b1r (rate gyro bias effect due to rate random walk process) and bax , bay , baz (constant bias) are modeled as states in the EKF. Remark 8.4. Split Architecture: Here two EKF’s are implemented, where one estimates ( ) attitude (ψ, θ, ϕ) and rate gyroscope bias b1p , b1q , b1r which are in turn used in implementing second ( ) EKF to estimate position (λ, µ, h), velocity (vn , ve , vd ) and acclerometer bias bax , bay , baz . Trajectory Start (o); Trajectory Finish (x)

205

Alt (m)

200

195

190

185 200 0

1500 1000

−200

Y (m)

500

−400

0 −600

−500

X (m)

(a) Steady Level Flight Trajectory Start (o); Trajectory Finish (x)

205

Alt (m)

200

195

190

185 200 0

1500 1000

−200

Y (m)

500

−400

0 −600

−500

X (m)


Figure 8.1: Simulated MAV Trajectories. 99

8.1.1

Initial Conditions: Model & Filter

The model and filter initial conditions (for Figures 8.1(a) & 8.1(b))  ˆ (0)     λ 0 λ (0)  µ  µ (0)   0   ˆ (0)      h  h (0)   200   ˆ (0)      vˆ (0)  vn (0)   0   n      vˆ (0)  ve (0)   0   e      vˆ (0)  vd (0)   0   d      ˆ  ψ (0)   0   ψ (0)      ˆ ◦    θ (0) ˆ (0) =  x (0) =  x  θ (0)  =  3.62 ,   ϕ (0)   0   ϕˆ (0)       b1p (0)   0   ˆb1 (0)      p  b1 (0)   0   ˆb (0)     q  1q  b1 (0)   0   ˆ     r  b1r (0)  ba (0)   0  ˆ     x  bax (0)   bay (0)   0   ˆbay (0) 0 baz (0) ˆbaz (0) | {z } | {z Model Initial Condition

are 

                          =                         

0 0 0 0 0 0 0 0 0 0 0 0 0 0 0

                         

(8.1)

}

EKF Initial Condition

8.1.2

Sensor Noise Parameters

The true sensor noise parameters for the accelerometer, rate gyroscope and the GPS are given in Table 8.1 below: Sensor Type Rate Gyroscope Rate Gyroscope Rate Gyroscope Rate Gyroscope Accelerometer Accelerometer Magnetometer GPS GPS

Noise Type Sampling Noise (Std. Dev.) Weiner Process (Std. Dev.) Colored Noise (Std. Dev.) Colored Noise (Time constant) Sampling Noise (Std. Dev.) constant bias Sampling Noise (Std. Dev.) Velocity Sampling Noise (Std. Dev.) Position Sampling Noise (Std. Dev.)

Notation σp , σq , σr σ1p , σ1q , σ1r σ2p , σ2q , σ2r τp , τq , τr σax , σay , σaz bax , bay , baz vψ σvngps , σvegps , σvdgps σxgps , σygps , σzgps

Table 8.1: Sensor noise standard deviations (Std. Dev.).

100

Value 0.05◦ /s 0.05◦ /s 0.05◦ /s 100/s 0.1 m/s2 0.01 m/s2 0.5◦ /s 0.1 m/s 10 m

8.1.3

EKF Parameters: Q, Rk , P (0)

The EKF estimates of the standard deviation entries for Q as represented in Eq. (7.73) are chosen as σ ˆp σ ˆ 1p σ ˆ ax σ ˆb1x

= = = =

σ ˆq = σ ˆr = 0.001◦ /s σ ˆ1q = σ ˆ1r = 0.001◦ /s σ ây = σ âz = 0.001m/s2 σ ˆb1y = σ ˆb1z = 0.001m/s2

(8.2) (8.3) (8.4) (8.5)

Furthermore, the estimates for the standard deviations of the measurements of λ, µ, h, vn , ve , vd , ψ as represented in Eq. (7.73) are chosen as σ ˆ vλ = σ ˆvµ = σ ˆvh = 10m σ ˆvvn = σ ˆvve = σ ˆvvd = 0.1m/s σ ˆψ = σ ˆθ = σ ˆϕ = 0.5◦ /s

(8.6) (8.7) (8.8)

Remark 8.5. For the cases studied in Remarks 8.1 ∼ 8.4, the filter parameters (P (0), Q, R, x(0)) remain unchanged, but the matrix dimensions change according to states simulated in these filters. 8.2

Simulation Scenario - NO GPS Outage

For the MAV trajectories shown in Figures 8.1(a) and 8.1(b), the following cases are studied: 1. Nine state EKF (3 position states, 3 velocity states, 2 attitude states and 1 heading state. Accelerometer and rate gyroscope bias was not considered in the model.) 2. Twelve state EKF (3 position states, 3 velocity states, 2 attitude states, 1 heading state and 3 rate gyroscope bias states. Accelerometer bias states not considered in the model.) 3. Fifteen state EKF (3 position states, 3 velocity states, 2 attitude states, 1 heading state, 3 rate gyroscope bias states and 3 accelerometer bias states.) 4. Split architecture (six state EKF (2 attitude states and 1 heading state and 3 rate gyroscope bias) + nine state EKF (3 position states, 3 velocity states and 3 accelerometer bias states) implemented). Remark 8.6. The sensor set chosen for the four cases studied are as follows: 1. Nine, twelve, fifteen State filters: 7 measurements (3 GPS position, 3 GPS velocity, heading from magnetometer) 2. Split architecture (EKF 1- EKF 2): EKF 1 has θ, ϕ and ψ as measurements where θ and ϕ are derived from an IMU set and GPS while ψ is obtained from a magnetometer. EKF 2 has 3 GPS position and 3 GPS velocity measurements. 101

Remark 8.7. Remark 8.8: The readings of the tri-axial accelerometers in the MEMS IMU set are assumed to be corrupted with constant bias and AWGN processes while the readings of the tri-axial rate gyroscopes in the MEMS IMU set are assumed to be corrupted with time varying bias (Wiener and colored) and AWGN processes. For all the above simulation cases, the noise parameters presented in Table 8.1 is used. 8.2.1

Case 1: Estimation using Nine State Filter

Figure 8.2 and 8.4 shows the estimation results for the MAV trajectories shown in Figures 8.1(a) and 8.1(b) using nine state filter at GPS update rate of 10 Hz. Observability index


11

x (m)

10 9

10

63.821

0

63.82 0

8

observability index

Estimation Errors

63.822

200

−10

400

0

200

400

0

200

400

200

400

7 0.2

10

0

0

y (m)

6 5

−0.2 4

0

200

−10

400

3

h (m)

2 1

210

10

200

0

190 0

0

100

200

300

400

0

200

500

Time (secs)

(a) Observability index Truth (Solid); Estimate (Dotted)

Estimation Errors

0

200

400

1

0

0

−100

−1

V

d

Ve

100

0

200

400

1

1

0

0

−1

−1

0

200

400

ψ (deg)

0 −1

400

0

200

400

0

200

400

0

Time (secs)

200

400

200

0

0

−0.2

0

100

200

300

400

500

0

3

−0.2

0

100

200

300

400

500

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0.2

0 −5 −10 −15

0 100

200

300

Time (secs)

(c) Velocity (m/s)

0

0.2

3.5

0

Time (secs)


4

θ (deg)

0

Time (secs)


1

−100

0

(b) Position

φ (deg)

Vn

100

−10

400

Time (secs)

400

500

−0.2

Time (secs)

(d) Euler Angles (deg)

Figure 8.2: Nine State EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(a). Figure 8.2(a) show the observability index. Figures 8.2(b), 8.2(c) and 8.2(d) show the performance of the EKF in estimating the position, velocity, attitude and heading. Thus 102

it is shown that if there is no bias errors in the IMU measurement suit, all the states are estimated via implementing EKF as nine state filter. Observability index


11

x (m)

10 9

20

63.821

10

63.82 0

8

observability index

Estimation Errors

63.822

200

0

400

0

200

400

0

200

400

200

400

7 0.2

y (m)

6 5

10

0

0

−0.2

−10

4

0

200

400

3

h (m)

2 1

210

20

200

10

190 0

0

100

200

300

400

0

200

500

Time (secs)


0

0 −1

0

200

400

Time (secs)


ψ (deg)

vn

Estimation Errors

−100

0

200

0

(b) Position

1

100

0

400

Time (secs)

200 0

400

0

100

200

300

400

500

Estimation Errors 1 0 −1 −2 −3

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

Estimation Errors 0

0

−100

−1

0

200

400

v

d

2

θ (deg)

1

0

200

0

0 0

200

400

−1

0

Time (secs)

200

0 3 0

1

1

1

4

400

φ (deg)

ve

100

400

200

300

500

1 0 100

200

300

Time (secs)

(c) Velocity (m/s)

400

0 −5 −10 −15 0

Time (secs)

100

−1

400

500

−1

Time (secs)


Figure 8.3: Nine State EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(a). Here the IMU sensor suit is assumed to be corrupted by the sensor bias as presented in Remark 8.7. And the sensor bias (both accelerometer bias and rate gyroscope bias) is not modeled in the EKF. Figure 8.3(a) show the observability index. Figures 8.3(b), 8.3(c) and 8.3(d) show the performance of the EKF in estimating the position, velocity, attitude and heading. Thus it is shown that if sensor bias is not modeled on the EKF, the estimation errors accumulate with time.

103

Observability index


11

x (m)

9

10

63.82

0

63.818

−10

8

observability index

Estimation Errors

63.822

10

0

200

400

0

200

400

0

200

400

200

400

7 0.2

y (m)

6 5

10

0

0

−0.2

−10

4

0

200

400

3 210

h (m)

2 1 0

0

100

200

300

400

10

200

0

190

−10

0

200

500

400

(a) Observability index Truth (Solid); Estimate (Dotted) 0 −1

0

200

400

0

200

400

0

0

−100

−1

0

200

400

1

0

200

1

0

0

−1

−1

0

200

400

200

0

0

−0.2

0

100

200

300

400

500

0

Time (secs)

200

400

0

100

200

300

400

500

200

300

400

500

−0.2

0

100

200

300

400

500

0

100

200

300

400

500

0.2

0 −5 −10 −15

0 100

200

300

Time (secs)

(c) Velocity (m/s)

100

0

3

0

Time (secs)

0

0.2

4 2

400


5

1

θ (deg)

Ve

100

d

400

ψ (deg)

0

V


Estimation Errors 1

−100

Time (secs)

(b) Position

φ (deg)

Vn

100

0

Time (secs)

Time (secs)

400

500

−0.2

Time (secs)


Figure 8.4: Nine State EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(b). For the MAV trajectory shown in Figure 8.1(b), Figures 8.4(b), 8.4(c) and 8.4(d) show that the position, velocity and Euler angle estimate, illustrating the robustness of the EKF to various MAV maneuvers.

104

Observability index


11

x (m)

63.822

10 9

63.82 63.818

observability index

8

0

200

Estimation Errors 10 0 −10 −20 −30

400

0

200

400

0

200

400

200

400

7 0.2

10

0

0

y (m)

6 5

−0.2 4

0

200

−10

400

3 210

h (m)

2 1

190 0

0

100

200

300

400

10 0 −10 −20 −30

200 0

200

400

0

Time (secs)

500

Time (secs)

Time (secs)


Estimation Errors


0

0

−100

−1

0

200

400

400

ψ (deg)

1

100

vn

(b) Position

0

200

200 0

400

Estimation Errors 6 4 2 0

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

Estimation Errors 0

v

d

−100

5

θ (deg)

1 0 0

200

400

−1

2

1

1

0

0

200

0 0

200

400

−1

0

Time (secs)

200

400

1

4

0

3 2

400

φ (deg)

ve

100

−1 0

200

300

500

2 1 0 100

200

300

Time (secs)

(c) Velocity (m/s)

400

0 −5 −10 −15 0

Time (secs)

100

400

500

−1

Time (secs)


Figure 8.5: Nine State EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(a). Here the IMU sensor suit is assumed to be corrupted by the sensor bias as presented in Remark 8.7. And the sensor bias (both accelerometer bias and rate gyroscope bias) is not modeled in the EKF. Figure 8.5(a) show the observability index. Figures 8.5(b), 8.5(c) and 8.5(d) show the performance of the EKF in estimating the position, velocity, attitude and heading. Thus it is shown that if sensor bias is not modeled on the EKF, the estimation errors accumulate with time. 8.2.2

Case 2: Estimation using Twelve State Filter

Here as the accelerometer and rate gyroscope sensors are corrupted with constant bias and time varying bias respectively. The time varying bias (b1p , b1q , b1r ) is modeled in the EKF and the effect of not modeling the constant bias (bax , bay , baz ) on the EKF performance is to be studied. Thus leading to increment in number of states from nine to twelve. This calls for 105

computing the observability index to check if the system is observable with 7-measurements. Observability index 14

12

observability index

10

8

6

4

2

0

0

100

200

300

400

500

Time (secs)

(a) MAV Trajectory in Figure 8.1(a) Observability index 14

12

observability index

10

8

6

4

2

0

0

100

200

300

400

500

Time (secs)

(b) MAV Trajectory in Figure 8.1(b).

Figure 8.6: Observability index Twelve State Filter Figure 8.6 shows the observability index for the system simulated with 7 measurements. The computed observability index is 12 for both the MAV trajectories shown in Figure 8.1(a) and 8.1(b). 106


Estimation Errors

20

vn

63.821

0 200

−10

400

0

0

−20

−1

0

200

400

10

0

0

−0.2

−10

0

200

400

210

0

190

−10

0

200

400

100

200

0

200

400

0

0 −1

0

100

200

0

200

400

100

200

300

400

500

b1 (deg/sec) 100

200

300

400

b1 (deg/sec)

0

q

3 2

0

100

200

300

400

500

−1

0

100

200

300

400

0 100

200

0

100

200

300

Time (secs)

400

500

0

400

500

0

100

200

300

400

500

400

500

0

100

200

300

400

500

Time (secs)

100

200

300

400

500

0

0

−0.2 200

−0.2

0

200

400

0

200

400

200

400

0.2

0

0

−0.2 200

400

−0.2

0.2

0.2 0

0

−0.2 200

Time (secs)

(c) Euler Angles (deg)

400

0.2

0

Time (secs)


0

0 −1

300

0.2

500

1

0 −5 −10 −15

500

−1

0

1

4

400

0

500

r

θ (deg)

5

0

300


p

0

200

(b) Velocity (m/sec)

b1 (deg/sec)

ψ (deg)

0

100

Time (secs)

Estimation Errors

−1

300

−2 0

1

0

0

1

(a) Position

200

500

1

Time (secs)


400

2

Time (secs)

400

300

−20

10

200

0

20

ve

y (m)

0.2

φ (deg)

Estimation Errors 1

63.82 0

h (m)


10

vd

x (m)

63.822

400

−0.2

0

Time (secs)

(d) Gyro bias (deg/sec)

Figure 8.7: Twelve State EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(a). Figure 8.7 shows the EKF performance in estimating position, velocity, Euler angles and rate gyroscope bias. Because of not modeling accelerometer constant bias in the EKF it is seen from Figures 8.7(b) and 8.7(c) that the estimates of velocities and Euler angles are observed to be biased. Figure 8.7(d) shows the estimates of rate gyro bias estimates (b1p , b1q , b1r ). Thus we can say that it is necessary to model the accelerometer bias in the EKF implementation.

107


Estimation Errors 0 −10

0

200

400

0

200

400

0

0 −0.5

10

0

0

−0.2

−10

0

200

400

210

0

190

−10

0

200

400

100

200

200

400

0

0 −0.5

0

100

200

0

200

400

100

200

300

400

500

b1 (deg/sec) 100

200

300

400

q

−1

b1 (deg/sec)

0

2

0

100

200

300

400

500

0

100

200

300

400

0 100

200

0

100

200

300

Time (secs)

400

500

0

400

500

0

100

200

300

400

500

400

500

0

100

200

300

400

500

Time (secs)

100

200

300

400

500

0

0

−0.2 200

−0.2

0

200

400

0

200

400

200

400

0.2

0

0

−0.2 200

400

−0.2

0.2

0.2 0

0

−0.2 200

Time (secs)

(c) Euler Angles (deg)

400

0.2

0

Time (secs)


0

0 −1

300

0.2

500

1

0 −5 −10 −15

500

−2

0

1

4

400

0

500

r

φ (deg)

θ (deg)

6

0

300


p

0

200

(b) Velocity (m/s)

b1 (deg/sec)

ψ (deg)

0

100

Time (secs)

Estimation Errors

−1

300

−2 0

1

0

0

2

(a) Position

200

500

0.5

Time (secs)


400

2

Time (secs)

400

300

−20

10

200

0

20

0


−20

ve

y (m)

0.2

20

vn

63.82 63.818

h (m)


10

vd

x (m)

63.822

400

−0.2

0

Time (secs)

(d) Gyro bias (deg/sec)

Figure 8.8: Twelve State EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(b). And for the MAV trajectory shown in Figure 8.1(b), Figures 8.8(a), 8.8(b) and 8.8(c) show that estimates of position, velocity an Euler angles. The degradation of EKF performance is observed because of not modeling the accelerometer bias in the filter implementation. Thus to model the accelerometer bias also in the EKF leads to a fifteen state EKF. 8.2.3

Case 3: Estimation using Fifteen State Filter

Here, the rate gyroscope and accelerometer sensors are corrupted with sensor bias (b1p , b1q , b1r ) and (bax , bay , baz ) respectively. These are modeled in the filter as shown in Eqn. (7.23). Thus the number of states to be estimated are 15. The corresponding state vector is shown in Eqn. (7.47). This calls for computing the observability index to check if the system is observable with 7-measurements.

108

Observability index


16

x (m)

14

observability index

10

63.821

0

63.82

12

0

10

200

400

y (m)

6

0

0 −10

0

200

400

h (m)

210

2

0

100

200

300

400

200

0

190

−10

0

200

500

400

0

200

400

0

0 −1

0

200

400

d

1

ψ (deg) 0

200

0 −2

400

0

0 −1

0

200

400

0

Time (secs)

200

0

100

0

100

400

0

100

200

300

400

200

300

400

Estimation Errors

0

100 200 300 400 500

x

0

y

0

−0.2 0

100 200 300 400 500

−0.2

0

100 200 300 400 500

0

0

−0.2 0

100 200 300 400 500

Time (secs)

−0.2

400

500

0

100

200

300

400

500

0

100

200

300

400

500

Estimation Errors 0

0.4 0.2 0 −0.2

200

400

0

100 200 300 400 500

Time (secs)

200

400

0

200

400

200

400

0 0

200

400

−0.5

0.2

0

0

−0.2

−0.2

0

200

Time (secs)

(e) Gyro bias (deg/sec)

0

0.5

0.2 z

0.2

0.2

300

−0.5 0

ba

0

0.4 0.2 0

100 200 300 400 500

0.2

0.2

200

0.5

0 −0.2

500

4 2 0 −2 −4

100


ba

0 −0.2

500

4 2 0 −2 −4

0

Time (secs)

0.2

0.2

500


ba

b1p (deg)

400

Time (secs)

(c) Velocity (m/sec)

b1q (deg)

300

0 −5 −10 −15

Time (secs)


200

6 4 2 0

400

1

−1

400

Estimation Errors

0

1

−100

V

200

200

0

2

200

θ (deg)

Ve

100

0

400

Time (secs)

400

φ (deg)

Vn

0 −1

200


Estimation Errors

0

0

(b) Position

1

−100

400

Time (secs)


200

10

Time (secs)

100

0

10

−0.2

4

b1r (deg)

−10

0.2

8

0

Estimation Errors

63.822

400

0

Time (secs)

(f) Accelerometer bias (m/sec)

Figure 8.9: Fifteen state EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(a). 109

Figure 8.9(a) shows the observability index for the system simulated with 7 −measurements. The computed observability index is 13 as shown in Figure 8.1(a). Therefore loss in observability is observed in a fifteen state filter with 7 − measurements. At GPS update rate of 10 hz and with sensor bias in rate gyroscopes and accelerometers it can be seen from Figure 8.9(f) that the accelerometer bias estimates (bax , bay ) are drifting away. This is because of loss in observability while implementing fifteen state filter. The effect of the degradation in the accelerometer bias estimates manifests itself in the performance of ϕ and θ via the measurement model of EKF 1. This can be seen in Figure 8.9(d) which shows a drift building up in the estimates of ϕ and θ. Observability index


16

x (m)

14

10

63.82

0

63.818

−10

10

0

200

400

0.2

y (m)

observability index

12

8 6 4

h (m) 0

100

200

300

400

0

0

−0.2

−10

0

200

400

200

0

190

−10

0

200

500

400

0

100

200

300

400

500

20

2

0

0

−20

0

100

200

300

400

500

−2

1

2

0

0

−1


0

100

200

300

400

500

−2

ψ (deg) 0

100

200

300

400

500

θ (deg)

0 −1

0

100

200

0

100

Time (secs)

200

200

400

200

400

0

Time (secs)

400

300

400

500

φ (deg)

vn (m/sec) ve (m/sec) vd (m/sec)

Estimation Errors

0

0

(b) Position

1

−20

400

Time (secs)


200

10

Time (secs)

20

0

10

210

2 0

Estimation Errors

63.822

300

400

500

200

0

0

−2

6 4 2 0 −2

0

0

100

100

200

200

300

300

100

200

300

Time (secs)

(c) Velocity (m/s)

400

400

500

500

0 −5 −10 −15 0

Time (secs)

Estimation Errors 2

400

500

4 2 0 −2 −4

4 2 0 −2 −4

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

Time (secs)


Figure 8.10: Fifteen state EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(b).

110

x

0 100 200 300 400 500

−0.2

0

−0.5

100 200 300 400 500

0

−0.2 0

100 200 300 400 500

−0.2

ba

0

0

0

0

100 200 300 400 500

−0.2

200

400

0

200

400

0

200

400

200

400

0.5

0 −1

100 200 300 400 500

0 0

200

−0.5

400

0.2

0

−0.2

0

−0.5

0.2

0.2

0 −0.5

0.5 y

0.2

0.2


0

z

b1q (deg)

Truth (Solid); Estimate (Dotted) 1 0.5

ba

0 −0.2 0

b1r (deg)


0

100 200 300 400 500

Time (secs)

0.2

0

0

−0.2

−0.2

ba

b1p (deg)


0

200

Time (secs)

400

0

Time (secs)

(a) Gyro bias (deg/sec)

Time (secs)

(b) Accelerometer bias (m/sec)

Figure 8.11: Fifteen state EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(b). Remark 8.8. The loss of observability associated with a full 15 state INS-GPS EKF implementation has motivated the idea of splitting the navigation and bias states as position, velocity and accelerometer bias and attitude, heading and rate gyroscope bias. 8.2.4

Case 1: Estimation using Split Architecture

The loss of observability seen in fifteen state filter can be avoided by implementing 2 − EKFs: One for Attitude and heading estimation problem and other for inertial navigation solution (six state EKF (2 attitude states and 1 heading state and 3 rate gyroscope bias) + nine state EKF (3 position states, 3 velocity states and 3 accelerometer bias states) implemented). The state vector and measurement vector for attitude and heading estimation are shown in Eqn. (7.48) and the state vector and measurement vector for inertial navigation are shown in Eqn. (7.49). The output of accelerometers and rate gyroscopes are corrupted with sensor bias (b1p , b1q , b1r ) and (bax , bay , baz ) respectively. Observability index


8

x (m)

63.822

AHRS

6 4


63.821

0

63.82 0

200

400

−10

0

200

400

0

200

400

200

400

2 0.2 0

100

200

300

400

500

y (m)

0

10

10

0

0

−0.2

−10

0

200

400

6

210

h (m)

INS

8

4 2 0

0

100

200

300

400

500

10

200

0

190

−10

0

200

400

0

Time (secs)

Time (secs)

(a) Observability index

(b) Position

111

Time (secs)

Truth (Solid); Estimate (Dotted) 0 −1

0

200

400

0

200

0

0

−0.5

400

0

0 −1

0

200

400

1

0

200

0

0 −1

0

200

400

100

0

Time (secs)

200

0 −0.5

0

100

300

400

500

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0 100

200

300

400

500

−0.5

Time (secs)

Truth (Solid); Estimate (Dotted) ba (m/sec)

0.2


0

0

−0.2

−0.1

x

0

200

400

200

400

0.1 0 −0.1

0

200

400

0.2

0

200

400

0

200

400

200

400

0.1

0

0

−0.2

−0.1

y

0 −0.2

ba (m/sec)

0.2

0

q

b1 (deg/sec)

200


0

200

400

200

400

0.1 0 −0.1

0

200

400

0.02

0.01

0

0

−0.02

−0.01

z

0 −0.2

ba (m/sec)

0.2

0

r

b1 (deg/sec)

100

Time (secs)

Estimation Errors 0

0

0.5

0

0.1

−0.1

500

0 −5 −10 −15

400

p

b1 (deg/sec)


400

3


−0.2

300

0.5

Time (secs)

0.2

200

3.5

400

1

−1

0

4

1

−100


200

θ (deg)

Ve

100

d

400

ψ (deg)

0 −100

V


Estimation Errors 1

φ (deg)

Vn

100

0

200

Time (secs)

400

0

200

400

Time (secs)

0

200

Time (secs)


400

0

Time (secs)


Figure 8.12: Split Architecture EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(a). Figure 8.12(a) shows the observability index for split-architecture (both AHRS and INS). Thus it shows that system is F ull state observable. The estimates of the position, velocity, attitude, gyroscope bias and accelerometer bias estimates clearly track the truth data as seen in Figures 8.13(b) ∼ 8.12(f) respectively.

112

Observability index


8

x (m)

6

AHRS

Estimation Errors

63.822

4

10

63.82

0

63.818

−10

0

200

400

0

200

400

0

200

400

200

400

2 0.2 0

100

200

300

400

500

y (m)

0

10

10

0

0

−0.2

−10

0

200

400

6

210

h (m)

INS

8

4 2 0

0

100

200

300

400

10

200

0

190

−10

500

0

200

400

(a) Observability index Estimation Errors


0

0 −1

0

100

200

300

400

500

0

100

200

300

400

0

0

−0.5

500

2

0

0

−20

−2

0

100

200

300

400

500

1 0

0 −2

0

100

200

300

400

500

100

200

0

Time (secs)

100

200

300

400

2

500

100

200

300

400

0

100

300

400

500

200

300

400

500

−2

0

100

200

300

400

500

0

100

200

300

400

500

0 100

200

300

400

500

−2

Time (secs)

(d) Euler Angles (deg) Truth (Solid); Estimate (Dotted) 0.2


0

0

x

ba (m/sec)

0 −0.1

200

Time (secs)

Estimation Errors

0

100

2

0

0.1

−0.2

0

0

0 −5 −10 −15

500

p

b1 (deg/sec)


200

400

0.1

0

0

−0.2

−0.1

200

400

−0.2

0

200

−0.2 100

400

0.2

200

300

400

0.2

0

0

−0.2

−0.2

y

0.2

0

ba (m/sec)

0

q

b1 (deg/sec)

500

3


200

400

0.1

0

0

−0.2

−0.1

200

400

0

200

400

0.2

0

200

400

200

400

0.2

0

0

−0.2

−0.2

z

0.2

0

ba (m/sec)

0

r

b1 (deg/sec)

400

2

Time (secs)

0.2

300

4

2

−1

0

5

0


200

θ (deg)

20

400

ψ (deg)

1

−20

Time (secs)

(b) Position

φ (deg)

vd (m/sec)

ve (m/sec)

vn (m/sec)


0

Time (secs)

Time (secs)

0

200

Time (secs)

400

0

200

400

Time (secs)

250

300

350

400

Time (secs)


450

0

Time (secs)


Figure 8.13: Split Architecture EKF Performance for Inertial Navigation - MAV Trajectory in Figure 8.1(b). 113

Figure 8.13(a) shows the observability index for split-architecture (both AHRS and INS). Thus it shows that system is F ull state observable. The estimates of position, velocity, attitude, gyroscope bias and accelerometer bias estimates clearly track the truth data as seen in Figures 8.13(b) ∼ 8.13(f) respectively. 8.2.5

Case 1: Monte Carlo simulations for Split Architecture - Intermittent GPS outage

Two blocks of GPS outage (250 ∼ 270, 390 ∼ 405) was considered in the simulations. 1000 Monte Carlo simulations were made on split architecture algorithm for the maneuvers shown in Figures 8.1(a) and 8.1(b). Figures 8.14(a), 8.15(a) and 8.16(a) show the observability indices computed during the Monte Carlo simulations of INS/GPS with GPS update rates 4 Hz, 5 Hz and 10 Hz respectively. These plots clearly indicate that there is no loss in observability through out the simulations. Observability index

RMSE: Position (m)

8 10

xrmse

AHRS

6 4

0 −10

2 0

200

400

600

800

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0 −10

10

10

5

hrmse

INS

0 10

1000

yrmse

0

0 −10

0

0

200

400

600

800

1000

Time (secs)

Number of simulation

(a) Observability index 4 Hz

(b) GPS 4 Hz

RMSE: Position (m)

RMSE: Position (m) 10

xrmse

xrmse

10 0 −10 0

100

200

300

400

500

0

yrmse

yrmse

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

10

10 0

0 −10

−10 0

100

200

300

400

500 10 rmse

10 0

0

h

hrmse

0 −10

−10

−10 0

100

200

300

400

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 8.14: EKF Performance in Estimating Position - MAV Trajectory in Figure 8.1(a). 114

1000 Monte Carlo simulations were simulated For the MAV trajectory show in Figure 8.1(a). Figures 8.14(b), 8.14(c) and 8.14(d) show the RMSE plots for Position (x, y, h) with GPS update rate of 4 Hz, 5 Hz an 10 Hz respectively. As the GPS update rate is increased the RMSE in estimates is observed to decrease. Observability index

RMSE: Velocities (m/sec)

8

2 rmse

vn

AHRS

6 4

−2

2

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2 200

400

600

800

1000

rmse

0

ve

0

0

10

0 −2

rmse

5

vd

INS

2

0

0 −2

0

200

400

600

800

1000

Time (secs)



(b) GPS 4 Hz RMSE: Velocities (m/sec)


2 rmse

0 −2

vn

vn

rmse

2

−2 0

100

200

300

400

500

rmse

ve

rmse

ve

0

100

200

300

400

500

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2 rmse

0

vd

rmse

vd

100

0 −2

0

2

−2

0

2

2

−2

0

0

100

200

300

400

0

−2

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 8.15: EKF Performance in Estimating Velocities - MAV Trajectory in Figure 8.1(a). Figures 8.15(b), 8.15(c) and 8.15(d) show the RMSE plots for velocity (vn , ve , vd ) with GPS update rate of 4 Hz, 5 Hz an 10 Hz respectively. As seen in the Figure 8.14 it is observed that the error in the estimates are decreased with increase in the GPS measurement update rate.

115

Observability index


8

2

ψrmse

AHRS

6 4

0 −2

2

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2 200

400

600

800

1000

rmse

0

θ

0

10

0

−2

rmse

5

φ

INS

2

0

0

200

400

600

800

0

−2

1000


Time (secs)


(b) GPS 4 Hz



ψrmse

ψ

rmse

2 0 −2

−2 0

100

200

300

400

500

θrmse

rmse

θ

0

100

200

300

400

500

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2

φrmse

0 2

rmse

100

0 −2

−2

φ

0

2

2

0 −2

0

0

100

200

300

400

0 −2

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 8.16: EKF Performance in Estimating Euler angles - MAV Trajectory in Figure 8.1(a). Figures 8.16(b), 8.16(c) and 8.16(d) show the RMSE plots for Euler angles (ψ, θ, ϕ) with GPS update rate of 4 Hz, 5 Hz an 10 Hz respectively. It is observed that during GPS outage the RMSE increases until there is a GPS packet available. As soon as GPS packet is made available the error decreases. And it is observed that when there is no GPS outage the RMSE is within the standard deviation (1 − σ) of the white gaussian noise (AWGN) as given in Table 8.1. Figures 8.17(a), 8.18(a) and 8.19(a) show the Observability indices computed during the Monte Carlo simulations of INS/GPS for MAV trajectory shown in Figure 8.1(b) with GPS update rates 4 Hz, 5 Hz and 10 Hz respectively. These plots clearly indicate that there is no loss in observability through out the simulations.

116

RMSE: Position (m)

Observability index 50

xrmse

8

AHRS

6

0

4 −50 2 0

200

400

600

800

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0 −10

10

10

5

hrmse

INS

100

10

1000

yrmse

0

0

0 −10

0

0

200

400

600

800

1000


Time (secs)


(b) GPS 4 Hz

RMSE: Position (m)


xrmse

xrmse

50 0 −50

−50 0

100

200

300

400

500

yrmse

yrmse

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

10

10 0

0 −10

−10 0

100

200

300

400

500

10

10

hrmse

hrmse

0

0 −10

0 −10

0

50

100

150

200

250

300

350

400

450

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 8.17: EKF Performance in Estimating Position - MAV Trajectory in Figure 8.1(b). 1000 Monte Carlo simulations were performed for the MAV trajectory show in Figure 8.1(b). Figures 8.17(b), 8.17(c) and 8.17(d) show the RMSE plots for Position (x, y, h) with GPS update rate of 4 Hz, 5 Hz an 10 Hz respectively. As the GPS update rate is increased the RMSE in estimates is observed to decrease.

117


Observability index

2 rmse

8

vn

AHRS

6

0

4 −2 2

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2 200

400

600

800

1000

rmse

0

ve

0

10

0

−2

rmse

5

vd

INS

2

0

0

200

400

600

800

0

−2

1000

Time (secs)




RMSE: Velocities (m/sec) 2 rmse

0 −2

vn

vn

rmse

2

−2 0

100

200

300

400

500

rmse

ve

rmse

ve

0

100

200

300

400

500

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2 rmse

0

vd

rmse

vd

100

0

−2 0

2

−2

0

2

2

−2

0

0

100

200

300

400

0

−2

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 8.18: EKF Performance in Estimating Velocities - MAV Trajectory in Figure 8.1(b). Figures 8.18(b), 8.18(c) and 8.18(d) show the RMSE plots for velocity (vn , ve , vd ) with GPS update rate of 4 Hz, 5 Hz an 10 Hz respectively. As seen in the Figure 8.17 it is observed that the error in the estimates are decreased with increase in the GPS measurement update rate.

118

Observability index


8

ψrmse

2

AHRS

6 4

−2

2

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2 0

200

400

600

800

1000

θrmse

0

0

10

0 −2

5

φrmse

INS

2

0

0

200

400

600

800

0 −2

1000


Time (secs)


(b) GPS 4 Hz RMSE: Euler Angles (deg)


2

ψrmse

ψrmse

2 0 −2

0

100

200

300

400

−2

500

θrmse

θrmse

0

0

100

200

300

400

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2

φrmse

φrmse

100

0 −2

500

2 0 −2

0

2

2

−2

0

0

100

200

300

400

0

−2

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 8.19: EKF Performance in Estimating Euler angles - MAV Trajectory in Figure 8.1(b). Figures 8.19(b), 8.19(c) and 8.19(d) show the RMSE plots of Euler angles (ψ, θ, ϕ) for MAV trajectory shown in Figure 8.1(b) with GPS update rate of 4 Hz, 5 Hz an 10 Hz respectively. It is observed that during GPS outage the RMSE increases until there is a GPS packet available. As soon as GPS packet is made available the error decreases. And it is observed that when there is no GPS outage the RMSE is within the standard deviation (1 − σ) of the white gaussian noise (AWGN) as given in Table 8.1. 8.3

Other Maneuvers

Any EKF algorithm has to be implemented for various maneuvers which helps us to study the robustness of the filter. Various maneuvers were simulated using a 6 degree-of-freedom (DOF) model adapted from the open-source Flight Dynamics and Control toolbox (FDC) [33], turbulence is introduced to the model via the Dryden model [34]. The trajectories 119

shown in Figure 6.24 are the trajectories that are considered for MAV Inertial Navigation. Trajectory Start (o); Trajectory Finish (x)

205

Alt (m)

200

195

190

185 200 100

100

50

Y (m)

0

0

X (m)

−50 −100

−100

(a) Coordinated Turn (Approximately Constant Altitude)


205

Alt (m)

200

195

190

185 200 100

100

Y (m)

50 0

0 −50 −100

−100

X (m)

(b) Coordinated Turn (Approximately Constant Altitude): Turbulence


120

8.4

Simulation Scenario: Intermittent GPS outage

Two blocks of GPS outage (250 ∼ 270, 390 ∼ 405) was considered in the simulations. 1000 Monte-carlo simulations were made on split architecture algorithm for the maneuvers shown in Figures 8.20(a) and 8.20(b). 8.4.1

Case 1: Monte Carlo simulations for Split-Architecture - Intermittent GPS outage

Figures 8.21(a), 8.22(a) and 8.23(a) show the Observability indices computed during the Monte Carlo simulations of INS/GPS for MAV trajectory shown in Figure 8.20(a) with GPS update rates 4 Hz, 5 Hz and 10 Hz respectively. These plots clearly indicate that there is no loss in observability through out the simulations. RMSE: Position (m)


xrmse

10

AHRS

6 4

0 −10

2 0

200

400

600

800

1000

yrmse

0

100

200

300

400

500

0

100

200

300

400

500

0 −10

10

10

5

hrmse

INS

0 10

0 −10

0

0

200

400

600

800

1000

0

50

100


300

350

400

450

500


xrmse

10

xrmse

250

(b) GPS 4 Hz

RMSE: Position (m)

0

0 −10

−10 0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

10

yrmse

10

yrmse

200

Time (secs)


0

0 −10

−10 0

50

100

150

200

250

300

350

400

450

500 10

hrmse

10

hrmse

150

0

0 −10

−10 0

100

200

300

400

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 8.21: EKF Performance in Estimating Position - MAV Trajectory in Figure 8.20(a). 1000 Monte Carlo simulations were performed for the MAV trajectory show in Figure 121

8.20(a). Figures 8.21(b), 8.21(c) and 8.21(d) show the RMSE plots for Position (λ, µ, h) with GPS update rate of 4 Hz, 5 Hz an 10 Hz respectively. It is observed that during GPS outage the RMSE increases until there is a GPS packet available. As soon as GPS packet is made available the error is observed to decrease. RMSE: Velocities (m/sec)

Observability index

2 rmse

8

vn

AHRS

6

0

4 −2 2

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2 200

400

600

800

1000

rmse

0

ve

0

10

0 −2

rmse

5

vd

INS

2

0

0 −2

0

200

400

600

800

1000

Time (secs)




RMSE: Velocities (m/sec) 2 rmse

0 −2

vn

vn

rmse

2

0

100

200

300

400

−2

500

rmse

ve

rmse

ve

0

0

100

200

300

400

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

rmse

2

0

vd

rmse

vd

100

0 −2

500

2

−2

0

2

2

−2

0

0

−2 0

100

200

300

400

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 8.22: EKF Performance in Estimating Velocity (m/sec) - MAV Trajectory in Figure 8.20(a). Figures 8.22(b), 8.22(c) and 8.22(d) show the RMSE plots for velocity (vn , ve , vd ) with GPS update rate of 4 Hz, 5 Hz an 10 Hz respectively. It is observed that during GPS outage the RMSE increases until there is a GPS packet available. As soon as GPS packet is made available the error decreases. And it is observed that when there is no GPS outage the RMSE is within the standard deviation (1 − σ) of the white gaussian noise (AWGN) as given in Table 8.1.

122



ψrmse

8

AHRS

6

0

4 −2 2

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

500

2 0

200

400

600

800

1000

θrmse

0

10

0

−2

5

0

φrmse

INS

2

0

200

400

600

800

0

−2

1000


Time (secs)




ψrmse

ψrmse

2 0 −2

0

100

200

300

400

−2

500

0

0

100

200

300

400

200

300

400

0

100

200

300

400

500

0

100

200

300

400

500

2

φrmse

φrmse

100

0 −2

500

2

0

−2

0

2

θrmse

θrmse

2

−2

0

0

100

200

300

400

0

−2

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 8.23: EKF Performance in Estimating Euler angles - MAV Trajectory in Figure 8.20(a). Figures 8.23(b), 8.23(c) and 8.23(d) show the RMSE plots of Euler angles (ψ, θ, ϕ) for MAV trajectory shown in Figure 8.20(a) with GPS update rate of 4 Hz, 5 Hz an 10 Hz respectively. It is observed that during GPS outage the RMSE increases until there is a GPS packet available. As soon as GPS packet is made available the error decreases. And it is observed that when there is no GPS outage the RMSE is within the standard deviation (1 − σ) of the white gaussian noise (AWGN) as given in Table 8.1. Figures 8.24(a), 8.25(a) and 8.26(a) show the Observability indices computed during the Monte Carlo simulations of INS/GPS for MAV trajectory shown in Figure 8.20(b) with GPS update rates 4 Hz, 5 Hz and 10 Hz respectively. These plots clearly indicate that there is no loss in observability through out the simulations.

123

RMSE: Position (m)

Observability index

50

xrmse

8

AHRS

6

0

4 −50 2 200

400

600

800

1000

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

0 −10

10

10 5

hrmse

INS

100

10 0

yrmse

0

0

0 −10

0

0

200

400

600

800

1000

Time (secs)


(a) observability index 4 Hz

(b) GPS 4 Hz

RMSE: Position (m)


xrmse

xrmse

50 0 −50

−50 0

100

200

300

400

500

yrmse

yrmse

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

10

10 0

0 −10

−10 0

100

200

300

400

500 10

hrmse

10

hrmse

0

0

0 −10

−10 0

100

200

300

400

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 8.24: EKF Performance in Estimating Position - MAV Trajectory in Figure 8.20(b). 1000 Monte Carlo simulations were performed for the MAV trajectory show in Figure 8.20(b). Figure 8.24(a) shows the observability index computed during the Monte Carlo simulations, which clearly indicates there is no loss in observability through out the simulations. Figures 8.24(b), 8.24(c) and 8.24(d) show the RMSE plots for Position (λ, µ, h) with GPS update rate of 4 Hz, 5 Hz an 10 Hz respectively.

124


Observability index

2 rmse

8

vn

AHRS

6 4

−2

2

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2 200

400

600

800

1000

rmse

0

ve

0

0

10

0

−2

rmse

5

vd

INS

2

0

0

−2 0

200

400

600

800

1000

Time (secs)



(b) GPS 4 Hz


RMSE: Velocities (m/sec) rmse

2

0 −2

vn

vn

rmse

2

0

50

100

150

200

250

300

350

400

450

−2

500

rmse

ve

rmse

ve

0

100

200

300

400

500

200

300

400

500

0

100

200

300

400

500

rmse

2

0

vd

rmse

vd

100

0 −2

0

2

−2

0

2

2

−2

0

0

−2 0

50

100

150

200

250

300

350

400

450

500

0

50

100

150

200

250

300

350

400

450

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 8.25: EKF Performance in Estimating Velocity (m/sec) - MAV Trajectory in Figure 8.20(b). 1000 Monte Carlo simulations were performed for the MAV trajectory show in Figure 8.20(b). Figure 8.25(a) shows the observability index computed during the Monte Carlo simulations, which clearly indicates there is no loss in observability through out the simulations. Figures 8.25(b), 8.25(c) and 8.25(d) show the RMSE plots for velocity (vn , ve , vd ) with GPS update rate of 4 Hz, 5 Hz an 10 Hz respectively.

125


Observability index

2

ψrmse

8

AHRS

6

0

4 −2 2 0

200

400

600

800

1000

θrmse

0

0

100

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2

10

0

−2

5

0

φrmse

INS

2

0

200

400

600

800

0

−2

1000

Time (secs)





ψrmse

ψrmse

2 0 −2

−2 0

100

200

300

400

500

θrmse

θrmse

0

0

100

200

300

400

200

300

400

500

0

100

200

300

400

500

0

100

200

300

400

500

2

φrmse

φrmse

100

0

−2

500

2

0

−2

0

2

2

−2

0

0

−2 0

100

200

300

400

500

Time (secs)

Time (secs)

(c) GPS 5 Hz

(d) GPS 10 Hz

Figure 8.26: EKF Performance in Estimating Euler angles - MAV Trajectory in Figure 8.20(b). Figures 8.26(b), 8.26(c) and 8.26(d) show the RMSE plots of Euler angles (ψ, θ, ϕ) for MAV trajectory shown in Figure 8.20(b) with GPS update rate of 4 Hz, 5 Hz an 10 Hz respectively. It is observed that during GPS outage the RMSE increases until there is a GPS packet available. As soon as GPS packet is made available the error decreases. And it is observed that when there is no GPS outage the RMSE is within the standard deviation (1 − σ) of the white gaussian noise (AWGN) as given in Table 8.1.

126

CONCLUSION The design of an EKF is addressed for the purpose of estimating the position, velocity, attitude and heading of an MAV. While, estimating the attitude and heading of an MAV is typically addressed via an AHRS formulation, estimating the position, velocity along with the attitude and heading of the MAV is conventionally studied under the INS formulation. The approach uses onboard tri-axial accelerometers, tri-axial rate gyroscopes & tri-axial magnetometers and GPS measurements, as an external reference, to aid the EKF algorithm. An EKF solution for AHRS was implemented with GPS measurements aiding the IMU. The estimation was carried out assuming GPS update rates of 1, 4, 5 and 10 Hz while the IMU update rate was assumed to be 40 Hz. GPS outage durations of 20 s and 15 s were considered for a total simulation duration of 500 s. During the outage, two ways of implementing the EKF correction phase was proposed and simulations were carried out for attitude and heading estimation using these correction techniques. In an AHRS problem, it is typical to model the rate gyroscope bias states as a part of the EKF state vector while not including the accelerometer bias states as a part of the EKF state vector since in an AHRS problem accelerometer bias is not observable. Simulation results for an AHRS problem of an MAV are presented in Chapter 6. It is observed that when there is no sensor bias the attitude and heading estimation errors are observed to lie within ±0.1◦ . If there is sensor bias which is not accounted for in the EKF, the performance of the EKF in estimating the attitude and the heading of an MAV is degraded. This warrants the need for modeling the bias in the model for the EKF. The time varying sensor bias (b1p b1q b1r ) is modeled as a wiener process, while sensor bias (b2p b2q b2r ) are treated as unmodeled bias and EKF performance in estimating both attitude and rate gyroscope bias is studied. It is observed that the estimates lie within ±0.5◦ . 1000 Monte-Carlo simulations were made on the proposed EKF algorithm for various MAV trajectories and RMSE for attitude and heading estimates was computed for 1000 simulations for a duration of 500 s and it is observed that error is within ±0.5◦ . The design of an EKF was carried out for a nine, twelve, fifteen state as well as a split architecture EKF. Simulation results for implementing above filters are presented in Chapter 8. In the split architecture, the observability of each individual model is guaranteed thereby ensuring that all the 15 states are observable via this architecture (rank of observability matrix for six state filter is 6; rank of observability matrix for nine state filter is 9).

127

Appendix A Optimal Kalman Gain A.1

Deriving the Optimal Kalman Gain, Kk

The objective of a Kalman filter is to design the optimal Kalman gain matrix Kk such that ∑ the mean square state error is minimized. Thus argminKk ( ni=1 (¯ e2i )k ) = Kk∗ , where e¯i refers to the ith state error of ek such that    e¯1  e¯2  [  ]    Pk = E  ..  e¯1 e¯2 . . . e¯n   .  | {z } e e¯n   k e¯21 e¯1 e¯2 . . . e¯1 e¯n  e¯2 e¯1 e¯2 . . . e¯2 e¯n  2   = E  .. .. .. ..   . . . .  e¯n e¯1 e¯n e¯2 . . . e¯2n k   2 E [¯ e1 ] E [¯ e1 e¯2 ] . . . E [¯ e1 e¯n ]  E [¯ e2 e¯1 ] E [¯ e22 ] . . . E [¯ e2 e¯n ]    =  (A.1)  .. .. .. ..   . . . . E [¯ en e¯1 ] E [¯ en e¯2 ] . . . E [¯ e2n ] k

Thus the trace of the matrix Pk reduces to [ ] [ ] [ ] T r (Pk ) = E e¯21 + E e¯22 + . . . + E e¯2n ] [ ⇒ T r (Pk ) = E e¯21 + e¯22 + . . . + e¯2n = M.S.E

(A.2)

where, M.S.E denotes mean square error. Thus the minimization can be thought of as minimizing the trace of the matrix Pk in Eq. (4.16). This leads to the following expression1 ) ) ( ( ( ) T r (Pk ) = T r Pk− − T r Pk− HkT KkT − T r Kk Hk Pk− ) ( ) ( (A.3) +T r Kk Hk Pk− HkT KkT + T r Kk Rk KkT 1

The minimized M.S.E is abbreviated as MMSE or minimum mean square error.

128

The standard procedure of performing this minimization is to differentiate the expression in Eq. (A.3) with respect to the Kalman gain matrix, Kk , and then setting the differentiated expression to 0. By this procedure, we obtain ( ) ( ) ( ) ∂ ∂ ∂ ∂ T r (Pk ) = T r Pk− − T r Pk− HkT KkT − T r Kk Hk Pk− ∂Kk ∂Kk ∂Kk ∂Kk ( ( ) ) ∂ ∂ + T r Kk Hk Pk− HkT KkT + T r Kk Rk KkT ∂Kk ∂Kk − T − T − T = −Pk Hk − Pk Hk + Kk Hk Pk Hk + Kk Hk Pk− HkT + Kk Rk + Kk Rk = −2Pk− HkT + 2Kk Hk Pk− HkT + 2Kk Rk (A.4) where the partial derivatives in Eq. (A.4) have been computed based on Corollaries 2.12 and 2.14 and Fact 2.20. Setting the expression in Eq. (A.4) to 0 we obtain an expression for the optimal Kalman gain matrix as −2Pk− HkT + 2Kk Hk Pk− HkT + 2Kk Rk = 0 ⇒ Kk Hk Pk− HkT + Kk Rk = Pk− HkT

( )−1 ⇒ Kk = Pk− HkT Hk Pk− HkT + Rk

129

(A.5)

Appendix B Derivation B.1

Deriving Eqs. (4.32) and (4.33) for the System in Section 4.2

We only need to prove Eqs. (4.32) and (4.33) because Eqs. (4.34), (4.35) and (4.36) have been shown in Section 4.1.3 (Eqs. (4.25), (4.26) and (4.27)). Recall the continuous system dynamics of an observable, nonlinear dynamical system where the process dynamics is continuous and the measurement is discrete, as x˙ (t) = f (x (t)) + Γc w (t) ,

x (0) = x0

(B.1)

˙ Eq. (B.1) can be written as Assuming an Euler expansion of x, x (t + ∆t) − x (t) = f (x (t)) + Γc w (t) ∆t ⇒ x (t + ∆t) − x (t) = f (x (t)) ∆t + Γc w (t) ∆t ⇒ x (t + ∆t) = f (x (t)) ∆t + x (t) +Γc w (t) ∆t {z } | | {z } ¯ f (x(t))

(B.2)

wk−1

where, ∆t is the simulation time step. Furthermore, denoting xk ≡ x (t + ∆t) and xk−1 ≡ x (t), Eq. (B.2) reduces to xk = ¯f (xk−1 ) + Γc wk−1

(B.3)

where, ¯f (xk−1 ) = f (xk−1 ) ∆t + xk−1 . Notice that Eq. (B.3) is in the form of Eq. (4.1). Thus the predicted state and the predicted state error covariance matrix for the system in Eq. (B.3) follow Eqs. (4.21) and (4.23) and can be written as ˆ− = ¯f (ˆ xk−1 ) x k T + Γc Qk−1 ΓTc , Pk− = F¯k−1 Pk−1 F¯k−1

130

F¯k−1

∂ ¯f = ∂xk−1 xk−1 =ˆxk−1

(B.4) (B.5)

where F¯k−1 =

( ) ∂ ¯f ∂f = ∆t + I = F ∆t + I ∂xk−1 xk−1 =ˆxk−1 ∂xk−1 xk−1 =ˆxk−1

⇒ F¯k−1 = F ∆t + I

(B.6)

Substituting Eq. (B.6) into Eq. (B.5) and noting that Qk−1 = Q∆t [8], we obtain Pk− = (F ∆t + I) Pk−1 (F ∆t + I)T + Γc Q∆tΓTc = F ∆tPk−1 + Pk−1 F T ∆t + F ∆t2 Pk−1 F T + Pk−1 + Γc Q∆tΓTc

Pk− − Pk−1 ⇒ = F Pk−1 + Pk−1 F T + F ∆tPk−1 F T + Γc QΓTc ∆t

(B.7)

Thus, taking the limit of Eq. (B.7) as ∆t → 0 and noting from Eq. (4.31) that Pk−1 = P (t), we obtain P˙ (t) = F P (t) + P (t)F T + Γc QΓTc

(B.8)

which proves Eq. (4.33). Furthermore, the predicted state estimate for the system in Eq. (B.3) follows Eq. (4.21) as ˆ− ˆ k−1 x = ¯f (ˆ xk−1 ) = f (ˆ xk−1 ) ∆t + x k

⇒

ˆ− ˆ k−1 x k −x = f (ˆ xk−1 ) ∆t

(B.9)

ˆ k−1 = x ˆ (t), we obtain Thus, taking the limit of Eq. (B.9) as ∆t → 0 and noting that x ˆ˙ (t) = f (ˆ x x(t)) which proves Eq. (4.32).

131

(B.10)

Appendix C Basics C.1

MAV Heading, Track, Course

The parameters of interest such as heading, track, course and magnetic variation can be visualized from Figure C.1. Some definitions that are of interest are: 1. True North (N ): True north is the direction in which the north pole is located along the Earth’s rotational axis. It is also called as the geographic north or geographic north pole, shown in Figure C.2. The other end of the true north is the true south or geographic south. 2. Magnetic North (NM ): Magnetic north is the direction towards which the compass needle points, shown in Figure C.2. In other words, magnetic north corresponds to the point on the Earth’s surface where the magnetic lines (Mf in Figure C.2) make an angle of 90◦ , i.e. perfect verticals. The other end of the magnetic north is the magnetic south. 3. Compass North (NC ): Ideally, a compass needle points in the direction of magnetic north. Hence, ideally, the compass north is the same as magnetic north. However, in reality, there is always a deviation, due to local magnetic fields, between the compass north and the magnetic north which is called as the magnetic deviation. 4. Vehicle Heading (H): An aircraft’s heading is the direction that the aircraft’s nose is pointing. It is referenced by using either the standard magnetic compass or a standard heading indicator. 5. Vehicle Track (tA → tB ): The track or course over ground, is the actual path followed by the vessel, such as from tA to tB in Figure C.1. 6. Course: Course or course angle is the angle that the intended path of the vehicle makes with a fixed reference line (typically true north). Course is measured in degrees from 0◦ clockwise to 360◦ in compass convention (0◦ being north, 90◦ being east). 7. Heading Angle (ψm ): The aircraft heading angle is the angle that the aircraft’s nose makes with respect to the true north (geographic north).

132

8. Magnetic Heading Angle (ψmh ): The aircraft magnetic heading angle is the angle that the aircraft’s nose makes with respect to the magnetic north. 9. Compass Heading Angle (ψc ): The aircraft compass heading angle is the angle that the aircraft’s nose makes with respect to the compass north. 10. Magnetic Variation (mv ): The magnetic variation, also called as magnetic declination, is the angle between the magnetic north (the direction the north end of a compass needle points) and the true north. It is positive when the magnetic north is east of true north. It varies from place to place and also with the passage of time. 11. Magnetic Deviation (md ): Magnetic deviation is the error induced in a compass by local magnetic fields, which must be allowed for, along with magnetic declination, if accurate bearings are to be calculated. 12. Ground Track Angle (cw + ξ + ψm ): The ground track angle is the angle that the vehicle or aircraft ground track makes with the true north. This is calculated as the aircraft heading, ψm , plus the deviation due to cross winds and tidal currents known as drift/crab angle, (cw + ξ). Under conditions of 0 winds, i.e. (cw + ξ = 0), the vehicle track and vehicle heading coincide.

C.2

Earth North-South Pole: True, Magnetic

In Figure C.2, the following are defined: 1. N ′ S ′ : Earth’s Axis of Rotation. 2. N : Geographic North Pole. 3. S: Geographic South Pole. 4. NM : Magnetic North Pole. 5. SM : Magnetic South Pole. ′ ′ 6. NM SM : Earth’s Magnetic Field at 90◦ .

7. Mf : magnetic Field Lines.

133

Figure C.1: MAV Heading, Track and Course.

134

Figure C.2: Showing True North and Magnetic North.

135

Appendix D Frames of Reference There are a few coordinate systems in which the aircraft position and velocity make sense and a few other coordinate systems in which the phenomenon of interest is most naturally expressed such as the effects of wind on an aircraft. For example, to study navigation, it would be beneficial to choose a coordinate system with respect to Earth and while evaluating the aircraft performance, choosing a coordinate system with respect to the atmosphere is prudent, because wind/atmosphere has a direct bearing over how well an aircraft handles/performs, whether it is steady state performance or dynamic/transient performance. D.1

Frames of Reference

Some of the types of aircraft coordinate systems/frame of reference include: D.1.1

Inertial Frame: FI

1. The origin of FI may be any point that is completely unaccelerated and the orientation of the axes is not completely relevant as long as they are fixed with respect to the inertial space. 2. The center or origin of FI is taken to be the great galactic center, i.e. the rotational center of the Milky way galaxy. D.1.2

Earth Fixed Frame: FEF

1. Abbreviated in this thesis as FEF and depicted in Figure D.1. 2. FEF is a rotating, non-inertial frame (rotates with Earth). However, locally FEF can be considered as a non-rotating reference frame. 3. The origin, OE , is fixed to an arbitrary point on the surface of the Earth such as maybe a radar site or in some applications fixed to the center of gravity of the air vehicle. 4. XE points due true North, YE points due East and ZE points towards the Earth’s center. 5. Thus, FEF is sometimes also referred to as the North-East-Down (NED) reference frame. 136

6. FEF rotates with an angular velocity of ωE , which is the angular velocity of Earth.

Figure D.1: Earth Fixed Reference Frame, FEF .

D.1.3

Earth Centered Earth Fixed Frame: FECEF

1. Abbreviated in this thesis as FECEF and depicted in Figure D.2. 2. FECEF is a rotating, non-inertial frame. (rotates with Earth) 3. The origin, OECEF , is fixed to Earth’s center of mass. 4. XECEF intersects the sphere of Earth at 0 latitude and 0 longitude, YECEF is in the equatorial plane perpendicular to XECEF and ZECEF is parallel to Earth’s rotation axis and points to the true north. 5. FECEF is a special case of Earth Fixed frame, FEF , with the origin at Earth’s center of mass. 6. FECEF rotates with an angular velocity of ωE , which is the angular velocity of Earth.

D.1.4

Body Fixed Frame: FB

1. Abbreviated in this thesis as FB and depicted in Figure D.3. 2. FB is a rotating, non-inertial frame. It moves with the aircraft, i.e. fixed to the aircraft. 3. The origin, OB , is fixed to the CG of the aircraft. 137

Figure D.2: Earth Centered Earth Fixed Reference Frame, FECEF . 4. XB points through the nose of the aircraft, ZB lie in the X − Z plane and points downwards, perpendicular to XB and YB is orthogonal to XB and ZB and lies in the X − Y plane to the right of the XB axis.

Figure D.3: Body Fixed Reference Frame, FB .

138

D.1.5

Inertial Navigation Unit Frame: FIN U

1. Abbreviated in this thesis as FIN U and depicted in Figure D.4. 2. FIN U is a local inertial frame. 3. The origin, OIN U , is fixed to the inertial navigation unit and NOT the center of gravity of the vehicle. Thus, in this reference frame the orientation, velocity and acceleration of the aircraft must be computed as an offset from the center of gravity in order to determine the actual values in the true inertial frame. 4. XIN U wanders from true north by an angle αIN U , YIN U is orthogonal to XIN U and ZIN U points in a direction opposite to the gravity vector. 5. In some air vehicles INU frame, FIN U , is the same as the NED frame or FEF (Earth fixed frame).

Figure D.4: Inertial Navigation Unit Reference Frame, FIN U .

139

Appendix E Relation between MSE, bias and variance of an estimator ˆ ∈ Rn be an estimate of x ∈ Rn , obtained via an estimator. The bias (bxˆ ) and the Let x variance (Vxˆ ) of the estimator are respectively given by the following relations: bxˆ = x − E [ˆ x] [ ] Vxˆ = E (ˆ x − E [ˆ x]) (ˆ x − E [ˆ x])T

(E.1) (E.2)

where, E [·] denotes the expectation operator. Furthermore, the mean square error (MSE) is given as ( [ ]) ˆ ) (x − x ˆ )T M SExˆ = tr E (x − x (E.3) where, tr denotes the trace operator1 . Substituting Eq. (E.1) into Eq. ((E.3)) yields ( [ ]) ˆ ) (bxˆ + E [ˆ ˆ )T M SExˆ = tr E (bxˆ + E [ˆ x] − x x] − x ( [ ]) T = tr E (bxˆ − (ˆ x − E [ˆ x])) (bxˆ − (ˆ x − E [ˆ x])) ( [ ]) = tr E bxˆ bTxˆ + (ˆ x − E [ˆ x]) (ˆ x − E [ˆ x])T − bxˆ (ˆ x − E [ˆ x])T − (ˆ x − E [ˆ x]) bTxˆ   ]  [ ( [ ]) T  ⇒ M SExˆ = tr E bxˆ bTxˆ + tr  E (ˆ x − E [ˆ x ]) (ˆ x − E [ˆ x ])   {z } | Vx ˆ

( [ ]) ]) ( [ T −tr E bxˆ (ˆ x − E [ˆ x]) − tr E (ˆ x − E [ˆ x]) bTxˆ 1

The trace of a matrix is the sum of the diagonal elements of the matrix.

140

(E.4)

Further, substituting Eq. (E.2) into Eq. ((E.4)) and noting that expectation of a constant is the constant itself, yields ]) ( [ ( ) ( ) M SExˆ = tr bxˆ bTxˆ + tr (Vxˆ ) − tr bxˆ E (ˆ x − E [ˆ x])T − tr E [(ˆ x − E [ˆ x])] bTxˆ     ( ) ( ) [ T]   ˆ − E [ˆ = tr bxˆ bTxˆ + tr (Vxˆ ) − tr bxˆ E x x]T  − tr (E [ˆ x] − E [ˆ x]) bTxˆ  | {z } {z } | (

=0

) T

=0

⇒ M SExˆ = tr bxˆ bxˆ + tr (Vxˆ )

(E.5)

From trace property we have ( ) ( ) tr bxˆ bTxˆ = tr bTxˆ bxˆ

(E.6)

Furthermore, since bTxˆ bxˆ is a scalar, trace of a scalar is simply the scalar and we have ( ) tr bTxˆ bxˆ = bTxˆ bxˆ = ∥bxˆ ∥22 (E.7) Also, given that bxˆ is a vector, the RHS of the relation in Eq. (E.7) denotes the square of the norm of a vector and we have bTxˆ bxˆ = ∥bxˆ ∥22

(E.8)

Note that the relation in Eq. (E.8) is also called as the Frobenius norm or the Hilbert-Schmidt norm and is typically represented as ∥ · ∥F . Substituting Eq. (E.8) in Eq. (E.5) we have M SExˆ = ∥bxˆ ∥22 + tr (Vxˆ )

≈

accuracy × accuracy + precision

(E.9)

Typically, bias represents accuracy and variance corresponds to precision. Thus minimizing the MSE of the estimator implicitly means improving the estimator accuracy and precision. Figure E.1 shows pictorially the difference between accuracy and precision.

Figure E.1: Accuracy and Precision Remark E.1. For the case of scalars, i.e. xˆ ∈ R an estimate of x ∈ R, the relation in Eq. 141

(E.9) reduces to M SExˆ = b2xˆ + vxˆ [ ] where, vxˆ = E (ˆ x − E [ˆ x])2 and bxˆ = x − E [ˆ x].

142

(E.10)

Appendix F The Matrix Inversion Lemma Consider the matrices A ∈ Rn×n and C ∈ Rm×m and assume that they are non-singular, i.e. A, C are invertible. Consider two other matrices B ∈ Rn×m and D ∈ Rm×n . Then the following identity holds: ( )−1 (A + BCD)−1 = A−1 − A−1 B DA−1 B + C −1 DA−1 (F.1) The matrix inversion lemma is popularly known as the Woodbury matrix identity. To prove the identity in Eq. (F.1), the following steps are adopted: ( ) ( )−1 (A + BCD) A−1 − A−1 B DA−1 B + C −1 DA−1 = I + BCDA−1 ( )−1 −B DA−1 B + C −1 DA−1 ( )−1 −BCDA−1 B DA−1 B + C −1 DA−1 = I + BCDA−1 ( )−1 −BCC −1 DA−1 B + C −1 DA−1 ( )−1 −BCDA−1 B DA−1 B + C −1 DA−1 = I + BCDA−1 ( ) −BC C −1 + DA−1 B ( )−1 DA−1 B + C −1 DA−1 −1 −1 ( ) = I + BCDA − BCDA ( ) −1 ⇒ (A + BCD) A−1 − A−1 B DA−1 B + C −1 DA−1 = I ( )−1 ⇒ (A + BCD)−1 = A−1 − A−1 B DA−1 B + C −1 DA−1 which proves the result in Eq. (F.1).

143

Appendix G The Exponential Function and its Identities For a given constant square matrix A ∈ Rn×n , the following exponential identities hold: eAt = I + At +

A2 t2 A3 t3 A4 t4 + + + ... + ... 2! 3! 4!

(G.1)

where I denotes an identity matrix of dimension n × n. The corresponding exponential identity for the transpose of A is given by ( T )2 2 ( T )3 3 ( T )4 4 A A A t t t AT t T e = I +A t+ + + + ... + ... (G.2) 2! 3! 4! Furthermore, we have e−At = I − At +

A2 t2 A3 t3 A4 t4 − + + ... + ... 2! 3! 4!

(G.3)

The derivative of an exponential matrix function can be written as d ( At ) A2 t A3 t2 A4 t3 e = A+2 +3 +4 + ... + ... dt 3! 4! ( 2! ) A2 t2 A3 t3 A4 t4 = A I + At + + + + ... + ... 2! 3! 4! = ( AeAt ) A2 t2 A3 t3 A4 t4 = I + At + + + + ... + ... A 2! 3! 4! = eAt A The integral of an exponential matrix function can be written as ∫ At2 A2 t3 A3 t4 A4 t5 eAt dt = tI + + + + + . . . + . . . + αc 2! 3! 4! 5!

144

(G.4)

(G.5)

where, αc is the constant of integration. Furthermore if A is invertible, then we have ) ( ∫ At2 A2 t3 A3 t4 A4 t5 At −1 e dt = A A tI + + + + + . . . + . . . + αc 2! 3! 4! 5! ( ) A2 t2 A3 t3 A4 t4 A5 t5 −1 = A At + + + + + . . . + . . . + Aαc (G.6) 2! 3! 4! 5! Adding and subtracting I to the Eq. (G.6) further yields ( ) ∫ A2 t2 A3 t3 A4 t4 A5 t5 At −1 e dt = A + + + + . . . + . . . + Aαc − I + I At + 2! 3! 4! 5! ) ( A2 t2 A3 t3 A4 t4 A5 t5 −1 = A I + At + + + + + . . . + . . . + αc − A−1 2! 3! 4! 5! −1 At −1 = A e + αc − A (G.7) Furthermore, if the integral in Eq. (G.7) is a definite integral from limits 0 to ∆t, then Eq. (G.7) reduces to ∫ 0

∆t

∆t eAt dt = A−1 eAt + αc − A−1 0 = A−1 eA∆t − A−1 ( ) = A−1 eA∆t − I

(G.8)

Consider a constant square matrix M ∈ Rn×n . Given the expansion of the term eAt from T T Eq. (G.1) and the expansion of eA t from Eq. (G.2), the product eAt M eA t can be formed as ) ( T )2 2 ( ) ( 2 2 A t A t T eAt M eA t = I + At + + . . . M I + AT t + + ... 2! 2! ) ( T )2 2 ( )( A t A2 M t2 = M + AM t + + ... I + AT t + + ... 2! 2! ( ) ( ) A2 M t2 A2 M AT t3 T T 2 = M + AM t + + . . . + M A t + AM A t + + ... 2! 2! ( ) 2 2 2 M AT t2 AM AT t3 A2 M AT t4 + + + + ... (G.9) 2! 2! (2!)2 Furthermore, the indefinite integral of Eq. (G.9) can be computed as ( ) ( ) ∫ AM t2 A2 M t3 M AT t2 AM AT t3 A2 M AT t4 At AT t e M e dt = Mt + + + ... + + + + ... 2! 3! 2! 3! 4 · 2! ( ( )2 ) ( )2 ( )2 M AT t3 AM AT t4 A2 M AT t5 + + + + . . . + . . . + αc (G.10) 3! 4 · 2! 5 (2!)2 145

where, αc is the constant of integration. Furthermore, if the integral in Eq. (G.10) is a definite integral from limits 0 to ∆t, then the integral reduces to {( ) ( ) ∫ ∆t AM t2 A2 M t3 M AT t2 AM AT t3 At AT t e M e dt = Mt + + + ... + + + ... 2! 3! 2! 3! 0 ( ) } ∆t 2 2 M AT t3 AM AT t4 + + + . . . + . . . + αc 3! 4 · 2! 0 ) ( ) ( M AT ∆t2 AM AT ∆t3 A2 M AT ∆t4 AM ∆t2 + ... + + + = M ∆t + 2! 2! 3! 4 · 2! ( ( )2 ) ( ) 2 M AT ∆t3 AM AT ∆t4 +... + + + ... + ...+ (G.11) 3! 4 · 2!

146

Appendix H Solution of linear systems H.1

Solution of Linear, Time Invariant Systems

Consider the following linear time invariant (LTI) system: ˙ x(t) = Ax(t) + Bu(t), y(t) = Cx(t)

x(t0 ) = xt0

(H.1) (H.2)

where, x ∈ Rn is the n-dimensional state vector of the system, xt0 is the initial value of the state vector, u ∈ Rm is the m-dimensional input vector of the system, y ∈ Rp is the p-dimensional output vector of the system, A ∈ Rn×n is the system matrix, B ∈ Rn×m is the input matrix, C ∈ Rp×n is the output matrix and t denotes the time. The goal is to find an analytical solution for x(t). Re-write Eq. (H.1) as ˙ − Ax(t) = Bu(t) x(t)

(H.3)

Pre-multiply Eq. (H.3) with the term e−At , where e denotes the exponential function. Thus we have ˙ − Ax(t)) = e−At Bu(t) e−At (x(t) d ⇒ e−At x(t) = e−At Bu(t) dt

(H.4) (H.5)

Integrating Eq. (H.5) between limits t0 to t results in ∫ t ∫ t d −Aσ e x(σ)dσ = e−Aσ Bu(σ)dσ dσ t t0 ∫ t ∫ 0t ( −Aσ ) ⇒ d e x(σ) = e−Aσ Bu(σ)dσ t0 t ∫ 0t t ⇒ e−Aσ x(σ) t0 = e−Aσ Bu(σ)dσ t ∫ 0t ⇒ e−At x(t) − e−At0 x(t0 ) = e−Aσ Bu(σ)dσ t0

(H.6) 147

∫

t

x(t0 ) + e e−Aσ Bu(σ)dσ t0 ∫ t A(t−t0 ) ⇒ x(t) = e x(t0 ) + eA(t−σ) Bu(σ)dσ

⇒ x(t) = e

A(t−t0 )

At

(H.7)

t0

Denote the state transition matrix as ΦA (t, t0 ) = eA(t−t0 )

(H.8)

Therefore, Eq. (H.7) can be expressed as ∫

t

x(t) = ΦA (t, t0 ) xt0 +

ΦA (t, σ) Bu(σ)dσ

(H.9)

t0

which is the solution of the differential equation in Eq. (H.1). Furthermore, the output in Eq. (H.2) can be expressed as ∫ t y(t) = Cx(t) = CΦA (t, t0 ) xt0 + CΦA (t, σ) Bu(σ)dσ (H.10) t0

Remark H.1. Let t0 = 0 and u(t) be given by a delta function1 . Then the solution x(t) in Eq. (H.9) reduces to x(t) = ΦA (t, 0) x0 + ΦA (t, 0) B1 ⇒ x(t) = eAt (x0 + bu ) 1

A normalized delta function, also called a unit impulse function, δ(t) is given by { 1 t=0 δ(t) = 0 t ̸= 0

(H.14)

(H.11)

The integral of δ(t) is given by ∫

∞

δ(t)dt =

1

(H.12)

−∞

Furthermore, we have ∫

∞

f (t)δ(t)dt = f (0) −∞

148

(H.13)

where, 1 is a column vector of one’s with dimension m × 1 and bu = B1 is given by  m  Σj=1 b1j      Σm b2j   j=1    bu =  (H.15)    ..   .     m Σj=1 bnj The output is given by y(t) = CeAt (x0 + bu )

(H.16)

x(t) = eAt bu y(t) = CeAt bu

(H.17) (H.18)

Furthermore, if x0 = 0 we have

H.2

Solution of Linear, Time Varying Systems

Consider the following linear time varying (LTV) system: ˙ x(t) = A(t)x(t) + B(t)u(t), y(t) = C(t)x(t)

x(t0 ) = xt0

(H.19) (H.20)

where, x ∈ Rn is the n-dimensional state vector of the system, xt0 is the initial value of the state vector, u ∈ Rm is the m-dimensional input vector of the system, y ∈ Rp is the p-dimensional output vector of the system, A(t) ∈ Rn×n is the system matrix, B(t) ∈ Rn×m is the input matrix, C(t) ∈ Rp×n is the output matrix and t denotes the time. The goal is to find an analytical solution for x(t). Let the matrix T (t) ∈ Rn×n be nonsingular ∀ t ≥ 0 such that x(t) = T (t)z(t)

(H.21)

Differentiating Eq. (H.21) with respect to time we have ˙ ˙ x(t) = T˙ (t)z(t) + T (t)z(t)

(H.22)

Equating Eq. (H.19) and Eq. (H.22) yields ˙ A(t)x(t) + B(t)u(t) = T˙ (t)z(t) + T (t)z(t)

149

(H.23)

Furthermore, substituting Eq. (H.21) into Eq. (H.24) yields ˙ A(t)T (t)z(t) + B(t)u(t) = T˙ (t)z(t) + T (t)z(t) ( ) ˙ ⇒ T (t)z(t) = A(t)T (t) − T˙ (t) z(t) + B(t)u(t) Therefore the equivalent LTV system is given by ( ) ˙z(t) = T −1 (t) A(t)T (t) − T˙ (t) z(t) + T −1 (t)B(t)u(t) y(t) = C(t)T (t)z(t)

(H.24)

(H.25) (H.26)

Remark H.2. The matrix T (t) is called the fundamental matrix when ∥T (t0 )∥ = ̸ 0

T˙ (t) = A(t)T (t),

(H.27)

Based on Remark H.2, if T (t) is a fundamental matrix, then Eq. (H.25) reduces to ˙ z(t) = T −1 (t)B(t)u(t) Integrating Eq. (H.28) between limits of t0 and t we have ∫ t z(t) = z(t0 ) + T −1 (σ)B(σ)u(σ)dσ

(H.28)

(H.29)

t0

Thus with Eq. (H.29), Eq. (H.21) ∫

t

x(t) = T (t)z(t0 ) +

T (t)T −1 (σ)B(σ)u(σ)dσ

(H.30)

t0

Furthermore we have z(t0 ) = T −1 (t0 )x(t0 )

(H.31)

Thus with Eq. (H.31), Eq. (H.30) reduces to x(t) = T (t)T

−1

∫

t

(t0 )x(t0 ) +

T (t)T −1 (σ)B(σ)u(σ)dσ

(H.32)

t0

Define the state transition matrix as Φ (t, t0 ) = T (t)T −1 (t0 )

(H.33)

Thus, Eq. (H.32) reduces to ∫

t

x(t) = Φ (t, t0 ) x(t0 ) +

Φ (t, σ) B(σ)u(σ)dσ t0

150

(H.34)

This further implies that the output is given by ∫ t y(t) = C(t)Φ (t, t0 ) x(t0 ) + C(t)Φ (t, σ) B(σ)u(σ)dσ t0

151

(H.35)

Appendix I Observability I.1

Observability of Linear, Time Invariant Systems

Consider the following linear time invariant (LTI) system: ˙ x(t) = Ax(t), y(t) = Cx(t)

x(0) = x0

(I.1) (I.2)

where, x ∈ Rn is the n-dimensional unknown state vector of the system, x0 is the initial value of the state vector, y ∈ Rp is the p-dimensional known output vector of the system, A ∈ Rn×n is the system matrix, C ∈ Rp×n is the output matrix and t denotes the time. We say that the LTI system in Eqs. (I.1) and (I.2) is observable if the following holds:       C     CA    CA2  rank  = n (I.3)    ..    .   CAn−1    {z } | O∈Rnp×n

⇒ rank (O) = n

(I.4)

where, O is a constant matrix called as the observability matrix. The condition in Eq. (I.3) is called as the observability rank condition (ORC) for LTI systems. To derive the ORC we may proceed as follows: Since the only known source in Eqs. (I.1) and (I.2) is the output y(t), we must reconstruct x(t) from y(t) and its derivatives. Hence we have y(t) ˙ y(t) ¨ (t) y .. . yn−1 (t)

= Cx(t) ˙ = C x(t) = CAx(t) ˙ = CAx(t) = CA2 x(t) . = .. ˙ = CAn−2 x(t) = CAn−1 x(t) 152

(I.5) (I.6) (I.7) (I.8)

Note that Eqs. (I.5) ∼ (I.8) in matrix form can be represented as     y(t) C  y(t)   CA   ˙     y   CA2  ¨ (t)   =   x(t)     .. ..     . . n−1 n−1 y (t) CA | {z } np×1 ¯ Y(t)∈R

¯ ⇒ Y(t) = Ox(t)

(I.9)

Now given that the dimension of O is np × n where np ≥ n, the rank of O can at best be n, i.e. rank(O) ≤ n. Furthermore, given that O is not always square1 , it is not always invertible. Hence, consider Eq. (I.9) and pre-multiply by OT to obtain ¯ OT Y(t) = OT Ox(t)

(I.10)

Notice that the dimension of OT O is n × n and hence if rank(O) = n then rank(OT O) = n making it invertible and thereby allowing one to obtain a solution for x. Hence the rank of the observability matrix should always be equal to the dimension of the state vector of the system for the system to be observable. We summarize observability with the definition below. Definition. With respect to the LTI system in Eqs. (I.1) and (I.2), the pair (A, C) is observable if for all t1 > 0 and initial condition x (t0 ) ∈ Rn , the knowledge of the output y(t), t ∈ [t0 , t1 ) suffices to determine the state x (t0 ). Remark I.1. To fully understand Definition I.1, consider Eq. (H.10) with t0 = 0 and u = 0. Thus we have y(t) = CeAt x0 Furthermore, Eq. (I.9) with Eqs. (I.11) and   y(t)  y(t)   ˙   y  ¨ (t)   =   ..   . n−1 y (t) ¯ ⇒ Y(t) =

(I.11)

(G.4) reduces to   C  CA     CA2  At   e x0   ..   . n−1 CA OeAt x0

(I.12)

Thus, with known pair (A, C) and the knowledge of the output y(t), the state vector of the system at t = 0 can be determined. Furthermore, observability of the LTI system in Eqs. 1

square only when p = 1

153

(I.1) and (I.2) requires that yT (t)y(t) > 0, ∀ t > 0 which implies that: (∫ t ) ∫ t T T AT σ T Aσ x0 y (σ)y(σ)dσ = x0 e C Ce dσ 0 0 {z } |

(I.13)

WO (t)

where, WO (t) is called the observability Grammian. The pair (A, C) of the LTI system in Eqs. (I.1) and (I.2) is observable if and only if WO (t) > 0, ∀ t > 0. I.2

Observability of Nonlinear, Time Invariant Systems

Consider the following nonlinear dynamical system:  f1 (x (t))    f2 (x (t))   x˙ (t) = f (x (t)) =   ..  .   fn (x (t)) 

h1 (x (t))    h2 (x (t))   y (t) = h (x (t)) =   ..  .   hp (x (t))

      ,     

x(0) = x0

(I.14)

         

[ ]T where, x = x1 x2 . . . xn ∈ Rn is the n-dimensional state of the system, x(0) = [ ]T [ ]T x10 x20 . . . xn0 denotes the initial state vector of the system, y = y1 y2 . . . yp ∈ p n n R is the p-dimensional system measurement, f : R → R is the nonlinear mapping of the system states called as the system dynamics such that fi ∈ Rn → R, i = 1, 2, . . . , n and h : Rn → Rp is the nonlinear mapping of the system states to the measurement such that hj ∈ Rn → R, j = 1, 2, . . . , p. The vector of measurements and its time derivatives up to

154

and including n − 1 is formed as         ¯ Y =       

y y˙ ¨ y .. . yn−1





L0f (h)



        L1 (h)  f            L2 (h)  f =  = gy (x)          ..    .       n−1 Lf (h)

(I.15)

where, yn−1 denotes the (n − 1)st time derivative of y, L0f (h) , · · · , Lfn−1 (h) denote the Lie derivatives [?] of the measurement and gy (·) : (x ∈ Rn ) → Rnp×1 . The Lie derivatives are given by  0      L1f y1 h1 (x (t))              L0   y2   h2 (x (t))   2f            L0f =  (I.16) = =   ..   ..    ..  .   .    .             0 Lpf yp hp (x (t))  ( ∂L0 )   )  ( 1f ∂h1 n n  1  f Σ f Σ L1f  i=1 ∂xi i   i=1 ∂xi i           ( ) ( ∂L0 )       n  2f ∂h2  L1   Σn  Σi=1 ∂xi fi   2f   i=1 ∂xi fi        =  (I.17) L1f =  =     ..      . .  .   .. ..                )  (  ( ∂L0 )   L1pf p f Σni=1 ∂h Σni=1 ∂xpif fi ∂xi i  ( ∂L1 )   2  Σni=1 ∂x1if fi L1f        ( )    1  ∂L2  L2   n f   2f   Σi=1 ∂xi fi       (I.18) L2f =  =   ..    ..  .    .          ( ∂L1 )  L2pf Σni=1 ∂xpif fi (I.19)

155

.. . . = .. 

Ln−1 f

.. . Ln−1 1f

   Ln−1  2f  =   ..  .   Ln−1 pf

.. . .. .. .  .. (. n−2. ) ∂L1 f fi   Σni=1 ∂xi    ( n−2 )   ∂L2   n f    Σi=1 fi ∂xi   =     ..   .    ( n−2 )  ∂L n Σi=1 ∂xpfi fi

               

(I.20)

Taking the Taylor series of Eq. (I.15) about a nominal value, say x∗ , and neglecting the higher order terms, we obtain ¯ = gy (x) Y

∂g (x) y ≈ gy (x∗ ) + (x − x∗ ) ∂x x=x∗ {z } | (

O(x∗ )∈Rnp×n

⇒ x = x∗ + OT O

)−1

( ) ¯ − gy (x∗ ) OT Y

(I.21)

where, as before, O is called as the observability matrix.( Once ) again, as before, we see T n×n T from Eq. (I.21) that O O ∈ R is invertible if rank O O = n and Eq. (I.21) can ¯ This is called as the nonlinear observability rank be used to compute x given x∗ and Y. condition. Of course, it is imperative to note here that the measurement and its subsequent n − 1 time derivatives need to first made available in order to compute x. This requires large computational resources in order to store y and its n − 1 time derivatives and even more so to perform an inversion. Furthermore, since y is typically the measurement and is, in reality, corrupted with noise, the derivatives of y will only amplify the effects of noise. Hence, in reality, a recursive based approach is a more likely candidate for online estimation of the state vector x. Furthermore, since the higher order Taylor series terms were ignored in order to arrive at the expression in Eq. (I.21), observability for nonlinear systems is confined to a local domain around the point or vector (in the case of non-scalar systems) x∗ . In the case of linear systems, with f (x) = Ax and h (x) = Cx, the notion of observability is global. In the case of nonlinear systems, the following notion is of consequence: Definition. The pair of states x1 (t) ̸= x2 (t) ∈ Rn is called indistinguishable from the output y(t) = h (x(t)), where y : Rn → Rp , if h (x1 (t)) = h (x2 (t)) ,

∀t≥0

(I.22)

In the case of linear systems since h (x(t)) = Cx(t), then y(t) = Cx1 (t) = Cx2 (t) which means that if x1 (t) ̸= x2 (t) then Cx1 (t) ̸= Cx2 (t). Thus for linear systems the property of indistinguishability is inherent. 156

Nonlinear Observability I.3

Nonlinear Observability Analysis: AHRS, X-axis Accelerometer Bias, No Gyro Bias, 3 Measurements

We examine the nonlinear observability condition of the AHRS model with only the X-axis accelerometer bias and without modeling gyroscopic bias. The measurements used are ψ, θ and ϕ. The state vector is   ψ  θ   x =   ϕ  bax ⇒n = 4 (I.23) where, n is the dimension of the state vector, bax is the bias in axm . Assumption I.2. It is assumed that bay = baz = 0, b1p = b1q = b1r = 0, b2p = b2q = b2r = 0 and bwp = bwq = bwr = 0. Assumption I.3. The accelerometer bias, bax , is modeled as a Wiener process such that b˙ ax = wax

(I.24)

where, wax is a 0 mean, band-limited AWGN process of specified covariance. Based on Assumption I.3, the AHRS model collapses to   sϕ cϕ   ˙  q + r ψ c c   | θ {z θ}           f1           ˙   θ    qcϕ − rsϕ      | {z }       =   + f 2             ϕ˙     p + qsϕ tθ + rcϕ tθ         | {z }       f 3        b˙ ax 0

0



    0     wa x   0      1 (I.25)

157



zk

ψmk + vψk

    =   θmk + vθk    ϕmk + vϕk

         

(I.26)

Furthermore, we make use of the following expressions: ∂f1 ∂ψ ∂f1 ∂θ ∂f1 ∂ϕ ∂f2 ∂ψ ∂f2 ∂θ ∂f2 ∂ϕ ∂f3 ∂ψ ∂f3 ∂θ ∂f3 ∂ϕ

= 0 = f1 tθ =

f2 cθ

= 0 = 0 = −f1 cθ = 0 =

f1 cθ

= f2 tθ

158

(I.27)

Thus with Eq. (I.27), the Lie derivatives are computed as   ψ L0f =  θ  ϕ 

L1f

 f1 =  f2  f3 

L2f

  =   

f1 f2 tθ +

−f1 f3 cθ f1 f2 cθ

 L3f

   =   

f2 f3 cθ

     

+ f2 f3 tθ f1 f22 c2θ

(2 + s2θ ) + 3f22 f3 ctθθ − f1 (f1 sθ + f32 ) −f2 (f12 + f32 + f1 f3 sθ )

f22 c2θ

      

(I.28)

(f1 + 2f3 + 2f1 sθ ) + f3 (f22 t2θ − f12 − f1 f3 sθ )

which then yields gy (x). At this point to obtain O and then OT O, we use the symbolic toolbox in MATLAB, which results in O ∈ R12×4 . Furthermore, a simple OT O ∈ R4×4 operation in MATLAB shows that all the entries of the 4th row of OT O are 0. Thus, ( ) [ ]T rank OT O < 4 ⇒ the system in Eq. (I.25) with z = ψ θ ϕ is unobservable. Thus, for the AHRS formulation, accelerometer bias is not considered.

159

Bibliography [1] D. H. Titterton and J. L. Weston. “Strapdown Inertial Navigation Technology”, chapter 1. Peter Peregrinus Ltd. Institution of Electrical Engineers, 1997. [2] Venkatesh K. Madyastha, Vishal C. Ravindra, and Girija Gopalratnam. Air Vehicle Reference Frames, Flight Angles and Onboard Sensors. Technical Report D-1-144, National Aerospace Laboratories (CSIR-NAL), Dec 2010. [3] Demoz Gebre-Egziabher. Roger C. Hayward and J. David Powell. “design of multisensor attitude determination systems”. In IEEE Transactions on Aerospace and Electronic Systems, volume 40 of IEEE, pages 627 – 649, Aug 2001. [4] K. C. Wong T. Sperry. “design and development of a micro air vehicle (µav) concept: Project bidule”. Technical report, School of Aerospace, Mechanical and Mechatronic Engineering, University of Sydney, New South Wales, 2006. [5] Joel M. Grasmeyer and Matthew T. Keennon. “development of the black widow micro air vehicle”. In American Institute of Aeronautics and Astronautics, AIAA-2001-0127, 2001. [6] H. Norton. “Transducer Fundamentals” in Handbook of Transducers, chapter 2. Englewood Cliffs, NJ: Prentice Hall, 1989. [7] Michiel Hazewinkel. Taylor’s Theorem, Encyclopedia of Mathematics. Springer, 2001. [8] Robert Brown and Patrick Hwang. “Introduction to Random Signals and Applied Kalman Filtering, Encyclopedia of Mathematics”. John Wiley and Sons, Inc.,, 1992. [9] D. G. Luenberger. “observing the state of a linear system”. In IEEE Trans. Milit. Electr., page 8(74), 1963. [10] N. Wiener. Extrapolation, Interpolation and Smoothing of stationary time series. John Wiley & Sons, Inc., New York, 1949. [11] R. A Singer and P. A. Frost. “on the relative performance of the kalman and wiener filters”. In IEEE Transactions on Automatic Control, pages 17(4):390 – 394, August 1969. [12] R. E. Kalman. “a new approach to linear filtering and prediction problems”. In Transactions of the ASME - Journal of Basic Engineering, volume 82 of D, pages 35 – 45, 1960. 160

[13] R. E. Kalman and R. S. Bucy. “new results in linear filtering and prediction theory”. In Transactions of the ASME - Journal of Basic Engineering, volume 83 of D, pages 95 – 108, 1961. [14] C. Hutchinson. “an example of the equivalence of the kalman and wiener filters”. In IEEE Transactions on Automatic Control, page 11(2):324, April 1966. [15] Nijmeijer. H and Fossen. T. New directions in nonlinear observer design. editors, Lecture Notes in Control and Information Sciences 244. Springer Verlag, London,, 1999. [16] Fred Daum. Nonlinear filters: beyond the kalman filter”. In IEEE Aerospace and Electronic Systems Magazine, volume 20 no.8, pages 57–59, August 2005. [17] P. Eykhoff. System Identification. Wiley, 1974. [18] A. H. Jazwinski. Processes and Filtering Theory. Newyork, academic, 1970. [19] Hisashi Tanizaki. Nonlinear Filters: Estimation and Applications. 2nd Edition, SpringerVerlag, Berlin, 1996. [20] H. Cox. “estimation of state variables via dynamic programming”. In In the Proceedings of the Joint Automatic Control Conference, Stanford, CA, pages 376 – 381, June 1964. [21] W.H. Fleming and S.K. Mitter. “Optimal Control and Nonlinear Filtering for Nondegenerate Diffusion Processes”. Stochastics, Vol. 8, 1982. [22] M.G. Safonov and M. Athans. “robustness and computational aspects of nonlinear stochastic estimators and regulators”. In emphIEEE Transactions on Automatic Control, volume AC - 23, pages 717 – 726, August 1978. [23] A. Gelb. Applied Optimal Estimation. Cambridge, MA: MIT Press, 1984. [24] H. Sorenson. “guest editorial: On applications of kalman filtering”. In IEEE Transactions on Automatic Control, volume AC-28, No. 3, pages 254 – 255, March 1983. [25] Jeffrey H. Ahrens and Hassan K. Khalil. “closed-loop behavior of a class of nonlinear systems under ekf - based control”. In IEEE Transactions On Automatic Control, volume 52, No. 3, pages 254 – 255, March 2007. [26] A. Bensoussan J. S. Baras and M. R. James. “dynamic observers as asymptotic limits of recursive filters: Special cases”. In SIAM Journal of Applied Mathematics, volume 48, No. 5, pages 1147 – 1158, 1988. [27] Roger C. Hayward Demoz Gebre-Egziabher and J. David Powell. ,“design of multisensor attitude determination systems”. In IEEE Transactions on Aerospace and Electronic Systems, volume 40, No. 2, pages 627–649, April 2004. [28] Lawrence R. Weill Mohinder S. Grewal and Angus P. Andrews. Global Positioning Systems, Inertial Navigation and Integration. John Wiley and Sons, Inc., 2001. 161

[29] X. Rong Li Y. Bar-Shalom and T. Kirubarajan. Estimation with Applications to Tracking and Navigation. Wiley-Interscience, 2003. [30] Benard Etkin and Lloyd Duff Reid. “Dynamics of Flight Stability and Control”. John Wiley and Sons, Inc.,, 1996. [31] ANTARIS Protocol Specification, Ublox. http : //www.u − blox.com/en/download/documents − a” − resources/antaris − 4 − gps − modules − resources. [32] Randal Beard, Derek Kingston, Morgan Quigley, Deryl Synder, Reed Christiansen, Walt Johnson, Timothy McLain, and Michael A. Goodrich. “autonomous vehicle technologies for small fixed-wing uavs”. In Journal of Aerospace Computing, Information and Communication, 2005. [33] The Flight Dynamics & Control Toolbox. http : //92.254.81.198/index.html. [34] Aerospace Blockset, Mathworks, chapter www.mathworks.com/help/toolbox/aeroblks. /drydenwindturbulencemodelcontinuous.html.

162

Vita

Sanketh Ailneni was born in Nizamabad, Andhra Pradesh, India. After completing his preuniversity degree requirements at the Alphores Junior College, Karimnagar in 2005, he began his undergraduate studies at the Jawaharlal Nehru Technological University in Hyderabad. He received his degree of Bachelors in Technology in June 2009 from the department of aeronautical engineering. Soon after his graduation, he worked as a teaching assistant from August 2009 to June 2010 at the department of aeronautical engineering in the Institute of Engineering and Management, Hyderabad. In August 2010, he enrolled for a masters program at the Academy of Scientific and Innovative Research, National Aerospace Laboratories, Bangalore, India. His research interests include estimation theory, extended Kalman filter, micro aerial vehicles, to name a few. This thesis was typed by Sanketh Ailneni.

163

multi rate sensors based inertial navigation for micro

multi rate sensors based inertial navigation for micro

Suggest Documents

Fusion of Imaging and Inertial Sensors for Navigation

Fusion of Low-Cost Imaging and Inertial Sensors for Navigation

MEMS Inertial Sensors-Based Multi-Loop Control Enhanced by ... - MDPI

Research on In-Flight Alignment for Micro Inertial Navigation ... - MDPI

Multi-aided Inertial Navigation for Ground Vehicles in Outdoor ... - CEC

Web-Based MEMS Inertial Navigation Simulator - Lassena

Planar-Based Visual Inertial Navigation: Observability ...

3. Inertial Navigation

Vision Aided Inertial Navigation

Efficient Multi-Camera Visual-Inertial SLAM for Micro ... - ais.uni-bonn.de

Low Cost Inertial Navigation

Sensors for mobile robot navigation

Integration of Vision and Inertial Sensors for Home-based ... - CiteSeerX

Multi-Layered Mapping and Navigation for Autonomous Micro Aerial ...

Multi-Layered Mapping and Navigation for Autonomous Micro Aerial ...

WISDOM: Wheelchair Inertial Sensors for ... - Julien Pansiot

Miniature Inertial and Augmentation Sensors for ... - CiteSeerX

modification of strapdown inertial navigation

Multi-rate Sensor Fusion for GPS Navigation Using ... - CiteSeerX

An introduction to inertial navigation

fast particle filters for multi-rate sensors - ISY - LiU

U sing Inertial Sensors of iPhone 4 for Car Navigation - IEEE Xplore

U sing Inertial Sensors of iPhone 4 for Car Navigation - IEEE Xplore

Homography-based Vision-aided Inertial Navigation of ... - CiteSeerX