Estimating Lower Limb Kinematics Using a Lie Group Constrained Extended Kalman Filter with a Reduced Wearable IMU Count and Distance Measurements ^†

Abstract

Tracking the kinematics of human movement usually requires the use of equipment that constrains the user within a room (e.g., optical motion capture systems), or requires the use of a conspicuous body-worn measurement system (e.g., inertial measurement units (IMUs) attached to each body segment). This paper presents a novel Lie group constrained extended Kalman filter to estimate lower limb kinematics using IMU and inter-IMU distance measurements in a reduced sensor count configuration. The algorithm iterates through the prediction (kinematic equations), measurement (pelvis height assumption/inter-IMU distance measurements, zero velocity update for feet/ankles, flat-floor assumption for feet/ankles, and covariance limiter), and constraint update (formulation of hinged knee joints and ball-and-socket hip joints). The knee and hip joint angle root-mean-square errors in the sagittal plane for straight walking were $7.6\pm 2.6^{\circ }$ and $6.6\pm 2.7^{\circ }$, respectively, while the correlation coefficients were $0.95\pm 0.03$ and $0.87\pm 0.16$, respectively. Furthermore, experiments using simulated inter-IMU distance measurements show that performance improved substantially for dynamic movements, even at large noise levels ($\sigma =0.2$ m). However, further validation is recommended with actual distance measurement sensors, such as ultra-wideband ranging sensors.

1. Introduction

Human pose estimation is the tracking of position and orientation (i.e., pose) of body segments. Joint angles can then be calculated from the relative pose of linked body segments. Applications exist in robotics, virtual reality, animation, and healthcare (e.g., gait analysis). Traditionally, human pose is captured within a laboratory setting using optical motion capture (OMC) systems with up to millimeter level position accuracy [1] when properly configured and calibrated. However, the setup for OMC systems is time consuming and inconvenient (e.g., multiple markers are taped to the body) and requires considerable expertise. Recent miniaturization of inertial measurements units (IMUs) has paved the path toward inertial motion capture (IMC) systems suitable for prolonged use outside of the laboratory. Some examples of clinical applications in the current literature include determining level of spinal damage [2] and Parkinson’s disease diagnosis [3]. Furthermore, developing a comfortable IMC for routine daily use may facilitate interactive rehabilitation [4,5], and allow the study of movement disorder progression to enable predictive diagnostics.

Commercial IMCs attach one sensor per body segment (OSPS) [6], which may be considered too cumbersome and expensive for routine daily use by a consumer due to the number of IMUs required. Similar works also exist in the literature using quaternions [7], Kalman filters (KF) [8], and particle filters [9]. Each IMU typically tracks the orientation of the attached body segment using an orientation estimation algorithm (e.g., [10,11]), which is then connected via linked kinematic chain, usually rooted at the pelvis. A reduced-sensor-count (RSC) configuration, where IMUs are placed on a subset of body segments, can improve user comfort, reduce setup time and system cost. However, using fewer wearable sensor units necessarily reduces the kinematic information available, which must otherwise be inferred from (i) our knowledge of human movement (e.g., enforcing mechanical joint constraints or making dynamic balance assumptions), or (ii) by using additional sensing modalities within each wearable sensor unit. Each approach will be described in the next subsections.

1.1. Existing Algorithms for Human Motion Tracking Using Fewer IMUs

The performance of algorithms with RSC configuration depends (i) on how the kinematic information of uninstrumented body segments is inferred and (ii) on how body pose is represented.

The kinematic information of body segments which do not have sensors attached to them may be inferred by the algorithm, either through data obtained in the past (i.e., observed correlations between co-movement of different body segments or data-driven approaches) or by using a simplified kinematic model of the human body (i.e., model-based approaches). Data-driven approaches (e.g., nearest-neighbor search [12] and bi-directional recurrent neural network [13]) are able to recreate realistic motion suitable for animation-related applications. However, these approaches are normally biased toward motions already contained in the database, which may limit their use in monitoring pathological gait. Model-based approaches reconstruct human movement using kinematic and biomechanical models (e.g., linear regression [14], constrained KF [15], extended KF (EKF) [16], particle filter [9], and window-based optimization [17]). The use of optimization-based estimators is sometimes favoured due to its relative ease to setup and ease of understanding. However, the algorithm can be very inefficient when tracking a larger number of dimensions (e.g., when tracking body pose over a long duration). To significantly increase the efficiency of the algorithm when estimating body pose across time, a recursive estimator can take advantage of the state substructure and reduce the state dimension being tracked [18].

Body poses are usually represented using Euler angles or quaternions [9,16]. However, recent work on pose estimation has shown that using a Lie group to represent the states of recursive estimator is a promising approach. Such algorithms typically represent the body pose as a chain of linked segments using matrix Lie groups to represent the orientation or pose of each body segment; specifically the special orthogonal group, $SO\left(n\right)$, and special Euclidean group, $SE\left(n\right)$, where $n=2,3$, are the spatial dimensions for humam body kinematics problems. The early works of Wang et al. [19] and Barfoot et al. [20] investigated representations and propagation of pose uncertainty, the former in the context of manipulator kinematics and the latter focused on $SE\left(3\right)$, followed by the formulation of recursive estimators using Lie group representation (e.g., EKF [21] and unscented KF (UKF) [22]). Recent literature has reported the use of Lie group based recursive estimators to estimate human movement. Cesic et al. estimated the full body pose from OMC marker measurements and achieved significant improvements compared to an Euler angle representation [23]; and even supplemented the approach with an observability analysis [24]. Joukov et al. represented the human body using $SO\left(n\right)$, tracking the pose using measurements from IMUs under an OSPS configuration [25]. Joukov et al.’s algorithm was tested using an arm tracking experiment, where the results improved especially during arm poses that cause a singularity when using an Euler angle representation (i.e., Lie group representation is singularity free).

1.2. Improving Human Pose Estimation Using Additional Sensor Measurements

Another approach is to supplement kinematic information from the IMU with another kind of sensor, which inherently increases cost and reduces battery life. Note that we will focus on systems that supplement pose estimation, not on the global position estimation of the subject (e.g., [26]). For example, IMCs can be supplemented with standard video cameras (e.g., fused using an optimization-based algorithm [27], and deep neural networks [28]) or depth cameras [29] at fixed locations in the capture environment, external to the subject. The combination of IMCs and portable cameras solves a weakness of OMCs (i.e., marker or body segment occlusion) and a weakness of IMCs (i.e., global position drift). However, the system still requires an external sensor that is carried by another person or requires some quick setup. IMCs can also be supplemented by distance measurements (using ultrasonic devices and KF in OSPS configuration [30], using constrained KF in RSC configuration [31]), removing dependence on any external non-body-worn sensor.

1.3. Novelty

This paper describes a novel human pose estimator that represents the state using Lie groups with the state propagated iteratively using a constrained EKF (CEKF) to estimate lower body kinematics for an RSC configuration of IMUs and inter-IMU distance measurements; the Lie group framework and inclusion of inter-IMU distance measurements, along with the exploration of its effect on pose estimation accuracy, are the major advancements made in this paper. It extends the work of [32] and builds on prior work of [15,31], but instead represents the state variables as elements of Lie groups, specifically $SE\left(3\right)$, to track both position and orientation (whereas [15] only tracks position and assumes orientation measurements are noise-free). Furthermore, this paper presents in detail a novel Lie group formulation for assumptions typically used in human pose estimation (e.g., zero velocity update, constant body segment lengths, and a hinged knee joint). While not our focus here, our algorithm, with its $SE\left(3\right)$ representation, is able to track the global position of body segments, taking into account IMU measurements during the prediction step, and a simpler implementation of certain measurement assumptions (e.g., zero velocity update), though at the expense of having an additional constraint step to ensure satisfaction of biomechanical constraints. The algorithm design was motivated by the need for a body pose representation that more closely models the human biomechanical system (without a dramatic increase in the dimensions of the tracked state) from which the missing kinematic information of uninstrumented body segments are inferred. The contributions of this paper advance the development of gait assessment tools for comfortable and long-term monitoring of lower body movement.

2. Mathematical Background: Lie Group and Lie Algebra

The matrix Lie group G is a group of $n\times n$ matrices (e.g., $SE\left(3\right)$). Mathematically speaking, it is also a smooth manifold with smooth group composition and inversion (i.e., matrix multiplication and inversion). The Lie algebra $\mathfrak{g}$ can be represented in the vector space and is closely related to Lie group G. It represents a tangent space of a group at the identity element [33]. The elegance of Lie theory lies in its ability to represent pose using a vector space (e.g., Lie group G is represented by $\mathfrak{g}$) without additional constraints (e.g., without requiring ${\mathbf{R}}^T\mathbf{R}=\mathbf{I}$ which is using a rotation matrix representation of orientation, or $\left|\right|\mathit{q}\left|\right|=1$ which is using a quaternion representation of orientation) [34].

The matrix exponential $exp{}_G:\mathfrak{g}\to G$ (Equation (1)) and matrix logarithm $log{}_G:G\to \mathfrak{g}$ relates (i.e., local diffeomorphism) the Lie group G and its Lie algebra $\mathfrak{g}$. The Lie algebra $\mathfrak{g}$ is a $n\times n$ matrix that can be represented compactly in an n-dimensional vector space. A linear isomorphism (i.e., one-to-one mapping) between $\mathfrak{g}$ and ${\mathbb{R}}^n$ is given by operators ${\left[\right]}_G^{\vee }:\mathfrak{g}\to {\mathbb{R}}^n$ and ${\left[\right]}_G^{\wedge }:{\mathbb{R}}^n\to \mathfrak{g}$, which map between the compact and matrix representation of the Lie algebra $\mathfrak{g}$. Figure 1 shows an illustration of the said mappings.

Furthermore, the adjoint operator of a Lie group, ${\mathrm{Ad}}_G\left(\mathbf{X}\right)$, the adjoint operator of a Lie algebra, ${\mathrm{ad}}_G\left(\mathit{v}\right)$, and the right jacobian, ${\mathsf{\Phi }}_G\left(\mathit{v}\right)$ (Equation (2)), where $\mathbf{X}\in G$ and ${\left[\mathit{v}\right]}_G^{\wedge }\in \mathfrak{g}$ will be used in later sections. Multiplying an n-dimensional vector representation of a Lie algebra with ${\mathrm{Ad}}_G\left(\mathbf{X}\right)\in {\mathbb{R}}^{n\times n}$ (i.e., the product ${\mathrm{Ad}}_G\left(\mathbf{X}\right)\mathit{v}$) transforms the vector from one coordinate frame to another, similar to how rotation matrices transform points from one frame to another. ${\mathrm{ad}}_G\left(\mathit{v}\right)$ is the Lie algebra of ${\mathrm{Ad}}_G\left(\mathbf{X}\right)$.A summary of the operators for Lie groups $SO\left(3\right)$, $SE\left(3\right)$, and $R^n$ will be explained in the next subsections. They will serve as building blocks to implement the algorithm being described by this paper. For a more detailed introduction to Lie groups refer to [18,34,35].

$$\begin{array}{c}exp\left({\left[\mathit{v}\right]}_G^{\wedge }\right)={\sum }_{n=0}^{\infty }\frac{1}{n!}{\left({\left[\mathit{v}\right]}_G^{\wedge }\right)}^n\end{array}$$

(1)

$$\begin{array}{c}{\mathsf{\Phi }}_G\left(\mathit{v}\right)={\sum }_{i=0}^{\infty }\frac{{(-1)}^i}{(i+1)!}{\mathrm{ad}}_G{\left(\mathit{v}\right)}^i,\mathit{v}\in {\mathbb{R}}^n\end{array}$$

(2)

2.1. Special Orthogonal Group $SO\left(3\right)$

The special orthogonal group, $SO\left(3\right):=\left\{\mathbf{R}\in {\mathbb{R}}^{3\times 3}|\mathbf{R}{\mathbf{R}}^T=1,det\mathbf{R}=1\right\}$, represents orientation, where $\mathbf{R}$ is the typical $3\times 3$ rotation matrix whose column vectors represent the x, y, and z basis vectors. The operations for $SO\left(3\right)$ are listed below, and will serve as building blocks for $SE\left(3\right)$, which will be described in the next subsection. Note that ${\left[\mathit{a}\right]}_{SO\left(3\right)}^{\wedge }\mathit{b}$ is equivalent to the cross product of $\mathit{a}$ and $\mathit{b}$. See Chapter 7 of [18] for details.

$$\begin{array}{c}{\left[\mathbf{\varphi }\right]}_{SO\left(3\right)}^{\wedge }={\left[\begin{array}{c}{\varphi }_1\\ {\varphi }_2\\ {\varphi }_3\end{array}\right]}_{SO\left(3\right)}^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right],{\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]}_{SO\left(3\right)}^{\vee }=\left[\begin{array}{c}{\varphi }_1\\ {\varphi }_2\\ {\varphi }_3\end{array}\right]=\mathbf{\varphi }\end{array}$$

(3)

If $\mathbf{\varphi }/\left|\mathbf{\varphi }\right|$ represents a unit vector axis we wish to rotate around, and $\left|\mathbf{\varphi }\right|$ is the angle by which we wish to rotate, then the rotation matrix, $\mathbf{R}$, which will implement this rotation is given by Equation (4), which is also known as the Rodrigues’ axis-angle rotation formula. When $\mathbf{\varphi }$ is very small, $\mathbf{R}\approx {\mathbf{I}}_{3\times 3}+{\left[\mathbf{\varphi }\right]}_{SO\left(3\right)}^{\wedge }$.

$$\begin{array}{c}\mathbf{R}=exp\left({\left[\mathbf{\varphi }\right]}_{SO\left(3\right)}^{\wedge }\right)=cos\left(\left|\mathbf{\varphi }\right|\right){\mathbf{I}}_{3\times 3}+(1-cos\left(\left|\mathbf{\varphi }\right|\right))\frac{\mathbf{\varphi }{\mathbf{\varphi }}^T}{{\left|\mathbf{\varphi }\right|}^2}+sin\left(\left|\mathbf{\varphi }\right|\right){\left[\frac{\mathbf{\varphi }}{\left|\mathbf{\varphi }\right|}\right]}_{SO\left(3\right)}^{\wedge }\end{array}$$

(4)

Furthermore, the Lie algebra adjoint, Lie group adjoint, and inverse operators are listed in Equation (5).

