Heading constraint algorithm for foot-mounted PNS using low-cost IMU

GUI Jing,ZHAO Heming,and XU Xiang

School of Electronic and Information Engineering,Soochow University,Suzhou 215006,China

Abstract: Foot-mounted pedestrian navigation system (PNS) is a common solution to pedestrian navigation using micro-electro mechanical system (MEMS) inertial sensors.The inherent problems of inertial navigation system (INS) by the traditional algorithm,such as the accumulated errors and the lack of observation of heading and altitude information,have become obstacles to the application and development of the PNS.In this paper,we introduce a heuristic heading constraint method.First of all,according to the movement characteristics of human gait,we use the generalized likelihood ratio test (GLRT) detector and introduce a time threshold to classify the human gait,so that we can effectively identify the stationary state of the foot.In addition,based on zero velocity update (ZUPT) and zero angular rate update (ZARU),the cumulative error of the inertial measurement unit (IMU) is limited and corrected,and then a heuristic heading estimation is used to constrain and correct the heading of the pedestrian.After simulation and experiments with low-cost IMU,the method is proved to reduce the localization error of endpoint to less than 1% of the total distance,and it has great value in application.

Keywords: pedestrian navigation system (PNS),zero velocity update (ZUPT),gait detection,heading constraint.

1.Introduction

With the development of portable sensing technology,pedestrian navigation and positioning technologies have increasingly become a hot topic in the area of industry and academia [1],and there are already a wide range of solutions available.

A traditional practice is to equip with global navigation satellite system (GNSS);however,weak satellite signals in urban canyons and indoor environments render GNSSbased methods ineffective in such scenarios.On top of these GNSS-based methods,current navigation technologies can be roughly divided into three categories:intersection processing technology based on wireless communication signals,map matching technology based on database and dead reckoning (DR) technology based on microelectro mechanical system (MEMS) inertial sensors [2].

Bluetooth,ZigBee and ultra wide band (UWB) positioning technology,based on wireless communication signals,are mainly used to locate pedestrians by establishing multiple wireless network positioning nodes [2].Among them,the UWB system has many advantages such as high positioning accuracy,low power consumption and good penetration.However,it requires very precise clock synchronization and high cost,which is not conducive to widespread application.

WIFI fingerprint matching and simultaneous localization and mapping (SLAM) technology [3,4],based on database,are currently successful indoor positioning systems.Nonetheless,these methods have the drawback of requiring the creation and maintenance of the relevant databases.

The DR technology based on MEMS inertial sensors,which offer a beacon-free solution,is an independent navigation and positioning technology [5,6].Due to its small size,low power consumption,low cost,and easy digitization,MEMS inertial sensors are widely used in the field of navigation and positioning with strict hardware cost and volume requirements.Nevertheless,the output of the MEMS sensors typically contains zero bias,bias instability,and other errors,which are integrated through the navigation equation to generate position errors that accumulate with time [7,8].Thus,it is necessary to use other methods to reduce the errors due to this integral drift and improve the pedestrian DR (PDR) system using MEMS inertial sensors.

Foxlin proposed a foot-mounted pedestrian navigation method to estimate the position by the zero velocity update (ZUPT) algorithm [9] in 2005,and pointed out that the ZUPT method is effective in reducing the cumulative error of inertial sensors [10,11].The principle of the ZUPT algorithm comes from the fact that when pedestrians walk,a stance phase occurs,and at that moment,the velocity of the foot is practically zero [12].At this point,an acceleration integral could be avoided.However,the heading angle errors still cannot be well corrected,which is due to the unobservability of the heading angle error itself.Therefore,how to reduce the heading angle error to a reasonable range is a major challenge in pedestrian navigation research.Many methods use additional sensors (e.g.,magnetometers [12−14]) to provide accurate heading information.If no additional sensor is equipped,many academic teams also have done various attempts and studies.Based on the assumption that the angular rate change is zero when a stance phase occurs,the zero angular rate update (ZARU) method has been proposed to limit the accumulation of gyroscopic errors.In addition,Borenstein et al.[15] corrected heading errors by taking into account the fact that building usually has four directions and most pedestrians travel along the corridor.This method is known as the heuristic drift elimination (HDE)method,and Jimenez et al.[16] proposed an improved HDE (iHDE) method based on this.

