Nonlinear Kalman Filtering for Multi-Sensor Navigation of Unmanned Aerial Vehicles Series Editor Jean-Paul Bourrières Nonlinear Kalman Filtering for Multi-Sensor Navigation of Unmanned Aerial Vehicles Application to Guidance and Navigation of Unmanned Aerial Vehicles Flying in a Complex Environment Jean-Philippe Condomines First published 2018 in Great Britain and the United States by ISTE Press Ltd and Elsevier Ltd Apart from any fair dealing for the purposes of research or private study, or criticism or review, as permitted under the Copyright, Designs and Patents Act 1988, this publication may only be reproduced, stored or transmitted, in any form or by any means, with the prior permission in writing of the publishers, or in the case of reprographic reproduction in accordance with the terms and licenses issued by the CLA. Enquiries concerning reproduction outside these terms should be sent to the publishers at the undermentioned address: ISTE Press Ltd Elsevier Ltd 27-37 St George’s Road The Boulevard, Langford Lane London SW19 4EU Kidlington, Oxford, OX5 1GB UK UK www.iste.co.uk www.elsevier.com Notices Knowledge and best practice in this field are constantly changing. As new research and experience broaden our understanding, changes in research methods, professional practices, or medical treatment may become necessary. Practitioners and researchers must always rely on their own experience and knowledge in evaluating and using any information, methods, compounds, or experiments described herein. In using such information or methods they should be mindful of their own safety and the safety of others, including parties for whom they have a professional responsibility. To the fullest extent of the law, neither the Publisher nor the authors, contributors, or editors, assume any liability for any injury and/or damage to persons or property as a matter of products liability, negligence or otherwise, or from any use or operation of any methods, products, instructions, or ideas contained in the material herein. ® MATLAB is a trademark of The MathWorks, Inc. and is used with permission. The MathWorks does not ® warrant the accuracy of the text or exercises in this book. This book’s use or discussion of MATLAB software or related products does not constitute endorsement or sponsorship by The MathWorks of a particular pedagogical approach or particular use of the MATLAB® software. For information on all our publications visit our website at http://store.elsevier.com/ © ISTE Press Ltd 2018 The rights of Jean-Philippe Condomines to be identified as the author of this work have been asserted by him in accordance with the Copyright, Designs and Patents Act 1988. British Library Cataloguing-in-Publication Data A CIP record for this book is available from the British Library Library of Congress Cataloging in Publication Data A catalog record for this book is available from the Library of Congress ISBN 978-1-78548-285-4 Printed and bound in the UK and US Preface The use of Unmanned Aerial Vehicles (UAVs) is exploding in the civil sector.Withamarketexpectedto exceedtwobillion eurosby 2015in France alone, 345 UAV operators had already registered a total of 585 aircraft with the Directorate General for Civil Aviation (DGAC) by late October 2012. AlthoughUAVsareoldnewsinthemilitarysector,theyareabrandnewfield for civil applications, such as pipeline monitoring, public protection or tools for processing and analyzing crops. New applications that use UAVs as experimental vectors are currently being researched. Among many other possible applications, UAVs seem especially promising in the fields of aerologyandmeteorology,wheretheycanbeusedtostudyandmeasurelocal phenomena such as wind gradients and cloud formations. Interestingly, since 2006, mini-UAVs and micro-UAVs account for most new aircraft. Both belongtothecategoryofsub-30-kgUAVs,whichwillbetheprimaryfocusof this book. These aircraft have the advantage of being relatively lightweight and easy to transport, unlike other types of UAV, which can weigh over 150 kg. Other than weight, UAVs can be classified by battery life, which determines their operating range. Accordingly, they are often categorized as Short Range (SR), Close Range (CR) or Medium Range (MR) aircraft. Although design configurations can vary wildly, the UAVs in any given category tend to share the following characteristics: (1) take-off weight, empty weight, nominal weight and size – these parameters create strong constraintsonthemaximumnumberandperformanceoftheUAV’son-board sensors;(2)batterylifeandmaximumrange–theseparametersdeterminethe applications for which the UAV is most suitable; (3) flight parameters with variousdegreesofuncertaintyandcompatibilitywithscenariossuchasflying x NonlinearKalmanFilteringforMulti-SensorNavigationofUnmannedAerialVehicles indoors,inclutteredenvironments,againstthewind,etc.Moregenerallythan the specific context of UAVs, a common approach to optimizing the performance of an avionic system is to establish specifications in terms of autonomy properties and closed-loop flight characteristics that satisfy the expectedmissionrequirementsoftheaircraft. Concretely, we can distinguish between the hardware components of an avionic system, which would typically consist of an embedded processor, sensors, an array of actuators and a ground/air communication module, and itssoftwarecomponents,whichwouldincludethefollowing: –signal processing algorithms for a wide range of functions, such as denoising the sensor outputs or estimating and reconstructing the state of the aircraft or other flight parameters by merging all on-board measurement data (which often have low levels of redundancy), corroborated against the output of a predictive mathematical model describing the dynamic behavior of the aircraft; –“low-level”controlalgorithmsforclosed-loopoperationandguidanceof theaircraft,allowingittobeprogrammedwithinstructions; –“high-level” control algorithms for navigation, rerouting or decision support (AI) in the absence of a human operator, or in suboptimal scenarios (lossofcomms,mechanicalfailure,etc.). Thus, control algorithms clearly have an essential role to play, as do the algorithms that estimate the state or parameters of the aircraft, especially since cost and space constraints limit the capacities of the underlying sensor and actuator technologies. This is particularly relevant for micro-UAVs and mini-UAVs. Estimation algorithms allow us to merge imperfect information obtained from different sensors in real time in order to construct estimates, e.g. of the state of the UAV (orientation, velocity, position), by running control algorithms on the on-board processor. The closed-loop controls need to guarantee that the UAV remains stable regardless of the order in which instructions are received from the operator or automatic flight management system, while also ensuring that all instructions were received correctly. Estimation and control are therefore crucial aspects of every mission. One extremely important dimension of the mini-UAVs discussed in this book is their payload capacity. Mini-UAVs have relatively limited space. Combined with the budgetary constraints of mini-UAV development projects, this Preface xi ultimately means that only so-called “low-cost” equipment is viable. Despite significant progress in miniaturization and a steady growth in on-board processing power (see Moore’s law), these mini-UAVs can therefore only realistically use limited-performance sensors to accomplish the ever-expanding panel of missions with which they are entrusted. For these new missions, mini-UAVs must be able to safely enter and share civil airspace; they must be able to pass flight certifications equivalent to those imposed on cargo flights operated by commercial airlines. In the context of safety, using estimation techniques to consolidate the UAV’s on-board knowledge of its own state becomes an essential component of the control framework, especially in suboptimal operating conditions (sensor failure, intermittent loss of signal, noise and perturbations from the environment, imperfect measurements, etc.). Attempting to tackle these challenges has naturally led researchers to explore relatively new problems, some of which are quite different from those encountered in civil and military aeronautics, whose avionic systems can differ drastically from those considered in this book.Inourcase, weneedextremelysophisticatedavionicsystemsthatmust perform a diverse range of on-board functions with minimal weight or space usage. In particular, our estimation algorithms must satisfy a number of very strong constraints, not only in terms of performance, but also execution time, memoryspaceandconvergenceproperties. This book presents an original algorithmic solution to the problem of estimating the state of a mini-UAV in flight in a manner that is compatible with the inherent payload constraints of the system. Our approach is oriented towardmodel-basednonlinearestimationmethods.Ourfirststepistodefinea dynamic model that describes the flight of the mini-UAV. This model should be sufficiently general as to work with multiples types of mini-UAV (fixed-wing, quadcopter, etc.). Next, two original estimation algorithms, called IUKF and π-IUKF, are developed on the basis of this model, then tested, first against simulations, then against real data for the π-IUKF algorithm. These two methods apply the general framework of invariant observers to the nonlinear estimation of the state of a dynamic system using an Unscented Kalman Filter (UKF) method, from the more general class of Sigma Point (SP) nonlinear filtering algorithms. In future, the solutions outlined here will be integrated into the avionics of the mini-UAVs studied at the ENAC laboratory; the source code is already available as part of the autopilotofthePaparazziproject. xii NonlinearKalmanFilteringforMulti-SensorNavigationofUnmannedAerialVehicles Organization of this book Thisbookisdividedintofivechapters: – Chapter 1 presents the background of aerial robotics at the time of writing, giving an overview of the various technological advancements that have allowed it to experience such extensive growth in the civil sector. The growth of aerial robotics has inspired a variety of so-called “open source” development projects, which aim to provide a comprehensive autopilot system for mini-UAVs. In the conclusion of this chapter, we explain the motivation behind the research presented throughout the rest of the book and discuss the challenges associated with designing a mini-UAV state estimator that is compatible with the inherent capacity constraints of the open-source Paparazziframework. – Chapter 2 presents the latest advancements in estimation techniques. This literature review is used as a reference throughout the rest of the book, and focuses in particular on two specific techniques: Kalman filtering and invariant observers. Any readers who are unfamiliar with differential geometry can additionally refer to Appendix A. It is based on a combination of these two techniques that we were able to develop and validate our own twononlinearestimationalgorithms. – In Chapter 3, we outline the various kinematic models commonly used tomanagethenavigationofdynamicsystems.Theestimatorfiltersdeveloped in this book were built from these more general models, which allowed us to account for perturbations to the system in the form of random errors. We conducted a detailed observability study to determine whether the state variables can be reconstructed from the known system inputs and measurements. We shall see that accurate dynamic models of inertial navigation can have several degrees of freedom, and so a certain number of model assumptions are required to guarantee that the estimation problem remainsobservable. – Based on the models established in Chapter 3, Chapter 4 documents the development of a set of original methodological principles that allowed us to construct two nonlinear estimation algorithms, IUKF and π-IUKF, which differintermsoftheirformulation.Thesealgorithms,foundedonthetheories of invariant observers and so-called “unscented” Kalman filters, offer an extremely valuable algorithmic solution to the challenges of inertial navigation. By comprehensively summarizing the initial results which Preface xiii characterize the IUKF algorithms, we demonstrate their well-foundedness andhighlighttheiradvantages,boththeoreticalandpractical. – Chapter 5 presents all of the results obtained so far regarding the π-IUKFalgorithm.Thefirstpartofthischaptercomparestheperformanceof the SR-UKF (standard) and π-IUKF algorithms. The analysis is based on simulated noisy data generated from the general models introduced in Chapter 3 after accounting for imperfections in each type of sensor. Continuing with the case of a complete navigation model, the second part of this chapter presents several experimental results obtained by estimating the state of the mini-UAV from real data using the three primary algorithms considered: SR-UKF, π-IUKF and the classical invariant observer approach. These results validate the approach, allowing a specific correction to be derived for the prediction obtained from any given representation of a nonlinear state used in the estimation, in such a way that the dynamics of the constructedobserversatisfythesymmetrypropertiesofthesystem. Finally,thebookisbroughttoaclosebyacollectionofappendices,which arereferencedwhereverrelevantthroughouttherestofthebook. Jean-PhilippeCONDOMINES June2018 1 Introduction to Aerial Robotics 1.1. Aerial robotics Aerial robots, also known as Remotely Piloted Aerial Systems (RPAS) or Unmanned Aerial Vehicles (UAVs), are unmanned aircraft that can complete their mission with some degree of autonomy. Their primary purpose is to execute a task more safely or effectively than a remotely-controlled aircraft. The possibilities offered by autonomous systems such as UAVs in the civil sector have been thoroughly explored over the last few years. Various research projects, including some financed by the European Commission, have studied the potential civil applications of UAVs. Similar aircraft had previously been frequently used for specific military purposes in various interventions,includinginIraqandAfghanistan,wheretheyplayedakeyrole asactivelinksofinformation,andindecisionandactionnetworks.Today,the operational benefits provided by UAVs with access to sufficient decisional resources have revolutionized the intervention scenarios of missions conducted in hostile zones with significant risk to human life. Surveillance missions have undergone a similar metamorphosis and are completely unrecognizablecomparedtojustafewdecadesago.Thesystemnowassumes responsibility for piloting and guidance tasks, as well as lookout tasks for which human vigilance has proven fallible. UAVs can now relieve their human operators, allowing the latter to dedicate more time to managing the mission at a higher level. UAVs are also currently being studied as experimentalvectorsforvariousapplications.