H Fourati
email: hassen.fourati@gipsa-lab.grenoble-inp.fr

This paper proposes a foot-mounted Zero Velocity Update (ZVU) aided Inertial Measurement Unit (IMU) filtering algorithm for pedestrian tracking in indoor environment. The algorithm outputs are the foot kinematic parameters, which include foot orientation, position, velocity, acceleration, and gait phase. The foot motion filtering algorithm incorporates methods for orientation estimation, gait detection, and position estimation. A novel Complementary Filter (CF) is introduced to better pre-process the sensor data from a foot-mounted IMU containing tri-axial angular rate sensors, accelerometers, and magnetometers and to estimate the foot orientation without resorting to GPS data. A gait detection is accomplished using a simple states detector that transitions between states based on acceleration measurements. Once foot orientation is computed, position estimates are obtained by using integrating acceleration and velocity data, which has been corrected at step stance phase for drift using an implemented ZVU algorithm, leading to a position accuracy improvement. We illustrate our findings experimentally by using of a commercial IMU during regular human walking trial in a typical public building. Experiment results show that the positioning approach achieves approximately a position accuracy less than 1 m and improves the performance regarding a previous work of literature.

I. INTRODUCTION

edestrian navigation is useful for finding and guiding emergency first responders, blind persons, security personal and for a wide range of augmented reality applications [START_REF] Sagawa | Non-restricted measurement of walking distance[END_REF][START_REF] Fischer | Tutorial: implementation of a pedestrian tracker using foot-mounted inertial sensors[END_REF][START_REF] Rantakokko | Accurate and reliable soldier and first responder indoor positioning: multisensor systems and cooperative localization[END_REF]. Numerous applications requiring a selfcontained personal navigation system working in indoor and outdoor environments, does not need any infrastructure support, and is not susceptible to interference [START_REF] Fischer | Location and Navigation Support for Emergency Responders: A Survey[END_REF], [START_REF] Zhou | A Low-Power Shoe-Embedded Radar for Aiding Pedestrian Inertial Navigation[END_REF]. Position tracking of human movement commonly requires an unrestricted line-of-sight to an installed infrastructure consisting of one or more transmitters and/or receivers. Such systems require extensive setup and calibration of the tracking volume, which may be of limited size and may suffer from occlusion. Examples of this type of tracking are generally based on radio frequency, such as GPS that is able to receive satellite signals in most indoor environments [START_REF] Skournetou | Pulse shaping investigation for the applicability of future gnss signals in indoor environments[END_REF], [START_REF] Bebek | Personal navigation via high-resolution gaitcorrected inertial measurement units[END_REF], or may use optical-based systems, such as video tracking. An infrastructure-independent solution for this problem is a pedestrian dead reckoning system based on shoe-mounted Inertial Measurements Units (IMUs). Applications using commercially available IMUs containing triads of orthogonally mounted accelerometers, angular rate sensors, and magnetometers have been successfully demonstrated [START_REF] Bebek | Personal navigation via high-resolution gaitcorrected inertial measurement units[END_REF][START_REF] Skog | Zero-velocity detection-An algorithm evaluation[END_REF][START_REF] Foxlin | Pedestrian tracking with shoe-mounted inertial sensors[END_REF]. Many researches have focused on using inertial sensors combined with magnetic ones to estimate the pedestrian's location relative to a known starting position.

Throughout the research, one of the main approaches to position tracking is an adaptation of the well-known strapdown navigation algorithm, which incorporates double integration of the measured acceleration to estimate distance and/or position. The growth in position uncertainty that arises from the integration of the acceleration error is mitigated by a technique that is commonly referred to as Zero Velocity Updating (ZVU). Most types of human movement, such as walking and running, have repeated recognizable periods during which the velocity and acceleration of the foot are zero. These brief periods occur before entering the swing phase of the gait cycle each time the foot contacts the ground during the stance phase, as shown in Fig. 1. The use of a foot-mounted IMU provides sensor data for recognition of these periods. This provides a means to determine the velocity drift error and to facilitate its correction in preparation for subsequent integration to derive position. Since this correction is applied at the end of every walking step, it provides a type of immediate recalibration of the sensor. Shoe-mounted navigation systems have been investigated before. Some previous works have exploited the ZVU technique. As a matter of fact, one of the first systems based on the above solution can be attributed to Foxlin [START_REF] Foxlin | Pedestrian tracking with shoe-mounted inertial sensors[END_REF]. Besides the ZVU, Foxlin's system uses a tri-axial magnetometer sensor to compute a yaw measurement that is used as an input measurement to an Extended Kalman Filter (EKF). The step detector is based on thresholding the average value of the gyro and the accelerometer output signals during a period

