Whole-Body Human Inverse Dynamics with Distributed Micro-Accelerometers, Gyros and Force Sensing ^†

Abstract

Human motion tracking is a powerful tool used in a large range of applications that require human movement analysis. Although it is a well-established technique, its main limitation is the lack of estimation of real-time kinetics information such as forces and torques during the motion capture. In this paper, we present a novel approach for a human soft wearable force tracking for the simultaneous estimation of whole-body forces along with the motion. The early stage of our framework encompasses traditional passive marker based methods, inertial and contact force sensor modalities and harnesses a probabilistic computational technique for estimating dynamic quantities, originally proposed in the domain of humanoid robot control. We present experimental analysis on subjects performing a two degrees-of-freedom bowing task, and we estimate the motion and kinetics quantities. The results demonstrate the validity of the proposed method. We discuss the possible use of this technique in the design of a novel soft wearable force tracking device and its potential applications.

1. Introduction

Human whole-body motion tracking is nowadays a well-established tool in the analysis of human movements. This tool has found a wide variety of real-world applications ranging from entertainment (movies and games) to sport and rehabilitation frameworks. Several commercial solutions for whole-body motion tracking are available. Well known examples include: a wearable marker-less technology suitable for outdoor motion capturing produced by Xsens [1] (Xsens Technologies B.V., Enschede, Netherlands), a state-of-the-art marker-based technology for in-lab applications produced by Vicon (Vicon Motion Systems Ltd, Oxford, UK) and Microsoft Kinect depth camera system (Microsoft Corporation, Redmond, Washington), which allows marker-less low-cost whole-body motion tracking for indoor applications [2]. In biomechanics, soft wearable and stretchable systems have been proposed for measuring human body motion [3,4], as well as in health activity monitoring [5]. Combinations of different technologies have also been used for detecting human motion: in [6], a video-based motion technique was adopted for capturing realistic human motion from video sequences. Although existing technologies provide a high level of accuracy in computing motion quantities, they have several limitations in their ability to measure kinetic quantities in real-time (kinetics considers forces that cause movements). A key problem lies in the fact that motion capture methods typically employ only kinematic measurement modalities (position, velocities and accelerations) [7] and do not include information on the kinetics of human movements.

The importance of including and exploiting all dynamic information is a crucial point in several research areas such as ergonomics for industrial scenarios, developing prosthetic devices and exoskeleton systems in rehabilitation fields, or in human-robot interaction. For these reasons, whole-body force tracking is not a new challenge for the scientific community, but the topic has been seldom explored in situ due to the computational difficulties of the analysis and even more rarely analyzed in real-time modality. Although several recent studies are going in this direction, it is limited to prototypical and non-wearable technologies. Typical non-wearable technologies involve the combination of a motion capture system (e.g., a Vicon camera system ), with commercial force plates and Newtonian physics to perform inverse dynamics computations . Some recent prototypes [8] have been proposed for kinematics and dynamics motion capture. The suggested approach involves a Kinect-like sensing and pressure-sensing shoes to reconstruct the whole-body dynamics.

To obtain forces and torques acting on a body, accurate data on the mass, center of mass and inertias of each body segment are needed. Different methods for the determination of body segment parameters can be described in [9], but the real difficulty is that a universal acknowledged procedure for estimating inertial parameters with accuracy does not exist yet. In this framework, it is fundamental to specify exactly what the kinematics and external boundaries are.

Towards compensating for aforementioned drawbacks, one approach is to supplement or even replace standard marker-based technologies in several applications with a system design embedding a combination of Inertial Measurement Unit (IMU) sensors and force sensing for evaluating contact forces. However, in order to properly model and understand the role of dynamic quantities, an appropriate understanding of the dynamic interaction between the elements of the models (such as reaction and contact forces, accelerations of links and forces exchanged between them) is needed.

Inspired by a recent research study on sensor fusion for whole-body estimation on the humanoid iCub robot [10] (Istituto Italiano di Tecnologia, Genova, Italy), in this paper, we propose a novel framework for wearable dynamics (WearDY) aiming to bridge the gap in dynamics analysis by fusing motion and force capture. The novelty of the approach consists of framing computations in a probabilistic Gaussian framework in the presence of redundant (and noisy) measurements. In this way, sensors play an active role in the computation since the classical boundary condition in recursive Newton–Euler algorithms (i.e., linear-angular velocities and acceleration at the base link and forces–torques at the end-effector) are replaced with measurements coming from sensors. WearDY is an attempt at combining dynamic computations with stochastic estimation of dynamics variables. The early stage of WearDY tool encompass different modules: (1) a motion capture system computing human joint model configuration and kinematics; (2) an additional inertial sensor; (3) force platform sensing; and (4) a probabilistic algorithm framework. The final prototype will replace the motion capture system with a soft wearable suit embedding sensing structure.

In this paper, we focus mainly on describing the fourth module and its integration into the physical tool. We test the WearDY prototype on human subjects performing a two Degrees-of-Freedom (DOF) bowing task. The subjects are equipped with the basic elements of the proposed suit in the form of a chest-mounted IMU, a conventional motion capture system along with a force plate. The Gaussian algorithm is then applied to compute joint torques as well as other dynamic quantities such as link accelerations and transmission forces throughout the motion. The results demonstrate the applicability of the proposed method for the simultaneous force and motion tracking in dynamic motion.

The paper is structured as follows. Section 2 presents an overview of some important definitions of the algebraic notation used for computations along with the assumptions on the dynamic model. Section 3 provides an analytic statement of dynamic analysis wherein the probabilistic computation is framed. In Section 4, the probabilistic estimation theory is shown. The experimental analysis on human subjects is presented in Section 4 followed by concluding remarks in Section 4.

2. Notation

2.1. Spatial Algebra Description

The notation adopted in WearDY architecture mirrors that in [11] (readers already familiar with the notation can jump to Section III): all variables are $spatial$ vectors (six dimensional vectors including angular quantities in the first three components and the rest as linear quantities). Within this notation, an articulated rigid body is a system modeled as an oriented kinematic tree (Figure 1) with $N_B$ moving links numbering from 1 to $N_B$ (0 is the fixed base). Each link in the model is associated with a unique node in the tree. Node numbers can be always selected in a topological order so that each node i has a higher number than its unique parent $\lambda \left(i\right)$ and a smaller number than all the nodes in the set of its children $\mu \left(i\right)$. Links i and its parent $\lambda \left(i\right)$ are coupled with joint i according to Denavit–Hartenberg convention for joint numbering [12]. Joint i motion freedom subspace is modeled with ${\mathit{S}}_i\in {\mathbb{R}}^{6\times n_i}$, being $n_i$ the number of DOF of the joint i and $n=n_1+\dots +n_{N_B}$ the total number of DOFs of the system excluding the fixed base. For each link i and joint i, the following spatial quantities are defined:

${\mathit{v}}_i$

velocity of link i;

${\mathit{a}}_i$

acceleration of link i;

${\mathit{f}}_i$

force transmitted from $\lambda \left(i\right)$ to body i;