$$\begin{array}{c}{\mathrm{ad}}_{SO\left(3\right)}\left(\mathbf{\varphi }\right)={\left[\mathbf{\varphi }\right]}_{SO\left(3\right)}^{\wedge },{\mathrm{Ad}}_{SO\left(3\right)}\left(\mathbf{R}\right)=\mathbf{R},{\mathbf{R}}^{-1}={\mathbf{R}}^T\end{array}$$

(5)

Lastly, to approximate the compound rotations, ${\mathbf{R}}_1{\mathbf{R}}_2$, in the Lie algebra space where ${\mathbf{R}}_1=exp\left({\left[{\mathbf{\varphi }}_1\right]}_{SO\left(3\right)}^{\wedge }\right)$ and ${\mathbf{R}}_2=exp\left({\left[{\mathbf{\varphi }}_2\right]}_{SO\left(3\right)}^{\wedge }\right)$, we can use Equation (6). The right Jacobian, ${\mathsf{\Phi }}_{SO\left(3\right)}\left(\mathbf{\varphi }\right)\in {\mathbb{R}}^{3\times 3}$, is obtained using Equation (7).

$$\begin{array}{c}{[log\left({\mathbf{R}}_1{\mathbf{R}}_2\right)]}_{SO\left(3\right)}^{\vee }\approx {\mathbf{\varphi }}_1+{\mathsf{\Phi }}_{SO\left(3\right)}{\left({\mathbf{\varphi }}_1\right)}^{-1}{\mathbf{\varphi }}_2\in \mathfrak{so}\left(\mathfrak{3}\right)\end{array}$$

(6)

$$\begin{array}{c}{\mathsf{\Phi }}_{SO\left(3\right)}\left(\mathbf{\varphi }\right)=\frac{sin\left(\right|\mathbf{\varphi }\left|\right)}{\left|\mathbf{\varphi }\right|}{\mathbf{I}}_{3\times 3}+\left(1-\frac{sin\left(\right|\mathbf{\varphi }\left|\right)}{\left|\mathbf{\varphi }\right|}\right)\frac{\mathbf{\varphi }{\mathbf{\varphi }}^T}{{\left|\mathbf{\varphi }\right|}^2}-\frac{1-cos\left(\right|\mathbf{\varphi }\left|\right)}{\left|\mathbf{\varphi }\right|}{\left[\frac{\mathbf{\varphi }}{\left|\mathbf{\varphi }\right|}\right]}_{SO\left(3\right)}^{\wedge }\in {\mathbb{R}}^{3\times 3}\end{array}$$

(7)

2.2. Special Euclidean Group, $SE\left(3\right)$

