Height Compensation Using Ground Inclination Estimation in Inertial Sensor-Based Pedestrian Navigation

In an inertial sensor-based pedestrian navigation system, the position is estimated by double integrating external acceleration. A new algorithm is proposed to reduce z axis position (height) error. When a foot is on the ground, a foot angle is estimated using accelerometer output. Using a foot angle, the inclination angle of a road is estimated. Using this road inclination angle, height difference of one walking step is estimated and this estimation is used to reduce height error. Through walking experiments on roads with different inclination angles, the usefulness of the proposed algorithm is verified.

The inertial-based navigation algorithms can be classified into two groups. One is that a step length is estimated using some parameters such as one step walking time or accelerometer maximum values [5,6]. The other is that the position is computed using the inertial navigation algorithm [12], where results in [7][8][9][10] belong to this group. Basic inertial pedestrian navigation algorithms are proposed in [7,8].
In [9], inertial navigation algorithm is combined with a map matching algorithm. In [10], an HMM filter is used to estimate gait phases and detect zero velocity intervals. Also we note that the inertial navigation algorithm is also used to analyze gait for medical purposes [13,14]. This paper is related to the latter, where the inertial navigation algorithm is used. A main advantage of an inertial sensor-based algorithm is that no landmarks or devices need to be installed on the environment. On the other hand, an inertial navigation algorithm has an inherent problem that the position error grows as time elapses.
To reduce the error growth, all inertial-based algorithms employ the zero velocity updating [15]. During walking, a foot touches the ground almost periodically for a short time interval and we know the velocity of a foot during the interval is zero. This interval is called a zero velocity interval. The errors of the inertial navigation algorithm are reduced using this zero velocity interval.
However, even with the zero velocity, the error growth can be significant. Experiment results reveal that z axis position (height) error growth could be in particular significant (see left plots in Figure 8 and Figure 9). One explanation is that acceleration in the z axis could be very large when a foot touches the ground and z axis acceleration data could be saturated (see Figure 4). This will degrade the position accuracy of all three axes and the effect is most dominant in the z axis position and velocity since the acceleration is the largest in the z axis direction.
In this paper, we propose a new algorithm, which reduces the z-axis position error. We use the fact that a foot angle is almost the same as the inclination angle of a ground when a foot is on the ground (see Figure 2). If we measure a foot angle during the zero velocity (that is, when a foot is on the ground), the inclination angle of a ground can be estimated. Using the estimated inclination angle, the z-axis position error can be compensated.
The paper is organized as follows. In Section 2, basic equations of inertial sensor-based pedestrian navigation algorithm is introduced and detailed equations are given in Appendix. In Section 3, a height compensation algorithm using a foot angle estimation is proposed. In Section 4, the proposed algorithm is verified through experiments. Conclusion is given in Section 5.

Inertial Sensor-Based Pedestrian Navigation System
In this section, a brief introduction to an inertial navigation algorithm is given. Details about inertial navigation algorithms can be found in [12,16,17]. The inertial navigation algorithm used in this paper is from [10].
Let r ∈ R 3 , v ∈ R 3 and q ∈ R 4 be a position, a velocity and attitude quaternion in the navigation coordinate frame. The navigation coordinate frame has axes with the direction north (x axis), west (y axis), and the local vertical (up, z axis). The body coordinate frame is fixed on the sensor unit. The rotation matrix associated with the quaternion q is expressed as C(q). In the inertial navigation algorithm, r, v and q are estimated from inertial sensors (accelerometers and gyroscopes) and magnetic sensors.
Let ω b , a b ,g andm be defined as follows: • ω b ∈ R 3 : body angular rates • a b ∈ R 3 : body acceleration without gravitational acceleration •g ∈ R 3 : gravitational acceleration vector in the navigation coordinate frame •m ∈ R 3 : earth magnetic field vector in the navigation coordinate frame.
The sensor output equations are given by where y g ∈ R 3 is a gyroscope output, y a ∈ R 3 is an accelerometer output, y m ∈ R 3 is a magnetic sensor output, b g ∈ R 3 is gyroscope bias, b a ∈ R 3 is accelerometer bias, v g ∈ R 3 is gyroscope sensor noise, v a ∈ R 3 is accelerometer sensor noise, and v m ∈ R 3 is magnetic sensor noise. An indirect Kalman filter is used to estimate q, r and v. In an indirect filter, q, r and v are not directly estimated. Instead q, v and r are first estimated by appropriately integrating or double integrating y g and y a and their errors are estimated using a Kalman filter [10]. The indirect Kalman filter equations are given in Appendix.
The following assumptions are made in the paper: • Walking direction is only forward and no side walking and backward walking are allowed.
• There is no staircase on the walking path The assumptions are satisfied during normal walking situations: that is, a person walks forward only and stopping is allowed.