${\mathit{f}}_{\mathit{i}}^B$

net force on link i;

${\mathit{f}}_{\mathit{i}}^x$

external force acting on link i;

${\mathit{q}}_i$

joint i position;

$\dot{{\mathit{q}}_i}$

joint i velocity;

$\ddot{{\mathit{q}}_i}$

joint i acceleration;

${\mathit{\tau }}_i$

joint i torque;

${\mathit{v}}_{Ji}$

velocity across joint i.

All variables are expressed in body i coordinates, except for ${\mathit{f}}_{\mathit{i}}^x$ which is convenient to express in absolute (i.e., body 0) coordinates. To each link i, a spatial inertia tensor is also associated:

$${\mathit{I}}_i=\left[\begin{array}{cc}{\mathit{I}}_{C,i}+m_i{\mathit{c}}_i\times {\mathit{c}}_i^{\top }& m_i{\mathit{c}}_i\times \\ m_i{\mathit{c}}_i{\times }^{\top }& m_i{\mathit{I}}_{3\times 3}\end{array}\right],$$

where ${\mathit{I}}_{C,i}$ is the spatial inertia tensor with respect to (w.r.t.) the link center of mass, $m_i$ is the total mass, ${\mathit{c}}_i$ is the relative displacement between the center of mass and the origin of the reference frame associated to the link. Within this spatial framework, in the paper, the following operations are adopted: × is the motion cross product operator such as, if

$$\mathit{r}=\left[\begin{array}{c}{r}_x\\ r_y\\ r_z\end{array}\right]\Rightarrow \mathit{r}\times =\left[\begin{array}{c}{r}_x\\ r_y\\ r_z\end{array}\right]\times =\left[\begin{array}{ccc}0& -r_z& r_y\\ r_z& 0& -r_x\\ -r_y& r_x& 0\end{array}\right].$$

Its dual operator ${\times }^{*}$ is the force cross product operator. The motion vector transformation ${}^B{\mathit{X}}_A$ from A to B coordinates and its analogous transformation for a force vector ${}^B{\mathit{X}}_A^{*}$ are, respectively:

$${}^B{\mathit{X}}_A=\left[\begin{array}{cc}{}^B{\mathit{R}}_A& {\mathbf{0}}_{3\times 3}\\ {-}^B{\mathit{R}}_A{(}^A\mathit{r}\times )& {}^B{\mathit{R}}_A\end{array}\right],{}^B{\mathit{X}}_A^{*}=\left[\begin{array}{cc}{}^B{\mathit{R}}_A& {-}^B{\mathit{R}}_A{(}^A\mathit{r}\times )\\ {\mathbf{0}}_{3\times 3}& {}^B{\mathit{R}}_A\end{array}\right].$$

A homogeneous transformation matrix from A to B is also described as follows:

$${}^B{\mathit{T}}_A=\left[\begin{array}{cc}{}^B{\mathit{R}}_A& {-}^B{\mathit{R}}_A{(}^A\mathit{r})\\ 0& 1\end{array}\right].$$

Throughout the paper, we denoted with ${}^A\dot{\mathit{x}}$ the temporal first order derivative and with ${}^A\ddot{\mathit{x}}$ the temporal second order derivative of a generic vector x in A coordinates.

2.2. Stochastic Notation

Given a stochastic variable x, we denote with $p\left(\mathit{x}\right)$ its probability density and with $p\left(\mathit{x}\right|\mathit{y})$ the conditional probability density of x given the assumption that another stochastic variable y has occurred. Since y is associated to a deterministic function $f\left(\mathit{x}\right)$, with $E_{\mathit{x}}[f\left(\mathit{x}\right)]$ we denote the expected value of $f\left(\mathit{x}\right)$ w.r.t. the probability distribution $p\left(\mathit{x}\right)$. With ${\mathit{\mu }}_{\mathit{x}}$, ${\mathbf{\Sigma }}_{\mathit{x}}$, we denote the mean and covariance of x, i.e., ${\mathbf{\mu }}_{\mathit{x}}=E[\mathit{x}]$ and ${\mathbf{\Sigma }}_{\mathit{x}}=E[\mathit{x}{\mathit{x}}^{\top }]$. The probability density function of a multivariate Gaussian distribution $\mathit{x}\in {\mathbb{R}}^n$ is $p\left(\mathit{x}\right)\sim \mathcal{N}({\mathbf{\mu }}_{\mathit{x}},{\mathbf{\Sigma }}_{\mathit{x}})$:

$$p\left(\mathit{x}\right)={\left(2\pi \right)}^{-\frac{n}{2}}{\left|{\mathbf{\Sigma }}_{\mathit{x}}\right|}^{-\frac{1}{2}}exp\left\{-\frac{1}{2}{\left(\mathit{x}-{\mathbf{\mu }}_{\mathit{x}}\right)}^{\top }{\mathbf{\Sigma }}_{\mathit{x}}^{-1}\left(\mathit{x}-{\mathbf{\mu }}_{\mathit{x}}\right)\right\},$$

where $\left|{\mathbf{\Sigma }}_{\mathit{x}}\right|$ denotes the determinant of the matrix ${\mathbf{\Sigma }}_{\mathit{x}}\in {\mathbb{R}}^{n\times n}$. It is worth to notice that when in a multivariate normal distribution the covariance $\mathbf{\Sigma }$ is not a full-rank matrix, then the distribution is degenerate and does not have a density. In order to avoid the problem, it can be useful restrict the problem on a subset of $\mathbf{\Sigma }$ such that the covariance matrix for this subset is positive definite.

3. Problem Statement and Formulation

The dynamic estimation algorithm has been originally developed in [10] as a framework for the probabilistic estimation of whole-body robot dynamics with redundant measurements. The methodology was here adapted to fit the needs of the human motion. The present section discusses the estimation problem in details. After discussing the recursive Newton–Euler algorithm for inverse dynamics computation in Section 3.1, we arrange the resulting equations in a matrix formulation (Section 3.2). Section 3.3 introduces the estimation problem by discussing the case in which the boundary conditions of the Newton–Euler algorithm are replaced with a set of redundant measurements expressed in a new equation form.

3.1. Recursive Newton–Euler Algorithm

In [11], the inverse dynamics problem is formulated as the problem of finding the forces required to produce a given acceleration. It can be summarized by the following function:

$$\begin{array}{c}\hfill \mathbf{\tau }=\mathrm{InvD}(model,\mathit{q},\dot{\mathit{q}},\ddot{\mathit{q}},{\mathit{f}}^x)\end{array}$$

In biomechanics literature, different inverse dynamics approach are used [13]. Let us assume use of a "top-down" approach. We will assume that all quantities depending on q and $\dot{\mathit{q}}$ have been precomputed, including the transformation matrices ${}^j{\mathit{X}}_i$, ${}^j{\mathit{X}}_i^{*}$ and the velocities ${\mathit{v}}_{Ji}$, ${\mathit{v}}_i$ which can be efficiently computed with the following recursive equation:

