A Human Gait Tracking System Using Dual Foot-Mounted IMU and Multiple 2D LiDARs

Abstract

This paper proposes a human gait tracking system using a dual foot-mounted IMU and multiple 2D LiDARs. The combining system aims to overcome the disadvantages of each single sensor system (the short tracking range of the single 2D LiDAR and the drift errors of the IMU system). The LiDARs act as anchors to mitigate the errors of an inertial navigation algorithm. In our system, two 2D LiDARs are used. LiDAR 1 is placed around the starting point, and LiDAR 2 is placed at the ending point (in straight walking) or at the turning point (in rectangular path walking). Using the LiDAR 1, we can estimate the initial headings and positions of each IMU without any calibration process. We also propose a method to calibrate two LiDARs that are placed far apart. Then, the measurement from two LiDARs can be combined in a Kalman filter and the smoother algorithm to correct the two estimated feet trajectories. If straight walking is detected, we update the current stride heading and the foot position using the previous stride headings. Then, it is used as a measurement update in the Kalman filter. In the smoother algorithm, a step width constraint is used as a measurement update. We evaluate the stride length estimation through a straight walking experiment along a corridor. The root mean square errors compared with an optical tracking system are less than 3 cm. The performance of proposed method is also verified with a rectangular path walking experiment.

1. Introduction

Human gait monitoring and analysis are important in clinical rehabilitation [1,2], functional mobility diagnostics [3,4], and sports training [5,6,7]. With some technological tools, the gait phase and human gait parameters can be determined through gait analysis. The Vicon optical tracking system [8] and the GAITRite system [9] are the gold standards of gait analysis since they provide accurate measurements. The optical system captures the position of passive reflective markers attached on human body using multi-cameras, while the GAITRite system measures gait parameters via an electronic force platform. However, these standard systems are bulky, expensive, and require a long time for set up and post-processing the data. It may be difficult to deploy these evaluation systems in some rural areas or remote locations.

Wearable sensors, such as inertial sensors or inertial measurement units (IMUs), are used as low-cost alternative instruments in gait analysis [10]. They are small and lightweight, have low power consumption, and are integrated in most smart wearable devices. They can be attached to various parts of the human body including the foot, waist, chest, and head. The gait parameters are then estimated using the recorded movement signal. For example, the walking step length can be estimated with a waist-mounted IMU using a linear regression model [11]. This method requires a calibration process to construct the step length model. A common method for foot-mounted IMU is the numerical integration of inertial data to estimate foot velocity and position. However, the drawback of low-cost IMU is the sensor noises, which result in the accumulated integration errors. A most widely used technique to reduce these errors is the zero-velocity update (ZVU) [12,13], which assumes that the foot velocity is zero when it contacts the ground. Once the stance phase is detected, a ZVU is used as a pseudo measurement of the Kalman filter to reset the errors. However, as the position and heading are still unobservable, the heading drift grows over time. Thus, the positioning accuracy is decreased.

Some researchers use a dual foot-mounted IMU system to reduce the symmetrical heading errors [14,15]. There are two problems in the dual foot-mounted IMU system. The first one is the initial heading and position of each IMU, which is usually solved using a calibration process. The other one is the heading drift over time. Using the mapping information, a heuristic drift elimination (HDE) algorithm is proposed to update the heading with the dominant direction of building [16,17]. This method cannot apply to large space, where the building direction is not available. Based on the relative position of two feet, some distance constraints are proposed as the measurements in the Kalman filter frame work [18,19,20,21,22]. These methods update the stride heading and foot position if straight walking is detected or use the maximum range of stride length or two feet step length to constrain the divergence of the moving foot. However, the heading cannot be effectively corrected, especially in the turning phase.

In human gait analysis, a 2D LiDAR is also used to track the lower limb. The LiDAR does not require external markers and has simple installation. It can be mounted on a moving platform such as a mobile robot [23,24] or a smart walker [25,26], or can be fixed on the ground at a height of human shin [27,28,29,30]. Using the LiDAR with moving platform requires more additional sensors as well as a control system. Thus, the system is complicated and bulky. The system with the LiDAR is installed on the ground is simple. It scans human leg and estimates the leg center point during walking. The LiDAR-based human leg estimation is quite accurate and contains the walking direction with respect to the local environment. However, the effective human leg tracking range of a single 2D LiDAR is limited within a few meters. In [31], a fusion system of multiple 2D LiDARs has been proposed to extend the human leg tracking range. However, the LiDARs cannot be placed far apart to guarantee the continuity of leg scan data. Therefore, the tracking range is still limited. For example, for 20 m of straight walking, three LiDARs need to be used.

The aim of this paper is to combine multiple 2D LiDARs with a dual foot-mounted IMU system to overcome the disadvantage of each single system. The proposed system can increase the human gait tracking range and reduce the heading drift of the inertial sensor system. Two 2D LiDARs are used in this paper. The first one is placed around the starting point. The other one is placed based on the walking route: at the ending point in straight walking or at the turning point in rectangular path walking. The LiDAR-based human leg center point estimation is then used as a measurement update of the inertial sensor system. The main contributions of this paper are as follows:

• Multiple 2D LiDARs are combined in the dual foot-mounted IMU system for heading and position correction;
• The initial heading and position of each IMU are automatically estimated using the first LiDAR without any calibration process;
• A calibration algorithm of two 2D LiDARs (placed far apart) is used, and the calibration parameters are then used to transform the human leg center point into the same coordinate system;
• In contrast with the HDE algorithm, which uses the building heading information, we propose a simple walking stride heading update algorithm when straight walking is detected. The foot position estimation from the updated heading is then used as a measurement update of the Kalman filter;
• The estimated errors of the Kalman filter are then compensated using a quadratic optimization-based smoother algorithm [32]. A constraint of the step width is also proposed in the smoother algorithm if the straight walking is detected.

2. System Overview

The proposed system is configured, as shown in Figure 1. Two foot-mounted IMU systems are synchronized and consist of a microSD card to store the initial data. Two 2D LiDARs are horizontally placed on the ground to scan human leg. The data from IMUs and LiDARs are then post-processed using Matlab.

As we can see in Figure 1, there are four sensor coordinate systems: two IMUs and two LiDARs. To track the human gait, we define the global coordinate system and two foot body coordinate systems. The left/right foot body coordinate systems are set to coincide with corresponding left/right IMU coordinate system. Subscripts L and R denote the left and right side, respectively. The three axes of the global coordinate system are chosen as follows: x and y axes coincide with the LiDAR 1 coordinate system, the z axis points upwards, and the origin is the projection of LiDAR 1 origin to the horizontal plane of two IMUs. The height of the global coordinate system is shifted so that the initial $z-$positions of two feet are zero. For a given vector, we sometimes use subscript b (body) and w (global) to emphasize that a vector is expressed in the particular coordinate.

The inertial sensor outputs are acceleration $y_a\in {\mathbf{R}}^3$ and angular velocity $y_g\in {\mathbf{R}}^3$, and are modeled as follows:

$$\begin{array}{cc}\hfill y_a& =a_b+C_w^b\left(t\right)\tilde{g}+v_a\left(t\right),\hfill \\ \hfill y_g& ={\omega }_b\left(t\right)+v_g\left(t\right),\hfill \end{array}$$

(1)