Position Estimation Approach by Complementary Filter-aided IMU for Indoor Environment

H. Fourati, and N. Manamanni, Member, IEEE P of time. Experimental results showed a 0.3% error performance of the walked distance. In another work by Ojeda and Borenstein [START_REF] Ojeda | Non-GPS Navigation for Security Personnel and First Responders[END_REF], 2% error of walked distance was reported. A combined GPS and shoe-mounted navigation system is proposed by Godha et al. in [START_REF] Godha | Foot mounted inertial system for pedestrian navigation[END_REF]. The system implements a standard 15-state error model and exploits GPS measurements to bound drift errors in outdoor scenarios. The step detection is carried out through the analysis of the magnitude of the accelerometer signals and its moving acceleration variance over a number of samples. According to the authors, the system performed quite well in indoor (with no GPS) and outdoor tests. Another pedestrian navigation system was proposed by Suh et al. in [START_REF] Suh | Pedestrian inertial navigation with gait phase detection assisted zero velocity updating[END_REF]. The system consists of a 15-state error model where attitude is represented by means of quaternion. A very sophisticated step detector based on force sensors, attached to the shoe's sole, and hidden Markov model filtering is used to determine when to apply zero velocity measurements to the EKF. The system performance was tested with short paths of 14 meters long maximum. Therefore, no conclusions can be made about the advantage of using such a sophisticated step detector. Moreover, the need for additional sensors to detect a step may be a serious drawback. In a recent work by Bebek et al. [START_REF] Bebek | Personal navigation via high-resolution gaitcorrected inertial measurement units[END_REF], a position error of less than 1% was reported. This work incorporated an additional calibration based on the total drift accumulated during an initial walk. This information was used to correct subsequent walks to produce desirable results. Since these works were based on the ZVU, it was necessary to identify the time instants of swing phase and stance phase with a high degree of accuracy. Some researchers have developed specific electronic circuits to aid in the detection of the foot stance phase. For example, in [START_REF] Zhou | A Low-Power Shoe-Embedded Radar for Aiding Pedestrian Inertial Navigation[END_REF], a shoe-mounted radar was developed to detect the instances of the foot zero velocity.

In this paper, we propose a foot-mounted CF-aided IMU approach for pedestrian tracking in indoor environments without resorting to GPS data. The primary contributions of this paper are the following:

1) The use of robust CF instead of the known EKF. Indeed, in attitude estimation, EKF presents some drawbacks such as the difficulty to guarantee the global convergence of the filter due to the linear approximation of the nonlinear process model [START_REF] Foxlin | Pedestrian tracking with shoe-mounted inertial sensors[END_REF]. In this paper, the overall CF design is greatly simplified to accurately preprocess the sensors data from an IMU (accelerometer, magnetometer and gyroscope) and to estimate orientation in complementary way during both static and dynamic motion. Previous works in pedestrian navigation, for example [START_REF] Fischer | Tutorial: implementation of a pedestrian tracker using foot-mounted inertial sensors[END_REF], [START_REF] Fischer | Location and Navigation Support for Emergency Responders: A Survey[END_REF], [START_REF] Skog | Zero-velocity detection-An algorithm evaluation[END_REF][START_REF] Foxlin | Pedestrian tracking with shoe-mounted inertial sensors[END_REF][START_REF] Ojeda | Non-GPS Navigation for Security Personnel and First Responders[END_REF], estimate the quaternion of attitude only by integrating the well-known quaternion kinematic differential equation. However, this solution would be prone to drifting over time due to the build-up of bias and drift errors in gyroscope.

2) CF approach without the intermediate step of gyro bias estimation and correction. Previous attitude estimation approaches added this step, which increases the computing time [START_REF] Mahony | Nonlinear complementary filters on the special orthogonal group[END_REF], [START_REF] Cunha | Output-feedback control for stabilization on SE(3)[END_REF].