$$\begin{array}{ccc}\hfill {\mathit{v}}_{Ji}& =& {\mathit{S}}_i\dot{{\mathit{q}}_i}\hfill \end{array}$$

(1a)

$$\begin{array}{ccc}\hfill {\mathit{v}}_i& =& {}^i{\mathit{X}}_{\lambda \left(i\right)}{\mathit{v}}_{\lambda \left(i\right)}+{\mathit{v}}_{Ji.}\hfill \end{array}$$

(1b)

A classical efficient numerical solution of inverse dynamics problem is given by the recursive Newton–Euler algorithm (RNEA) consisting of the following steps, expressed in body i coordinates:

$$\begin{array}{ccc}\hfill {\mathit{a}}_i& =& {}^i{\mathit{X}}_{\lambda \left(i\right)}{\mathit{a}}_{\lambda \left(i\right)}+{\mathit{S}}_i{\ddot{\mathit{q}}}_i+{\mathit{v}}_i\times {\mathit{v}}_{Ji}\hfill \end{array}$$

(2a)

$$\begin{array}{ccc}\hfill {\mathit{f}}_i^B& =& {\mathit{I}}_i{\mathit{a}}_i+{\mathit{v}}_i{\times }^{*}{\mathit{I}}_i{\mathit{v}}_i\hfill \end{array}$$

(2b)

$$\begin{array}{ccc}\hfill {\mathit{f}}_i& =& {\mathit{f}}_i^B-{}^i{\mathit{X}}_0^{*}{\mathit{f}}_i^x+\sum _{j\in \mu \left(i\right)}{}^i{\mathit{X}}_j^{*}{\mathit{f}}_j\hfill \end{array}$$

(2c)

$$\begin{array}{ccc}\hfill {\mathbf{\tau }}_i& =& {\mathit{S}}_i^{\top }{\mathit{f}}_i\hfill \end{array}$$

(2d)

Equations (1a), (1b) and (2a) are propagated from $i=1$ to $N_B$ with initial conditions ${\mathit{v}}_0=\mathbf{0}$ and ${\mathit{a}}_0=-{\mathit{a}}_g$, which corresponds to the gravitational spatial acceleration vector expressed in the body frame 0 (null in its first three components and equal to the gravitational acceleration in the last three). Equations (2b)–(2d) are propagated from $i=N_B$ to 1.

3.2. RNEA Matrix Formulation and the Measurements Equation

In this section, a matrix arrangement of the RNEA is presented. Equation (2) can be seen as a set of equations which the below listed dynamic variables have to satisfy. Let us first define a spatial vector d of dynamic variables as follows:

$$\begin{array}{c}\hfill \mathit{d}={\left[\begin{array}{cccc}{\mathit{d}}_1^{\top }& {\mathit{d}}_2^{\top }& \cdots & {\mathit{d}}_{N_B}^{\top }\end{array}\right]}^{\top }\in {\mathbb{R}}^{24N_B+2n}\end{array}$$

(3a)

$$\begin{array}{c}\hfill {\mathit{d}}_i={\left[\begin{array}{cccccc}{\mathit{a}}_i^{\top }& {{\mathit{f}}_i^B}^{\top }& {\mathit{f}}_i^{\top }& {\mathbf{\tau }}_i& {{\mathit{f}}_i^x}^{\top }& {\ddot{q}}_i\end{array}\right]}^{\top }\in {\mathbb{R}}^{24+2n_i}\end{array}$$

(3b)

Given Equation (3), Equation (2) can be compactly written in the following matrix equation:

$$\mathit{D}(\mathit{q},\dot{\mathit{q}})\mathit{d}+{\mathit{b}}_D(\mathit{q},\dot{\mathit{q}})=\mathbf{0}$$

(4)

where D is a block matrix $\in {\mathbb{R}}^{(18N_B+n)\times d}$ and ${\mathit{b}}_D$ is a vector $\in {\mathbb{R}}^{18N_B+n}$. Let us define how to build D matrix and $b_D$ vector:

$$\begin{array}{c}\hfill \mathit{D}=\left[\begin{array}{ccc}{\mathit{D}}_{1,1}& \cdots & {\mathit{D}}_{1,N_B}\\ ⋮& \ddots & ⋮\\ {\mathit{D}}_{N_B,1}& \cdots & {\mathit{D}}_{N_B,N_B}\end{array}\right],{\mathit{b}}_D=\left[\begin{array}{c}{\mathit{b}}_1\\ ⋮\\ {\mathit{b}}_{N_B}\end{array}\right].\end{array}$$

In particular:

$$\begin{array}{c}{\mathit{D}}_{i,i}=\left[\begin{array}{cccccc}-\mathbf{1}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& S_i\\ {\mathit{I}}_i& -\mathbf{1}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{1}& -\mathbf{1}& \mathbf{0}& -{}^i{\mathit{X}}_0^{*}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& {\mathit{S}}_i^{\top }& -\mathbf{1}& \mathbf{0}& \mathbf{0}\end{array}\right],\\ \\ \forall j\in \mu \left(i\right){\mathit{D}}_{i,j}=\left[\begin{array}{cccccc}\mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& {}^i{\mathit{X}}_j^{*}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\end{array}\right],j=\lambda \left(i\right){\mathit{D}}_{i,j}=\left[\begin{array}{cccccc}{}^i{\mathit{X}}_{\lambda \left(i\right)}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\end{array}\right],\\ \\ \mathrm{if}\lambda \left(i\right)=\mathbf{0}{\mathit{b}}_i=\left[\begin{array}{c}{}^i{\mathit{X}}_0{\mathit{a}}_{\mathbf{0}}+{\mathit{v}}_i\times {\mathit{S}}_i\dot{{\mathit{q}}_i}\\ {\mathit{v}}_i{\times }^{*}{\mathit{I}}_i{\mathit{v}}_i\\ \mathbf{0}\\ \mathbf{0}\end{array}\right],\mathrm{if}\lambda \left(i\right)\ne \mathbf{0}{\mathit{b}}_i=\left[\begin{array}{c}{\mathit{v}}_i\times {\mathit{S}}_i\dot{{\mathit{q}}_i}\\ {\mathit{v}}_i{\times }^{*}{\mathit{I}}_i{\mathit{v}}_i\\ \mathbf{0}\\ \mathbf{0}\end{array}\right].\end{array}$$

Remarkably, Equation (4) represents the set of linear constraints in d and, in a sense, inverse dynamic computation consists of computing d given ${\mathit{f}}_i^x$ and ${\ddot{\mathit{q}}}_i$. Our contribution is in moving away from this classical approach replacing RNEA boundary conditions with measurements coming from sensors. For this purpose, let us also define an explicit equation for measurements by indicating with y $\in {\mathbb{R}}^y$ the values vector measured by sensors:

$$\mathit{Y}(\mathit{q},\dot{\mathit{q}})\mathit{d}+{\mathit{b}}_Y(\mathit{q},\dot{\mathit{q}})=\mathit{y}$$

(5)

The structure of Y matrix depends on the number of sensors $N_S$ used for each link i as follows:

$$\begin{array}{cccc}\hfill \mathit{Y}& ={\left[\begin{array}{c}{\mathit{Y}}_{\mathbf{1}}\cdots {\mathit{Y}}_{N_B}\end{array}\right]}^{\top }\in {\mathbb{R}}^{N_S\times d},\hfill & \hfill {\mathit{Y}}_i& ={\left[\begin{array}{c}{\mathit{Y}}_{i,1}^{\top }\cdots {\mathit{Y}}_{i,{N_S}_i}^{\top }\end{array}\right]}^{\top }\in {\mathbb{R}}^{{N_S}_i\times d_i},\hfill \end{array}$$

being $N_S={\sum }_i{N_S}_i$ the amount of sensors. With the same methodology, the structure of the bias vector ${\mathit{b}}_Y$ is also defined:

$$\begin{array}{cccc}\hfill {\mathit{b}}_Y& ={\left[\begin{array}{c}{\mathit{b}}_{Y_1}\cdots {\mathit{b}}_{Y_{N_B}}\end{array}\right]}^{\top }\in {\mathbb{R}}^{N_S},\hfill & \hfill {\mathit{b}}_{Y_i}& ={\left[\begin{array}{c}{\mathit{b}}_{Y_{i,1}}^{\top }\cdots {\mathit{b}}_{Y_{i,{N_S}_i}}^{\top }\end{array}\right]}^{\top }\in {\mathbb{R}}^{{N_S}_i}.\hfill \end{array}$$

3.3. Considerations on the Representation

Equation (4) is one of many possible representations of the system dynamics. A common alternative description is the one obtained with the Euler–Lagrange formalisms [12]:

$$\begin{array}{c}\hfill \mathit{M}\left(\mathit{q}\right)\ddot{\mathit{q}}+\mathit{C}(\mathit{q},\dot{\mathit{q}})\dot{\mathit{q}}+\mathit{g}\left(\mathit{q}\right)=\mathbf{\tau }+{\mathit{J}}^{\top }\left(\mathit{q}\right){\mathit{f}}^x.\end{array}$$

These equations can be obtained from Equation (4) as hereafter described. First, the vector d and the columns of D should be rearranged so that they respect the following order: ${\mathit{a}}_1..{\mathit{a}}_{N_B}$, ${\mathit{f}}_1^B..{\mathit{f}}_{N_B}^B$, ${\mathit{f}}_1..{\mathit{f}}_{N_B}$, ${\ddot{\mathit{q}}}_1..{\ddot{\mathit{q}}}_{N_B}$, ${\mathit{f}}_1^x..{\mathit{f}}_{N_B}^x$ and ${\mathbf{\tau }}_1..{\mathbf{\tau }}_{N_B}$. The resulting D and b are:

With this reorganization, the Euler–Lagrange equation can be obtained as follows:

$$\begin{array}{c}{\mathit{D}}_{a,f,f_B}^{\left(2\mathrm{d}\right)}\left\{{\left[{\mathit{D}}_{a,f,f_B}^{\left(2\mathrm{a}\right)-\left(2\mathrm{c}\right)}\right]}^{-1}\left[-{\mathit{b}}_D^{\left(2\mathrm{a}\right)-\left(2\mathrm{c}\right)}-{\mathit{D}}_{\ddot{q}}^{\left(2\mathrm{a}\right)-\left(2\mathrm{c}\right)}\ddot{\mathit{q}}-{\mathit{D}}_{f^x}^{\left(2\mathrm{a}\right)-\left(2\mathrm{c}\right)}{\mathit{f}}^x-{\mathit{D}}_{\tau }^{\left(2\mathrm{a}\right)-\left(2\mathrm{c}\right)}\mathbf{\tau }\right]\right\}+{\mathit{D}}_{\ddot{q}}^{\left(2\mathrm{d}\right)}\ddot{\mathit{q}}+{\mathit{D}}_{f^x}^{\left(2\mathrm{d}\right)}{\mathit{f}}^x+{\mathit{D}}_{\tau }^{\left(2\mathrm{d}\right)}\mathbf{\tau }+{\mathit{b}}_D^{\left(2\mathrm{d}\right)}=\mathbf{0}.\end{array}$$

There are two reasons for preferring Equation (4) to alternative formulations such as the Euler–Lagrange equation. On the one hand, Equation (4) can be used to represent uncertainties that capture relevant modeling approximations (see also Section 3.4). In particular, approximations result from the fact that human bones coupling is neither rigid nor purely rotational and these can be captured with additive noise Equation (9) on Equation (2). More accurate models would be possible [14], but Equation (9) would still capture approximations on bone couplings, which are often relevant and meaningful. On the other hand, there are numerical advantages associated to Equation (4). In the case of inverse and forward dynamics, the numerical advantages are exactly those obtained by algorithms like the RNEA and the ABA (Articulated-Body Algorithm) presented in [11] and whose relations to Equation (4) is discussed in [10].

3.4. Over Constrained RNEA

Combining properly Equations (4) and (5), we obtain:

$$\begin{array}{c}\hfill \left[\begin{array}{c}\mathit{Y}(\mathit{q},\dot{\mathit{q}})\\ \mathit{D}(\mathit{q},\dot{\mathit{q}})\end{array}\right]\mathit{d}+\left[\begin{array}{c}{\mathit{b}}_Y(\mathit{q},\dot{\mathit{q}})\\ {\mathit{b}}_D(\mathit{q},\dot{\mathit{q}})\end{array}\right]=\left[\begin{array}{c}\mathit{y}\\ \mathbf{0}\end{array}\right]\end{array}$$

(6)

Since our main purpose is to make WearDY a versatile and flexible tool, we consider the incorporation of redundant (and noisy) measurements involved in the analysis. Within this new framework, there might be conditions in which Equation (6) becomes overdetermined and an exact solution does not exist. If there is a valid reason to assume that all the constraints have equal relevance, we can use the Moore–Penrose pseudo-inverse to obtain a least square solution. Otherwise, if we have good reason for weighting differently the constraints, we can use the weighted pseudo-inverse to obtain a weighted square solution. However, finding proper weights might be not an easy task. Our solution, proposed in the next section, is framing the estimation of d given y in a Gaussian framework by means of a minimum-variance estimator.

4. Maximum a Posteriori (MAP) Estimator

The first assumption for adopting the Maximum a Posteriori (MAP) estimation approach is to consider d and y as stochastic variables with Gaussian distributions. Let us first define their suitable joint probability density using the factorization $p(\mathit{d},\mathit{y})=p\left(\mathit{d}\right)p\left(\mathit{y}\right|\mathit{d})$ being $p(·)$ the probability density and $p(·|·)$ its conditioned version. Given $p(\mathit{d},\mathit{y})$, we can compute an estimation of d using a MAP estimator (which, in Gaussian distributions, coincides with the mean of the distribution) as follows:

$${\mathit{d}}_{MAP}=arg\underset{\mathit{d}}{max}p\left(\mathit{d}\right|\mathit{y})\propto arg\underset{\mathit{d}}{max}p(\mathit{d},\mathit{y}),$$