where $a_b\in {\mathbf{R}}^3$ is the acceleration produced by forces other than the gravitational field; $\tilde{g}={\left[\begin{array}{ccc}0& 0& 9.8\end{array}\right]}^T$ is the gravitational vector in the global coordinate system; $C_w^b\in {\mathbf{R}}^{3\times 3}$ is the rotation matrix from global coordinate system to the body coordinate system; ${\omega }_b\in {\mathbf{R}}^3$ is the body angular velocity; and $v_a\in {\mathbf{R}}^3$ and $v_g\in {\mathbf{R}}^3$ are the accelerometer and gyroscope measurement noises, respectively. We assume that $v_a$ and $v_g$ are white Gaussian noises, where their covariances are given by:

$$R_a=\mathrm{E}\left\{v_a{v}_a^T\right\},R_g=\mathrm{E}\left\{v_g{v}_g^T\right\}.$$

(2)

The block diagram of our proposed system is shown in Figure 2. Firstly, the data from sensors is preprocessed. The single LiDAR processing module estimates the human leg trajectories using LiDAR scan data. The leg positions are expressed in each LiDAR coordinate system. The first calibration algorithm calibrates two LiDARs coordinate systems to transform the leg positions from LiDAR 2 into the LiDAR 1 coordinate system. The second calibration algorithm uses the IMUs and LiDAR 1 data to output the initial rotation angles and position of each IMU in the LiDAR 1 coordinate system. Then, the Kalman filter and the smoother algorithm are used to estimate the two feet trajectories. In the Kalman filter, the zero-velocity update (ZVU) and the LiDAR-based stance foot positions are used as a measurement update. A simple straight walking detection algorithm is also proposed to correct the heading estimation and the foot position. A smoother algorithm is then used to reduce the estimation error and smooth the trajectories of the two feet. In this smoother algorithm, the straight-walking-based heading and position update is replaced by the step width constraint measurement update.

3. Data Preprocessing

The aim of this part is to estimate the human leg center point from LiDAR and estimate the initial heading and position of each IMU in the global system. In this paper, the symbols $|.|$ and $\left|\right|.\left|\right|$ represent the absolute value and the Euclidean norm, respectively. We also denote the estimated value, the true value, and the error of x as $\widehat{x}$, $\tilde{x}$, and $x_e$, respectively, where $\tilde{x}=\widehat{x}+x_e$.

3.1. Single LiDAR-Based Human Leg Center Point Estimation

The LiDAR scan data are then used to estimate the human leg center point trajectories using the algorithm in [27,31]. The outputs of this module are the human leg positions in stance phases which are represented by diamond shapes in Figure 3.

Let $p_{i,j,k_j}^{L_i}$ denote the positions of left and right legs positions in the stance phase expressed in the LiDAR i coordinate system, where $L_i$ represents LiDAR i, $j=L,R$ is the left or right leg, and $k_j=0,1,2,...$ denotes the sequence of stance phases of j leg. For example, $k_L=0$ means the left foot is in initial standing still period. Since these leg positions are expressed in different LiDAR coordinate systems, the pose between two LiDARs is computed in the next section.

3.2. Calibration 1: Two LiDARs Calibration

The calibration parameters of two LiDARs are the rotation angle and the translation vector. In our previous research of human gait tracking using multiple LiDARs, the working space is shared by the LiDARs; therefore, a cylinder is placed arbitrarily in the common scanning space to calibrate the system. In this research, two LiDARs are placed far apart and there is no common scan data. Therefore, in stead of arbitrary choice, we place the cylinder at some fixed points which are already known their geometric relationship.

The calibration configuration is shown in Figure 4. The cylinder with measured radius ${\widehat{r}}_c$ is placed at four points $A_1$, $B_1$ (in LiDAR 1 scanning range), $A_2$, and $B_2$ (in LiDAR 2 scanning range). These four points are manually calibrated to identify the following parameters: the intersection O of $A_1{B}_1$ and $A_2{B}_2$, the angle $\widehat{\alpha }$ between $A_1{B}_1$ and $A_2{B}_2$, and the corresponding distance from O to each point ${\widehat{d}}_{m_i}$, where $m=A,B$ and $i=1,2$. We also denote the measurement errors of ${\tilde{r}}_c$, $\tilde{\alpha }$, and ${\tilde{d}}_{m_i}$ by $r_{c,e}$, ${\alpha }_e$, and $d_e$, respectively.

The coordinate systems of two LiDARs are represented by $O_1-x_1{y}_1$ and $O_2-x_2{y}_2$. The calibration parameters are the translation vector $T_{12}$ from $O_1$ to $O_2$ and the rotation angle ${\theta }_{12}$ from the LiDAR 2 coordinate system to the LiDAR 1 coordinate system. To account the uncertainties of the LiDAR data measurement and the measured configuration parameters, we propose an iterative algorithm to compensate the calibration parameters errors. The state vector $x_c$ consists of the calibration parameters and the cylinder’s estimated center points at points $A_1$ and $B_1$:

$$x_c={\left[\begin{array}{cccc}{\theta }_{12}& T_{12}^T& {\left(c_{A_1}^{L_1}\right)}^T& {\left(c_{B_1}^{L_1}\right)}^T\end{array}\right]}^T\in {\mathbf{R}}^7.$$

(3)

Let $s_{m_i,k}^{L_i}$ represent the cylinder sample scan data of LiDAR i at position $m_i$. At each position, the total number of scan data point is $N_{m_i}$.

3.2.1. Initialization

We firstly initialize the cylinder’s center points using the least square fitting algorithm. The initial cylinder’s center points ${\widehat{c}}_{m_i,0}^{L_i}$ are then used to estimate the initial calibration parameters as follows:

• The unit vector is estimated by: ${\widehat{v}}^{L_i}=\frac{{\widehat{c}}_{A_i,0}^{L_i}-{\widehat{c}}_{B_i,0}^{L_i}}{\parallel {\widehat{c}}_{A_i,0}^{L_i}-{\widehat{c}}_{B_i,0}^{L_i}\parallel },i=1,2$;
• The initial rotation angle is estimated by: ${\widehat{\theta }}_{12,0}=\angle ({\widehat{v}}^{L_1},-R\left(\widehat{\alpha }\right){\widehat{v}}^{L_2})$;
• The initial translation vector is estimated by:
  ${\widehat{T}}_{12,0}={}^1{\widehat{c}}_{A_1,0}+{\widehat{d}}_{A_1}{\widehat{v}}^{L_1}+{\widehat{d}}_{A_2}R\left(\widehat{\alpha }\right){\widehat{v}}^{L_1}-R\left({\widehat{\theta }}_{12,0}\right){}^2{\widehat{c}}_{A_2,0}$.

In the above equations, $R\left(\phi \right)$ is the rotation matrix of an angle $\phi$ with respect to the z-axis: $R\left(\phi \right)=\left[\begin{array}{cc}\cos \phi & -\sin \phi \\ \sin \phi & \cos \phi \end{array}\right]$.

3.2.2. Iterative Algorithm

The errors state vector is defined as follows:

$$x_{c,e}={\left[\begin{array}{cccc}{\theta }_{12,e}& T_{12,e}^T& {\left(c_{A_1,e}^{L_1}\right)}^T& {\left(c_{B_1,e}^{L_1}\right)}^T\end{array}\right]}^T\in {\mathbf{R}}^7.$$

The measurement equations are then identified using the LiDAR scan data.