Height Compensation Algorithm
The inertial sensor unit is installed on top of a shoe as in Figure 1. When a person is standing on a flat ground, the unit is not completely level and the roll angle at that time is denoted by θ init . When a foot is on a slope with the inclination angle θ ground (see Figure 2), the roll angle θ of the sensor unit is different from θ init , which is the roll angle when a foot is on a flat ground. The relationship between θ and θ ground is given by Since θ init is constant, we can estimate the ground inclination angle θ ground once we know the roll angle θ. The roll angle θ can be computed from the attitude quaternionq. Or θ can be also estimated using accelerometer outputs during the zero velocity interval since there is no external acceleration. From Equation (1), we obtain the following ignoring sensor noises and external acceleration where φ is the pitch angle. Thus θ and φ can be estimated using the following: In this paper, roll angle θ is estimated using Equation (3) during each zero velocity interval: θ is computed for each discrete time during a zero velocity interval and the averaged value is used asθ, which is an estimated value of θ. Thusθ is updated whenever zero velocity intervals are encountered. In the update, a low pass filter is used to suppress a sudden change ofθ.
There are many methods to detect zero velocity intervals [15]. In this paper, we used both accelerometer values and force sensors (Tekscan FlexiForce Sensors), which are installed inside a shoe. Note that the measured force increases when a foot is on the ground since the human weight is applied on the sensors. We assume a discrete time i belongs to a zero velocity interval if the measured force sensor is larger than the prespecified value and θ i (θ value computed using Equation (3) at the discrete time i) satisfies the following Note that Equation (4) is equivalent to the condition that changes of accelerometers are small. A typical foot movement trajectory when a person is walking on a slope is given in Figure 3. Note that r k 1 is the foot position at the discrete time k 1 . In this example, the foot is on the slope at the discrete time k 1 and k 2 . A person walked one step between the time k 1 and k 2 . Thus r k 1 is the position before one step walking and r k 2 is the position after the step. Let δ z,k 1 ,k 2 and δ xy,k 1 ,k 2 be the horizontal and vertical distances between r k 1 and r k 2 , respectively: Note that δ z,k 1 ,k 2 and δ xy,k 1 ,k 2 are horizontal and vertical distances of one walking step. We will drop k 1 and k 2 subscripts in δ for simplicity.
Assuming that walking is mostly up or down along the slope, δ z and δ xy have the following relationship: Equation (6) is used in the measurement update of the Kalman filter in Appendix. Letθ k 1 be estimated θ value at time k 1 using Equation (3) andr k 1 be the position estimate of the inertial navigation algorithm. Letr − k 2 be the position estimate of the inertial navigation algorithm before the measurement update (that is, the zero velocity updating). Thusr − k 2 is obtained by double integrating acceleration starting from time k 1 with the initial valuer k 1 .
From Equation (6), letδ z (estimate of δ z ) be defined bŷ Note thatδ z in Equation (7) is the vertical distance (height difference) computed using the ground inclination angle. We have assumed thatθ k 1 is relatively accurate and errors in x and y position estimation in the inertial navigation algorithm are small: that is, we have assumed the following is satisfied With the assumptions we have the following approximation from Equations (5) and (6): Let v z be the approximation error in Equation (8), we can rewrite Equation (8) as following: where recall that r e,k = r k −r − k in Equation (13). Let v δ = v z − 0 0 1 r e,k 1 , then Equation (9) can be written as follows: where v δ represents the approximation errors.
We have the following measurement equation for the Kalman filter in Appendix: This measurement update equation is combined with the zero velocity updating equation in Equation (16). Note that the measurement noise v δ contains all the approximation errors in the derivation of Equation (10). So an exact analytic formulation for the covariance is not easy to derive. A small positive value is assigned to E{v δ,k 2 v ′ δ,k 2 } in the paper. The proposed algorithm combined with the inertial navigation algorithms summarized in the following: if ( the start of the zero velocity interval ) θ ground =θ ground,previous else if ( the end of the zero velocity interval ) computeθ ground,previous using Equations (2) and (3) end zero velocity updating Equation (16) and height compensation Equation (10) updater