where we applied Bayes’ rule, i.e., $p\left(\mathit{d}\right|\mathit{y})=p(\mathit{d},\mathit{y})/p(\mathit{y})$, and where we omitted the term $p\left(\mathit{y}\right)$ since it does not depend on d and does not contribute to the optimization.

Let us first give an expression for $p\left(\mathit{y}\right|\mathit{d})$:

$$p\left(\mathit{y}\right|\mathit{d})\sim \mathcal{N}\left({\mathbf{\mu }}_y,{\mathbf{\Sigma }}_y\right),{\mathbf{\mu }}_y=\mathit{Y}(\mathit{q},\dot{\mathit{q}})\mathit{d}+{\mathit{b}}_Y,$$

which implicitly makes the assumption that the measurements from Equation (5) are affected by a Gaussian noise with zero mean and covariance ${\mathbf{\Sigma }}_y$. Its probability distribution is:

$$p\left(\mathit{y}\right|\mathit{d})\propto exp\left\{-\frac{1}{2}{(\mathit{y}-{\mathbf{\mu }}_y)}^{\top }{\mathbf{\Sigma }}_{\mathit{y}}^{-1}(y-{\mathbf{\mu }}_y)\right\}$$

(7)

The second assumption is to define a probability density for d. Pursuing the same methodology, we would like to have the following distribution $p\left(\mathit{d}\right)\sim \mathcal{N}\left({\mathbf{\mu }}_D,{\mathbf{\Sigma }}_D\right)$ :

$$p\left(\mathit{d}\right)\propto exp\left\{-\frac{1}{2}\mathit{e}{\left(\mathit{d}\right)}^{\top }{\mathbf{\Sigma }}_D^{-1}\mathit{e}\left(\mathit{d}\right)\right\}$$

(8)

taking into account constraints in Equation (4) with $\mathit{e}\left(\mathit{d}\right)=\mathit{D}(\mathit{q},\dot{\mathit{q}})\mathit{d}+{\mathit{b}}_D$.

However, this intuitive choice leads to a degenerate normal distribution and a term of regularization has to be adopted. For example, if we have a Gaussian prior knowledge on d in the form of $p\left(\mathit{d}\right)\sim \mathcal{N}\left({\mathbf{\mu }}_d,{\mathbf{\Sigma }}_d\right)$ distribution, we can reformulate Equation (8) as $p\left(\mathit{d}\right)\sim \mathcal{N}\left({\overline{\mathbf{\mu }}}_D,{\overline{\mathbf{\Sigma }}}_D\right)$:

$$p\left(\mathit{d}\right)\propto exp\left\{-\frac{1}{2}[\mathit{e}{\left(\mathit{d}\right)}^{\top }{\mathbf{\Sigma }}_D^{-1}\mathit{e}\left(\mathit{d}\right)+{(\mathit{d}-{\mathbf{\mu }}_d)}^{\top }{\mathbf{\Sigma }}_{\mathit{d}}^{-1}(d-{\mathbf{\mu }}_d)]\right\}$$

(9)

with

$$\begin{array}{ccc}\hfill {\overline{\mathbf{\Sigma }}}_D& =& {\left({\mathit{D}}^{\top }{\mathbf{\Sigma }}_D^{-1}\mathit{D}+{\mathbf{\Sigma }}_d^{-1}\right)}^{-1}\hfill \end{array}$$

(10a)

$$\begin{array}{ccc}\hfill {\overline{\mathbf{\mu }}}_D& =& {\overline{\mathbf{\Sigma }}}_D({\mathbf{\Sigma }}_d^{-1}{\mathbf{\mu }}_d-{\mathit{D}}^{\top }{\mathbf{\Sigma }}_D^{-1}{\mathit{b}}_D)\hfill \end{array}$$

(10b)

Given Equations (7) and (9), we can build the joint probability as $p\left(\mathit{d}\right|\mathit{y})\sim \mathcal{N}\left({\mathbf{\mu }}_{d|y},{\mathbf{\Sigma }}_{d|y}\right)$ being

$$\begin{array}{ccc}\hfill {\mathbf{\Sigma }}_{d|y}& =& {\left({\overline{\mathbf{\Sigma }}}_D^{-1}+{\mathit{Y}}^{\top }{\mathbf{\Sigma }}_y^{-1}\mathit{Y}\right)}^{-1}\hfill \end{array}$$

(11a)

$$\begin{array}{ccc}\hfill {\mathbf{\mu }}_{d|y}& =& {\mathbf{\Sigma }}_{d|y}\left[{\mathit{Y}}^{\top }{\mathbf{\Sigma }}_y^{-1}(\mathit{y}-{\mathit{b}}_Y)+{\overline{\mathbf{\Sigma }}}_D^{-1}{\overline{\mathbf{\mu }}}_D\right]\hfill \end{array}$$

(11b)

where Equation (11b) is exactly the estimation ${\mathit{d}}_{MAP}$.

On the Benefits of MAP Dynamics

In this section, we discuss the benefits of multi-sensor data fusion for solving the dynamic estimation problem by characterizing the effects of data fusion on the covariance of associated estimator. The general idea we would like to pursue is that the more sensors we use in the estimation, the better the estimation itself will be (see Appendix A for the metric used for the estimation quality). Since we are interested in the analytic solution of MAP, the estimator must have the following covariance (combining Equations (11a) and (10a)):

$$\begin{array}{c}\hfill {\mathbf{\Sigma }}_{\mathit{d}|\mathit{y}}={\left({\mathit{D}}^{\top }{\mathbf{\Sigma }}_D^{-1}\mathit{D}+{\mathbf{\Sigma }}_d^{-1}+{\mathit{Y}}^{\top }{\mathbf{\Sigma }}_y^{-1}\mathit{Y}\right)}^{-1}\end{array}$$

(12)

Assuming multiple measurements ${\mathit{y}}_1={\mathit{Y}}_1\mathit{d}+{\mathit{b}}_{Y_1}$, $\dots$, ${\mathit{y}}_m={\mathit{Y}}_m\mathit{d}+{\mathit{b}}_{Y_m}$ statistically independent, this implies a diagonal structure to the matrix ${\mathbf{\Sigma }}_y^{-1}$. Thus, we have:

$$\begin{array}{c}{\mathit{Y}}^{\top }{\mathbf{\Sigma }}_y^{-1}\mathit{Y}=\left[\begin{array}{ccc}{\mathit{Y}}_1^{\top }& \cdots & {\mathit{Y}}_m^{\top }\end{array}\right]\left[\begin{array}{ccc}{\mathbf{\Sigma }}_{y_1}^{-1}& \cdots & 0\\ ⋮& \ddots & ⋮\\ 0& \cdots & {\mathbf{\Sigma }}_{y_m}^{-1}\end{array}\right]\left[\begin{array}{c}{\mathit{Y}}_1\\ ⋮\\ {\mathit{Y}}_m\end{array}\right]\Rightarrow {\mathit{Y}}^{\top }{\mathbf{\Sigma }}_y^{-1}\mathit{Y}={\mathit{Y}}_1^{\top }{\mathbf{\Sigma }}_{y_1}^{-1}{\mathit{Y}}_1+\cdots +{\mathit{Y}}_m^{\top }{\mathbf{\Sigma }}_{y_m}^{-1}{\mathit{Y}}_m.\end{array}$$

