# Model-Based Real-Time Motion Tracking Using Dynamical Inverse Kinematics
## 2 Background
### Problem Statement  
Motion tracking algorithms aim to find the model joint configuration given a set of targets describing the kinematics of the model links. The targets are the measurements of the link pose and velocity expressed in a world reference frame, and can be retrieved from various sensors measurements, e.g., processing of IMUs data [32]. The process of estimating the configuration of a mechanical system from task space measurements is generally referred to as inverse kinematics, and can be formulated as follows:  
**Problem 1**.: _Given a set of \(n_{p}\) frames \(\mathcal{P}=\{\mathcal{P}_{1},\mathcal{P}_{2},....\mathcal{P}_{n_{p}}\}\) with the associated target position \({}^{\mathcal{I}}p_{\mathcal{P}_{i}}(t)\in\mathbb{R}^{3}\) and target linear velocity measurements \({}^{\mathcal{I}}p_{\mathcal{P}_{i}}(t)\in\mathbb{R}^{3}\), and given a set of \(n_{o}\) frames \(\mathcal{O}=\{\mathcal{O}_{1},\mathcal{O}_{2},....\mathcal{O}_{n_{o}}\}\) with the associated target orientation \({}^{\mathcal{I}}R_{\mathcal{O}_{j}}(t)\in SO(3)\) and target angular velocity measurements \({}^{\mathcal{I}}\omega_{\mathcal{O}_{j}}(t)\in\mathbb{R}^{3}\), and given the kinematic description of the model, find the state configuration (\(q(t)\),\(\
u(t)\)) such that:_  
\[\begin{cases}^{\mathcal{I}}p_{\mathcal{P}_{i}}(t)=h^{p}_{\mathcal{P}_{i}}(q(t )),&\forall i=1,\dots,n_{p}\\ ^{\mathcal{I}}R_{\mathcal{O}_{j}}(t)=h^{o}_{\mathcal{O}_{j}}(q(t)),&\forall j=1,\dots,n_{o}\\ ^{\mathcal{I}}\dot{p}_{\mathcal{P}_{i}}(t)=h^{\ell}_{\mathcal{P}_{i}}(q(t))\
u (t),&\forall i=1,\dots,n_{p}\\ ^{\mathcal{I}}\omega_{\mathcal{O}_{j}}(t)=J_{\mathcal{O}_{j}}^{a}(q(t))\
u(t),& \forall j=1,\dots,n_{o}\\ ^{A}q(t)\leq b^{q},&\\ ^{A}v^{s}(t)\leq b^{v},&\end{cases} \tag{1}\]  
_where \(A^{q}\) and \(b^{q}\) are two constant parameters that represent the limits for model joints configuration, and \(A^{v}\) and \(b^{v}\) are two constant parameters that represent the limits for joint velocity._  
The following quantities are defined in order to have a compact representation of Problem (1). The targets are collected in a _pose target vector_\(x(t)\) and _velocity target vector_\(\mathbf{v}(t)\):  
\[x(t):=\begin{bmatrix}^{\mathcal{I}}p_{\mathcal{P}_{1}}(t)\\ \vdots\\ ^{\mathcal{I}}p_{\mathcal{P}_{n_{p}}}(t)\\ ^{\mathcal{I}}R_{\mathcal{O}_{1}}(t)\\ \vdots\\ ^{\mathcal{I}}R_{\mathcal{O}_{n_{o}}}(t)\end{bmatrix},\ \ \mathbf{v}(t):=\begin{bmatrix}^{ \mathcal{I}}\dot{p}_{\mathcal{P}_{1}}(t)\\ \vdots\\ ^{\mathcal{I}}\dot{p}_{\mathcal{P}_{n_{p}}}(t)\\ ^{\mathcal{I}}\omega_{\mathcal{O}_{1}}(t)\\ ^{\mathcal{I}}\omega_{\mathcal{O}_{n_{o}}}(t)\end{bmatrix}, \tag{2}\]while, forward geometrical kinematics and Jacobians are expressed respectively as a single vector \(h(q(t))\) and a single matrix \(J(q(t))\), such as:  
\[h(q(t)):=\begin{bmatrix}h^{p}_{\mathcal{P}_{1}}(q(t))\\ \vdots\\ h^{p}_{\mathcal{P}_{\alpha_{p}}}(q(t))\\ h^{o}_{\mathcal{O}_{1}}(q(t))\\ \vdots\\ h^{o}_{\mathcal{O}_{n_{0}}}(q(t))\end{bmatrix},\;\;J(q(t)):=\begin{bmatrix}J^{ \ell}_{\mathcal{P}_{1}}(q(t))\\ \vdots\\ J^{\ell}_{\mathcal{P}_{\alpha_{p}}}(q(t))\\ J^{a}_{\mathcal{O}_{1}}(q(t))\\ \vdots\\ J^{a}_{\mathcal{O}_{n_{0}}}(q(t))\end{bmatrix}, \tag{3}\]  
the set of equations in (1) can then be written compactly, using the definitions of (2) and (3), as the following two equations that describe the _forward kinematics_ and _differential kinematics_ respectively for all the target frames:  
\[x(t) =h(q(t)), \tag{4a}\] \[\mathrm{v}(t) =J(q(t))\
u(t). \tag{4b}\]  
As mentioned in Section 1, in the case of highly articulate systems, like humans, finding an analytical solution for the state configuration (\(q(t)\),\(\
u(t)\)), satisfying the measurement Equations (4a) and (4b), is often not possible or too demanding. Hence, a numerical solution, such as non-linear optimization, is usually preferred (a general formulation of the non-linear optimization problem can be found in Section 5.1).