Autonomous Navigation for Airborne Applications Jonghyuk Kim A thesis submitted in fulfillment of the requirements for the degree of Doctor of Philosophy Australian Centre for Field Robotics Department of Aerospace, Mechanical and Mechatronic Engineering The University of Sydney May 2004 Declaration I hereby declare that this submission is my own work and that, to the best of my knowledge and belief, it contains no material previously published or written by another person nor material which to a substantial extent has been accepted for the award of any other degree or diploma of the University or other institute of higher learning, except where due acknowledgement has been made in the text. Jonghyuk Kim May 1, 2004 i ii Abstract Jonghyuk Kim Doctor of Philosophy The University of Sydney May 2004 Autonomous Navigation for Airborne Applications Autonomousnavigation (or localisation) is theprocess of determininga platform’s pose without the use of any a priori information external to the platform except for what the platform senses about the environment. That is, the determination of the platform’s pose without the use of predefined maps or infrastructure developed for navigation purposes such as terrain aided navigation systems or Global Navigation Satellite System (GNSS). The objective of this thesis is to both develop and demon- strate autonomous localisation algorithms for airborne platforms. The emphasis is placed on the importance of the algorithms to function appropriately and accurately using low cost inertial sensors (where the rapid drift in navigation output requires an increasing reliance on frequent absolute sensing), within an environment where the highly dynamic nature of the platform motion provides unreliable and infrequent absolute sensing. There are five main contributions to this thesis: Firstly, is the theoretical formulation of the autonomous localisation algorithm for a 6DoF (Degree of Freedom) platform. The process takes on the form of the Simultaneous Localisation and Mapping (SLAM) algorithm which has been quite extensivelyformulatedfortheindoorroboticscommunityandforoutdoorlandvehicle applications. In all these applications though only a 2D problem is posed simplifying the task significantly. By formulating the problem within a 6DoF framework, the SLAM algorithm is now opened to any platform description. In order to develop such a generic model, no absolute platform model can be implemented (which is advantageous) and hence the use of inertial navigation techniques are required in order to allow for prediction of state information, which is developed within this thesis. Secondly, is the recasting of the SLAM algorithm in order to improve its compu- tational efficiency. SLAM is an expensive process, and more so when the framework calls for 6DoF implementation. Moreover, increasing the number of states which are required to be estimated such as inertial sensor errors, and having the fundamental requirement of high sampling rates when using inertial sensors, further exacerbates the problem. To overcome this the algorithm is casted into its error form which iii models the error propagation in SLAM, that is, the error propagation of the states and the map. Since, in most cases, the dynamics of the error propagation is signifi- cantly slower than the dynamics of the platform itself, then dramatic improvements in computational efficiency take place. Thirdly, the thesis will add to the already significant research activity in the development of multi-vehicle SLAM, where platforms share map information in order to both improve the quality and the localisation of the platforms. The main focus is not the development of a new algorithm, but the actual implementation of the 6DoF framework within this context. Fourthly, in order to validate the effectiveness of SLAM, the real-time implemen- tation of the algorithm is developed for a highly dynamic Uninhabited Air Vehicle (UAV). The purpose is to provide a significant engineering contribution towards the knowledge of implementation. The results of the real-time algorithm is compared to an GNSS/Inertial navigation system, to illustrate the validity of the output. Finally, this thesis also presents a reliable GNSS/Inertial navigation system which couples information from a barometric altimeter. Although not a primary goal (the development was only required to provide a tool to validate the SLAM output), it was found that within highly dynamic environments, low-cost GNSS sensors are vulnera- ble to outages and long satellite reacquisition times, and hence the INS requires extra aiding, predominately in the form of height information. Furthermore, the real-time implementation of the GNSS/Inertial navigation system is also presented, forming another main engineering contribution to this thesis. Acknowledgements I would like to thank my supervisor Dr Salah Sukkarieh for his support, guidance, optimism and enthusiasm throughout the past four years. Salah was always available to give help whenever it was needed. I would also like to thank Professor Eduardo Nebot and Professor Hugh Durrant-Whyte for their support and help during this research. I must give special thanks to all the members of the ANSER project: Stuart Wishart for building the avionic systems and gluing the whole system to make it work together, Jeremy Randle for building and flying the UAVs, Matt Ridley for building the vision system, Ali Go¨ktogan for his effort with the communication and radar system, Eric Nettleton for building the decentralised system, Alan Trinder for laughs and the fake snake in Marulan, Gurce Isikyildiz and Chris Mifsud for the hardware and software they designed. Thanks also to the staff at BAE Systems for their help: Julia, Owen and Paul for the advices and discussions to improve the system. We spent lots of days and nights on the test sites to make the system work, and finally watching the working system was the most exciting moment being here. To all the other members of the ACFR group, I owe a special thanks to Jose Guivant for his invaluable helps and discussions during the period. To Juan Nieto, for his help demonstrating the GINTIC project. To Ross Hennessy for the beautiful fish tank next to my desk. To Alex and Fred for the night climbing of Mt. Fuji in Japan. To Gerold, Tim, Richard, Ralph, Trevor, Mark, Andrew, Tomo, Mari, Shron, Fabio, Mitch and Ben for their help and sense of humour, and all the others who have visited and gone from ACFR. I would particularly like to thank Anna for her help in ACFR. Thanks to Simon Lacroix, Allonzo Kelly and Stefan Williams for the reviews of my thesis and invaluable advices. Thanks to my family for their loves and concerns. To Charles and Ruby, you brought me the joy and happiness of the life. Finally a special thank to Hyeja, for your love and understanding. iv To Charles, Ruby and Hyeja Contents Declaration i Abstract ii Acknowledgements iv Contents vi List of Abbreviations xii List of Figures xiii List of Tables xxiii 1 Introduction 1 1.1 Airborne Simultaneous Localisation and Mapping . . . . . . . . . . . 3 1.2 GNSS Aided Airborne Navigation . . . . . . . . . . . . . . . . . . . . 7 1.3 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 1.4 Thesis Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 2 Statistical estimation 12 2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 2.2 Bayesian Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 2.3 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 2.4 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . 19 vi CONTENTS vii 2.5 Information Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22 2.6 Extended Information Filter . . . . . . . . . . . . . . . . . . . . . . . 25 2.7 Filter Configurations for Aided Inertial Navigation . . . . . . . . . . . 26 2.7.1 Advantages and Disadvantages of the Direct and Indirect filter Structures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 2.7.2 6DoF SLAM Structure . . . . . . . . . . . . . . . . . . . . . . 29 2.7.3 GNSS/Inertial Navigation Structure . . . . . . . . . . . . . . 31 2.7.4 GNSS/Inertial/Baro Navigation Structure . . . . . . . . . . . 31 2.8 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 3 Strapdown Inertial Navigation 34 3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34 3.2 Inertial Measurement Unit (IMU) . . . . . . . . . . . . . . . . . . . . 35 3.3 Coordinate Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 3.3.1 Inertial Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 3.3.2 Earth-Centred Earth-Fixed Frame . . . . . . . . . . . . . . . . 37 3.3.3 Geographic Frame . . . . . . . . . . . . . . . . . . . . . . . . 38 3.3.4 Earth-Fixed Local-Tangent Frame . . . . . . . . . . . . . . . . 38 3.3.5 Body Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 3.3.6 Sensor Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 3.4 Inertial Navigation Equations . . . . . . . . . . . . . . . . . . . . . . 40 3.4.1 Attitude Equations . . . . . . . . . . . . . . . . . . . . . . . . 40 3.4.2 Velocity/Position Equations . . . . . . . . . . . . . . . . . . . 50 3.5 Error Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 3.5.1 Attitude Error Equations . . . . . . . . . . . . . . . . . . . . . 54 3.5.2 Velocity/Position Error Equations . . . . . . . . . . . . . . . . 56 3.6 IMU Lever-arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59 3.7 Vibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60 3.7.1 Sampling Frequency . . . . . . . . . . . . . . . . . . . . . . . 60 3.7.2 Coning Error . . . . . . . . . . . . . . . . . . . . . . . . . . . 62 3.8 Initial Calibration and Alignment . . . . . . . . . . . . . . . . . . . . 65 3.9 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 CONTENTS viii 4 Airborne 6DoF SLAM Navigation 67 4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 4.2 6DoF SLAM Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . 68 4.2.1 Augmented State . . . . . . . . . . . . . . . . . . . . . . . . . 69 4.2.2 Nonlinear Process Model . . . . . . . . . . . . . . . . . . . . . 70 4.2.3 Relationship between Observation and Landmarks . . . . . . . 74 4.2.4 Nonlinear Observation Model . . . . . . . . . . . . . . . . . . 76 4.2.5 Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79 4.2.6 Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79 4.2.7 Data Association . . . . . . . . . . . . . . . . . . . . . . . . . 80 4.2.8 New Landmark Augmentation . . . . . . . . . . . . . . . . . . 81 4.2.9 Error Analysis on the Initialised Landmarks . . . . . . . . . . 83 4.3 Indirect 6DoF SLAM Algorithm . . . . . . . . . . . . . . . . . . . . . 85 4.3.1 External Inertial Navigation Loop . . . . . . . . . . . . . . . . 87 4.3.2 External Map . . . . . . . . . . . . . . . . . . . . . . . . . . . 87 4.3.3 Augmented Error State . . . . . . . . . . . . . . . . . . . . . . 88 4.3.4 Error Process Model . . . . . . . . . . . . . . . . . . . . . . . 88 4.3.5 Error Observation Model . . . . . . . . . . . . . . . . . . . . . 91 4.3.6 Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92 4.3.7 Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92 4.3.8 Data Association and New Landmark Initialisation . . . . . . 93 4.3.9 Feedback Error Correction . . . . . . . . . . . . . . . . . . . . 94 4.4 DDF 6DoF SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95 4.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100 5 GNSS/Inertial Airborne Navigation 102 5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102 5.2 GNSS Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103 5.3 GNSS/Inertial Integration . . . . . . . . . . . . . . . . . . . . . . . . 104 CONTENTS ix 5.3.1 Process Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 106 5.3.2 Observation Model . . . . . . . . . . . . . . . . . . . . . . . . 108 5.3.3 Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109 5.3.4 Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109 5.3.5 Feedback Error Correction . . . . . . . . . . . . . . . . . . . . 110 5.4 Observation Matching . . . . . . . . . . . . . . . . . . . . . . . . . . 111 5.4.1 Observation Latency . . . . . . . . . . . . . . . . . . . . . . . 111 5.4.2 GNSS Lever-arm . . . . . . . . . . . . . . . . . . . . . . . . . 114 5.4.3 GNSS Lever-arm Error Analysis . . . . . . . . . . . . . . . . . 114 5.5 Baro-altimeter Augmented GNSS/Inertial Navigation . . . . . . . . . 118 5.5.1 Baro-altimeter Error Model . . . . . . . . . . . . . . . . . . . 123 5.5.2 Process Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 124 5.5.3 Observation Model . . . . . . . . . . . . . . . . . . . . . . . . 124 5.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125 6 Real-time Implementation 126 6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 6.2 The ANSER Project . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 6.3 Flight Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127 6.4 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 129 6.4.1 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130 6.4.2 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131 6.4.3 Baro-altimeter . . . . . . . . . . . . . . . . . . . . . . . . . . . 133 6.4.4 Inclinometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134 6.4.5 Vision System . . . . . . . . . . . . . . . . . . . . . . . . . . . 134 6.4.6 Vision/Laser System . . . . . . . . . . . . . . . . . . . . . . . 137 6.4.7 Radar System . . . . . . . . . . . . . . . . . . . . . . . . . . . 137 6.5 GNSS/Inertial Navigation System . . . . . . . . . . . . . . . . . . . . 139 6.5.1 Hardware Development . . . . . . . . . . . . . . . . . . . . . . 139