With an abuse of notation, let us denote with $\mathit{d}|{\mathit{y}}_i$ the estimator which exploits all measurements up to i-th, i.e., ${\mathit{y}}_1,\cdots ,{\mathit{y}}_i$. The addition of one measurement induces changes in the associated covariance matrix according to the following recursive equation:

$$\begin{array}{c}\hfill {\mathbf{\Sigma }}_{d|y_i}^{-1}={\mathbf{\Sigma }}_{d|y_{i-1}}^{-1}+{\mathit{Y}}_i^{\top }{\mathbf{\Sigma }}_{y_i}^{-1}{\mathit{Y}}_i\end{array}$$

(13)

where, for $i=1$ , the initial condition is ${\mathbf{\Sigma }}_{d|y_0}^{-1}={\mathit{D}}^{\top }{\mathbf{\Sigma }}_D^{-1}\mathit{D}+{\mathbf{\Sigma }}_d^{-1}$.

Considering the Weyl inequality for the largest eigenvalue ${\lambda }_1$ of Equation (13) (see Appendix B):

$$\begin{array}{c}\frac{{\lambda }_N\left({\mathbf{\Sigma }}_{d|y_{i-1}}\right)}{1+{\lambda }_N\left({\mathbf{\Sigma }}_{d|y_{i-1}}\right){\lambda }_1\left({\mathit{Y}}_i^{\top }{\mathbf{\Sigma }}_{y_i}^{-1}{\mathit{Y}}_i\right)}\le {\lambda }_1\left({\mathbf{\Sigma }}_{d|y_i}\right)\le \frac{1}{L_1},L_1=\underset{i+j=N+1}{max}\left[\frac{1}{{\lambda }_{N-i+1}\left({\mathbf{\Sigma }}_{d|y_{i-1}}\right)}+{\lambda }_j\left({\mathit{Y}}_i^{\top }{\mathbf{\Sigma }}_{y_i}^{-1}{\mathit{Y}}_i\right)\right].\end{array}$$

The maximum benefit is obtained by lowering the upper bound on ${\lambda }_1\left({\mathbf{\Sigma }}_{d|y_i}\right)$. Trivially, this can be obtained by choosing high values for all the eigenvalues of ${\mathit{Y}}_i^{\top }{\mathbf{\Sigma }}_{y_i}^{-1}{\mathit{Y}}_i$. Obviously, this is not always possible and benefits can be obtained by maximizing $L_1$.

5. Experimental Results

5.1. Experimental Set-up

Experiments have been conducted on 12 adult healthy subjects (nine males and three females). Subjects provided their written informative consent before becoming involved in the research. Motion data were collected at Istituto Italiano di Tecnologia (IIT), Genova, Italy, using a motion capture system (Vicon Motion Systems Ltd, Oxford, UK) with eight infrared cameras, at a sampling rate of 100 Hz. A total of 14 passive retro-reflective markers were attached to each participant at key anatomical landmarks. Markers are properly positioned in order to capture the pitch motion on the sagittal plane. Lower body markers are placed in the following positions: left and right back of the foot (heel), left and right outside of the ankle, left and right 2nd toe of the foot, left and right hip joint. For the upper body, they are placed on the bony prominence on top of both shoulders and one in the center of the upper torso. No arm and head motions are evaluated in this experiment. Kinematics information are recorded at a sampling rate of 100 Hz using an inertial sensor unit (Xsens Technologies B.V., Enschede, Netherlands) attached with an elastic strip on the trunk, including an accelerometer and a gyroscope (Figure 2b). Three markers are placed on the IMU external surface in order to compare data from the Vicon system and the inertial sensor. The task is performed on a standard force platform AMTI OR6 (Advanced Mechanical Technology Inc., Watertown, USA) synchronized with the Vicon system and data are recorded at a sampling rate of 1 kHz (Figure 2a). Each subject is asked to perform a bowing task without bending the knee in order to assume legs as a rigid link. Each participant experiment session consisted of four trials each composed of three bows. A general scheme of the experimental set-up is summarized in Figure 3.

5.2. Human Body Modeling

Inspired by state-of-the-art human stance modeling [15], the human body was modeled as a double-inverted-pendulum (DIP). A two DOF ($N_B=2$, $\mathit{d}\in {\mathbb{R}}^{52}$) model of the human body for each subject was developed by using the Universal Robot Description Format (URDF) which is an XML-based file format for representing kinematics and dynamics of multibody systems. The location of four imaginary points in the body ($P_0,P_1,P_2,P_3$) are computed by averaging the location of Vicon markers. The final model consists of three rigid links representing the feet (link 0 or fixed base), legs (link 1) and torso (link 2) and two revolute joints positioned at ankle (joint 1) and hip (joint 2) and subjected to gravity. Points $P_0,P_1,P_2,P_3$ represent the origin of the reference frames associated to each link (with the exception for $P_3$ as link 3 does not exist). In particular, reference frames have the same orientation of the body to which they are associated. No rigid links for head or arms were considered. Figure 4 shows human modeling passing from a marker-like Figure 4a to a URDF-like model Figure 4b.

5.3. Proof of Concept

Analysis is performed on MATLAB using a toolbox released by Roy Featherstone [16] and freely available under an open source licence. The core of the experiment consists in using the MAP algorithm to estimate for both links in the model the dynamic vectors ${\mathit{d}}_1({\mathit{a}}_1,{\mathit{f}}_1^B,{\mathit{f}}_1,{\mathbf{\tau }}_1,{\mathit{f}}_1^x,{\ddot{\mathit{q}}}_1)$ and ${\mathit{d}}_2({\mathit{a}}_2,{\mathit{f}}_2^B,{\mathit{f}}_2,{\mathbf{\tau }}_2,{\mathit{f}}_2^x,{\ddot{\mathit{q}}}_2)$. In the proposed experiment, $\mathit{q}={[{\mathit{q}}_1{\mathit{q}}_2]}^{\top }$ was obtained from Vicon marker analysis with a frequency filtering in order to smooth acquisition noise. Joint velocities $\dot{\mathit{q}}={[{\dot{\mathit{q}}}_1{\dot{\mathit{q}}}_2]}^{\top }$ and accelerations $\ddot{\mathit{q}}={[{\ddot{\mathit{q}}}_1{\ddot{\mathit{q}}}_2]}^{\top }$ have been computed using a weighted sum of windows of elements, with a third-order polynomial Savitzky–Golay filtering (as shown in Figure 5). Considering that during the experiment no external force was applied, ${\mathit{f}}^x={[{\mathit{f}}_1^x{\mathit{f}}_2^x]}^{\top }$ was null and was simulated by associating a null measurement. Above-obtained variables and the dynamic model were used in building the constraint Equation (4).

