Foot Pose Estimation Using an Inertial Sensor Unit and Two Distance Sensors

Abstract

There are many inertial sensor-based foot pose estimation algorithms. In this paper, we present a methodology to improve the accuracy of foot pose estimation using two low-cost distance sensors (VL6180) in addition to an inertial sensor unit. The distance sensor is a time-of-flight range finder and can measure distance up to 20 cm. A Kalman filter with 21 states is proposed to estimate both the calibration parameter (relative pose of distance sensors with respect to the inertial sensor unit) and foot pose. Once the calibration parameter is obtained, a Kalman filter with nine states can be used to estimate foot pose. Through four activities (walking, dancing step, ball kicking, jumping), it is shown that the proposed algorithm significantly improves the vertical position estimation.

1. Introduction

Foot pose (position and orientation) estimation is used in many areas, such as gait analysis [1–3], exergaming [4,5] and pedestrian navigation systems [6]. The most accurate method to estimate foot pose is using an optical motion tracker. However, optical motion tracking is rather expensive, and it can only capture the motion in a limited space, which is determined by the number of cameras.

Recently, inertial sensor-based foot pose estimation methods are increasingly used, since the sensor can be attached to a shoe, which does not require any infrastructure in an environment and, thus, removes the space constraint imposed by optical motion trackers. As inertial sensors are becoming less expensive and smaller, inertial sensor-based motion tracking (ISBMT) is expected to be more popular.

The basic principle of an ISBMT is integration of gyroscope output (which gives the orientation) and double integration of accelerometer outputs (which gives the position). In the process, the sensors noises are also integrated. For example, if there is a nonzero accelerometer sensor bias term, it affects the position estimation error proportional to the square of elapsed time [7]. Furthermore, initial orientation error also significantly affects the position estimation error, since the gravitational acceleration is not correctly removed during the position estimation [7].

In foot pose tracking, the error increase can be mitigated using zero velocity updating [8,9]. Whenever a foot touches the ground, we know the velocity of the foot is zero. Using this zero velocity information, the estimation error can be significantly reduced [6].

Another approach to reduce the estimation error is to use a smoother [10]. The smoother uses both prior and posterior data to estimate the current state. This combination reduces the estimation error. One drawback of a smoother is that its computation cannot be done online and, thus, is not suitable for applications, such as gaming.

The pose estimation error also can be reduced by using additional sensors. In [11], a pressure sensor is attached on a shoe to accurately detect the zero velocity intervals, which increases the estimation accuracy. In [12], an ultrasound range sensor is used to measure the distance to walls. The ultrasound sensor does not improve foot pose estimation, but improves the location accuracy for indoor pedestrian navigation. In [13], a camera is used to read markers on the floor, which gives the absolute position and orientation of a foot. One disadvantage of this approach is that the markers must be installed on the floor, so the experiment space is limited by the number of makers. In [14], a camera and infrared LEDs are used to measure the relative pose between two feet.

In this paper, we present a methodology to improve the accuracy of foot pose estimation by attaching low-cost distance sensors on a shoe in addition to an inertial sensor unit. Since the distance sensor gives height information, it can help to improve height estimation accuracy. The improved accuracy could be helpful in gait analysis and exergaming applications. The foot pose estimation algorithm is implemented using a Kalman filter. To combine inertial sensors and distance sensors, the relative pose between two sensors should be known. This pose parameter is included in the proposed Kalman filter.

The paper is organized as follows. In Section 2, the system hardware and coordinate systems are introduced. In Section 3, dynamic equations of the Kalman filter for ISBMT are given. In Section 4, measurement equations of the Kalman filter are provided. In Section 5, the proposed algorithm is tested, and the estimated positions are compared with optical tracker measurement values. The discussion is given in Section 6.

2. System Overview