For LiDAR 1, the circle fitting equation is as follows:

$${\tilde{r}}_c=\parallel {\tilde{s}}_{m_1,k}^{L_1}-{\tilde{c}}_{m_1}^{L_1}\parallel ,m=A,B.$$

(4)

By inserting the measurement errors, we can identify the measurement equation for each scan data point of LiDAR 1 as follows:

$$z_{m_1,k}={\widehat{r}}_c-\parallel s_{m_1,k}^{L_1}-{\widehat{c}}_{m_1}^{L_1}\parallel =h_{m_1,k}{c}_{m_1,e}^{L_1}+\left[\begin{array}{cc}{h}_{m_1,k}& -1\end{array}\right]\left[\begin{array}{c}{v}_{s_{m_1,k}}\\ r_{c,e}\end{array}\right],$$

(5)

with $h_{m_1,k}=\frac{-{(s_{m_1,k}^{L_1}-{\widehat{c}}_{m_1}^{L_1})}^T}{\parallel s_{m_1,k}^{L_1}-{\widehat{c}}_{m_1}^{L_1}\parallel }$ and $v_{s_{m_1,k}}$ is the measurement errors of sample data $s_{m_1,k}^{L_1}$.

For LiDAR 2, we fit the circle after transforming the scan data into the LiDAR 1 coordinate system:

$${\tilde{r}}_c=\parallel R\left({\tilde{\theta }}_{12}\right){\tilde{s}}_{m_2,k}^{L_2}+{\tilde{T}}_{12}-{\tilde{c}}_{m_2}^{L_1}\parallel ,m=A,B,$$

(6)

where ${\tilde{c}}_{m_2}^{L_1}={\tilde{c}}_{A_1}^{L_1}+{\tilde{d}}_{A_1}{\tilde{v}}^{L_1}+{\tilde{d}}_{m_2}R\left(\tilde{\alpha }\right){\tilde{v}}^{L_1}$. The small error of unit vector ${\tilde{v}}^{L_1}$ can be represented by a small angle ${\beta }_e$ as follows: ${\tilde{v}}^{L_1}=R\left({\beta }_e\right){\widehat{v}}^{L_1}\approx {\widehat{v}}^{L_1}+{\widehat{u}}^{L_1}{\beta }_e$, with ${\widehat{u}}^{L_1}=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]{\widehat{v}}^{L_1}$. The rotation matrix is approximated as follows: $R\left(\tilde{\theta }\right)=R(\widehat{\theta }+{\theta }_e)\approx R\left(\widehat{\theta }\right)+G\left(\widehat{\theta }\right){\theta }_e$, where $G\left(\widehat{\theta }\right)=R\left(\widehat{\theta }\right)\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]$. By inserting the measurement errors, (6) becomes:

$$\begin{array}{cc}\hfill {\widehat{r}}_c& +r_{c,e}=\parallel (R\left({\widehat{\theta }}_{12}\right)+G\left({\widehat{\theta }}_{12}\right){\theta }_e)(s_{m_2,k}^{L_2}-v_{s_{m_2,k}})+{\widehat{T}}_{12}+T_{12,e}-({\widehat{c}}_{A_1}^{L_1}+c_{A_1,e}^{L_1}+\hfill \\ \hfill & ({\widehat{d}}_{A_1}+d_e)({\widehat{v}}^{L_1}+{\widehat{u}}^{L_1}{\beta }_e)+({\widehat{d}}_{m_2}+d_e)(R\left(\widehat{\alpha }\right)+G\left(\widehat{\alpha }\right){\alpha }_e)({\widehat{v}}^{L_1}+{\widehat{u}}^{L_1}{\beta }_e))\parallel .\hfill \end{array}$$

The measurement equation at point $m_2,m=A,B$ is as follows:

$$\begin{array}{cc}\hfill & z_{m_2,k}={\widehat{r}}_c-\parallel f_{m_2,k}\parallel =h_{m_2,k}(G\left({\widehat{\theta }}_{12}\right)s_{m_2,k}^{L_2}{\theta }_{12,e}+T_{12,e}-c_{A_1,e}^{L_1}-R\left({\widehat{\theta }}_{12}\right)v_{s_{m_2,k}}\hfill \\ \hfill & -({\widehat{v}}^{L_1}+R\left(\widehat{\alpha }\right){\widehat{v}}^{L_1})d_e-({\widehat{d}}_{A_1}{\widehat{u}}^{L_1}+{\widehat{d}}_{m_2}R\left(\widehat{\alpha }\right){\widehat{u}}^{L_1}){\beta }_e-{\widehat{d}}_{m_2}G\left(\widehat{\alpha }\right){\widehat{v}}^{L_1}{\alpha }_e)-r_{c,e},\hfill \end{array}$$

(7)

where $f_{m_2,k}=R\left({\widehat{\theta }}_{12}\right)s_{m_2,k}^{L_2}+{\widehat{T}}_{12}-({\widehat{c}}_{A_1}^{L_1}+{\widehat{d}}_{A_1}{\widehat{v}}^{L_1}+{\widehat{d}}_{m_2}R\left(\widehat{\alpha }\right){\widehat{v}}^{L_1})$, and

$h_{m_2,k}=\frac{f_{m_2,k}^T}{\parallel f_{m_2,k}\parallel }$.

By combining (5) and (7), the measurement equation for all four positions of cylinder is as follows:

$$z_c=H_c{x}_{c,e}+B_c{w}_c,$$

(8)

where $z_c=\left[\begin{array}{c}{\widehat{r}}_c-\parallel s_{m_1,k}^{L_1}-{\widehat{c}}_{m_1}^{L_1}\parallel \\ {\widehat{r}}_c-\parallel f_{m_2,k}\parallel \end{array}\right]\in {\mathbf{R}}^{N_a\times 1}$, $w_c=\left[\begin{array}{c}{v}_{s_{m_1,k}}\\ v_{s_{m_2,k}}\\ r_{c,e}\\ d_e\\ {\beta }_e\\ {\alpha }_e\end{array}\right]\in {\mathbf{R}}^{2N_a+4}$, and $H_c\in {\mathbf{R}}^{N_a\times 7}$ and $B_c\in {\mathbf{R}}^{N_a\times 2N_a+4}$ can be identified using (5) and (7). Here, $N_a={\sum }_{m=(A,B),i=(1,2)}^{}{N}_{m_i}$ is the total number of sample scan data at four positions of cylinder. We assume that the measurement noises are zero mean white Gaussian noises, where covariances are given by:

$$\begin{array}{cc}\hfill R_{r_c}& =\mathrm{E}\left\{r_{c,e}^T{r}_{c,e}\right\}\in \mathbf{R},R_d=\mathrm{E}\left\{d_e^T{d}_e\right\}\in \mathbf{R},\hfill \\ \hfill R_{\beta }& =\mathrm{E}\left\{{\beta }_e^T{\beta }_e\right\}\in \mathbf{R},R_{\alpha }=\mathrm{E}\left\{{\alpha }_e^T{\alpha }_e\right\}\in \mathbf{R}.\hfill \end{array}$$

(9)

The covariance of LiDAR measurements can be found in [31]. Then, we can compute the covariance matrix of measurement noises of (8) as follows:

$$Q_c=B_c\mathrm{E}\left\{w_c{w}_c^T\right\}{B}_c^T.$$