3) A reliable gait detection method requiring only input from foot-mounted accelerometer sensors without adding a threshold on gyroscope measurements as it was used in previous literature works [START_REF] Ojeda | Non-GPS Navigation for Security Personnel and First Responders[END_REF]. 4) Real-world experimental results, which indicate that the aforementioned methods are accurate. 5) A practical comparison with a previous famous work developed in [START_REF] Ojeda | Non-GPS Navigation for Security Personnel and First Responders[END_REF]. The remainder of this paper presents previous foot motion filtering algorithms and their drawbacks. Section II describes the proposed foot motion filtering algorithm and includes a separate section for each of the major components. Discussion and results pertaining to real-world experiments are presented in Section III with a comparison to previous work. The final section summarizes the conclusion that can be drawn from this paper.

II. FOOT MOTION ALGORITHM OVERVIEW

This section presents the motion filtering algorithm based on the use of a foot-mounted IMU. A block diagram of the approach is shown in Fig. 2. At any given moment, the gait phase (gait is the pattern of movement of the limbs of humans during locomotion over a solid substrate), denoted by a detector d , will have one of two values, which correspond to the swing phase (encompasses the entire time the foot is in the air for limb advancement) or the stance phase (encompasses the entire period during which the foot is on the ground) of the normal walking cycle. Each of the main components of the filter algorithm is described in detail in the following sub-sections.

1234567289 1234567289 A949B43CD A949B43CD EFC347 EFC347 1 p v 1 ⊕ + - G ˆn n q f q * ⊗ ⊗ f f CF99D42 CF99D42 3F49 3F49 C539D4243CD C539D4243CD 843243CD 843243CD G 1 h f ˆn q d A 1234567289 1234567289 A949B43CD A949B43CD EFC347 EFC347 1 p v 1 ⊕ + - G ˆn n q f q * ⊗ ⊗ f f CF99D42 CF99D42 3F49 3F49 C539D4243CD C539D4243CD 843243CD 843243CD G 1 h f ˆn q d A

A. Quaternion based CF for orientation estimation

The upper portion of Fig. 2 shows a CF for estimating orientation of a foot to which an IMU is attached. This part represents one of the technical contributions of the paper. The input to this filter is nine components: three components of the accelerometer measurement f , three components of the local magnetic field measurement h , and three components of the angular rate measurement G ω . The output of the filter is the estimated foot orientation represented by a quaternion n q . All measurements provided by the IMU are performed in the body-fixed frame ( ) As stated above, the objective of this section is to design a CF for tracking of foot orientation. To do so, it is necessary to establish a process model representing motion dynamics of the foot segment. For simplicity, we adopted the wellknown quaternion kinematic differential equation [START_REF] Fourati | Nonlinear Filtering Approach for the Attitude and Dynamic Body Acceleration Estimation Based on Inertial and Magnetic Sensors: Bio-logging Application[END_REF], [START_REF] Shuster | A survey of attitude representations[END_REF]:

, , B B B B X Y Z with respect to the Earth-fixed frame ( ) , , N N N N X Y Z ,
1 2 G q q ω = ⊗ 1 (1)
where the product ⊗ between q and g ω is quaternion multiplication.

0 2 3 = 4 5 T T vect q q q
is the theoretical unit quaternion that denotes the mathematical representation of rigid body attitude between two frames 1) body-fixed frame B and 2) Earth-fixed frame N . = 2 3 4 5

T vect q q q q represents the vector part of q . More details about quaternion can be found in [START_REF] Kuipers | Quaternion and Rotation Sequences[END_REF]. 0 ω is measured by a 3-axis gyroscope and can be often corrupted with noises and bias [START_REF] Brown | Introduction to Random Signal and Applied Kalman Filtering[END_REF]. Equation (1) describes the time rate of attitude variation as a result of rigid body angular rates measured by the gyroscope.

T T G g ω ω 2 3 = 4
Applying the quaternion multiplication in (1), we obtain:

, 3 3 ,0 , 1 2 T n vect n g n n vect q q I q q ω × × 2 3 - 6 7 = 2 3 6 7 + 4 5 4 5
1 (2)
where n q is the real quaternion,

, × 2 3 4 5
n vect q represents the skew-symmetric matrix [START_REF] Shuster | A survey of attitude representations[END_REF], [START_REF] Fourati | A quaternion-based Complementary Sliding Mode Observer for attitude estimation: application in free-ranging animal motions[END_REF]: q q q q q q q q q q

