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

Abstract

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.

1. Introduction

A pedestrian navigation system provides a person’s location indoors or outdoors. Many different technologies are used for the pedestrian navigation such as vision [1], wireless technology [2], ultrasonic sensors [3,4], and inertial sensors [5–10]. Among them, an inertial sensor-based pedestrian navigation system computes the location using inertial sensors installed on a shoe [5–10]. This inertial navigation system can be used for first respondents and soldiers [11].

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–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 algorithmis 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.

2. 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³, v ∈ R³ and q ∈ R⁴ 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͂ and m͂ be defined as follows:

• ωb ∈ R³ : body angular rates

• a_b ∈ R³ : body acceleration without gravitational acceleration

• g͂ ∈ R³ : gravitational acceleration vector in the navigation coordinate frame

• m͂ ∈ R³ : earth magnetic field vector in the navigation coordinate frame.

The sensor output equations are given by

$$\begin{array}{l}{y}_g={\omega }_b+b_g+v_g\\ y_a=C(q)\tilde{g}+a_b+b_a+v_a\\ y_m=C(q)\tilde{m}+v_m\end{array}$$

(1)

where y_g ∈ R³ is a gyroscope output, y_a ∈ R³ is an accelerometer output, y_m ∈ R³ is a magnetic sensor output, b_g ∈ R³ is gyroscope bias, b_a ∈ R³ is accelerometer bias, v_g ∈ R³ is gyroscope sensor noise, v_a ∈ R³ is accelerometer sensor noise, and v_m ∈ R³ 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.

3. 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

$${\theta }_{\mathit{groud}}=\theta -{\theta }_{\mathit{init}}.$$

(2)

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 quaternion q̂. 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

$$y_a\approx C(q)\left[\begin{array}{l}\begin{array}{c}0\\ 0\end{array}\\ g\end{array}\right]=\left[\begin{array}{l}\begin{array}{c}-\text{sin}\theta \\ \text{cos}\theta \text{sin}\phi \end{array}\\ \text{cos}\theta \text{cos}\phi \end{array}\right]g$$

where φ is the pitch angle. Thus θ and φ can be estimated using the following:

$$\begin{array}{lll}\varphi \hfill & =\hfill & \text{atan}2(y_{a,y},y_{a,z})\hfill \\ \theta \hfill & =\hfill & \text{atan}2(-y_{a,x},\hspace{0.17em}\sqrt{y_{a,y}^2\hspace{0.17em}+\hspace{0.17em}{y}_{a,z}^2}).\hfill \end{array}$$

(3)

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

$$|{\theta }_i-{\theta }_{i-1}|<2^{\circ }.$$

(4)

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_k1 is the foot position at the discrete time k₁. In this example, the foot is on the slope at the discrete time k₁ and k₂. A person walked one step between the time k₁ and k₂. Thus r_k1 is the position before one step walking and r_k2 is the position after the step. Let δ_z,k1,k2 and δ_xy,k1,k2 be the horizontal and vertical distances between r_k1 and r_k2, respectively:

$$\begin{array}{l}{\delta }_{\mathit{xy}}={\Vert \left[\begin{array}{l}\begin{array}{ccc}1& 0& 0\end{array}\\ \begin{array}{ccc}0& 1& 0\end{array}\end{array}\right](r_{k_2}-r_{k_1})\Vert }_2\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{\delta }_z=\left[\begin{array}{ccc}0& 0& 1\end{array}\right](r_{k_2}-r_{k_1}).\end{array}$$

(5)

Note that δ_z,k1,k2 and δ_xy,k1,k2 are horizontal and vertical distances of one walking step. We will drop k₁ and k₂ subscripts in δ for simplicity.

Assuming that walking is mostly up or down along the slope, δ_z and δ_xy have the following relationship:

$${\delta }_z\approx \text{tan}({\theta }_{\mathit{ground}}){\delta }_{\mathit{xy}}.$$

(6)

Equation (6) is used in the measurement update of the Kalman filter in Appendix. Let θ̂_k1 be estimated θ value at time k₁ using Equation (3) and r̂_k1 be the position estimate of the inertial navigation algorithm. Let ${\widehat{r}}_{k_2}^{-}$ be the position estimate of the inertial navigation algorithm before the measurement update (that is, the zero velocity updating). Thus ${\widehat{r}}_{k_2}^{-}$ is obtained by double integrating acceleration starting from time k₁ with the initial value r̂_k1.

From Equation (6), let δ̂_z (estimate of δ_z) be defined by

$${\widehat{\delta }}_z=\text{tan}({\widehat{\theta }}_{k_1}-{\theta }_{\mathit{init}}){\Vert \left[\begin{array}{l}\begin{array}{ccc}1& 0& 0\end{array}\\ \begin{array}{ccc}0& 1& 0\end{array}\end{array}\right]({\widehat{r}}_{{}_{k_2}}^{-}-{\widehat{r}}_{k_1})\Vert }_2.$$

