Linköping Studies in Science and Technology. Dissertations. No. 998 Estimation of Nonlinear Dynamic Systems Theory and Applications Thomas B. Schön Department of Electrical Engineering Linköpings universitet, SE–581 83 Linköping, Sweden Linköping 2006 Estimation of Nonlinear Dynamic Systems – Theory and Applications c 2006 Thomas B. Schön [email protected] www.control.isy.liu.se Division of Automatic Control Department of Electrical Engineering Linköpings universitet SE–581 83 Linköping Sweden ISBN 91-85497-03-7 ISSN 0345-7524 Printed by LiU-Tryck, Linköping, Sweden 2006 I dedicate this thesis to the memory of my brother Erik Abstract This thesis deals with estimation of states and parameters in nonlinear and non-Gaussian dynamic systems. Sequential Monte Carlo methods are mainly used to this end. These methods rely on models of the underlying system, motivating some developments of the model concept. One of the main reasons for the interest in nonlinear estimation is that problems of this kind arise naturally in many important applications. Several applications of nonlinear estimation are studied. The models most commonly used for estimation are based on stochastic difference equations, referred to as state-space models. This thesis is mainly concerned with models of this kind. However, there will be a brief digression from this, in the treatment of the mathematically more intricate differential-algebraic equations. Here, the purpose is to write these equations in a form suitable for statistical signal processing. The nonlinear state estimation problem is addressed using sequential Monte Carlo methods, commonly referred to as particle methods. When there is a linear sub-structure inherent in the underlying model, this can be exploited by the powerful combination of the particle filter and the Kalman filter, presented by the marginalized particle filter. This algorithm is also known as the Rao-Blackwellized particle filter and it is thoroughly de- rived and explained in conjunction with a rather general class of mixed linear/nonlinear state-space models. Models of this type are often used in studying positioning and tar- get tracking applications. This is illustrated using several examples from the automotive and the aircraft industry. Furthermore, the computational complexity of the marginalized particle filter is analyzed. The parameter estimation problem is addressed for a relatively general class of mixed linear/nonlinear state-space models. The expectation maximization algorithm is used to calculate parameter estimates from batch data. In devising this algorithm, the need to solve a nonlinear smoothing problem arises, which is handled using a particle smoother. The use of the marginalized particle filter for recursive parameter estimation is also inves- tigated. The applications considered are the camera positioning problem arising from aug- mented reality and sensor fusion problems originating from automotive active safety sys- tems. The use of vision measurements in the estimation problem is central to both appli- cations. In augmented reality, the estimates of the camera’s position and orientation are imperative in the process of overlaying computer generated objects onto the live video stream. The objective in the sensor fusion problems arising in automotive safety systems is to provide information about the host vehicle and its surroundings, such as the posi- tion of other vehicles and the road geometry. Information of this kind is crucial for many systems, such as adaptive cruise control, collision avoidance and lane guidance. v Sammanfattning Denna avhandling behandlar skattning av tillstånd och parameterar i olinjära och icke- gaussiska system. För att åstadkomma detta används huvudsakligen sekventiella Monte Carlo-metoder. Dessa metoder förlitar sig på modeller av det underliggande systemet, vilket motiverar vissa utvidgningar av modellkonceptet. En av de viktigaste anledningarna till intresset för olinjär skattning är att problem av detta slag uppstår naturligt i många viktiga tillämpningar. Flera tillämpade olinjära skattningsproblem studeras. De modeller som används för skattning är normalt baserade på stokastiska differen- sekvationer, vanligtvis kallade tillståndsmodeller. Denna avhandling använder huvudsak- ligen modeller av detta slag. Ett undantag utgörs dock av de matematiskt mer komplice- rade differential-algebraiska ekvationerna. Målet är i detta fall att skriva om ekvationerna på en form som lämpar sig för statistisk signalbehandling. Det olinjära tillståndsskattningsproblemet angrips med hjälp av sekventiella Monte Carlo-metoder, även kallade partikelmetoder. En linjär substruktur ingående i den un- derliggande modellen kan utnyttjas av den kraftfulla kombination av partikelfiltret och kalmanfiltret som tillhandahålls av det marginaliserade partikelfiltret. Denna algoritm går även under namnet Rao-Blackwelliserat partikelfilter och den härleds och förklaras för en generell klass av tillståndsmodeller bestående av såväl linjära, som olinjära ekvationer. Modeller av denna typ används vanligen för att studera positionerings- och målföljnings- tillämpningar. Detta illustreras med flera exempel från fordons- och flygindustrin. Vidare analyseras även beräkningskomplexiteten för det marginaliserade partikelfiltret. Parameterskattningsproblemet angrips för en relativt generell klass av blandade lin- jära/olinjära tillståndsmodeller. “Expectation maximization”-algoritmen används för att beräkna parameterskattningar från data. När denna algoritm appliceras uppstår ett olinjärt glättningsproblem, vilket kan lösas med en partikelglättare. Användandet av det margina- liserade partikelfiltret för rekursiv parameterskattning undersöks också. De tillämpningar som betraktas är ett kamerapositioneringsproblem härstammande från utökad verklighet och sensor fusionproblemet som uppstår i aktiva säkerhetssystem för fordon. En central del i båda dessa tillämpningar är användandet av mätningar från kamerabilder. För utökad verklighet används skattningarna av kamerans position och ori- entering för att i realtid överlagra datorgenererade objekt i filmsekvenser. Syftet med sen- sor fusionproblemet som uppstår i aktiva säkerhetssystem för bilar är att tillhandahålla information om den egna bilen och dess omgivning, såsom andra fordons positioner och vägens geometri. Information av detta slag är nödvändig för många system, såsom adaptiv farthållning, automatisk kollisionsundvikning och automatisk filföljning. vii Acknowledgments During my work with this thesis I have met and interacted with many interesting people who have, in one way or another, influenced the path of my research. First of all I would like to express my deepest gratitude towards Professor Fredrik Gustafsson, who has been my thesis advisor during the past four years. He is a never-ending source of inspiration and ideas and I just wish I could make use of them all. Furthermore, I very much appre- ciate his great enthusiasm for the subject and his cheerful attitude. We have had a lot of fun over the years. I truly enjoy working with you, Fredrik! I am very grateful to Professor Lennart Ljung for creating an excellent environment for conducting research. A special thanks goes to Dr. Jan Maciejowski and Professor Lennart Ljung for introducing me to the world of academic research during my time at the University of Cambridge in Cambridge, United Kingdom in 2001. Without that pleasant stay, this thesis would not exist today. I would also like to thank Brett Ninness for inviting me to the University of Newcastle in Newcastle, Australia. During my time there I had many interesting experiences, rang- ing from research discussions on nonlinear estimation to diving with sharks. My office mates, Dr. Adrian Wills and Dr. Sarah Johnson were great and they really made me feel at home. I thank Professor Tomoyuki Higuchi for inviting me to the Institute of Statistical Mathematics in Tokyo, Japan on my way to Australia. Several colleges deserve special thanks for always taking their time to listen to my ideas and answering my questions. Dr. Fredrik Tjärnström, for being my scientific mentor in the beginning of my studies. We have had and still have many interesting discussions regarding research strategies, system identification and many non-scientific topics as well. Dr. Martin Enqvist, for our interesting discussions. Jeroen Hol, for a good collaboration on inertial sensors and image processing. Ulla Salaneck, for being the wonderful person she is and for always helping me with administrative issues. Gustaf Hendeby, for always helping me in my constant trouble of getting LaTeX to do what I want. I am very grateful to my co-authors for all the interesting discussions we have had while carrying out the research leading to our publications. They are (in alphabetical or- der), Andreas Eidehall, Markus Gerdin, Professor Torkel Glad, Professor Fredrik Gustafs- son, Dr. Anders Hansson, Dr. Rickard Karlsson, Professor Lennart Ljung, Brett Ninness, Per-Johan Nordlund and Dr. Adrian Wills. Parts of the thesis have been proofread by Andreas Eidehall, Markus Gerdin, Pro- fessor Fredrik Gustafsson, Gustaf Hendeby, Jeroen Hol, Dr. Rickard Karlsson, Martin Ohlson, Henrik Tidefelt and Dr. Fredrik Tjärnström. Your comments and suggestions have improved the quality of the thesis substantially and I am very grateful for that. I am responsible for any remaining errors. During my work with this thesis I have been involved in two applied research projects, Markerless real-time Tracking for Augmented Reality Image