Thereupon,according to the characteristics of pedestrian movement,this paper uses the generalized likelihood ratio test (GLRT) method proposed by Skog et al.[17]and introduces a time threshold to detect the gait of the person,so as to accurately identify the stance phase of the foot.In addition,the ZUPT and ZARU methods are integrated to limit and correct the cumulative errors of the IMU,and then the heuristic heading constraint method is used to correct the direction of pedestrian movement.After testing and verification,this method can well restrain heading divergence and reduce positioning error.

This paper is organized as follows.Section 2 presents the initial information processing.Section 3 introduces the gait detection.Section 4 expounds the proposed heading constraint method and pedestrian navigation framework.The simulation and experimental results are presented in Section 5.Finally,Section 6 concludes this paper.

2.Initial information processing

2.1 Self-alignment

This article selects the East-North-Up geographical coordinate system as the navigation coordinate system (nframe) and the Right-Front-Up orientation of the pedestrian’s right foot as the body coordinate system (bframe).The definition ofn-frame andb-frame are shown in Fig.1.

Fig.1 Diagram of different coordinate system

Use the initial stationary phase to estimate the initial zero bias of the gyroscope and accelerometer asand

Self-alignment usually includes leveling and compass alignment.The former initializes the roll angle and the pitch angle,while the latter initializes the yaw angle.Normally leveling is done first,followed by compass alignment [18].

According to the leveling principle,the pitch angleθ and roll angle ϕ in the initial state can be obtained by using the accelerometer output in the stationary state.

Due to the comparatively large gyroscopic drift of the consumer inertial sensor MPU9250,it is not capable of compass alignment.Therefore,it is necessary to assume that the initial heading angle is zero or to obtain the initial yaw angle ψ0with additional equipment.

2.2 Navigation calculation

Firstly,the attitude transfer matrix is updated using the angular rate measured by the gyroscope [19,20].

Due to the low accuracy of consumer grade MEMS inertial sensors,the Earth’s rotation rate can usually be ignored to simplify the calculation.And the specific force measurements are averaged over time,so the coordinate transformation matrix should also be averaged,from which the acceleration values inn-frame can be derived.

wheregn=[0 0 −g] andgis the local gravity acceleration.

The simplified equations for velocity and position updates are as follows:

3.Gait detection

The process of walking can be seen as a cyclical movement of alternately falling and lifting the feet and each complete gait can usually be divided into four processes,as shown in Fig.2.Or simplify it to two processes of the stance phase and the swing phase.

Fig.2 Pedestrian gait classification

An important prerequisite of pedestrian navigation is to accurately identify the stance phase,which is usually detected by distinguishing between changes in acceleration and angular velocity of the inertial sensors.Generally,the following two conditions must be met.

(i) Angular velocity conditions:

(ii) Acceleration conditions:

The logic signal is used to divide whether it is in a stance phase.When a stance phase occurs,the detection signal is 1;otherwise,when a swing phase occurs,the detection signal is 0.

The traditional single-threshold gait detection algorithms mainly include acceleration variance detection,acceleration amplitude detection,angular rate energy detection,and GLRT method.In some specific kinematic conditions such as going up and down the stairs or running,it is difficult to achieve accurate gait detection with a single acceleration or angular rate detection method.Therefore,the GLRT method is a good choice.It combines the first three methods through hypothesis testing and it is judged as a stance phase when all three are satisfied at the same time.The final formula is given by the method of statistical test [17] as follows:

SelectTdas the threshold value for gait detection,whenTn≤Td,the pedestrian gait is judged to be in a stance phase and the output is a logic value of 1;otherwise,the pedestrian is in a swing phase and the output is 0.

Due to the gait characteristics of pedestrians and the performance limitations of consumer-grade inertial sensors,when only the GLRT method is used to detect the stance phase,there will be phenomena as shown in Fig.3.In order to overcome this shortcoming,after comprehensive consideration,this article adds time threshold and gait classification based on the GLRT method,and performs multiple detections on gait to enhance the accuracy.Alternatively,between the toe-off and the next heelstrike can be considered a swing phase.To avoid outliers,only the stance phase between the heel-strike and the next toe-off is detected.The results of the gait detection and the intercepts are shown in Fig.4,where the grey area is the stance phase and represents the detection signal of 1.

Fig.3 Phenomenon where the swing state is below a threshold value

Fig.4 Results of the gait detection and the intercepts

4.Proposed heuristic heading constraint

4.1 Straight detection