(10)

The error-state vector is estimated as follows:

$$x_{c,e}={\left(H_c^T{Q}_c^{-1}{H}_c\right)}^{-1}{H}_c^T{Q}_c^{-1}{z}_c.$$

(11)

The state vector (3) is then updated using $x_{c,e}$. Using the updated cylinder center points at $A_1$ and $B_1$, the unit vector ${\widehat{v}}^{L_1}$ is calculated at each iteration. This process is iterated until a stop condition is satisfied:

$$\parallel x_{c,e}\parallel <\gamma .$$

(12)

Using the calibration parameters, we can transform the stance leg positions from LiDAR 2 coordinate into LiDAR coordinate as follows:

$$p_{2,j,k_j}^{L_1}=\left[\begin{array}{cc}R\left({\widehat{\theta }}_{12}\right)& {\widehat{T}}_{12}\end{array}\right]p_{2,j,k_j}^{L_2},j=L,R.$$

(13)

Figure 5 shows an example of transformation result of 20 m straight walking. The stance leg positions are transformed from LiDAR 2 into the LiDAR 1 coordinate system.

3.3. Calibration 2: Initial Heading and Position of Each IMU Estimation

The initial rotation angle and position of each IMU in the LiDAR 1 coordinate system are automatically estimated using the first walking stride data from IMUs and LiDAR 1. To estimate the initial rotation angle, we use the TRIAD algorithm [33]. This algorithm requires two pairs of reference and corresponding observation unit vectors. Two reference vectors in the LiDAR 1 coordinate system are gravitational vector $\tilde{g}$ and the first walking stride direction ${\left[\begin{array}{cc}{\left(S_{1,j,1}^{L_1}\right)}^T& 0\end{array}\right]}^T$ which is estimated from LiDAR 1, where $S_{1,j,1}^{L_1}=p_{1,j,1}-p_{1,j,0},j=L,R$. Two corresponding observation vectors are the average accelerometer output of one second standing still ${\overline{y}}_{a,j,0}$ and the first walking stride direction estimated from each IMU ${\left[\begin{array}{cc}{\left(S_{j,1}^{I_j}\right)}^T& 0\end{array}\right]}^T$. To estimate the IMU-based first walking stride direction, a simple inertial navigation algorithm with ZVU is applied to the first walking stride data of each IMU. The initial rotation matrix can be estimated using the following equations:

$$\begin{array}{cc}\hfill {\overline{y}}_{a,j,0}& =C_{w,0}^b\tilde{g},\hfill \\ \hfill \left[\begin{array}{c}{S}_{j,1}^{I_j}\\ 0\end{array}\right]& =C_{w,0}^b\left[\begin{array}{c}{S}_{1,j,1}^{L_1}\\ 0\end{array}\right],j=L,R.\hfill \end{array}$$

(14)

After that, we can estimate the initial rotation angles using $C_{w,0}^b$.

The initial positions of each IMU ($r_{j,0}={\left[\begin{array}{ccc}{x}_{j,0}& y_{j,0}& 0\end{array}\right]}^T,j=L,R$) are estimated using the estimation of LiDAR-based human leg center points (Figure 6). We assume that the human leg is vertically straight during the initial standing still phase, as well as the mid-stance phase of walking. Therefore, the ankle joint $K_j$ can be approximated as the projection of leg center point on the $x-y$ plane. By assuming that the coordinate of $K_j$ in IMU j, $r_{K_j}^{I_j}$, is pre-calibrated, the initial position of each IMU in LiDAR 1 coordinate can be computed by:

$$r_{j,0}=\left[\begin{array}{c}{p}_{1,j,0}\\ 0\end{array}\right]+{\left(C_{w,0}^b\right)}^T{r}_{K_j}^{I_j},j=L,R.$$

(15)

4. Proposed Algorithm

The proposed human gait tracking algorithm can be referred to as an inertial navigation system (INS).

4.1. Basic Inertial Navigation System Mechanization

The basic equations [34] for inertial navigation are given as follows:

$$\begin{array}{cc}\hfill \dot{q}& =\frac{1}{2}\Omega \left({\omega }_b\right)q,\hfill \\ \hfill \dot{v}& =C^T\left(q\right)a_b,\hfill \\ \hfill \dot{r}& =v,\hfill \end{array}$$

(16)

where $q\in {\mathbf{R}}^4$ is the quaternion which represents the attitude of foot, and $v\in {\mathbf{R}}^3$ and $r\in {\mathbf{R}}^3$ are the velocity and position of foot in global coordinate, respectively. The symbol $\Omega \left({\omega }_b\right)$ is defined by:

$$\Omega \left({\omega }_b\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].$$

The inertial navigation algorithm is used to estimate $\widehat{q}$, $\widehat{v}$ and $\widehat{r}$ by integrating (16). Due to sensor noises in the low-cost inertial sensor, the errors increase quickly if there is no reference measurement. To overcome this problem, a ZVU method is used to reset the accumulated errors. The method assumes that the velocity of the foot is zero when it touches the ground. Thus, once the stance phase is detected, the ZVU can be fused with the INS algorithm by an error-state Kalman filter.

To detect the stance phase during walking, a simple zero-velocity detection algorithm is used [35,36]. A discrete time k is determined to belong to the zero-velocity interval if the following conditions are satisfied:

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

(17)

where $B_g$ and $B_a$ are threshold values, and $N_g$ and $N_a$ are even number integers.

The errors in the numerically integrated values of (16) are estimated using a Kalman filter. Let $q_e\in {\mathbf{R}}^4$, $v_e\in {\mathbf{R}}^3$, and $r_e\in {\mathbf{R}}^3$ be the estimation errors in $\widehat{q}$, $\widehat{v}$, and $\widehat{r}$, which are defined as follows:

$$\begin{array}{cc}\hfill q_e& ={\widehat{q}}^{*}\otimes q,\hfill \\ \hfill v_e& =v-\widehat{v},\hfill \\ \hfill r_e& =r-\widehat{r},\hfill \end{array}$$

(18)

where ⊗ is the quaternion multiplication and $q^{*}$ denotes the complex quaternion conjugate of q. It is assumed that the quaternion errors are small. Thus, $q_e$ can be approximated by:

$$q_e=\left[\begin{array}{c}1\\ {\overline{q}}_e\end{array}\right]\in \left[\begin{array}{c}\mathbf{R}\\ {\mathbf{R}}^3\end{array}\right].$$

With this assumption, the attitude error is represented by the three-dimensional vector ${\overline{q}}_e$. We define the error-state vector as follows:

$$x_e=\left[\begin{array}{c}{\overline{q}}_e\\ r_e\\ v_e\end{array}\right]\in {\mathbf{R}}^9.$$

(19)

The state transition equation is given by:

$${\dot{x}}_e=Ax+w,$$

(20)

where $A=\left[\begin{array}{ccc}[-y_g\times ]& {\mathbf{0}}_3& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{I}}_3\\ -2C^T\left(\widehat{q}\right)[y_a\times ]& {\mathbf{0}}_3& {\mathbf{0}}_3\end{array}\right]$ and $w=\left[\begin{array}{c}-0.5v_g\\ 0\\ -C^T\left(\widehat{q}\right)v_a\end{array}\right]$. For a vector $p\in {\mathbf{R}}^3$, $[p\times ]$ is defined by:

