# Model-Based Real-Time Motion Tracking Using Dynamical Inverse Kinematics
## 3 Method  
### Velocity Correction Using Rotation Matrix  
As a first instance, it is necessary to define some displacement measurements to be minimized. The displacements from the pose target vector \(x(t)\), given the current state \(q(t)\), are collected in a residual vector \(r(q(t),x(t))\) defined as follows:  
\[r(q(t),x(t))=\left[ \begin{array}{c}\mathcal{I}p_{\mathcal{P}_{1}}(t)-h^{p}_{ \mathcal{P}_{1}}(q(t))\\ \vdots\\ \\ \mathcal{I}p_{\mathcal{P}_{np}}(t)-h^{p}_{\mathcal{P}_{np}}(q(t))\\ \mathrm{sk}(h^{o}_{\mathcal{O}_{1}}(q(t))^{T}\mathcal{I}^{T}R_{\mathcal{O}_{1 }}(t))^{\vee}\\ \vdots\\ \mathrm{sk}(h^{o}_{\mathcal{O}_{n_{0}}}(q(t))^{T}\mathcal{I}R_{\mathcal{O}_{n_ {0}}}(t))^{\vee}\end{array} \right]. \tag{5}\]  
Note that the position displacement is measured as Euclidean distance, while, the displacement between two orientation measurements is computed on SO(3) using rotation matrices with the \(\mathrm{sk}(. )^{\vee}\) operator. The displacements from the target velocities \(\mathrm{v}(t)\) are collected in the velocity residual vector \(u(q(t),\
u(t),\
u(t))\). In this case all the vectors lie in a space over \(\mathbb{R}\), and the residual vector is defined as:  
\[u(q(t),\
u(t),\
u(t))=\mathrm{v}(t)-J(q(t))\
u(t). \tag{6}\]  
At this stage, we assume the state velocity \(\
u(t)\) being the control input of a dynamical system described by the differential Equations (5) and (6), where we want to control the output residual vectors \(r(t)\) and \(u(t)\) to be driven to zero. As a consequence of the orientation error properties presented in [33], the following result holds. **Corollary 1**. : _Assume \(r(q(t),x(t))\) defined as in (5), \(u(q(t),\
u(t),\
u(t))\) defined as in (6), and the system_  
\[u(q(t),\
u(t),\
u(t))+Kr(q(t),x(t))=0, \tag{7}\]  
_where \(K\in\mathbb{R}^{(3n_{p}+3n_{o})}\times\mathbb{R}^{(3n_{p}+3n_{o})}\) is a positive definite diagonal matrix. Then, \((r,u)=(0,0)\) denotes an (almost) globally asymptotically stable equilibrium point for the system._The proof is provided in Appendix A. Corollary 1 shows that we can control the system so that \(r(t)\) and \(u(t)\) converge to zero for (almost) any initialization \(q(t_{0})\). The rate of convergence depends on the magnitude of the elements of matrix \(K\), higher values of \(K\) imply faster convergence of the system (7) towards zero. Replacing the expression of \(u(q(t),v(t))\) presented in (6), into the system (7), we can derive an expression that is linear in the control input, i.e., state velocity \(\
u(t)\):  
\[J(q(t))\
u(t)=\
u(t)+Kr(q(t),x(t)). \tag{8}\]  
The control input \(\
u(t)\) can be obtained from Equation (8), the strategy to compute it will be the topic of the following Section 3.2. **Remark 1**. : _The existence of a solution for Equation (8) depends on the rank of the Jacobian matrix \(J(q(t))\), and of the augmented matrix \([J(q(t))|(\
u(t)+Kr(q(t),x(t)))]\), there are many cases in which the solution can be found only numerically as a least-squares optimization. The presence of the optimization residual error \(\varepsilon\), such that \(J(q(t))\
u(t)=\
u(t)+Kr(q(t),x(t))+\varepsilon\), prevents the direct application of Corollary 1. However, the system convergence is ensured within a neighbourhood of the origin that depends on \(\left\|\varepsilon\right\|_{2}\) (which is minimized within the optimization problem). The proof of it is beyond the scope of this paper._  
**Remark 2**. : _The Lyapunov analysis leading to Equation (8) has been done considering continuous-time. Some further considerations have to be done for discrete-time implementation. The discrete control input \(\
u(t_{k})\) is obtained by replacing Equation (8) with the following discrete-time equation:_  
\[J(q(t_{k-1}))\
u(t_{k})=\
u(t_{k})+Kr(q(t_{k-1}),x(t_{k})). \tag{9}\]  
_Moreover, discrete time solution bounds the values of \(K\) depending on the sampling time [34].