# Autonomous Localization in Unknown Environments Jonas Callmer

Linköping studies in science and technology. Dissertations. No. 1520 Autonomous Localization in Unknown Environments Jonas Callmer Department of Electrical Engineering Linköping University, SE–581 83 Linköping, Sweden Linköping 2013 Cover illustration: Example of positioning result using a foot-mounted inertial magnetic sensor unit. The user is visiting every room in a corridor, similar to how fire fighters operate. The result is overlaid on a floor plan that was unknown to the positioning system. Linköping studies in science and technology. Dissertations. No. 1520 Autonomous Localization in Unknown Environments Jonas Callmer [email protected] www.control.isy.liu.se Division of Automatic Control Department of Electrical Engineering Linköping University SE–581 83 Linköping Sweden ISBN 978-91-7519-620-6 ISSN 0345-7524 Copyright © 2013 Jonas Callmer Printed by LiU-Tryck, Linköping, Sweden 2013 Till mina föräldrar Abstract Over the last 20 years, navigation has almost become synonymous with satellite positioning, e.g. the Global Positioning System (gps). On land, sea or in the air, on the road or in a city, knowing ones position is a question of getting a clear line of sight to four or more satellites. Unfortunately, since the signals are extremely weak there are environments the gps signals cannot reach but where positioning is still highly desired, such as indoors and underwater. Also, because the signals are so weak, gps is vulnerable to jamming. This thesis is about alternative means of positioning for three scenarios where gps cannot be used. Indoors, there is a desire to accurately position first responders, police officers and soldiers. This could make their work both safer and more efficient. In this thesis, an inertial navigation system using a foot mounted inertial magnetic measurement unit is studied. For such systems, zero velocity updates can be used to significantly reduce the drift in distance travelled. Unfortunately, the estimated direction of movement is also subject to drift, causing large positioning errors. We have therefore chosen to throughly study the key problem of robustly estimating heading indoors. To measure heading, magnetic field measurements can be used as a compass. Unfortunately, they are often disturbed indoors making them unreliable. For estimation support, the turn rate of the sensor can be measured by a gyro but such sensors often have bias problems. In this work, we present two different approaches to estimate heading despite these shortcomings. Our first system uses a Kalman filter bank that recursively estimates if the magnetic readings are disturbed or undisturbed. Our second approach estimates the entire history of headings at once, by matching the cumulative sum of gyro measurements to a vector of magnetic heading measurements. Large scale experiments are used to evaluate both methods. When the heading estimation is incorporated into our positioning system, experiments show that positioning errors are reduced significantly. We also present a probabilistic stand still detection framework based on accelerometer and gyro measurements. The second and third problems studied are both maritime. Naval navigation systems are today heavily dependent on gps. Since gps is easily jammed, the vessels are vulnerable in critical situations. In this work we describe a radar based backup positioning system to be used in case of gps failure. radar scans are matched using visual features to detect how the surroundings have changed, thereby describing how the vessel has moved. Finally, we study the problem of underwater positioning, an environment gps signals cannot reach. A sensor network can track vessels using acoustics and the disturbances they induce to the earth magnetic field. But in order to do so, the sensors themselves must first be positioned. We present a system that accurately positions underwater sensors using a friendly vessel with a known magnetic signature and trajectory. Simulations show that by studying the magnetic disturbances caused by the vessel, the location of each sensor can be accurately estimated. v Populärvetenskaplig sammanfattning För att bestämma sin position utomhus använder man idag ofta satellitbaserade positioneringssystem såsom det amerikanska gps-systemet. På en öppen plats kan man med en enkel mottagare få reda på sin position på några meter när inom loppet av ett par sekunder. Det krävs att platsen är öppen eftersom man måste kunna se minst fyra satelliter samtidigt för att positionen ska kunna bestämmas. I de flesta fall är det inget problem, men i vissa fall innebär kravet en stor begränsning. Den här avhandlingen handlar om problemet att kunna bestämma sin position i miljöer där gps inte går att använda. Vi har valt att studera positionering i huvudsak inomhus men även på och under vatten. Signalerna från gps-systemet är nämligen så svaga att de inte kan penetrera byggnader eller vatten alls. I de få fall man kan ta emot en signal, har den oftast studsat mot andra objekt på vägen. Eftersom man då inte har fri sikt till fyra satelliter så går det ändå inte att positionera sig med god precision. För att positionera sig i sådana miljöer krävs det alltså helt nya metoder. Idag efterfrågas inomhuspositioneringssystem för exempelvis brandmän, soldater och poliser. Tanken är exempelvis att om ett rökdykarpars positioner är kända i realtid kan räddningsinsatsen bli både säkrare och effektivare. Deras uniformer kan innehålla specialanpassad utrustning som räknar ut rökdykarnas positioner och vidareförmedlar dessa till berörda parter. Rökdykare arbetar ofta i okända inomhusmiljöer. I framtiden kan man tänka sig att digitala kartor kommer att finnas tillgängligt för räddningstjänsten om det börjar brinna i till exempel ett hotell, ett sjukhus eller i en skola. Kartan kan då användas för att hjälpa till att beräkna rökdykarens position i en stor byggnad. I den här avhandlingen har vi studerat problemet att positionera sig i miljöer där kartor troligtvis inte kommer finnas tillgängliga under många år framöver, såsom i lägenheter, villor, små kontor och dylikt. Positioneringssystemet baseras på en sensor som mäter acceleration, vinkelhastighet och magnetfält. Vinkelhastighet är hur snabbt sensorn roterar. Acceleration tillsammans med vinkelhastighet kan användas för att uppskatta hur användaren rör sig och magnetfältet kan ge information om hur användaren är riktad i förhållande till norr. Systemet är ett så kallat dödräkningssystem eftersom det utgår från en position och sedan försöker beräkna förflyttningen från denna punkt. Sensorn är fastsatt på foten, och användarens rörelser beräknas steg för steg. Genom att utnyttja att sensorn är still när foten är i kontakt med marken så kan ett stegs längd beräknas på några centimeter när. Dödräkningssystemet baseras alltså på steg för steg-förflyttningar som sker i antagna riktningar, men om rökdykaren egentligen rör sig i en annan riktning än den man tror, kommer den uppskattade positionen bli väldigt fel väldigt snabbt. Att ha en mycket god kännedom om i vilken riktning rökdykaren faktiskt rör sig är således avgörande. Det problem som studeras mest i den här avhandlingen är därför riktningsskattvii viii Populärvetenskaplig sammanfattning ning. Vi använder en magnetfältssensor som mäter jordens magnetfält och därför ger oss riktningen mot norr, samt ett gyro som mäter vinkelhastigheter för att beräkna riktningen. Tyvärr är det dock svårt att mäta jordens magnetfält inomhus. Stålstrukturer, elskåp, kablar och liknande producerar magnetiska störningar som ofta är starkare än jordens magnetfält. Sådana störningar är mycket vanliga inomhus varför magnetfältssensorn är mycket opålitlig inomhus. I den här avhandlingen presenteras två metoder att uppskatta användarens riktning trots dessa störningar. Dessa metoder har inkluderats i dödräkningssystemet för att minska de fel som orsakades av osäker riktning. Experiment visar att felet i den uppskattade positionen minskar betydligt när pålitlig information om användarens riktning finns tillgängligt. Eftersom signalerna från gps-satelliterna är så otroligt svaga så är de också mycket känsliga för störningar. Idag är det lätt att införskaffa störsändare som sänder ut signaler som överröstar signalerna från satelliterna, vilket slår ut all gpspositionering inom ett stort område. Ofta är det dock inte illvilja som ligger bakom sådana störningar. I de flesta fall är det användaren själv som omedvetet sänder ut signaler inom fel frekvensband som förstör positioneringen. I somliga miljöer, såsom fartygsnavigation, används gps av många olika system varför en störning i fel läge kan få allvarliga konsekvenser. Vi har därför tagit fram ett reservsystem för positionering som är oberoende av gps. Systemet är istället baserat på fartygets egen radar. Ett sådant system kan användas för att detektera gps-störningar och minska dess inverkningar. Positioneringssystemet använder sig av de bilder av omgivningen som radarsystemet ger. Öar och kustlinjen avtecknas väl och avståndet och vinkeln till dessa kan mätas med god precision. Ofta finns det vissa objekt i omgivningarna som syns särskilt väl såsom hus eller branta klippor. Sådana objekt kan observeras över en lång tid och kallas för landmärken. Genom att studera flera sådana landmärken och hur deras positioner relativt fartyget förändras över tiden, kan fartygets förflyttning beräknas. Vi presenterar ett positioneringssystem som använder sådana radar-baserade landmärken och experiment visar att fartyget kan positioneras med god precision över långa sträckor. Det tredje problemet som behandlas i avhandlingen är undervattenspositionering. Att exakt kunna bestämma en position under vatten är generellt sett dyrt och svårt eftersom gps-signalerna inte når ner. I det här fallet är det speciella undervattenssensorer som vi ska bestämma positionen för, varför problemet går att lösa med hjälp av sensorernas egna mätningar. För att upptäcka och följa inkommande fartyg kan man använda ett nätverk av sensorer på havets botten. Sensorerna känner av de magnetfältstörningar som fartyget ger upphov till och uppfattar även ljudet från fartygets framdrivningssystem. Dessa kan användas för att positionsbestämma ett fartyg med god precision, givet att sensorernas egna positioner är kända. I den här avhandlingen studerar vi problemet att positionsbestämma sensorerna själva med hjälp av ett fartyg som ger upphov till en känd magnetisk störning. Fartyget framförs genom området där sensorerna finns och de magnetiska störningar som fartyget ger upphov till Populärvetenskaplig sammanfattning ix kan användas för att beräkna sensorernas positioner. Om även gps-mätningar av fartygets rutt tas med i beräkningarna så kan sensorernas positioner bestämmas med god precision. Systemet utvärderas med hjälp av simuleringar. Acknowledgments This thesis marks the end of a journey. Five years went by pretty fast even though it doesn’t feel that way when one is in the middle of it. The improvements one experience are slow but steady until one day you realize that you have actually learned quite a lot. The person who has been my greatest source of learning is my supervisor Professor Fredrik Gustafsson. Your broad knowledge and experience has been a great guiding light throughout this journey. Thank you for your never ending ability to find time to assist despite your extremely busy schedule. The final proof to me of my progress was when we went from you always telling me ’Isn’t that what I said a month ago?’ to me finally getting to tell you once: ’That’s what I said a month ago’. It has been a pleasure. For all those things I needed to discuss, my co-supervisor Dr David Törnqvist has always been there to help. Your enthusiastic and helpful manners combined with your solid knowledge makes me coming back to learn more. Thank you for your assistance! The Automatic Control group is a very well functioning group with a welcoming and helpful atmosphere combined with a culture of hard work and a willingness to excel in all fields. It is steadily steered by Professor Svante Gunnarsson who took over after Professor Lennart Ljung who built the group from the ground up. Not until I got a good insight into how some other groups function, did I fully realize how exceptionally well organized our group really is. You have both done a tremendous job. Whenever I run into administrative issues, our secretary Ninna Stensgård and before her Åsa Karmelind and before her Ulla Salaneck, have always been there to help. Thank you! I would also like to thank Dr Gustaf Hendeby and Dr Henrik Tidefelt for creating an astounding LATEXtemplate making writing this thesis a matter of only that: writing. Your attention to detail never ceases to amaze me. Various parts of this thesis have been proofread by Fredrik, David and Dr Karl Granström. Your input has significantly improved this thesis and I thank you for that. All remaining errors are naturally mine. A little more than a year ago, I had the good fortune of doing a predoc in the US. When the original plans were cut extremely short, Professor Arogyaswami Paulraj at Stanford University was kind enough to welcome me to visit his group instead. While there, Professor Stephen Boyd took time from his intense schedule to supervise me. Your suggestion that I should look at the heading estimation problem spawned plenty of ideas, that now constitute about half of this thesis. Thank you very much for your time and enthusiasm! I would also like to thank Professor Peter Stoica who helped to set up this arrangement on such a short notice. The research school Forum Securitatis of which I have been participating, xi xii Acknowledgments deserve a special acknowledgement for funding the visit. It was a great experience for me and I want to thank you all so very much. During these years I have been lucky enough to have some of my best friends as colleagues. Dr Karl Granström and I go back to the days of our master thesis project and despite seeing you almost every day for something like seven years, I do not get tired of your company. We have had many good times in Sydney, Tasmania, Samoa, Japan, the Balkans and of course in Linköping and even though this marks the end of an era I do not think it marks the end of a great friendship. Lic Martin Skoglund and I go back even longer, sharing a few thousand cups of coffee and thereby plenty of time to look for treasure. Enough time it turned out, to make Maria jealous of me more than once. Thank you for the happy times that have been and the ones that are yet to come. Dr Christian Lundquist is a man of many talents and his great sense of humour always makes him terrific company. Our discussions have been plenty and I have enjoyed them all. And I will never forget your comment ’I don’t like nature!’ halfway through our trip through the Scottish highlands, which is nothing but nature! In the end we found what we were looking for: Aberlour 10 – rich but delicate. Lic Zoran Sjanic is as tall as he is fun. And under that surface, lies a great and very helpful source of knowledge and experience. Sometimes when I have one of those questions, Zoran will provide me with all that I ever needed to know. And thank you for showing us Sarajevo. Lic Hanna Fager is a great friend with whom I have shared many talks, fikas, travels and parties during these years. Let us hope we can make it last for a long time. A few more deserve an honorary mention. Lic Daniel Peterson, my bother in arms, is always there to laugh at my super hacker genius related issues. Dr Henrik Ohlsson for our Ella elle l’a fridays during more than two years as office mates. Lic Sina Khoshfetrat Pakazad serves these meat with a side of meat-barbecues and movies with special effects but no plot. I am surprised and a bit disappointed though that you still have not seen the greatest bad movie of all time, Frankenfish, but I guess it is a people problem. Three years ago Christian, David, Dr Per Skoglar, Dr Peter Bunus, Fredrik and I founded the spin off company SenionLab. It has been a very interesting journey to combine the academic research with the in many ways very different business driven corporate research. This experience has taught me two completely separate ways of thinking. My will of doing an academic Master thesis project led me into the realm of Dr Juan Nieto and Dr Fabio Ramos, the dynamic duo of ACFR. Our great time together made me want to start this second journey towards a PhD. Thank you Acknowledgments xiii for the good times and for the inspiration! My old friends from the Y-years, my childhood and from various corners of the globe, I hope we can maintain our friendships despite the fact that our lives are slowly diverging as time goes by. Hopefully, the completion of this project will provide me with some spare time again that I would be happy to share with you. I have had the extraordinary luck in life to have been blessed with a family that always supports me. My sister Kajsa and her fiance Mikael have the good sense of also living in Linköping and have created my wonderful little niece Maja. No one can say mooboo like you! My parents Gunnar and Eva, thank you for all your never-ending love and support in everything I have ever decided to do. It has made me the one I am today and it is the greatest gift of all. I would finally like to gratefully acknowledge CADICS, a Linnaeus center funded by the Swedish Research Council, and the Excellence Center at Linköping–Lund in Information Technology, ELLIIT, for providing the funding for this work. Linköping, April 2013 Jonas Callmer Contents Notation I xix Background 1 Introduction 1.1 Problem Description . . . . . . 1.1.1 Indoor Localization . . . 1.1.2 Surface Localization . . 1.1.3 Underwater Localization 1.2 Contributions . . . . . . . . . . 1.2.1 Additional Publications 1.2.2 SenionLab . . . . . . . . 1.3 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 4 4 6 6 6 8 14 15 2 Sensor Fusion 2.1 Sensors . . . . . . . . . . . . . . . . . . . . . 2.1.1 Inertial Magnetic Measurement Unit 2.1.2 RADAR . . . . . . . . . . . . . . . . . 2.1.3 Global Navigation Satellite System . 2.2 Models . . . . . . . . . . . . . . . . . . . . . 2.2.1 Continuous Models . . . . . . . . . . 2.2.2 Discrete Time Models . . . . . . . . . 2.2.3 Dynamic Model Restructuring . . . . 2.3 Estimation Theory . . . . . . . . . . . . . . . 2.3.1 Kalman Filter . . . . . . . . . . . . . 2.3.2 Extended Kalman Filter . . . . . . . 2.3.3 Optimization Formulation . . . . . . 2.3.4 Kalman Filter Banks . . . . . . . . . 2.3.5 Interacting Multiple Model . . . . . 2.3.6 Hidden Markov Model . . . . . . . . 2.4 Estimation under Disturbances . . . . . . . 2.4.1 Problem Fundamentals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 18 19 21 22 23 23 23 25 26 27 27 29 30 32 34 35 36 xv . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xvi CONTENTS 2.4.2 2.4.3 2.4.4 2.4.5 General Solution Outline . State and Mode Estimation Examples of Problems . . Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 37 38 39 3 Indoor Positioning 3.1 Human Positioning in an Unknown Environment . . . . . . . . . . 3.2 Human Positioning in a Known Environment . . . . . . . . . . . . 3.2.1 Map Matching . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2.2 Radio Positioning . . . . . . . . . . . . . . . . . . . . . . . . 3.3 Foot Mounted IMMU for Dead Reckoning . . . . . . . . . . . . . . 3.3.1 Stand Still Detection . . . . . . . . . . . . . . . . . . . . . . 3.3.2 Stand Still Detection Performance for Different IMMU Positions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41 42 44 44 45 45 46 4 Discussion and Future Work 4.1 Discussion . . . . . . . . . . . . . . . . 4.1.1 Indoor Localization . . . . . . . 4.1.2 RADAR SLAM . . . . . . . . . . 4.1.3 Underwater Sensor Positioning 4.2 Future Work . . . . . . . . . . . . . . . 55 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59 59 59 61 61 62 A Quaternion Properties A.1 Operations and Properties . . . . . . . . A.2 Describing a Rotation using Quaternions A.3 Rotation Matrix . . . . . . . . . . . . . . A.4 Quaternion Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65 65 66 66 67 Bibliography II 69 Publications A Robust Heading Estimation Indoors 1 Introduction . . . . . . . . . . . . . . . . . . . . . 2 Related Work on Magnetic Disturbances Indoors 2.1 Disturbance Studies . . . . . . . . . . . . . 2.2 Influence Reduction . . . . . . . . . . . . . 2.3 Disturbance Detection . . . . . . . . . . . 2.4 Alternative Yaw Estimation Approaches . 3 Magnetometer Signal Evaluation . . . . . . . . . 4 Principles of Yaw Estimation . . . . . . . . . . . . 5 Adaptive Filtering . . . . . . . . . . . . . . . . . . 5.1 Gyro Sensor Error Modeling . . . . . . . . 5.2 Magnetic Disturbance Modeling . . . . . 5.3 Estimation System . . . . . . . . . . . . . 6 Divergence Monitoring and Reinitialization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77 79 82 82 82 82 83 83 86 88 88 88 89 89 xvii CONTENTS 7 Implementation . . . . . . . . . . . 7.1 Models . . . . . . . . . . . . 7.2 Filter Implementation, IMM 8 Experimental Results . . . . . . . . 8.1 Detailed Evaluation . . . . . 8.2 Large Scale Evaluation . . . 9 Conclusions . . . . . . . . . . . . . Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90 . 90 . 91 . 93 . 94 . 97 . 101 . 102 B Robust Heading Estimation Indoors using Convex Optimization 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 Problem Fundamentals . . . . . . . . . . . . . . . . . . . . . . 3 Heading Estimation . . . . . . . . . . . . . . . . . . . . . . . . 3.1 Detect Disturbed Magnetometer Readings . . . . . . . 3.2 Parameter Estimation . . . . . . . . . . . . . . . . . . 3.3 Magnetic Heading Vector Unwrapping . . . . . . . . . 3.4 Solver Outline . . . . . . . . . . . . . . . . . . . . . . . 4 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . 4.1 Detailed Experiment . . . . . . . . . . . . . . . . . . . 4.2 Mass Experiments . . . . . . . . . . . . . . . . . . . . . 5 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105 107 110 111 111 112 113 114 115 116 116 120 120 122 C An Inertial Navigation Framework for Indoor Positioning with Robust Heading 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 An Inertial Navigation Framework . . . . . . . . . . . . . . . . . . 2.1 Principles of IMMU Based Dead Reckoning . . . . . . . . . 2.2 States and Inputs . . . . . . . . . . . . . . . . . . . . . . . . 2.3 Dynamic Model . . . . . . . . . . . . . . . . . . . . . . . . . 3 Exogenous Information Framework . . . . . . . . . . . . . . . . . . 3.1 Information Sources . . . . . . . . . . . . . . . . . . . . . . 3.2 Discrete Hidden Markov Model Framework . . . . . . . . . 3.3 Test Statistics . . . . . . . . . . . . . . . . . . . . . . . . . . 3.4 Optimal HMM Filter . . . . . . . . . . . . . . . . . . . . . . 4 Stand Still Detection . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1 Test Statistics . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Mode Switch Probability . . . . . . . . . . . . . . . . . . . . 4.3 Stand Still Measurement Models . . . . . . . . . . . . . . . 4.4 Stand Still Measurement Update . . . . . . . . . . . . . . . 4.5 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 Robust Heading for Indoor Positioning . . . . . . . . . . . . . . . . 5.1 Principles for Utilizing Magnetic Heading in an INS . . . . 5.2 Magnetic Disturbance Detection . . . . . . . . . . . . . . . 5.3 Heading Vector Estimation . . . . . . . . . . . . . . . . . . . 123 125 127 127 128 128 129 129 129 130 130 131 131 132 132 132 133 133 136 136 138 xviii 5.4 Trajectory Postprocessing using Heading . 5.5 Magnetic Heading Utilization Discussion 6 Experimental Results . . . . . . . . . . . . . . . . 6.1 Experiment 1 . . . . . . . . . . . . . . . . 6.2 Experiment 2, Corridor . . . . . . . . . . . 7 Conclusions . . . . . . . . . . . . . . . . . . . . . Bibliography . . . . . . . . . . . . . . . . . . . . . . . . CONTENTS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140 140 142 142 142 144 146 D RADAR SLAM using Visual Features 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . 2 Background and Relation to SLAM . . . . . . . . . . . . . 3 Theoretical Framework . . . . . . . . . . . . . . . . . . . . 3.1 Detection Model . . . . . . . . . . . . . . . . . . . . 3.2 Measurement Model . . . . . . . . . . . . . . . . . 3.3 Motion Model . . . . . . . . . . . . . . . . . . . . . 3.4 Multi-Rate Issues . . . . . . . . . . . . . . . . . . . 3.5 Alternative Landmark Free Odometric Framework 4 sift Performance on radar Images . . . . . . . . . . . . . 4.1 Matching for Movement Estimation . . . . . . . . . 4.2 Loop Closure Matching . . . . . . . . . . . . . . . . 4.3 Feature Preprocessing . . . . . . . . . . . . . . . . 5 Experimental Results . . . . . . . . . . . . . . . . . . . . . 5.1 Results . . . . . . . . . . . . . . . . . . . . . . . . . 5.2 Map Estimate . . . . . . . . . . . . . . . . . . . . . 6 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149 151 154 155 156 157 158 159 160 163 164 164 165 166 168 169 169 172 E Silent Localization of Underwater Sensors using Magnetometers 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 Methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1 System Description . . . . . . . . . . . . . . . . . . . . 2.2 State Estimation . . . . . . . . . . . . . . . . . . . . . . 2.3 Cramer-Rao Lower Bound . . . . . . . . . . . . . . . . 3 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 Magnetometers Only . . . . . . . . . . . . . . . . . . . 3.2 Magnetometers and GNSS . . . . . . . . . . . . . . . . 3.3 Trajectory Evaluation using CRLB . . . . . . . . . . . . 3.4 Sensitivity Analysis, Magnetic Dipole . . . . . . . . . 3.5 Sensitivity Analysis, Sensor Orientation . . . . . . . . 4 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 175 177 179 179 181 183 184 184 185 187 187 188 190 192 Notation Abbreviations Abbreviation crlb crm ekf esdf fim gnss gps hmm imm immu ins kf mems pdr radar rmse sift slam zupt Meaning Cramer-Rao Lower Bound Corrosion Related Magnetism Extended Kalman Filter Exactly Sparse Delayed-state Filter Fisher Information Matrix Global Navigation Satellite Systems Global Positioning System Hidden Markov Model Interacting Multiple Model Inertial Magnetic Measurement Unit Inertial Navigation System Kalman Filter Micro-Machined Electromechanical System Pedestrian Dead Reckoning RAdio Detection And Ranging Root Mean Square Error Scale-Invariant Feature Transform Simultaneous Localization And Mapping Zero Velocity Update xix xx Notation Estimation Notation x(t) ẋ(t) xk y1:k wk , ek Qk , Rk T x̂k|N Pk|N g θ λk δk µk ψk qk dk N (m, Σ) χ2 (k, g) p(x|y) Meaning State at time t Derivative of x(t) at time t State at time step k Set of measurement from time step 1 to k Process and measurement noise at time step k Process and measurement noise covariance at time step k Sampling Time State estimate at time step k given measurements up to and including time step N Covariance of state estimate at time step k given measurements up to and including time step N Gravitation vector (0 0 9.82)T Constant parameter vector Test statistic at time step k Mode at time step k Estimated mode probability at time step k Heading state at time step k Quaternion orientation at time step k Deterministic disturbance at time step k Gaussian probability density function with mean m and covariance Σ Non-central χ2 distribution with k degrees of freedom and non-centrality parameter g Probability density function of x given y Part I Background 1 Introduction Localization requires a map and a way to positioning a user in that map. Traditionally the map has been created first. Localization was then solved by placing aiding landmarks in the area like lighthouses for ships, navigation satellites orbiting the planet, or radio beacons on land. These enabled the position to be computed by using distance and/or angle to multiple such landmarks. The methods are called triangulation or multilateration and the position can be acquired by solving an equation system. This thesis covers the more complex problem of localization in unknown environments. In these scenarios, choosing reliable landmarks in the environment becomes a part of the problem. One must also determine position using a trajectory of earlier positions, where the relationship between different time instances is depending on sensors measuring the system dynamics. The required mathematics is called nonlinear filtering. The positioning can be performed on any type of unit. Estimating the position of a robot exploring a sewer system, of a car in a city, of a ship in an archipelago or tracking a fire fighter searching through a burning building, all is localization. If one is outdoors and a Global Navigation Satellite System (gnss) is available, position estimation becomes straightforward given that the provided measurement accuracy is enough for the application. There are though many environments where gnss signals are not available. Such signals are extremely weak making their penetrating ability highly limited. For example indoors, underground or underwater gnss signals cannot be detected. Even outdoor the gnss signals can be corrupted. This is commonly caused by the signals being reflected or that the line of sight to a satellite is blocked by trees or 3 4 1 Introduction high buildings. Lately, intentional or unintentional jamming of the gnss signals has emerged as a potential major problem. Jamming the system is very easy since the broadcasted signals are so weak. This makes systems that depend entirely on gnss quite vulnerable. Different, redundant means of positioning are therefore required, ones that are tailored for each specific problem. The solutions must be reliable and use all other available information to get the best possible positioning estimate. That is the problem of localization. 1.1 Problem Description Three localization problems have been studied in this thesis. The first is indoor localization for first responders, soldiers and other professional users, the second is surface vessel positioning using a naval radar, and the third is underwater sensor positioning using a friendly vessel. 1.1.1 Indoor Localization The problem of indoor localization for professional users has received a lot of attention in the last couple of years Beauregard (2007); Feliz et al. (2009); Foxlin (2005); Ojeda and Borenstein (2007); Godha et al. (2006); Woodman and Harle (2009); Grzonka et al. (2010); Aggarwal et al. (2011); Jiménez et al. (2010a); Robertson et al. (2009); Widyawan et al. (2008); Abdulrahim et al. (2011); Jiménez et al. (2010b); Bebek et al. (2010); Angermann and Robertson (2012). Be it firefighters, soldiers or police officers, being able to track the position of each individual user in real time while in a building, is the dream of the operational management. In case something urgent happens, knowing where all the personnel are and where they have been, enables swift and accurate cooperation to solve the problem. Having a positioning system would therefore greatly enhance the safety and efficiency of the personnel. Figure 1.1 shows the envisioned scenario and positioning presentation such a system will provide in the future. The firefighters are equipped with small lightweight sensors that do not interfere with their ability to do their job. The inertial/magnetic navigation system in the positioning system is supported by collaborative positioning utilizing the distance measured between users, beacons on the trucks providing distance to a fixed position, gps if available like on the roof and the digital map. The positioning system works in real time, positioning each user with meter level accuracy and broadcasting the information. The information is presented to the operational manager overlaid on an informative 3D map. For a large venue like a school, hospital, shopping mall or a hotel, the positioning presentation can be put into even more context. In the future, one can envision that not only accurate maps of the building are available but also that smart sensors such as special fire detectors have been installed that can broadcast signals that aid the positioning system, while also providing more detailed information 1.1 Problem Description 5 Figure 1.1: Visionary illustration of a first responder operation in an urban environment. Firefighters in the building are localized and presented for the commander in real time. Digital maps, collaborative positioning, gps and beacons on the trucks are used to assist the positioning system. Courtesy of FOI, illustration by Martin Ek. about for example the current location and spread of a fire. Also information about where dangerous materials are stored, which hotel rooms that are occupied, in which areas children are likely to be present and so on, can be included to provide an overview of the complete scenario for the management. The problem studied in this thesis is localization in smaller structures such as residential houses or offices. Larger facilities have the potential of being equipped with designated sensors and maps as described above. For smaller venues such systems are unlikely even in the future, why the positioning system to a large extent has to rely on the sensors brought by the users. The solution should be as simple as possible, using as few sensors as possible and based on as few assumptions about user movements and the environment as possible. The localization system studied in this thesis is therefore a pedestrian dead reckoning system using a foot mounted sensor. A subproblem of the indoor positioning problem has been given the most attention in this thesis: the problem of heading estimation. Knowing in which direction the user is moving is crucial when determining ones position using dead reckoning. Earlier, heading has not been accurately estimated indoors causing 6 1 Introduction it to drift and therefore the position estimates to drift. Solutions to the heading estimation problem are presented and incorporated into a position estimation system to enhance the positioning performance. 1.1.2 Surface Localization Modern maritime navigation is highly gnss centered. It is not only used for positioning but often also as a compass, to track communication satellites and some systems rely on the very accurate measurement of time it produces. Since gnss signals are easily jammed, a backup system is needed when navigating in critical environments. Even though pilots are often present in such scenarios, reducing the impact of gnss failure will further improve the safety of the system. We present a positioning system based entirely on the measurements from the ship’s RAdio Detection And Ranging (radar) where the scans are used to estimate the relative position, the velocity and the heading of the vessel. 1.1.3 Underwater Localization A passive surveillance sensor network can position a surface or submerged vessel using underwater sensors. They sense the magnetic field disturbances and acoustic noises caused by the vessel and can thereby determine its position. One problem is that the exact positions of the sensors are seldom known unless a large amount of time and money have been spent on determining their exact positions. Rapid sensor deployment is therefore difficult since the sensors have to be dropped from a surface vessel and currents can make them move while sinking. Without correct positions of the sensors in the network, accurate tracking of intruding vessels cannot be achieved. In this thesis we have studied the localization problem of determining the positions of the sensors using a friendly vessel with a known magnetic signature. By knowing where the vessel has been and when, the positions of the sensors can be determined. Now when the true sensor positions are known the network can start undertaking its original task: search for naval intruders. 1.2 Contributions The second part of this thesis constitutes a compilation of five publications. Robust Heading Estimation Indoors Paper A, J. Callmer, D. Törnqvist, and F. Gustafsson. Robust heading estimation indoors. IEEE Transactions on Signal Processing, 2013a. Submitted. 1.2 Contributions 7 presents a Kalman filter bank based heading estimation system. Indoors, magnetic heading is not a reliable measurement to use due to frequent and large disturbances. To aid the estimation, measurements of angular velocity from a low grade gyro are incorporated. The Kalman filter bank is used to detect disturbed and undisturbed data segments. To detect filter divergence, a secondary system is used, independent of the filter estimates. The performance of the system is evaluated using more than 500 datasets. The first author has produced the majority of the ideas, theory and writing and all the implementations, but not all of the data collection. Robust Heading Estimation Indoors using Convex Optimization Paper B, J. Callmer, D. Törnqvist, and F. Gustafsson. Robust heading estimation indoors using convex optimization. In International Conference on Information Fusion, 2013b. Submitted. presents a convex optimization based heading estimation system. It states that the gyro signal is correct down to a small gain error and bias. Those two parameters and the initial heading are estimated by tweaking the summed up vector of gyro measurements to match the magnetic heading vector as closely as possible. The matching is done using regularized weighted least squares which can be implemented very cheaply. Also presented is a method to unwrap the magnetic heading vector to enable the matching. The estimation system is shown to work well on more than 500 datasets. The first author has produced more or less all of the ideas, theory, implementation and writing, but again, not all of the data collection. An Inertial Navigation Framework for Indoor Positioning with Robust Heading Paper C, J. Callmer, D. Törnqvist, and F. Gustafsson. An inertial navigation framework for indoor positioning with robust heading. IEEE Transactions on Instrumentation and Measurement, 2013c. Submitted. presents indoor positioning using a foot mounted inertial magnetic measurement unit. The dead reckoning positioning system is based on a stand still detection system that was in part presented in J. Rantakokko, J. Rydell, P. Strömbäck, P. Händel, J. Callmer, D. Törnqvist, F. Gustafsson, M. Jobs, and M. Grudén. Accurate and reliable soldier and first responder indoor positioning: multisensor systems and cooperative localization. Wireless Communications, IEEE, 18(2): 10–18, 2011. J. Callmer, D. Törnqvist, and F. Gustafsson. Probabilistic stand still detection using foot mounted IMU. In Proceedings of the International Conference on Information Fusion (FUSION), 2010b. 8 1 Introduction To solve the common issue of drift in heading that leads to significant error in position, the system incorporates a robust heading estimation system very similar to the one in Paper B. The positioning performance is shown to improve significantly on two challenging experiments. The first author has produced the vast majority of the ideas, theory, implementation, experiments and writing in this paper. RADAR SLAM using Visual Features Paper D, J. Callmer, D. Törnqvist, H. Svensson, P. Carlbom, and F. Gustafsson. Radar SLAM using visual features. EURASIP Journal on Advances in Signal Processing, 2011. presents a radar based backup system for surface vessel positioning that can be used in case the global navigation satellite system, gnss, is out. The system estimates relative change in position and heading and velocity using radar scans of the surroundings. The radar scans are treated like a bird eye’s view of the surroundings and consecutive scans are matched using visual features. By studying how the features move over time, the vessel position can be estimated. The system is evaluated using a 32 km experiment. The first author has produced a majority of the theoretical framework and the writing. Implementation and experiments were produced by Henrik Svensson. Silent Localization of Underwater Sensors using Magnetometers Paper E, J. Callmer, M. Skoglund, and F. Gustafsson. Silent localization of underwater sensors using magnetometers. EURASIP Journal on Advances in Signal Processing, 2010a. presents a positioning system for underwater sensors. An underwater sensor network can be used to track surface or submerged vessels using for example magnetic disturbances and acoustics. For the system to work well, the position of each sensor has to be known which can be hard to determine. This work presents a way to passively position the sensors using a friendly vessel that travel through the area. If the friendly vessel has a known magnetic signature and a known trajectory, the sensor positions can be estimated. The system is evaluated using simulations. This was joint work between primarily the first and second author who produced the ideas, theory, implementation and most of the writing. 1.2.1 Additional Publications Other publications where the author has contributed that are not covered in this thesis are shortly presented below. 1.2 9 Contributions (a) Skewed image due to rolling shutter and camera turning sideways to the right. (b) Rectified image using sensors of the phone. Figure 1.2: Rectification system for skewed images using the unit sensors. Gyro measurements are used to calculate the distortion of the image which can then be corrected. Smartphone Stabilization In G. Hanning, N. Forslöw, P.-E. Forssén, E. Ringaby, D. Törnqvist, and J. Callmer. Stabilizing cell phone video using inertial measurement sensors. In In Proceedings of the IEEE International Workshop on Mobile Vision (IWMV11), 2011. the video stream of an iPhone was stabilized using the inertial sensors. Tracking the orientation of the phone using the gyros and the gravity component, two major errors could be corrected. The first problem is that the image becomes skewed if the phone is moving while the image is taken since the entire image is not recorded at the same time, a so called rolling shutter camera, Figure 1.2. The second problem is that the image becomes unstable if the one holding the camera is moving while filming. Both these problems were solved and the outcome of this master thesis project was the iPhone app DollyCam. The author served as supervisor to Nicklas Forslöw during the master thesis project that was the foundation of the paper. It was awarded best paper at the workshop. Vehicle Tracking using Magnetometers The two papers N. Wahlström, J. Callmer, and F. Gustafsson. Single target tracking using vector magnetometers. In Proceedings of the International Conference on Acoustics, Speech and Signal Processing (ICASSP), 2011. N. Wahlström, J. Callmer, and F. Gustafsson. Magnetometers for tracking metallic targets. In Proceedings of the International Conference on Information Fusion (FUSION), 2010. 10 1 Introduction are about vehicle tracking using a three axis magnetometer. Passing vehicles disturb the earth magnetic field, making it possible to estimate the position and direction of each passing vehicle. Multiple filters were initiated from all possible directions once a vehicle was detected. The probability of each trajectory was estimated and only one estimate survived. The results from an experiment is shown in Figure 1.3. The publications are based on a master thesis project undertaken by Niklas Wahlström that was supervised by the author. Geo-referencing for UAV Positioning As backup for gnss, F. Lindsten, J. Callmer, H. Ohlsson, D. Törnqvist, T. B. Schön, and F. Gustafsson. Geo-referencing for UAV navigation using environmental classification. In Proceedings of 2010 International Conference on Robotics and Automation (ICRA), 2010. covered the problem of using preexisting maps and environmental classification to create a measurement of the global position of an Unmanned Aerial Vehicle (uav). Photos from a downwards facing camera on the uav were classified into grass, houses, roads etc, which could be matched to a map of the area, Figure 1.4. Using a rotation invariant probabilistic class matching system, dubbed ’donuts’, a likelihood for the position of the vehicle could be provided. Merged with a inertial/visual odometry based positioning system, the overall drift in the position estimate could be significantly reduced. The main contribution of the author was within the image classification. 1.2 11 Contributions (a) The vehicle is coming from the rear turning right. 5 Sensor 2 Left EKF Right EKF 0 Sensor 1 −5 −10 −15 Rear EKF −20 −25 −20 −15 −10 −5 0 5 10 15 20 25 (b) The trajectories according to three ekfs with different vehicle position initializations. Normalized filter bank weights Propability [] 1 Left is dropped 0 Left Right Rear Right is dropped 0.5 0 1 2 3 4 5 6 7 Time [s] (c) The probabilities that the vehicle is coming from the left, the right and the rear. Only the hypothesis that the vehicle is coming from the rear survives and the other two are dropped. Figure 1.3: Tracking experiment result with three differently initialized extended Kalman filters estimating the trajectory of the vehicle in Figure 1.3a. 12 1 Introduction (a) Image from camera onboard a uav. (b) Extracted superpixels. (c) Superpixels classified as grass, asphalt or house. (d) Three circular regions used for computing class histograms. 1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 (e) Calculated likelihood for all available locations. Figure 1.4: From raw image to position likelihood. Image is classified and class histograms are used to calculate position likelihood. 1.2 Contributions 13 Loop Closure Detection for SLAM The last two publications are spinoffs from the master thesis project undertaken by Karl Granström and the author at Australian Centre for Field Robotics, Sydney University in 2007/08. K. Granström, J. Callmer, F. Ramos, and J. Nieto. Learning to detect loop closure from range data. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2009. J. Callmer, K. Granström, J. Nieto, and F. Ramos. Tree of words for visual loop closure detection in urban SLAM. In Proceedings of the 2008 Australasian Conference on Robotics and Automation (ACRA), 2008. Both papers are about loop closure detection methods for large scale urban simultaneous localization and mapping (slam). Granström et al. (2009) we presented a laser scan based matching method that matched 360◦ 2D range slices of the surroundings using rotation invariant features. Adaboost were used to produce a reliable scan matcher. Callmer et al. (2008) detected loop closures using photos of the surroundings. Visual features were extracted from the images and approximated as one of a large number of predefined words. Two images were matched by comparing the list of words they contained. Both methods were incorporated into a slam estimation system to produce maps of the area, Figure 1.5. Figure 1.5: Laser map of based on the results of a slam experiment, overlaid on an aerial photograph. 14 1 1.2.2 Introduction SenionLab During the fall of 2010, the spinoff company SenionLab was cofounded by the author. The company provides indoor navigation solutions for primarily cell phones, Figure 1.6. The positioning system is intended for facilities like shopping malls, airports, hospitals, stadiums etc. At the time of writing, the largest commercial deployment is 50 malls in Singapore. Figure 1.6: Indoor positioning using smartphone. Courtesy of SenionLab AB. A patent application on the technology has been filed. C. Lundquist, P. Skoglar, F. Gustafsson, D. Törnqvist, and J. Callmer. Method and device for indoor positioning. US Patent Application 20120203453, August 8, 2012. The system uses all available sensors in the device to position the user. Radio environment mapping and sensing enables accurate positioning that is fused with a dead reckoning system for a smooth user experience. Several challenges have been met that greatly illustrate the difference between commercial product research and development on one side and academic research on the other. A major challenge is for example how to handle many different types of sensors with different sampling times to make the system work as well as possible on all types of known and even unknown units. Even more challenging is the difference in what different sensors measure. The received signal strength indicator of WiFi measurements differ for example greatly between different phones. Besides sensor variabilities, the application scenario is a tremendous source of 1.3 Thesis Outline 15 problems that need to be solved. How different users handle the unit and how they move is one such. Even greater are practical matters like errors in floor plans, reconstructions, the frequent magnetic disturbances, WiFi access points that move and WiFi access points that change broadcasted signal strength depending on the current workload. Also, to produce maps of the radio environment, one must have measurements of it, but one must also know exactly where each measurement was taken. And in order to make deployment cheap and fast, a logging tool must be created that is fault tolerant and intuitive enough to be handled by just about anybody. The logging tool should preferably also signal to the one doing the logging that what he or she is doing is important and must be done meticulously. These are challenging and important issues one does not normally experience when doing academic research. But the practical issues can also lead to new research ideas. In fact, the experience of magnetic disturbances in practice has been an inspirational source for Papers A and B. In the end, commercial deployment of a solution to a research problem introduces whole new problems that are at least as hard as the original problem. Constructing a system that should work a billion times on a million different units is very different from academic research. 1.3 Thesis Outline This thesis is divided into two parts. The first part provides background theory and gives context to the second part which is constituted of the five edited publications. Chapter 2 is a brief introduction to the basic sensor fusion tools of sensors, modeling and estimation theory that is the fundament of all publications. It ends with a discussion about a very central problem in this thesis: estimation using disturbed measurements. Chapter 3 describes the problem of indoor positioning. Indoor navigation using dead reckoning for first responders is discussed and one of the systems later used in Paper C is described in more detail. Chapter 4 summarizes the first part of the thesis with conclusions and a discussion about future work. 2 Sensor Fusion Sensor fusion is the problem of estimating some properties xt of a unit, using sensors that provide measurements yt that depend on xt . In order to do this, models of how yt is related to xt and of how xt changes over time, are used. The former are called measurement models and the latter are called process models. The properties xt are called states and can represent any sought system property. The states can for example be related to the sensor platform representing the position or orientation of the unit, they can be some unknown constant properties such as the unit weight or they can be of the surrounding environment such as the positions of environmental landmarks. In the problem of localization the states are commonly position, velocity and orientation of the unit which are key features representing ’where is the unit?’ and ’where is the unit going?’. The states xt are estimated using a filter that fuses the information from all the sensors and the models. Besides from the state estimates x̂t , the filter also provides an estimate of the uncertainties of x̂t . The joint estimate x̂t is in some sense better than what one could get using the sensors individually. The meaning of better is application dependent and could for example mean more accurate estimates, more robust estimates or that the same estimation precision can be achieved using fewer sensors. A schematic overview of a sensor fusion framework is given in Figure 2.1. The sensor measurements enter the estimation system that relates these to the system states using the measurement models. The states are updated using the process models describing the system, which are then fused with the measurements. The system output is the state estimates which can be used by other applications. Three components are needed in sensor fusion: sensors producing measurements 17 18 2 Sensor Fusion Sensor Fusion Sensors State Estimation State Estimates System Models Process Model Measurement Model Figure 2.1: Overview of a sensor fusion framework. Sensor data is fused with dynamic system models using measurement models to produce state estimates. yt that are related to the system states xt , models that describe the dynamic properties of the system and the measurements, and a state estimation system that produces the state estimates. This chapter will describe all three parts and provide the background theory on the subject needed for the publications part of the thesis. 2.1 Sensors The sensors produce measurements that relate to the sought system properties in some way. Which sensors that are used is highly application dependent. In localization problems, the sensors are usually of one of two kinds. The first option is that they produce measurement that are related to the unit directly such as its movement, position, direction etc. The second option is that they are indirectly related to the unit, measuring for example some aspect of the surrounding environment that will change when the unit properties change. They can be cameras filming the surroundings, range finders measuring the distances to objects around the unit, or magnetometers measuring the surrounding magnetic field, among others. The sensor primarily used in this thesis is an inertial magnetic measurement unit (immu). It is actually not just a sensor but a sensor unit containing nine separate sensors: three accelerometers, three gyros and three magnetometers. It has been used in Papers A, B and C. In Paper D, a radar sensor was used for maritime localization experiments. Paper E contains only simulations why the sensor data came from a simulated three axis magnetometer and a pressure sensor. The immu and the radar sensor are described in this section and also a short description of gnss. 2.1 Sensors 19 Figure 2.2: An Xsens MT motion sensor, courtesy of Xsens Technologies B.V. 2.1.1 Inertial Magnetic Measurement Unit An inertial magnetic measurement unit contains an accelerometer, a gyroscope and a magnetometer, all three dimensional. The accelerometer measures acceleration, the gyro measures the angular velocity and the magnetometer measures the magnetic field. Besides from these, the immu often contains a thermometer to enable correcting temperature related sensor errors. There are many different kinds of immus with different price, size and precision but in this section we will focus on only one type: micro-machined electromechanical systems (mems). mems sensors are small, rugged, low cost, lightweight and low on power consumption, making them popular to include in all sorts of devices. On the downside, their performance is in many aspects quite poor even though it is constantly improving. A mems immu can be made to work better through calibration, but this is labor intensive which is expensive. A mems immu is also a strap-down system, meaning that the sensors are mounted on the device making the measurements in the body frame. The immu used in the standstill detection in Chapter 3 is an Xsens MT motion sensor, Figure 2.2. The immu used in Paper C was an MicroStrain 3DM-GX3-25. The signals were sampled in 100 Hz using a 16 bit A/D converter. Accelerometer The accelerometer actually measures the specific force which is a type of acceleration. Specific force is defined as non-gravitational force per unit mass, meaning it is the acceleration relative to free-fall. A free-falling accelerometer therefore experiences no specific force while a sensor at rest senses the normal force from the surface that cancels the gravity. An accelerometer at rest therefore measures the gravitational constant g but pointing upwards, not downwards. There are mainly two types of accelerometers: mechanical sensors and solid state sensors. The mechanical sensor measures how a suspended mass is displaced due to an applied force. Using Newtons second law F = ma, the acceleration can be 20 2 Sensor Fusion measured. A solid state sensor is for example the surface acoustic wave (saw) accelerometer. It uses a mass attached to a beam that is vibrating at a particular frequency. When a force is applied, the beam bends, changing the frequency. Thereby the force can be measured. A mems accelerometer can be based on any of these techniques. The main errors associated with mems accelerometers, besides the ever present additive white noise, are • Bias - the sensor value has a slight offset • Temperature - a change in temperature gives a change in measured output • Calibration errors - such as alignment errors, gain errors and so on Bias errors can sometimes be estimated depending on the application and other sensors available. Temperature errors are commonly handled by the sensor unit. But since the temperature errors are often highly nonlinear the effect is often not completely removed. Calibration errors are very hard to estimate if present, especially alignment errors. Gyro The gyro measures angular velocity, i.e. rate of turn. The gyro is often the weakest point in an inertial navigation system, ins. Accurately estimating the orientation of the device is crucial in such a system and the orientation is tracked using the gyro. A high grade gyro is often based on optics such as ring laser gyros or fibre optic gyros. The sensor is based on light inference. Two light beams are shone into opposite ways of a track. The track can be an optical fibre or a mirror path, for example. When the beams return, the inference, i.e. phase shift, reveals if one of the beams has travelled a shorter path due to that the path has rotated. This is called the Sagnac effect. Optical gyros often have high precision, but are hard to reduce too much in size since a shorter path means worse precision. A mems gyro is based on that a moving element that is effected by a rotation gives away a force in the perpendicular direction F c = −2m(ω × v). (2.1) This is known as the Coriolis effect. The velocity is often represented using a vibrating mass to create F c . Since v is a vibration, F c changes direction with the direction change in v, why it is also vibrating. Today the mems gyros cannot match the precision of the optical gyros. As for the mems accelerometers, the main sources of error are • Bias • Bias stability - the bias actually moves around slightly and is not as constant as a bias should be 2.1 21 Sensors • Temperature effects • Calibration errors. and the additive white noise. Since the rotations are often integrated over time to produce an orientation estimate, the fact that the bias does not even have the decency to be still is integrated into a significant orientation drift over time. Magnetometer Magnetometers measure the magnetic fields. The magnetic fields consists of the earth magnetic field and local magnetic disturbances. If the earth magnetic field is stronger than the disturbances, information of the direction of magnetic north is available. There are many different approaches to magnetic sensing such as Hall effect sensor, magneto-diode, magneto-transistor etc. mems magnetometers are often based on the Lorentz-force which acts on a current-carrying conductor in a magnetic field. F = q[E + (v × B)] (2.2) where q is the charge, E is the electric field, B is the magnetic field and v is the velocity of the charge. This force can be measured in different ways, for example by sensing the strain this force applies on piezo-resistors or by sensing a frequency shift in a beam caused by the force. One can also detect the force by studying the displacement of the mems structure. 2.1.2 RADAR A pulse radar sends out radio waves in different directions which are reflected or scattered when hitting an object. The reflected signals are picked up by a receiver, usually at the same place as the transmitter, and the time of flight for the signal is calculated. This time is proportional to the distance to the object that reflected the signal and the heading of the sensor when the signal was transmitted gives the direction to the object. The strength of the reflected signal can also provide some information about the properties of the reflecting object. Two measurements are provided by each reflected wave: range and angle from the sensor to the object. q (2.3) r = (sx − px )2 + (sy − py )2 α = arctan sy − py sx − px (2.4) where s = (sx , sy ) is the position of the reflecting structure and p = (px , py ) is the position of the radar sensor. Since the uncertainties in angle and range are independent, the total measurement uncertainties will be banana shaped. 22 2 Sensor Fusion A naval radar commonly rotates with a constant speed, transmitting and receiving in one direction at a time. The reflections are plotted in the current direction when they are received. This gives a circular image of the surrounding islands and vessels that is updated one degree at a time. By saving one 360◦ radar sweep as an image, a view of the surroundings is provided. The radar sensor used in Paper D was a military one making the characteristics of that sensor secret. What we do know is that it had a range of roughly 5 km and a range resolution of about 5 meters. It rotates one revolution in 1.5 seconds giving measurements in roughly 2000 directions. One way of using a radar in localization is to take some strong reflections in a full 360◦ radar scan and try to detect them again in the next scan. The objects creating these reflections are called landmarks and are assumed stationary. By measuring the distance and heading to the landmarks and see how these change over time, how the radar equipped unit is moving can be estimated. If some landmarks move in a manner that is inconsistent with the other landmarks, it is probably a different unit and the reflections should not be used for localization. 2.1.3 Global Navigation Satellite System Global Navigation Satellite System use multiple satellites and triangulation to determine the position of a user anywhere on earth. The most well known such system, the Global Positioning System (gps), provides a positioning accuracy of about 10 meters. Besides from location, gps also gives very accurate estimates of current time, making it useful also in applications where only accurate time and not position is needed. This is for example used in cellphone base station synchronization for some systems. The system consists of 30 satellites and free line of sight to at least 4 of them is required for the positioning to work. Other systems exist or are planned. The Russian glonass system mostly covers the northern hemisphere, in particular Russia, and is today short of the 24 satellites needed to cover the whole planet. The European Galileo system will use 30 satellites to cover the entire planet and the full deployment is expected to be finished in 2019. Also a Chinese system, compass, using 35 satellites to cover the planet will be deployed in the future. As of today, a smaller system covering only China and the immediate surroundings is in place. A future gnss receiver, using signals from all systems will pretty much always have free line of sight to at least 4 satellites. This will give accurate positioning also in places that are difficult to cover today such as urban canyons. One shortcoming with gnss systems is the weakness of the signals. The signal is weaker than the background noise and only because the receivers know what to look for can the signals be found. This makes the system sensitive to signal disturbances due to intentional or unintentional jamming. Today, gps jammers that can easily knock out all gps reception in an area of many square kilometers are available at a low cost; Economist (2011); Grant et al. (2009). This problem and a suggested solution for maritime vessels is discussed in more detail in Paper D. 2.2 2.2 23 Models Models In estimation, mathematical models are used to describe how the states are related to eachother and to the measurements. A process model describe the dynamic properties of the system by stating how the states depend on one-another and on additional inputs. For a vehicle model, the process model describes how the velocity states translate into a change in position states over time for example. It will also put restrictions on a system by stating that a vehicle cannot not travel sideways for example. Dynamic system models are often relating the states to eachother using differential equations. To simplify implementation, these models are most often approximated as discrete time difference models. A measurement model relates what is measured by the sensors to the unit states. The measurements can be of the states themselves or they can be functions of one or more states. The mathematical models used to describe the relationship between the measurements and the states are often nonlinear functions. 2.2.1 Continuous Models The models are commonly on a state space form where a state vector x(t) describes the system properties at time t. The process model is f ( · ) and the measurement model is h( · ). The fundamental continuous time model is ẋ(t) = f (x(t), u(t), w(t)) y(t) = h(x(t), u(t), e(t)) (2.5) (2.6) where u(t) is a known input signal and w and e are model and measurement noise terms, respectively. f ( · ) and h( · ) are in general nonlinear functions. Even though the process model (2.5) is often based on fundamental relationships between states described by differential equations, some simplifications have always been made of the true system. The dynamic model is therefore associated with a process noise, which is the assumed input that is driving the true system. The process noise should also incorporate the model uncertainties. Related to each measurement in the measurement model (2.6), is a measurement noise e(t). No matter the sensor, there is always a noise present in the measurements. The noise term e(t) therefore reflects the quality of the sensor, with larger noise covariance terms for poor sensors. For a presentation on random signals in continuous time, see e.g. Jazwinski (1970). 2.2.2 Discrete Time Models Estimation methods are primarily based on discrete time systems due to the implementational simplifications that follow. Therefore, continuous time models need to be discretized before they can be used in an estimation systems. 24 2 Sensor Fusion Discretization Discretization means that the differential equations in a continuous time model are replaces by approximate difference models that resemble the original ones. Most often discretization is a complex task. One exception is a linear continuous time system. ẋ(t) = Fx(t) + Gw(t) y(t) = Cx(t) + e(t) (2.7) To discretize such a system, one must first assume that wk is piecewise constant over the sampling interval T . The matrices of the sampled systems can then be computed as A = e FT ZT B= e Fτ dτ G (2.8) (2.9) 0 giving the discrete time linear system xk+1 = Axk + Bwk yk = Cxk + Dek . (2.10) where wk ∼ N (0, Qk ) and ek ∼ N (0, Rk ) Note that the model matrices A, B, C and D need not be constant. For most other cases sampling a continuous time system is quite challenging. For details see Gustafsson (2010). General Discrete Time model A general description of a physical system as a state space model in discrete time is xk+1 = f (xk , uk , wk ) yk = h(xk , ek ) (2.11) An important special case is when the process and measurement noises are modeled as additive xk+1 = f (xk , uk ) + wk yk = h(xk ) + ek . (2.12) It is an intuitively straightforward model with a deterministic part utilizing basic physical properties and a random part representing everything that is unknown that affects the system. An example of a system with partially nonlinear dynamics and measurements is given in Example 2.1. 2.2 25 Models Often the input uk is unknown making the model xk+1 = f (xk ) + wk yk = h(xk ) + ek (2.13) This is the case when one wants to estimate the properties of a system one does not control, like in target tracking. The linear system (2.10) is a very common special case of modeling since it allows Kalman filter theory to be applied when solving the problem, if the process and measurement noises are assumed Gaussian. 2.1 Example Model of an inertial navigation system estimating the sensor position using an accelerometer and a gyro and also global measurements of position from gps. The states position p, velocity v and acceleration a, all in global coordinates. Orientation is represented by quaternions q and angular velocity ω in the local coordinate system. The measurements acceleration ya and angular velocity yω , both measured in the local coordinate system and global position yp from the gps receiver. All measurements have additive noise Gaussian zero mean noise e. To reduce the model complexity, the acceleration state a can be replaced by the acceleration measurements ya in the dynamic model. The corresponding simplification can be done to the orientation, replacing ω by yω in the dynamic model. The main difference this induces is that high frequency components in ya and yω are not filtered out, Callmer (2011). Quaternion dynamics and properties are described in Appendix A but a very brief explanation will be given here. S 0 (qk )ωk describes how local angular velocities translate into changes in quaternions. R(qk ) is the rotation matrix from the global to the local coordinate system which is based on the quaternions qk . 2 ! 0 T pk+1 I T I 0 pk T2 I R (qk )ya,k − g + ea,k (2.14) vk+1 = 0 I 0 vk + T I 0 yω,k + eω,k T 0 qk+1 0 0 I qk 0 S (q ) k 2 yp,k = pk + ep,k (2.15) where g is the gravity component. 2.2.3 Dynamic Model Restructuring In some cases we have a dynamic model like (2.12) where the input uk is known. We then have xk+1 = f (xk , uk ) + wk xk = f (xk−1 , uk−1 ) + wk−1 (2.16) (2.17) xk+1 = f (f (xk−1 , uk−1 ) + wk−1 , uk ) + wk . (2.18) which can be rewritten as 26 2 Sensor Fusion This procedure can be repeated over and over until xk+1 is only depending on the initial states x0 , the known inputs u0:k and the process noise terms w0:k . In the general case the full expression becomes hideous and unmanageable, but in the special case f (xk , uk ) = xk + uk (2.19) (2.18) becomes xk+1 = x0 + k X ui + k X i=0 (2.20) wi . i=0 That means that in those cases, xk+1 can be written as a simple function of only initial states, inputs and noise terms. The resulting measurement model is yk = h(xk ) + νk = h(x0 + k−1 X ui + i=0 k−1 X wi ) + νk . (2.21) i=0 In this thesis, this form has been applied to the one dimensional heading estimation problem. The heading state ψk is driven using the gyro measurements ykω which have gaussian measurement noise ek . ψk+1 = ψk + T ykω + T ek (2.22) can therefore be written as ψk+1 = ψ0 + T k X yiω + T i=0 k X ei . (2.23) i=0 The state vector ψ0:k+1 can hence be written on batch form very easily. Such an approach also means that we assume that the gyro sensor has the actual bandwidth to capture all the dynamics of the system. This model structure will be used in Papers B and C where heading is estimated in batch form. 2.3 Estimation Theory The estimation problem is the problem of estimating the posterior distribution of the states given the measurements, p(xk |y1:k ). The states are often intricately related to the measurements, making them difficult to estimate. With the use of Bayes’ theorem p(x|y) = p(y|x)p(x) p(y) (2.24) 2.3 Estimation Theory 27 the problem can be reformulated into three straightforward parts. p(y|x) is the likelihood of receiving the measurement y given the states x, p(x) is the prior probability of the states incorporating all our previous knowledge about the states and p(y), the probability of the measurement, normalizes the state probabilities. Estimation theory can be divided into two cases: linear and nonlinear estimation. Linear estimation is straightforward and the results are trustworthy but the problem is in reality rare. Nonlinear estimation is more difficult and the solutions are prone to diverge but unfortunately the problem is very common. In this section we will first present the Kalman filter used for linear estimation problems and the extended Kalman filter used for slightly nonlinear problems. We will then present Kalman filter banks which is a type of filters where multiple Kalman filters with different system assumptions, so called modes, are run in parallel. The final estimate is a mixture between all the filter estimates. This is followed by a presentation of Hidden Markov Models which can be used when only mode is sought. The section ends with a discussion about estimation using disturbed measurements and a suggested solution outline. 2.3.1 Kalman Filter The linear estimation problem where a discrete time linear system (2.10) is assumed to have Gaussian process and measurement noise, is optimally solved using the Kalman filter, Kalman (1960). Since the problem is Gaussian, estimating the mean and the covariance of xk provides the entire solution to p(xk |y1:k ). The Kalman filter works in a two step procedure with a time update and a measurement update. The time update predicts the future states x̂k+1|k using the process model. Since the model is not perfect, the process noise covariance Qk is added to the state covariance Pk|k to illustrate the increase in estimate uncertainty that the model introduces. The measurement update uses the difference between the measurement and the predicted measurement, the so called innovation, to update the states. How much the new measurement should affect the states is decided by the Kalman gain, Kk . Kk depends on Qk and the measurement noise covariance Rk which describes how trustworthy the measurements are. The relation between these two parameters determines the filter performance. If Qk is small in relation to Rk , the model is deemed more reliable than the measurements and vice versa. The ratio between Qk and Rk affects the state estimates while the magnitudes of Qk and Rk determines the size of the state estimate covariances. The equations defining the Kalman filter are shown in Algorithm 1. 2.3.2 Extended Kalman Filter If the problem is of the form (2.11) or (2.12) and only mildly nonlinear, the extended Kalman filter (ekf) can be applied. It approximates the nonlinearities using a first order Taylor approximation around the latest state estimate and then 28 2 Sensor Fusion Algorithm 1 Kalman Filter Require: Signal model (2.10), initial state estimate x̂0|0 and covariance P0|0 . 1: Time Update x̂k+1|k = Ax̂k|k Pk+1|k = APk|k AT + Q 2: (2.25) Measurement Update −1 Kk+1 = Pk+1|k C T CPk+1|k C T + R x̂k+1|k+1 = x̂k+1|k + Kk+1 yk+1 − C x̂k+1|k Pk+1|k+1 = Pk+1|k − Kk+1 CPk+1|k (2.26) applies the Kalman filter to the linearized problem. Convergence cannot be guaranteed, in particular because the ekf gives the optimal solution to the wrong problem. That means, the solution to the linearized problem is the optimal one, unfortunately the linearized problem is not the true problem. Despite this, the ekf most often works well in practice. The ekf is primarily used in Papers C and E. Starting with the nonlinear function (2.12), a first order Taylor expansion of the measurement function h( · ) around the linearization point x̂k is h(xk ) ≈ h(x̂k ) + h0x (x̂k )(xk − x̂k ) (2.27) where h0x (x̂k ) is the Jacobian h0x (x̂k ) ∂h(xk ) = ∂xk xk =x̂k (2.28) The measurement model can now be approximated according to yk = h(x̂k ) + h0x (x̂k )(xk − x̂k ) + ek yk − h(x̂k ) + h0x (x̂k )x̂k = h0x (x̂k )xk + ek ȳk = h0x (x̂k )xk + ek . (2.29) Correspondingly the dynamic model can be expanded around x̂k as f (xk ) ≈ f (x̂k ) + fx0 (x̂k )(xk − x̂k ) (2.30) xk+1 = f (x̂k ) + fx0 (x̂k )(xk − x̂k ) + wk xk+1 − f (x̂k ) + fx0 (x̂k )x̂k = fx0 (x̂k )xk + wk x̄k+1 = fx0 (x̂k )xk + wk (2.31) giving the new model 2.3 29 Estimation Theory where fx0 (x̂k ) = ∂f (xk ) ∂xk xk =x̂k (2.32) If the signal model is (2.11), the Jacobians with respect to the noise parameters w and e are also needed. ∂f (xk , wk ) ∂h(xk , ek ) 0 0 he (x̂k ) = (2.33) fw (x̂k ) = ∂wk ∂ek xk =x̂k xk =x̂k The extended Kalman filter is summarized in Algorithm 2. Algorithm 2 Extended Kalman Filter Require: Signal model (2.11), initial state estimate x̂0|0 and covariance P0|0 . 1: Time Update x̂k+1|k = f (x̂k|k ) Pk+1|k = fx0 (x̂k|k )Pk|k fx0 (x̂k|k )T + fw0 (x̂k|k )Qfw0 (x̂k|k )T 2: (2.34) Measurement Update Sk+1 = h0x (x̂k+1|k )Pk+1|k h0x (x̂k+1|k )T + h0e (x̂k+1|k )Rh0e (x̂k+1|k )T −1 Kk+1 = Pk+1|k h0x (x̂k+1|k )T Sk+1 x̂k+1|k+1 = x̂k+1|k + Kk+1 yk+1 − h(x̂k+1|k ) −1 0 Pk+1|k+1 = Pk+1|k − Pk+1|k h0x (x̂k+1|k )T Sk+1 hx (x̂k+1|k )Pk+1|k 2.3.3 (2.35) Optimization Formulation A system with state vector xk with dynamic model xk+1 = Axk + Buk + wk (2.36) yk = Cxk + ek (2.37) and measurement model can, as was described in Section 2.3.1, be estimated using a Kalman filter. The Kalman filter measurement update can be written as a one step optimization problem. x̂k = arg min (yk − Cxk )T R−1 k (yk − Cxk )+ xk −1 (xk − Axk−1 − Buk )T Pk|k−1 (xk − Axk−1 − Buk ) (2.38) where xk−1 is the previous estimate that is fixed and Pk|k−1 is the time updated state covariance. This is an unconstrained regularized weighted least squares problem. The closed form solution is the Kalman filter. To instead produce a smoothing estimate x̂0:k , (2.38) can be written on batch form. 30 2 Sensor Fusion Thereby, the entire vector x̂0:k is produced in one step using all measurements y0:k . The batch formulation includes both time and measurement update and also a prior on the initial state x0 . x̂0:k = arg min (y0:k − Cx0:k )T R−1 0:k (y0:k − Cx0:k )+ x0:k −1 (Āk x0:k − Bu1:k )T Q1:k (Āk x0:k − Bu1:k ) + x0T P0−1 x0 (2.39) where Āk is I −A 0 0 I −A Āk = . .. .. . 0 0 ... ... ... .. . 0 0 I −A (2.40) In most practical applications, estimating x̂0:k using (2.39) is unnecessary. If one wants to include future measurements in an estimate, a fixed-lag smoothing is computationally more efficient and the estimate is very similar to the one produced using (2.39) due to the exponential forgetting factor of old measurements. More on smoothing in Gustafsson (2010). However, in some application this formulation can be very useful. In this thesis we have one such scenario, in Papers B and C, where heading is estimated on batch form to handle large and frequent disturbances in the measurements. 2.3.4 Kalman Filter Banks In some cases one has a model that depend on a discrete mode δk . It can represent many different features such as type of dynamic model (turning or straight) in target tracking, a faulty or a non-faulty system, missing data or disturbed or undisturbed measurements. In the general case, δ can take on S different values. A common scenario is the binary case S = 2: disturbed/undisturbed, faulty/nonfaulty etc. To handle such systems, a Kalman filter bank can be used. It runs multiple filters in parallel that each have its own assumption of which mode δk the system is currently in. A Kalman filter bank is a group of Kalman filters where each filter at each time instant has a parameter setup that is depending on a discrete mode, δk . Consider the following jump Markov linear system xk+1 = A(δk ) + B(δk )uk + E(δk )wk yk = C(δk )xk + D(δk )uk + ek (2.41) where wk is process noise distributed as wk ∼ N (0, Qk (δk )) and ek is measurement noise distributed as ek ∼ N (0, Rk (δk )). The matrices A( · ), B( · ), C( · ), D( · ) and E( · ) are known. For a dynamic system, each time instant has a mode associated to it. That means 2.3 31 Estimation Theory that for an experiment we will have an unknown mode sequence δ1:k that is sought. i The estimated state sequence x0:k will be based on the assumed mode sequence i δ̂1:k . Each mode sequence has its individual combination hypothesis of what δ1:k has actually been. Since the models are mode dependent, (2.41), different state i sequences x0:k will be acquired for each mode sequence. The mode sequence δ1:k to choose is the one that minimizes the prediction errors of the Kalman filter. To explore the entire solution space, one should evaluate all possible mode combinations δ1:k . In the general case the number of combinations are S k . This becomes unfeasible very soon since the computational complexity grows exponentially with time. Hence, approximations have to be made. General Full Solution The posterior distribution of the state can be expressed as a Gaussian mixture of all possible mode sequence realizations p(xk |y1:k ) = Sk X i i p(δ1:k |y1:k )p(xk |δ1:k , y1:k ) (2.42) i i i , Pk|k ). µ1:k N (xk ; x̂k|k (2.43) i=1 = Sk X i=1 i µ1:k i = p(δ1:k |y1:k ) is subsequently a weight based on the probability of a certain i mode sequence δ1:k given the measurements y1:k . For a specific mode sequence i, i i N (xk ; x̂k|k , Pk|k ) is straightforwardly calculated using a Kalman filter. The minimum variance state estimate is therefore given as x̂k|k = Sk X i µ1:k x̂ki . (2.44) i=1 i are calculated for a certain sequence by recursively applying The weights w1:k Bayes’ theorem i i = p(δ1:k |y1:k ) = µ1:k i p(δ1:k ) i ) p(y1:k |δ1:k p(y1:k ) (2.45) = i i )p(δ1:k−1 ) p(δki |δ1:k−1 i i p(yk |δ1:k , y1:k−1 )p(y1:k−1 |δ1:k ) p(yk |y1:k−1 )p(y1:k−1 ) (2.46) = i i ) , y1:k−1 ) p(δ1:k−1 p(yk |δ1:k i i p(y1:k−1 |δ1:k−1 )p(δki |δ1:k−1 ) p(y1:k−1 ) p(yk |y1:k−1 ) (2.47) i p(yk |δ1:k , y1:k−1 ) . ) k−1 p(yk |y1:k−1 ) i Π(δi ,δi = µ1:k−1 k (2.48) 32 2 Sensor Fusion Π(δk ,δk−1 ) is the mode switching probability that states what the probability is to switch from one mode to another. Π(δk ,δk−1 ) is assumed constant. The mode state is modeled as a time invariant Markov chain why the probability of mode switch is only dependent on the previous mode and not on the full mode history. Implementation Approximations A way of visualizing how the number of mode sequence combinations grows with i time is to see it as a tree. For each new measurement, each combination δ1:k is spawned off into S more branches to provide all possible mode combinations. Since the number of Gaussians in the mixture (2.42) grows exponentially as S k , certain approximations must be applied to implement it. The two main approaches are pruning and merging. Pruning means that mode sequences with a low probability are discarded. In the tree visualization, pruning means that low probably branches are simply cut i off. If p(δ1:k |y1:k ) is very small, continually branching it is a waste of computational resources. For details on pruning and how to implement it, see Gustafsson (2010). In merging, the branches are not cut off, they are joined with other branches, keeping the total number of branches constant. One can for example maintain i , and merge these the full mode sequence over a given time window L, δk−L:k branches when another measurement is available. A mixture of N Gaussians can be approximated as p(xk ) = N X i µ1:k N (xk ; x̂ki , Pki ) i=1 ≈ N (xk ; x̂k , Pk ) (2.49) where x̂k = Pk = N X i=1 N X µik x̂ki (2.50) µik Pki + (x̂ki − x̂k )(x̂ki − x̂k )T (2.51) i=1 This approximation preserves the first and second moments. 2.3.5 Interacting Multiple Model One of the most popular merging methods is the Interacting Multiple Model (imm) filter Bar-Shalom et al. (2001). For a system with S modes, it makes the approximation p(xk |y1:k ) ≈ S X i=1 i i µik N (xk ; x̂k|k , Pk|k ). (2.52) 2.3 33 Estimation Theory Note that the posterior mode probabilities are µik = p(δki |y1:k ), why the entire mode sequence history has been discarded. In the imm filter, only the probability of the current mode is maintained. The imm filter is applied in Paper A. Filter Equations In the imm filter, the states xki with covariance Pki are estimated, one pair for each mode, and the mode probabilities µik , one for each mode, using the measurements y1:k . The algorithm of how to merge the estimates, the time update, the measurement update and the mode probability estimation are given below for convenience. When a new time update is needed, the first step is to calculate the mixing probji Nδ abilities {µk−1|k−1 }i,j=1 j ji µk−1|k−1 Πij µk−1 = PN δ l=1 Πil µlk−1 (2.53) . In doing so it uses the mode switching probabilities Πij that states what the probability is of being in mode i, if one just previously was in mode j. N N δ δ 0i 0i The mixed estimates {x̂k−1|k−1 }i=1 and covariances {Pk−1|k−1 }i=1 are then produced as 0i x̂k−1|k−1 = Nδ X ji j (2.54) µk−1|k−1 x̂k−1|k−1 j=1 0i = Pk−1|k−1 Nδ X ji j µk−1|k−1 [Pk−1|k−1 + j=1 j 0j j 0j + (x̂k−1|k−1 − x̂k−1|k−1 )(x̂k−1|k−1 − x̂k−1|k−1 )T ] (2.55) In this step, a bit of the other state estimates from the other modes are mixed in. How much is mixed in is based on the current mode probabilities and the mode switching probabilities. These mixed estimates are now time updated using i 0i x̂k|k−1 = Aik x̂k−1|k−1 i 0i i i iT Pk|k−1 = Aik Pk−1|k−1 AiT k + Ek Qk Ek . (2.56) In both the time update and the measurement update, the model matrices associated to each corresponding mode is used. That means that when the state vector xki is updated, the state transition matrix Aik = Ak (δ i ) is used, for example. The new measurement yk is used to update the states using the standard Kalman 34 2 Sensor Fusion filter measurement update. i i i x̂k|k = x̂k|k−1 + Kki (yk − Cki x̂k|k−1 ) i Pk|k Ski Kki = = = i − Kki Ski KkiT Pk|k−1 i CkiT + Rik Cki Pk|k−1 i Pk|k−1 CkiT (Ski )−1 (2.57) (2.58) (2.59) (2.60) The mode probabilities wki are then updated using the previous mode probabilities, the mode switching probabilities and the innovation with innovation covariance for each filter. PNδ j i N (yk ; Cki x̂k|k−1 , Ski ) j=1 Πji µk−1 i µk = PN (2.61) j δ i l l PNδ l=1 N (yk ; Ck x̂k|k−1 , Sk ) j=1 Πjl µk−1 The updated mode probabilities µik are finally used to calculate the overall estimate x̂k|k and covariance Pk|k as x̂k|k = Pk|k = Nδ X i=1 Nδ X i µik x̂k|k (2.62) i i i µik [Pk|k + (x̂k|k − x̂k|k )(x̂k|k − x̂k|k )T ]. (2.63) i=1 2.3.6 Hidden Markov Model If a discrete mode δk is the only state that is sought, a Hidden Markov Model (hmm) can be used. In a system with S possible modes, the hmm estimates the probability of being in each mode using a test statistic λk , and a mode switching probability Πi,j . hmms are used in Paper C. The test statistic λk can be constructed using the measurements yk and has a known distribution for each mode δki . For example we could have λik ∼ N (0, σi2 ) j if δk = i and λk ∼ N (mj , σj2 ), if δk = j. To make the modes easily distinguishable, we want σj , σi and mj , 0. The mode switching probability Πi,j introduces a sort of dynamics to the system. Most often, λk depends only on the latest measurement yk , and Πi,j ensures that µk does not solely depend on yk but on previous measurements as well. Πi,j does this by stating the probability of switching from mode j to mode i. So if λk indicates that a very improbable mode switch has occurred, Πi,j states that the final mode probabilities should only indicate a minor change. In short, to facilitate an improbable mode switch, the test statistic likelihood for the new mode has to be very large or else multiple test statistics need to indicate that the switch has occurred for the mode probabilities to change completely. 2.4 35 Estimation under Disturbances When a new measurement yk is available, λk is calculated as λk = g(yk ). The probability of being in mode i at time k, sively (2.64) µik = P (δki |yk ), is calculated recur- µik = P (δki |yk ) ∝ p(yk |δki )P (δki |yk−1 ) = p(yk |δki ) Nδ X j Πji µk−1 (2.65) j=1 = p(λk |δki ) Nδ X j Πji µk−1 (2.66) j=1 since yk and λk contain the same information. µik then has to be normalized. Hence we have PNδ j p(λk |δki ) j=1 Πji µk−1 i . (2.67) µk = PN j δ l PNδ l=1 p(λk |δk ) j=1 Πjl µk−1 If the probability transition matrix is chosen as having the same probability of change between all modes, i.e. 1/S . . . 1/S .. .. Πc = ... (2.68) . . 1/S . . . 1/S the probability of each mode is only depending on the latest measurement. PNδ c j p(λk |δki ) j=1 Πji µk−1 i µk = PN P Nδ δ l c j l=1 p(λk |δk ) j=1 Πjl µk−1 PNδ j p(λk |δki )α j=1 µk−1 = PN P j Nδ δ l l=1 p(λk |δk )α j=1 µk−1 p(λk |δki ) = PN δ l l=1 p(λk |δk ) (2.69) All dynamics are then removed and only the latest measurement determines the mode. This makes it prone to give an erroneous mode classification after receiving just one troublesome measurement. 2.4 Estimation under Disturbances This section discusses a fundamental estimation problem that occurs in many shapes, and has certain applications in this thesis. The situation occurs when 36 2 Sensor Fusion there are two related measurements. The first sensor measures a physical quantity with a large deterministic disturbances, which can hardly be modeled as a stochastic process. The second sensor measures the derivative of the physical quantity, but with a small offset. Thus, integrating the second sensor corresponds to the physical quantity but with a linear drift over time. 2.4.1 Problem Fundamentals We want to estimate a feature pk of a dynamic system like a position or a headp ing. The feature is measured, yk , but the measurements suffer from deterministic p disturbances dk , like location dependent disturbances. Available is also a measurement of the system movements, like the derivative of p pk , called ykv . This measurement is unaffected by dk but the signal is corrupted by noise and slight sensor deficiencies like for example a slight bias. These errors are of the types that cannot be determined once and for all in a calibration procedure, since they might change slowly over time or be different every time the sensor is started. We assign a state vk for the derivative state to get the dynamic model ! ! ! ! 1 T pk pk+1 T 2 /2 + wk = 0 1 vk vk+1 T (2.70) p where wk ∼ N (0, Qk ). We also have the the measurement models where p ek yk = h(pk ) + dk + ek p p (2.71a) ykv ekv (2.71b) = g(vk ) + p ∼ N (0, Rp ) and ekv ∼ N (0, Rv ). p Now, if dk = 0 a nonlinear filter can be applied to estimate pk and vk . If h( · ) and g( · ) are linear a Kalman filter can be used. p If all measurements y1:k are corrupted by large deterministic disturbances, i.e. p d1:k , 0, and p0 is unknown, pk cannot be estimated correctly. This is natural since if all measurements of position is bad, estimating position using only velocity is impossible. p The question is how to estimate pk and vk when some di , 0? The estimation system should first and foremost be robust against the disturp bances dk . By robust we mean that p̂k should not be pulled away by the disturbances. Also, during the disturbed periods, the filter has to rely on the dynamic model only, but despite errors in ykv , p̂k should not drift away. In short, we want the filter to be both resistant to disturbances and movement measurement deficiencies. 2.4 37 Estimation under Disturbances 2.4.2 General Solution Outline p When the disturbances come and go, pk is observable when dk = 0 if we also p p know that dk = 0. So in order to utilize yk in our estimation system at all, we p p need to detect when dk = 0 and when dk , 0. Given that we know or have an p estimate of when dk = 0, pk can be estimated for those time instances. During the disturbed periods, we have to rely on the dynamic model (2.70) and (2.71b). Unfortunately, errors in ykv will cause p̂k to drift off. To be able to handle long periods of disturbances, the sensor inaccuracies corrupting ykv will therefore also have to be modeled and estimated. We assume that the sensor deficiencies can be modeled using the constant error parameters θ. We extend the state vector to also contain the assumed constant fault parameter vector pk+1 1 T 0 pk T 2 /2 vk+1 = 0 1 0 vk + T wk (2.72) θ 0 0 1 θ 0 p p p yk = h(pk ) + dk + ek (2.73a) ykv (2.73b) = g(vk , θ) + ekv If the error modeling is feasible and θ̂ is estimated correctly, this model is a better description of the true system. It can therefore be relied upon during much longer periods of disturbance. In most practical applications though, eventually p̂k will start drifting off. So, not only do we have to estimate pk and vk , we also have to estimate θ. Given p that we know when the measurements yk are disturbed and when they are not, p the undisturbed sections of yk can be used to estimate θ and pk . p p Since we do not know if dk = 0 or dk , 0, this has to be estimated as well. In the p p end, we have to estimate pk , vk , θ and the system mode, i.e. if dk = 0 or dk , 0. 2.4.3 State and Mode Estimation This type of problem fits into the mode based estimation systems such as a Kalman filter bank, Section 2.3.4, or an hmm filter, Section 2.3.6. In a Kalman filter bank solution, the system switches between different models depending on the estimated mode state. The mode estimate depends on the likei lihood of the measurement given the mode and the previous one, p(yk |δ1:k , y1:k−1 ). In the imm filter, this means that the mode estimation is based on the innovation p,i p of each filter, i.e. ỹk = yk − h(p̂ki ). The advantage of estimating state and mode jointly using an imm filter, is that 38 2 Sensor Fusion p when the filter has converged, detecting if dk , 0 is very straightforward. Unp fortunately, if the filter has diverged, all measurements yk:N will be erroneously interpreted as disturbed. The filter has to be reinitialized to function properly again. p Using an hmm, filter mode probabilities µk can be estimated using a test statistic independent of the states. A test statistic can be constructed as p λk = f (yk , ykv ) (2.74) It is quite common that the distribution of the test statistic is known in the usdisturbed case but unknown for the disturbed case, which has to be approximated. Once the state mode has been estimated, the states can be estimated using a nonp linear filter where for example Rk = Rp (µk ). If the vector of estimates p̂0:k is sought and the system has linear dynamics, the state estimation can often be formulated on batch form, see Section 2.3.3. Again, p the measurement noise Rk can be made mode dependent. Using a batch formulation, the smoothing estimate of p̂0:k−1 and the filtering estimate of p̂k is acquired. The number of states can be reduced by using a dynamic model on input form where ykv drives the time update. pk+1 = pk + f (ykv , θ) + ek (2.75) Such a model can be approximated as a function of only p0 and θ, see Section 2.2.3. In this thesis this has been applied on the heading estimation problem, see Papers B and C. 2.4.4 Examples of Problems The problem described above is quite common in practical sensor fusion applications. The simplest case is the scalar one where pk is one-dimensional. • Position estimation on rails or roads from a speedometer, which is reliable and accurate up to scale and bias, and position measurements from for example gps or landmark tracking. The position estimates are uncertain due to satellite occlusion or lack of steady landmarks. • Speed estimation for an autonomous underwater vehicle (auv). pk is vep locity, accelerometer measurements ykv are used as input signal and yk are measurements from an unreliable speedometer, e.g. Doppler velocity. p • Heading estimation where yk are either heading measurements from a magnetometer or difference-of-arrival (doa) from radio transmitters estimated by an antenna array. Both measurement types are prone to large and frequent disturbances. The doa estimates suffer from multipathing, where the angle may correspond to the position of strong reflectors in the environment, see Figure 3.2 from Erkstam and Tjernqvist (2012) for an example. Magnetometers are often disturbed by location dependent interferences indoors, see Paper A. To support the heading estimation, a gyro can be used 2.4 Estimation under Disturbances 39 as ykv but such measurements often have bias and scale errors. The heading estimation problem using a disturbed magnetometer and a low grade gyro will be studied in depth in this thesis, see Section 3 and Papers A, B and C. A two dimensional example of such a system is positioning of a car with gps and wheel encoders. The gps measurements are often good but can be disturbed by large buildings or tunnels. The wheel encoders measure how the wheels are turning, thereby giving estimates of the velocity and turning rate of the vehicle. Unfortunately these movement estimates will have errors due to wheel slip, sensor errors, slightly different wheel diameters etc. So in this scenario, pk is vehicle pop sition and heading, yk is the gps measurement and ykv are the wheel encoders. If the road network is known, the problem complexity can be reduced significantly. 2.4.5 Discussion On most occasions the measurement function h( · ) is just a measurement of the state, h(pk ) = pk . In the case of heading estimation using a magnetometer it is instead h(pk ) = pk mod 2π. How to handle this in the batch estimation case is discussed in Paper B. The question of using the full state model (2.72) or the input model (2.75) is to a large extent a question of how many states one prefer. As was shown in Callmer (2011), the full model introduces a time delay in the system dynamics and also a low pass filtering of the noise in ykv . If ykv does not suffer from outliers, missing data or similar, the input form is the more attractive one because of the delay and lower state dimension. If the dynamic model (2.75) is also on the reduced form presented in Section 2.2.3, the only unknown states are p0 and θ. Estimating only these few constant states p v using y1:k and y1:k on batch form may seem like quite an overkill, but the resulting estimates p̂0:k will be very resistant to both disturbances and sensor errors. More on this in Papers B and C. 3 Indoor Positioning The ability to position people or equipment indoors has become a very hot topic in the last couple of years. What used to be a mostly academic or military field of interest has now also entered the commercial sector. Along the way one thing has been lost though: a clear understanding of what we are actually talking about. The question has become if one can solve the indoor positioning problem (see for example Economist (2012)) but it is not a relevant question. The question in general has no answer since the solution relies heavily on a different question: for whom or what it shall be solved and for what purpose? There is no silver bullet that solves the entire problem, but there are many different types of solutions, each working in its own scenario. The broadest way to divide the indoor positioning issue into smaller manageable ones, is to start with if the indoor environment is known or unknown. There is a huge difference between positioning a soldier inside an unknown building in an unknown country, or to position ones own things in ones own building. If one has control over the environment, two things in particular can be utilized to aid in the positioning. First of all maps of the building can be utilized. They can restrict the possible user movements, thereby significantly lowering the uncertainty in the position estimate. The second thing one can do is to install equipment that aids the positioning such as radio beacons, tags for cameras or reflectors for scanners. Using these, one can produce environmental maps that show what will be measured where, which will provide great support for a positioning system. The other indoor positioning problem is the one in which we have no control over the environment. The positioning should be performed in an unknown building 41 42 3 Indoor Positioning where the only information available is from sensors carried by the subject. This makes this problem scenario much more challenging. Again, how to solve this problem depend on the reason why one wants it to be solved. If the device used is a robot, one can for example equip it with wheel encoders for odometry and cameras and laser scanners to support the positioning. In such a scenario the sensor platform is fixed making the geometrical relation between the sensors known. If the end user is a human such as a first responder or a soldier, there are limits to which sensors can be carried without them affecting the user’s ability to perform his or her task. Also, to keep the cost of the system low, the choice of sensor often becomes a cost/performance tradeoff. The problem of indoor positioning for humans in an unknown environment is the primary one studied in this thesis. Such systems are often based on a dead reckoning system estimating the step-to-step movements. In this chapter we will give a background to that problem and then describe a part of the dead reckoning system in a bit more detail. We will also give a short presentation of additional techniques in the literature that have been used to support the dead reckoning system, utilizing for example information about the building. 3.1 Human Positioning in an Unknown Environment In some scenarios, the operational environment is more or less completely unknown. The chances of there being a digital map available of a building in a warzone is slim at best. Maps of residential homes are also unlikely to be available to aid positioning for police or fire services, for the next couple of years. A positioning system for such environments will therefore have to be based on other techniques. A basic system often used in indoor positioning is the pedestrian dead reckoning system (pdr). It estimates user position relative to a known starting position using the estimated user movements. User movements are measured using an immu and the accelerations measured are translated into a change in position using double integration. For such a system to give useful position estimates, the sensor is often mounted on the foot. By detecting every time the foot is at rest, the accumulated estimation errors can be corrected. For descriptions of pedestrian dead reckoning systems, see for example Beauregard (2007); Feliz et al. (2009); Foxlin (2005); Ojeda and Borenstein (2007); Godha et al. (2006); Woodman and Harle (2009); Jiménez et al. (2010a); Bebek et al. (2010). Such a system will also be briefly described in Section 3.3 and a stand still detection system is presented in Section 3.3.1. Using such a system, the distance traveled can be estimated well but there is a drift in heading causing positioning errors. In this thesis the problem of heading estimation is given a lot of attention, Papers A and B, and when such a system is included in a positioning system, the performance increases dramatically. This is shown in Paper C. Our system is still just a dead reckoning system. To achieve 3.1 Human Positioning in an Unknown Environment 43 even better positioning, more information is needed. A well studied problem in autonomous robotics is the simultaneous localization and mapping (slam) problem. It is the problem of creating a map of the surroundings while at the same time positioning oneself in the very same map. A tutorial on the subject is Durrant-Whyte and Bailey (2006); Bailey and DurrantWhyte (2006). In slam, a key to reducing the errors in estimated position is the so called loop closure. If one can detect that an area is being revisited, the error and uncertainty in estimated user position can be reduced significantly. To produce loop closures in indoor positioning for humans, for example a camera can be used, Rydell and Andersson (2010). If one does not want to add more sensors, user activities can instead be used as loop closure features. In Grzonka et al. (2010), the ’opening door’ activity was detected and such events were connected to each other. By estimating also the number of doors and their specific locations, ’door opening’ events could be used as loop closures. This was shown to significantly reduce the errors in estimated user position. Such passive events could be replaced by active events like user induced markers in the data. For example, if one believes the area one is in will be revisited, a marker in the data, like a special sensor wiggle, could be added. By connecting those markers, the same effect can be achieved. Loop closures can also be made by having the user place out radio beacons along the path, Renaudin et al. (2007). The next time the beacon is passed, a loop closure is called and the built up errors are reduced. More on radio based positioning below. In Angermann and Robertson (2012), slam was performed using only the user movements. By assuming that the user walk the same paths multiple times, the paths can be connected. When backtracking a path, assuming that the two paths are one and the same reduces the drift. That way biases in the system can also be estimated. Even if one does not have a map of the building, some information about it is often still available. In Abdulrahim et al. (2011); Borenstein and Ojeda (2010) the user is assumed to primarily only walk in the principal directions of the building. For any given building, and especially office buildings, the main directions one can walk indoors are aligned with the outer walls. These four directions, sometimes expanded to eight, are the directions one can assume the user will most often be walking in. The estimated heading is therefore pushed towards the nearest principal direction. Only knowing the outer walls of a building can also reduce the accumulated positioning errors, Beauregard et al. (2008). How much information such a map will provide is though strongly correlated to the layout of the building. In that work, the building was L-shaped and narrow, providing quite a lot of information. If the building is large and square, the positioning improvements will most likely be marginal. 44 3.2 3 Indoor Positioning Human Positioning in a Known Environment When one is in control of the building or at least has a solid knowledge of it, positioning can be aided by utilizing this information. A map can be used to put restrictions on where it is actually possible to walk, which can provide a lot of support to a dead reckoning system. To get actual measurements of absolute user positions, location dependent features like visual tags, Zachariah and Jansson (2012), or radio environment measurements can be used. Knowing which wifi access points that can be seen where, provides a lot of support to the positioning system. For many more techniques available for various kinds of indoor positioning, see Muthukrishnan et al. (2005); Gu et al. (2009). 3.2.1 Map Matching The most common approach in map matching is to let the estimate of user position roam free as long as it does not try to walk through a wall. In Woodman and Harle (2008), a pdr system is fused with map matching using a particle filter. The map is represented as a 2.5D set of polygons that have either passable or un-passable edges. Also stairs are included in the same manner, linking the floors. This representation is shown to significantly reduce the error in position, providing a drift free estimate in user position once the filter has converged. Widyawan et al. (2007) also applies a pdr system to a map matching system using a particle filter. The map representation is here based directly on the walls, giving zero weight to particles that want to walk through a wall. A similar approach was taken in Krach and Robertson (2008). A problem with such a system is that a position estimate in a large room will be given a higher weight, and therefore be perceived as more likely, than a position estimate in a more closed space, regardless of actual user position. Since the only thing limiting the system is walls, position estimates where the particles have a high risk of running into the walls, will be bad ones while the open areas will be good ones. One can therefore experience depletion where particles in closed spaces are resampled into open spaces even if the closed space area is the one the user is actually in. In Kaisera et al. (2013) this was handled by weighting the particles using an angular probability density function that specifies some directions as more probable than others. Particles travelling along a corridor then get a higher weight than ones that will hit the wall soon. A map aided dead reckoning system restricts the possible movements of the user but does not provide any measurements of absolute position. If the position estimates have diverged, only the uniqueness of the path the user has travelled can make it possible to rediscover the true position. 3.3 Foot Mounted IMMU for Dead Reckoning 3.2.2 45 Radio Positioning To get a measurement of the absolute position of the user, other features like the radio environment can be used. For example the wifi coverage in a building can be used to greatly restrict the number of possible user positions if measured. Radio based positioning is an entire research area in itself and will here only be covered briefly on a more conceptual level. For more details, Bahl and Padmanabhan (2000); Li et al. (2006); Roos et al. (2002); Youssef and Agrawala (2005); Honkavirta et al. (2009) are a good start. There are two main methods of getting a position measurement from the radio environment: model-based and map-based. Model-based approaches estimates the distance to access points in known locations using radio propagation models and the measured drop in signal strength. Map based methods use a radio map that for a given position specifies which signals that should be measured and what strengths these signals should have. The actual radio signals measured by the user can then be compared to the map to get a measurement of user position. The more radio signals and the more the indoor environment makes them change, the more accurate the position estimate will be. Unfortunately, it can be hard to get a good positioning performance, since also such a simple thing as the user orientation will affect the result, Dil and Havinga (2010) Model-based approaches are cheap since the site need not be visited. Known access point locations and a map can be used to produce an approximate model. The downside is that the model is just approximate if the signals have to travel through walls. The location of the access points must also be known and so also the broadcasted signal strengths. For the map-based approaches it is the complete opposite. The environment is often mapped manually, ignoring the problems of radio propagation models and knowing the access point locations. On the other hand, the map creation part is often tedious and expensive and if the access points move, the mapping has to be remade. In Woodman and Harle (2009), wifi fingerprinting was used to initialize the user position and support the pdr/map matching positioning system. Similar approaches were taken in Seitz et al. (2010); Wang et al. (2007) with equally successful results. As long as the radio map is accurate, a highly robust position estimate is achieved. If there are multiple users and few beacons, collaborative localization can improve the positioning significantly by measuring and sharing the distances between the users, Wymeersch and J. Lien (2009); Rantakokko et al. (2011). 3.3 Foot Mounted IMMU for Dead Reckoning The pedestrian dead reckoning system mentioned above has become a very popular system and will most likely be the foundation of future commercial indoor 46 3 Indoor Positioning navigation systems. It is cheap, hard to jam, easy to operate and gives quite good positioning results. The immu mounted on the boot has three dimensional accelerometers, gyroscopes and magnetometers to measure the movements of the users. Double integrating the accelerometer measurements gives user position relative to the starting point if the gravity component can be removed correctly. Since sensor orientation is estimated using semi grade gyros, the orientation estimate will be a little bit off. This results in parts of the gravity component to be interpreted as user accelerations, resulting in an ever growing position error. By utilizing the stand still phases, i.e. detecting and exploiting when the foot is on the ground, accumulated velocity errors can be corrected. This so called zero velocity update significantly improves the position estimates, and reduces the error in distance travelled from cubic in time to linear. To have robust zero velocity updates, we need robust zero velocity detection. We have therefore derived a stand still detection system with very few false positives. The remainder of this chapter is a presentation of our probabilistic stand still detection system. The system is described in detail and its performance is evaluated for a couple of different sensor positions. This dead reckoning system is described in Paper C, so to avoid too much overlap it is not presented here. 3.3.1 Stand Still Detection A reliable stand still detection system is key to creating a good dead reckoning system. Since most detection systems in the end become a tradeoff between missed detections and false positives, for a system like this, avoiding false positives is much more important than a few missed detections. The main purpose of the stand still updates is to anchor the estimates at a regular basis, clearing the built up errors in estimated velocity. Missing one or two stand stills here and there is therefore not a big problem. A stand still detection framework can be based on two types of sensors. Either one uses special sensors just to detect that the foot is touching the ground, or one uses the immu to detect that the sensor is at rest. Theoretically, the former approach could provide safer stand still detections, but there are also some drawbacks. Often sensors designed to detect contact have some moving parts which are prone to be worn out by usage. Also, if the user is crawling, the foot is sometimes at a stand still even though the sole is not touching the ground. That means, to detect different types of standstill, multiple contact detecting sensors are needed on the boot. And finally, the fewer the sensors, the less that can go wrong. If one can detect the stand stills using the immu data itself, it is therefore better. immu based stand still detection has most often been done by comparing the accelerometer or gyro signal to a threshold. Here we put the stand still detection in a probabilistic framework using a test statistic with known distributions and an hmm. The result is an estimated probability of stand still that is consecutively 3.3 Foot Mounted IMMU for Dead Reckoning 47 calculated at every time instant. This can be used for zero velocity updates in a filtering framework. Parts of our probabilistic framework for stand still detection have previously been presented in Callmer et al. (2010b), Callmer (2011) and Rantakokko et al. (2011). A very compressed version is also given in Paper C. Related Work The most common approach to stand still detection is to use averaged accelerometer or gyro measurement and compare it to a threshold; Beauregard (2007); Feliz et al. (2009); Foxlin (2005); Ojeda and Borenstein (2007). The threshold is chosen ad hoc and is normally quite restrictive to minimize false positives. Another approach is the moving variance used in Godha et al. (2006) where the variance computed over a sliding window is compared to a threshold. One of the problems with this approach is that in order to make it work properly, the time interval the boot needs to be still is quite long. The system can therefore work quite well if the user is walking but not so much during running. Probabilistic zero velocity detection has previously been proposed in Skog (2009) and Skog et al. (2010) where a hypothesis test was used to determine if the foot was stationary or moving. The hypothesis test was performed using a test statistic based on a Generalized Likelihood Ratio Test (glrt). The test statistic had a known stand still distribution and an unknown movement distribution that was approximated as a constant. Since one of the probability density functions was constant, the glrt test boiled down to comparing a test statistic of the norm of the acceleration measurements and the angular velocity measurements, to a threshold. If the test statistic was smaller than the threshold, a stand still was declared. Since the test statistic had an unknown movement distribution, the threshold was chosen ad hoc, making the framework in practice similar to the ones in Beauregard (2007); Feliz et al. (2009); Foxlin (2005); Ojeda and Borenstein (2007). System Outline The stand still detection system is mode based and uses an hmm filter to estimate the probability of stand still and movement, respectively, Section 2.3.6. As input, the hmm uses a test statistic based on the immu signals. The test statistic is based on the latest gyro and accelerometer measurements and has a known distribution during stand still, since the sensor should then be at rest. The immu was mounted by the shoe laces to get a good tradeoff between stand still detection performance and gyro dynamic range. Test Statistics Derivation An example of a walking sequence with a shoe lace mounted immu is shown in Figure 3.1. The foot is stationary around sample 530, 650, 770, 880 and 980. During these phases the norm of the accelerometer signals is the gravitation constant with some added noise. At the same time the norm of the angular velocity signal is zero with some additive noise. These are the key 48 Angular Velocity, [rad/s] Acceleration, [m/s2] 3 Indoor Positioning Accelerometer Data 50 0 ï50 500 600 700 800 900 1000 900 1000 Gyro Data 10 0 ï10 500 600 700 800 Sample Figure 3.1: Example of accelerometer data (where x, y and z is solid blue, dashed green and dashdotted red, respectively) and gyro data (ωx , ωy and ωz is solid blue, dashed green and dashdotted red, ωi is angular rotation rate around axis i) during a walking sequence. The foot is stationary around time instances 530, 650, 770, 880 and 980. characteristics of the test statistic. Sensor Models The signal model is # " a# yak (δk ) v yk = ω + ωk yk (δk ) vk " (3.1) where yak and yω k denote the acceleration measurement vector and the angular velocity measurement vector, respectively. Further, δk denotes the model dependence on the phase of the human step sequence at time k. Naturally, the model differs significantly between when the foot is at stand still and when it is moving. The measurements are assumed to have additive independent identically distributed Gaussian noise va ∼ N (0, σa2 ) and vω ∼ N (0, σω2 ) where σω2 = σω2 I and σa2 = σa2 I and I is the 3 × 3 identity matrix. During stand still the sensor model is " a# " # " a# v yk guk = + ωk , yω 0 v k k (3.2) where uk is the unknown gravitational direction vector and g is the gravitational constant 9.81. Since the orientation of the boot changes over time, so does uk . 3.3 49 Foot Mounted IMMU for Dead Reckoning When the foot is moving the sensor model changes to " a# " # " a# yk v guk + ak = + ωk yω ω v k k k (3.3) where ak is user acceleration and ωk is user angular velocity which therefore have unknown distributions. Test Statistics We construct a test statistic by combining the acceleration and angular velocity measurements λk = kyak k2 σa2 + 2 kyω kk (3.4) σω2 where λk ∼ χ2 (6, λ) during stand still. It has a non-central chi-square distribution since yak has nonzero mean when the foot is stationary. Its non-centrality parameter λ = g 2 /σa2 and 6 is the number of degrees of freedom. Test Statistic Appearance during Walking Sequence The test statistic for the walking sequence in Figure 3.1 is plotted in log-scale in Figure 3.2. The mean of the stand still distribution is marked with a dashed line. The stand still events occurring around time instances 530, 650, 770, 880 and 980 are clearly visible. Combined Test Statistic 5 10 4 10 3 10 2 10 500 600 700 800 Sample 900 1000 Figure 3.2: Logarithmic plot of the test statistic with the mean of the stand still distribution marked with a dashed line. The foot is stationary around time instances 530, 650, 770, 880 and 980. λk has a movement distribution that is significantly larger than the stand still distribution. The stand still phases are all close to the mean of the stand still distri- 50 3 Indoor Positioning bution. Considering the significant spread between the stand still and movement distributions, detecting the stand still phases should be quite straightforward. Test Statistic Distribution Validation The test statistic stand still and movement distributions must be validated using experimental data. We need to make an approximation of the movement distribution for the hmm and also to ensure that the experimental stand still distribution is similar to the theoretical ones. The latter are plotted in Figure 3.3 where the empirical stand still distribution has been approximated using a histogram. There is a slight offset between the theoretical distribution and the empirical distribution. This is mainly related to how the data for the empirical evaluation was chosen. If the stand still data samples are chosen more conservatively, the mean of the empirical distribution will decrease. The empirical movement distribution is shown as a histogram and is plotted with its approximation, Figure 3.4. The movement distribution was approximated using two Gaussians as pm (λ) = 0.92 · N (3000, 7000) + 0.08 · N (600, 150). The histograms were created using a large amount of experimental data. Figure 3.5 show the histograms of both the stand still data and the experimental data. Clearly the two distributions are well separated. The same movement distribution can be used for other types of movement as well like running, since the differences between different movement distributions are quite small, especially in the border region close to the stand still distributions. Mode Probability Estimation To determine the probability of stand still, an hmm is used, Section 2.3.6. In this case it has two modes: stand still and movement, and it estimates the probability of being in each mode at every time instant. The mode transition probability matrix states the probability of switching modes which introduces some dynamics into the mode estimation. A lower mode transition probability requires a measurement with a higher likelihood for a mode switch to occur and vice versa. The mode transition probability matrix used in the experiment is " # 0.98 0.02 Π= 0.02 0.98 (3.5) which states that the probability of going from stand still to moving or vice versa, is 2%. During normal walking the right foot takes about one step per second which results in roughly 2 mode transitions every 100 measurements. The mode probabilities at time k are calculated using the recursion (2.67). The probability density function of movement used in the hmm, is an approximation set to resemble the empirical movement density function, Figures 3.4. 3.3 51 Foot Mounted IMMU for Dead Reckoning Pdfs of Distribution for Test Statistic during Stand Still 0.012 Theoretical Dist Empirical Dist 0.01 0.008 0.006 0.004 0.002 0 300 400 500 600 700 800 λ Figure 3.3: Theoretical stand still distribution of λ and histogram of stand still measurements of λ from experimental data. Offset is mainly due to how empirical data was chosen. For the binary mode case stand still and movement, the algorithm can be summarized as in Algorithm 4. Experimental Results The mode estimation system was evaluated on the data sequence in Figure 3.1. The resulting mode probabilities provided by the hmm are shown in Figure 3.6. The measurement noises were set as σa = 0.5 and σω = 0.08, estimated from data when the sensor was at rest. The stand still detection framework has no trouble detecting the stand still phases, Figure 3.6. The movement probability is very close to 1 during the movement phases while the stand still probability is about 0.999 when the foot is at rest. The stand still probabilities fluctuate a bit because they are quite sensitive to movements. Since a foot mounted immu is not entirely still just because the foot is touching the ground, this is entirely natural. Longer walking experiments with 318 steps reveal that all 318 stationary phases were detected with no false positives. In these experiments a winter boot with a toe mounted sensor was used. One of the big problems when evaluating indoor navigation systems is how to acquire reliable ground truth data. You can count the number of steps you take but that only works when you are just walking straight ahead and that is the least interesting case since it is the simplest. A data set consisting of hundreds of steps 52 3 4.5 Indoor Positioning −4 Histogram and Approx Distribution of T during Movement x 10 Empirical Movement Dist Approx Movement Dist 4 3.5 3 2.5 2 1.5 1 0.5 0 3 4 10 10 λ Figure 3.4: Histogram of movement measurements of λ from experimental data, and the approximated Gaussian sum that resemble it. when you open and close doors, move around furniture and such, is much more difficult to analyze afterwards to detect exactly when the foot was still. One way to evaluate stand still detection performance is to incorporate it in a positioning system. If the positioning works well, the stand still detection is at least not working poorly. Conclusions A test statistics with known stand still distribution has been evaluated for stand still detection. In conjunction with an hmm, the mode probabilities are readily calculated and can be used for zero velocity updates. The framework has been shown to safely detect stand still phases in the data. 3.3 53 Foot Mounted IMMU for Dead Reckoning 0 Histograms of Normalized Test Statistic Distribution 10 Stand Still Movement −1 10 −2 10 −3 10 −4 10 3 10 4 10 Figure 3.5: Histograms of stand still and movement data of λ from experimental data. The two different types of movements are well separated. Algorithm 3 Stand Still Detection Framework Require: Stand still distribution ps and movement distribution pm of test statistic T . Measurements ya and yω with noise parameters σa and σω . Mode transition probability matrix Π. 1: for k = 1, . . . , N do 2: Compute test statistic. kya k2 kyω k2 λk = k2 + k2 (3.6) σa σω 3: Estimate stand still and movement probabilities, µsk and µm k. Ps,k Pm,k wkm = Ps,k + Pm,k Ps,k + Pm,k Ps,k = ps (λk ) Πss µsk−1 + Πsm µm k−1 Pm,k = pm (λk ) Πms µsk−1 + Πmm µm k−1 wks = where 4: end for (3.7) 54 3 Indoor Positioning Probability of Each Mode 1.005 P(still) P(walking) P(mode) 1 0.995 0.99 500 600 700 800 Sample 900 1000 Figure 3.6: Mode probabilities from the data set in Figure 3.1. The foot is stationary around samples 530, 650, 770, 880 and 980. 3.3 Foot Mounted IMMU for Dead Reckoning 3.3.2 55 Stand Still Detection Performance for Different IMMU Positions For a localization framework with a foot mounted immu, the sensor must be mounted on the shoe. Where the immu is attached on the boot affects the performance, not only of the stand still detection but also of the positioning. For stand still detection, the sensor placement affects the length of the stand still phases since different parts of the boot are stationary during different sequences of the stance. Also, during some movements like running, the boot is stationary for a shorter time and parts like the heel are rarely stationary at all. In this segment we will study how the sensor placement affects the stand still detection performance for different movements. The first sensor position is by the heel, Figure 3.7a. In a real end user application one idea is to hide the sensor in the thick sole of the heel to protect it, Nilsson et al. (2012). Another option is to put the sensor by the toes since they are involved in almost all movements, Figure 3.7b. Intuitively, the toes should be stationary during pretty much all conceivable standing motions sequences. Since the sensors are getting smaller and smaller, the problem of hiding the sensor in the thinner sole by the toe could soon be solved. A third option is to put the sensor by the shoelaces, Figure 3.7c. This position could be common if the sensor is not integrated with the shoe, but is instead strapped onto the preexisting boot. The boot type is also important for stand still detection performance. A sturdy boot, Figure 3.7a, rolls over the ground without getting reshaped by the surface while a softer boot, Figure 3.7b, absorbes more of the contact force and alters shape. If the sensor is attached to a sturdy boot, the rolling will result in a sensor that is never really still making stand still phases very hard to detect. A softer boot makes the stand still phases easier to detect but is rarely the boot of choice for firefighters, soldiers etc. The stand still detection performance was evaluated for both boots so the results could be compared. For the experiments, the sensor was attached using duct tape, Figure 3.7. The sensor is thereby sturdily attached to the boot while it is still very easy to setup experiments. All three sensor positions were evaluated using both boots for walking and running. The main problem with evaluating even more realistic firefighter movements like crawling is that it is very difficult to obtain reliable ground truth data for such experiments. The experiment has to take place in a high precision positioning lab, for example a vicon1 lab, but not even such experiments will be completely like actual crawling around furniture and such in an apartment. Since the immu data is not really periodic, the stand still phases are not straightforward to pinpoint. The more distinct and repetitive the movement is, the easier it is to differentiate between true and false positives based on the sensor data. 1 http://www.vicon.com 56 3 (a) Sturdy hiking boot with the sensor by the heel. (b) Softer winter boot with the sensor by the toe. Indoor Positioning (c) Sturdy boot with the sensor by the shoe laces. Figure 3.7: Sensor positions and available boots for stand still detection experiments. Experimental Results A stand still has been assumed detected if the estimated stand still probability is above 90% during a true stand still. If no stand still did occur it is considered a false positive. The 90% threshold is mostly important in the running case. During a walking sequence, the stand still phases are commonly marked out with a stand still probability above 99.9%. In the running sequence the stand still phases are short making it hard to even reach 90% stand still probability. The preferable result is a combination of high stand still detection ability combined with no or very few false positives. All in all, the best sensor position for stand still detection seems to be a toe mounted sensor due to its enhanced ability to detect stand stills while running. Walking The first movement that was evaluated was walking. This is the most common movement used in localization experiments, Most likely, this is because walking is easy to perform, the data is straightforward to analyze and the stand still phases are long and therefore easy to detect. The results from the walking sessions are shown in Table 3.1. In general, all stand still phases are detected with very few false positives, no matter the boot or the sensor position. Walking sequence Robust boot, 50 steps Stand stills detected False positives Soft boot, 50 steps Stand stills detected False positives Heel 50 1 Heel 50 0 Toe 50 0 Toe 50 0 Lace 50 0 Lace 50 0 Table 3.1: Experimental results of the stand still detection performance of a walking sequence using different sensor positions on two different boots. 3.3 57 Foot Mounted IMMU for Dead Reckoning Running The second movement is running which is much more difficult. While running, the stand still phases are very short and even then the boot tends to roll quite a lot. Also, when running with sturdy boots the heel is not always even touching the ground. Table 3.2 shows the stand still detection performance during running. Clearly there are now different results for different settings. It is for once much harder to detect stand stills when a sturdy boot is used than when a soft boot is used. The sturdy boot rolls over the ground making the sensor never really still. Especially the gyro is excited in the angle around the direction pointing straight out to the side. The stand still phases are therefore pretty much non existent when a rigid hiking boot is used. The lace mounted sensors have significant problems detecting any stand stills, even when the softer winter boot is used due to this rolling effect. One way to improve the results was therefore to remove the problematic gyro dimension. In doing so, the lace mounted rigid boot was able to detect 30 out of the 41 running steps without any false positives, compared to just finding 1 before. Unfortunately, for walking this test statistic instead introduced 19 new false positives. Running sequence Robust boot, 41 steps Stand stills detected False positives Soft boot, 41 steps Stand stills detected False positives Heel 0 0 Heel 20 0 Toe 16 0 Toe 37 1 Lace 1 0 Lace 0 0 Table 3.2: Stand still performance during a running sequence using different sensor positions. Discussion The problem of reliable stand still detection using a foot mounted immu is not fully solved yet. In the end the solution will have to be application specific. We have in this section shown that both the type of boot and the position of the sensor can alter the stance detection performance greatly. These results are not final though. The heel mounted sensor for example will probably experience a different type of movements when actually mounted inside the heel. This would also be true for the toe mounted sensor when the immu becomes small enough to hide in the front sole. Even though the toe mounted sensor is the best for stand still detection, the question does not end there. Different sensor positions excite the sensors differently. The toe mounted sensor for example experiences the biggest angular velocity of the three positions, up to almost 20 rad/s compared to about 10-12 rad/s for the other positions. And this is just during regular walking. The sensor position 58 3 Indoor Positioning thereby puts a higher demand on the sensor quality, requiring that it is accurate for a very large dynamic range. 4 Discussion and Future Work This chapter ends the first part of the thesis constituted of background theory. The discussion primarily covers the publications which are the second part of the thesis. We will end with suggested directions for future work. 4.1 Discussion Some of the conclusions have been drawn under their respective sections in the publications, but some will be repeated and extended here for completeness. 4.1.1 Indoor Localization Our work in indoor localization for professional users covers the problem of positioning in small or medium sized venues of which there is no prior information available. The approach taken was to extend and improve the foot mounted immu based dead reckoning systems suggested in the literature. It is a simple yet fundamental system that should be a part of a final positioning system. The main shortcoming of such systems is the drift in position estimate. This is primarily caused by a drift in heading, not in distance traveled. The stand still detection and utilization in the filtering has significantly reduced the latter. To reduce the drift in heading, approaches like adding sensors, for example cameras, have been taken. Also extensive system calibration has been used to reduce the drift but this is not really a solution to the problem, only a postponement. Our feeling is that one can do more with just one immu. Before additional sensors were added, had the limit for positioning using only one immu really been reached? Our primary goal hence became solving the heading estimation problem. If one 59 60 4 Discussion and Future Work could have both a reliable heading estimate and movement length estimate, the final position estimate should be quite good. To do this we decided to use the sensors already available in the immu: the magnetometer and the gyro. Two heading estimation methods have been presented. The first one presented in Paper A is a Kalman filter bank based filtering method. Two modes are estimated, disturbed and undisturbed, deciding which magnetometer measurements can be used for heading estimation and which should be discarded. The final result is a filtering estimate that is significantly better than the raw magnetometer measurements in a vast majority of all experiments. The second system presented in Paper B is an optimization based approach. The estimation is done in batch form over the entire dataset producing a smoothing estimate for the heading of the complete experiment. The strength of the system is its ability to handle significant disturbances while maintaining the smoothness of the gyro integration trajectory. A weakness is the somewhat ad hoc approach and that its basic assumptions of the gyro quality is perhaps not valid for all sensors. The heading estimation approaches have been merged with a dead reckoning system. The system is based on a probabilistic stand still detection framework described in Section 3.3.1. The hmm based stand still detection produces a probability of standstill that experiments verify is very reliable. The best stand still detection ability has turned out to be achieved when the sensor is mounted near the toe. It has also been shown that a more rigid boot makes stand still detection more difficult since it has a tendency to roll over the ground. The stand still detection system cannot handle running well today, but it could probably be solved by also incorporating the step frequency that will be present in such a scenario. To include the heading estimation system in the positioning system, it is crucial that it is only used once per stand still phase. If the heading is estimated multiple times per stand still, basically the exact same measurement of magnetometer heading and system orientation will be used by the heading estimation system over and over again. This will ruin the heading estimation since magnetic disturbances become extremely hard to detect. Including the magnetic heading measurements in the stand still update is not a good approach. It can be done using for example the imm based heading estimation framework in Paper A. Unfortunately, the resulting estimated position trajectory becomes very uncertain. Since a small change in orientation will cause the position estimates to jump significantly, the position trajectory becomes very shaky. To make the trajectory smooth, all previous position estimates must be adjusted using smoothing. The approach we suggest is to use accelerometer and gyro to produce estimates of step to step movements using stand still detection and measurement integration. These estimates are then reduced to estimates in 2D, x̂0:N , ŷ0:N and heading ψ̂0:N . This vector of gyro based heading estimates are then adjusted to match a vector of magnetic heading measurements to remove the drift. These new head- 4.1 Discussion 61 ing estimates ψ̂ˆ 0:N are finally used to produce new position estimates x̂ˆ0:N , ŷˆ0:N . The experimental results show that the estimated user trajectory improves significantly when the magnetometer is included in the estimation. 4.1.2 RADAR SLAM Naval vessels today rely heavily on the gnss system. Not only is it used for positioning, but also for its exact time estimates. It is not unusual that a large number of control and decision systems are receiving information from the gnss system. That means that if gnss fails, a number of crucial systems on the ship will fail simultaneously. In controlled experiments, gnss jamming has turned out to cause complete chaos on the bridge. This makes naval vessels very vulnerable to gnss disturbances, intentional or unintentional, in critical situations. To reduce the impact of gnss outage, we have designed a positioning system that will produce accurate estimates of velocity, heading and position of the vessel throughout the outage, using only the vessel’s own radar sensor. This system is presented in Paper D. It uses the reflections from the surrounding islands and stable landmarks are tracked over time. By studying how the islands move relative to the vessel, its position can be estimated. A strength of the system is its simplicity. All commercial vessels are already equipped with a radar sensor why no new expensive equipment is needed. One weakness of the system is of course that if there are no islands available to track, no position estimate can be produced. This is unfortunate, but if one experiences gnss outage in the middle of the ocean, the situation is not as critical as if one is in an archipelago or harbor, why no backup system is really needed in the first place. So the system will only work if the vessel is surrounded by islands or a coastline, but this is also the only scenario in which it would actually be needed. 4.1.3 Underwater Sensor Positioning A passive underwater sensor localization scheme is presented in Paper E. It uses triaxial magnetometers and a friendly vessel with known magnetic characteristics to determine the sensors positions. Simulations indicate that if the vessel is equipped with gnss, a majority of all sensors can be positioned accurately. The simulations also indicate that our positioning scheme is quite insensitive to minor errors in sensor orientation and magnetic signature, when gnss is used to position the vessel throughout the trajectory. The strength of the system is that it solves a challenging positioning problem using a very cheap method. The only thing needed is a friendly vessel that travels through the area. The weakness is that it requires that the magnetic signature of the vessel is quite well known in advance. That means that a different system is needed to estimate the signature if it is not actively produced on board. There is also quite a lot of synchronization needed to ensure that all measurements can be used jointly. The measurements from all sensors and the vessel must be 62 4 Discussion and Future Work synchronized for this to work in practice. In the end quite a lot of measurement management will probably be needed in the estimation system. 4.2 Future Work The indoor positioning system described in this thesis is performing well but is quite limited. For example, it does not provide any information about the building layout to the operational management. The next step now that the dead reckoning is stable, is to incorporate more sensors in the system to produce more reliable and informative localization. A basic extension is to use multiple immus. When only one foot mounted immu is used, some motions like crawling is hard to track. While crawling, the feet have a tendency of wobbling why correctly detecting stand stills become difficult. A knee mounted immu would solve this since the knee is often stationary while crawling. The system would then switch between using the foot mounted sensor and the knee mounted sensor depending on what the data suggests the user is doing. Joint positioning is another option. If multiple users are in the building, measuring the distances between the users will restrict the overall error growth. One way of solving this would be to communicate the estimated user positions and their relative distances to each other to an outside system that use this to produce a joint estimate of all positions. Other sensors like cameras or infrared cameras can be used. Stable landmarks in the environment can be tracked from image to image, giving estimates of how the user is moving. The camera can be used with an immu to produce even more robust position estimates. If an area is revisited, camera images can be used to detect this, a so called loop closure. The position estimate can then be updated with this information. If multiple cameras equipped users are in the building together, one user can get a loop closure from another user’s images. That means that one user does not have to revisit a location for it to be a loop closure. If the images from one user can be connected to the images from a second user, their estimated positions can be connected. This could be useful in the scenario where police officers or soldiers search through a building. The trucks used to get to the scene can be equipped with transponders. These signals could be picked up by sensors worn by the user to aid positioning. Unfortunately, signal multipathing might be an issue in such a system. Such a system would probably also have deteriorating performance with building size. It might be working well in a residential house but not in a skyscraper. The underwater positioning system needs experimental validation but this requires expensive underwater sensors with cables on the sea floor and a large enough steel vessel to be used as target. Since we currently have neither sensors nor vessel, this has been put on hold. Appendix A Quaternion Properties Quaternions were discovered by Hamilton as an extension for the imaginary numbers into three dimensions, Hamilton (1844). Later, the unit quaternion started to be used for angle representation in rotations providing singularity free rotations. For a thorough description of quaternions see Kuipers (1999). A.1 Operations and Properties A quaternion is a four-tuple of real numbers denoted by q = (q0 , q1 , q2 , q3 ). Alternatively it can be described as consisting of a scalar part q0 and the vector q. q0 ! q q0 1 q = = (A.1) q2 q q3 Quaternion multiplication is denoted as and defined as ! ! ! p q p0 q0 − p · q pq= 0 0 = . p q p0 q + q0 p + p × q 65 (A.2) 66 A Quaternion Properties Some quaternions properties are: (A.3) pq,qp v u t 3 X norm(q) = qi2 (A.4) i=0 q −1 q = 0 q !−1 ! q0 = −q (A.5) The unit quaternion used for rotation operations also fulfills norm(q) = 1. A.2 Describing a Rotation using Quaternions The quaternion ! cos δ q= sin δn (A.6) describes a rotation around the vector n with the angle 2δ. There are two ways of depicting a rotation: either the coordinate frame is rotated and the vector is fixed or the vector is rotated and the coordinate frame is fixed. The difference is in the sign of the rotation. In this work the vector will be assumed constant and the coordinate system is rotated. A rotation of a vector v around n can be written as u = q−1 v q (A.7) where v= 0 v ! and u= ! 0 . u (A.8) This is the assumed standard rotation in this work. The resulting rotation is ! q · uq0 − (q0 u − q × u) · q (A.9) v= (q · u)q + q0 (q0 u − q × u) + (q0 u − q × u) × q which simplifies to ! 0 v= . 2(q · u)q + (q02 − q · q)u − 2q0 q × u A.3 (A.10) Rotation Matrix The quaternion rotation (A.10) can be rewritten as a matrix multiplication v = R(q)u (A.11) A.4 67 Quaternion Dynamics where 2 q0 + q12 − q22 − q32 R(q) = 2(q1 q2 − q0 q3 ) 2(q1 q3 + q0 q2 ) 2(q1 q2 + q0 q3 ) q02 − q12 + q22 − q32 2(q2 q3 − q0 q1 ) 2(q1 q3 − q0 q2 ) 2(q2 q3 + q0 q1 ) . 2 2 2 2 q0 − q1 − q2 + q3 (A.12) See Kuipers (1999) for details. A.4 Quaternion Dynamics In the case of the quaternions describing a rotation between a global coordinate system and a local one attached to a moving sensor, the description of the quaternions will contain some dynamics. The full derivation of the quaternion dynamics can be studied in Kuipers (1999) but parts will be recited here. Let the quaternion qlg represent the rotation of the local coordinate system in the global one. The angular velocity of the sensor unit in the local coordinate system l is ωlg which can be written as l ωlg 0 ! ω 0 x = = ωy ω ωz (A.13) qlg q0 ! q q0 1 = = . q2 q q3 (A.14) while qlg is The quaternion derivative can now be written as ! 1 1 −q · ω l = q̇lg = qlg ωlg 2 2 q0 ω + q × ω −(q1 ωx + q2 ωy + q3 ωz ) 1 −ωz ωy q1 0 = 0 −ωx q2 2 q0 ω − ωz −ω ωx 0 q3 y 0 −ωx −ωy −ωz q0 −q1 1 ωx 0 ωz −ωy q1 1 q0 = = 0 ωx q2 2 q3 2 ωy −ωz q3 −q2 ωz ωy −ωx 0 | | {z } l ) S(ωlg −q2 −q3 q0 q1 {z S 0 (qlg ) −q3 ω q2 x ωy −q1 ωz q0 } 68 A Quaternion Properties If the angular velocity ωk is assumed constant over the sampling interval the noise free discrete time model is 1 qk+1 = e 2 S(ωk )T qk (A.15) where a Taylor series expansion gives e 1 2 S(ωk )T = = ∞ X n=0 ∞ X n=0 n 1 2 S(ωk )T n! T 2 n 1 S(ωk )n . n! (A.16) If the sampling time T is short, the expansion can be approximated with the first two terms 1 T e 2 S(ωk )T ≈ I + S(ωk ) (A.17) 2 giving the discrete time model T S(ωk )qk 2 T = qk + S 0 (qk )ωk . 2 qk+1 = qk + (A.18) If the angular velocity noise term νω is included the discrete model becomes qk+1 = qk + T T 0 S (qk )ωk + S 0 (qk )νω,k . 2 2 (A.19) Bibliography K. Abdulrahim, C. Hide, T. Moore, and C. Hill. Aiding low cost inertial navigation with building heading for pedestrian navigation. Journal of Navigation, 64: 219–4233, 2011. P. Aggarwal, D. Thomas, L. Ojeda, and J. Borenstein. Map matching and heuristic elimination of gyro drift for personal navigation systems in GPS-denied conditions. Measurement Science and Technology, 22(2):025205, 2011. M. Angermann and P. Robertson. FootSLAM: Pedestrian simultaneous localization and mapping without extroceptive sensors – hitchiking on human perception and cognition. Proceedings of the IEEE, 100(13):1840–1848, 2012. P. Bahl and V.N Padmanabhan. RADAR: an in-building rf-based user location and tracking system. In Proceedings of the Nineteenth Annual Joint Conference of the IEEE Computer and Communications Societies INFOCOM, 2000. T. Bailey and H. Durrant-Whyte. Simultaneous localization and mapping (SLAM): part II. Robotics & Automation Magazine, IEEE, 13(3):108 – 117, 2006. Y. Bar-Shalom, X. R. Li, and T. Kirubarajan. Estimation with Applications to Tracking and Navigation. Wiley, New York, 2001. S. Beauregard. Omnidirectional pedestrian navigation for first responders. In Proc. of the 4th Workshop on Positioning, Navigation and Communication, WPNC07, Hannover, Germany, 2007. S. Beauregard, Widyawan, and M. Klepal. Indoor PDR performance enhancement using minimal map information and particle filters. In IEEE/ION Position, Location and Navigation Symposium, 2008. Ö. Bebek, M.A. Suster, S. Rajgopal, M.J. Fu, X. Huang, M.C. Çavuşoğlu, D.J. Young, M. Mehregany, A.J. van den Bogert, and C.H. Mastrangelo. Personal navigation via high-resulution gait-corrected inertial measurement units. IEEE Transactions on Instrumentation and Measurement, 59(11):3018–3027, 2010. J. Borenstein and L. Ojeda. Heuristic drift elimination for personnel tracking systems. Journal of Navigation, 63(3):591 – 606, 2010. 69 70 Bibliography J. Callmer. Topics in localization and mapping. Linköping University, Sweden, 2011. Licentiate Thesis No 1489. J. Callmer, K. Granström, J. Nieto, and F. Ramos. Tree of words for visual loop closure detection in urban SLAM. In Proceedings of the 2008 Australasian Conference on Robotics and Automation (ACRA), 2008. J. Callmer, M. Skoglund, and F. Gustafsson. Silent localization of underwater sensors using magnetometers. EURASIP Journal on Advances in Signal Processing, 2010a. J. Callmer, D. Törnqvist, and F. Gustafsson. Probabilistic stand still detection using foot mounted IMU. In Proceedings of the International Conference on Information Fusion (FUSION), 2010b. J. Callmer, D. Törnqvist, H. Svensson, P. Carlbom, and F. Gustafsson. Radar SLAM using visual features. EURASIP Journal on Advances in Signal Processing, 2011. J. Callmer, D. Törnqvist, and F. Gustafsson. Robust heading estimation indoors. IEEE Transactions on Signal Processing, 2013a. Submitted. J. Callmer, D. Törnqvist, and F. Gustafsson. Robust heading estimation indoors using convex optimization. In International Conference on Information Fusion, 2013b. Submitted. J. Callmer, D. Törnqvist, and F. Gustafsson. An inertial navigation framework for indoor positioning with robust heading. IEEE Transactions on Instrumentation and Measurement, 2013c. Submitted. B.J. Dil and P.J.M. Havinga. RSS-based localization with different antenna orientations. In Australasian Telecommunication Networks and Applications Conference, 2010. H. Durrant-Whyte and T. Bailey. Simultaneous localization and mapping (SLAM): part I. Robotics & Automation Magazine, IEEE, 13(2):99 – 110, 2006. The Economist. No jam tomorrow. The Economist Technology Quarterly, 398 (8724):20–21, 2011. The Economist. Finding the way inside. The Economist Technology Quarterly Q4, 2012. E. Erkstam and E. Tjernqvist. Performance enhancement of bearing navigation to known radio beacons. Master’s thesis, Linköping University, 2012. R. Feliz, E. Zalama, and J. G. Garcia-Bermejo. Pedestrian tracking using inertial sensors. Journal of Physical Agents, 3(1):35–43, 2009. E. Foxlin. Pedestrian tracking with shoe-mounted inertial sensors. IEEE Computer Graphics and Applications, 25(6):38–46, 2005. Bibliography 71 S. Godha, G. Lachapelle, and M. E. Cannon. Integrated GPS/INS system for pedestrian navigation in signal degraded environment. In Proc. of ION GNSS, 2006. K. Granström, J. Callmer, F. Ramos, and J. Nieto. Learning to detect loop closure from range data. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2009. A. Grant, P. Williams, N. Ward, and S. Basker. GPS jamming and the impact on maritime navigation. Journal of Navigation, 62(2):173–187, 2009. S. Grzonka, F. Dijoux, A. Karwath, and W. Burgard. Mapping indoor environments based on human activity. In Proc. IEEE International Conference on Robotics and Automation (ICRA), Anchorage, Alaska, 2010. Y. Gu, A. Lo, and I. Niemegeers. A survey of indoor positioning systems for wireless personal networks. IEEE COMMUNICATIONS SURVEYS and TUTORIALS, 11(1):13–32, 2009. F. Gustafsson. Statistical Sensor Fusion. Studentlitteratur, 2010. S.W.R. Hamilton. On quaternions; or on a new system of imaginaries in algebra. Philosophical Magazine, xxv:10–13, 1844. G. Hanning, N. Forslöw, P.-E. Forssén, E. Ringaby, D. Törnqvist, and J. Callmer. Stabilizing cell phone video using inertial measurement sensors. In In Proceedings of the IEEE International Workshop on Mobile Vision (IWMV11), 2011. V. Honkavirta, T. Perala, S. Ali-Loytty, and R. Piché. A comparative survey of WLAN location fingerprinting methods. In Workshop on Positioning, Navigation and Communication (WPNC 2009), 2009. A.H. Jazwinski. Stochastic Processes and Filtering Theory. Academic Press, 1970. A.R. Jiménez, F. Seco, J.C. Prieto, and J. Guevara. Indoor pedestrian navigation using an ins/ekf framework for yaw drift reduction and a foot-mounted imu. In Positioning Navigation and Communication (WPNC), 2010 7th Workshop on, pages 135 –143, march 2010a. A.R. Jiménez, F. Seco, J.C. Prieto, and J. Guevara. Indoor pedestrian navigation using an ins/ekf framework for yaw drift reduction and a foot-mounted imu. In 7th Workshop on Positioning, Navigation and Communication, WPNC, 2010b. S. Kaisera, M. Khidera, and P. Robertson. A pedestrian navigation system using a map-based angular motion model for indoor and outdoor environments. Journal of Location Based Services, 7(1), 2013. R.E. Kalman. A new approach to linear filtering and prediction problems. Trans. ASME J Basic Engr., 82:35–45, 1960. B. Krach and P. Robertson. Cascaded estimation architecture for integration of foot-mounted inertial sensors. In Proc of the IEEE/ION Position, Location and Navigation Symposium, 2008. 72 Bibliography J.B. Kuipers. Quaternions and Rotation Sequences. Princeton University Press, 1999. B. Li, J. Salter, A.G. Dempster, and C. Rizos. Indoor positioning techniques based on wireless LAN. Technical report, School of Surveying and Spatial Information Systems, UNSW, Sydney, Australia, 2006. F. Lindsten, J. Callmer, H. Ohlsson, D. Törnqvist, T. B. Schön, and F. Gustafsson. Geo-referencing for UAV navigation using environmental classification. In Proceedings of 2010 International Conference on Robotics and Automation (ICRA), 2010. C. Lundquist, P. Skoglar, F. Gustafsson, D. Törnqvist, and J. Callmer. Method and device for indoor positioning. US Patent Application 20120203453, August 8, 2012. K. Muthukrishnan, M.E. Lijding, and P.J.M. Havinga. Towards smart surroundings: Enabling techniques and technologies for localization. In Proceedings of the International Workshop on Location-and Context-Awareness, 2005. J.-O. Nilsson, I. Skog, P. Händel, and K. Hari. Foot-mounted INS for everybody an open-source embedded implementation. In Position Location and Navigation Symposium (PLANS), 2012 IEEE/ION, pages 140–145, 2012. L. Ojeda and J. Borenstein. Non-GPS navigation for security personnel and first responders. Journal of Navigation, 60(3):391–407, 2007. J. Rantakokko, J. Rydell, P. Strömbäck, P. Händel, J. Callmer, D. Törnqvist, F. Gustafsson, M. Jobs, and M. Grudén. Accurate and reliable soldier and first responder indoor positioning: multisensor systems and cooperative localization. Wireless Communications, IEEE, 18(2):10–18, 2011. V. Renaudin, O. Yalak, P. Tomé, and B. Merminod. Indoor navigation of emergency agents. European Journal of Navigation, 5(3), 2007. P. Robertson, M. Angermann, and B. Krach. Simultaneous localization and mapping for pedestrians using only foot-mounted inertial sensors. In Proceedings of the 11th international conference on Ubiquitous computing, Ubicomp ’09, pages 93–96, 2009. T. Roos, P. Myllymäki, H. Tirri, P. Misikangas, and J. Sievänen. A probabilistic approach to WLAN user location estimation. International Journal of Wireless Information Networks, 9(3), 2002. J. Rydell and T. Andersson. Camera-based navigation, mapping and 3d model generation for indoor environments. In Proceedings of the 2010 International Technical Meeting of The Institute of Navigation, 2010. J. Seitz, T. Vaupel, J. Jahn, S. Meyer, J.G. Boronat, and J. Thielecke. A hidden markov model for urban navigation based on fingerprinting and pedestrian dead reckoning. In Proceedings of the International Conference on Information Fusion (FUSION), 2010. Bibliography 73 I. Skog. Low-cost Navigation Systems. PhD thesis, KTH, Stockholm, Sweden, 2009. I. Skog, P. Händel, J.-O. Nilsson, and J. Rantakokko. Zero-velocity detection – an algorithm evaluation. IEEE Transactions on Biomedical Engineering, 57, 2010. N. Wahlström, J. Callmer, and F. Gustafsson. Magnetometers for tracking metallic targets. In Proceedings of the International Conference on Information Fusion (FUSION), 2010. N. Wahlström, J. Callmer, and F. Gustafsson. Single target tracking using vector magnetometers. In Proceedings of the International Conference on Acoustics, Speech and Signal Processing (ICASSP), 2011. H. Wang, H. Lenz, A. Szabo, J. Bamberger, and U.D. Hanebeck. WLAN-based pedestrian tracking using particle filters and low-cost MEMS sensors. In 4th workshop in positioning, navigation and communication (WPNC), 2007. Widyawan, M. Klepal, and D. Pesch. A bayesian approach for RF-based indoor localisation. In 4th International Symposium on Wireless Communication Systems ISWCS, 2007. Widyawan, M. Klepal, and S. Beauregard. A backtracking particle filter for fusing building plans with PDR displacement estimates. In Positioning, Navigation and Communication, 2008. WPNC 2008. 5th Workshop on, pages 207 –212, march 2008. O. Woodman and R. Harle. Pedestrian localisation for indoor environments. In Proceedings of the 10th international conference on Ubiquitous computing (UbiComp), 2008. O. Woodman and R. Harle. Rf-based initialisation for inertial pedestrian tracking. In Proceedings of the 7th International Conference on Pervasive Computing, 2009. H. Wymeersch and M.Z. Win J. Lien. Cooperative localization in wireless networks. Proceedings of the IEEE, 97(2):427–450, 2009. M. Youssef and A. Agrawala. The horus WLAN location determination system. In Proceedings of the 3rd international conference on Mobile systems, applications and services (MobiSys), 2005. D. Zachariah and M. Jansson. Fusing visual tags and inertial information for indoor navigation. In IEEE/ION Position Location and Navigation Symposium (PLANS), 2012. PhD Dissertations Division of Automatic Control Linköping University M. Millnert: Identification and control of systems subject to abrupt changes. Thesis No. 82, 1982. ISBN 91-7372-542-0. A. J. M. van Overbeek: On-line structure selection for the identification of multivariable systems. Thesis No. 86, 1982. ISBN 91-7372-586-2. B. Bengtsson: On some control problems for queues. Thesis No. 87, 1982. ISBN 91-7372593-5. S. Ljung: Fast algorithms for integral equations and least squares identification problems. Thesis No. 93, 1983. ISBN 91-7372-641-9. H. Jonson: A Newton method for solving non-linear optimal control problems with general constraints. Thesis No. 104, 1983. ISBN 91-7372-718-0. E. Trulsson: Adaptive control based on explicit criterion minimization. Thesis No. 106, 1983. ISBN 91-7372-728-8. K. Nordström: Uncertainty, robustness and sensitivity reduction in the design of single input control systems. Thesis No. 162, 1987. ISBN 91-7870-170-8. B. Wahlberg: On the identification and approximation of linear systems. Thesis No. 163, 1987. ISBN 91-7870-175-9. S. Gunnarsson: Frequency domain aspects of modeling and control in adaptive systems. Thesis No. 194, 1988. ISBN 91-7870-380-8. A. Isaksson: On system identification in one and two dimensions with signal processing applications. Thesis No. 196, 1988. ISBN 91-7870-383-2. M. Viberg: Subspace fitting concepts in sensor array processing. Thesis No. 217, 1989. ISBN 91-7870-529-0. K. Forsman: Constructive commutative algebra in nonlinear control theory. Thesis No. 261, 1991. ISBN 91-7870-827-3. F. Gustafsson: Estimation of discrete parameters in linear systems. Thesis No. 271, 1992. ISBN 91-7870-876-1. P. Nagy: Tools for knowledge-based signal processing with applications to system identification. Thesis No. 280, 1992. ISBN 91-7870-962-8. T. Svensson: Mathematical tools and software for analysis and design of nonlinear control systems. Thesis No. 285, 1992. ISBN 91-7870-989-X. S. Andersson: On dimension reduction in sensor array signal processing. Thesis No. 290, 1992. ISBN 91-7871-015-4. H. Hjalmarsson: Aspects on incomplete modeling in system identification. Thesis No. 298, 1993. ISBN 91-7871-070-7. I. Klein: Automatic synthesis of sequential control schemes. Thesis No. 305, 1993. ISBN 91-7871-090-1. J.-E. Strömberg: A mode switching modelling philosophy. Thesis No. 353, 1994. ISBN 917871-430-3. K. Wang Chen: Transformation and symbolic calculations in filtering and control. Thesis No. 361, 1994. ISBN 91-7871-467-2. T. McKelvey: Identification of state-space models from time and frequency data. Thesis No. 380, 1995. ISBN 91-7871-531-8. J. Sjöberg: Non-linear system identification with neural networks. Thesis No. 381, 1995. ISBN 91-7871-534-2. R. Germundsson: Symbolic systems – theory, computation and applications. Thesis No. 389, 1995. ISBN 91-7871-578-4. P. Pucar: Modeling and segmentation using multiple models. Thesis No. 405, 1995. ISBN 91-7871-627-6. H. Fortell: Algebraic approaches to normal forms and zero dynamics. Thesis No. 407, 1995. ISBN 91-7871-629-2. A. Helmersson: Methods for robust gain scheduling. Thesis No. 406, 1995. ISBN 91-7871628-4. P. Lindskog: Methods, algorithms and tools for system identification based on prior knowledge. Thesis No. 436, 1996. ISBN 91-7871-424-8. J. Gunnarsson: Symbolic methods and tools for discrete event dynamic systems. Thesis No. 477, 1997. ISBN 91-7871-917-8. M. Jirstrand: Constructive methods for inequality constraints in control. Thesis No. 527, 1998. ISBN 91-7219-187-2. U. Forssell: Closed-loop identification: Methods, theory, and applications. Thesis No. 566, 1999. ISBN 91-7219-432-4. A. Stenman: Model on demand: Algorithms, analysis and applications. Thesis No. 571, 1999. ISBN 91-7219-450-2. N. Bergman: Recursive Bayesian estimation: Navigation and tracking applications. Thesis No. 579, 1999. ISBN 91-7219-473-1. K. Edström: Switched bond graphs: Simulation and analysis. Thesis No. 586, 1999. ISBN 91-7219-493-6. M. Larsson: Behavioral and structural model based approaches to discrete diagnosis. Thesis No. 608, 1999. ISBN 91-7219-615-5. F. Gunnarsson: Power control in cellular radio systems: Analysis, design and estimation. Thesis No. 623, 2000. ISBN 91-7219-689-0. V. Einarsson: Model checking methods for mode switching systems. Thesis No. 652, 2000. ISBN 91-7219-836-2. M. Norrlöf: Iterative learning control: Analysis, design, and experiments. Thesis No. 653, 2000. ISBN 91-7219-837-0. F. Tjärnström: Variance expressions and model reduction in system identification. Thesis No. 730, 2002. ISBN 91-7373-253-2. J. Löfberg: Minimax approaches to robust model predictive control. Thesis No. 812, 2003. ISBN 91-7373-622-8. J. Roll: Local and piecewise affine approaches to system identification. Thesis No. 802, 2003. ISBN 91-7373-608-2. J. Elbornsson: Analysis, estimation and compensation of mismatch effects in A/D converters. Thesis No. 811, 2003. ISBN 91-7373-621-X. O. Härkegård: Backstepping and control allocation with applications to flight control. Thesis No. 820, 2003. ISBN 91-7373-647-3. R. Wallin: Optimization algorithms for system analysis and identification. Thesis No. 919, 2004. ISBN 91-85297-19-4. D. Lindgren: Projection methods for classification and identification. Thesis No. 915, 2005. ISBN 91-85297-06-2. R. Karlsson: Particle Filtering for Positioning and Tracking Applications. Thesis No. 924, 2005. ISBN 91-85297-34-8. J. Jansson: Collision Avoidance Theory with Applications to Automotive Collision Mitigation. Thesis No. 950, 2005. ISBN 91-85299-45-6. E. Geijer Lundin: Uplink Load in CDMA Cellular Radio Systems. Thesis No. 977, 2005. ISBN 91-85457-49-3. M. Enqvist: Linear Models of Nonlinear Systems. Thesis No. 985, 2005. ISBN 91-8545764-7. T. B. Schön: Estimation of Nonlinear Dynamic Systems — Theory and Applications. Thesis No. 998, 2006. ISBN 91-85497-03-7. I. Lind: Regressor and Structure Selection — Uses of ANOVA in System Identification. Thesis No. 1012, 2006. ISBN 91-85523-98-4. J. Gillberg: Frequency Domain Identification of Continuous-Time Systems Reconstruction and Robustness. Thesis No. 1031, 2006. ISBN 91-85523-34-8. M. Gerdin: Identification and Estimation for Models Described by Differential-Algebraic Equations. Thesis No. 1046, 2006. ISBN 91-85643-87-4. C. Grönwall: Ground Object Recognition using Laser Radar Data – Geometric Fitting, Performance Analysis, and Applications. Thesis No. 1055, 2006. ISBN 91-85643-53-X. A. Eidehall: Tracking and threat assessment for automotive collision avoidance. Thesis No. 1066, 2007. ISBN 91-85643-10-6. F. Eng: Non-Uniform Sampling in Statistical Signal Processing. Thesis No. 1082, 2007. ISBN 978-91-85715-49-7. E. Wernholt: Multivariable Frequency-Domain Identification of Industrial Robots. Thesis No. 1138, 2007. ISBN 978-91-85895-72-4. D. Axehill: Integer Quadratic Programming for Control and Communication. Thesis No. 1158, 2008. ISBN 978-91-85523-03-0. G. Hendeby: Performance and Implementation Aspects of Nonlinear Filtering. Thesis No. 1161, 2008. ISBN 978-91-7393-979-9. J. Sjöberg: Optimal Control and Model Reduction of Nonlinear DAE Models. Thesis No. 1166, 2008. ISBN 978-91-7393-964-5. D. Törnqvist: Estimation and Detection with Applications to Navigation. Thesis No. 1216, 2008. ISBN 978-91-7393-785-6. P-J. Nordlund: Efficient Estimation and Detection Methods for Airborne Applications. Thesis No. 1231, 2008. ISBN 978-91-7393-720-7. H. Tidefelt: Differential-algebraic equations and matrix-valued singular perturbation. Thesis No. 1292, 2009. ISBN 978-91-7393-479-4. H. Ohlsson: Regularization for Sparseness and Smoothness — Applications in System Identification and Signal Processing. Thesis No. 1351, 2010. ISBN 978-91-7393-287-5. S. Moberg: Modeling and Control of Flexible Manipulators. Thesis No. 1349, 2010. ISBN 978-91-7393-289-9. J. Wallén: Estimation-based iterative learning control. Thesis No. 1358, 2011. ISBN 97891-7393-255-4. J. Hol: Sensor Fusion and Calibration of Inertial Sensors, Vision, Ultra-Wideband and GPS. Thesis No. 1368, 2011. ISBN 978-91-7393-197-7. D. Ankelhed: On the Design of Low Order H-infinity Controllers. Thesis No. 1371, 2011. ISBN 978-91-7393-157-1. C. Lundquist: Sensor Fusion for Automotive Applications. Thesis No. 1409, 2011. ISBN 978-91-7393-023-9. P. Skoglar: Tracking and Planning for Surveillance Applications. Thesis No. 1432, 2012. ISBN 978-91-7519-941-2. K. Granström: Extended target tracking using PHD filters. Thesis No. 1476, 2012. ISBN 978-91-7519-796-8. C. Lyzell: Structural Reformulations in System Identification. Thesis No. 1475, 2012. ISBN 978-91-7519-800-2.

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

### Related manuals

Download PDF

advertisement