Experiments
As an inertial sensor unit, XSens MTi28A53G25 is used, whose specifications are given in Table 1. In Figure 4, typical accelerometer data (y a ) are given. The accelerometer output from XSens MTi28A53G25 is a low pass filtered signal with the bandwidth 30 Hz (see Table 1). The full scale range of the accelerometer is 50 m/s 2 and note that there is saturation in y a,z , which may cause large z axis velocity and position errors. The detected zero velocity interval is also given.  To test the proposed algorithm, four roads are selected (see Figure 5). These roads are more than 50 m long and the inclination angles are almost constant. The inclination angle of each road is measured with a digital inclinometer: inclination angles are measured at several points (13-17 points for each road) and the average value is considered as the inclination angle of a road. The results are given in Figure 6.  In Figure 5, the inclination angles of four roads are −0.046 • , 2.54 • , 6.14 • and 7.52 • , respectively. We walked up on each road 50 m and computed θ angle using Equation (3) during the angle measurement interval. In Figure 7, computedθ ground using Equations (2) and (3) is given. Note that each point in the figure corresponds to a computedθ ground for each walking step. For the reference, the road inclination angles are also given. We can see thatθ ground is close to the inclination angle and thuŝ θ ground can be used as a road inclination angle estimate. The z axis position estimation (the third element of r) result for road A (indoor corridor) is given in Figure 8. The left graph in Figure 8 shows the z axis position estimation without the height compensation, where the inertial navigation algorithm with zero velocity updating is used. The straight line in the plot is the estimated actual z axis position, which is computed from the inclination angle of the road and the walking distance (50 m). We can see the error increases rapidly. The error growth depends on many elements such as sensor scaling factor calibration, bias stability, sensor axis alignments and sensor saturation. We only performed simple calibrations. An initial gyroscope bias is estimated by averaging initial 1 minute gyroscope data while the sensor unit is not moving. Also, the accelerometer offset is estimated by rotating the accelerometer 360 • and finding the center value. With this simple calibration, the error seems to be large. The right graph in Figure 8 shows that the z axis position is corrected using the proposed height compensation algorithm. The z axis position estimation result for road C (inclination angle of the road is 6.14 • ) is given in Figure 9, where a person walked up 50 m along the road. The straight line is drawn between 0 and the computed final z axis position (50 m × sin(6.14 • ) = 5.348 m). Note that we measured 50 m using a tape measure on the road and thus 50 m corresponds to r N − r 1 2 , where r N is the final position and r 1 is the initial position. Without the height compensation, we can see that the z axis position error is large (final z axis position error is 3.61 m). On the other hand, with the height compensation, the z axis position error compensation is greatly reduced (final z axis position error is 0.62 m).
For the same road C, we walked down 50 m along the road and the result is given in Figure 10. It can be seen that without the height compensation, the z axis position error diverges quickly. In the right plot, it can be seen that the z axis position is compensated with the height compensation algorithm.
For four roads, three walking experiments are done. The average z axis position errors are given in Table 2 without and with the compensation algorithm. The true final z position is computed using sin(θ ground ) × 50 m. We can see that the proposed height compensation algorithm reduces the z axis position error significantly. In Table 2, position errors (with compensation) of Road D seem to be large. We believe this is due to the fact that the road D does not have a smooth surface, which can be verified from Figure 6. Thus the computed true height (that has been estimated using the estimated slope angle 7.52 • ) may not be accurate.  We note although the proposed method reduces the z axis position error growth, the position error divergence cannot be avoided over the long time.
Now instead of walking up and down along the slope, a person walked up and down the slope diagonally. In this experiment, pitch angle φ is not zero. We measured road inclination angles along line B in Figure 11 11. Height compensation experiment while walking up and down the slope diagonally.