(7)

Note that δ̂_z in Equation (7) is the vertical distance (height difference) computed using the ground inclination angle.

We have assumed that θ̂_k1 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

$$\left[\begin{array}{l}\begin{array}{ccc}1& 0& 0\end{array}\\ \begin{array}{ccc}0& 1& 0\end{array}\end{array}\right]({\widehat{r}}_{k_2}^{-}-{\widehat{r}}_{k_1})\approx \left[\begin{array}{l}\begin{array}{ccc}1& 0& 0\end{array}\\ \begin{array}{ccc}0& 1& 0\end{array}\end{array}\right](r_{k_2}-r_{k_1}).$$

With the assumptions we have the following approximation from Equations (5) and (6):

$${\widehat{\delta }}_z\approx \left[\begin{array}{ccc}0& 0& 1\end{array}\right](r_{k_2}-r_{k_1}).$$

(8)

Let v_z be the approximation error in Equation (8), we can rewrite Equation (8) as following:

$$\begin{array}{l}{\widehat{\delta }}_z=\left[\begin{array}{ccc}0& 0& 1\end{array}\right](r_{k_2}-r_{k_1})+v_z\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}=\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\left[\begin{array}{ccc}0& 0& 1\end{array}\right](r_{k_2}-{\widehat{r}}_{k_2}^{-}+{\widehat{r}}_{k_2}^{-}-r_{k_1})+v_z\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}=\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\left[\begin{array}{ccc}0& 0& 1\end{array}\right](r_{e,k_2}+{\widehat{r}}_{k_2}^{-}-{\widehat{r}}_{k_1}-r_{e,k_1})+v_z\end{array}$$

(9)

where recall that $r_{e,k}=r_k-{\widehat{r}}_k^{-}$ in Equation (13).

Let $v_{\delta }=v_z-\left[\begin{array}{ccc}0& 0& 1\end{array}\right]r_{e,k_1}$, then Equation (9) can be written as follows:

$${\widehat{\delta }}_z-\left[\begin{array}{ccc}0& 0& 1\end{array}\right]({\widehat{r}}_{k_2}^{-}-{\widehat{r}}_{k_1})=\left[\begin{array}{ccc}0& 0& 1\end{array}\right](r_{k_2}-{\widehat{r}}_{k_2}^{-})+v_{\delta }$$

where v_δ represents the approximation errors.

We have the following measurement equation for the Kalman filter in Appendix:

$${\widehat{\delta }}_z-\left[\begin{array}{ccc}0& 0& 1\end{array}\right]({\widehat{r}}_{k_2}^{-}-{\widehat{r}}_{k_1})=\left[\begin{array}{ccccccccccccccc}0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right]x_{k_2}+v_{\delta }.$$

(10)

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 $\mathrm{E}\left\{v_{\delta ,k_2}{v}_{\delta ,k_2}^{\prime }\right\}$ in the paper.

The proposed algorithm combined with the inertial navigation algorithms summarized in the following:

• while (true)

•     compute ${\widehat{r}}_k^{-}$, ${\widehat{v}}_k^{-}$ and ${\widehat{q}}_k^{-}$

•     if (zero velocity interval)

•       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)

•       update r̂_k, v̂_k and q̂_k

•     else

•        ${\widehat{r}}_k={\widehat{r}}_k^{-}$, ${\widehat{v}}_k={\widehat{v}}_k^{-}$ and ${\widehat{q}}_k={\widehat{q}}_k^{-}$

•     end

•     k = k + 1

• end

4. Experiments

As an inertial sensor unit, XSens MTi28A53G25 is used, whose specifications are given in

Table 1. Specifications of XSens MTi28A53G25.

	rate of turn	acceleration	magnetic field
full scale	±1, 500°/s	±50 m/ s2	±750 mGauss
bandwidth	40 Hz	30 Hz	10 Hz
bias stability	20°/ h	0.02 m/s2	0.1 mGauss
noise	0.05°/ s/ $\sqrt{\text{Hz}}$	0.002 m/ s2/ $\sqrt{\text{Hz}}$	0.5 mGauss

.

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² 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 thus θ̂_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₁||₂, where r_N is the final position and r₁ 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. z axis position error (average value of 3 experiments).

		position error without compensation (m)	position error with compensation (m)
road A		5.76	0.41
road B	walking up	1.59	0.05
	walking down	3.51	0.30
road C	walking up	1.64	0.40
	walking down	8.94	0.79
road D	walking up	4.62	1.19
	walking down	8.33	2.11

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 and the average road inclination angle is 7.83°. A person walked up and down 3 times. The z axis position error with the height compensation was 0.45, 0.43, 0.15 m (walking up) and 0.41, 0.30, 0.21 m (walking down). Thus we can see the proposed algorithm is working when φ is not zero.

5. 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.