The special Euclidean group, $SE\left(3\right):=\left\{\mathbf{T}=\left[\begin{array}{cc}\hfill \mathbf{R}& \mathit{t}\\ \hfill {\mathbf{0}}^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|\left\{\mathbf{R},\mathit{t}\right\}\in SO\left(3\right)\times {\mathbb{R}}^3\right\}$, represents orientation and translation, where $\mathbf{T}$ is the typical $4\times 4$ transformation matrix, $\mathbf{R}$ is the rotation matrix, and $\mathit{t}$ represents a coordinate point in Euclidean space. The operations for $SE\left(3\right)$ are listed below. ${\mathbf{I}}_{i\times i}$ and ${\mathbf{0}}_{i\times j}$ denote $i\times i$ identity and $i\times j$ zero matrices. See Chapter 7 of [18] for details.

$$\begin{array}{c}{\left[\mathbf{\xi }\right]}_{SE\left(3\right)}^{\wedge }={\left[\begin{array}{c}\mathbf{\rho }\\ \mathbf{\varphi }\end{array}\right]}_{SE\left(3\right)}^{\wedge }=\left[\begin{array}{cc}{\left[\mathbf{\varphi }\right]}_{SO\left(3\right)}^{\wedge }& \mathbf{\rho }\\ {\mathbf{0}}_{1\times 3}& 0\end{array}\right],{\left[\begin{array}{cc}{\left[\mathbf{\varphi }\right]}_{SO\left(3\right)}^{\wedge }& \mathbf{\rho }\\ {\mathbf{0}}_{1\times 3}& 0\end{array}\right]}_{SE\left(3\right)}^{\vee }=\left[\begin{array}{c}\mathbf{\rho }\\ \mathbf{\varphi }\end{array}\right]\end{array}$$

(8)

$$\begin{array}{c}\mathbf{T}=exp\left({\left[\mathbf{\xi }\right]}_{SE\left(3\right)}^{\wedge }\right)=\left[\begin{array}{cc}exp\left({\left[\mathbf{\varphi }\right]}_{SO\left(3\right)}^{\wedge }\right)& {\mathsf{\Phi }}_{SO\left(3\right)}(-\mathbf{\varphi })\mathbf{\rho }\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& \mathit{t}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\end{array}$$

(9)

$$\begin{array}{c}{\mathrm{ad}}_{SE\left(3\right)}\left(\mathbf{\xi }\right)=\left[\begin{array}{cc}{\left[\mathbf{\varphi }\right]}_{SO\left(3\right)}^{\wedge }& {\left[\mathbf{\rho }\right]}_{SO\left(3\right)}^{\wedge }\\ {\mathbf{0}}_{3\times 3}& {\left[\mathbf{\varphi }\right]}_{SO\left(3\right)}^{\wedge }\end{array}\right],{\mathrm{Ad}}_{SE\left(3\right)}\left(\mathbf{T}\right)=\left[\begin{array}{cc}\mathbf{R}& {\left[\mathbf{\rho }\right]}_{SO\left(3\right)}^{\wedge }\mathbf{R}\\ {\mathbf{0}}_{3\times 3}& \mathbf{R}\end{array}\right],{\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^T& -{\mathbf{R}}^T\mathbf{\rho }\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\end{array}$$

(10)

Lastly, we note the useful identity defined in Equation (11) where ${\left[\mathit{a}\right]}_{SE\left(3\right)}^{\wedge },{\left[\mathit{b}\right]}_{SE\left(3\right)}^{\wedge }\in \mathfrak{se}\left(\mathfrak{3}\right)$ which is the Lie algebra of the Lie Group $SE\left(3\right)$ ([18], Equation (72)), which will be used to compute the Jacobians of our model later.

$$\begin{array}{c}{\left[{\mathit{a}}_{}^{}\right]}_{SE\left(3\right)}^{\wedge }\mathit{b}={\left[\mathit{b}\right]}_{SE\left(3\right)}^{\odot }\mathit{a},\mathrm{where}\mathit{b}=\left[\begin{array}{c}\mathbf{ϵ}\\ \eta \end{array}\right],{\left[\mathit{b}\right]}_{}^{\odot }=\left[\begin{array}{cc}\eta {\mathbf{I}}_{3\times 3}& -{\left[\mathbf{ϵ}\right]}_{SO\left(3\right)}^{\wedge }\\ {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}\end{array}\right],\mathbf{ϵ}\in {\mathbb{R}}^3,\eta \in \mathbb{R}\end{array}$$

(11)

2.3. Real Numbers ${\mathbb{R}}^n$

In order to represent vector state variables (e.g., translation, velocity, and acceleration) and be consistent with how we used $SE\left(3\right)$ to represent pose, we represented the real numbers $\mathit{s}\in {\mathbb{R}}^n$ as $SE\left(n\right)$ poses with position and no rotation. The operations for ${\mathbb{R}}^n$ are listed below.

$$\begin{array}{c}{\left[\mathit{s}\right]}_{{\mathbb{R}}^n}^{\wedge }=\left[\begin{array}{cc}{\mathbf{0}}_{n\times n}& \mathit{s}\\ {\mathbf{0}}_{1\times n}& 0\end{array}\right],{\left[\begin{array}{cc}{\mathbf{0}}_{n\times n}& \mathit{s}\\ {\mathbf{0}}_{1\times n}& 0\end{array}\right]}_{{\mathbb{R}}^n}^{\vee }=\mathit{s}\end{array}$$

(12)

$$\begin{array}{c}\mathbf{S}=exp\left({\left[\mathit{s}\right]}_{{\mathbb{R}}^n}^{\wedge }\right)=\left[\begin{array}{cc}{\mathbf{I}}_{n\times n}& \mathit{s}\\ {\mathbf{0}}_{1\times n}& 1\end{array}\right],{\left[log\left(\left[\begin{array}{cc}{\mathbf{I}}_{n\times n}& \mathit{s}\\ {\mathbf{0}}_{1\times n}& 1\end{array}\right]\right)\right]}_{{\mathbb{R}}^n}^{\vee }=\mathit{s},exp{\left({\left[\mathit{s}\right]}_{{\mathbb{R}}^n}^{\wedge }\right)}^{-1}=\left[\begin{array}{cc}{\mathbf{I}}_{n\times n}& -\mathit{s}\\ {\mathbf{0}}_{1\times n}& 1\end{array}\right]\end{array}$$

(13)

$$\begin{array}{c}{\mathrm{ad}}_{{\mathbb{R}}^n}\left(\mathit{s}\right)={\mathbf{0}}_{n\times n},{\mathrm{Ad}}_{{\mathbb{R}}^n}\left(\mathbf{S}\right)={\mathbf{I}}_{n\times n},{\mathsf{\Phi }}_{{\mathbb{R}}^n}\left(\mathit{s}\right)={\mathbf{0}}_{n\times n}\end{array}$$

(14)

Note that the multiplication of two elements of the Lie group (i.e., the exponential of ${\mathit{s}}_1$ and ${\mathit{s}}_2$) is equivalent to the vector addition of two elements of the Lie algebra (i.e., ${\mathit{s}}_1+{\mathit{s}}_2$).

$$\begin{array}{c}{\left[log\left(exp\left({\left[{\mathit{s}}_1\right]}_{{\mathbb{R}}^n}^{\wedge }\right)exp\left({\left[{\mathit{s}}_2\right]}_{{\mathbb{R}}^n}^{\wedge }\right)\right)\right]}_{{\mathbb{R}}^n}^{\vee }={\mathit{s}}_1+{\mathit{s}}_2\end{array}$$

(15)

3. Algorithm Description

The proposed algorithm, L5S-3IMU, uses a similar model and assumptions to our prior works in [15,31], denoted as CKF-3IMU and CKF-3IMU+D, albeit expressed in Lie group representation. The algorithm uses measurements from three IMUs attached at the pelvis (sacrum) and shanks (slightly above the ankles), and the inter-IMU distance measurements to estimate the orientation of five body segments (i.e., pelvis, thighs, and shanks) with respect the world frame, W (Figure 2). The Lie group representation enables the tracking of both position and orientation (note that CKF-3IMU only tracked position and assumed orientation measurements were noise-free). Figure 3 shows an overview of the proposed algorithm. L5S-3IMU predicts the ankle and pelvis positions through double integration of their linear 3D acceleration, and predicts the shank and pelvis orientation through integration of their linear 3D angular velocity. The orientation estimates are further updated using a third-party orientation estimation algorithm. Drift in the position estimates due to sensor noise, which accumulates quadratically in the double integration of acceleration, was mitigated through the following assumptions: (1) the pelvis position is approximated as the height of the pelvis when the leg(s) are unbent, or as informed by inter-IMU distance measurements, when available; and (2) the ankle velocity and height above the floor are zeroed whenever a footstep is detected. Furthermore, a pseudo-measurement equal to the current global position estimate of the pelvis and ankles is made with a fixed covariance to contain the ever-growing error covariance of the said states. Lastly, constant body segment lengths and hinged knee joints (one degree of freedom (DOF)) with limited range of motion (ROM) were enforced as biomechanical constraints. The pre- and post-processing parts remain exactly the same as the CKF-3IMU algorithm (e.g., acceleration due to body movement was calculated by expressing the acceleration of the instrumented body segment in the world frame using the orientation estimate and then subtracting acceleration due to gravity (i.e., $\mathit{g}={\left[009.81\right]}^T$) [6]).

3.1. System, Measurement, and Constraint Models

The system, measurement, and constraint models are presented below

$$\begin{array}{c}{\mathbf{X}}_k=f({\mathbf{X}}_{k-1},{\mathit{u}}_k,{\mathit{n}}_k)={\mathbf{X}}_{k-1}exp\left({[\Omega ({\mathbf{X}}_{k-1},{\mathit{u}}_k)+{\mathit{n}}_k]}_G^{\wedge }\right)\end{array}$$

(16)

$$\begin{array}{c}{\mathbf{Z}}_k=h\left({\mathbf{X}}_k\right)exp\left({\left[{\mathit{m}}_{\mathit{k}}\right]}_G^{\wedge }\right),{\mathbf{D}}_k=c\left({\mathbf{X}}_k\right)\end{array}$$

(17)

where k is the time step. ${\mathbf{X}}_k\in G$ is the system state, an element of state Lie group G. $\Omega \left({\mathbf{X}}_{k-1},{\mathit{u}}_k\right):G\to {\mathbb{R}}^p$ is a non-linear function which describes how the model acts on the state and input, ${\mathit{u}}_{k-1}$, where p is the number of dimensions of the compact vector representation for Lie algebra $\mathfrak{g}$. ${\mathit{n}}_k$ is a zero-mean process noise vector with covariance matrix $\mathcal{Q}$ (i.e., ${\mathit{n}}_k\sim {\mathcal{N}}_{{\mathbb{R}}^p}({\mathbf{0}}_{p\times 1},\mathcal{Q})$). ${\mathbf{Z}}_k\in G_m$ is the system measurement, an element of the measurement Lie group $G_m$. $h\left({\mathbf{X}}_k\right):G\to G_m$ is the measurement function. ${\mathit{m}}_k$ is a zero-mean measurement noise vector with covariance matrix ${\mathcal{R}}_k$ (i.e., ${\mathit{m}}_k\sim {\mathcal{N}}_{{\mathbb{R}}^q}({\mathbf{0}}_{q\times 1},{\mathcal{R}}_k)$ where q is the number of dimensions of available measurements). ${\mathbf{D}}_k\in G_c$ is the constraint state, an element of constraint Lie group $G_c$. $c\left({\mathbf{X}}_k\right):G\to G_c$ is the equality constraint function the state ${\mathbf{X}}_k$ must satisfy (i.e., $c\left({\mathbf{X}}_k\right)={\mathbf{D}}_k$). Similar to [23,36], the state distribution of ${\mathbf{X}}_k$ is assumed to be a concentrated Gaussian distribution on Lie groups (i.e., ${\mathbf{X}}_k={\mathbf{\mu }}_k{exp}_G{\left[\mathbf{ϵ}\right]}_G^{\wedge }$, where ${\mathbf{\mu }}_k$ is the mean of ${\mathbf{X}}_k$ and Lie algebra error $\mathbf{ϵ}\sim {\mathcal{N}}_{{\mathbb{R}}^p}({\mathbf{0}}_{p\times 1},\mathbf{P})$) [19].

The Lie group state variables ${\mathbf{X}}_k$ model the position, orientation, and velocity of the three instrumented body segments (i.e., pelvis and shanks) as ${\mathbf{X}}_k=\mathrm{diag}({\mathit{T}}_{}^p,{\mathit{T}}_{}^{ls},{\mathit{T}}_{}^{rs},exp({[{[{\left({\mathit{v}}_{}^p\right)}^T{\left({\mathit{v}}_{}^{ls}\right)}^T{\left({\mathit{v}}_{}^{rs}\right)}^T]}^T]}_{{\mathbb{R}}^9}^{\wedge }))$∈$G=SE{\left(3\right)}^3\times {\mathbb{R}}^9$ where ${\mathit{T}}_{}^b\in SE\left(3\right)$ represents the pose (i.e., orientation and position) of body segment b relative to world frame W, and ${\mathit{v}A}_{}^b$ is the velocity of body segment b relative to frame A. If frame A is not specified, assume reference to the world frame, W. The Lie algebra error is denoted as $ϵ={[{\left({\mathbf{ϵ}}_{\mathbf{T}}^p\right)}^T{\left({\mathbf{ϵ}}_{\mathbf{T}}^{ls}\right)}^T{\left({\mathbf{ϵ}}_{\mathbf{T}}^{rs}\right)}^T{\left({\mathbf{ϵ}}_{\mathit{v}}^{mp}\right)}^T{\left({\mathbf{ϵ}}_{\mathit{v}}^{la}\right)}^T{\left({\mathbf{ϵ}}_{\mathit{v}}^{ra}\right)}^T]}^T$ where the first three variables correspond to the Lie group in $SE\left(3\right)$ while the latter three are for ${\mathbb{R}}^9$. ${\left[\right]}_G^{\vee }$, $exp\left({\left[\right]}_G^{\wedge }\right)$, ${[log\left(\right)]}_G^{\vee }$, ${\mathrm{Ad}}_G\left({\mathbf{X}}_k\right)$, and ${\mathsf{\Phi }}_G\left(\right)$ are constructed similarly as ${\mathbf{X}}_k$(e.g., ${\mathrm{Ad}}_G\left({\mathbf{X}}_k\right)=\mathrm{diag}({\mathrm{Ad}}_{SE\left(3\right)}\left({\mathit{T}}_{}^p\right),{\mathrm{Ad}}_{SE\left(3\right)}\left({\mathit{T}}_{}^{ls}\right),{\mathrm{Ad}}_{SE\left(3\right)}\left({\mathit{T}}_{}^{rs}\right),{\mathrm{Ad}}_{{\mathbb{R}}^9}(exp({[{[{\left({\mathit{v}}_{}^p\right)}^T{\left({\mathit{v}}_{}^{ls}\right)}^T{\left({\mathit{v}}_{}^{rs}\right)}^T]}^T]}_{{\mathbb{R}}^9}^{\wedge })))$. Refer to Section 2.2 and Section 2.3 for definition of $SE\left(3\right)$ and ${\mathbb{R}}^n$ operators.

3.2. Lie Group Constrained EKF (LG-CEKF)

The a priori, a posteriori, and constrained state estimate (i.e., estimated mean of ${\mathbf{X}}_k$) for time step k are denoted by ${\widehat{\mu }}_k^{-}$, ${\widehat{\mu }}_k^{+}$, and ${\tilde{\mu }}_k^{+}$, respectively. Note that the true state ${\mathbf{X}}_k$ can be expressed as ${\mathbf{\mu }}_{k}exp\left({\left[ϵ\right]}_G^{\wedge }\right)$ where ${\mathbf{\mu }}_k$ is one of the state means just mentioned with error, ${\left[\mathbf{ϵ}\right]}_G^{\wedge }$. The a priori and a posteriori error covariance matrices are denoted as ${\mathbf{P}}_k^{-}$ and ${\mathbf{P}}_k^{+}$, respectively. Note the error covariance is not updated at the constrain update step. The KF is based on the Lie group EKF, as defined in [36], where the state means (${\widehat{\mu }}_k^{-}$, ${\widehat{\mu }}_k^{+}$, and ${\tilde{\mu }}_k^{+}$) and state error covariance matrices (${\mathbf{P}}_k^{-}$ and ${\mathbf{P}}_k^{+}$) are propagated by the KF at each time step.

3.2.1. Prediction Step

Prediction step estimates the a priori state ${\widehat{\mu }}_k^{-}$ at the next time step. The mean propagation of the three instrumented body segments is governed by Equation (18) where $\Omega ({\tilde{\mu }}_{k-1}^{+},{\mathit{u}}_k)$ (Equation (19)) is the motion model for the three instrumented body segments. Note that the state propagation may not necessarily respect the biomechanical constraints, so joints may become dislocated after this step. The input ${\mathit{u}}_k$ is defined in Equation (20), where the orientation and acceleration as obtained by the IMU attached to segment b with respect world frame W are denoted as ${\breve{\mathit{R}}}_k^b$ and ${\breve{\mathit{a}}}_k^b$ for $b\in \{p,ls,rs\}$, while the angular velocity as obtained by the IMU attached to segment b expressed in frame b is denoted as ${\breve{\mathbf{\omega }}b}_k^{}$.

$$\begin{array}{c}{\widehat{\mu }}_k^{-}={\tilde{\mu }}_{k-1}^{+}exp\left({[\Omega ({\tilde{\mu }}_{k-1}^{+},{\mathit{u}}_k)]}_G^{\wedge }\right)\end{array}$$

(18)

$$\begin{array}{c}\begin{array}{cc}\hfill \Omega ({\tilde{\mu }}_{k-1}^{+},{\mathit{u}}_k)& =\underset{1\times 3}{\underbrace{\begin{array}{c}[{(\Delta t{\tilde{\mathit{v}}}_{k-1}^{mp+}+\frac{\Delta t^2}{2}{\breve{\mathit{a}}}_k^p)}^T{\breve{\mathit{R}}}_k^p\end{array}}} \underset{1\times 3}{\underbrace{\begin{array}{c}\Delta t{\breve{\mathbf{\omega }}p}_k^T\end{array}}} \underset{1\times 3}{\underbrace{\begin{array}{c}{(\Delta t{\tilde{\mathit{v}}}_{k-1}^{la+}+\frac{\Delta t^2}{2}{\breve{\mathit{a}}}_k^{ls})}^T{\breve{\mathit{R}}}_k^{ls}\end{array}}} \underset{1\times 3}{\underbrace{\begin{array}{c}\Delta t{\breve{\mathbf{\omega }}ls}_k^T\end{array}}}\hfill \\ & \underset{1\times 3}{\underbrace{\begin{array}{c}{(\Delta t{\tilde{\mathit{v}}}_{k-1}^{ra+}+\frac{\Delta t^2}{2}{\breve{\mathit{a}}}_k^{rs})}^T{\breve{\mathit{R}}}_k^{rs}\end{array}}} \underset{1\times 3}{\underbrace{\begin{array}{c}\Delta t{\breve{\mathbf{\omega }}rs}_k^T\end{array}}} \underset{1\times 9}{\underbrace{\begin{array}{c}\Delta t{\left({\breve{\mathit{a}}}_k^{mp}\right)}^T\Delta t{\left({\breve{\mathit{a}}}_k^{la}\right)}^T\Delta t{\left({\breve{\mathit{a}}}_k^{ra}\right)}^T{]}^T\end{array}}}\hfill \end{array}\end{array}$$

(19)

$$\begin{array}{c}{\mathit{u}}_k=\left[\begin{array}{ccccccccc}{\breve{\mathit{R}}}_k^p& {\breve{\mathit{R}}}_k^{ls}& {\breve{\mathit{R}}}_k^{rs}& {\breve{\mathit{a}}}_k^p& {\breve{\mathit{a}}}_k^{ls}& {\breve{\mathit{a}}}_k^{rs}& {\breve{\mathbf{\omega }}p}_k^{}& {\breve{\mathbf{\omega }}ls}_k^{}& {\breve{\mathbf{\omega }}rs}_k^{}\end{array}\right]\end{array}$$

(20)

The state error covariance matrix propagation is governed by Equation (21), where ${\mathcal{F}}_k$ represents the matrix Lie group equivalent to the Jacobian of $f({\mathbf{X}}_{k-1},{\mathit{n}}_{k-1})$, $\mathcal{Q}$ is the covariance matrix of the process noise, and ${\mathcal{C}}_k=\frac{\partial }{\partial \mathbf{ϵ}}\Omega ({\tilde{\mu }}_{k-1}^{+}exp\left({\left[ϵ\right]}_G^{\wedge }\right),{\mathit{u}}_k){|}_{\mathbf{ϵ}=0}$ represents the linearization of the motion model with an infinitesimal perturbation $\mathbf{ϵ}$. The process noise covariance matrix, $\mathcal{Q}$, is calculated from the input-to-state matrix $\mathcal{G}$ and the noise variances of the measured acceleration and angular velocity, ${\mathbf{\sigma }}_a^2$ and ${\mathbf{\sigma }}_{\omega }^2$, respectively.

$$\begin{array}{c}{\mathbf{P}}_k^{-}={\mathcal{F}}_k{\mathbf{P}}_{k-1}^{+}{\mathcal{F}}_k^T+{\mathsf{\Phi }}_G(\Omega ({\tilde{\mu }}_{k-1}^{+},{\mathit{u}}_k))\mathcal{Q}{\mathsf{\Phi }}_G{(\Omega ({\tilde{\mu }}_{k-1}^{+},{\mathit{u}}_k))}^T\end{array}$$

(21)

$$\begin{array}{c}{\mathcal{F}}_k={\mathrm{Ad}}_G\left({exp}_G(-{[\Omega ({\tilde{\mu }}_{k-1}^{+},{\mathit{u}}_k)]}_G^{\wedge })\right)+{\mathsf{\Phi }}_G(\Omega ({\tilde{\mu }}_{k-1}^{+},{\mathit{u}}_k)){\mathcal{C}}_k\end{array}$$

(22)

$$\begin{array}{c}\mathcal{Q}=\mathcal{G}\mathrm{diag}({\mathbf{\sigma }}_a^2,{\mathbf{\sigma }}_{\omega }^2){\mathcal{G}}^T\end{array}$$

(23)

(24)

$$\begin{array}{c}\mathcal{G}=\left[\begin{array}{cccccc}\Delta t^2/2{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \Delta t{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& \Delta t^2/2{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \Delta t{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \Delta t^2/2{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \Delta t{\mathbf{I}}_{3\times 3}\\ & \hfill \Delta t{\mathbf{I}}_{9\times 9}\hfill & & & \hfill {\mathbf{0}}_{9\times 9}\hfill & \end{array}\right]\end{array}$$

(25)

3.2.2. Measurement Update

Measurement update estimates the state at the next time step by: (i) updating the orientation state using new orientation measurements of body segments from IMUs; by (ii) encouraging pelvis position to be above the feet, as informed by either some pseudo-measurement or inter-IMU distance measurements; and by (iii) enforcing ankle velocity to reach zero, and the ankle z position to be near the floor level, $z_f$ when step is detected. When only IMU measurements are available, (iia) pelvis z position is encouraged to be close to initial standing height, $z_p$. When inter-IMU distance measurements are available, (iia) is not used. Instead, (iib) ankle distance is directly incorporated while pelvis position is inferred from inter-IMU distance measurements assuming hinged knee joints and constant body segment lengths. The a posteriori state mean ${\widehat{\mu }}_k^{+}$ is calculated following the Lie EKF equations below. Note that ${[log\left(h{\left({\widehat{\mu }}_k^{-}\right)}^{-1}{\mathbf{Z}}_k\right)]}_{G_m}^{\vee }$ in Equation (27) is akin to the KF innovation/residual, where $h{\left({\widehat{\mu }}_k^{-}\right)}^{-1}{\mathbf{Z}}_k$ (derived from Equation (17) assuming ${\mathit{m}}_k=\mathbf{0}$ and ${\mathbf{X}}_k={\widehat{\mu }}_k^{-}$, i.e., ${\mathbf{Z}}_k=h\left({\widehat{\mu }}_k^{-}\right)$) is the innovation/residual in Lie group $G_m$ brought to the vector representation of the Lie algebra space using the inverse exponential (i.e., logarithm) mapping.

$$\begin{array}{c}{\widehat{\mu }}_k^{+}={\widehat{\mu }}_k^{-}{exp}_G\left({\left[{\mathbf{\nu }}_k\right]}_G^{\wedge }\right)\end{array}$$

(26)

$$\begin{array}{c}{\mathbf{\nu }}_k={\mathbf{K}}_k({[log(h{({\widehat{\mu }}_k^{-})}^{-1}{\mathbf{Z}}_k)]}_{G_m}^{\vee })\end{array}$$

(27)

$$\begin{array}{c}{\mathbf{K}}_k={\mathbf{P}}_k^{-}{\mathcal{H}}_k^T{({\mathcal{H}}_k{\mathbf{P}}_k^{-}{\mathcal{H}}_k^T+{\mathcal{R}}_k)}^{-1}\end{array}$$

(28)

${\mathcal{H}}_k$ can be seen as the matrix Lie group equivalent to the Jacobian of $h\left({\mathbf{X}}_k\right)$, and is defined as the concatenation of ${\mathcal{H}}_{ori}$ and ${\mathcal{H}}_{mp,k}$ when inter-IMU distance measurement is not available. When inter-IMU distance measurement is available, ${\mathcal{H}}_{mp,k}$ is replaced by ${\mathcal{H}}_{dist,k}={\left[{\mathcal{H}}_{pla,k}^T{\mathcal{H}}_{pra,k}^T{\mathcal{H}}_{lra,k}^T\right]}^T$. ${\mathcal{H}}_{ls,k}$ and/or ${\mathcal{H}}_{rs,k}$ are also concatenated to ${\mathcal{H}}_k$ when the left and/or right foot contact (FC) is detected (See Equation (9) of [15]). Each component matrix will be described later. The measurement matrix ${\mathbf{Z}}_k\in G_m$, measurement function $h\left({\mathbf{X}}_k\right)\in G_m$, and measurement covariance noise ${\mathcal{R}}_k$ are constructed similarly to ${\mathcal{H}}_k$, but combined using diag instead of concatenation (e.g., ${\mathcal{R}}_k=\mathrm{diag}({\mathbf{\sigma }}_{ori}^2,{\mathbf{\sigma }}_{mp}^2)$).

$$\begin{array}{c}\begin{array}{cc}\hfill {\mathcal{H}}_k& =\frac{\partial }{\partial \mathbf{ϵ}}{\left[log\left(h{({\widehat{\mu }}_k^{-})}^{-1}h({\widehat{\mu }}_k^{-}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)\right]}_{G_m}^{\vee }{|}_{\mathbf{ϵ}=0}\hfill \\ & =\left\{\begin{array}{cc}{\left[{\mathcal{H}}_{ori}^T{\mathcal{H}}_{mp/dist}^T\right]}^T\hfill & \mathrm{no}\mathrm{FC}\hfill \\ [{\mathcal{H}}_{ori}^T{\mathcal{H}}_{mp/dist}^T{\mathcal{H}}_{ls,k}^T{]}^T\hfill & \mathrm{left}\mathrm{FC}\hfill \\ [{\mathcal{H}}_{ori}^T{\mathcal{H}}_{mp/dist}^T{\mathcal{H}}_{rs,k}^T{]}^T\hfill & \mathrm{right}\mathrm{FC}\hfill \\ [{\mathcal{H}}_{ori}^T{\mathcal{H}}_{mp/dist}^T{\mathcal{H}}_{ls,k}^T{\mathcal{H}}_{rs,k}^T{]}^T\hfill & \mathrm{both}\mathrm{FC}\hfill \end{array}\right.\hfill \end{array}\end{array}$$

(29)

Orientation Update

The orientation update utilizes the orientation measurement to update the state estimate as defined by Equation (30), with measurement noise variance ${\mathbf{\sigma }}_{ori}^2$ ($9\times 1$ vector).

$$\begin{array}{c}{h}_{ori}\left({\mathbf{X}}_k\right)=\mathrm{diag}({\mathit{R}}_k^p,{\mathit{R}}_k^{ls},{\mathit{R}}_k^{rs})\in SO{\left(3\right)}^3,{\mathbf{Z}}_{ori}=\mathrm{diag}({\breve{\mathit{R}}}_k^p,{\breve{\mathit{R}}}_k^{ls},{\breve{\mathit{R}}}_k^{rs})\end{array}$$

(30)

${\mathcal{H}}_{ori}$ along with other components of ${\mathcal{H}}_k$ are calculated by applying Equation (29) to their corresponding measurement functions, followed by tedious algebraic manipulation and first order linearization (i.e., $exp\left({\left[ϵ\right]}_{}^{\wedge }\right)\approx \mathbf{I}+{\left[ϵ\right]}_{}^{\wedge }$). The derivation for ${\mathcal{H}}_{ori}$ (Equation (31)) can be solved trivially as ${[log\left(h_{ori}{\left({\widehat{\mu }}_k^{-}\right)}^{-1}{h}_{ori}({\widehat{\mu }}_k^{-}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)]}_{}^{\vee }={[{({\mathbf{ϵ}}_{\mathbf{\varphi }}^p)}^T{({\mathbf{ϵ}}_{\mathbf{\varphi }}^{ls})}^T{({\mathbf{ϵ}}_{\mathbf{\varphi }}^{rs})}^T]}^T$, where ${\mathbf{ϵ}}_{\mathbf{T}}^b={[{({\mathbf{ϵ}}_{\mathbf{\rho }}^b)}^T{({\mathbf{ϵ}}_{\mathbf{\varphi }}^b)}^T]}^T$ for body segment $b\in \{p,ls,rs\}$.

$$\begin{array}{c}{\mathcal{H}}_{ori}=\left[\begin{array}{cccc}{\mathbf{0}}_{3\times 3}{\mathbf{I}}_{3\times 3}& & & \\ & {\mathbf{0}}_{3\times 3}{\mathbf{I}}_{3\times 3}& & {\mathbf{0}}_{9\times 9}\\ & & {\mathbf{0}}_{3\times 3}{\mathbf{I}}_{3\times 3}& \end{array}\right]\end{array}$$

(31)

Pelvis Height Assumption

The pelvis height assumption softly constrains the pelvis z position to be close to initial standing height $z_p$ as defined by Equation (32) (represented in vector space of its Lie algebra) and Equation (33), with measurement noise variance ${\mathbf{\sigma }}_{mp}^2$ ($1\times 1$ vector). This assumption is used only when inter-IMU distance measurement is not available. ${\mathit{i}}_x$, ${\mathit{i}}_y$, ${\mathit{i}}_z$, and ${\mathit{i}}_0$ denote $4\times 1$ vectors whose 1st to 4th rows, respectively, are 1, while the rest are 0; they are used below to select rows, columns, or elements from matrices.

$$\begin{array}{c}{[log\left(h_{mp}^{}\left({\mathbf{X}}_k\right)\right)]}_{}^{\vee }={\mathit{i}}_z^T{\mathit{T}}_k^p{\mathit{i}}_0=\left[\begin{array}{cccc}0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}{\mathit{R}}_k^p& {\mathit{p}}_k^p\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{cccc}0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{\mathit{p}}_k^p\\ 1\end{array}\right]=p_{z,k}^p\in \mathbb{R}\end{array}$$

(32)

$$\begin{array}{c}log\left({\mathbf{Z}}_{mp}^{}\right){]}_{}^{\vee }=z_p\in \mathbb{R}\end{array}$$

(33)

The derivation of ${\mathcal{H}}_{mp,k}=\frac{\partial }{\partial \mathbf{ϵ}}{[log\left(h_{mp}^{}{\left({\widehat{\mu }}_k^{-}\right)}^{-1}{h}_{mp}^{}({\widehat{\mu }}_k^{-}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)]}_{}^{\vee }{|}_{\mathbf{ϵ}=0}$ is shown in Equations (34)–(36). Taking best estimate ${\mathbf{X}}_k={\widehat{\mu }}_k^{-}$ gives us Equation (34).

$$\begin{array}{c}{[log\left(h_{mp}^{}\left({\widehat{\mu }}_k^{-}\right)\right)]}_{}^{\vee }={\mathit{i}}_z^T{\widehat{\mathit{T}}}_k^{p-}{\mathit{i}}_0\end{array}$$

(34)

$$\begin{array}{c}\begin{array}{cc}\hfill {[log\left(h_{mp}^{}({\widehat{\mu }}_k^{-}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)]}_{}^{\vee }& ={\mathit{i}}_z^T{\widehat{\mathit{T}}}_k^{p-}exp\left({\left[{\mathbf{ϵ}}_{\mathbf{T}}^p\right]}_{}^{\wedge }\right){\mathit{i}}_0\hfill \\ & \approx {\mathit{i}}_z^T{\widehat{\mathit{T}}}_k^{p-}{\mathit{i}}_0+{\mathit{i}}_z^T{\widehat{\mathit{T}}}_k^{p-}{\left[{\mathbf{ϵ}}_{\mathbf{T}}^p\right]}_{}^{\wedge }{\mathit{i}}_0,1\mathrm{st}\mathrm{order}\mathrm{linearization}\hfill \\ & \mathrm{Use}\mathrm{Equation}(11),{\left[{\mathit{a}}_{}^{}\right]}_{}^{\wedge }{\mathit{b}}_{}^{}={\left[{\mathit{b}}_{}^{}\right]}_{}^{\odot }{\mathit{a}}_{}^{},\mathrm{to}\mathrm{bring}{\mathbf{ϵ}}_{\mathbf{T}}^p\mathrm{to}\mathrm{right}\mathrm{of}{\mathit{i}}_0\hfill \\ & ={[log\left(h_{mp}^{}\left({\widehat{\mu }}_k^{-}\right)\right)]}_{}^{\vee }+{\mathit{i}}_z^T{\widehat{\mathit{T}}}_k^{p-}{\left[{\mathit{i}}_0\right]}_{}^{\odot }{\mathbf{ϵ}}_{\mathbf{T}}^p\hfill \end{array}\end{array}$$

(35)

Remember ${\mathbf{ϵ}}_{\mathbf{T}}^p$ is a subvector of ${\mathbf{ϵ}}_{}^{}$ as defined in Section 3.1 and is the Lie algebra error of the state in its compact vector representation. Note that if measurement function $h_a^{}\left({\mathbf{X}}_k\right)\in$ Lie group ${\mathbb{R}}^b$, then ${[log\left(h_a^{}{\left({\widehat{\mu }}_k^{-}\right)}^{-1}{h}_a^{}\left({\mathbf{X}}_k\right)\right)]}_{}^{\vee }={[log\left(h_a^{}\left({\mathbf{X}}_k\right)\right)]}_{}^{\vee }-{[log\left(h_a^{}\left({\widehat{\mu }}_k^{-}\right)\right)]}_{}^{\vee }={[log\left(h_a^{}({\widehat{\mu }}_k^{-}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)]}_{}^{\vee }-{[log\left(h_a^{}\left({\widehat{\mu }}_k^{-}\right)\right)]}_{}^{\vee }$ by applying Equations (15) and (13) (inverse of Lie group ${\mathbb{R}}^n$). Finally, ${\mathcal{H}}_{mp,k}$ is calculated as shown in Equation (36).

(36)

Zero Velocity Update and Flat Floor Assumption

When step is detected, the ankle velocity is enforced to be zero and the ankle z position is brought to near the floor level, $z_f$ (i.e., flat floor assumptions). The corresponding measurement function is defined by Equation (37), with measurement noise variance ${\mathbf{\sigma }}_{ls}^2$ ($4\times 1$ vector).

$$\begin{array}{c}{[log\left(h_{ls}^{}\left({\mathbf{X}}_k\right)\right)]}_{}^{\vee }=\left[\begin{array}{c}{\mathit{v}}_{}^{ls}\\ {\mathit{i}}_z^T{\mathit{T}}_k^{ls}{\mathit{i}}_0\end{array}\right]=\left[\begin{array}{c}{\mathit{v}}_{}^{ls}\\ p_{z,k}^{ls}\end{array}\right]\in {\mathbb{R}}^4,{[log\left({\mathbf{Z}}_{ls}^{}\right)]}_{}^{\vee }=\left[\begin{array}{c}{\mathbf{0}}_{3\times 1}\\ z_f\end{array}\right]\end{array}$$

(37)

The zero velocity part of ${\mathcal{H}}_{ls,k}$ (Equation (38)) and ${\mathcal{H}}_{rs,k}$ can also be calculated trivially, while the flat floor assumption can be calculated similarly as ${\mathcal{H}}_{mp,k}$ but the z position set to floor height, $z_f$, instead of the pelvis standing height, $z_p$.

(38)

Left and Right Ankle Distance Measurement

When the inter-IMU distance between the ankles, ${\breve{d}}_k^{lra}$, is available, ankle distance measurement is incorporated as a soft distance constraint. The measurement function is defined by Equation (40), with measurement noise variance ${\mathbf{\sigma }}_{lra}^2$ ($1\times 1$ vector). ${\tau }_{lra}^{}\left({\mathbf{X}}_k\right)$ (Equation (39)) is the vector that points from the right ankle to the left ankle, where ${\mathit{p}ls}_{}^{la}$ is the position of the left ankle expressed in left shank frame, and ${\mathit{p}rs}_{}^{ra}$ is the position of the right ankle expressed in right shank frame. We have chosen that the ankles are at the origin of their respective shank frames. Note that matrix $\mathbf{E}$ converts homogeneous $4\times 1$ coordinates to standard $3\times 1$ coordinates (i.e., drops the 1 from the end of the $4\times 1$ vector).

$$\begin{array}{c}{\tau }_{lra}^{}\left({\mathbf{X}}_k\right)=\stackrel{\mathbf{E}}{\overbrace{\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 1}\end{array}\right]}}(\stackrel{\mathrm{left}\mathrm{ankle}\mathrm{in}W}{\overbrace{{\mathit{T}}_k^{ls}{\mathit{p}ls}_{}^{la}}}-\stackrel{\mathrm{right}\mathrm{ankle}\mathrm{in}W}{\overbrace{{\mathit{T}}_k^{rs}{\mathit{p}rs}_{}^{ra}}}),{\mathit{p}ls}_{}^{la}={\mathit{p}rs}_{}^{ra}=\stackrel{\mathrm{origin}\mathrm{of}\mathrm{frame}}{\overbrace{{\left[\begin{array}{cccc}0& 0& 0& 1\end{array}\right]}^T}}\end{array}$$

(39)

By taking the squared Euclidean distance of ${\tau }_{lra}^{}\left({\mathbf{X}}_k\right)$ (i.e., $\left|\right|{\tau }_{lra}^{}\left({\mathbf{X}}_k\right){\left|\right|}^2$), we can get the ankle distance measurement model.

$$\begin{array}{c}{[log\left(h_{lra}^{}\left({\mathbf{X}}_k\right)\right)]}_{}^{\vee }={\left({\tau }_{lra}^{}\left({\mathbf{X}}_k\right)\right)}^T{\tau }_{lra}^{}\left({\mathbf{X}}_k\right)\in \mathbb{R},{[log\left({\mathbf{Z}}_{lra}^{}\right)]}_{}^{\vee }={\left({\breve{d}}_k^{lra}\right)}^2\end{array}$$

(40)

To solve for ${\mathcal{H}}_{lra,k}$ (Equation (44)), we first solved for ${[log\left(h_{lra}^{}\left({\mathbf{X}}_k\right)\right)]}_{}^{\vee }$ at ${\mathbf{X}}_k={\widehat{\mu }}_k^{-}$ (Equation (41)).

$$\begin{array}{c}{\tau }_{lra}^{}\left({\widehat{\mu }}_k^{-}\right)=\mathbf{E}({\widehat{\mathit{T}}}_k^{ls-}{\mathit{p}ls}_{}^{la}-{\widehat{\mathit{T}}}_k^{rs-}{\mathit{p}rs}_{}^{ra}),{[log\left(h_{lra}^{}\left({\widehat{\mu }}_k^{-}\right)\right)]}_{}^{\vee }={\left({\tau }_{lra}^{}\left({\widehat{\mu }}_k^{-}\right)\right)}^T{\tau }_{lra}^{}\left({\widehat{\mu }}_k^{-}\right)\end{array}$$

(41)

Then solve for ${\tau }_{lra}^{}({\widehat{\mu }}_k^{-}exp\left({\left[ϵ\right]}_G^{\wedge }\right))$ and ${[log\left(h_{lra}^{}({\widehat{\mu }}_k^{-}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)]}_{}^{\vee }$ as shown in Equations (42) and (43).

$$\begin{array}{c}\begin{array}{cc}\hfill {\tau }_{lra}^{}({\widehat{\mu }}_k^{-}exp\left({\left[ϵ\right]}_G^{\wedge }\right))& =\mathbf{E}({\widehat{\mathit{T}}}_k^{ls-}exp\left({\left[{\mathbf{ϵ}}_{\mathbf{T}}^{ls}\right]}_{}^{\wedge }\right){\mathit{p}ls}_{}^{la}-{\widehat{\mathit{T}}}_k^{rs-}exp\left({\left[{\mathbf{ϵ}}_{\mathbf{T}}^{rs}\right]}_{}^{\wedge }\right){\mathit{p}rs}_{}^{ra})\hfill \\ & \mathrm{Take}\mathrm{the}1\mathrm{st}\mathrm{order}\mathrm{approximation}\hfill \\ & \approx \mathbf{E}({\widehat{\mathit{T}}}_k^{ls-}{\mathit{p}ls}_{}^{la}-{\widehat{\mathit{T}}}_k^{rs-}{\mathit{p}rs}_{}^{ra}+{\widehat{\mathit{T}}}_k^{ls-}{\left[{\mathbf{ϵ}}_{\mathbf{T}}^{ls}\right]}_{}^{\wedge }{\mathit{p}ls}_{}^{la}-{\widehat{\mathit{T}}}_k^{rs-}{\left[{\mathbf{ϵ}}_{\mathbf{T}}^{rs}\right]}_{}^{\wedge }{\mathit{p}rs}_{}^{ra})\hfill \\ & ={\tau }_{lra}^{}\left({\widehat{\mu }}_k^{-}\right)+\stackrel{{\mathbf{\Gamma }}_{lra}}{\overbrace{\mathbf{E}\left({\widehat{\mathit{T}}}_k^{ls-}{\left[{\mathit{p}ls}_{}^{la}\right]}_{}^{\odot }{\mathbf{ϵ}}_{\mathbf{T}}^{ls}-{\widehat{\mathit{T}}}_k^{rs-}{\left[{\mathit{p}rs}_{}^{ra}\right]}_{}^{\odot }{\mathbf{ϵ}}_{\mathbf{T}}^{rs}\right)}},\mathrm{Using}\mathrm{Equation}(11)\hfill \end{array}\end{array}$$

(42)

(43)

(44)

Pelvis-to-Ankle Distance Measurement

In addition to the soft ankle distance constraint, the ankle to pelvis vector is inferred from the ankle to pelvis distance measurements while assuming hinged knee joints and constant body segment lengths. The measurement function is defined by Equation (45), with measurement noise variance ${\mathbf{\sigma }}_{pla}^2$ ($3\times 1$ vector), where ${\mathit{p}p}_{}^{mp}$ is the position of the mid-pelvis expressed in pelvis frame, and ${\mathit{p}ls}_{}^{la}$ is the position of the left ankle expressed in left shank frame. We have chosen that the mid-pelvis and ankle are at the origin of their corresponding reference frames.

$$\begin{array}{c}{[log\left(h_{pla}^{}\left({\mathbf{X}}_k\right)\right)]}_{}^{\vee }=\mathbf{E}(\stackrel{\mathrm{mid}-\mathrm{pelvis}\mathrm{in}W}{\overbrace{{\mathit{T}}_k^p{\mathit{p}p}_{}^{mp}}}-\stackrel{\mathrm{left}\mathrm{ankle}\mathrm{in}W}{\overbrace{{\mathit{T}}_k^{ls}{\mathit{p}ls}_{}^{la}}})\in {\mathbb{R}}^3,{\mathit{p}p}_{}^{mp}={\mathit{p}ls}_{}^{la}={\left[\begin{array}{cccc}0& 0& 0& 1\end{array}\right]}^T\end{array}$$

(45)

The measurement pelvis to left ankle vector can be calculated from the measured pelvis to left ankle distance, ${\breve{d}}_k^{pla}$ as shown in Equation (46) which is the Lie Group reformulation of [31] (Equation (4)). In essence, Equation (47) calculates the most probably knee angle assuming hinged knee joint and constant body segment lengths, then Equation (46) adds the thigh (expressed in shank coordinate system with knee angle ${\widehat{\theta }}_k^{lk}$) and shank long axis to the hips to obtain the pelvis-to-ankle vector. See Appendix A for derivation. There are two solutions for ${\widehat{\theta }}_k^{lk}$ due to the inverse cosine in Equation (47). We chose the ${\widehat{\theta }}_k^{lk}$ value as that closer to the current left knee angle estimate from the prediction step. Note that this measurement function could also be formulated as a linearized Euclidean distance between the pelvis and ankle (i.e., similar to Equation (44)); however, a preliminary exploration of this approach showed poorer performance.

$$\begin{array}{c}{[log\left({\mathbf{Z}}_{pla,k}^{}\right)]}_{}^{\vee }=\stackrel{{\mathbf{\psi }}_{pla}=\mathrm{half}\mathrm{pelvis}y-\mathrm{axis}+\mathrm{shank}z-\mathrm{axis}}{\overbrace{\frac{d^p}{2}{\widehat{\mathit{T}}}_k^{p-}{\mathit{i}}_y^{}-d^{ls}{\widehat{\mathit{T}}}_k^{ls-}{\mathit{i}}_z^{}}}+d^{lt}{\widehat{\mathit{T}}}_k^{ls-}\stackrel{\mathrm{thigh}z-\mathrm{axis}\mathrm{in}\mathrm{shank}\mathrm{frame}}{\overbrace{({\mathit{i}}_x^{}sin\left({\widehat{\theta }}_k^{lk}\right)-{\mathit{i}}_z^{}cos\left({\widehat{\theta }}_k^{lk}\right))}}\in {\mathbb{R}}^3\end{array}$$

(46)

$$\begin{array}{c}{\widehat{\theta }}_k^{lk}={cos}^{-1}\left(\frac{\alpha \gamma \pm \beta \sqrt{{\alpha }^2+{\beta }^2-{\gamma }^2}}{{\alpha }^2+{\beta }^2}\right)\mathrm{where}\begin{array}{cc}& \alpha =-2d^{lt}{\mathbf{\psi }}_{pla}^T{\widehat{\mathit{T}}}_k^{ls-}{\mathit{i}}_z^{},\beta =2d^{lt}{\mathbf{\psi }}_{pla}^T{\widehat{\mathit{T}}}_k^{ls-}{\mathit{i}}_x^{},\hfill \\ & \gamma ={\left({\breve{d}}_k^{pla}\right)}^2-{\mathbf{\psi }}_{pla}^T{\mathbf{\psi }}_{pla}-{\left(d^{lt}\right)}^2\hfill \end{array}\end{array}$$

(47)

To calculate for ${\mathcal{H}}_{pla,k}$, we first solved for ${[log\left(h_{pla}^{}\left({\mathbf{X}}_k\right)\right)]}_{}^{\vee }$ at ${\mathbf{X}}_k={\widehat{\mu }}_k^{-}$ similar to Equation (41).

$$\begin{array}{c}{[log\left(h_{pla}^{}\left({\widehat{\mu }}_k^{-}\right)\right)]}_{}^{\vee }={\tau }_{pla}^{}\left({\widehat{\mu }}_k^{-}\right)=\mathbf{E}({\widehat{\mathit{T}}}_k^{p-}{\mathit{p}p}_{}^{mp}-{\widehat{\mathit{T}}}_k^{ls-}{\mathit{p}ls}_{}^{la})\end{array}$$

(48)

Then solve for ${[log\left(h_{pla}^{}({\widehat{\mu }}_k^{-}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)]}_{}^{\vee }$ similar to Equation (42) (i.e., distance between mid-pelvis and left ankle) giving us ${[log\left(h_{pla}^{}({\widehat{\mu }}_k^{-}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)]}_{}^{\vee }={\tau }_{pla}^{}\left({\widehat{\mu }}_k^{-}\right)+{\mathbf{\Gamma }}_{pla}$. ${\mathcal{H}}_{pla,k}$ is then calculated as shown in Equation (49). The right side of the pelvis-to-ankle distance measurement (i.e., $h_{pra}\left({\widehat{\mu }}_k^{-}\right)$, ${\mathbf{Z}}_{pra}$, ${\mathcal{H}}_{pra,k}$) can be solved similarly to the left side.

(49)

Covariance Limiter

Lastly, the error covariance of the position estimates of the three instrumented body segments must be prevented from growing unbounded and/or becoming badly conditioned, as will occur naturally when tracking global position of objects without any global position reference. At this step, a pseudo-measurement equal to the current state ${\widehat{\mu }}_k^{+}$ is used (implemented by Equation (50)) with some measurement noise of variance ${\mathbf{\sigma }}_{lim}^{}$ ($9\times 1$ vector). The covariance ${\mathbf{P}}_k^{+}$ is then calculated through Equations (51)–(53).

(50)

$$\begin{array}{c}{\mathcal{H}}_k^{\prime }={\left[{\mathcal{H}}_k^T{\mathcal{H}}_{lim}^T\right]}^T,{\mathcal{R}}_k^{\prime }=\mathrm{diag}({\mathbf{\sigma }}_k^2,{\mathbf{\sigma }}_{lim}^2)\end{array}$$

(51)

$$\begin{array}{c}{\mathbf{K}}_k^{\prime }={\mathbf{P}}_k^{-}{\mathcal{H}}_k^{\prime T}{({\mathcal{H}}_k^{\prime }{\mathbf{P}}_k^{-}{\mathcal{H}}_k^{\prime T}+{\mathcal{R}}^{\prime })}^{-1}\end{array}$$

(52)

$$\begin{array}{c}{\mathbf{P}}_k^{+}={\mathsf{\Phi }}_G\left({\mathbf{\nu }}_k\right)(\mathbf{I}-{\mathbf{K}}_k^{\prime }{\mathcal{H}}_k^{\prime }){\mathbf{P}}_k^{-}{\mathsf{\Phi }}_G{\left({\mathbf{\nu }}_k\right)}^T\end{array}$$

(53)

3.2.3. Satisfying Biomechanical Constraints

After the preceding updates, the joint positions or angles may be beyond their allowed range (i.e., knee hyperflexion). The constraint update corrects the kinematic state estimates to satisfy the biomechanical constraints of the human body by projecting the current a posteriori state estimate ${\widehat{\mu }}_k^{+}$ onto the constraint surface, guided by our uncertainty in each state variable, which is encoded by ${\mathbf{P}}_k^{+}$. The following biomechanical constraint equations are enforced: (i) estimated thigh long axis vector lengths equal the thigh lengths; (ii) both knees act as hinge joints (formulation similar to Section 2.3, Equation (4) of [9]); and (iii) the knee joint angle is within realistic range. The constraint functions are similar to Section II-E.3 of [15] but expressed under $SE\left(3\right)$ state variables. The constrained state ${\tilde{\mu }}_k^{+}$ can be calculated using the equations below, similar to the measurement update of [36] with zero noise, where ${\mathcal{C}}_k={\left[{\mathcal{C}}_{L,k}^T{\mathcal{C}}_{R,k}^T\right]}^T$. ${\mathcal{C}}_{L,k}$ is the concatenation of ${\mathcal{C}}_{ltl,k}$, ${\mathcal{C}}_{lkh,k}$, and ${\mathcal{C}}_{lkr,k}$; the last matrix is not concatenated when the knee angle, ${\alpha }_{lk}$, is within its allowed range (i.e., ${\alpha }_{lk,min}\le {\alpha }_{lk}\le {\alpha }_{lk,max}$). ${\mathcal{C}}_{ltl,k}$, ${\mathcal{C}}_{lkh,k}$, and ${\mathcal{C}}_{lkr,k}$ corresponds to the biomechanical constraint for the left thigh length (ltl), left knee hinged joint (lkh), and left knee angle ROM (lkr), respectively, which will be described more later. ${\mathcal{C}}_{R,k}$ can be derived similarly, while ${\mathbf{D}}_k$ and $c\left({\widehat{\mu }}_k^{+}\right))$ are constructed similarly to ${\mathbf{Z}}_k$.

$$\begin{array}{c}{\tilde{\mu }}_k^{+}={\widehat{\mu }}_k^{+}exp\left({\left[{\mathbf{\nu }}_k\right]}_G^{\wedge }\right)\end{array}$$

(54)

$$\begin{array}{c}{\mathbf{\nu }}_k={\mathbf{K}}_k({[log\left(c{\left({\widehat{\mu }}_k^{+}\right)}^{-1}{\mathbf{D}}_k\right)]}_{G_c}^{\vee }\end{array}$$

(55)

$$\begin{array}{c}{\mathbf{K}}_k={\mathbf{P}}_k^{+}{\mathcal{C}}_k^T{\left({\mathcal{C}}_k{\mathbf{P}}_k^{+}{\mathcal{C}}_k^T\right)}^{-1})\end{array}$$

(56)

$$\begin{array}{c}{\mathcal{C}}_k=\frac{\partial }{\partial \mathbf{ϵ}}{[log\left(c{\left({\widehat{\mu }}_k^{+}\right)}^{-1}c({\widehat{\mu }}_k^{+}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)]}_{G_c}^{\vee }{|}_{\mathbf{ϵ}=0}\end{array}$$

(57)

Thigh Length Constraint

Firstly, the thigh length constraint is shown in Equation (59), where ${\tau }_z^{lt}\left({\mathbf{X}}_k\right)$ (Equation (58)) denotes the thigh long axis vector and $d^{lt}$ denotes the measured thigh length during calibration. ${\mathit{p}p}_{}^{lh}$ is the position of the left hip expressed in pelvis frame, and ${\mathit{p}ls}_{}^{lk}$ is the position of the left knee expressed in left shank frame. We have chosen that the left hip to be $\frac{d_{}^p}{2}$ to the left of the mid-pelvis origin, and the left knee to be $d_{}^{ls}$ from the left shank origin (i.e., from the left ankle).

$$\begin{array}{c}{\tau }_z^{lt}\left({\mathbf{X}}_k\right)=\mathbf{E}(\stackrel{\mathrm{hip}\mathrm{jt}.\mathrm{pos}.\mathrm{in}W}{\overbrace{{\mathit{T}}_{}^p{\mathit{p}p}_{}^{lh}}}-\stackrel{\mathrm{knee}\mathrm{jt}.\mathrm{pos}.\mathrm{in}W}{\overbrace{{\mathit{T}}_{}^{ls}{\mathit{p}ls}_{}^{lk}}}),{\mathit{p}p}_{}^{lh}={\left[\begin{array}{cccc}0& \frac{d^p}{2}& 0& 1\end{array}\right]}^T,{\mathit{p}ls}_{}^{lk}={\left[\begin{array}{cccc}0& 0& d^{ls}& 1\end{array}\right]}^T\end{array}$$

(58)

$$\begin{array}{c}log\left(c_{ltl}^{}\left({\mathbf{X}}_k\right)\right){]}_{}^{\vee }={\left({\tau }_z^{lt}\left({\mathbf{X}}_k\right)\right)}^T{\tau }_z^{lt}\left({\mathbf{X}}_k\right)\in \mathbb{R},{[log\left({\mathbf{D}}_{ltl}^{}\right)]}_{}^{\vee }={\left(d^{lt}\right)}^2\end{array}$$

(59)

${\mathcal{C}}_{ltl,k}$ is calculated using Equation (60).

$$\begin{array}{c}\begin{array}{cc}\hfill {\mathcal{C}}_{ltl,k}& =\frac{\partial }{\partial \mathbf{ϵ}}{[log\left(c_{ltl}^{}{\left({\widehat{\mu }}_k^{+}\right)}^{-1}{c}_{ltl}^{}({\widehat{\mu }}_k^{+}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)]}_{}^{\vee }{|}_{\mathbf{ϵ}=0}\hfill \\ & =\frac{\partial }{\partial \mathbf{ϵ}}\left({[log\left(c_{ltl}^{}({\widehat{\mu }}_k^{+}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)]}_{}^{\vee }-{[log\left(c_{ltl}^{}\left({\widehat{\mu }}_k^{+}\right)\right)]}_{}^{\vee }\right){|}_{\mathbf{ϵ}=0}\hfill \end{array}\end{array}$$

(60)

Following similar procedure to ${\mathcal{H}}_{lra,k}$, we obtain ${\tau }_z^{lt}({\widehat{\mu }}_k^{+}exp\left({\left[ϵ\right]}_G^{\wedge }\right))={\tau }_z^{lt}\left({\widehat{\mu }}_k^{+}\right)+{\mathbf{\Gamma }}_{ltz}$ (similar to Equation (42)), and ${[log\left(c_{ltl}^{}({\widehat{\mu }}_k^{+}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)]}_{}^{\vee }={[log\left(c_{ltl}^{}\left({\widehat{\mu }}_k^{+}\right)\right)]}_{}^{\vee }+2{\left({\tau }_z^{lt}\left({\widehat{\mu }}_k^{+}\right)\right)}^T\mathbf{E}({\widehat{\mathit{T}}}_k^{p+}{\left[{\mathit{p}p}_{}^{lh}\right]}_{}^{\odot }{\mathbf{ϵ}}_{\mathbf{T}}^p-{\widehat{\mathit{T}}}_k^{ls+}{\left[{\mathit{p}ls}_{}^{lk}\right]}_{}^{\odot }{\mathbf{ϵ}}_{\mathbf{T}}^{ls})$ (similar to Equation (43)), which if we substitute in Equation (60) gives us Equation (61)

(61)

Hinge Knee Joint Constraint

Secondly, the hinge knee joint constraint as defined by Equation (62) is enforced by having the long (z) axis of the thigh to be perpendicular to the mediolateral axis (y) of the shank. For example, on the left leg, we would want ${\mathit{r}}_y^{ls}$ be perpendicular to the thigh long axis vector ${\tau }_z^{lt}\left({\widehat{\mu }}_k^{+}\right)$ (i.e., the dot product of ${\mathit{r}}_y^{ls}$ and ${\tau }_z^{lt}\left({\widehat{\mu }}_k^{+}\right)$ should be 0). Refer to Figure 2 for visualization. This formulation is similar to Section 2.3, Equation (4) of [9].

$$\begin{array}{c}{[log\left(c_{lkh}^{}\left({\mathbf{X}}_k\right)\right)]}_{}^{\vee }={\left(\mathbf{E}{\mathit{T}}_{}^{ls}{\mathit{i}}_y\right)}^T{\tau }_z^{lt}\left({\mathbf{X}}_k\right)={\left({\mathit{r}}_y^{ls}\right)}^T{\tau }_z^{lt}\left({\mathbf{X}}_k\right)\in \mathbb{R},{[log\left({\mathbf{D}}_{lkh}^{}\right)]}_{}^{\vee }=0\end{array}$$

(62)

Following similar procedure to ${\mathcal{C}}_{ltl,k}$ and taking ${\mathbf{X}}_k={\widehat{\mu }}_k^{+}$, ${[log\left(c_{lkh}^{}\left({\widehat{\mu }}_k^{+}\right)\right)]}_{}^{\vee }$ and ${[log\left(c_{lkh}^{}({\widehat{\mu }}_k^{+}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)]}_{}^{\vee }$ can be calculated as shown in Equations (63) and (64), respectively.

$$\begin{array}{c}{[log\left(c_{lkh}^{}\left({\widehat{\mu }}_k^{+}\right)\right)]}_{}^{\vee }={\left(\mathbf{E}{\widehat{\mathit{T}}}_{}^{ls+}{\mathit{i}}_y\right)}^T{\tau }_z^{lt}\left({\widehat{\mu }}_k^{+}\right)\end{array}$$

(63)

(64)

${\mathcal{C}}_{lkh,k}$ can be calculated using Equation (65).

$$\begin{array}{c}\begin{array}{cc}\hfill {\mathcal{C}}_{lkh,k}& =\frac{\partial }{\partial \mathbf{ϵ}}{[log(c_{lkh}^{}{\left({\widehat{\mu }}_k^{+}\right)}^{-1}{c}_{lkh}^{}({\widehat{\mu }}_k^{+}exp\left({\left[ϵ\right]}_G^{\wedge }\right)))]}_{}^{\vee }{|}_{\mathbf{ϵ}=0}\hfill \\ & =\frac{\partial }{\partial \mathbf{ϵ}}\left({[log\left(c_{lkh}^{}({\widehat{\mu }}_k^{+}exp\left({\left[ϵ\right]}_G^{\wedge }\right))\right)]}_{}^{\vee }-{[log\left(c_{lkh}^{}\left({\widehat{\mu }}_k^{+}\right)\right)]}_{}^{\vee }\right){|}_{\mathbf{ϵ}=0}\hfill \end{array}\end{array}$$

(65)

Substituting Equations (63) and (64) into Equation (65) gives us Equation (66).

(66)

Knee Range of Motion Constraint

Thirdly, the knee ROM constraint is defined by Equation (69) and is only enforced if the knee angle, ${\alpha }_{lk}$, is outside the allowed ROM. The bounded knee angle, ${\alpha }_{lk}^{\prime }$, is calculated by Equation (67). Equation (69) is obtained by expanding Equation (67) to Equation (68) which when rearranged gives us ${[log\left(c_{lkr}^{}\left({\mathbf{X}}_k\right)\right)]}_{}^{\vee }$ (i.e., Lie group representation of Equation (26) in [15]). Note that ${\mathit{r}ls}_z^{lt}$ is the normalized thigh long axis expressed in the left shank frame.

$$\begin{array}{c}{\alpha }_{lk}={tan}^{-1}\left(\frac{-{\left({\mathit{r}}_z^{ls}\right)}^T{\mathit{r}}_z^{lt}}{-{\left({\mathit{r}}_x^{ls}\right)}^T{\mathit{r}}_z^{lt}}\right)+\frac{\pi }{2},{\alpha }_{lk}^{\prime }=\mathbf{min}({\alpha }_{lk,max},\mathbf{max}({\alpha }_{lk,min},{\alpha }_{lk}))\end{array}$$

(67)

$$\begin{array}{c}\frac{-{\mathit{r}}_z^{lt}·{\mathit{r}}_z^{ls}}{-{\mathit{r}}_z^{lt}·{\mathit{r}}_x^{ls}}=\frac{sin({\alpha }_{lk}^{\prime }-\frac{\pi }{2})}{cos({\alpha }_{lk}^{\prime }-\frac{\pi }{2})}\end{array}$$

(68)

$$\begin{array}{c}[log\left(c_{lkr}^{}\left({\mathbf{X}}_k\right)\right){]}_{}^{\vee }={(\mathbf{E} {\mathit{T}}_{}^{ls}\stackrel{{\mathit{r}ls}_z^{lt}=\mathrm{long}\mathrm{axis}\mathrm{of}\mathrm{left}\mathrm{thigh}\mathrm{in}\mathrm{shank}\mathrm{frame}}{\overbrace{\left({\mathit{i}}_{z}cos({\alpha }_{lk}^{\prime }-\frac{\pi }{2})-{\mathit{i}}_{x}sin({\alpha }_{lk}^{\prime }-\frac{\pi }{2})\right)}})}^T{\tau }_z^{lt}\left({\mathbf{X}}_k\right)\in \mathbb{R},{[log\left({\mathbf{D}}_{lkr}^{}\right)]}_{}^{\vee }=0\end{array}$$

(69)

Following a similar procedure to ${\mathcal{C}}_{lkh,k}$ (i.e., replace ${\mathit{i}}_y^{}$ in Equation (64) with ${\mathit{r}ls}_z^{lt}$) and taking ${\mathbf{X}}_k={\widehat{\mu }}_k^{+}$, ${\mathcal{C}}_{lkr,k}$ can be calculated from $c_{lkr}({\widehat{\mu }}_k^{+}exp\left({\left[ϵ\right]}_G^{\wedge }\right))={[log\left(c_{lkr}^{}\left({\widehat{\mu }}_k^{+}\right)\right)]}_{}^{\vee }+{(\mathbf{E} {\widehat{\mathit{T}}}_{}^{ls+}{\mathit{r}ls}_z^{lt})}^T\mathbf{E}\left({\widehat{\mathit{T}}}_k^{p+}{\left[{\mathit{p}p}_{}^{lh}\right]}_{}^{\odot }{\mathbf{ϵ}}_{\mathbf{T}}^p-{\widehat{\mathit{T}}}_k^{ls+}{\left[{\mathit{p}ls}_{}^{lk}\right]}_{}^{\odot }{\mathbf{ϵ}}_{\mathbf{T}}^{ls}\right)+{\left({\tau }_z^{lt}\left({\widehat{\mu }}_k^{+}\right)\right)}^T\mathbf{E} {\widehat{\mathit{T}}}_{}^{ls+}{\left[{\mathit{r}ls}_z^{lt}\right]}_{}^{\odot }{\mathbf{ϵ}}_{\mathbf{T}}^{ls}$, as shown in Equation (70).

(70)

3.3. Post-Processing

The orientation of the pelvis and shanks are obtained from the state ${\tilde{\mu }}_k^{+}$. The orientation of the left thigh, ${\tilde{\mathit{R}}}_{}^{lt+}$, can be calculated using ${\tilde{\mathit{R}}}_{}^{lt+}$ = = , where ${\tilde{\mathit{r}}}_z^{lt+}={\tau }_z^{lt}\left({\tilde{\mu }}_k^{+}\right)/\left|\right|{\tau }_z^{lt}\left({\tilde{\mu }}_k^{+}\right)\left|\right|$. The orientation of the right thigh, ${\tilde{\mathit{R}}}_{}^{rt+}$, is calculated similarly.

4. Experiment

An extension of the dataset from [15] was used to evaluate our L5S based algorithms. The movements involved are listed in

Table 1. Types of movements done in the validation experiment.

Movement	Description	Duration	Group
Walk	Walk straight and return	∼30 s	F
Figure-of-eight	Walk along figure-of-eight path	∼60 s	F
Zig-zag	Walk along zig-zag path	∼60 s	F
5-minute walk	Unscripted walk and stand	∼300 s	F
Speedskater	Speedskater on the spot	∼30 s	D
TUG	Timed up-and-go test	∼30 s	D
Jog	Jog straight and return	∼30 s	D
Jumping jacks	Jumping jacks on the spot	∼30 s	D
High-knee jog	High-knee jog on the spot	∼30 s	D

F denotes free walk, D denotes dynamic movements.

(note the addition of dynamic movements), and were collected from from nine healthy subjects (7 men and 2 women, weight $63.0\pm 6.8$ kg, height $1.70\pm 0.06$ m, age $24.6\pm 3.9$ years old), with no known gait abnormalities. Raw data were captured using a commercial IMC (i.e., Xsens Awinda) at 100 Hz sampling rate with IMUs attached to the pelvis and ankles, compared against a benchmark OMC (i.e., the setup followed Vicon Plug-in Gait protocol in a ∼$4\times 4$ m${}^2$ capture area). The experiment was approved by the Human Research Ethics Board of the University of New South Wales (UNSW) with approval number HC180413.

Frame alignment and yaw offset calibrations are similar to Section III-B of [15]. The experiments and algorithm were implemented using Matlab 2020a, with initial state ${\tilde{\mu }}_0^{+}$ (i.e., position, orientation, and velocity) obtained from the OMC system (i.e., Vicon) and initial error covariance ${\mathbf{P}}_0^{+}$ set to $0.5{\mathbf{I}}_{27\times 27}$. The variance parameters for the process and measurement error covariance matrix $\mathcal{Q}$ and $\mathcal{R}$ are shown in

Table 2. Parameters for error covariance matrices, $\mathcal{Q}$ and $\mathcal{R}$.

$\mathcal{Q}$ Parameters		$\mathcal{R}$ Parameters
${\mathbf{\sigma }}_a^2$	${\mathbf{\sigma }}_{\omega }^2$	${\mathbf{\sigma }}_{ori}^2$	${\mathbf{\sigma }}_{mp}^2$	${\mathbf{\sigma }}_{ls}^2$ and ${\mathbf{\sigma }}_{rs}^2$	${\mathbf{\sigma }}_{dl}^2$ and ${\mathbf{\sigma }}_{dr}^2$	${\mathbf{\sigma }}_{da}^2$	${\mathbf{\sigma }}_{lim}^2$
(m${}^2$·s${}^{-4}$)	(rad${}^2$·s${}^{-2}$)	(rad${}^2$)	(m${}^2$)	(m${}^2$·s${}^{-2}$ and m${}^2$)	(m${}^2$)	(m${}^2$)	(m${}^2$)
${10}^2{\mathbf{1}}_9$	${10}^3{\mathbf{1}}_9$	$1{\mathbf{1}}_9$	$0.1$	$\left[0.01{\mathbf{1}}_3{10}^{-4}\right]$	10	1	$10{\mathbf{1}}_{18}$

where ${\mathbf{1}}_n$ is an $1\times n$ row vector with all elements equal to 1.

.

The inter-IMU distance measurements, ${\breve{d}}^{pla}$, ${\breve{d}}^{pra}$, and ${\breve{d}}^{lra}$, were simulated by calculating the distance from the mid-pelvis to the left and right ankles and adding normally distributed positional noise with different standard deviations (i.e., ${\sigma }_{dist}\in \{0,0.01,\cdots ,0.1,0.15,0.2\}$ m). Each trial was simulated five times.

Lastly, the evaluation was done using the following metrics: (1) Mean position and orientation root-mean-square error (RMSE) (e.g., similar to [15,17] as shown in Equations (71) and (72)), where ${\mathit{p}}_k^b$ and ${\mathit{R}}_k^b$ are obtained from the benchmark OMC system, ${\tilde{\mathit{p}}}_k^{b+}$ and ${\tilde{\mathit{R}}}_k^{b+}$ are obtained from the algorithm. Note that as the global position of the estimate is still prone to drift due to the absence of an external global position reference, the root position of our system was set equal to that of the benchmark system (i.e., the mid-pelvis is placed at the origin in the world frame for all RMSE calculations). (2) Joint angles RMSE with bias removed (i.e., the mean difference between the angles over each entire trial was subtracted) and correlation coefficient (CC) of the hip in the sagittal (Y), frontal (X), and transverse (Z) planes and of the knee in the sagittal (Y) plane. Note that these joint angles are commonly used parameters in gait analysis. (3) Spatiotemporal gait parameters (e.g., total travelled distance (TTD) deviation, average stride length, and gait speed of the foot). Refer to Section III of [15] for more details.

$$\begin{array}{c}{e}_{pos,k}=\frac{1}{N_{pos}}{\sum }_{b\in \mathbb{DP}}\left|\right|{\mathit{p}}_k^b-{\tilde{\mathit{p}}}_k^{b+}\left|\right|,N_{pos}=6,\mathbb{DP}=\{lh,rh,lk,rk,la,ra\}\end{array}$$

(71)

$$\begin{array}{c}{e}_{ori,k}=\frac{1}{N_{ori}}{\sum }_{b\in \mathbb{DO}}\left|\right|{[log({\mathit{R}}_k^b{\left({\tilde{\mathit{R}}}_k^{b+}\right)}^T)]}_{}^{\vee }\left|\right|,N_{ori}=2,\mathbb{DO}=\{lt,rt\}\end{array}$$

(72)

5. Results

5.1. Mean Position and Orientation RMSE, Joint Angle RMSE and CC

In this experiment, multiple variations of the algorithm were tested as shown in

Table 3. The experiment was tested on the following algorithm variations.

Algorithm	Inter-IMU Distance	Summary Description
L5S-3IMU	N	Tracks position and orientation as described in Section 3 with parameters listed in Table 2.
L5S-3IMU+D	Y
CKF-3IMU [15]	N	Only tracks position using a constrained KF.
CKF-3IMU+D [31]	Y
CKF-3I-KB	N	Modified CKF-3IMU using similar parameters as L5S-3IMU (Table 2). This also allows knee bending during the constraint update.
CKF-3I-KB+D	Y
L5S-3I-NO	N	L5S-3IMU with parameters that assume noise-free orientation (NO) measurements like CKF-3IMU.
L5S-3I-NO+D	Y
OSPS	N	Output from a commercial OSPS wearable IMC system.

. Firstly, L5S-3IMU is the algorithm described in this paper (Section 3) with parameters listed in Table 2. The parameter for L5S-3IMU were selected by taking the best joint CC (i.e., mean of free walk and dynamic movements) from a grid search of parameters ${\mathbf{\sigma }}_{\omega }^2=\{1,10,{10}^2,{10}^3\}$ rad${}^2$/s${}^2$ and ${\mathbf{\sigma }}_{ori}^2=\{{10}^{-2},{10}^{-1},1,10\}$ rad${}^2$. Secondly, CKF-3IMU and CKF-3IMU+D were the algorithms described in [15,31], respectively. Thirdly, CKF-3I-KB is a modified CKF-3IMU using similar parameters, measurement, and constraint functions as L5S-3IMU. The key difference between CKF-3IMU and CKF-3I-KB is that CKF-3I-KB allows knee bending, denoted by the suffix KB, during the constraint update. Fourthly, L5S-3I-NO is a variation of L5S-3IMU with ${\mathbf{\sigma }}_{\omega }^2={10}^7$ rad${}^2$/s${}^2$, ${\mathbf{\sigma }}_{ori}^2={10}^{-1}$ rad${}^2$, and ${\breve{\mathbf{\omega }}b}_k^{}=0$ rad. The parameters were chosen to have high uncertainty on the tracked orientation (i.e., effectively not using the orientation measurements at all), leading to a variation of L5S-3IMU that is similar to our prior work CKF-3IMU which assumed orientation measurements were noise-free. Lastly, the black box output (i.e., pelvis, thigh, and shank orientations) from the MVN Studio software (denoted as OSPS), which illustrates the performance of a widely-accepted commercial wearable IMC system with an OSPS configuration. For the first to fourth variations, the +D suffix means simulated inter-IMU distance measurements (${\sigma }_{dist}=0.1$ m) was used instead of the pelvis height assumption.

Figure 4 shows the mean position and orientation RMSE, mean knee Y and hip joint angle RMSE (bias removed) and CC of different variations of CKF-3IMU and L5S-3IMU for both free walking and dynamic motions. Y, X, and Z refers to the sagittal, frontal, and transverse planes, respectively. CKF-3IMU performed well with free walking ($e_{pos}=4.27$ cm, $e_{ori}=15.{85}^{\circ }$, $CC=0.66$) [15]. However, a more extensive evaluation showed that it performed poorly for certain dynamic movements (e.g., high-knee jog with $e_{pos}=18.15$ cm, $e_{ori}=24.{87}^{\circ }$, $CC=0.02$). Removing the no-knee-bending assumption during the constraint update fixed this issue, as shown by the performance of CKF-3I-KB (e.g., high-knee jog improved by ∼9 cm $e_{pos}$, ∼$9^{\circ }$ $e_{ori}$, ∼$0.4$ CC). L5S-3I-NO which is the L5S version of CKF-3IMU expectedly have similar performance with CKF-3I-KB (i.e., $\Delta e_{pos}<0.5$ cm, $\Delta e_{ori}<1^{\circ }$, and $\Delta$CC $0.02$ differences). L5S-3IMU, which tracked both position and orientation while assuming there is noise in the orientation measurements, had a slightly better performance (e.g., improved jumping jacks and high-knee jog by ∼$0.1$ CC, <0.03 CC difference with other movement types). The use of simulated distance measurement with ${\sigma }_{dist}=0.1$ m on CKF-3I-KB, L5S-3I-NO, and L5S-3IMU had slight effects for free walking, and a significant improvement for dynamic movements. For free walking, joint angle RMSE and CC of L5S-3IMU+D compared to L5S-3IMU improved by ∼$1^{\circ }$ and <0.01 CC, while $e_{pos}$ and $e_{ori}$ slightly disimproved (<0.5 cm and <1 ${}^{\circ }$). The similar results suggest that inferring pelvis position from simulated distance measurement (${\sigma }_{dist}=0.1$ m) is comparable to our pelvis height assumption at least for free walking. For dynamic movements, the $e_{pos}$, $e_{ori}$, joint angle RMSE, and CC of L5S-3IMU+D improved by 2–16 cm, 0–40 ${}^{\circ }$, 1–9 ${}^{\circ }$, and <0.42, respectively—more significantly for movements TUG and high-knee jog.

To give insight on how the accuracy of the simulated inter-IMU distance measurements affect pose estimation performance, Figure 5 shows the mean of knee Y and hip joint angle RMSE and CC at different ${\sigma }_{dist}$ values. At ${\sigma }_{dist}=0.1$ m, the simulation showed comparable performance between L5S-3IMU, which implements pelvis height assumption, and L5S-3IMU+D, which implements inter-IMU distance measurement to supplement the pelvis position estimate, for free walking. Significant improvement for dynamic movements can be seen even for ${\sigma }_{dist}=0.2$ m. These results suggest that the actual distance measurement sensor must have noise standard deviation ${\sigma }_{dist}\le 0.1$ m to improve pose estimate performance. Note that the +D variation in Figure 4 and in the experiments that follow were evaluated at ${\sigma }_{dist}=0.1$ m.

5.2. Hip and Knee Joint Angle RMSE and CC

Figure 6 shows the knee and hip joint angle RMSE (bias removed) and CC of L5S-3IMU and L5S-3IMU+D compared against the OMC output. Y, X, and Z refers to the sagittal, frontal, and transverse planes, respectively. Turning movements and half steps were manually removed from the per-step result of Walk movement and was denoted as Straight Walk. Note that sensor-to-body calibration was only done at the beginning of trial, not for each step. Between L5S-3IMU and L5S-3IMU+D, there was minimal hip and knee joint angle RMSE and CC improvement for free walking (∼$1^{\circ }$ RMSE and ∼$0.03$ CC difference). However, there was significant improvement for most dynamic movements, specifically, speed-skater, jog, high-knee jog, and TUG (e.g., $4^{\circ }$–${17}^{\circ }$ knee Y and hip Y joint angle RMSE improvements). Furthermore, the CC for dynamic movements started to reach similar performance with the free walk movement, indicating that inter-IMU distance measurements have indeed made the pose estimator capable of tracking more ADLs and not just walking.

Figure 7 shows a sample walk trial. At the peaks of knee Y angle, the distance between the pelvis and ankle positions of L5S-3IMU+D were a few cm shorter (i.e., pelvis position was lower than actual while ankle position was higher) than the actual distance resulting in higher knee Y angle peaks. Violations of our biomechanical constraints are also apparent at $t=4$ to $5.5$ s, where the subject makes a ${180}^{\circ }$ turn. After the turn, L5S-3IMU and L5S-3IMU+D were able to recover during the straight walking ($t=5.5$ to $9.74$ s of Figure 7). Notice that the bias between OSPS and OMC can be observed at $t=0$ of the hip Y joint angle.

5.3. Spatiotemporal Gait Parameters

Table 4. Total travelled distance (TTD) deviation from optical motion capture (OMC) system at the ankles.

Algo.	Side	TTD	Stride Length (cm)				Gait Speed (cm.s ${}^{-1}$)
		Error	Actual		Error		Actual		Error
		% dev	$\mathsf{\mu }$	med	$\mathsf{\mu }$ ± $\mathit{\sigma }$	RMS	$\mathsf{\mu }$	med	$\mathsf{\mu }$ ± $\mathit{\sigma }$	RMS
Freewalk	L	8.97%	91	99	$-8.1\pm 6$	9.9	70	74	$-6.0\pm 5$	7.7
L5S-3IMU	R	9.00%	93	99	$-8.3\pm 6$	10.3	71	75	$-6.2\pm 5$	8.2
Freewalk	L	5.23%	91	99	$-4.7\pm 7$	8.3	70	74	$-3.6\pm 6$	6.6
L5S-3IMU+D	R	5.85%	93	99	$-5.4\pm 8$	9.4	71	75	$-4.1\pm 6$	7.4
Jog	L	21.35%	81	86	$-17.4\pm 23$	28.5	107	118	$-19.2\pm 33$	38.0
L5S-3IMU	R	26.79%	85	97	$-22.9\pm 25$	33.8	111	124	$-26.4\pm 34$	43.1
Jog	L	22.40%	81	86	$-18.2\pm 22$	28.4	107	118	$-21.6\pm 30$	37.0
L5S-3IMU+D	R	26.70%	85	97	$-22.8\pm 24$	32.8	111	124	$-27.5\pm 31$	41.4
TUG	L	18.20%	74	76	$-13.5\pm 18$	22.1	58	60	$-10.0\pm 15$	18.0
L5S-3IMU	R	20.98%	79	90	$-16.6\pm 15$	22.5	63	67	$-13.1\pm 13$	18.4
ine TUG	L	3.80%	74	76	$-2.8\pm 6$	6.7	58	60	$-2.3\pm 5$	5.9
L5S-3IMU+D	R	4.22%	79	90	$-3.3\pm 6$	6.8	63	67	$-2.7\pm 5$	5.6

where $\mathsf{\mu }$ and $\sigma$ denote mean and standard deviation. Error denotes estimate minus actual value, while TTD % dev denotes $\mathrm{abs}\left(error\right)/\mathrm{actual}\mathrm{TTD}$.

shows the TTD, stride length, and gait speed accuracy computed from the global ankle position estimate of L5S-3IMU, L5S-3IMU+D, and the OMC system for free walk, jogging, and TUG. The use of inter-IMU distance measurements (${\sigma }_{dist}=0.1$ m) helped improve the TTD, stride length, and gait speed accuracy of free walk and TUG (e.g., TTD improved from ∼$9\%$ to ∼$5\%$). Refer to the code repository for links to videos of sample trials.

6. Discussion

In this paper, a Lie group EKF algorithm for lower body pose estimation using only three IMUs, ergonomically placed on the ankles and sacrum to facilitate continuous recording outside the laboratory, was described and evaluated. The algorithm utilizes fewer sensors than other approaches reported in the literature, at the cost of reduced accuracy.

6.1. Mean Position and Orientation RMSE

The mean position and orientation RMSE of L5S-3IMU, L5S-3IMU+D, and related literature (sparse orientation poser (SOP) and sparse inertial poser (SIP) [17]) are listed in

Table 5. Mean position and orientation RMSE of L5S-3IMU, L5S-3IMU+D, OSPS, sparse orientation power (SOP) and sparse inertial poser (SIP) [17].

	${\mathit{e}}_{\mathbf{pos}}$ (cm)			${\mathit{e}}_{\mathbf{ori}}$, No Bias (cm)
	Free Walk	Jog	Jumping Jacks	Free Walk	Jog	Jumping Jacks
L5S-3I	$5.1\pm 1.2$	$7.3\pm 1.4$	$8.7\pm 1.6$	$17.5\pm 2.7^{\circ }$	$20.2\pm 3.8^{\circ }$	$12.8\pm 4.0^{\circ }$
L5S-3I+D	$5.5\pm 1.0$	$6.2\pm 1.1$	$4.9\pm 0.9$	$18.0\pm 2.5^{\circ }$	$17.4\pm 3.2^{\circ }$	$12.6\pm 3.2^{\circ }$
OSPS	$5.4\pm 1.5$	$5.6\pm 1.2$	$5.5\pm 1.6$	$12.9\pm 4.0^{\circ }$	$10.3\pm 1.8^{\circ }$	$7.6\pm 3.3^{\circ }$
SOP [17]	∼$5.0$	∼$8.0$	∼$8.0$	∼$15.0^{\circ }$	∼$22.0^{\circ }$	∼$20.0^{\circ }$
SIP [17]	∼$3.0$	∼$5.0$	∼$4.0$	∼$11.0^{\circ }$	∼$16.0^{\circ }$	∼$16.0^{\circ }$

. SOP used orientation measured by IMUs and biomechanical constraints, while SIP used similar information but with the addition of acceleration. Both SOP and SIP were benchmarked against an OSPS system tracking the full body while our algorithm was benchmarked against an OMC system tracking only the lower body. The $e_{pos}$ and $e_{ori}$ (no bias) performance of L5S-3IMU and compared to SOP for free walking and jogging were comparable ($\Delta e_{pos}$ 0.1–0.5 cm and $\Delta e_{ori}$ 2.5${}^{\circ }$–3${}^{\circ }$ differences). The $e_{pos}$ and $e_{ori}$ (no bias) of SIP was better than L5S-3IMU and L5S-3IMU+D for free walking (∼2.1–2.5 cm and 6.5${}^{\circ }$–7${}^{\circ }$ difference). Although this improvement was expected, as SIP optimizes the pose over multiple frames whereas our algorithm, like CKF-3IMU, optimizes the pose for each individual frame. For jumping jacks, the $e_{ori}$ of L5S-3IMU and L5S-3IMU+D was significantly (∼4${}^{\circ }$–8${}^{\circ }$) better than SOP’s and SIP’s. However, this difference is probably because both SOP and SIP were evaluated on the full body (our algorithm was only evaluated on the lower body) and errors in arm pose estimation may have increased $e_{ori}$ for the SOP and SIP algorithms.

Comparing processing times, L5S-3IMU and L5S-3IMU+D were slower than CKF-3IMU, but can still be used in real-time; specifically, CKF-3IMU, L5S-3IMU, and L5S-3IMU+D processed a 1000-frame sequence (i.e., 10 s long) in ∼$0.7$, ∼2, ∼$3.5$ s, respectively, on an Intel Core i5-6500 3.2 GHz CPU [15], while SIP [17] took 7.5 min on a quad-core Intel Core i7 3.5 GHz CPU. All set-ups used single-core non-optimized Matlab code. Albeit slower than CKF-3IMU, L5S-3IMU and L5S-3IMU+D could also be used to provide real-time gait parameter measurement to inform actuation of assistive or rehabilitation robotic devices.

6.2. Hip and Knee Joint Angle RMSE and CC

The knee and hip joint angle RMSEs (no bias) and CCs of L5S-3IMU, L5S-3IMU+D, OSPS and related literature for straight walking (i.e., per step evaluation) are shown in

Table 6. Knee and hip angle RMSE no bias (top) and CC (bottom) of CKF-3IMU, OSPS, and related literature for free walk.

Joint Angle RMSE (${}^{\circ }$)	Knee Sagittal	Hip Sagittal	Hip Frontal	Hip Transverse
L5S-3IMU	$7.6\pm 2.6$	$6.6\pm 2.7$	$5.0\pm 2.6$	$8.6\pm 3.6$
L5S-3IMU+D	$7.1\pm 2.1$	$7.5\pm 2.1$	$5.1\pm 2.3$	$8.9\pm 3.7$
OSPS	$5.0\pm 1.8$	$3.6\pm 1.7$	$4.1\pm 2.2$	$11.9\pm 4.3$
CKF-3IMU [15]	$5.7\pm 2.2$	$4.4\pm 1.9$	$5.5\pm 2.6$	$9.0\pm 3.8$
Cloete et al. [37]	$8.5\pm 5.0$	$5.8\pm 3.8$	$7.3\pm 5.2$	$7.9\pm 4.9$
Hu et al. [38]	$4.9\pm 3.5$	$6.8\pm 3.0$	-	-
Tadano et al. [7]	$10.1\pm 1.0$	$7.9\pm 1.0$	-	-
Joint Angle CC	Knee Sagittal	Hip Sagittal	Hip Frontal	Hip Transverse
L5S-3IMU	$0.95\pm 0.03$	$0.87\pm 0.16$	$0.76\pm 0.18$	$0.36\pm 0.36$
L5S-3IMU+D	$0.95\pm 0.03$	$0.83\pm 0.14$	$0.72\pm 0.19$	$0.29\pm 0.37$
OSPS	$0.97\pm 0.04$	$0.95\pm 0.06$	$0.72\pm 0.19$	$0.26\pm 0.20$
CKF-3IMU [15]	$0.98\pm 0.03$	$0.96\pm 0.08$	$0.73\pm 0.17$	$0.26\pm 0.39$
Cloete et al. [37]	$0.89\pm 0.15$	$0.94\pm 0.08$	$0.55\pm 0.40$	$0.54\pm 0.20$
Hu et al. [38]	$0.95\pm 0.04$	$0.97\pm 0.04$	-	-
Tadano et al. [7]	$0.97\pm 0.02$	$0.98\pm 0.01$	-	-

[7,15,37,38]. Similar to IMC based systems, L5S-3IMU and L5S-3IMU+D also follows the trend of having sagittal (Y axis) joint angles similar to that captured by OMC systems ($0.95$ knee Y and >0.83 hip Y CCs), but with significant difference in frontal and transverse (X and Z axis) joint angles [15,37]. CKF-3IMU performed slightly better (e.g., $0.03$ knee Y, $0.09$ hip Y CC), which is expected as the biomechanical constraint (i.e., no-knee-bending) assumption of CKF-3IMU was designed specifically for walking, at the cost of being less accurate for other dynamic movements. Both L5S-3IMU and L5S-3IMU+D were comparable, and at times even better (within $2.5^{\circ }$ RMSE, $0.1$ CC difference) than the results of Hu et al. and Tadano et al., indicating excellent per-step reconstruction in the sagittal plane [7,38]. Hu et al. used 4 IMUs (two at the pelvis and one on each foot) and Tadano et al. used an OSPS configuration. Both systems can only estimate the pose in the sagittal plane.

Despite the promising performance when using inter-IMU distance measurements, further validation with actual hardware implementation is needed, as the sensor noise in the real world may not necessarily follow a normal distribution and may be non-stationary.

For reference, portable ultrasound-based distance measurement can achieve millimetre accuracy with a sampling rate of 125 Hz [30], while a commercial UWB-based distance measurement devices can achieve ∼10 cm accuracy with a sampling rate of 200 Hz [39,40].

Lastly, despite L5S-3IMU and L5S-3IMU+D achieving $0.95$ joint angle CCs in the sagittal plane, the unbiased joint angle RMSE (>5${}^{\circ }$) makes its utility in clinical applications uncertain [41]. Although the algorithm is expected to work on pathological gait where our biomechanical assumptions are satisfied, overall performance still needs more improvement. To achieve clinical utility, one may either use more accurate sensors or average out cycle-to-cycle variation in estimation errors over many gait cycles; for example, use a more accurate distance measurement sensor (${\sigma }_{dist}<0.1$ m). Furthermore, the accuracy must also be validated on a larger and more diverse cohort to quantify its ultimate clinical utility. The evaluation of how these solutions can bridge the gap to clinical application for the proposed system will be part of future work.

6.3. Spatiotemporal Gait Parameters

The focus of the proposed algorithms, L5S-3IMU and L5S-3IMU+D, are to estimate joint kinematics. However, as L5S-3IMU and L5S-3IMU+D both track the global position of the ankles, it is also capable of calculating spatiotemporal gait parameters (performance listed in Table 4). The TTD deviation of our algorithms compared against the gold standard OMC were not as good as CKF-3IMU [15] ($3.6$–$3.81$% TTD deviation) or other state-of-the-art dead reckoning algorithms [42,43] ($0.2$–$1.5$% TTD deviation). Two possible sources of inaccuracy lies (1) in the dead reckoning approximation done in the prediction step, and (2) in the assumption that the velocity of the shank IMU is zero when the associated foot touches the floor, but of course this IMU continues to move with some small velocity on the lower shank during the stance phase. To illustrate the dead reckoning approximation, let us look at the predicted pelvis pose in Equation (73). In our algorithm, we assumed ${\mathbf{\psi }}_p\approx {\mathbf{I}}_{3\times 3}$ (note that ${\mathsf{\Phi }}_{}(-\Delta t{\breve{\mathbf{\omega }}p}_k^{})\approx {\mathbf{I}}_{3\times 3}$ and ${\tilde{\mathit{R}}}_{k-1}^{p+}{\left({\breve{\mathit{R}}}_k^p\right)}^T\approx {\mathbf{I}}_{3\times 3}$ since $\Delta t{\breve{\mathbf{\omega }}p}_k^{}$ is small) which did not significantly affect the joint kinematic estimate, but slightly affected the global position estimate. Nevertheless, body drift has been reduced substantially compared to Marcard et al.’s SIP [17].

$$\begin{array}{c}\begin{array}{cc}\hfill {\widehat{\mathit{T}}}_k^{p-}& ={\tilde{\mathit{T}}}_{k-1}^{p+}exp({[\left[\begin{array}{c}{\left({\breve{\mathit{R}}}_k^p\right)}^T(\Delta t{\tilde{\mathit{v}}}_{k-1}^{mp+}+\frac{\Delta t^2}{2}{\breve{\mathit{a}}}_k^p)\\ \Delta t{\breve{\mathbf{\omega }}p}_k^{}\end{array}\right]]}_{}^{\wedge })\hfill \\ & =\left[\begin{array}{cc}{\tilde{\mathit{R}}}_{k-1}^{p+}exp\left({[\Delta t{\breve{\mathbf{\omega }}p}_k^{}]}_{}^{\wedge }\right)& {\tilde{\mathit{p}}}_{k-1}^{mp+}+\stackrel{{\mathbf{\psi }}_p\approx {\mathbf{I}}_{3\times 3}}{\overbrace{\begin{array}{c}{\tilde{\mathit{R}}}_{k-1}^{p+}{\mathsf{\Phi }}_{}(-\Delta t{\breve{\mathbf{\omega }}p}_k^{}){\left({\breve{\mathit{R}}}_k^p\right)}^T\end{array}}}{\tilde{\mathit{R}}}_{k-1}^{p+}{\mathsf{\Phi }}_{}(-\Delta t{\breve{\mathbf{\omega }}p}_k^{}){\left({\breve{\mathit{R}}}_k^p\right)}^T(\Delta t{\tilde{\mathit{v}}}_{k-1}^{mp+}+\frac{\Delta t^2}{2}{\breve{\mathit{a}}}_k^p)\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\hfill \end{array}\end{array}$$

(73)

6.4. Limitations and Future Work

L5S has similar pelvis drift, covariance matrix numerical issue, and flat floor limitation as CKF-3IMU, which is expected as L5S implements the same measurement and constraint update as CKF-3IMU, albeit formulated using Lie group representation instead of vectors and quaternions [15]. The pelvis height and flat floor assumption helps prevent the pelvis and the ankles from drifting towards each other (i.e., pelvis drift downward while ankles drift upward). However, it will also prevent accurate pose estimation of motions such as sitting, lying down, or standing on one leg, where the pose is maintained for a duration much longer than that of a typical gait cycle. The covariance limiter (Section 3.2.2) helps prevent the covariance becoming badly conditioned (i.e., singular), especially for longer duration trials (e.g., 5-minute walk) where the position uncertainty grows at a faster rate for the pelvis position than the ankle position. As can be observed from Figure 6, substituting the pelvis height assumption with inter-IMU distance measurements can increase the algorithm’s accuracy especially for tracking dynamic movements. If the distance measurement is accurate enough (i.e., smaller ${\mathbf{\sigma }}_{dist}^2$), the inter-IMU distance measurement update may be enough to limit the growth of pelvis position uncertainly and possibly making the covariance limiter not needed.

Figure 6 shows that the optimized performance of L5S-3IMU, even if it allows the tracked orientation to be corrected by inter-IMU distance measurements and the tracked position estimate, was only slightly better than CKF-3IMU/L5S-3I-NO, which effectively assumed the measurement input from the orientation estimation algorithm to be perfect (i.e., trusted the tracked orientation less). As L5S-3IMU requires more computing resources, such result suggests that CKF-3IMU may be more suitable to use when computing power is limited. To fully leverage the advantages brought by the Lie group representation, additional sensor measurements that can help correct tracked orientation will be needed (e.g., estimating angle of arrival between two sensors [44] or using fish eye cameras to improve pose estimate [45]).

Additional sensor measurements provide new opportunities for automatic calibration even under RSC configuration. IMC systems typically need anthropometric measurements (i.e., measurement of body segments such as $d^{ls}$) beforehand. By taking the initial distance measurement at some predetermined posture, anthropometric measurements can be automatically inferred. The formulation for a hinge joint with two IMUs on both sides has been leveraged to enable automatic sensor-to-segment calibration (i.e., align sensor frame to body frame) and even a completely magnetometer free orientation estimation [46,47]. Magnetometer free orientation estimation rids us of the yaw offset issue from an inhomogeneous magnetic field in indoor environments, typically with stronger disturbances closer to the floor [48]. An approach using a hinge joint with two IMUs may not be applicable to RSC configurations (e.g., our algorithm only has one IMU on one side of the hinge joint). However, distance measurements may be use to compensate for the missing IMU information from the uninstrumented segment, and a modified version may be developed for a RSC configuration.

Enabling longer-term tracking of ADL in the subject’s natural environment may lead to novel investigations of movement disorder progression and the identification of early intervention opportunities. This work is just one of the early steps towards seamless remote gait monitoring. Developing solutions to further increase accuracy, increase the number of body segments tracked (e.g., track full body under RSC [17]), or use even fewer IMUs (tracking lower body using two IMUs [49]) will be investigated in the future.

7. Conclusions

This paper presented a Lie group CEKF-based algorithm (L5S-3IMU) to estimate lower limb kinematics using a RSC configuration of IMUs, supplemented by inter-IMU distance measurements in one implementation. The knee and hip joint angle RMSEs in the sagittal plane for straight walking were $7.6^{\circ }\pm 2.6^{\circ }$ and $6.6^{\circ }\pm 2.7^{\circ }$, respectively, while the CCs were $0.95\pm 0.03$ and $0.87\pm 0.16$, respectively. We also showed that inter-IMU distance measurement is a promising new source of information to improve the pose estimation of IMC under a RSC configuration. Simulations show that performance improved dramatically for dynamic movements even at higher noise levels (e.g., ${\sigma }_{dist}=0.2$ m), and that similar performance to L5S-3IMU was achieved at ${\sigma }_{dist}=0.1$ m for free walk movements. However, further validation is recommended with actual distance measurement from real sensors. The source code for the L5S algorithm, and links to sample videos will be made available at https://git.io/JTRQ3.