*2.3. EOMs: The Discrete Index-3 Form*

To transform the second order differential equation into first-order differential equations, it is common to introduce the velocity variable *q*˙ = *v*, allowing to write Equation (4) in a first-order form as:

$$\begin{cases} \mathcal{g}(\dot{\boldsymbol{\upsilon}}, \boldsymbol{\upsilon}, \dot{\boldsymbol{\eta}}, q, \lambda, \boldsymbol{\mu}) = \begin{cases} \mathcal{g}\_1 = \boldsymbol{\upsilon} - \dot{\boldsymbol{\eta}} = \boldsymbol{0}\_{\boldsymbol{\upsilon}} \\ \mathcal{g}\_2 = \mathcal{M}(q)\dot{\boldsymbol{\upsilon}} + f\_{\text{nl}}(\boldsymbol{\upsilon}, q, \boldsymbol{\mu}) + \boldsymbol{J}^T \lambda = \mathbf{0}\_{\boldsymbol{\eta}} \\ \mathcal{g}\_3 = \boldsymbol{\phi}(q) = \mathbf{0}\_{\boldsymbol{\lambda}} \end{cases} \end{cases} \tag{8}$$

These equations represent a system of constrained-DAEs of index 3 [21]. Numerical differentiation is generally employed [22] to convert Equation (8) into a discrete form.

In this work, a first order, constant time-step Backward Differentiation Formula (BDF-1), also known as Backward Euler, is employed. This choice does not imply that other differentiation schemes cannot be applied to discretize the EOMs in time. However, the time-discretization must be consistent with the defined estimation sampling and particular attention should be paid to choosing the differentiation schemes (e.g., forward or backward) because it influences the achievement of the discrete-time E-ODE form of the EOMs, as will be discussed in Section 3.

The single step method BDF-1 can be written as

$$\begin{cases} \dot{v}\_{k+1} = \frac{1}{\hbar}(v\_{k+1} - v\_k) \\ v\_{k+1} = \frac{1}{\hbar}(q\_{k+1} - q\_k) \end{cases} \tag{9}$$

where *<sup>h</sup>* represents the constant time step size and *<sup>k</sup>* <sup>∈</sup> <sup>Z</sup> the *<sup>k</sup>* <sup>−</sup> *th* time instance. By substituting Equation (9) into Equation (8) the discrete-time EOMs *gd* are obtained:

$$\begin{cases} \mathcal{g}\_{d1} = v\_{k+1} - \frac{1}{h}(q\_{k+1} - q\_k) = 0\_v\\ \mathcal{g}\_{d2} = M(q\_{k+1})\frac{v\_{k+1} - v\_k}{h} + f\_{nl}(q\_{k+1}, q\_k, u\_{k+1}) + f^T(q\_{k+1})\lambda\_{k+1} = 0\_\emptyset \quad . \end{cases} \tag{10}$$

This can be summarized as