$$[p\times ]\triangleq \left[\begin{array}{ccc}0& -p_3& p_2\\ p_3& 0& -p_1\\ -p_2& p_1& 0\end{array}\right].$$

4.2. The Proposed Dual Foot-Mounted IMU Algorithm

The dual foot-mounted IMU errors state vector is defined by combining the left and right foot error states:

$$X_e={\left[\begin{array}{cc}{x}_{e,L}^T& x_{e,R}^T\end{array}\right]}^T\in {\mathbf{R}}^{18}.$$

(21)

The dynamic equation is expressed as follows:

$${\dot{X}}_e=Fx+W,$$

(22)

where $F=\left[\begin{array}{cc}{A}_L& {\mathbf{0}}_9\\ {\mathbf{0}}_9& A_R\end{array}\right]$ and $W=\left[\begin{array}{c}{w}_L\\ w_R\end{array}\right]$.

Let $\tau$ be the sampling period of the inertial sensor output. The system equation is discretized as follows:

$$X_{e,k+1}={\Phi }_k{X}_{e,k}+W_k,$$

(23)

where ${\Phi }_k=e^{F\tau }\approx I_{18}+F\tau +0.5{\left(F\tau \right)}^2$ and $Q_k=\mathrm{E}\left\{W_k{W}_k^T\right\}$.

In the proposed system, the model and observation equations are linear except the quaternion model, which is approximately linearized. The divergence can be the direct consequence of modeling errors. In the previous initial heading estimation section, although the initial heading of the INS system is unknown and has large uncertainty, it does not affect our proposed system. Three measurement updates are used in this Kalman filter: the zero-velocity update, the LiDAR-based stance foot position update, and the straight-walking-based heading and foot position update.

4.2.1. Zero-Velocity Update

If the left/right foot is in the stance phase, the corresponding measurement equations are given by:

$$\begin{array}{cc}\hfill z_{v,L}& ={\mathbf{0}}_{3\times 1}-{\widehat{v}}_{L,k}=H_{v,L}{X}_{e,k}+n_{v,L},\hfill \\ \hfill z_{v,R}& ={\mathbf{0}}_{3\times 1}-{\widehat{v}}_{R,k}=H_{v,R}{X}_{e,k}+n_{v,R},\hfill \end{array}$$

(24)

where $H_{v,L}=\left[\begin{array}{cccccc}{\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{I}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3\end{array}\right]$, $H_{v,R}=\left[\begin{array}{cccccc}{\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{I}}_3\end{array}\right]$, $n_{v,L}$ and $n_{v,R}$ are the white Gaussian measurement noises, and $R_v=\mathrm{E}\left\{n_{v,j}{n}_{v,j}^T\right\}$.

4.2.2. Lidar-Based Stance Foot Position Update

This update is used in the stance phase of each foot if the stance leg position estimation of LiDAR $p_{i,j}$ ($i=1,2$ and $j=L,R$) is available. Here, the LiDAR-based stance leg positions are already transformed into the LiDAR 1 coordinate system, so that we ignore the right superscript $L_1$ in $p_{i,j}$.

As we mention in the Section 3.3, we assume that the human leg is vertically straight during the mid-stance phase; therefore, we can transform the LiDAR-based stance leg center point position to the $x-y$ coordinate of stance foot position $f_{i,j}$ using the following equation:

$$\left[\begin{array}{c}{f}_{i,j}\\ *\end{array}\right]=\left[\begin{array}{c}{p}_{i,j}\\ 0\end{array}\right]+{\left(C\left({\widehat{q}}_j\right)\right)}^T{r}_{K_j}^{I_j},$$

(25)

where ${\widehat{q}}_j$ is the currently estimated quaternion of IMU j, and $r_{K_j}^{I_j}$ is the pre-calibrated coordinate of ankle joint in IMU j coordinate, as demonstrated in Section 3.3. Now, we can update the $x-y$ coordinate of the stance foot using the following measurement equations:

$$\begin{array}{cc}\hfill z_{lidar,L}& =f_{i,L}-\left[\begin{array}{c}{\widehat{r}}_{L,x}\\ {\widehat{r}}_{L,y}\end{array}\right]=H_{r,L}{X}_{e,k}+n_{lidar,L},\hfill \\ \hfill z_{lidar,R}& =f_{i,R}-\left[\begin{array}{c}{\widehat{r}}_{R,x}\\ {\widehat{r}}_{R,y}\end{array}\right]=H_{r,R}{X}_{e,k}+n_{lidar,R},\hfill \end{array}$$

(26)

where $H_{r,L}=\left[\begin{array}{cccccc}{\mathbf{0}}_{2\times 3}& \left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]& {\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 3}\end{array}\right]$,

$H_{r,R}=\left[\begin{array}{cccccc}{\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 3}& \left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]& {\mathbf{0}}_{2\times 3}\end{array}\right]$, $n_{lidar,L}$, and $n_{lidar,R}$ are the white Gaussian measurement noises with covariance $R_{lidar}$.

4.2.3. Heading and Position Update during Straight Walking

Instead of using the pre-defined building heading, we update the current walking stride heading using the previous stride headings if straight walking is detected. The new position of current stance foot is then calculated based on this heading. Then, it is used as the measurement update in the Kalman filter. Since we use the heading of previous strides, the heading errors are still accumulated until the measurement data of LiDAR 2 is available. These errors are compensated later in the smoother algorithm.

Figure 7 shows an example of the proposed algorithm for current stance left foot. Let ${\widehat{r}}_{L,s}$ denote the current estimated left foot position, where s is the discrete index of the current stance phase. The current walking stride on the right side and the adjacent walking stride on the left side are used. Their stance foot positions are represented by ($r_{R,s}$, $r_{R,s-1}$), and ($r_{L,s-1}$, $r_{L,s-2}$), respectively. The straight detection and heading correction procedures are as follows:

• Condition 1: The current step length is calculated:

  $$\parallel {\widehat{S}}_{LR}\parallel =\parallel {\widehat{r}}_{L,s}-r_{R,s}\parallel .$$
  Only the walking step with $\parallel {\widehat{S}}_{LR}\parallel >{\delta }_s$ is considered. Here, the threshold is empirically chosen to check whether a normal walking step has indeed been taken.
• Condition 2: If the current walking step is normal, we compute the angles between the current stride vector ${\widehat{S}}_{L,c}={\widehat{r}}_{L,s}-r_{L,s-1}$ and the previous left stride vector $S_L=r_{L,s-1}-r_{L,s-2}$ and the right stride vector $S_R=r_{R,s}-r_{R,s-1}$:

  $${\alpha }_1=\angle ({\widehat{S}}_{L,c},S_L),{\alpha }_2=\angle ({\widehat{S}}_{L,c},S_R).$$
  If they are smaller than a threshold ${\delta }_{\alpha }$, straight walking is detected. Here, we use the same threshold values used in [22], i.e., ${\delta }_s=0.5$ m and ${\delta }_{\alpha }=8^o$.
• The current stride vector is set as the average value of previous left and right stride vectors ${\widehat{S}}_{L,c,new}=\mathrm{mean}\{s_L,s_R\}$. Then, we normalize the ${\widehat{S}}_{L,c,new}$ and update the new position of the current left foot as follows:

  $${\widehat{r}}_{L,s,new}=r_{L,s-1}+\parallel {\widehat{S}}_{L,c}\parallel \frac{{\widehat{S}}_{L,c,new}}{\parallel {\widehat{S}}_{L,c,new}\parallel }.$$