An inertial measurement unit (three-axis accelerometers and three-axis gyroscopes, Xsens MTi) is attached on a shoe as in Figure 1. The sampling frequency of the inertial sensors is 100 Hz. Two distance sensors (VL6180 [15]) are also attached on a shoe, where symbols A and B are used to distinguish the two sensors. The distance sensor VL6180 measures the distance by measuring the time-of-flight of infrared light, and the measurement range is up to 20 cm. This sensor is most often used as a proximity sensor in smartphones. The sampling frequency of the distance sensors is 33.33 Hz.

Two coordinate systems are used in this paper: the body and world coordinate systems. The three axes of the body coordinate system coincide with the three axes of the inertial sensor unit. The z axis of the world coordinate system is in the direction of the local gravitational field: the z axis is pointing upward. The x and y axes are chosen arbitrarily. The origin of the world coordinate system is assumed to be on the floor.

The relative position and orientation of a distance sensor with respect to the inertial sensor unit are required in the foot pose estimation algorithm. As shown in Figure 1, the positions of two distance sensors are denoted [r_A]_b ∈ R³ and [r_B]_b ∈ R³, which are the position coordinates in the body coordinate system. The notation [p]_b ([p]_w) for a vector p ∈ R³ is used to emphasize that a vector is represented in the body (world) coordinate system. When there is no confusion, [p]_b (or [p]_w) is just expressed by p. The pointing direction of the distance sensor is denoted by a unit vector n_A ∈ R³ and n_B ∈ R³.

The distance sensor parameters (r_A, r_B, n_A, n_B in Figure 1) could be determined using a ruler and a protractor. It would be, however, not easy to determine the parameters with high accuracy. Thus, the small errors in the parameters are estimated in the Kalman filter. We model the parameters as follows:

$$\begin{array}{ccc}{r}_A& =& {\widehat{r}}_A+{\overline{r}}_A\\ r_B& =& {\widehat{r}}_B+{\overline{r}}_B\\ n_A& =& {\widehat{n}}_A+{\overline{n}}_A\\ n_B& =& {\widehat{n}}_B+{\overline{n}}_B\end{array}$$

(1)

where r̂_A ∼ n̂_B are initial estimated parameter values (most likely measured values using a ruler and a protractor) and r̄_A ∼ n̄_B are errors in the parameter estimation.

The distance between two distance sensors is denoted by d_AB = ‖r_A − r_B‖, which is measured by a ruler. This scalar quantity can be measured more accurately comparing with other vector parameters, which are rather difficult to measure and sometimes require guesswork. Thus, the error of d_AB is not estimated. The measured value of d_AB is denoted by z_AB:

$$z_{AB}=d_{AB}+v_{AB}$$

(2)

where v_AB is the measurement noise.

3. Kalman Filter for ISBMT

In this section, the foot pose tracking algorithm is given.

Let r ∈ R³ and v ∈ R³ denote the position and velocity of the inertial sensor unit in the world coordinate system. Let q ∈ R⁴ be the quaternion [16] representing the rotation relationship between the body and world coordinate systems. Let C(q) ∈ SO(3) be the rotation matrix corresponding to the quaternion q.

3.1. Basic Pose Equations

The basic equations for r, v and q are given by [17]:

$$\begin{array}{ccc}\dot{q}& =& \frac{1}{2}\mathrm{\Omega }\left(\omega \right)q\\ \dot{v}& =& C{\left(q\right)}^{\prime }{\left[a\right]}_b\\ \dot{r}& =& v\end{array}$$

(3)

where [a]_b ∈ R³ is the acceleration expressed in the body coordinate system and ω = [ω_x ω_y ω_z]′ is the angular velocity of the body coordinate system with respect to the world coordinate system. The symbol Ω(ω) is defined by:

$$\mathrm{\Omega }\left(\omega \right)\triangleq \left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]$$

3.2. Numerical Integration

The basic principle of ISBMT is that q, v and r can be computed by numerically integrating Equation (3) [7]. To do that, we need to know ω and [a]_b, which can be measured using gyroscopes and accelerometers.

The gyroscope output (y_g ∈ R³) and accelerometer output (y_a ∈ R³) can be modeled as follows:

$$\begin{array}{lll}{y}_g& =& \omega +v_g\\ y_a& =& {\left[a\right]}_b+C\left(q\right){\left[\tilde{g}\right]}_w+v_a\end{array}$$

(4)

where [g̃]_w = [ 0 0 g ]′ ∈ R³ is the local gravitational vector in the world coordinate system and g is the magnitude of the gravitational acceleration. The measurement noises v_g and v_a are assumed to be uncorrelated zero mean white Gaussian.

We can integrate Equation (3) by replacing ω by y_g and replacing [a]_b by y_a − C(q)g̃. The numerical integration algorithm is given in [18]. Let the integrated values be denoted by q̂, r̂ and v̂.

Since ω ≠ y_g and [a]_b ≠ y_a − C(q̂)g̃, there are errors in q̂, r̂ and v̂, which are denoted by q̄ ∈ R³, r̄ ∈ R³ and v̄ ∈ R³:

$$\begin{array}{lll}\overline{q}& =& \left[\begin{array}{cc}{0}_{3\times 1}& I_3\end{array}\right]\left({\widehat{q}}^{*}\otimes q\right)\\ \overline{r}& =& r-\widehat{r}\\ \overline{v}& =& v-\widehat{v}\end{array}$$

(5)

where ⊗ denotes the quaternion multiplication and q* is the conjugate quaternion of q ∈ R⁴. The definition of q̄ ∈ R^3×1 is from the assumption that the estimation error of q̂ is small, and thus, the following is satisfied [19]:

$$q\approx \widehat{q}\otimes \left[\begin{array}{c}1\\ \overline{q}\end{array}\right]$$

(6)

The errors q̄, r̄ and v̄ along with the parameter errors r̄_A ∼ n̄_B in Equation (1) are estimated in the Kalman filter. Combining nine states in Equation (5) and 12 states in Equation (1), we have the following state for a Kalman filter:

$$x=\left[\begin{array}{c}\overline{q}\\ \overline{r}\\ \overline{v}\\ {\overline{r}}_A\\ {\overline{r}}_B\\ {\overline{n}}_A\\ {\overline{n}}_B\end{array}\right]\in R^{21\times 1}$$

(7)

Once the calibration parameters r̂_A ∼ n̂_B are estimated, the parameter error terms r̄_A ∼ n̄_B in Equation (7) can be removed for fast computation. The dynamic equation of x is given by:

$$\dot{x}\left(t\right)=A\left(t\right)x\left(t\right)+w\left(t\right)$$

(8)

where:

$$A\left(t\right)=\left[\begin{array}{ccccccc}\left[-y_g\times \right]& 0& 0& 0& 0& 0& 0\\ 0& 0& I_3& 0& 0& 0& 0\\ -2C{\left(\widehat{q}\right)}^{\prime }\left[y_a\times \right]& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\end{array}\right],w\left(t\right)=\left[\begin{array}{c}-\frac{1}{2}{v}_g\\ 0\\ -C{\left(\widehat{q}\right)}^{\prime }{v}_a\\ 0\\ 0\\ 0\\ 0\end{array}\right]$$

The dynamic equations for q̄, r̄ and v̄ are from the result in [20]. The derivatives of r̄_A ∼ n̄_B are zero, since r_A ∼ n_B are constant parameters.

4. Measurement Equation of the Kalman Filter

Two measurement equations are used in the Kalman filter. One is the measurement from distance sensors (Section 4.1), and the other is the measurement equation using the zero velocity intervals (Section 4.2).

4.1. Distance Sensor Output and Parameters

The distance sensor outputs z_A ∈ R and z_B ∈ R can be modeled as follows:

$$\begin{array}{ccc}{z}_A& =& d_A+v_A\\ z_B& =& d_B+v_B\end{array}$$

(9)

where d_A and d_B are true distance values of sensors A and B, respectively. Symbols v_A and v_B denote measurement noises of A and B, respectively. We note that d_A and d_B are not the same as the heights of distance sensors A and B if they are not perpendicular to the floor. The heights can be obtained from d_A and d_B if we know the orientation of distance sensors.