,1 ,3 ,2 , ,2 ,3 ,1 ,3 ,2 ,1 0 0 0 ×
(3) and 3 3 × I is the identity matrix of dimension 3.

The process model ( )

2 composed of (2)
and the linear measurement model y can be written such as:

( ) , 3 3 ,0 , 1 2 :
T n vect n g n n vect T x y z x y z q q I q q y f f f h h h ω × × 8 2 3 - 9 = 6 7 9 2 3 + 6 7 Σ 4 5 A 4 5 9 2 3 = 9 4 5 B 1 ( 4
)
where the output 6 y ∈ ℜ is built by stacking the accelerometer and magnetometer measurements. The relation between the outputs y and the state of the system n q will be mentioned later in equations ( 7) and ( 8).

The algorithm designed for orientation estimation is a type of filter that blends two sources of data in a complementary manner. In this case, the filter blends the static lowfrequency information provided by accelerometers and magnetometers, and the dynamic high-frequency information provided by the angular rate sensors. The aim of the CF is to ensure a compromise between the accuracy provided by short-term integration of the gyroscope data and the longterm measurements precision obtained by the accelerometer and the magnetometer [START_REF] Mahony | Nonlinear complementary filters on the special orthogonal group[END_REF]. To compensate for the drifts on the estimated quaternion that are observed during the integration of the differential equation in (4), a correction term T is introduced in this equation based on a quaternion product ⊗ . We propose the following CF: ,

ˆT

n vect n g n n vect q q 1 T I q q × × C D 2 3 - E F 6 7 = ⊗ E F 2 3 6 7 + 4 5 4 5
1 ( 5
)
where ˆn q denotes the estimated quaternion. The correction term T is calculated from a fusion approach of accelerometer and magnetometer data. To calculate the correction term T , we consider the modeling error ( ) ( ) ˆn 2 q y y = -. The estimated output is given by ŷ :

ˆˆˆˆˆˆT x y z x y z y f f f h h h 2 3 = 4 5 (6)
Measurements of the estimated accelerations ˆx f , ˆy f and ˆz f can be calculated assuming that there are no acceleration except gravity such as:

ˆˆˆˆˆ0 T x y z n q n f f f f q G q * 2 3 = = ⊗ ⊗ 4 5 (7)
where

[ ] 0 0 0 9.8 T q G =
is a pure vector quaternion with the scalar part equal to zero and the vector part [ ] 0 0 9.81 T G = corresponding to the theoretical components of the gravity. ˆn q * is the complementary quaternion of ˆn q , i.e.

,0 ,

ˆˆˆˆˆT

n n n n n q q q q q * 2 3 = - - - 4
5 .

Measurements of the estimated Earth's magnetic field ˆx h , ˆy h and ˆz h can be calculated such as:

ˆˆˆˆˆ0 T x y z n q n h h h h q m q * 2 3 = = ⊗ ⊗ 4 5 (8)
where

[ ] 0 0 T q x z m m m =
is a pure vector quaternion with the scalar part equal to zero and the vector part

[ ] 0 T x z m m m =
corresponding to the theoretical components of the Earth's magnetic field. Currently, the parameters of the theoretical model of the geomagnetic field m closest to reality can be deduced from [START_REF] Astrosurf | [END_REF]. In our case, we considered that the magnetic disturbances are low and their impact is negligible on the attitude estimation.

The minimization of the modeling error ( ) ˆn 2 q is performed from a regression method that minimizes the scalar squared error criterion function ( ) ˆn 3 q related to ( )

ˆn 2 q : ( ) ( ) ( ) ˆˆT n n n 3 q 2 q 2 q = (9)
The Levenberg Marquardt Algorithm is used to minimize the non-linear function ( ) ˆn 3 q . The unique solution to this problem can be written in the following form:

( ) ( )

ˆn n 4 q K2 q = ( 10
)
where

1 3 3 T T K k X X 5I X - × 2 3 = + 4 5
is the gain of the filter. is the Jacobian matrix defined by: 0 0

2 2 0 0 0 0 T z y z y T z x z x y x y x f f h h X f h f f h h f f h h × × - - 2 3 6 7 2 3 2 3 2 3 =- =- - - 6 7 4 5 4 5 4 5 6 7 - - 4 5 (11)
The positive constant 5 is chosen to ensure the nonsingularity of the minimization problem. The positive constant k combines low bandwidth accelerometer and magnetometer readings with high bandwidth gyroscope measurements. ( )