$$g\_d(v\_{k+1'}, q\_{k+1'}, v\_{k'}, q\_{k'}, \lambda\_{k+1'}, \mu\_{k+1}) = 0. \tag{11}$$

Assuming the generalized coordinates *qk* and velocities *vk* at the time instance *k* to be known and given the input *uk*<sup>+</sup><sup>1</sup> at the time instance *k* + 1, Equation (10) is solved for *qk*<sup>+</sup><sup>1</sup> and *λk*+<sup>1</sup> by substituting *gd*<sup>1</sup> in *gd*2, and applying a Newton-Raphson-based iterative algorithm with iteration gradient *JNR*:

$$J\_{NR} = \begin{bmatrix} \mathbb{K}\_t + \beta \mathbb{C}\_t + \gamma \mathcal{M}\_{\mathbb{H}} & J^T \\ J & \mathbf{0}\_{\lambda, \lambda} \end{bmatrix}. \tag{12}$$

Here, *Kt*, *Ct* and *Mt* are the tangent stiffness, damping and mass matrices obtained from the partial derivatives of the continuous *g*<sup>2</sup> equations in Equation (8) evaluated at time step *k* + 1:

$$K\_{\mathbf{f}} = \left. \frac{\partial \mathcal{G}\_2}{\partial q} \right|\_{k+1}; \qquad \qquad \mathsf{C}\_{\mathbf{f}} = \left. \frac{\partial \mathcal{G}\_2}{\partial v} \right|\_{k+1}; \qquad \qquad \qquad M\_{\mathbf{f}} = \left. \frac{\partial \mathcal{G}\_2}{\partial v} \right|\_{k+1}, \tag{13}$$

and *β* and *γ* are matrix coefficients function of the defined integration rule, which are given for the BDF-1 scheme by:

$$\beta = \frac{\partial v\_{k+1}}{\partial q\_{k+1}} = \frac{\partial \psi\_{k+1}}{\partial v\_{k+1}} = \frac{1}{h} I\_q; \qquad \qquad \gamma = \frac{\partial \psi\_{k+1}}{\partial q\_{k+1}} = \frac{1}{h^2} I\_q; \tag{14}$$

$$\frac{\partial \upsilon\_{k+1}}{\partial q\_k} = \frac{\partial \upsilon\_{k+1}}{\partial v\_k} = -\beta; \qquad \qquad \qquad \frac{\partial \upsilon\_{k+1}}{\partial q\_k} = -\gamma. \tag{15}$$

This time integration scheme will be exploited in the following section to set up a suitable solver structure for use in an ODE-base estimation framework.

#### **3. An Explicit Linearized Approximation for Use of the Multibody Model in State-Estimation**

The aim of this work is to combine MB models with a Kalman filter-based estimator to concurrently estimate the system states and unknown forces of a mechanism. These presented estimators, as will be discussed in Section 4, require the linearized time-discrete model matrices. In this section, a new approach to linearize the EOMs of a MB model starting from an ID-DAE form is presented. Subsequently, the linearized system matrices are analytically assembled.

By introducing the state vector *<sup>x</sup>* <sup>∈</sup> <sup>R</sup>*nx* for a time step *<sup>k</sup>*

$$\mathbf{x}\_k = \begin{bmatrix} v\_k & q\_k & \lambda\_k \end{bmatrix}^T \tag{16}$$

the ID-DAE form of the EOMs in Equation (11) can be re-written as:

$$g\_d(\mathbf{x}\_{k+1}, \mathbf{x}\_k, \boldsymbol{\mu}\_{k+1}) = \mathbf{0}\_{\mathbf{x}}.\tag{17}$$

In this work we assume the function *gd* of Equation (17) to be continuously differentiable in all its variables. Therefore, an explicit discrete function *fd* locally exists by

applying the implicit function theorem. Through a first order Taylor expansion, *gd* can be approximated as

$$\mathbf{g}\_d^0 + \frac{\partial \mathbf{g}\_d}{\partial \mathbf{x}\_{k+1}} \Big|\_{0} \left( \mathbf{x}\_{k+1} - \mathbf{x}\_{k+1}^0 \right) + \frac{\partial \mathbf{g}\_d}{\partial \mathbf{x}\_k} \Big|\_{0} \left( \mathbf{x}\_k - \mathbf{x}\_k^0 \right) + \dots$$

$$\frac{\partial \mathbf{g}\_d}{\partial \mathbf{u}\_{k+1}} \Big|\_{0} \left( \mathbf{u}\_{k+1} - \mathbf{u}\_{k+1}^0 \right) + \mathcal{O}(\mathbf{x}\_{k+1}, \mathbf{x}\_k, \mathbf{u}\_{k+1}) = \mathbf{0}\_{\mathbf{x}};\tag{18}$$

where (*x*<sup>0</sup> *k*+1,*x*<sup>0</sup> *<sup>k</sup>* ,*u*<sup>0</sup> *<sup>k</sup>*+1) represents the linearization set point while *<sup>g</sup>*<sup>0</sup> *<sup>d</sup>* = *gd*(*x*<sup>0</sup> *k*+1,*x*<sup>0</sup> *<sup>k</sup>* ,*u*<sup>0</sup> *<sup>k</sup>*+1) for convenience of notation. Manipulating Equation (18) and neglecting the higher order terms, it can be made explicit as

$$\mathbf{x}\_{k+1} = \mathbf{x}\_{k+1}^{0} - \left[\frac{\partial \mathbf{g}\_{d}}{\partial \mathbf{x}\_{k+1}}\Big|\_{0}\right]^{-1} \left(\mathbf{g}\_{d}^{0} + \frac{\partial \mathbf{g}\_{d}}{\partial \mathbf{x}\_{k}}\Big|\_{0} (\mathbf{x}\_{k} - \mathbf{x}\_{k}^{0}) + \frac{\partial \mathbf{g}\_{d}}{\partial \mathbf{u}\_{k+1}}\Big|\_{0} (\boldsymbol{u}\_{k+1} - \boldsymbol{u}\_{k+1}^{0})\right). \tag{19}$$

In compact form Equation (19) becomes:

$$\mathbf{x}\_{k+1} = f\_d(\mathbf{x}\_k, \boldsymbol{\mu}\_{k+1}) = \mathbf{x}\_{k+1}^0 + f\_d^0(\mathbf{x}\_k, \boldsymbol{\mu}\_{k+1}) + A\_{k+1}^0(\mathbf{x}\_k - \mathbf{x}\_k^0) + B\_{k+1}^0(\boldsymbol{\mu}\_{k+1} - \boldsymbol{\mu}\_{k+1}^0). \tag{20}$$

*A*0 *<sup>k</sup>*+<sup>1</sup> <sup>=</sup> *<sup>∂</sup> fd ∂x* 0 *<sup>k</sup>*+<sup>1</sup> and *<sup>B</sup>*<sup>0</sup> *<sup>k</sup>*+<sup>1</sup> <sup>=</sup> *<sup>∂</sup> fd ∂u* 0 *<sup>k</sup>*+<sup>1</sup> are the linearized system and input matrices around the linearization set point.

Starting from Equation (19) and differentiating Equation (10), the Jacobians for the backward Euler implicit time-integrator can be computed as:

$$G\_{\mathbf{X}k+1} = \frac{\partial \mathcal{G}\_d}{\partial \mathbf{x}\_{k+1}} = \begin{bmatrix} \frac{\partial \mathcal{G}\_{d1}}{\partial v\_{k+1}} & \frac{\partial \mathcal{G}\_{d1}}{\partial q\_{k+1}} & \frac{\partial \mathcal{G}\_{d1}}{\partial \lambda\_{k+1}} \\ \frac{\partial \mathcal{G}\_{d1}}{\partial v\_{k+1}} & \frac{\partial \mathcal{G}\_{d2}}{\partial q\_{k+1}} & \frac{\partial \mathcal{G}\_{d2}}{\partial \lambda\_{k+1}} \\ \frac{\partial \mathcal{G}\_{d3}}{\partial v\_{k+1}} & \frac{\partial \mathcal{G}\_{d3}}{\partial q\_{k+1}} & \frac{\partial \mathcal{G}\_{d3}}{\partial \lambda\_{k+1}} \end{bmatrix} = \begin{bmatrix} I\_{\upsilon} & -\beta & 0\_{\upsilon,\lambda} \\ 0\_{q,\upsilon} & \gamma M\_{t} + \beta \mathcal{C}\_{t} + K\_{t} & J^{\top} \\ 0\_{\lambda,\upsilon} & J & 0\_{\lambda,\lambda} \end{bmatrix};\tag{21}$$

$$\mathbf{G}\_{\mathbf{x}\_{k}} = \frac{\partial \mathbf{g}\_{d}}{\partial \mathbf{x}\_{k}} = \begin{bmatrix} \frac{\partial \mathbf{\mathcal{X}}\_{d1}}{\partial \mathbf{v}\_{k}} & \frac{\partial \mathbf{\mathcal{X}}\_{d1}}{\partial \mathbf{q}\_{k}} & \frac{\partial \mathbf{\mathcal{X}}\_{d1}}{\partial \lambda\_{k}} \\ \frac{\partial \mathbf{\mathcal{X}}\_{d2}}{\partial \mathbf{v}\_{k}} & \frac{\partial \mathbf{\mathcal{X}}\_{d2}}{\partial \mathbf{q}\_{k}} & \frac{\partial \mathbf{\mathcal{X}}\_{d2}}{\partial \lambda\_{k}} \\ \frac{\partial \mathbf{\mathcal{X}}\_{d3}}{\partial \mathbf{v}\_{k}} & \frac{\partial \mathbf{\mathcal{X}}\_{d3}}{\partial \mathbf{q}\_{k}} & \frac{\partial \mathbf{\mathcal{X}}\_{d3}}{\partial \lambda\_{k}} \end{bmatrix} = \begin{bmatrix} 0\_{\mathbf{v},\mathbf{v}} & \beta & 0\_{\mathbf{v},\lambda} \\ -\beta \mathbf{M}\_{t} & -\gamma \mathbf{M}\_{t} - \beta \mathbf{C}\_{t} & 0\_{\mathbf{q},\lambda} \\ 0\_{\lambda,\mathbf{v}} & 0\_{\lambda,\mathbf{q}} & 0\_{\lambda,\lambda} \end{bmatrix};\tag{22}$$

$$G\_{\mathfrak{U}\_{k+1}} = \frac{\partial \mathcal{g}\_d}{\partial u\_{k+1}} = \begin{bmatrix} \frac{\partial \mathcal{g}\_{d1}}{\partial u\_{k+1}}\\ \frac{\partial \mathcal{g}\_{d2}}{\partial u\_{k+1}}\\ \frac{\partial \mathcal{g}\_{d3}}{\partial u\_{k+1}} \end{bmatrix} = \begin{bmatrix} \mathcal{O}\_{\upsilon,u} \\ \mathcal{U}\_{\mathfrak{f}} \\ \mathcal{O}\_{\lambda,u} \end{bmatrix} \tag{2.3}$$

where *Ut* represents the tangent input matrix of Equation (7).

The linearized system and input matrices can be computed for any working point at the time step *k* + 1 as:

$$A\_{k+1} = -G\_{\chi\_{k+1}}^{-1} G\_{\chi\_k};\tag{24}$$

$$B\_{k+1} = -G\_{x\_{k+1}}^{-1} G\_{\mu\_{k+1}}.\tag{25}$$

These equations enable the evaluation of the linearized time-discretized explicit description of the EOMs suitable for estimation methods such as the extended Kalman filter. Their deployment of this estimation scheme is discussed in the next section.

It is important to note that the invertibility of the matrix *Gxk*<sup>+</sup><sup>1</sup> is guaranteed thanks to the choice of BDF-1 differentiation scheme and that in contrast, it would not be possible. For completeness, the limitations of a forward differentiation scheme is demonstrated in the Appendix A.

#### **4. State-Input Estimation for MB Models**

The augmented discrete extended Kalman filter (ADE-KF) tailored for MB models is discussed in this section. In this section we discuss all the required components to set up a Kalman filter for assimilating the different multibody states and unknown inputs. More in details, in Section 4.1, the general form of the discrete-time system and measurement model equations are summarized. In Section 4.2, the measurement equations are augmented with the constraint equations *φ*, to deal with the constrained nature of the MB EOMs, leading to a constrained estimation problem. Moreover, in Section 4.3 the adopted approach to compute the linearized measurement matrices *C* and *D* are presented since they are required in the estimation framework and not directly available. Finally, in Section 4.4 the ADE-KF is assembled to deal with the estimation of states and unknown inputs, and the Kalman filter steps tailored for MB models are reported.

#### *4.1. Model and Measurement Equations with Uncertainty*

The system of EOMs in Equation (4) described in the previous section are assumed as deterministic. However, in practice, various noise sources will disturb the behavior of the system. For the dynamic model equations, the process noise vector *ν*∗ *<sup>x</sup>*,*k*+<sup>1</sup> influences the response:

$$g\_d(\mathbf{x}\_{k+1'}, \mathbf{x}\_{k'} \mathbf{u}\_{k+1}) = \vec{v}\_{\mathbf{x}, k+1'} \tag{26}$$

where *ν*˜*x*,*k*+<sup>1</sup> is associated with the noise term *νx*,*k*+<sup>1</sup> described by the following equation:

$$\mathbf{x}\_{k+1} = \mathbf{x}\_{k+1}^\* + \boldsymbol{\nu}\_{\mathbf{x},k+1}.\tag{27}$$

Here, *x*∗ *<sup>k</sup>*+<sup>1</sup> is the deterministically varying state vector while *νx*,*k*+<sup>1</sup> is a zero-mean noise term with a (possibly time-varying) covariance *Qk*.

The estimation framework relies on combining the prior model information with measurements *<sup>y</sup>* <sup>∈</sup> <sup>R</sup>*ny* on the physical process. To compare the prediction of the model with these measurements, a set of measurement equations *h* are necessary which are affected by measurement noise *νy*,*k*<sup>+</sup>1:

$$y = h(\dot{\upsilon}\_{k+1}, \chi\_{k+1}, \mu\_{k+1}) + \upsilon\_{\circ, k+1}.\tag{28}$$

where again *νy*,*k*+<sup>1</sup> is assumed to be zero-mean with a (time-varying) covariance *Rk*. Similar to the model measurement equations *<sup>y</sup>*, the concept of virtual sensor (VS) *yvs* <sup>∈</sup> <sup>R</sup>*nyvs* is introduced:

$$y\_{\text{vs}} = h\_{\text{vs}}(\dot{v}\_{k+1}, \mathbf{x}\_{k+1}, u\_{k+1}) + \boldsymbol{\upsilon}\_{\text{vs}}.\tag{29}$$

A virtual sensor represents model-based information; more specifically, it is a physical quantity that can be expressed as a function of the system variables, such as joint forces, body motion trajectories (e.g., linear and angular positions, velocities, and accelerations) and body strains/stresses. A VS equation can be treated similarly to a measurement equation but evaluated after the a posteriori Kalman filter step and hence to exploited in the estimation process itself.

#### *4.2. The Augmented Constraint Measurement Equations*

In [23] different approaches have been proposed to deal with constrained state estimation, although the authors deem it impossible to give a general guideline for constrained filtering because each individual problem is unique and dependent on how the constraints are handled.

In this work, the combination of hard and soft constraint methods is considered [23], where the constraint measurement equations are implemented in the Kalman filter scheme by augmenting the measurement equations *y* of Equation (28) with the constraint equations:

$$\Psi = \begin{cases} h(\dot{\nu}\_{k+1}, \mathbf{x}\_{k+1}, \mathbf{u}\_{k+1}) + \nu\_{\mathcal{Y}, k+1} \\ \phi(q\_{k+1}) + \nu\_{\phi, k+1} \end{cases} . \tag{30}$$

Here, *νφ*,*k*+<sup>1</sup> is a small zero-mean noise term added to the idealized constraint equations *φ*(*qk*<sup>+</sup>1) if small constraints violation is allowed (soft constraints method). In the case *νφ*,*k*+<sup>1</sup> is assumed to be zero, the constraint equations are treated as perfect measurements (hard constraint method).

#### *4.3. An Efficient Strategy for the Measurement Sensitivities Computation*

To evaluate the EKF equations at each filter step, the linearized measurement sensitivity matrices *Ck*<sup>+</sup><sup>1</sup> and *Dk*<sup>+</sup><sup>1</sup> are required and they are defined as:

$$\mathbf{C}\_{k+1} = \frac{\partial \overline{y}}{\partial \mathbf{x}}\Big|\_{k+1} = \begin{bmatrix} \frac{dy}{d\overline{x}}\\ \frac{d\Phi}{d\mathbf{x}} \end{bmatrix}\_{k+1} = \begin{bmatrix} \frac{dy}{d\overline{v}} & \frac{dy}{d\overline{q}} & \frac{dy}{d\overline{\Lambda}}\\ 0\_{\lambda,v} & J & 0 \end{bmatrix}\_{k+1};\tag{31}$$

$$D\_{k+1} = \frac{\partial \bar{y}}{\partial u}\Big|\_{k+1} = \begin{bmatrix} \frac{dy}{du} \\ 0\_{\lambda,u} \end{bmatrix}\_{k+1}.\tag{32}$$

Generally, the measurement equations can be expressed as a function of the generalized accelerations, velocities, positions, Lagrange multipliers, and inputs, Equation (30).

However, due to the state-space description presented in Section 3, the explicit relationship of the sensor equations with respect to the generalized accelerations is lost. More in particular, when considering acceleration sensors, their sensitivity with respect to the generalized accelerations is non zero, thus it must be indirectly included. Acceleration sensors are really common in the mechanical engineering community due to their noninvasive and simple installation requirements. Within the estimation framework, they can be used either in form of measurements or in form of VSs.

Similarly, the sensitivity of the measurement equations with respect to the system inputs are not always available or nontrivial to obtain.

Therefore, an approach to explicitly obtain these dependencies is here derived by means of the acceleration level constraints.

Introducing the auxiliary variables *<sup>z</sup>* <sup>∈</sup> <sup>R</sup>3*nq*+*n<sup>λ</sup>*

$$z = \begin{bmatrix} \dot{q}^T & \dot{q}^T & q^T & \lambda^T \end{bmatrix}^T = \begin{bmatrix} \dot{v}^T & v^T & q^T & \lambda^T \end{bmatrix}^T,\tag{33}$$

for a generic measurement sensor *y*, the sensitivities *<sup>∂</sup><sup>y</sup> <sup>∂</sup><sup>z</sup>* with respect to the auxiliary variable *z* can be analytically computed starting from their fundamental equations.

$$\frac{\partial \underline{y}}{\partial z} = \begin{bmatrix} \frac{\partial y}{\partial v} & \frac{\partial y}{\partial v} & \frac{\partial y}{\partial q} & \frac{\partial y}{\partial \lambda} \end{bmatrix}. \tag{3.4}$$

Applying the chain rule, the non-augmented linear measurement matrix *dy dx* for a generic time step can be written as

$$\frac{dy}{dx} = \frac{\partial y}{\partial z}\frac{dz}{dx}.\tag{35}$$

And the full derivative *dz dx* can be expressed as

$$\frac{dz}{dx} = \begin{bmatrix} \frac{\partial \bar{x}}{\partial v} & \frac{\partial \bar{x}}{\partial q} & \frac{\partial \bar{x}}{\partial \lambda} \\ \frac{\partial v}{\partial v} & \frac{\partial v}{\partial q} & \frac{\partial v}{\partial \lambda} \\ \frac{\partial q}{\partial v} & \frac{\partial q}{\partial q} & \frac{\partial q}{\partial \lambda} \end{bmatrix} = \begin{bmatrix} \frac{\partial \psi}{\partial v} & \frac{\partial \psi}{\partial q} & \frac{\partial \psi}{\partial \lambda} \\ I\_v & 0\_{v,q} & 0\_{v,\lambda} \\ 0\_{q,v} & I\_q & 0\_{q,\lambda} \\ \frac{\partial \lambda}{\partial v} & \frac{\partial \bar{\lambda}}{\partial q} & I\_\lambda \end{bmatrix}. \tag{36}$$

To analytically compute the unknown terms of Equation (36), we introduce the acceleration level constraints *φ*¨ defined as the second time derivative of the position level constraints. Starting from the velocity level constraints *φ*˙:

$$
\dot{\phi} = \frac{\partial \phi}{\partial q} \frac{dq}{dt} = Jv,\tag{37}
$$

the acceleration level constraint equations are obtained as follows:

$$\ddot{\boldsymbol{\phi}} = \boldsymbol{J}\boldsymbol{\upsilon} + \boldsymbol{J}\dot{\boldsymbol{\upsilon}} = \sum\_{i,j=1}^{n\_q} \boldsymbol{\upsilon}\_i \frac{\partial^2 \boldsymbol{\phi}}{\partial [q\_i, q\_j]} \boldsymbol{\upsilon}\_j + \boldsymbol{J}\dot{\boldsymbol{\upsilon}} = \boldsymbol{0}\_\lambda. \tag{38}$$

By means of the continuous dynamic equations *g*<sup>2</sup> of Equation (8) and the acceleration level constraint of Equation (38), a new set of equations *<sup>p</sup>* <sup>∈</sup> <sup>R</sup>*nq*+*n<sup>λ</sup>* can be constructed:

$$p(z,u) = \begin{cases} p\_1 = M(q)\dot{\upsilon} + f\_{nl}(\upsilon, q\_\prime u) + f^T \lambda = 0\_\emptyset \\ p\_2 = \sum\_{i,j=1}^{n\_\emptyset} \upsilon\_i \frac{\partial^2 \phi}{\partial [q\_{i\prime} q\_j]} \upsilon\_j + f \dot{\upsilon} = 0\_\lambda \end{cases} \tag{39}$$

Equations (39) represent the governing set of equations that implicitly determines the relation between the different coordinates *v*, *q*, *v*˙ and the Lagrange multipliers *λ*.

Therefore, the unknown terms of Equation (36) can be computed as

$$
\begin{split}
\begin{bmatrix}
\frac{\partial\psi}{\partial\upsilon} & \frac{\partial\psi}{\partial q} & \frac{\partial\psi}{\partial\lambda} \\
\frac{\partial\lambda}{\partial\upsilon} & \frac{\partial\lambda}{\partial q} & \frac{\partial\lambda}{\partial\lambda}
\end{bmatrix} &= -\begin{bmatrix}
\frac{\partial p}{\partial\upsilon} & \frac{\partial p}{\partial\lambda}
\end{bmatrix}^{-1} \begin{bmatrix}
\frac{\partial p}{\partial\upsilon} & \frac{\partial p}{\partial q} & \frac{\partial p}{\partial\lambda}
\end{bmatrix} = \\
& -\begin{bmatrix}
\mathcal{M}(q) & f^{T} \\
f & 0\_{\lambda,\lambda}
\end{bmatrix}^{-1} \begin{bmatrix}
\mathcal{C}\_{t} & \mathcal{K}\_{t} & f^{T} \\
2\sum\_{j=1}^{n\_{\upsilon}} \frac{\partial^{2}\phi}{\partial[q\_{j}q\_{j}]} \upsilon\_{j} & \sum\_{j=1}^{n\_{\upsilon}} \frac{\partial^{2}\phi}{\partial[q\_{j}q\_{j}]} \upsilon\_{j} & 0\_{\lambda,\lambda}
\end{bmatrix}
\end{split}
\tag{40}
$$

where the third order partial derivative of the constraint equations *φ*(*q*) with respect to the generalized coordinates *q* is zero for FNCF. Another important ingredient to fulfill the Kalman filter requirements is the measurement input matrix *dy du* of Equation (32). However, this matrix is not directly available and it is computed similarly to Equation (35) but with respect to input *u*:

$$\frac{d\underline{y}}{d\underline{u}} = \frac{\partial y}{\partial z}\frac{dz}{d\underline{u}} + \frac{\partial \underline{y}}{\partial \underline{u}}.\tag{41}$$

Here, the full derivative *dz du* represents how the vector *z* varies when the input *u* is perturbed obtaining:

$$\frac{dz}{du} = \begin{bmatrix} \frac{\partial \psi}{\partial u} & \frac{\partial \psi}{\partial u} & \frac{\partial q}{\partial u} & \frac{\partial \lambda}{\partial u} \end{bmatrix} = \begin{bmatrix} \frac{\partial \psi}{\partial u} & 0\_{\upsilon,u} & 0\_{q,u} & \frac{\partial \lambda}{\partial u} \end{bmatrix}.\tag{42}$$

From Equation (42) can be seen that only the sensitivities of the acceleration and Lagrange multipliers are non-zero terms. A second order system is fully defined by the position coordinates, velocity coordinates, and time. An external force only influences the force balance of that system and thus the acceleration coordinates and Lagrangian multipliers while only through time-integration the velocity and position coordinates. However, these derivatives are evaluated at a single time instance. Therefore, in Equation (36), the acceleration coordinates and Lagrangian multipliers depend on the position and velocity coordinates, but not the other way around: the position and velocity coordinates do not depend on the acceleration coordinates, Lagrangian multipliers, or the external forces.

Subsequently, the remaining terms of Equation (42) can be computed as

$$
\begin{bmatrix}
\frac{\partial \psi}{\partial \lambda} \\
\frac{\partial \lambda}{\partial \mu}
\end{bmatrix} = -\begin{bmatrix}
\frac{\partial p}{\partial \psi} & \frac{\partial p}{\partial \lambda}
\end{bmatrix}^{-1} \frac{\partial p}{\partial \mu} = -\begin{bmatrix}
M(q) & J^T \\
J & 0\_{\lambda,\lambda}
\end{bmatrix}^{-1} \begin{bmatrix}
\mathcal{U}\_{\mathbb{I}} \\
0\_{\lambda,\mu}
\end{bmatrix}.\tag{43}
$$

In Figure 1 a schematic summary of the followed steps required for the computation of the system and measurement matrices is reported.

**Figure 1.** Block diagram representation of the system and measurement matrices computation for a generic integration step.

#### *4.4. Augmented Discrete Extended Kalman Filter*

The goal of this work is to estimate the unknown input forces, often referred to as disturbances, *<sup>d</sup>* <sup>∈</sup> <sup>R</sup>*nd* , concurrently with the system states *<sup>x</sup>* through the augmented state vector *<sup>x</sup>*˜ <sup>∈</sup> <sup>R</sup>*nx*+*nd* defined as:

$$\mathfrak{x}\_{k+1} = \begin{bmatrix} \mathfrak{x}\_{k+1} \\ d\_{k+1} \end{bmatrix}. \tag{44}$$

As the estimation algorithm also needs a model for the unknown inputs, a random walk model is associated with the unknown input:

$$d\_{k+1} = d\_k + \nu\_{d,k};\tag{45}$$

with *νd*,*<sup>k</sup>* also being zero-mean, random noise with covariance matrix *Qd*,*k*.

In this work, the random walk model is chosen for the unknown disturbance. This approach is chosen as in general no prior information on the input is assumed to be known. However, if useful information is available, e.g., periodicity of the input, other input models can be employed, as in [24].

The augmented discrete-time state-space system of equations can be written combining Equations (26) and (45):

$$\mathfrak{g}\_d(\mathfrak{x}\_{k+1}, \mathfrak{x}\_k, \mathfrak{u}\_{k+1}) = \begin{cases} \mathfrak{g}\_d(\mathfrak{x}\_{k+1}, \mathfrak{x}\_{k'} \mathfrak{u}\_{k+1}) = \mathfrak{v}\_{\mathfrak{x},k} \\ d\_{k+1} - d\_k = \nu\_{d,k} \end{cases} . \tag{46}$$

The linearized system matrix F refers to the augmented system of equations Equation (46) at the time step *k* + 1 and it is assembled as

$$\mathcal{F} = \frac{\partial \overline{f}\_d}{\partial \overline{\mathfrak{X}}} \Big|\_{k+1} = \begin{bmatrix} A\_{k+1} & B\_{k+1}^d \\ 0\_{\mathbf{x},d} & I\_d \end{bmatrix};\tag{47}$$

where ˜ *fd* is the augmented explicit ODEs associated with Equation (46), is obtained from Equation (24), while *B<sup>d</sup> <sup>k</sup>*+<sup>1</sup> is the linear disturbance matrix obtained similarly to *Bk*<sup>+</sup><sup>1</sup> in Equation (25) but with respect to the unknown input *d*.

Subsequently, the linearized augmented measurement matrix H associated with the augmented state vector *x*˜ is assembled as:

$$\mathcal{H} = \frac{\partial \vec{y}}{\partial \vec{x}}\Big|\_{k+1} = \begin{bmatrix} \mathbb{C}\_{k+1} & D\_{k+1}^d \end{bmatrix} \tag{48}$$

where the matrix *D<sup>d</sup> <sup>k</sup>*+<sup>1</sup> is computed in a similar fashion as for *Dk*<sup>+</sup><sup>1</sup> in Equation (32) but with respect to the unknown input *d*.

Starting from these model, measurement and tangent matrices, an extended Kalman filter (EKF) procedure can be set up in order to estimate both the states and the unknown input forces. The application of this estimation approach is discussed in the following section.

#### *4.5. The Adopted Extended Kalman Filter Scheme*

In this work, we employ an extended Kalman filter (EKF) [25] to perform the stateestimation. With the various model and measurement equations, and their respective derivatives, defined for a MB model in the previous sections this EKF can be run efficiently. As all terms have been defined analytically it is not necessary to resort to numerical derivatives, which allows for an effective application of the EKF. Several researchers have shown the applicability of the estimators even for the strongly nonlinear case of MB systems [9,12,26].

In general, we can group the EKF scheme in two main steps: the a-priori and a posteriori steps.

• *A-priori step*: Assuming that the augmented states *x*˜ + *<sup>k</sup>* at the previous filter step and the input *uk*<sup>+</sup><sup>1</sup> are known, the a-priori state prediction *x*˜ − *<sup>k</sup>*+<sup>1</sup> and generalized accelerations *v*˙ − *<sup>k</sup>*+<sup>1</sup> can be computed solving the ID-DAEs of Equation (17):

$$\mathbf{g}\_d(\tilde{\mathbf{x}}\_{k+1'}^-, \tilde{\mathbf{x}}\_{k'}^+, \boldsymbol{\mu}\_{k+1}) = \mathbf{0}\_{\mathbf{x}}.\tag{49}$$

Knowing the estimated state covariance matrix <sup>P</sup><sup>+</sup> *<sup>k</sup>* for the previous timestep, the apriori covariance at the current time (*k* +1) step can be approximated from Equation (47) as

$$\mathcal{P}\_{k+1}^{-} = \mathcal{F}\mathcal{P}\_{k}^{+}\mathcal{F}^{T} + \bar{\mathcal{Q}}\_{k}.\tag{50}$$

where *Q*˜ *<sup>k</sup>* is the process noise matrix of the augmented state-space model. The predicted measurement *y*˜ − *<sup>k</sup>*+<sup>1</sup> can then be evaluated from Equation (30) as:

$$
\mathfrak{Y}\_{k+1}^- = \mathfrak{Y}(\mathfrak{v}\_{k+1'}^-, \mathfrak{x}\_{k+1'}^-, \mathfrak{u}\_{k+1}).\tag{51}
$$

The Kalman filter gain K allows achieving a desireable trade-off between the confidence in the model and the available measurements, and can be evaluated as:

$$\mathcal{K}\_{k+1} = \mathcal{P}\_{k+1}^{-} \mathcal{H}^{T} (\mathcal{H} \mathcal{P}\_{k+1}^{-} \mathcal{H}^{T} + \mathbb{R}\_{k})^{-1},\tag{52}$$

where <sup>H</sup> is obtained from Equation (48) and *<sup>R</sup>*˜ *<sup>k</sup>* is the measurement covariance matrix of the augmented measurement equations.

• *A-posteriori step*: When the real measurement *y*∗ *<sup>k</sup>*+<sup>1</sup> becomes available together with the predicted measurement *y*˜ − *<sup>k</sup>*+1, the a posteriori state vector *x*˜ + *<sup>k</sup>*+<sup>1</sup> is obtained as:

$$
\mathfrak{x}\_{k+1}^{+} = \mathfrak{x}\_{k+1}^{-} + \mathcal{K}\_{k+1} (y\_{k+1}^{\*} - \mathfrak{y}\_{k+1}^{-}) \tag{53}
$$

The inclusion of the actual measurements also affects the posterior covariance matrix P+ *<sup>k</sup>*+<sup>1</sup> and can be evaluated as:

$$
\mathcal{P}\_{k+1}^{+} = (\mathcal{I}\_{\mathbf{x}} - \mathcal{K}\_{k+1} \mathcal{H}) \mathcal{P}\_{k+1}^{-}. \tag{54}
$$

In Figure 2 a block diagram scheme summarizing a generic *k*th step of the recursive ADE-KF is shown.

**Figure 2.** Block diagram representation of the recursive ADE-KF scheme for a generic filter step.

With this scheme the entire state-input estimation can be performed for an arbitrary (flexible) multibody model. In the following section we will validate it on an experimental setup.

In practical applications the performance of the estimation scheme will strongly depend on the tuning of the model covariance *Q*˜ and measurement covariance *R*˜. Unfortunately this tuning is also highly case dependent, which makes it difficult to set up general guidelines. For the particular validation case considered in this work, a detailed discussion on a possible tuning strategy is presented in the following section.

#### **5. Validation: Joint State-Input Estimation**

In this section, the presented joint state-input estimation methodology is experimentally validated on a slider-crank mechanism.

#### *5.1. The Slider-Crank System*

The slider-crank system in Figure 3 is a mechanism widely used in industry to transform a rotational motion in a linear motion as for instance in pick and place operations or in car engines.

**Figure 3.** The slider-crank: experimental setup.

Figure 4 represents the multibody model of the experimental setup shown in Figure 3 consisting of 14 bodies: base block (BB), motor housing (*MH*), motor rotor (*MR*), motor support (*MS*), crank (*C*), crank shaft (*CSh*), crank support (*CS*), connecting rod (*CR*), crankrod bearing (*BC*−*CR*), slider (*S*), rod-slider bearing (*BCR*−*S*), slider accelerometer (*SA*), track rail (*TR*) and track support (*TS*).

**Figure 4.** The slider-crank: multibody model.

The mechanical properties of the various bodies are listed in Table 1.


**Table 1.** Mechanical properties expressed with respect to the center of gravity of each individual body.

Even though the presented slider-crank mechanism is an academic example it combines several challenging phenomena such as non-linear kinematics and complex slidertrack interaction phenomena. The various bodies are connected trough fixed, revolute, spherical, and universal joints to allow the desired kinematics. The slider and track bodies are connected through a contact stiffness **k***<sup>c</sup>* along the perpendicular directions to the sliding trajectory. The Pacejka friction model [27] defines the friction coefficient *μ* as a function of the sliding velocity Δ*v*:

$$\mu = \mathbf{d} \sin \left[ \mathfrak{c} \tan^{-1} \left( \mathbf{b} \Delta v - \mathbf{e} \left( \mathbf{b} \Delta v - \tan^{-1} (\mathbf{b} \Delta v) \right) \right) \right],\tag{55}$$

where the friction force *f <sup>f</sup>* is evaluated as

$$f\_f = \mu f\_\mathbb{n} = \mu(\mathbf{k}\_\mathbb{c}\delta + \mathbf{c}\_\mathbb{c}\dot{\delta}),\tag{56}$$

where *fn* is the resultant normal force acting on the slider and, *δ* is the local compliance between the slider and track rail interfaces projected onto the normal direction to the sliding direction. The adopted (identified) Pacejka friction model parameters are listed in Table 2.

**Table 2.** Pacejka friction model parameters.


The system motion is driven by a brushless servomotor "MAC3000"' with integrated controller MAC00-B4 from JVL (www.jvl.dk, accessed on 1 March 2021). It includes a motor encoder sensor allowing the measurement of the rotation angle and velocity of the motor shaft, indicated by *θ* and ˙ *θ* respectively.As can be seen in Figures 3 and 4, the slider is equipped with a mono-axial MEMS accelerometer (3711D1FB200G) from PCB (www.pcb.com) to measure its translational acceleration along the sliding direction indicated by *Y*¨. The entire system is controlled via the motor using a closed-loop PID controller targeting a desired rotational motor velocity while adapting the determined motor torque *T*.