• Similarly, if the right straight walking step is detected, the new position of the current right foot ${\widehat{r}}_{R,s,new}$ can be estimated.

Using the new current left/right foot positions, the measurement equations are given by:

$$\begin{array}{cc}\hfill z_{p,L}& ={\widehat{r}}_{L,s,new}-\left[\begin{array}{c}{\widehat{r}}_{L,x}\\ {\widehat{r}}_{L,y}\end{array}\right]=H_{r,L}{X}_{e,k}+n_{p,L},\hfill \\ \hfill z_{p,R}& ={\widehat{r}}_{R,s,new}-\left[\begin{array}{c}{\widehat{r}}_{R,x}\\ {\widehat{r}}_{R,y}\end{array}\right]=H_{r,R}{X}_{e,k}+n_{p,R},\hfill \end{array}$$

(27)

where $H_{r,L}$ and $H_{r,R}$ are given in (26), and $n_{p,L}$ and $n_{p,R}$ are the white Gaussian measurement noises with covariance $R_p$. Note that we only update the $x-y$ positions in this part.

In the Kalman filter, $X_{e,k}$ is estimated and used to compensate the errors in $\widehat{q}$, $\widehat{r}$, and $\widehat{v}$ using (18). After updating, the errors are set to zero.

4.3. Smoother Algorithm

Since we use the estimated previous heading to update the stride heading in straight walking, the heading errors are still accumulated. To archive our proposed correction performance, the errors of the Kalman filter estimated values (denoted by ${\widehat{q}}_{KF,k}$, ${\widehat{v}}_{KF,k}$, and ${\widehat{v}}_{KF,k}$) are compensated using the smoother algorithm [32]. For that, we define the estimation error-state vector as follows:

$$X_{SM,k}=\left[\begin{array}{c}{\overline{q}}_{L,k}\\ {\overline{r}}_{L,k}\\ {\overline{v}}_{L,k}\\ {\overline{q}}_{R,k}\\ {\overline{r}}_{R,k}\\ {\overline{v}}_{R,k}\end{array}\right]=\left[\begin{array}{c}\left[\begin{array}{cc}{\mathbf{0}}_{3\times 1}& {\mathbf{I}}_3\end{array}\right]{\widehat{q}}_{KF,L,k}^{*}\otimes q_{L,k}\\ r_{L,k}-{\widehat{r}}_{KF,L,k}\\ v_{L,k}-{\widehat{v}}_{KF,L,k}\\ \left[\begin{array}{cc}{\mathbf{0}}_{3\times 1}& {\mathbf{I}}_3\end{array}\right]{\widehat{q}}_{KF,R,k}^{*}\otimes q_{R,k}\\ r_{R,k}-{\widehat{r}}_{KF,R,k}\\ v_{R,k}-{\widehat{v}}_{KF,R,k}\end{array}\right].$$

(28)

The system equation for smoother estimation values is given by:

$${\zeta }_k+X_{SM,k+1}={\Phi }_k{X}_{SM,k}+W_k,$$

(29)

where ${\Phi }_k$ is defined in (23) and

$${\zeta }_k=\left[\begin{array}{c}{\tilde{q}}_{SM,L,k}\\ {\widehat{r}}_{KF,L,k+1}-f_{2,k}\\ {\widehat{v}}_{KF,L,k+1}-f_{3,k}\\ {\tilde{q}}_{SM,R,k}\\ {\widehat{r}}_{KF,R,k+1}-f_{5,k}\\ {\widehat{v}}_{KF,R,k+1}-f_{6,k}\end{array}\right],$$

(30)

$$\left[\begin{array}{c}{f}_{1,k}\\ f_{2,k}\\ f_{3,k}\\ f_{4,k}\\ f_{5,k}\\ f_{6,k}\end{array}\right]=f_k\left(\left[\begin{array}{c}{\widehat{q}}_{KF,L,k}\\ {\widehat{r}}_{KF,L,k}\\ {\widehat{v}}_{KF,L,k}\\ {\widehat{q}}_{KF,R,k}\\ {\widehat{r}}_{KF,R,k}\\ {\widehat{v}}_{KF,R,k}\end{array}\right],\left[\begin{array}{c}{v}_{g,L,k}\\ v_{a,L,k}\\ v_{g,R,k}\\ v_{a,R,k}\end{array}\right]\right),$$

(31)

$${\tilde{q}}_{SM,L,k}=\left[\begin{array}{cc}{\mathbf{0}}_{3\times 1}& {\mathbf{I}}_3\end{array}\right]f_{1,k}^{*}\otimes {\widehat{q}}_{KF,L,k+1},$$

$${\tilde{q}}_{SM,R,k}=\left[\begin{array}{cc}{\mathbf{0}}_{3\times 1}& {\mathbf{I}}_3\end{array}\right]f_{4,k}^{*}\otimes {\widehat{q}}_{KF,R,k+1}.$$

Function $f_k$ in (31) is the numerical integration of quaternion, position, and velocity from $k\tau$ to $(k+1)\tau$. Using (28), (30) and (31), we can expand the left side of (29) as the errors of the numerical integration of the Kalman filter estimated values. Thus, (29) represents how the estimation errors evolve after the integration of (31).

The smoothing problem can be formulated as a quadratic optimization problem using the method in [32]. Let the optimization variable $\tilde{X}$ be defined by:

$$\tilde{X}=\left[\begin{array}{c}{X}_{SM,1}\\ X_{SM,2}\\ ⋮\\ X_{SM,N}\end{array}\right]\in {\mathbf{R}}^{18N\times 1}.$$

(32)

We can estimate $\tilde{X}$ by minimizing the cost function:

$$\begin{array}{cc}\hfill J\left(\tilde{X}\right)& =\frac{1}{2}\sum _{k=1}^{N-1}{\left({\zeta }_k+X_{SM,k+1}-{\Phi }_k{X}_{SM,k}\right)}^T{Q}_k^{-1}\left({\zeta }_k+X_{SM,k+1}-{\Phi }_k{X}_{SM,k}\right)\hfill \\ \hfill & +\frac{1}{2}{(X_{SM,1}-X_{init})}^T{P}_{init}^{-1}(X_{SM,1}-X_{init})\hfill \\ \hfill & +\mathrm{constraint}\mathrm{terms}\mathrm{in}\left(36\right),\hfill \end{array}$$

(33)

where $P_{init}=\left[\begin{array}{cc}{P}_{L,init}& {\mathbf{0}}_9\\ {\mathbf{0}}_9& P_{R,init}\end{array}\right]$ in which $P_{j,init}=\left[\begin{array}{ccc}{P}_{q_{j,0}}& {\mathbf{0}}_3& {\mathbf{0}}_3\\ {\mathbf{0}}_3& P_{r_{j,0}}& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& P_{v_{j,0}}\end{array}\right],j=L,R$. The initial attitude error covariance $P_{q_{j,0}}$ is given by the algorithm in [33]. The initial position and velocity error covariance $P_{r_{j,0}}$, $P_{v_{j,0}}$ can be considered as design parameters.

In this smoother algorithm, we use three measurements: the zero-velocity update, the LiDAR-based stance foot positions update, and the step width constraint update. The first two measurements are explained in the previous section of the Kalman filter. The third measurement is proposed using an assumption that the width of walking step is almost constant during straight walking.