ˆn 4 q
represents a part of the correction term T . To achieve the quaternion product in (5), the term T must be of dimension 4 as:

( )

1 6 3 1 1 1 0 0 n T 2 q K × × 2 3 2 3 = 6 7 6 7 6 7 4 5 4 5 (12)
The scalar part of quaternion error is fixed to 1 to force the error quaternion to represent small angles of rotation. Finally, the complementary filter can be written as follows:

( )

, 3 3 ,0 , 1 0 0 0 0 0 0 ˆ1 0 1 ˆ0 2 ˆˆ0 T n vect n g n n n vect q q 1 2 q K I q q × × 2 3 6 7 C D 2 3 - 2

B. Position and Velocity Estimation with ZVU

The middle portion of Fig. 2 shows the position and velocity estimation approach. The input to this filter is the measured acceleration vector f in the body coordinates and the estimated quaternion representing the orientation of the body ( ) n q t generated by the complementary orientation filter, and the output is the estimated position ( ) p t and velocity ( ) v t relative to a fixed earth coordinate frame.

( ) n q t serves to transform the body coordinate acceleration into the earth coordinate system using the quaternion operator [START_REF] Kuipers | Quaternion and Rotation Sequences[END_REF]:

( ) ( ) * n n ˆâ q t f q t = ⊗ ⊗ (14)
The acceleration vectors f and a are treated as pure vector quaternions, with the scalar part being equal to zero when performing quaternion multiplication [START_REF] Kuipers | Quaternion and Rotation Sequences[END_REF]. After obtaining the acceleration vector 0

( ) ( ) e A t a t G = - (15)
The result of ( 15) is integrated to obtain the 3-D velocity vector in the earth coordinate system:

1 ( ) ( ) 2 1 ˆ. T T v t A t dt = 1 1 1 1 1 1 1 111 (16)
where [ ]

1 2
, T T denotes the sampling period. Theoretically, the velocity vector resulting from ( 16) can be immediately integrated once again to obtain position. However, due to the presence of measurement noise and drift in the measured acceleration vector f and the estimation errors in the estimated quaternion ( ) n q t , an immediate integration of ( ) v t results in unbounded error growth in position estimation in a relatively short time. An approach to reduce error growth in the position estimation is to apply a velocity correction method called the ZVU. This method is early introduced in [START_REF] Ojeda | Non-GPS Navigation for Security Personnel and First Responders[END_REF]. The concept of the ZVU is based on the observation that human foot motions are cyclic, and when a foot is in the stance phase or in contact with the ground, its velocity is theoretically to be zero. Due to bias error in acceleration measurements, the estimated foot velocity obtained from ( 16) may not be zero while the foot is in the stance phase. In this paper, we propose to correct the velocity bias error by resetting the velocity to zero at these moments. This corrected velocity is integrated again to obtain the estimated position ( )

p t such as: ( ) ( ) 2 1 ˆˆ. T T p t v t dt = 1 11 1 1 1 1 1111(17)
It is noted that all sensor measurements and position/velocity vectors are 3-D in this paper.

C. Gait phase detection algorithm

The use of the ZVU for correcting foot velocity needs to accurately detect the stance and swing phases of foot motion. The use of accelerometer data was examined for this purpose. The foot acceleration seemed to provide a means for the detection of the transitions between the stance phase and swing phase. The gait phase detection algorithm is essentially a state detector with two states Stance ( 0 d = ) and Swing ( 1 d = ). The operation of the state detector must be synchronized with the user's foot motion. That is, when the user's foot was in the stance phase, the state detector must be equal to 0. Conversely, when the user's foot is in the swing phase, the state detector should be equal to 1. When walking, the gait cycle begins just after the foot strikes the ground, usually with heels first, causing a large peak acceleration followed by an oscillation, see Fig. 3.

The foot remains stationary for approximately 0.5 s. During the stance phase the accelerometer should only measure the earth gravitational acceleration and the velocity is zero. In practice, this is not trivial, because the accelerometers suffer from drift, so they never show exactly gravity. To detect a walking event based on an accelerometer signal, we proceed as follows: We start by taking the squared Euclidean norm of the acceleration

