Published on June 14, 2016
1. Foot Mounted Pedestrian Navigation Systems Final Report April 30, 2015 Submitted by Sailesh Kumar G, Shyam Sundar S B.Tech Electronics and Communication Engineering Department of Electrical Engineering Project Mentor Amit K Gupta GT Silicon Pvt. Ltd- Internal Supervisor Prof. Vijay Kumar Chakka Department of Electrical Engineering Shiv Nadar University
2. 2 Table of contents: 1. Abstract 2. Introduction 3. Literature Survey 4. Inertial Navigation System (INS) a) Coordinate systems and frames b) Sensors c) Accelerometer d) Gyroscope e) Block diagram of Inertial Navigation system f) Dead Reckoning g) Errors in INS h) Errors in IMU 5. Mathematical model a) Select sensors and mounting: b) Choose a state space model c) Define a Measurement model d) Define a Process Model e) Defining Pseudo measurements f) Kalman Filter 6. Hardware a) High level System Description 7. Software a) Communication Protocol i) Types of Commands ii) Command Structure iii) Command Reception/Transmission b) IMU Interface c) I2C protocol d) Data storage mechanism e) Calibration Compensation i. Sensor Model ii. Calibration matrix iii. Fusion iv. Bias compensation 8. Implementation: a) Varying number of IMUs 9. Experimental Study a) Design of the experiment b) Results c) Observations
3. 3 10. Conclusion 11. References
4. 4 1.Abstract: Positioningis a fundamental component of human life to make meaningful interpretations of the environment. Without knowledge of position, human beings are like machines and have very limited capabilities to interact with the environment. Even machines in today’s world can be made smarter if positioning information is made available to them.Indoor positioning of pedestrians is the broad area considered in this thesis.A foot mounted pedestrian tracking device has been studied for this purpose. Systems which utilize foot mounted inertial navigation system has been in the literature for more than two decades. However very few real time implementations have been possible. The purpose of this thesis is to benchmark and improve the performance of one such implementation. The benchmarking of the device was carried out by incorporating varying number of sensors on board.For this purpose the embedded software on the device was modified and the calibration compensation was carried out correspondingly using a well-defined methodology. The height error is one of the most important metrics in indoor navigation. The device currently suffers from a height error of roughly 2.5%. To minimise this error a pressure sensor has been interfaced with the on-board microcontroller. Experimental study was then carried out where the device was tested under different conditions by taking into account the parameters which affect the tracking performance. Prof. Vijay Kumar Chakka Department of Electrical Engineering 2.Introduction:
5. 5 Human beings constantly need to acquire knowledge about their environment in order to interact with their surroundings. In the absence of knowledge of location it would limit our interaction capabilities with the physical world. Today machines face this problem and therefore require supervision of humans in order to make meaningful inferences. By integrating machines with the knowledge of location they can be made to autonomously interact with each other thus improving their efficiency. For human beings, Positioning provides basic information that can be integrated into a diverse set of fields. Global Positioning System (GPS) is the widely used technical standard for outdoor positioning. Any person with an access to a system such as a mobile phone integrated with GPS receiver is provided positioning information at free of cost with a horizontal accuracy of 3.5 metres. However atmospheric effects, sky blockage and receiver quality affects the free LOS (Line Of Sight) communication between the transmitter and the receiver. This situation degrades even more under circumstances such as moving underground, inside a tunnel or even inside buildings. From this arises a need to deploy GPS integrated with other systems to improve the accuracy for usage in indoor positioning. These systems can be categorised based on the nature of information sources. The data from passive infrastructure based sources such as RFID (Radio Frequency Identification) tags, WLAN are one of those. Also information from motion constraints such as building maps and pedestrian motion models can be used. However the major problems with these systems are their cost in deployment and maintenance. In case of emergency situations such as fire, the robustness of these systems fails destroying their purpose. Designing a robust system which does not depend on infrastructure or prior knowledge about the environment would satisfy a large scale of applications at a lower cost. The advancements in MEMS Technology have paved the way for low cost sensors which can be integrated into an Inertial Navigation System (INS). These sensors are also less power craving and occupy far lesser size. The INSs apart from providing the possibility to integrate with GPS in fact provide a suitable alternative to the same. Inertial Navigation Systems are self-contained systems that can operate in harsh radio environments without pre-installed infrastructure and prior knowledge of the environment. Conventional Inertial Navigation Systems calculate the change in position through Dead Reckoning. In Pedestrian Navigation this means measuring the length of the steps and the direction of the pedestrian given the initial position and heading is known. They calculate double integration of acceleration measurements and single integration of gyroscope measurements which are noisy and therefore suffer from huge accumulation of position and heading errors with time. This is because the new estimates of position and heading rely on previous estimates of the same. The use of a complementary information source can mitigate this problem by providing absolute measurements of position and orientation. However for emergency response situations the system has to be completely self-sufficient at least for a short period of time. Zero velocity detection (ZUPT) is one of the popular methods in literature to minimise the errors encountered in position and heading estimates. By exploiting the standstill phase in the walking motion of the pedestrian one can detect that the linear velocity of the foot at that instant is zero. This essentially means the sensor is stationary at standstill position and therefore any estimated velocity or rotation is interpreted as error and is used to recalibrate the estimated velocity to zero. ZUPT allows the device to estimate position only when the device detects a step and thus the error encountered is directly proportional to the number of steps measured. Without ZUPT the drift error would have grown cubic with time. It increases the accuracy of conventional inertial navigation systems thus making it suitable for a large number of promising applications in the area of indoor navigation in GPS denied environments. The targeted applications include localisation in emergency response situations such as fire fighting missions where prior knowledge of the environment or deploying costly infrastructure is not possible. This technology can also be integrated into other research areas such as Biomechanics and Biomedicine where a person’s walking patterns can be studied to connect it to various medical symptoms. Parkinson’s disease for example studies the stability of the foot. Other fields include Sports medicine where gait analysis of the athlete is performed and Behavioural Sciences where a person’s movement patterns are observed.
6. 6 This thesis is divided into the following sections. Chapter 3 provides a summary of the recent work in the area of Pedestrian navigation few of which served as a reference in writing this thesis. Chapter 4 describes the Inertial navigation system used in Osmium MIMU22BT with emphasis on the sensors and the equations used. Chapter 5 derives the mathematical model using which the algorithm written in the Embedded software operates. Chapter 6 outlines the hardware through a high level system description. Chapter 7 explains the integral part of the Embedded software in the device which are the IMUs. Chapter 8 gives a detailed description on the implementations carried out through writing Embedded Software code. Chapter 9 describes the experimental study carried out to study and benchmark the performance of the device at different conditions. Finally, Chapter 10 concludes this thesis with the evaluation of the objectives set. 3. Literature Survey: Mathiassen et al. (2010) designed a low cost Inertial Navigation System in integration with GPS for positioning. This system was able to track the position when GPS signals were lost. The IMU was supported by external sensors such as Barometer and Magnetometer. The simulations showed good results when the
7. 7 gyroscope output was filtered. Position accuracy with a standard deviation of 8.5 m for 60s time was achieved. But in the real world tests, the same system gave an error of 65 m for the same time. This is the reason Magnetometers are not used in Osmium MIMU22BT. Even though position errors tend to grow out of bounds and magnetometers can be used to bind the errors, they are not used so because during measurements they suffer from errors which are difficult to model. One such example is the presence of any ferromagnetic material near the pedestrian. As Pedestrian dead reckoning requires estimation of Step length, studying and categorizing human activity is essential before designing an algorithm for step detection. The variation in dynamics of movements such as walking, running, loitering and the type of mounting affects the performance of the device. The drift error measured as is less than 1% for many foot mounted systems in the literature compared to mounting on other body parts which have more than 1% at least in all cases. Nilsson et al. (2010) demonstrated that ZUPT aided foot mounted INS was able to achieve drift error in the range of (0.1%-0.44%). The errors on the lesser side utilized information from external sources. Problems are encountered in ZUPT algorithm when either the dynamics of the movement or speed or when the pedestrian is walking on different surfaces. Skog et al. (2010) worked towards designing different Zero velocity detectors and he achieved a position error of 0.14% in the acceleration magnitude detector and angular rate energy detector. Both of these are used in the implementation of Open-shoe modules in 2012. Wan and Foxlin (2010) used Intersense’s Navshoe TM to design a foot mounted system not dependent on external sources giving a position error of 0.27% of the total distance. In spite of the performance advantage in foot mounted systems these required specific arrangements to integrate the IMUs into the sole of a shoe. Therefore Nilsson et al. (2012) presented the first open source real time embedded implementation of a ZUPT aided foot mounted INS. By utilizing off-the-shelf components and assembly methods the cost of a single unit was 800$ with a position accuracy of (0.2-1%). But it could only be accessible using a USB interface. They also manufactured intelligent shoes to integrate the module close to the heel. Nilsson et al. (2014) further improved the Open Shoe module by adding a Bluetooth interface and flash memory to it and further reduced its cost to around 300$ while maintaining the Open source nature of the module. Skog et al. (2014) described a low cost calibration procedure by using a platonic solid icosahedron. This novel method replaces the need for a high cost mechanical platform for calibrating the IMUs with a low cost solution in which a trade-off for precision for reduction is cost is achieved. But the tradeoff in precision is sufficient to compensate for all the observable errors in the device. Angermann et al. (2010) presented a reference dataset for analysis and verification of different algorithms on foot mounted inertial sensors. It also included data collection methodology which can be applied to a large range of indoor applications. The reference data phase would help us understand the performance of algorithms in Pedestrian dead reckoning specifically during the standstill phase of the motion 4. Inertial Navigation System: The Inertial navigation system (INS) acts as the backbone of the device Osmium MIMU22BT. The ZUPT algorithm aided kalman filter takes measurements from this system.This chapter explains the working of the INS implemented inside the device. It describes the reference frames used for measurement, sensors used in the system, block diagram of the complete system, errors inside the system and the errors in an IMU.
8. 8 Coordinate systems and frames: The commonly used coordinate systems in inertial navigation are Cartesian, Curvilinear and spherical coordinate systems. The commonly used frames include Sensor’s frame, Local tangent plane and Earth centered Earth fixed frame.Local tangent plane refers to a frame fixed at a tangent point on the earth reference to which Zero position and heading is defined. This necessarily means that with reference to latitude and longitude values zero position and heading is defined.In addition to this, local gravity needs to be taken into account during calibration. In Earth centered earth fixed frame, position is defined relative to the center of the earth i.e a line passing from the Earth’s center through the equator. Sensor’s frame refers to the frame which is moving with the sensor and hence relative to the sensitivity axes of the sensor. In Osmium MIMU22BT the coordinate frame used is Local tangent plane. This essentially means that the rotation of earth is neglected as the module is designed for pedestrian tracking for a short duration. The coordinate system used is the Cartesian coordinate system. Sensors: The sensors in Osmium MIMU22BT are calledMEMS based Inertial Measurement Units (IMU). The improvement in MEMS technology based sensors has revolutionized the field of pedestrian tracking due to less cost, reduced power consumption andminiaturization. The IMU used here is Invensense’s MPU9150. Generally an IMU contains Accelerometers and Gyroscopes in three orthogonal axes. But MPU 9150 has a magnetometer in addition and therefore has nine degrees of freedom. However for the purpose of indoor tracking magnetometer is not currently used as it is susceptible to disturbances from ferrous objects around any building. The main utility of an IMU is to capture force and rotations in the three orthogonal sensitivity axes. MIMU22BT is equipped with multiple IMUs which provide an opportunity to apply sensor fusion techniques. Currently simple moving average is the fusion technique employed to fuse the data from IMUs. The quality of tacking data indeed depends on number of IMUs on board. A detailed analysis on this has been carried out in Section 7 & 8. Accelerometer:
9. 9 An accelerometer is a sensor which provides specific force measurements when it is subjected to a force. It contains a proof mass attached to a spring in each orthogonal axis. Therefore force measurements in three orthogonal axes are observed (s b ε R 3 ). The force each proof mass is subjected to is converted into an output voltage depending on the force. But the force contains acceleration due to gravity in addition to the inertial acceleration (actual acceleration of the body). Since the frame of the sensor changes continuously when the body is moving, the orientation of the accelerometers with respect to the local tangent plane keeps on changing. The inertial acceleration can be found only after knowing this orientation. s t ε R 3 provides acceleration with respect to the local tangent plane. To get inertial acceleration, acceleration due to gravity should be subtracted from this value. Gyroscope: The gyroscopes provide angular velocity measurements of a body whenit is moving with respect to a coordinate frame. The MEMS gyroscopes used in the IMU are vibratory gyroscopes which operate on the basis of Coriolis principle.According to Coriolis principle, an induced rotation in the velocity plane sets up a force called coriolis acceleration. The angular velocity can then be calculated using the following formula. Alternatively the circuits in MEMS sensors sense the coriolis acceleration and provide a value of capacitance correspondingly. Like accelerometers three gyroscopes are present in three orthogonal axes in order to measure angular velocity. The measured angular velocity is ω b ε R 3 . Ω = acor/2v where Ω refers to angular velocity acor refers to coriolis acceleration v refers to linear velocity
10. 10 Block diagram of an Inertial Navigation System: The following block diagram of an inertial navigation system is widely used in the literature. The facts such as neglecting the rotation of the earth simplify the inertial navigation system as follows. Here S n represents transformed value of acceleration from sensor’s frame to the navigation frame. S b represents acceleration measured in the sensor’s frame is a rotation matrix for transforming the accelerations from sensor’s frame to the navigation frame q defines quaternion which is taken as a function to the rotation matrix g n denotes acceleration due to gravity a n represents the true acceleration of the system ω b represents angular velocity of the system v n and q represent velocity and attitude of the system respectively Dead reckoning: Dead reckoning is an iterative process that has to be carried out with the values of position and heading derived from the system. The drawbacks of this system are that it requires initial values of position and heading and both these errors grow as a function of time. This can be attributed to the integrative nature of the system. The total position error will depend on three error factors. These are initial position and heading errors, translation errors when navigating from one frame to another and heading errors in the gyroscope. Out of all these errors the attitude errors of a gyroscope is the most critical factor. Therefore choosing a gyroscope with high accuracy will reduce the total position error. Errors in INS: The integrative nature of the inertial navigation system causes position error to grow as a cubic function of time. This is because it involves integrating accelerometer measurements twice by also using gyroscope measurements. This can be illustrated as follows.
11. 11 When the system moves from its initial position the frame of the sensor changes continuously and hence therefore its axes loses its alignment with the local tangent plane frame. Due to this misalignment an error in acceleration δωy t is observed. On double integration we get position error as a function of t 3 . Errors in IMU: The IMUs suffer from a certain set of errors after fabrication. The modeling of these errors and the calibration compensation process are discussed in Section 6. In this section the focus is on providing a brief description of the different types of errors. Deterministic errors can be corrected using the calibration process while the non-deterministic errors are taken care of during stochastic modeling and estimation (Kalman filter). Again determinsitic errors can be classified as well as quantified. The following are the deterministic errors. 1. Bias 2. Gain 3. Non- orthogonality 4. Inter IMU Misalignment. Note that Inter IMU misalignment occurs only when multiple IMUs are used on board.
12. 12 5.Mathematical Model: A mathematical model was derived by Nilsson et al. (2013) to solve the problem of positioning. In spite of time being a continuous quantity a mathematical model in continuous time is not suitable or it is laborious to perform approximations. Positioning involves findingthe quantity position which varies with time. Therefore time is required to be measured along with position. The information about the system is also required along with the position which will be used in creating Process models involving system and Measurement models which associate the system with the measurements. Thus the two fundamental quantities required to quantify position are Measurements and Models. The models create relations between measured quantities and the measurements measure those quantities. For example position values are not directly available from the sensor measurements. Instead they need to be derived from Acceleration and rotation measurements from accelerometer and gyroscope respectively. So Measurements require measuring Accelerometer and Gyroscope values whereas Models process this information and find out the position values. 1. Select sensors and mounting: The measurements carried out, depending on the error sources in the measurements affect the statistical models that are used to process this data. Therefore selecting the right kind of sensors and the mounting scheme will generate confidence in the collected measurements and hence meaningful values of position can be derived by the statistical models. IMUs when interfaced with a microcontroller to process the data and an application platform to receive position values make a self-contained system which is not dependent on prior knowledge of the environment or pre-installed infrastructure. Again mounting scheme cannot be overlooked. 2. Choose a state space model: A State space model is required in order to make meaningful inferences out of the measurements. This is because measurements cannot be infinite. Similarly the states are not absolute quantities and require a reference coordinate system and frame. The coordinate frame used in Pedestrian navigation is Local Tangent Plane. Here position and heading are initialized with reference to a tangent point on the earth. The coordinate system used is Cartesian coordinate system. 3. Define a Measurement model: The state space model can hold only static values of measurements. It cannot capture randomness in measurements or how these measurements changes with time. Therefore a measurement model can be defined to account for both these parameters. The subscript k refers to the discrete time measurements. Hk = (Xk,Uk,Nk) Here Xk refers to the measurements, Uk refers to the change in measurements with time and Nk refers to the randomness in the measurements. 4. Define a Process Model: Even though the values of measurements change with time these measurements are temporal quantities and hence the previous value of measurements heavily determines the current value of measurements. This means that the current value of measurements can be estimated by its previous value, change in current value with time and a random component. Xk = Fk(Xk-1,Uk,Wk)
13. 13 Fk represents process model, Xk represents current value of measurement, Xk-1 represents previous value of measurement, Uk refers to the change in measurements with time and Nk refers to the randomness in the measurements. 5. Defining Pseudo measurements: When the measurement values are complemented with the information about the environment and the dynamics of the system, the resulting information cannot be covered within a process model. This information is closely related to a measurement and hence the name Pseudo measurement. For example when a zero velocity update is passed by comparing the acquired velocity with a threshold value, the velocity value is not a measurement but a quantity derived from accelerometer and gyroscope measurements and hence the update passed is a pseudo measurement. 6. Kalman Filter: Utilizing the knowledge of process model, measurement model and pseudo measurement model a statistical model can be used to derive the signals present in the state space model. These signals represent the states of interest such as position that are to be estimated. One of the highly accurate state estimation models is the Kalman filter which is applicable to linear stochastic systems. For non- linear systems an extended kalman filter is used. The state equations of the kalman filter consists of Time update and Measurement update equations. The state prediction happens at time update .This value is fed into the measurement update where the state correction takes place.
14. 14 6. Hardware: MIMU22BT is an embedded foot mounted navigation system having a combination of hardware and software components which work together to give a superior tracking performance. In the hardware section only the high level system design is the focus area in this thesis. High level System Design: The hardware in the module consists of 32-bit floating point capable AT32UC3C2512 Atmel microcontroller which processes all the raw inertial data from the IMUs with the help of a navigation algorithm written in Embedded software. There are four Invensense MPU9150 IMUs on board with a compass inside the package which is connected to the microcontroller through the I2C buses initialised in the software. The Bluetooth used is Bluetooth v3.0 and the module used is SPBT2632C2A from ST Microelectronics. An External crystal oscillator provides a clock frequency of 16 MHZ to the microcontroller. It also contains a pressure sensor LPS331AP from ST Microelectronics which is connected through TWI (Two Wire Interface) in the microcontroller. The flash memory AT45DB081D is connected through the SPI interface in microcontroller. Besides Bluetooth communication is also possible through USB interface. Calculating position and performing dead reckoning is the sole purpose of the system. The computed position values in microcontroller are captured by an external application platform which is an Android powered device. This application sends commands to the module, which is provided with Bluetooth connectivity, and is received by making use of the USART communication interface. The communication in this interface between the two devices are performed by the TXD and RXD, transmitter and receiver pins depending on the conditions of the CTS and RTS, Clear to Send and Ready to Send in both the devices.The USART interface in the microcontroller is made up of Data Register, Control and Status Registers and Baud Rate Registers. It used Serial communication at a baud rate of 9600 bps. USART usually inputs byte by byte and reads the data from registers before further processing by the microcontroller. The Peripheral DMA controller (PDCA) acts as an interface between USART module and memory. Handshake interface is used to connect USART to Peripheral DMA. This essentially means that communication can only occur if valid data is present at the transmitter and receiver is ready to receive the data. High Speed Bus Matrix is used to connect the PDCA with on-board memory.
16. 16 7. Software: In the software section, the communication protocol devised by the openshoe team serves as the backbone for sending commands and receiving data at the application platform. IN communication protocol, the types of commands, command structure and the transmission and reception logic of commands will be discussed. One of the major interfaces in the device is the interface between the microcontroller and the IMUs. Though there are no dedicated I2C pins used in this interface, the protocol is implemented in the software. A brief note on the interface and the protocol is outlined. This is followed by the description on storing the data from the IMUs in a suitable format which helps the user to send the data in a well-defined format to the application platform. Next, calibration compensation discusses the methodology to remove the errors from the sensor’s output data following which the process of fusion and bias compensation is explained. Communication Protocol: The communication between the INS and the Android device is defined in a protocol provided by the open source embedded foot mounted INS system, Openshoe. This communication protocol provides a uniform structure for receiving a command from the user and transmits the appropriate response to the command received. The states where the raw and the computed results from the microprocessor is stored, the total lists of various commands that the user can transmit and the different processing functions that influence in tracking the pedestrians location are well defined in a framework of this protocol. The user can have access to all the data which are received from the IMUs by the microcontroller which are stored as variables in the system. Other computed data such as the fusion data and navigational states after the error model data are also stored in the variables of its own which are also accessible. These states are accessed by the user by transmitting the specific Command to the INS module. The user can request any output of the state at a specified rate that can be transmitted, can change the value stored in a variable state and even modify the processing sequence. A set of these commands together can be processed to perform a certain function which in turn called the processing function and is specified for task to either perform for computing the raw inertial values or even outputting the computed values. The states, commands and the processing functions are identified as separate entity by the microprocessor with the help of a single byte unique header assigned to each state, command and processing function respectively. Types of Commands: For the user’s convenience of handling the data there are five types of commands that can be used. For example, the requirement of the Module Id and for pinging the device small commands are used which are classified as Auxiliary commands. Since the users can modify the states stored in a variable and then check the output of the processing function, a set of commands which serve the purpose of testing and debugging are used. This is primarily used as to input a user defined inertial states of the IMUs and then compute the required navigational equations for testing. The user can request a specific or a combination of states stored in the microcontroller making use of Request data commands. Similarly a process function can be initiated by the user which itself can output certain states is made possible by the Request Processing functions command. For example Stepwise Dead Reckoning function can be initiated by the user which outputs the position of the pedestrian at every step. Certain commands can perform both initiations of the processing function as well as outputting the data for the user Command Structure: The command transmitted by the user is acknowledged by the module and then it retransmits the appropriate response to the user. The command, Acknowledgement and the Response have a specific
17. 17 structure defined by the protocol. The command comprises of single byte header for identification followed by payload which is of variable size depending on the command but specific for a particular command and then the two byte checksum which is the sum of all byte received prior to it for error detection. • Command Structure CH PL CS – CH – Command Header – PL – Varying Payload – CS - Checksum • Acknowledgement Structure A0 CH CS • Response Function Structure AA PN PS PL CS – PN- Package Number – Package Size Command Reception/Transmission: When receiving a command the module checks if the header corresponds to a valid command, if so it reads the bytes within a time limit and then checks the checksum. If the checksum is valid it then continues to parse the payload else it simply discards the bytes and waits for another header. If the command received is an acknowledgement it doesn’t send a reply ACK else it does. While transmitting the states the module assembles the data package in a buffer, allots a package number and adds the total bytes in the payload that is being transmitted. Then the package number is incremented and the data package is pushed to an output buffer which drops the packages once the buffer gets filled. Similarly if an ACK is needed to be transmitted it is prepared by command reception logic and added to the buffer. If the packages are transmitted wirelessly i.e. Bluetooth there are two modes of transmission. Lossy mode is where the packages are dropped and forgotten which is followed when transmitting through USB. In Lossless mode the packages are added to a package queue and then send the oldest package first. Once a command is reception is successful it then stores the arguments of the command in a buffer of size 512 bytes. This buffer is then passed to the appropriate response function that needed to be generated which executes the command sent by the user. IMU Interface: Following the command from application platform, the microcontroller reads raw data from the Inertial Measurement Units (IMUs). Raw data refers to accelerations and rotations measured in x, y and z axes by
18. 18 accelerometer and gyroscope respectively. The number of raw datasets read by the microcontroller in parallel is equal to the number of IMUs on board. Before further processing occurs the raw data is fused and averaged so that the navigation equations process a single datasetof accelerations and rotations in x, y and z axes (Ax , Ay , Az , Gx , Gy , Gz). In the following sections the process by which data is read from the IMU in parallel and stored in the microcontroller will be described. Also the calibration procedure and data fusion methods followed in Osmium boards in achieving a single raw dataset will be explained in detail. I2C protocol: The data is read from the IMU with the help of I2C buses initialized in the software. It makes use of a two wire interface in which the serial pins SDA ( Serial data) and SCL ( Serial Clock) are interconnected between master and slave devices. The microcontroller acts as the master device and the IMUs connected to the microcontroller act as slave devices. Data read from IMU Calibration Compensation Calibration Matrix Data fusion
19. 19 The command sequence used to transmit/receive data is as follows: 1. Send Start bit: The microcontroller alone can initiate data transfer only when both the SDA and SCL pins are high. The Start condition is generated when SDA is pulled to low by the microcontroller while the SCL pin is high. It is also a signal to the IMUs (Slave devices) that a new communication session is about to begin. 2.Data Transfer: • Data transfer begins when the microcontroller sends a byte . The most significant seven bits corresponds to the Slave device address and the LSB corresponds to whether Read / Write operation is taking place, 1 for Read, 0 for write. • After this byte is sent, the microcontroller waits for an acknowledgement from the IMU . • Once acknowledgement is received, the microcontroller waits for data serial data from the IMU. • The Serial clock is always under the control of the microcontroller periodically changing from low to high or from high to low and the data on SDA line can be changed only when SCL is low. • This means the IMU can send data to the microcontroller only when SCL pin is low. • For each byte of data received, the microcontroller has to send an acknowledgement. 3.Send Stop bit: Once data transfer is complete, the Stop condition is generated by the microcontroller where the SDA pin is set to high while the SCL pin is high. This is a signal that the bus is free once again and communication can be started by initiating Start condition.
20. 20 Data Storage mechanism: The data received from the IMUs in parallel through the SDA pins in the microcontroller are stored in a bit-- wise fashion in a data matrix. 1B1 1B2 1B3 …. …. 1B32 2B1 2B2 2B3 …. …. 2B32
21. 21 The 1 st and 2 nd elements of the data matrix will be in the above format each of size 32 bits where 1B1 represents 1st Bit sent from IMU 1, 1B2 represents 1st Bit sent from IMU 2. 2B1 represents 2nd Bit sent from IMU 1, 2B2 represents 2nd Bit sent from IMU 2. In General, nBm represents nth Bit sent from mth IMU. The data matrix is a column matrix which means that each row in the matrix is taken as a single element of the matrix. The number of bits in each row depends on the number of IMUs connected to the microcontroller. Data is sent from the IMU registers in the following order to the microcontroller: Accelerometer values Ax, Ay, Az followed by Temperature values and then the Gyroscope values Gx, Gy and Gz. Each measurement has a size of 2 bytes. Therefore each IMU sends 14 bytes of data to the microcontroller. The software now takes the first 32 elements in the column matrix and calculates the bitwise transpose of all the elements in this matrix. The resultant matrix is again a column matrix in which each row corresponds to Ax, Ay values from a particular IMU. The same process is repeated till the remaining values Az, Temperature, Gx, Gy, Gz are isolated. nBm nBm nBm …. …. nBm
22. 22 The Ax, Ay, Az, Gx, Gy, Gz and Temperature values of IMU 1 which are 2 bytes each will be stored in the format shown above where bytes from 1B1 to 16B1 represents Ax, 17B1 to 32B1 represents Ay, 33B1 to 48B1 represents Az, 65B1 to 80B1 represents Gx, 81B1 to 96B1 represents Gy, 97B1 to 112B1 represents Gz and 49B1 to 64B1 represents Temperature. These values are sorted and stored in a 2-D mimu_data array of size 32x7 where each element is of size 16-bit (2 byte) representing Ax,Ay,Az,Gx,Gy,Gz and Temperature.Here each element in the mimu_data matrix is of size 16-bit i.e. 2 byte. Axn Ayn Azn Gxn Gyn Gzn Tn The first row corresponds to Ax1, Ay1, Az1, Gx1, Gy1, Gz1 and T1 values of IMU 1. In General, nth row corresponds to the nth IMU.
23. 23 Calibration Compensation: The MEMS based IMUs used in the Pedestrian Navigation System (Osmium MIMU22BT) are generally not calibrated before the fabrication process. Also imperfections in fabrication process and packaging results in errorsin the sensitivity axes of the sensors present in the IMU.Calibration is required to be performed in order to compensate for these errors. The most popular method in literature for calibration is the usage of mechanical platforms to place the sensor at accurate orientations and at known angular velocity values. The output of the sensors is compared with a known force value which is applied as input. For a perfect sensor both the input force given and the output measured should be the same. But due to observable and unobservable errors there is deviation in the measured value. The accuracy of this platform is high but its cost exceeds the cost of an IMU. A novel low cost calibration algorithm was then proposed by et al. (Skog 2009) by using a platonic solid Icosahedron. The observable error parameters that can be estimated by this procedure are sensor gain, sensor bias, non-orthgonality of sensitivity axis and Inter IMU misalignment. Since twelve parameters are required to be estimated the IMU needs to be placed in atleast twelve different orientations. Using an icosahedron has two advantages. One is it provides the maximum number of sides in a polyhedron. The other advantage is it provides evenly distributed orientations. The error in the sensitivity axis and the inter IMU misalignment is due to mounting of the device in random orientations which also depends on the surface it is mounted. Since it is unlikely to calibrate the device in all possible random orientations, a maximum likelihood based system is proposed to solve this issue by using an icosahedron which consists of a number of evenly distributed yet unknown orientations. Due to this, the raw data from sensors has many errors because of factors such as sensor gain, inter IMU misalignment, sensitivity axis non-orthogonality and bias and hence the device requires calibration to compensate for these errors in order to obtain accurate data from the sensors. Sensor Model: The IMU used in the device MIMU22BT is Invensense’s MPU9150. The IMU has nine degrees of freedom which are accelerations from accelerometer, rotations from gyroscope and magnetic field from magnetometer. The accelerometer measures acceleration in three dimensions as Ax, Ay, Az and similarly the gyroscope measures rotations in three dimensions as Wx Wy Wz in the three sensitivity axes. The accelerometer can be digitally programmed to measure ±2g, ±4g, ±8g, or ±16g m/s 2 and the gyroscope can be digitally programmed to ±250, ±500, ±1000, or ±2000 degrees per second. The sensor axes should be orthogonal to each other ideally. But due to the reasons mentioned above there is a slight misalignment as represented below.
24. 24 In MPU9150, the Gyroscope is assumed to be free of all the errors except the Inter IMU misalignment which occurs either during packaging of the sensor inside the IMU or when mounting the sensor on the MIMU platform and bias. For accelerometers the calibration parameters are required to be estimated are sensor gain, inter IMU misalignment, sensitivity axis non-orthogonality and bias. The first step is to make sure that the three axes of a single IMU are orthogonal to each other before other errors such as gain, bias or Inter IMU misalignment can be taken care of. Because of this non-orthogonality, the accelerometer measurements in sensor’s frame are required transformed to accelerometer measurements in navigation frame. An = . Ai An - Accelerometer measurements in navigation frame Ai - Accelerometer measurements in sensor’s frame = 1 − 1 − − 1 Eliminating non-orthogonality requires the assumption that one of the axes of the sensor is aligned with the same axis in the navigation frame. Therefore the yn axis lying in the plane spanned by xi and yi and , , become zero. In the figure described below x-axis is assumed to aligned between both the frames. Here xi, yi, zi, refer to the accelerometers in X,Y and Z directions respectively in the frame of the sensor and xn, yn zn refer to the acelerometers in X,Y and Z directions respectively in the navigation frame.
25. 25 Following this, misalignment of Y-axis of the sensor can be calculated with respect to the X-Y plane of the navigation frame. So, the Y-axis sensor will have components in both the X and Y axes of the navigation frame. This X- axis component is added to the true force measurements in X-axis. The misalignment in Z –axis is now calculated by projecting the value of Z-axis sensor in the X-Y plane and measuring its components in both the X and Y axes. These two components now contribute to the true force measurements in X and Y axes respectively. In formulating the compensated acceleration equations, the misalignments in X, Y and Z axes between sensor’s frame and navigation frame is assumed to be very small. Hence the compensated acceleration equations do not have misalignment components about the same axis. = 1 − 0 1 − 0 0 1 These equations clearly suggest that the X-axis acceleration depends on contributions from Y and Z axes true force measurements which are evident from the methodology discussed above. Similarly the Y-axis acceleration depends on contribution from Z axis true force measurements.
26. 26 The calculation of Inter IMU misalignment axis requires choosing one of the IMU’s axes as the reference axis. The remaining IMUs are calibrated based on this reference axis. Recalling the calibration methodology followed, the only input to the accelerometer sensors is the gravity vector since the sensors are placed on a table. The value of gravity vector in X,Y and Z directions in the reference IMU is measured and then the same measurements are carried out in the remaining IMUs. By finding the deviation in X,Y and Z acceleration values in comparison with the reference IMU, the Inter IMU misalignment is compensated. = 1 − − 1 − 1 , ` , refer to the misalignments in X, Y and Z directions of a jth IMU in comparison with the reference IMU. Rij refers to the 3x3 rotation matrix representing the orientation between the ith and jth IMU chosen. Following this, the individual gains in all the three axes are multiplied and then any calculated bias in the three accelerometer axes are added or subtracted depending on its value. ± = Bz By Bx Az Ay Ax Kz Ky Kx Az Ay Ax 00 00 00 Here Kx, Ky and Kz refer to the gains of accelerometer in X,Y and Z axes respectively and Bx, By and Bz refer to the bias of accelerometer in X, Y and Z axes respectively. Ax, Ay and Az on the left hand side refers to the final compensated values of accelerations in X, Y and Z axes respectively. The overall process can be summarised by the following equations. Ai= K i (Li) -1 An +b i where K i represents Gain, Li represents Non-orthogonality, represents Inter IMU misalignment, b i represents bias , Ai and An represent Accelerometer measurements in sensor’s frame and navigation frame respectively. (Note that An = . Ai and thus the inverse). Here L i is dependent only on the mounting which is done physically whereas K i and b i are dependent on variation in temperature in addition to the physical mounting.
27. 27 − = Az Ay Ax Yz Yy Yx X Here Yx,Yy and Yz are the known force measurements that are input to the IMU. The error value measures the difference between the uncompensated and compensated acceleration output. This is calculated for all the IMUs in 20 different orientations. The resultant matrix is of size No of IMUs * orientations *3 x 1. Then it is fed into a non-linear equation solver with the initial guess value of unity gain and zero non-orthogonality, bias and Inter IMU misalignment in the same order. The measured force from accelerometer should have been independent of the orientation in which the IMUs are placed and should always be equal to the input force it is subjected to. Hence the value of X serves as a cost function in calibrating the IMU. The non-linear square equation solver takes the matrix of size No of IMUs * orientations *3 x 1 such that it is of the form = 1 2 … " # This cost function contains many local optima and if the summation has to converge to the true parameters it must be initialised with the Initial Guess Matrix.$ = 1 1 1 0 0 0 0 0 0 0 0 # in which each parameter refers to % & & & ' ' ' ( # The initial guess matrix makes sure that the result of non-linear equation solver converges to global minima. It contains unity gain and zero non-orthogonality, bias and Inter IMU misalignment in X, Y and Z axes respectively when read from left to right. The value of initial guess matrix is set from the datasheet of the IMU MPU-9150. The scaling factor is less than 3% (unit gain), The bias is ±2 m/s 2 ,non orthogonality is 0.5 % and Inter IMU misalignment is assumed to be close to zero as it is dependent only on errors due to physical mounting. )( 222 2 2 21minmin xnxxX K++= θθ ∑∑= = = M i N j est X 1 1 2 2 minθ θ M represents total number of orientations, N- total number of IMUs , $ -Initial Guess Matrix $)*+ –Result of Non- Linear Equation Solver which contains calibration parameters in the same order as Initial Guess Matrix. The result of this non-linear equation solver is a matrix in the same form as initial guess matrix. Once these parameters are estimated they can be put into the following matrices. , = & 0 0 0 & 0 0 0 & = 1 − 0 1 − 0 0 1 = 1 − − 1 − 1 ' = ' ' ' # Calibration Matrix: The calibration matrix used for compensating the errors in gain, non orthogonality of sensitivity axis and inter IMU misalignment is of size 32x18.
28. 28 ]17][311][310][31[ ]17][11][10][1[ ]17][01][00][0[ CCC CCC CCC LL MOMM MOMM LL LL All the values in the matrix help in compensating for the errors in accelerometer and gyroscope readings. Each row in the matrix can only correct the value of a particular IMU. The first nine elements in all the rows correct the accelerometer readings and remaining nine elements correct the gyroscope readings. The first nine elements of an mth row where m is the IMU under consideration are generated using the formula below - 1: 9 = , 0 12 K i , L i , E i represents the gain, non-orthogonality and Inter-IMU misalignment repectively,where K i L i E i represents the matrix multiplication of the matrices K i , L i , E i in the same order and R is a 3x3 identity matrix if the IMUs are placed on the top of the board and a 3x3 matrix of the form 0 −1 0 −1 0 0 0 0 −1 if the IMUs are placed on the bottom. The remaining nine elements of an mth row is generated by the formula below - 10: 18 = 0 12 The resultant matrix is scaled by the value 32768 to convert it into a 16 bit number so that it can be multiplied with the raw inertial readings. This is because each raw inertial reading has a size of 2 bytes. The accelerometer readings are then corrected in the following manner. [ ] [ ] = ]8][7][6][[ ]5][4][3][[ ]2][1][0][[ nCnCnC nCnCnC nCnCnC FzFyFxAzAyAx Here Ax,Ay,Az refer to the corrected values of acceleration and Fx,Fy,Fz refer to the raw inertial readings of the accelerometer. In the 3x3 matrix defined, each element is taken from the calibration matrix. The value of n varies depending on the IMU chosen for calibration. The following equation corrects the values of gyroscope readings. [ ] [ ] = ]17][16][15][[ ]14][13][12][[ ]11][10][9][[ nCnCnC nCnCnC nCnCnC gzgygxGzGyGx
29. 29 Here Gx,Gy,Gz refer to the corrected values of rotation and gx,gy,gz refer to the raw inertial readings of the gyroscope. The defined 3x3 matrix again takes the remaining values from the calibration matrix. Fusion: The corrected values of accelerations and rotations from different IMUs are fused into a single entity by calculating simple moving average of acceleration and rotation values in the three axes. n AnA2........A1 Aavg + = n GnG2........G1 Gavg + = Aavg and Gavg refer to the fused value of acceleration and rotation in a particular axis respectively. Bias Compensation: Following fusion, bias compensation takes place where the mean values of accelerometer and gyroscope bias is subtracted from the fused values of acceleration and rotation respectively. Ax= Aavgx- Abx Gx= Gavgx-Gbx Ay= Aavgy-Aby Gy= Gavgy-Gby Az= Aavgz-Abz Gz= Gavgz-Gbz Here Aavgx , Aavgy , Aavgz, Gavgx, Gavgy, Gavgz refer to the fused values of accelerations and rotations in x,y and z axes respectively, Abx,Aby,Abz,Gbx,Gby,Gbz refers to the mean value of accelerometer and gyroscope biasin x,y and z axes respectively. Ax,Ay,Az, Gx,Gy,Gz refer to the bias compensated values of acceleration and rotation in x,y and z axes respectively. These are the required values of acceleration and rotation on which processing happens through the use of navigational algorithms to get the final values of displacement and heading change.
30. 30 8. Implementation: The previous sections on hardware and software provide sufficient background to complete the objectives set. This chapter describes the evaluation of the objectives by summarizing the implementations carried out. Varying number of IMUs: Varying the amount of IMUs on board gives the user ability to check their performance individually or in different combinations. The device currently performs bias compensation after fusion occurs. That is the bias is compensated over averaged data. This would require the user to perform calibration every time if he/she wishes to vary the amount of IMUs on board. To overcome this tedious process the Embedded software has to perform calibration compensation individually for all IMUs before fusion. Also flexibility should be provided to the users so that they will be able to choose any of the four IMUs in all different combinations. The command supports a maximum of 32 IMUs which are the highest in the Osmium series. Taking all these factors into account and the knowledge of the communication protocol, a command was created to control the data read from the IMUs. Also a calibration script was created to generate a calibration matrix suitable for compensating all the errors in the IMUs individually before fusion. Correspondingly the application platform was enhanced to facilitate the user to choose the number of IMUs without which implementation would just be a prototype. The application platform here refers to an Android powered device with a minimum version of 1.1.
31. 31 9. Experimental study: Experiments were conducted on the foot mounted pedestrian navigation system Osmium MIMU22BT based on the standard performance metrics used in the industry for analyzing the reliability of the device. For this purpose the experiment was designed based on four main parameters which are the type of shoe that the device is mounted on, type of surface the experiments were conducted, type of the path profiles used and different speeds of motion during the experiment. The data from the device after processing is sent to the Android application (DaRe) which then filters and displays the position values in terms of X, Y and Z co- ordinates. Besides the application also captures the time taken, average speed and the distance covered by the user and store it in a text format. Design of the experiment: Path Profile: Type of Shoes: Running and Formal Shoes Type of Surface: Pavement and Grass Type of motion: Fast (~5.8Kmph), Medium (~4.5Kmph) and Slow (~3.5Kmph) The chosen standard performance metrics are: 1. Distance between start point and end point per 100m meter distance covered 2. Distance measurement error (RMS) per 100m distance covered 3. Change in z axis value (RMS) per 100m distance covered Images from left to right (Above): Formal Shoe, Running Shoe, DaRe Application. (Below) Real time tracking using Rectangular and Straight Paths respectively, Test case as viewed from Google Maps Rectangular Field Straight Path Perimeter-129m Length-26m Closed loop Path Sharp U turns 3 laps 2 laps Total distance- 387m Total distance- 104m
32. 32 Results: Sample tracking data obtained from the device (above) and Graphs plotted based on the experimental results (below) Observations: 1. Errors in X-Y axes are independent of errors in Z-axis. The accumulation in Z-error is higher than the distance and drift errors because the Z-axis accelerometer is not at par in performance when compared to X-Y axes as fabrication of Z- axis accelerometer inside the MEMS IC is tedious . 2. Less Z-axis errors are observed while walking on pavement compared to on grass. The distance and drift errors are comparable in both the cases. 3. A formal shoe performs better compared to a running shoe. As running shoes are designed for the user to be in motion continuously at a higher speed, the heels are elastic in nature and do not provide firm footing with the ground. Conversely a formal shoe provides the same at every step as it is meant for walking. -10 -5 0 5 10 15 20 25 30 35 40 45 -30-25-20-15-10 -5 0 5 10 Rectangle -3 0 3 6 9 12 15 18 21 24 27 -5 0 5 10 15 20 Straight 0 1 2 3 4 Z-error Distance Drift Shoe Running shoe Formal shoe 0 1 2 3 4 Z-error Distance Drift Surface Pavement Grass 0 1 2 3 4 Z- error Distance Drift Path profile Rectangle Straight 0 2 4 6 Z-error Distance Drift Speed Slow Medium Fast
33. 33 4. The walking speeds indeed matter as at a higher walking speed the device does not have ample time to sense Zero-Velocity. In ZUPT algorithm errors are corrected only when step is detected. In spite of that, the drift error is almost the same for all walking speeds. This proves that even though error is accumulated till the detection of next step, the presence of an on board floating point 32-bit processor, Multiple IMUs and the static calibration performed helps in better dead reckoning. 5. The device performs best at a medium walking speed as the algorithm is tuned on the device for this speed. Higher the mounting position, higher is the velocity detected as in the case of running shoes. If the threshold velocity of step detection is increased better results will be observed for fast motion whereas it degrades for slower motion.
34. 34 10. Conclusion: The knowledge of communication protocol was crucial in carrying out the implementations in Embedded software. The integration with the application platform (Android) also provides the ease of use from the end user’s perspective to incorporate varying amount of IMUs on board. Currently, data from the IMUS is fused by means of a simple moving average and the averaging factor is dependent on the number of IMUs selected by the user. Experimental study was indeed required to verify the implementation. It was also done to benchmark the performance of the device under various conditions by taking into account the parameters that directly or indirectly affect the tracking output data. The implications of these results discussed in Chapter 9 help us draw valid conclusions from the derived data. The device was programmed in Atmel Studio 6.2 using a custom made JTAG programmer at GT Silicon Pvt Ltd and the application platform was designed using Eclipse IDE.
35. 35 11. References: 1. Atmel,”32-bit AVR Microcontroller”,AT32UC3C datasheet, Oct. 2010 [Revised January.2012] 2. InvenSense, “MPU 9150 Product Specification”, PS-MPU-9150A-00 [Revised September. 2013] 3. InvenSense, “MPU 9150 Register Map and Descriptions”, RM-MPU-9150A-00[Revised September. 2013] 4. John-Olof Nilsson “ZUPT aided INS software components for an embedded platform”sourceforge.net, Nov. 11, 2011. [Online]. Available: http://sourceforge.net/projects/openshoe/ 5. I2C info – I2C Bus, Interface and Protocol [online], Available:http://i2c.info/ 6. John-Olof Nilsson, “Communication Protocol for the open shoe modules”openshoe.org, Jan.30, 2015. [Online]. Available: http://www.openshoe.org/wp- content/uploads/2015/01/OpenShoe_communication_protocol.pdf 7. Oliver J. Woodman. An Introduction to inertial navigation. Technical Report. University of Cambridge, UK, Computer Laboratory, August 2007. 8. Frequently Answered Questions, Inertial Elements, [online] 2012,Available: http://www.inertialelements.com/faq.php 9. GPS Accuracy, GPS.GOV,[online] 2014, Available: http://www.gps.gov/systems/gps/performance/accuracy 10. Foot Mounted INS for every foot, Open Shoe, [online] 2011,Available: http://www.openshoe.org/ 11. John-Olof Nilsson, Amit K Gupta, Peter Handel, "Foot mounted inertial navigation made easy”. In Proc Indoor Positioning & Indoor Navigation (IPIN), Busan, Korea, 2014. 12. Mathiassen, K., Hanssen, L. and Hallingstad, O. (2010): “A Low Cost Navigation Unit for Positioning of Personnel After Loss of GPS Position”, Proceedings of the 2010 International Conference on Indoor Positioning and Indoor Navigation (IPIN), September 15–17, 2010, Campus Science City, ETH Zurich, Switzerland. 13. Nilsson, J.O., Skog, I. and Händel, P. (2010): “Performance Characterisation of Foot-Mounted ZUPT-Aided INSs and Other Related Systems”, Proceedings of the 2010 International Conference on Indoor Positioning and Indoor Navigation (IPIN), September 15–17, 2010Campus Science City, ETH Zurich, Switzerland. 14. Skog, I., Nilsson, J. O. and Händel, P. (2010): “Evaluation of Zero-Velocity Detectors for Foot-Mounted Inertial Navigation Systems”, Proceedings of the 2010 International Conference on Indoor Positioning and Indoor Navigation (IPIN), September 15–17, 2010 Campus Science City, ETH Zurich, Switzerland. 15. Wan, S. and Foxlin, E. (2010): “Improved Pedestrian Navigation Based on Drift-Reduced MEMS IMU Chip”, Proceedings of ION 2010 International Technical Meeting, January 25–27, 2010,San Diego, California, USA, pp. 365–374. 16. J.-O. Nilsson, I. Skog, P. Handel, and K. Hari, “Foot-mounted INS for everybody – An open-source embedded implementation,” in Position, Location and Navigation Symposium (PLANS), 2012 IEEE/ION, (MyrtleBeach, SC, USA), 23-26 Apr. 2012. 17. J.-O. Nilsson, I. Skog and P. Handel, “Aligning the forces – Eliminating the misalignments in IMU Arrays” Instrumentation and Measurement, IEEE Transactions on , vol.63, no.10, pp.2498 — 2500, 2014
36. 36 18. I. Skog and P. Handel, “Calibration of a MEMS Inertial Measurement Unit” XVII IMEKO World Congress, pages 1-6, 2006.