As an example, during a current stance phase of left foot, the step width is estimated as follows:

• The walking step vector is computed: $S_{LR}={\widehat{r}}_{KF,L,k}-{\widehat{r}}_{KF,R,k}$;
• The orthogonal unit vector $n_L$ of the current walking direction vector ${\widehat{S}}_{L,c,new}$ that points toward opposite foot side is computed, so that $n_L^T{S}_{LR}>0$;
• The step width d is now calculated as follows:

  $$d=n_L^T{S}_{LR}=n_L^T({\widehat{r}}_{KF,L,k}+{\overline{r}}_{L,k}-{\widehat{r}}_{KF,R,k}-{\overline{r}}_{R,k}).$$

The step width measurement equation for the left stance foot is given as follows:

$$z_{d,L,k}=H_{d,L}{X}_{SM,k}+n_{d,L},$$

(34)

where $z_{d,L,k}=d-n_L^T({\widehat{r}}_{KF,L,k}-{\widehat{r}}_{KF,R,k})$ and

$H_{d,L}=\left[\begin{array}{cccccc}{\mathbf{0}}_{1\times 3}& \left[\begin{array}{cc}{n}_L^T& 0\end{array}\right]& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& \left[\begin{array}{cc}-n_L^T& 0\end{array}\right]& {\mathbf{0}}_{1\times 3}\end{array}\right].$

Similarly, the step width measurement equation for the right stance foot is given as follows:

$$z_{d,R,k}=H_{d,R}{X}_{SM,k}+n_{d,R},$$

(35)

where $z_{d,R,k}=d-n_R^T({\widehat{r}}_{KF,R,k}-{\widehat{r}}_{KF,L,k})$ and

$H_{d,R}=\left[\begin{array}{cccccc}{\mathbf{0}}_{1\times 3}& \left[\begin{array}{cc}-n_R^T& 0\end{array}\right]& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& \left[\begin{array}{cc}{n}_R^T& 0\end{array}\right]& {\mathbf{0}}_{1\times 3}\end{array}\right]$. In (34) and (35), $n_{d,L}$ and $n_{d,R}$ are the white Gaussian measurement noises with covariance $R_d$.

Consequently, the “constraint terms” in (33) are detected as follows:

$$\begin{array}{cc}\hfill \mathrm{constraint}\mathrm{terms}& =\frac{1}{2}\sum _{i=L,R}\hfill \\ \hfill & (\sum _{k\in Z_{v,i}}{(z_{v,i,k}-H_{v,i,k}{X}_{SM,i,k})}^T{R}_v^{-1}(z_{v,i,k}-H_{v,i,k}{X}_{SM,i,k})+\hfill \\ \hfill & \sum _{k\in Z_{r,i}}{(z_{r,i,k}-H_{r,i,k}{X}_{SM,i,k})}^T{R}_{lidar}^{-1}(z_{r,i,k}-H_{r,i,k}{X}_{SM,i,k})+\hfill \\ \hfill & \sum _{k\in Z_{d,i}}{(z_{d,i,k}-H_{d,i,k}{X}_{SM,i,k})}^T{R}_d^{-1}(z_{d,i,k}-H_{d,i,k}{X}_{SM,i,k})),\hfill \end{array}$$

(36)

where $Z_{v,i}$, $Z_{r,i}$, and $Z_{d,i}$ denote the sets of all discrete time indices belonging to zero-velocity intervals, LiDAR-based stance foot position measurement intervals, and step width measurement intervals, respectively. The cost function (33) can be rewritten as a quadratic function of $\tilde{X}$ as follows:

$$J\left(\tilde{X}\right)=\frac{1}{2}{\tilde{X}}^T{M}_1\tilde{X}+M_2\tilde{X}+M_3,$$

(37)

where $M_3$ is irrelevant in the optimization, and $M_1$ and $M_2$ can be easily computed using (33) and (36). The details can be found in [32]. This is the main advantage of the quadratic-based optimization smoother algorithm, where the measurement constraints can be easily included in the optimization problem. The minimizing solution of (37) can be computed by solving the following equation:

$$M_1{\tilde{X}}^{*}+M_2=0,$$

(38)

where ${\tilde{X}}^{*}$ is the minimizing solution. Once ${\tilde{X}}^{*}$ is computed, the smoother estimation values ${\widehat{q}}_{SM,L,k}$, ${\widehat{r}}_{SM,L,k}$, ${\widehat{v}}_{SM,L,k}$, ${\widehat{q}}_{SM,R,k}$, ${\widehat{r}}_{SM,R,k}$, and ${\widehat{v}}_{SM,R,k}$ can be found using (28).

5. Experimental Results

5.1. Hardware Description

A Xsens MTi-1 IMU module with a micro SD card is installed in each foot. The sampling rate of two IMUs is 100 Hz. Two LiDARs (RPLIDAR A3) have a scan rate of 10 Hz. The total cost of the system is around USD 1500 (USD 150 for an IMU and USD 600 for a LiDAR). The sensors specifications are shown in

Table 1. Sensor specifications.

Specification	Xsens MTi-1
	Accelerometer	Gyroscope
Full range	$\pm 16\mathrm{g}$	$\pm {2000}^{\circ }/\mathrm{s}$
Noise density	120 $\mathsf{\mu }\mathrm{g}/\surd \mathrm{Hz}$	$0.{007}^{\circ }/\mathrm{s}/\surd \mathrm{Hz}$
Bandwidth	324 Hz (z: 262 Hz)	255 Hz
Sampling rate	100 Hz	100 Hz
Specification	RPLIDAR A3
Distance range	10–25 m
Minimum operating ranging	0.2 m
Angular resolution	$0.{225}^{\circ }$
Accuracy	$1\%$ of range ≤3 m
	$2\%$ of range 3–5 m
	$2.5\%$ of range 5–25 m
Scan rate	10 Hz

. An OptiTrack optical motion capture system, which is composed of six Flex 13 cameras with a resolution of $1280\times 1024$ at 120 Hz, is used to provide the ground truth of the walking step length estimation.

The parameters used in this paper are given in

Table 2. Parameters used in this paper.

Parameters	Related Equations
$R_a=0.01{\mathbf{I}}_3$, $R_g=0.001{\mathbf{I}}_3$	(2)
$R_{r_c}=0.01$, $R_{\beta }=0.01$, $R_{\alpha }=0.01$	(9)
$\gamma =0.001$	(12)
$B_g=1.2$, $B_a=1.5$, $N_g=16$, $N_a=16$	(17)
$P_{r_{j,0}}=0.000001{\mathbf{I}}_3$, $P_{v_{j,0}}=0.000001{\mathbf{I}}_3$	(33)
$R_d=0.01$	(9) and (36)
$R_v=0.01{\mathbf{I}}_3$	(24)
$R_{lidar}=0.01{\mathbf{I}}_2$	(26)
$R_p=0.01{\mathbf{I}}_2$	(27)

.

5.2. Stride Length Estimation Evaluation

To evaluate the stride length estimation, we perform a straight walking experiment with the optical camera system. Two male healthy volunteers, which have no problems with gait, balance, and coordination, are recruited. Their information is given in

Table 3. Subjects information.

Subject	Age	Height (cm)	Weight (kg)
1	33	168	63
2	30	160	52