2 2 i 6 f = : 2 2 2 2 2 i x y z 6 f f f f = = + + (18)
where x f , y f , z f are the acceleration output of the IMU in three different axes. Eq. ( 18) can be altered to include several stationary properties of the system. We calculate after the variance of the squared norm from a sliding window using the following equation [START_REF] Skog | Zero-velocity detection-An algorithm evaluation[END_REF]:

( ) ( ) ( ) 2
where i 6 is the squared norm of the measured acceleration f and ( )

i j
6 e is the mean of i 6 over e samples. Properly applied, the variance of the squared norm can be useful for detecting gait events where the signal changes suddenly, such as a heel strike and the stance phase initiation. If the variance of a manipulated signal is to be used for gait event detection, the sample size e must be chosen appropriately so that the variance test is adequately sensitive to slow signal changes and adequately responsive to fast ones. It was found that a threshold of five to ten was suitable for a gait event. This parameter was adjusted by trial-and-error until a satisfactory result was achieved. The variance should be limited during the stance phase below an experimental threshold L :

( )

2 e Var j L < (20)
The detector d , with two states Stance ( 1 d = ) and Swing ( 0 d = ), which is introduced in this work uses the requirement reported in [START_REF] Astrosurf | [END_REF]. If this requirement is satisfied, the detector 1 d = which mean that we belong to the stance phase, else we are in the Swing phase and 0 d = . The threshold L was adjusted by trial-and-error until a satisfactory result was achieved. It was found that a sample size of fifteen to twenty was suitable to better identify the stance and swing phases.

III. INDOOR EXPERIMENTS AND RESULTS

A. Foot-mounted IMU

To examine the effectiveness of the motion filtering algorithm based on the use of a foot-mounted IMU, several experiments have been performed for different walking scenarios. We used the MTi IMU module developed by Xsens Technologies [21] to collect data. The module is mounted on a foot as shown in Fig. 4. The module samples sensor data for a tri-axial accelerometer, a tri-axial gyroscope and a triaxial magnetometer at 100 Hz and transfers the raw data to a laptop over USB, which in turn timestamps and logs it. The collected data are processed offline using MATLAB. The software consists of the quaternion based CF, the position and velocity estimation with ZVU and the gait phase detection algorithm. Experiments were preformed at an indoor location to collect walk data for different users. The set of experiments were performed in the corridors of CReSTIC laboratory at Reims Champagne Ardenne University. Data was collected for 3 volunteers at this location. In order to establish the ground truth trajectory, the subjects were made to walk on a manually surveyed path marked on the corridor laboratory. In the experiment the path starts and ends at the same position. The path includes rectangle shaped walking (length is 80 m) and had several straight stretches followed by sharp right-angle turns in clockwise and anti-clockwise directions to test the accuracy of our motion filtering algorithm (see Fig. 5). In this paper, we choose to represent the results obtained from one volunteer. The same experiment was repeated four times to appropriately choose the sample size e and the threshold L . These parameters were adjusted by trial-anderror until a satisfactory result was achieved so that the variance test is adequately sensitive to slow signal changes and adequately responsive to fast ones. It was found that a sample size 8 e = and the threshold 16 L = are suitable for a gait event. Fig. 6 Var j and the states detector d Fig. 7 shows the three-axis foot velocity v prior to applying the ZVU. The presence of drift is evident in all three components of velocity. Moreover, the drift in the velocity appears to be linear, which confirms the assumption that the acceleration bias is constant over the short period of a swing phase. Fig. 7 shows also the same velocity v after applying the ZVU. It is seen that the corrected foot velocity during the stance phase is now zero. 7. Three-axis foot velocity v prior and after applying the ZVU Fig. 8 shows the 3D view reconstruction result of the rectangle shaped walking by using the proposed motion filtering algorithm which follows the designed trajectory with high consistence. The difference between the starting and ending point is smaller than 0.3 m. Then, the position estimation error is less than 0.37 % of the total distance traveled.

B. Comparison with previous work