Similarly, from the sensor equations :

$$\begin{array}{ccc}\hfill {\mathit{y}}_{1,fp}& =& {}^{fp}{\mathit{X}}_0^{*}({}^0{\mathit{X}}_1^{*}{\mathit{f}}_1-{\mathit{I}}_0{}^0{\mathit{a}}_g),\hfill \\ \hfill {\mathit{y}}_{2,acc}& =& {\left({}^{imu}{\mathit{X}}_2{\mathit{a}}_2\right)}_{lin}+{\left({}^{imu}{\mathit{X}}_2{\mathit{v}}_2\right)}_{ang}\times {\left({}^{imu}{\mathit{X}}_2{\mathit{v}}_2\right)}_{lin},\hfill \\ \hfill {\mathit{y}}_{2,gyro}& =& {\left({}^{imu}{\mathit{X}}_2{\mathit{v}}_2\right)}_{ang},\hfill \end{array}$$

we built the measurements Equation (5) for three different cases: (1) $\mathit{y}=[\ddot{\mathit{q}}]$; (2) $\mathit{y}={[\ddot{\mathit{q}}{\mathit{y}}_1]}^{\top }$; (3) $\mathit{y}={[\ddot{\mathit{q}}{\mathit{y}}_1{\mathit{y}}_2]}^{\top }$. It is worth noticing that the gyroscope measurement has been numerically differentiated to obtain a measurement for the angular part of ${\mathit{a}}_i$ (i.e., ${\dot{\mathbf{\omega }}}_i$). Since the final MAP estimation ${\mathbf{\mu }}_{d|y}$ is built as a weighted sum of a priori distribution and sensor measurements, it has required defining ${\mathbf{\Sigma }}_D$ and ${\mathbf{\Sigma }}_d$ (in order to establish how much the dynamic model was reliable) and ${\mathbf{\Sigma }}_y$ to properly weight the contribution of each sensor. In the present analysis, the standard deviations σ have been chosen from sensors’ datasheets, and their values are reported in

Table 1. Standard deviation σ from sensors’ datasheets.

	Angular Part			Linear Part
${\sigma }_{\ddot{q}}$				$2.57\times {10}^{-3}\mathrm{rad}/{\mathrm{s}}^2$
${\sigma }_{y_1}$	1.5 $\mathrm{Nm}$	1.5 $\mathrm{Nm}$	0.7483 $\mathrm{Nm}$	7.68 $\mathrm{N}$	7.68 $\mathrm{N}$	6 $\mathrm{N}$
${\sigma }_{y_2}$	$7.1\times {10}^{-3}\mathrm{rad}/{\mathrm{s}}^2$ (gyro)			$3.33\times {10}^{-2}\mathrm{m}/{\mathrm{s}}^2$ (acc)

.

The experiment was conducted to prove exactly what was described in Section 4.1: passing progressively from sensor case 1 to case 3, the variance associated to each variable in the estimated vector ${\mathbf{\mu }}_{d|y}$ decrease. Figure 6 shows the σ decreasing behavior for three variables (${\sigma }_{\tau |y}$, ${\sigma }_{f|y}$ and ${\sigma }_{a|y}$) using MAP computation as sensors have risen. Variance data are reported in

Table 2. Standard deviation of estimated variables for link 1.

	${\mathbf{\sigma }}_{{\mathbf{\tau }}_{\mathbf{1}}}$	${\mathbf{\sigma }}_{{\mathbf{\mu }}_{\mathbf{1}}\left(\mathbf{1}\right)}$	${\mathbf{\sigma }}_{{\mathbf{\mu }}_{\mathbf{1}}\left(\mathbf{2}\right)}$	${\mathbf{\sigma }}_{{\mathbf{\mu }}_{\mathbf{1}}\left(\mathbf{3}\right)}$	${\mathbf{\sigma }}_{{\mathit{f}}_{\mathbf{1}}\left(\mathbf{1}\right)}$	${\mathbf{\sigma }}_{{\mathit{f}}_{\mathbf{1}}\left(\mathbf{2}\right)}$	${\mathbf{\sigma }}_{{\mathit{f}}_{\mathbf{1}}\left(\mathbf{3}\right)}$	${\mathbf{\sigma }}_{{\dot{\mathbf{\omega }}}_{\mathbf{1}}\left(\mathbf{1}\right)}$	${\mathbf{\sigma }}_{{\dot{\mathbf{\omega }}}_{\mathbf{1}}\left(\mathbf{2}\right)}$	${\mathbf{\sigma }}_{{\dot{\mathbf{\omega }}}_{\mathbf{1}}\left(\mathbf{3}\right)}$	${\mathbf{\sigma }}_{{\mathit{a}}_{\mathbf{1}}\left(\mathbf{1}\right)}$	${\mathbf{\sigma }}_{{\mathit{a}}_{\mathbf{1}}\left(\mathbf{2}\right)}$	${\mathbf{\sigma }}_{{\mathit{a}}_{\mathbf{1}}\left(\mathbf{3}\right)}$
$\ddot{q}$	4.990	2.075	1.153	2.088	2.423	2.279	2.371	0.028	0.027	0.029	0.025	0.027	0.028
$\ddot{q},y_1$	1.340	1.118	0.636	1.145	1.558	2.099	1.600	0.025	0.026	0.025	0.021	0.026	0.024
$\ddot{q},y_1,y_2$	0.970	1.020	0.633	0.977	1.364	1.659	1.382	0.019	0.020	0.019	0.019	0.022	0.022

for link 1 and

Table 3. Standard deviation of estimated variables for link 2.

	${\mathbf{\sigma }}_{{\mathbf{\tau }}_{\mathbf{2}}}$	${\mathbf{\sigma }}_{{\mathbf{\mu }}_{\mathbf{2}}\left(\mathbf{1}\right)}$	${\mathbf{\sigma }}_{{\mathbf{\mu }}_{\mathbf{2}}\left(\mathbf{2}\right)}$	${\mathbf{\sigma }}_{{\mathbf{\mu }}_{\mathbf{2}}\left(\mathbf{3}\right)}$	${\mathbf{\sigma }}_{{\mathit{f}}_{\mathbf{2}}\left(\mathbf{1}\right)}$	${\mathbf{\sigma }}_{{\mathit{f}}_{\mathbf{2}}\left(\mathbf{2}\right)}$	${\mathbf{\sigma }}_{{\mathit{f}}_{\mathbf{2}}\left(\mathbf{3}\right)}$	${\mathbf{\sigma }}_{{\dot{\mathbf{\omega }}}_{\mathbf{2}}\left(\mathbf{1}\right)}$	${\mathbf{\sigma }}_{{\dot{\mathbf{\omega }}}_{\mathbf{2}}\left(\mathbf{2}\right)}$	${\mathbf{\sigma }}_{{\dot{\mathbf{\omega }}}_{\mathbf{2}}\left(\mathbf{3}\right)}$	${\mathbf{\sigma }}_{{\mathit{a}}_{\mathbf{2}}\left(\mathbf{1}\right)}$	${\mathbf{\sigma }}_{{\mathit{a}}_{\mathbf{2}}\left(\mathbf{2}\right)}$	${\mathbf{\sigma }}_{{\mathit{a}}_{\mathbf{2}}\left(\mathbf{3}\right)}$