Assuming that the floor is flat and the origin of the world coordinate system lies on the floor, the following is satisfied:

$$\begin{array}{ccc}\left[\begin{array}{ccc}0& 0& 1\end{array}\right]{\left[r\right]}_w& =& -\left[\begin{array}{ccc}0& 0& 1\end{array}\right]C{\left(q\right)}^{\prime }{\left[r_A+n_A{d}_A\right]}_b\\ \left[\begin{array}{ccc}0& 0& 1\end{array}\right]{\left[r\right]}_w& =& -\left[\begin{array}{ccc}0& 0& 1\end{array}\right]C{\left(q\right)}^{\prime }{\left[r_B+n_B{d}_B\right]}_b\end{array}$$

(10)

In Equation (10), − (r_a + n_ad_a) is a vector (in the body coordinate system) from a point (intersection of line n_a and floor plane) on the floor to the inertial sensor. By pre-multiplying C(q)′ (the rotation matrix from the body coordinate system to the world coordinate system), the third component of C(q)′ (r_a + n_ad_a) is the height of the inertial sensor.

Now, we relate Equation (10) to the state Equation (7). Note that [21]:

$$C{\left(q\right)}^{\prime }=C{\left(\widehat{q}\right)}^{\prime }\left(I+2\left[\overline{q}\times \right]\right)$$

(11)

Inserting Equations (1), (5), (9) and (11), into the first equation of Equation (10), we have the following:

$$-\left[\begin{array}{ccc}0& 0& 1\end{array}\right]C{\left(\widehat{q}\right)}^{\prime }\left(I+2\left[\overline{q}\times \right]\right)({\widehat{r}}_A+{\overline{r}}_A+({\widehat{n}}_A+{\overline{n}}_A)(z_A-v_A))=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]\left({\widehat{r}}_A+\overline{r}\right)$$

(12)

Assuming that the error terms (q̄, r̄_A, n̄_A, r̄) and the noise term (v_a) are small, we can ignore the product terms: for example, [q̄×]r̄_A is ignored. By ignoring the product terms, we obtain:

$${\tilde{z}}_A=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]\left(2C{\left(\widehat{q}\right)}^{\prime }\left[\left({\widehat{r}}_A+{\widehat{n}}_A{z}_A\right)\times \right]\overline{q}-\overline{r}-C{\left(\widehat{q}\right)}^{\prime }{\overline{r}}_A-C{\left(\widehat{q}\right)}^{\prime }{z}_A{\overline{n}}_A+C{\left(\widehat{q}\right)}^{\prime }{\widehat{n}}_A{v}_A\right)$$

(13)

where:

$${\tilde{z}}_A=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]\left(\widehat{r}+C{\left(\widehat{q}\right)}^{\prime }\left({\widehat{r}}_A+{\widehat{n}}_A{z}_A\right)\right)\in R$$

Thus, the measurement z̃_A is related to the state x as follows:

$${\tilde{z}}_A=H_{A}x+C{\left(\widehat{q}\right)}^{\prime }{\widehat{n}}_A{v}_A$$

(14)

where:

$$H_A=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]\left[\begin{array}{ccccccc}2C{\left(\widehat{q}\right)}^{\prime }\left[\left({\widehat{r}}_A+{\widehat{n}}_A{z}_A\right)\times \right]& -I_3& 0& -C{\left(\widehat{q}\right)}^{\prime }& 0& -C{\left(\widehat{q}\right)}^{\prime }{z}_A& 0\end{array}\right]\in R^{1\times 21}$$

Similarly, we can derive a measurement equation for the distance sensor B:

$${\tilde{z}}_B=H_{B}x+C{\left(\widehat{q}\right)}^{\prime }{\widehat{n}}_B{v}_B$$

(15)

where:

$$\begin{array}{c}{\tilde{z}}_B=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]\left(\widehat{r}+C{\left(\widehat{q}\right)}^{\prime }\left({\widehat{r}}_B+{\widehat{n}}_B{z}_B\right)\right)\in R\\ H_B=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]\left[\begin{array}{ccccccc}2C{\left(\widehat{q}\right)}^{\prime }\left[\left({\widehat{r}}_B+{\widehat{n}}_B{z}_B\right)\times \right]& -I_3& 0& 0& -C{\left(\widehat{q}\right)}^{\prime }& 0& -C{\left(\widehat{q}\right)}^{\prime }{z}_B\end{array}\right]\in R^{1\times 21}\end{array}$$

In addition to Equations (13) and (15), some constraints are added in the measurement equations. The first constraint is n_a and n_b should be unit vectors since they represent direction vectors. The second constraint is that ‖r_a − r_b‖ = d_AB.

The constraint (n_a and n_b should be unit vectors) is expressed as:

$$\Vert n_A\Vert =\Vert {\widehat{n}}_A+{\overline{n}}_A\Vert ={\widehat{n}}_A^{\prime }{\widehat{n}}_A+2{\widehat{n}}_A^{\prime }{\overline{n}}_A+{\overline{n}}_A^{\prime }{\overline{n}}_A=1$$

(16)

From $\Vert {\hat{n}}_A\Vert ={\hat{n}}_A^{\prime }{\hat{n}}_A=1$ and ${\overline{n}}_A^{\prime }{\overline{n}}_A\approx 0$ (assuming that n̄_A is small), we have the following approximation:

$${\widehat{n}}_A^{\prime }{\overline{n}}_A=0$$

(17)

Thus, constraints on n_A and n_B are imposed through the following measurement equations:

$$\left[\begin{array}{c}0\\ 0\end{array}\right]=H_{\mathit{\text{constraint}}}x+v_{\mathit{\text{constraint}}}$$

(18)

where:

$$H_{\mathit{\text{constraint}}}\left[\begin{array}{ccccccc}{0}_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& {\widehat{n}}_A^{\prime }& 0_{1\times 3}\\ 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& {\widehat{n}}_B^{\prime }\end{array}\right]\in R^{2\times 21}$$

The noise term v_constraint ∈ R^2×1 is an artificial noise reflecting the fact that the constraint Equation (17) is an approximation.

Inserting Equation (2) into the constraint ‖r_A − r_B‖ = d_AB, we have:

$$z_{AB}=\Vert {\widehat{r}}_A+{\overline{r}}_A-\left({\widehat{r}}_B+{\overline{r}}_B\right)\Vert +v_{AB}$$

(19)

Since it has the nonlinear relationship, an extended Kalman filtering technique is used for this measurement.

$${\tilde{z}}_{AB}=H_{AB}x+v_{AB}$$

(20)

where:

$$\begin{array}{c}{\tilde{z}}_{AB}=z_{AB}-\Vert {\widehat{r}}_A-{\widehat{r}}_B\Vert \in R\\ \begin{array}{lll}{H}_{AB}& =& \left[\begin{array}{ccccccc}{0}_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& \frac{\partial d_{AB}}{\partial {\overline{r}}_A}& \frac{\partial d_{AB}}{\partial {\overline{r}}_B}& 0_{1\times 3}& 0_{1\times 3}\end{array}\right]\\ & =& \left[\begin{array}{ccccccc}{0}_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& \frac{{\left({\widehat{r}}_A-{\widehat{r}}_B\right)}^{\prime }}{\Vert {\widehat{r}}_A-{\widehat{r}}_B\Vert }& \frac{-{\left({\widehat{r}}_A-{\widehat{r}}_B\right)}^{\prime }}{\Vert {\widehat{r}}_A-{\widehat{r}}_B\Vert }& 0_{1\times 3}& 0_{1\times 3}\end{array}\right]\in R^{1\times 21}\end{array}\end{array}$$

In summary, Equations (14), (15), (18) and (20) are used as measurement equations for the Kalman filter if distance sensor data z_A and z_B are available. In this case, the measurement equation is given by:

$$z=\left[\begin{array}{c}{\tilde{z}}_A\\ {\tilde{z}}_B\\ 0_{2\times 1}\\ {\tilde{z}}_{AB}\end{array}\right]=\left[\begin{array}{c}{H}_A\\ H_B\\ H_{\mathit{\text{constraint}}}\\ H_{AB}\end{array}\right]x+\left[\begin{array}{c}C{\left(\widehat{q}\right)}^{\prime }{\widehat{n}}_A{v}_A\\ C{\left(\widehat{q}\right)}^{\prime }{\widehat{n}}_B{v}_B\\ v_{\mathit{\text{constraint}}}\\ v_{AB}\end{array}\right]$$

4.2. Zero Velocity Updating

In addition to distance sensors, zero velocity updating is used in a measurement equation. If we know when a foot is not moving (for example, when a foot is on the ground), the velocity error in the ISBMT algorithm can be reset, which significantly reduces foot pose estimation errors. Since velocity sensors (such as a Doppler velocity sensor in [22]) are not used, the zero velocity intervals are detected indirectly, where many zero velocity detection algorithms have been proposed [8,23].

In this paper, we use a simple zero velocity detection algorithm. Let Z_m be a set of all discrete time indices belonging to the zero velocity intervals. The discrete time k belongs to Z_m if the following conditions are satisfied:

$$\begin{array}{ll}\Vert y_{g,i}\Vert \le B_g,& k-\frac{N_g}{2}\le i\le k+\frac{N_g}{2}\\ \Vert y_{a,i}-y_{a_{i-1}}\Vert \le B_a,& k-\frac{N_a}{2}\le i\le k+\frac{N_a}{2}\end{array}$$

(21)

where N_g and N_a are even number integers.

During zero velocity intervals, we assume the following:

$$v=0_{3\times 1}$$

(22)

Inserting Equation (5) into Equation (22), we have the following measurement equation in the Kalman filter.

$$z_v=H_{v}x+v_{\mathit{\text{zero}}}$$

(23)

where:

$$\begin{array}{c}{z}_v=0_{3\times 1}-\widehat{v}\in R^{3\times 1}\\ H_v=\left[\begin{array}{ccccccc}{0}_{3\times 3}& 0_{3\times 3}& I_3& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]\in R^{3\times 21}\end{array}$$

The noise term v_zero ∈ R^3×1 is an artificial noise reflecting the fact that the true velocity may not be zero (the velocity may be almost zero, but not exactly zero).

5. Experiments and Results

The proposed algorithm is tested with several foot movement experiments. The foot position is measured with an inertial sensor unit (Xsens MTi) and two distance sensors (VL6180) as in Figure 2. The estimated positions are compared with the positions obtained using an optical motion tracker (Optitrack six Flex 13 camera system), which is considered as a ground truth.

The parameters used in the proposed algorithm are summarized in

Table 1. Parameters in the proposed algorithm.

Parameter	Value	Related Equations
initial value of r̂A	[ −0.008 0.080 0 ]′	(1)
initial value of r̂B	[ −0.006 −0.062 0 ]′	(1)
initial value of n̂A	[1 0 0 ]′	(1)
initial value of n̂B	[1 0 0 ]′	(1)
zAB	0.142	(2)
$\mathrm{E}\left\{v_g{v}_g^{\prime }\right\}$	0.00017I3	(5)
$\mathrm{E}\{v_a{v}_a^{\prime }\}$	0.0083I3	(5)
Pinit = initial value of E{xx′}	Pinit(2 : 3, 2 : 3) = 0.001 I2(*), Pinit(6, 6) = 0.0001,Pinit(10 : 21, 10 : 21) = 0.000001I12(all unspecified elements are zero)	(7)
$\mathrm{E}\{v_A{v}_A^{\prime }\}$	0.0004	(9)
$\mathrm{E}\{v_B{v}_B^{\prime }\}$	0.0004	(9)
$\mathrm{E}\{v_{\mathit{\text{constraint}}}{v}_{\mathit{\text{constraint}}}^{\prime }\}$	0.00000001I2	(18)
$\mathrm{E}\{v_{\mathit{\text{AB}}}^{\prime }\}$	0.000001	(20)
$\mathrm{E}\{v_{\mathit{\text{zero}}}{v}_{\mathit{\text{zero}}}^{\prime }\}$	0.0001I3	(23)