In Fig. 9, we compare our reconstruction result of the walking trajectory with the one obtained from previous work developed by Ojeda and Borenstein [10]. Fig. 9. Walking experiment: The reconstruction of the rectangle trajectory using method developed by Ojeda and Borenstein [START_REF] Ojeda | Non-GPS Navigation for Security Personnel and First Responders[END_REF] In [START_REF] Ojeda | Non-GPS Navigation for Security Personnel and First Responders[END_REF], the authors used only gyroscope measurements to estimate the quaternion of attitude by integrating the wellknown quaternion kinematic differential equation. However, this solution would be prone to drifting over time due to the build-up of bias and drift errors. In contrary, the approach herein proposed takes advantage of the precision near to zero ( 1 °2 ), given by the rate gyros integration in the short term ( 1 sec ≈

) and the reliable long term accuracy provided by accelerometer and magnetometer measurements. The resulting structure of the proposed CF blends two frequency regions and is based on the complementary filtering theory: 1) the accelerometer and magnetometer data are characterized by a low frequency region, where the attitude is usually more accurate, 2) the gyroscope data is located in the high frequency region, where the integration of the angular velocity yields better attitude estimates.

It is obvious that the reconstruction result using the method in [START_REF] Ojeda | Non-GPS Navigation for Security Personnel and First Responders[END_REF] is deviated from the designed trajectory further. Our proposed approach improves the performance greatly.

IV. CONCLUSION This paper presents an algorithm for estimating human foot position during normal walking based on estimates of foot orientation, velocity, acceleration, and gait phase using inertial/magnetic sensor measurements. The measurements are provided by an IMU attached to a foot. Orientation estimation is accomplished by a quaternion-based CF to blend the high frequency information provided by angular rate sensors and the low-frequency information provided by accelerometers and magnetometers. For this purpose, a foot gait phase detection algorithm based on the use of accelerometer measurements was presented. With foot orientation readily available as a result of the quaternionbased CF, foot acceleration in the body coordinate frame is conveniently converted into the earth coordinate frame using the foot orientation quaternion. Foot velocity is obtained by numerically integrating corrected foot acceleration measurements obtained during the swing phase. Due to sensor noise, accelerometer measurements tend to drift. The drift is corrected using the ZVU technique. Experiments were conducted to evaluate the algorithm. The experimental results suggest that the achievable position accuracy of the algorithm is about 0.37 % of the total walked distance.

1

 1

Fig. 1 .

 1 Fig.1. Key phases in a stride. During 1T , all velocity components of point A in the sole of the boot are zero[START_REF] Ojeda | Non-GPS Navigation for Security Personnel and First Responders[END_REF]

Fig. 2 .

 2 Fig. 2. Block diagram of the foot motion tracking algorithm that produces the foot orientation quaternion, foot position, foot velocity, and gait phase. The upper, middle, and lower portions of the diagram correspond to the three main components of the algorithm. The upper portion of the diagram depicts the quaternionbased CF for estimating foot orientation represented by a quaternion n q from the acceleration measurement f , the local magnetic field measurement h , and the angular rate measurement G ω . The middle portion of the diagram depicts the position and velocity estimation filter. The outputs are the foot velocity estimate v and the foot position estimate p . The lower portion of the diagram shows the gait phase detection algorithm. At any given moment, the gait phase (gait is the pattern of movement of the limbs of humans during locomotion over a solid substrate), denoted by a detector d , will have one of two values, which correspond to the swing phase (encompasses the entire time the foot is in the air for limb advancement) or the stance phase (encompasses the entire period during which the foot is on

  5 is a pure vector quaternion with the scalar part equal to zero and the vector part the theoretical components of the angular rate, expressed in B .

  g

Fig. 3 .

 3 Fig. 3. The norm squared of a walking foot. The black arrow shows the stance, respective stride phase's duration

1

 1

Fig. 4 .

 4 Fig. 4. Foot-mounted MTi

1

 1

Fig. 5 .

 5 Fig. 5. Indoor walking path during a 80 m trajectory length

1 Fig. 6 .

 16 Fig. 6. The squared norm of the measured acceleration i 6 , the average ( ) i j mo 6 e = , the variance

Fig.

  Fig. 7. Three-axis foot velocity v prior and after applying the ZVU

1 Fig. 8 .

 18 Fig. 8. Walking experiment: The reconstruction of the rectangle trajectory

  depicts the squared norm of the measured acceleration i 6 , the mean

			mo	=	(	i 6 e	)	j	of i 6 over e
	samples, the variance	2	( )

e Var j of the squared norm and the states detector d . As we can see, the stance and swing phases are clearly identified.