. They are asked to walk along the corridor with the travel distances of 20 m and 50 m. Each person repeats the walk 10 and 5 times in 20 m and 50 m experiments, respectively.

The configuration of the experiment is given in Figure 8. The optical system cannot cover all the walking path; thus, it is placed so that a walking path outside two LiDARs range can be captured. The walking path outside the LiDARs range has the least measurement update; therefore, the walking stride length errors may be largest. With this setup, our OptiTrack system can capture a walking range of 8–13 m measured from LiDAR 2 origin. By comparing the walking stride lengths captured in this path, the performance of our proposed system is evaluated. The LiDAR 2 and OptiTrack system positions are fixed during both experiments. The LiDAR 1 position is changed with the walking distance.

The subject may have difficulties in following the straight path during walking. We model a simple case of abnormal gait by removing the availability of straight walking in 5 s (20 m walking) and 10 s (50 m walking). The straight-walking-based foot position update is therefore not applied in this interval. The step width constraint is still used with larger uncertainty. The modeled measurement update is then fed to the proposed method. An example of measurement availability of two feet is given in Figure 9. The top plot is the normal availability, while the bottom one is the modeled availability of measurement update. The non-zero values represent the presence of measurements. We can see the periodicity of zero-velocity measurements during walking. The LiDAR 1-based stance foot position measurements are available for the first three and four stance phases of the left and right foot, respectively. LiDAR 2 provides the stance foot position update for the last four and three stance phases of the left and right foot, respectively. In the stance phases where the LiDAR data are not available, the straight-walking-based heading and position update (in the Kalman filter) or the step width constraint update (in the smoother algorithm) is integrated. As in the bottom plot, the straight-walking-based measurement update is not available during the interval from 15 to 25 s.

The two estimated feet trajectories of all 20 m and 50 m walking experiments using the normal and modeled update are given in Figure 10 and Figure 11. Figure 12 shows an example of a single 50 m walking estimation result for clearer vision. The LiDAR-based stance foot position estimation is denoted by a circle. We can see that the Kalman filter with the initial heading estimated from Section 3.3 and the foot position update during straight walking reduces the heading errors significantly. However, the heading errors can still be observed as the estimated trajectories do not match with the foot position update from LiDAR 2. This error is compensated using the smoother algorithm, where the step width constraint is used instead of using the heading and foot position update. If the heading and foot position update is used, the Kalman-filer-based heading estimation will be maintained in the smoother algorithm. Therefore, the heading errors are not compensated. Based on the heading estimation from the Kalman filter, the advantage of dual foot-mounted IMU is expressed in the step width constraint. From the all estimated walking trajectories results, we can see a small bulge in the middle of the walk. The further walking distance is, the bigger bulge is. This is because there is no actual heading observation outside the LiDARs’ range. However, with the measurement update from LiDAR 2, the heading estimation errors can be corrected. In 20 m and 50 m walking estimation, the results using the normal and modeled update are almost similar. This shows the robustness of the proposed method.

The walking stride length estimation is then evaluated. For each walking, the optical OptiTrack system can capture three to four walking strides for each foot. The total counts of captured walking strides are 98 and 101 for the left and right foot, respectively. We compare the estimated walking stride length from the proposed smoother algorithm ($S{L}_{IMU}$) with the optical estimation ($S{L}_{optical}$) by computing the errors:

$$e=S{L}_{optical}-S{L}_{IMU}.$$

Figure 13 shows the histogram of the stride length estimation errors.

Table 4. The stride length estimation results compared with the optical OptiTrack system.

Subject	Mean Stride Length (m)	Estimation Errors
		Leg (Strides)	Max (cm)	Mean (cm)	RMSE (cm)
1	1.503 ± 0.054	Left (45)	7.5	2.3	2.8
	1.489 ± 0.047	Right (56)	6.2	2.1	2.5
2	1.416 ± 0.032	Left (53)	7.6	2.7	3.2
	1.377 ± 0.056	Right (45)	6.4	2.5	3.1
All	Normal update	Left (98)	7.6	2.5	3.0
		Right(101)	6.4	2.3	2.8
	Modeled update	Left (98)	7.6	2.5	3.1
		Right(101)	6.4	2.3	2.8

shows the mean estimation of the stride length and variance of each subject with a normal measurement update. The estimation errors compared to the optical system estimation are also given. For all subjects, the mean and root mean square errors (RMSE) of stride length estimation are less than 3 cm, while the maximum errors are less than 8 cm. The performances of the stride length estimation of the normal measurement update and the modeled measurement update are almost identical.

5.3. Evaluation of Rectangular Path Walking Estimation

To verify the effectiveness of the proposed method in heading correction, a rectangular path walking experiment is carried out. In this experiment, a person starts from the starting point, walks counter-clockwise twice along a rectangular path, and stops at the ending point as in Figure 14. The total of walking distance is around 124 m, with six turning phases. The LiDARs are placed to be able to capture the human leg data during turning phases.

Figure 15 shows the left and right foot estimated trajectories results of the proposed algorithm. We plot the corridor wall scan data as a reference of the walking direction as well as the results of calibrating the two LiDARs. The shading circle area represents the human leg tracking range of each LiDAR. As we can see, the proposed method can correct the heading during turning phases using the LiDAR measurements. There is no intersection between two feet trajectories. For comparison, we also plot the result of the dual foot-mounted IMU algorithm, which is proposed in [22], as in Figure 16. The algorithm using only dual foot-mounted IMU with a foot position update during straight walking based on the maximum range of the one-side foot stride length and the both-sides feet step length. Since there is no measurement of the heading, the errors can be seen clearly after the second round.

6. Conclusions

This paper proposes a human gait tracking system using a dual foot-mounted IMU and multiple 2D LiDARs. Two LiDARs are placed far apart: LiDAR 1 is at the starting point and LiDAR 2 is at the destination in straight walking or at the turning point in rectangular path walking. A calibration algorithm is proposed to estimate the rotation angle and translation vector of two LiDARs. The calibration uses a cylinder placed at four positions which are known their geometrical information. An iterative algorithm is then used to estimate the calibration parameters with the including of LiDAR measurement errors and configuration errors. Then, we can use the LiDARs as anchors to correct the inertial sensor system estimation. The LiDAR-based stance leg position estimations are transformed to stance foot position, and then combined as the measurement update of the Kalman filter and the smoother algorithm.

Since the LiDAR can provide the environment information, the LiDAR 1 coordinate system is chosen as the global coordinate system. The initial heading and position of each IMU can be estimated using IMUs data and the leg scan data from LiDAR 1. For further improvement, we update the heading and foot position if straight walking is detected. This is used as a measurement update in the Kalman filter. In the smoother algorithm, the straight-walking-based foot position update is replaced by a relative step width constraint update. We assume that in normal walking, the width of walking step is almost constant. We verify the stride length estimation of the proposed system with straight walking of 20 m and 50 m. We also model the abnormal walking by assuming that the subject cannot follow the straight path during a time interval. The measurement update of straight walking is therefore not available during this interval. The root mean square errors of the walking stride length estimation of both normal and modeled update are less than 3 cm compared with an optical tracking system. Through the rectangular path walking experimental results, we can see the LiDAR measurement can be used to correct the heading effectively. The proposed system has the potential for use in many fields, such as human gait monitoring and human gait navigation systems.