^*P_init(2 : 3, 2 : 3) represents a R^2×2 matrix consisting of second and third rows and column elements.

. All noises are assumed to be uncorrelated white Gaussian noises.

Four motions (walking, dancing steps, kicking in a football, jumping) are tested. The estimated position is compared with two other inertial sensor-only pose estimations. In the first inertial sensor-only pose estimation, the same algorithm is used, except for the distance sensors: that is, Equations (14) and (15) are not used in the Kalman filter. We call this method “K.F. (zero velocity updating)”. In the second inertial sensor-only pose estimation, the height updating is added in the first inertial sensor-only pose estimation. If we assume that the floor is flat, the foot height during the zero velocity intervals (that is, when a foot is on the ground) should be the same. Thus, in addition to zero velocity updating, the z axis value of r is updated to the initial z axis value during each zero velocity interval. We call this method “K.F. (zero velocity + height updating)”

In Figures 3, 4–5, the estimated positions (walking case) by three methods (the proposed method and the two inertial sensor-only methods) are given along with the position by an optical tracker. Since the optical tracker coordinate system is different from the world coordinate system, the position data are translated and rotated. Furthermore, the inertial sensor data and optical tracker data are not synchronized at the hardware level. The data are synchronized by maximizing the cross-correlation.

We can see that x and y axis position estimations are similar in all three methods. This is not surprising, since the distance sensor only gives the z axis position (height) information. In the z axis position estimation, the proposed method gives better results (see the third graphs of Figures 3, 4–5 around 4.6, 6 and 7 s). Among inertial sensor-only estimations, the Kalman filter (zero velocity + height updating) is better, since its height is compensated during the zero velocity intervals.

Another example is given in Figures 6 and 7, where a ball kicking action is done. The x and y position estimation results are similar, both by the proposed method and K.F. (zero velocity + height updating). In this case, the position errors are quite large, presumably due to the fact that there is a rather long moving interval (2.8∼4-s interval) and a quick movement (large sensor values). In the z axis position estimation, we can see that the proposed method gives a significantly better result.

To compare the result quantitatively, RMS (root mean square) position errors are computed. The x axis RMS position error is defined by:

$$E_{\mathit{\text{RMS}},x}=\sqrt{\frac{1}{N}{\sum }_{k=1}^N{\left({\widehat{r}}_{x,k}-{\widehat{r}}_{\mathit{\text{optical}},x,k}\right)}^2}$$

where r̂_x,k is the x axis estimated position at the discrete time k, r̂_optical,x,k is the x axis position measured by the optical tracker and N is the total number of data. Similarly, we can define E_RMS,y and E_RMS,z

The RMS error results for four different activities are given in

Table 2. RMS position error comparison of different methods (unit: m).

Motion Type	Estimation Method	ERMS,x	ERMS,y	ERMS,z	Sum
walking	proposed method	0.0208	0.0118	0.0030	0.0356
	K.F. (zero velocity)	0.0243	0.0131	0.0121	0.0495
	K.F. (zero velocity + height updating)	0.0247	0.0130	0.0095	0.0472
dancing steps	proposed method	0.0231	0.0798	0.0084	0.1113
	K.F. (zero velocity)	0.0265	0.0805	0.0397	0.1466
	K.F. (zero velocity + height updating)	0.0269	0.0810	0.0266	0.1345
ball kicking	proposed method	0.0816	0.3513	0.0120	0.4449
	K.F. (zero velocity)	0.0871	0.3551	0.0714	0.5136
	K.F. (zero velocity + height updating)	0.0877	0.3583	0.0685	0.5145