Conclusions
In pedestrian navigation systems using inertial navigation algorithm, position error tends to diverge sooner or later. To reduce the position error growth, a zero velocity updating algorithm is used. Even with the zero velocity updating algorithm, position error growth could be still large. In particular, the z axis position (height) error growth could be significant.
In this paper, we have proposed a height compensation algorithm. An inclination angle of a road is estimated using foot angle estimation. Using the inclination angle, the height difference of a walking step is estimated. Using this estimation, z axis position in the inertial navigation algorithm is compensated. Through walking test under four different roads (with different inclination angles), the usefulness of the proposed method has been shown. Four different roads (with different inclination angle), 50 m walking test was done. Without the proposed height compensation algorithm, the average z axis position error range was 1.64-8.94 m over 50 m walking. On the other hand, the average error range with the proposed height compensation algorithm was 0.05-2.11 m.
We note that although the proposed method reduces the z axis position error growth, the position error divergence cannot be avoided over the long time. To avoid the divergence problem, external reference such as GPS should be used.
The current algorithm assumes that a person walks up or down on a slope direction. The current algorithm cannot deal with the staircase walking. The future work is to improve the proposed algorithm to cope with various situations such as the staircase walking. One possible solution is to use gait phase information (which can be determined using inertial sensors and force sensors [18]) to determine whether a person is stair climbing or descending.
where Ω(y g ) is defined by 0 −y g,x −y g,y −y g,z y g,x 0 y g,z −y g,y y g,y −y g,z 0 y g,x y g,z y g,y −y g,x 0 whereg is the gravitational acceleration vector. If there are no sensor noises and bias terms in y a and y g ,q,r, andv should be 100% accurate. For example, if y g = ω b , thenq = q (that is, there is no error inq). Due to sensor noises, however,q,r, and v contain errors.
Let q e , r e , v e be errors inq,r, andv, which are defined by where ⊗ is the quaternion multiplication. If the error q e is small, it can be approximated by The errors are estimated using a Kalman filter and the state for the Kalman filter is given by The state equation is given byẋ where A and B are given by (I 3 ∈ R 3×3 is an identity matrix and 0 3 ∈ R 3×3 is a zero matrix) For a vector p ∈ R 3 , [p×] is defined by The noise w bg and w ba are process noises for compensation of slowly time-varying gyroscope and accelerometer bias. As usual, we assume all noises in Equation (15) are uncorrelated, zero mean white Gaussian. When a foot is on the ground and thus is not moving, we can use the fact v = 0 in the measurement update of the Kalman filter. The zero velocity interval can be detected using y a and y g : if y a change is small and y g is small for more than a certain period, we can consider a foot is not moving [15,19]. The zero velocity interval also can be detected using the force sensors installed on a shoe. In this paper, the zero velocity interval is detected using y a change and the force sensors.
During the zero velocity interval, we can use the fact v = 0 in the measurement updating as follows: The velocity measurement y v is given by where v v is the measurement noise. We assume that the measurement noise v v is a uncorrelated white Gaussian noise. Let R v be the covariance of v v , which is defined by E{v v (t)v v (s) ′ } = R v δ(t − s). During the zero velocity interval (that is v = 0), we use 0 −v as an output to the indirect Kalman filter, where the output equation is given by In the zero velocity updating, v v is not the actual measurement noise since we are not directly measuring v but indirectly estimating v = 0. Thus covariance R v indicates our confidence in the zero velocity algorithm. If we use R v = 0 3 , we are assuming that the zero velocity algorithm is 100% correct. We note that there is a slight chance that the zero velocity interval detection is wrong. Thus we assign small positive value to R v . In the zero velocity updating, the velocity, position and attitude errors are greatly reduced [7,8].
During the zero velocity interval, we also update the heading using y m . As in [7], heading is only compensated at the end of the zero velocity interval since the attitude is most accurate at that time. For the heading update equation, we used the technique in [20], where y m only affects heading not pitch and roll. See Equation (21) in [20].
The sampling rate of the inertial sensors is 100 Hz and the appropriate discretized equations are used.