Enhanced Stance Phase Detection and Extended Kalman Filtering for Strapdown Pedestrian Dead Reckoning Msc in Mobile and Ubiquito
Total Page:16
File Type:pdf, Size:1020Kb
Enhanced Stance Phase Detection and Extended Kalman Filtering for Strapdown Pedestrian Dead Reckoning MSc in Mobile and Ubiquitous Computing Poorna Talkad Sukumar September 2010 ABSTRACT Pedestrian Dead Reckoning (PDR) using shoe-mounted inertial sensors is a self-contained navigation technique which could address the needs of emergency responders. An improved PDR system has been implemented in this thesis using an extended Kalman filter (EKF) formulation with periodic zero-velocity updates (ZUPTs). The system has been evaluated for a variety of data sets recorded using the Xsens MTx inertial measurement unit. The PDR system was implemented both with and without using the Xsens's orientation to also determine if we need to use the expensive Xsens device with its `black box' algorithm or if we can use less expensive inertial sensors with our own algorithms. Different stance phase detection methods have also been implemented in this thesis. Contents Table of Contents v List of Figures vii 1 Introduction 1 2 Background and Experimental Setup 4 2.1 Related Work ................................... 4 2.2 Strapdown Inertial Navigation System ..................... 5 2.3 The Extended Kalman Filter ........................... 7 2.4 Data sets used in this thesis ........................... 10 2.4.1 Long corridor ............................... 10 2.4.2 Data collected in University of Twente ................. 11 2.4.3 Biba Workshop .............................. 13 3 Stance Phase Detection and Basic EKF/ZUPTs Pedestrian Tracking using Xsens orientation 15 3.1 ZUPTs and Benefits of using EKF ....................... 16 3.2 INS and EKF Formulation using Xsens orientation .............. 20 3.2.1 Strapdown INS mechanization process ................. 20 3.2.2 The Extended Kalman Filter ...................... 23 3.3 Stance Phase Detection .............................. 26 3.3.1 Using 3D angular rate signal ....................... 28 3.3.2 Using 3D acceleration, 3D angular rate and local acceleration variance 29 3.3.3 Gait phase detection using Hidden Markov Model ........... 33 3.4 Results of the three stance phase detection algorithms ............. 38 3.4.1 Plots with highlighted zero-velocity periods .............. 38 3.4.2 Plots of 2D traces and altitude ..................... 42 3.4.3 Discussion ................................. 49 3.5 Results of the EKF/ZUPTs formulation using Xsens orientation ....... 52 3.6 Filter Tuning ................................... 60 3.7 Summary ..................................... 60 v CONTENTS vi 4 EKF tracking using raw IMU measurements 62 4.1 ZUPTs and ZARUs ................................ 63 4.1.1 Strapdown INS mechanization process ................. 63 4.1.2 The Extended Kalman Filter ...................... 67 4.2 Heuristic Drift Reduction (HDR) ........................ 70 4.3 Orientation Correction using Accelerometer signal ............... 72 4.4 Results ....................................... 76 4.5 Filter Tuning ................................... 88 4.6 Summary ..................................... 89 5 Conclusions 91 5.1 Contributions ................................... 91 5.2 Additional Observations ............................. 92 5.3 Future work .................................... 93 Bibliography 94 A MATLAB Code 98 List of Figures 1.1 Xsens MTx IMU firmly attached to the shoe ................. 3 2.1 Basic principle of an inertial navigation system ................. 6 2.2 Basic principle of an inertial navigation processor ............... 7 2.3 Estimated and actual trajectories for an extended Kalman filter ....... 9 2.4 Path travelled along corridors in InfoLab21 ................... 11 2.5 Ubisense plots for T1 and T2 .......................... 12 2.6 Ubisense plots for T3 and T4 .......................... 13 2.7 Path travelled in Biba workshop ......................... 14 3.1 2D velocity with and without ZUPTs ...................... 17 3.2 Basic principle of an integrated navigation system ............... 18 3.3 Position drift corrected by the EKF during ZUPTs. .............. 19 3.4 Strapdown INS mechanization process ..................... 21 3.5 Accelerometer signal demonstrating the different gait phases. ......... 27 3.6 Gait Cycle ..................................... 27 3.7 Results of the multi-condition stance phase detection algorithm ....... 32 3.8 State transition diagram ............................. 35 3.9 3D acceleration, angular rate and velocity signals highlighting the zero-velocity periods detected by algorithm 1 ......................... 39 3.10 3D acceleration, angular rate and velocity signals highlighting the zero-velocity periods detected by algorithm 2 ......................... 40 3.11 3D acceleration, angular rate and velocity signals highlighting the zero-velocity periods detected by algorithm 3 ......................... 40 3.12 3D velocity plots obtained using algorithm 1 (solid red line) and algorithm 2 (dotted black line) ................................ 41 3.13 Plots of 2D traces and altitude for L1 ...................... 43 3.14 Plots of 2D traces and altitude for L2 ...................... 43 3.15 Plots of 2D traces and altitude for L3 ...................... 44 3.16 Plots of 2D traces and altitude for L4 ...................... 44 3.17 Plots of 2D traces and altitude for T1 ...................... 45 3.18 Plots of 2D traces and altitude for T2 ...................... 46 vii LIST OF FIGURES viii 3.19 Plots of 2D traces and altitude for T3 ...................... 47 3.20 Plots of 2D traces and altitude for T4 ...................... 48 3.21 Plots of 2D traces and altitude for Biba ..................... 49 3.22 Plots of 2D traces and altitude, L1 ....................... 53 3.23 Plots of 2D traces and altitude, L2 ....................... 54 3.24 Plots of 2D traces and altitude, L3 ....................... 54 3.25 Plots of 2D traces and altitude, L4 ....................... 55 3.26 Plots of 2D traces and altitude, T1 ....................... 55 3.27 Plots of 2D traces and altitude, T2 ....................... 56 3.28 Plots of 2D traces and altitude, T3 ....................... 56 3.29 Plots of 2D traces and altitude, T4 ....................... 57 3.30 Plots of 2D traces and altitude, Biba ...................... 57 4.1 Strapdown INS mechanization process ..................... 64 4.2 Plots of 2D traces, L3 .............................. 79 4.3 Plots of altitudes, L3 ............................... 80 4.4 Plots of 2D traces, L4 .............................. 81 4.5 Plots of altitudes, L4 ............................... 82 4.6 Plots of 2D traces and altitudes, T1 ....................... 83 4.7 Plots of 2D traces and altitudes, T2 ....................... 84 4.8 Plots of 2D traces and altitudes, T3 ....................... 85 4.9 Plots of 2D traces and altitudes, T4 ....................... 86 4.10 Plots of 2D traces and altitudes, Biba ...................... 87 Chapter 1 Introduction Location and navigation support is essential in emergency response scenarios. GPS is not available indoors and all the current indoor navigation systems require installation of some infrastructure or an instrumented, marked or pre-mapped environment. Hence, they are impractical for use by emergency responders such as firefighters. Emergency responders must be able to keep track of their locations and reach safety quickly under dangerous conditions such as smoke and power failure [1{4]. Therefore, there is a need for a self-contained navigation system which will work reliably in any indoor or outdoor setting. Pedestrian Dead Reckoning (PDR) using shoe-mounted inertial sensors could address the needs of emergency responders. Dead reckoning is a funda- mental method which consists of adding a moving body's measured or calculated change in position to its previous position to obtain its current position. Inertial sensors usually com- prise of three accelerometers and three gyroscopes measuring the accelerations and angular rates in the x, y and z directions respectively. These inertial sensor outputs are integrated to obtain the velocity, position and attitude which are used to determine the current position with respect to a known reference using the dead reckoning method. But unaided inertial navigation systems suffer from drift. The small errors in the ac- 1 2 celeration are integrated into increasingly larger errors in the velocity which are integrated into even larger errors in position. Therefore, the position has to be periodically corrected using techniques such as zero velocity updates (ZUPTs). In pedestrian navigation, the foot alternates between a moving swing phase and a stationary stance phase. Zero velocity up- dates (ZUPTs) are applied during the stance phases since the velocity during those periods should theoretically be zero. ZUPTs limit the growth of the velocity errors in the inertial navigation system. Previous work on PDR include using shoe-mounted inertial sensors and applying periodic ZUPTs [1, 3{8]. An extended Kalman filter (EKF) is also used in some papers to combine the inertial navigation velocity solution with the zero velocity measurements [1,5,6,8]. Using an EKF not only helps in estimating the velocity errors, but the position and attitude errors, and accelerometer and gyro biases are also estimated. But most of the previous papers mentioned above have been able to achieve good tracking only for a few minutes. Also, the inertial sensors used and the environments in which the traces were collected differ among the different implementations. This thesis extends previous work to build