The original HDE method and iHDE algorithm can work well on a linear path in four or eight predefined major directions.Since most of the buildings are frame-type buildings,the corridors and walls are mostly either parallel or vertical.When pedestrians walk indoors,most of them walk along the corridors.Therefore,for the main direction of the building,these eight directions are defined as the main headings of pedestrian movement (as shown in Fig.5).The difference between the two main directions is∆θ,generally ∆θ equals 30° or 45°.When pedestrians walk in a straight line,we can use the matched main direction to restrict the pedestrian’s heading.

Fig.5 Common eight pre-defined major directions

However,there are usually numerous irregular paths in the outdoor environment.As shown in Fig.6,it is difficult for us to predefine the dominant direction of all straight paths.

Fig.6 Diagram of the walking route under Google maps

Thereupon,this article introduces a straight forward detection method to detect whether pedestrians are walking straight.Taking Fig.6 as an example,the pedestrian walks in a straight line from the starting point,and the average of the initial three-step directions is used to establish the initial dominant direction of the linear path 1.If the proposed algorithm detects that the pedestrian starts to walk along the curved path 2,the heading correction is stopped.

When the proposed algorithm again detects that the pedestrian is walking along a straight path (e.g.,straight line path 3),a temporary dominant direction is established.In contrast to the method for determining the initial dominant direction,the subsequent dominant direction is given by a combination of the dominant direction of the previous straight path and the direction change of the turning path.

Firstly,the walking direction of each step ψs(n) is defined as the heading angle at the end of the stance phase.

The dominant direction is calculated by the following formula:

Judge whether the pedestrian is going straight within three steps.

where Thθis the max change value of the heading angle within three steps.

4.2 Pedestrian tracking framework

The pedestrian tracking algorithm that we use is based on the framework proposed by Foxlin [9] and later refined by Jimenez et al.[14].The extended Kalman filter (EKF)model has 15-element error state vector,and the vector is composed of attitude error δΦ,velocity error δv,position error δp,the estimated bias of gyroscopes δbgand the estimated bias of accelerometers δbain the navigation coordinate system.

The error model for MEMS pedestrian navigation system (PNS) in the navigation coordinate system [21] is as follows:

There are two stages in the Kalman filter as follows.

(i) Time update

The filter propagates the error covariance matrixPk+1/k,which can be expressed as follows:

whereQis the covariance matrix of the process noise,andFkis the 15×15 state transition matrix.They can be expressed as follows:

(ii) Measurement update

The Kalman filter gain,Kk+1,can be calculated as

whereRis the covariance matrix of the measurement noise andHis the measurement matrix.

The state estimation can be calculated as

wherezk+1is the actual error measurement.There are three error measurements that would be measured.

i) ZUPT:When the feet are stationary,the velocities of the system are expected to be zero,and the estimated velocity at timekcan be taken as error measurements.

ii) ZARU:ZARU takes into account the fact that the attitude is invariable when a stance phase occurs.Thus any angular rate measured at that point is considered as error measurement.

iii) Heuristic yaw update:In case of straight ahead,the yaw error measurement can be calculated as follows:

Finally,the corresponding measurement matrixHcan be formulated as follows:

The predictive error covariance is

The last step is to correct the attitude,velocity and position estimate by removing the Kalman error estimates.

In addition,at timetk+1,the filter only performs a time update if the pedestrian is in a swing phase,and the filter performs a complete update (time update and measurement update) if the pedestrian is in a stance phase.

4.3 System structure

The overall frame and flow of the proposed algorithm is illustrated in Fig.7.As described above,the system mainly consists of navigation calculation,gait detection,and Kalman filter.

Fig.7 Structure of the proposed algorithm

5.Experiments and discussion

5.1 Simulation experiment

In order to verify the effectiveness of the algorithm proposed in this paper,a set of simulation experiments are designed to generate simulated pedestrian inertial sensor data when walking [22].

(i) Simulation experiment design

Assume that a person is advancing in a straight line in the horizontal plane,and the initial heading angle is ψ0.The initial attitude,velocity and position of the right foot in the navigation system are

where ψ0is the heading angle before the turn.

To simplify the experiment,each step of a pedestrian is divided into two stages:the swing and the stance phase,which takestswandtstrespectively,so that a full gait period isT.In each gait cycle,the change in position of the gait during the swing phase with respect to the initial state (or the previous stationary state) is set to

wherelsis the length of each step andlhis the maximum height of the raised foot surface.The velocity of the right foot can be derived from the differential of the change of position.