$\ddot{q}$	0.510	0.848	0.758	0.758	1.817	1.544	1.696	0.040	0.037	0.041	0.043	0.040	0.042
$\ddot{q},y_1$	0.356	0.650	0.669	0.615	1.423	1.238	1.100	0.038	0.036	0.037	0.035	0.033	0.030
$\ddot{q},y_1,y_2$	0.305	0.643	0.657	0.568	1.092	1.026	0.918	0.013	0.013	0.013	0.023	0.023	0.022

for link 2. In Figure 7, we focused on the estimated ${\mathbf{\mu }}_{\tau |y}$ for both links in the model. Estimations are represented with their standard deviations (2σ). It is worth noticing that, while the estimations for ${\mathbf{\tau }}_2$ are very comparable, estimations for ${\mathbf{\tau }}_1$ are less similar due to the fact that, in a sense, ${\mathbf{\tau }}_1$ is almost a direct measurement to compensate for the model error in the computation.

5.4. Method Robustness Test

This section describes the analysis we performed to test the robustness w.r.t. modeling errors of the MAP and RNEA. As in the previous experiments, we performed a bowing task. This time, the subject repeated the experiment in two different configurations, that is with and without an additional weight of 5 kg roughly positioned in correspondence of the center of mass (COM) of the torso. By exploiting the linearity property of the system, we started by considering the expression for the torques:

$$\begin{array}{c}\hfill {\mathbf{\tau }}_{(model+5kg)}-{\mathbf{\tau }}_{\left(model\right)}={\mathbf{\tau }}_{\left(5kg\right)}\end{array}$$

(15)

where we denoted with model+5kg the kinematic and dynamic model of the subject with the additional 5 kg mass on the torso, and with model the model of the subject without the weight. The above expression exploits the property of linearity of the model w.r.t. a set of dynamic parameters [12], and it is in general described by the following equation:

$$\begin{array}{c}\hfill \mathbf{\tau }=\mathit{Y}(\mathit{q},\dot{\mathit{q}},\ddot{\mathit{q}})\mathbf{\pi },\end{array}$$

where $\mathbf{\pi }$ is a vector of constant parameters and Y, usually known as regressor, is the matrix function of joint positions, velocities and accelerations.

To assess the robustness property of the two algorithms w.r.t. modeling errors, we then computed the inverse dynamics by using, respectively, RNEA and MAP, considering both the measurements collected while the subject was performing the task with the 5 kg weight ($y_{\text{subject}+5\mathrm{kg}}$) and the measurements during the task without the weight ($y_{\text{subject}}$), as follows:

$$\begin{array}{cc}& {\mathbf{\tau }}_{RNEA(model,y_{subject+5kg})}-{\mathbf{\tau }}_{RNEA(model,y_{subject})}={\widehat{\mathbf{\tau }}}_{RNEA\left(5kg\right)}\end{array}$$

(16)

$$\begin{array}{cc}& {\mathbf{\tau }}_{MAP(model,y_{subject+5kg})}-{\mathbf{\tau }}_{MAP(model,y_{subject})}={\widehat{\mathbf{\tau }}}_{MAP\left(5kg\right)}\end{array}$$

(17)

Then, we compare the right-hand side of Equations (16) and (17) with the right-hand one of Equation (15). Figure 8 show mean and standard deviation for the error, respectively, on ${\mathbf{\tau }}_1$ and ${\mathbf{\tau }}_2$ comparing $|{\mathbf{\tau }}_{\left(5kg\right)}-{\widehat{\mathbf{\tau }}}_{RNEA\left(5kg\right)}|$ (on the left part of each figure) and $|{\mathbf{\tau }}_{\left(5kg\right)}-{\widehat{\mathbf{\tau }}}_{MAP\left(5kg\right)}|$ (on the right part). It is worth noticing that the error on the estimation of ${\mathbf{\tau }}_2$ is statistically significant different (p-value < 0.05) because ${\mathbf{\tau }}_2$ is more influenced by the additional weight on the torso. It is also worth remarking that, in order to compare the obtained torques in the different trials, we expressed them w.r.t. a linear combination of configuration variables instead of expressing them w.r.t. the time. Indeed, it was not possible to evaluate, in time t, the comparison between torques of different trials because the same subject performed the trials in different times; thus, the estimation of ${\mathbf{\tau }}_2$ has been parameterized with a linear combination of the joint positions, i.e., $({\mathit{q}}_1+q_2)$, with the primary assumption that $\dot{\mathit{q}}$ and $\ddot{\mathit{q}}$ could be neglected as they induced a small change on the $\mathbf{\tau }$ estimation. We can observe that the error on the $\mathbf{\tau }$ estimation is lower in MAP than in RNEA since our procedure is able to represent the model uncertainties. From this point of view, RNEA is, in a sense, more sensitive to the modeling errors. Conversely, MAP takes into account these errors because a variance is also associated to the model itself.

6. Conclusions

In this paper, we presented a novel methodology to estimate dynamics quantities along with human motion in order to exploit fusion of sensor information on a probabilistic framework. The framework is based on the idea of building a joint probability for all dynamic variables d and measurements y coming from multiple sensors. The probability density $p(\mathit{d},\mathit{y})$ incorporates the dynamic constraints among variables. Preliminary results show that adding the i-th measurement induces changes in its covariance matrix according to Equation (13) for all variables estimated. We demonstrate that the variance of each estimated variable in ${\mathbf{\mu }}_{d|y}$ decreases as the number of considered measurements (sensors) increase. Moreover, it should be necessary to implement a procedure (Expectation-Maximization (EM) algorithm) to estimate sensor variance from data; in a sense, the goal is to build a data-driven variance estimation overcoming datasheet variance. While this paper presents a proof of concept of the theory behind WearDY, future works will be aimed at real-time implementation, at the design of the wearable garment and at the improvement of the sensor architecture in order to make the system reliable in situ. Computationally, it will be fundamental to integrate MAP dynamics with a state estimator such as an Extended Kalman filter combining obtained a posteriori estimates with a priori estimates of the filter state. From a design perspective, the wearable system will be comprised of a soft sensing garment with embedded sensors that facilitate free movement of the subject. The suit design will ideally exploit the material compliance in order to improve sensor reliability through elimination of the error introduced by the interface between the garment and the subject’s skin. Another important future objective concerns the possibility of using electromyography (EMG) analysis to provide information on muscular activity that is related to joint torques. Possible future applications of WearDY include rehabilitation monitoring where a wearable garment will be used for monitoring patients or creating more ergonomic and compliant prosthesis and exoskeleton systems. The proposed suit and the forthcoming improvements would permit substantial enhancements in the analysis of human movements in motion.