jumping	proposed method	0.0529	0.0705	0.0188	0.1423
	K.F. (zero velocity)	0.0630	0.0807	0.0313	0.1750
	K.F. (zero velocity + height updating)	0.0637	0.0808	0.0244	0.1689

. In general, there is no significant improvement in the x and y axis position estimation.

Once the calibration parameters are computed, the dimension of the Kalman filter state reduces to nine from 21, since r̄_A ∼ n̄_B can be removed from the state. The proposed algorithm is used with this nine-state Kalman filter. In one Kalman filter, the initial calibration parameters in Table 1 are used. In another Kalman filter, the estimated calibration parameters in Equation (24) (which are supposed to be more accurate) are used. The position estimation results of two nine-state Kalman filters are given in

Table 3. RMS position error comparison of different calibration parameters (unit: m).

Motion Type	Estimation Method	ERMS,x	ERMS,y	ERMS,z	Sum
walking	proposed method	0.0208	0.0118	0.0030	0.0356
	with fixed initial parameter	0.0244	0.0119	0.0034	0.0397
	with fixed estimated parameter	0.0201	0.0119	0.0029	0.0350
dancing steps	proposed method	0.0231	0.0798	0.0084	0.1113
	with fixed initial parameter	0.0362	0.0852	0.0089	0.1303
	with fixed estimated parameter	0.0196	0.0745	0.0083	0.1024
ball kicking	proposed method	0.0816	0.3513	0.0120	0.4449
	with fixed initial parameter	0.0764	0.3495	0.0120	0.4380
	with fixed estimated parameter	0.0853	0.3506	0.0120	0.4479
jumping	proposed method	0.0529	0.0705	0.0188	0.1423
	with fixed initial parameter	0.0630	0.0807	0.0313	0.1750
	with fixed estimated parameter	0.0486	0.0655	0.0188	0.1329

, where the result of the 21-state Kalman filter is repeated from Table 2 for an easy comparison. The nine-state Kalman filter with calibrated parameters gives slightly better results, except for the kicking case. Since the difference between the initial and calibrated parameters is small, the RMS error difference is also small. The calibration of parameters is a one time process, and after the calibration, we can estimate the position using the nine-state Kalman filter.

In the walking data, the calibration parameters are estimated in the Kalman filter. The calibration parameters from walking data are given by:

$${\widehat{r}}_A=\left[\begin{array}{c}-0.0124\\ 0.0806\\ 0.000\end{array}\right],{\widehat{r}}_B=\left[\begin{array}{c}-0.0033\\ -0.0611\\ -0.0006\end{array}\right],{\widehat{n}}_A=\left[\begin{array}{c}1.0000\\ 0.0001\\ 0.0000\end{array}\right],{\widehat{n}}_B=\left[\begin{array}{c}1.0000\\ 0.0002\\ -0.0001\end{array}\right]$$

(24)

6. Conclusions

There are many inertial sensor-based foot pose estimation algorithms. In this paper, the position estimation is improved additionally using distance sensors. The distance sensor has a 20-cm range limitation. However, this limitation does not degrade estimation accuracy much, since a foot is over the 20-cm range only for a short time, unless the foot is in the air intentionally. Longer range sensors exist, but they are either less accurate in short ranges or more expensive.

The inertial motion estimation algorithm is proposed using a 21-state Kalman (nine states for pose tracking and 12 states for calibration parameters). After the parameters are calibrated, the nine-state Kalman filter can be used to estimate the foot pose.

The proposed algorithm is tested for four activities: walking, dancing steps, ball kicking and jumping. It is shown in Table 2 that there is significant improvement in the z axis position estimation compared with inertial sensor-only foot pose estimation. On the other hand, there is no improvement in the x and y axis, since the distance sensor only gives the height information.

The proposed algorithm can be used in gait analysis (which requires a foot pose estimation), motion-based gaming (such as a soccer game) and exergaming (game-based exercises).