In each gait cycle,the attitude of the right foot is

where θmaxis the maximum change in pitch angle during travel,θ,ϕ,ψ are pitch,roll and heading angles respectively.

In order to obtain a closed walking trajectory and design experiments as simple as possible,assume that pedestrians can turn 90° without changing their position of the right foot in a turning periodtc,which can be expressed as

The local geographical coordinates areP0,P0=[ 31.306 175N 120.644 243E 10 ],and the simulated gyroscope and accelerometer outputs are as follows:

wherebg,ngare the bias and noise of the gyroscope respectively,andba,naare the bias and noise of the accelerometer respectively.

The overall parameter settings are shown in Table 1,and some of the graphics produced by the simulation are shown in Fig.8 and Fig.9.

Fig.8 Partial graphics generated by simulation

Fig.9 Generated pedestrian trajectory in simulation

Table 1 Simulation parameter settings

(ii) Analysis of results

For the inertial data generated by the simulation,we use the proposed method in this paper and the traditional ZUPT algorithm to solve at the same time.The comparison of the trajectories is shown in Fig.10.The endpoint positioning error of the proposed method is about 0.1%,which is less than 1%.And this can verify the effectiveness of the proposed method.

Fig.10 Comparison of results using different methods

5.2 Field experiments

(i) Experimental conditions

We have designed a real-time PNS hardware and software platform for the acquisition,processing and display of inertial navigation data.

The microcontroller unit (MCU) STM32F405 acquires data from the IMU and sends it to the host computer.The inertial sensor MPU9250 is chosen as the IMU,and its performance parameters are shown in Table 2.

Table 2 MPU9250 sensor performance parameters

(ii) Analysis of results

In order to verify the practical performance of the proposed method,we design two experiments.

In the first experiment,the participant walks around the rectangular indoor field.For the data collected,different methods are used to calculate the navigation information.We compare the navigation performance of ZUPT/ZARUbased inertial navigation system (INS) using the proposed heuristic heading constraint method,ZUPT/ZARU-based INS and ZUPT-based INS,and the final plane trajectory comparison is shown in Fig.11.

Fig.11 Plane trajectory comparison of closed rectangular paths using different algorithms

The end-point positioning error is calculated separately(as shown in Table 3),which shows that the end-point positioning error of the proposed method is 0.68% and less than 1%.The formula for calculating the positioning error of the end point is as follows:

wheredis the distance between the end point and the start point,andDis the total length of route.

The attitude angle and velocity information of this experiment after calculation are shown in Fig.12 and Fig.13.From these two figures,it can be seen how well the method limits errors in velocity and horizontal attitude angle.In addition,it also shows that the proposed method can suppress heading drifts in a straight line to some extent.

Fig.12 Attitude angles of a closed rectangular path in the navigation coordinate system

Fig.13 Velocity of the closed rectangular path in the navigation coordinate system

In the second experiment,participant walks around the campus as the route shown in Fig.6.Here we use an ellipsoid fitting algorithm to calibrate the acquired magnetometer data at the initial position,so as to obtain the initial yaw.Refer to experiment above,we use different methods to calculate navigation information from the collected data.

The final calculated plane trajectories are compared,as shown in Fig.14.It shows that the path calculated by the proposed method is almost the same as the real route in the map.As shown in Table 3,the end-point positioning error of the method proposed in this paper is still less than 1%.

Fig.14 Trajectory comparison of closed curve paths using different algorithms

Table 3 Comparison of walking experiment results for different methods

The calculated attitude and velocity information of this experiment are presented in Fig.15 and Fig.16 separately.

Fig.15 Attitude angles of closed curve paths in the navigation coordinate system

Fig.16 Velocity of closed curve paths in the navigation coordinate system

6.Conclusions

In this paper,we propose a heuristic heading constraint method for a foot-mounted PNS equipped with low-cost IMU.Due to the limited performance of the consumer grade inertial sensor MPU9250,in order to improve the accuracy of gait detection,this paper adopts a GLRT method and introduces a time threshold,combined with a gait classification method.Analysis of the heuristic heading constraint method shows that,together with ZUPT and ZARU,this method is effective in mitigating heading errors.We compare the navigation performance of ZUPT/ZARU-based INS using the proposed heuristic heading constraint method and ZUPT/ZARU-based INS.The experimental results show that the proposed PNS algorithm can reduce the positioning error of the endpoint to less than 1%.