**1. Introduction**

In many applications, it is required to acquire data in order to analyze the performance of a particular machine, to make decisions through control strategies or to gather data for durability purposes. Most of the time, it is possible to get all the data through sensors. However, there are particular cases where a sensor cannot be installed due to economical or technical issues. In these situations, data acquisition relies on estimations.

Kalman filter [1] stands out as one of the most used estimation techniques. It combines a model describing the system with sensor measurements. The estimations are based on the statistical properties of the desired variables conditioned on the sensors measurements [2]. Although the Kalman filter was designed for linear systems, several approaches have been developed in the literature extending its use to non-linear models. Such is the case of the extended Kalman filter (EKF) or unscented Kalman filter (UKF) [2]. These approaches introduced the Kalman filter to real problems, which are predominantly non-linear.

Similarly, the interest for mulibody models has grown in the recent years in several fields of the industry, such as automotive, biomechanics or machinery. Through multibody dynamics, a system can be modeled with high accuracy [3], at expenses of a moderate computational cost. Recently, multibody models have started to be used in combination with Kalman filters for estimation. Such is the case of virtual sensing in automotive applications [4]. However, the assembly between a multibody model and a Kalman filter is not trivial due to the high non-linearities of multibody models.

The first approach presented in the literature is [5], where an EKF was combined with a simple mechanism. However, the complexity of the implementation and the computa-

**Citation:** Rodríguez, A.J.; Sanjurjo, E.; Pastorino, R.; Naya, M.Á. Multibody-Based Input and State Observers Using Adaptive Extended Kalman Filter. *Sensors* **2021**, *21*, 5241. https://doi.org/10.3390/s21155241

Academic Editor: Stefano Mariani

Received: 31 May 2021 Accepted: 29 July 2021 Published: 3 August 2021

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2021 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

tional cost limits its application. Several approaches have been presented later on [6–9]. In [7], an UKF based on a multibody model is proposed, reducing the complexity of the implementation. However, it entails high computational cost: the estimations are based on a set of sample points propagated through the model. Hence, it requires to simulate as many multibody models as sample points each time step. From the work presented in [8], a new Kalman filter known as error-state extended Kalman filter (errorEKF) shows accurate results while reducing the computational cost. In addition, the filter entails low coupling with the model. Thus, the implementation of any model becomes simpler. The errorEKF has been improved in more recent works, extending its estimations to inputs [9] and parameters [10].

The Kalman filter, besides being developed for linear systems, assumes that the noise of the measurements and the process (or system) are zero-mean white Gaussian with known covariance matrices [1]. These assumptions are hard to satisfy in real scenarios. The use of wrong statistics in the design of the filter can lead to large estimation errors or even a divergence in the filter [11]. In practice, these matrices are estimated through a tedious trial-and-error procedure. This is especially critical for the process noise covariance matrix (PNCM), since it is not possible to determine the accuracy of the model. Regarding the measurement noise covariance matrix (MNCM), each sensor is characterized and, hence, an initial value can be approximated.

However, even though a proper value of the noise covariance matrices is found, they remain constant reducing the robustness of the filter [10,12]. In some scenarios, the system can experiment a noticeable change during a particular maneuver. This change can affect the model accuracy and, thus, invalidate the initial noise covariances. This explains the increasing interest on developing methods for noise covariance matrices estimation [13]. These methods are usually known as Adaptive Kalman filters (AKF).

Adaptive Kalman filters can be divided in two major approaches: multiple-model based adaptive estimation (MMAE) and innovation-based adaptive estimation (IAE). Both approaches share the same concept of using the new information of the innovation sequence of the filter (difference between real and estimated measurements) [14]. MMAE methods implement a bank of several different models and compute the probability for each model to be the true system model given the measurement sequence and under the assumption that one of the models in the bank is the correct one. IAE performs the adaptation based directly on the innovation sequence, using the fact that the innovation sequence is white noise if the filter has the right values of the covariance matrices [15].

Multibody models can be computationally expensive and achieving real-time performance in practical applications is a challenge [16]. Hence, executing several models in parallel following the MMAE approach would compromise real-time performance and, thus, the viability of the solution.

Focusing on the IAE, there are many alternatives presented in the literature. In [14], an approach based on the maximum likelihood criteria (ML) is proposed to estimate both the PNCM and MNCM for inertial navigation, where Kalman filters are employed to overcome the lack of GPS signals in certain areas. The proposed approach is also compared against conventional Kalman filters. The ML aims to maximize the probability that a set of data (i.e., innovation) is observed for a given a parameter (i.e., noise covariance matrices). The ML depends on a window of past innovation data. If the sample data grows without bound, the estimate converges to the true value of the covariance matrices. As a counterpart, the ML could be biased for small sample sizes (over-trained). This issue is addressed in [17] by combining the ML with a fuzzy control in order to select only the viable satellites for the estimation, removing outlier data from the innovation sequence.

In [18], an IAE adaptive Kalman filter is also proposed for sensor fusion in INS/GPS navigation. The algorithm is equivalent to the maximum likelihood criteria. Combining the GPS with an IMU, the position can be accurately tracked, even in environments without GPS signal.

For spacecraft navigation, in [12], an AKF based on ML combined with fuzzy logic is proposed. The fuzzy logic is based on the discrepancy between the estimated covariance and the theoretical covariance bounds. The use of fuzzy logic helped to reduce the computational cost. However, the use of fuzzy logic incremented the estimation errors.

In [19], a Sage-Husa (SH) filter is combined with a Kalman filter to obtain the angular velocities of a vehicle from an inertial navigation system which uses only accelerometers. The SH algorithm is equivalent to those resulting from maximum likelihood estimation [20]. It is based on the maximum a posterior probability (MAP), which maximizes the probability of a parameter for a given set of data. It means that the likelihood is weighted by the prior information. In practice, it can be seen as applying weights in the ML estimates, giving more relevance to the most recent estimations. Sage-Husa algorithm is an alternative to the ML when there are not enough data available. However, it cannot estimate the PNCM and MNCM simultaneously without causing divergence in the estimations [21].

The SH algorithm is also used with modifications in [22] to estimate the MNCM of an UKF for state estimation in a vehicle. A similar approach is followed in [21], where a robust UKF is complemented with the SH algorithm for estimating the PNCM for autonomous underwater vehicle acoustic navigation.

Another approach for adaptive Kalman filtering is known as Variational Bayesian (VB). While ML and MAP give a fixed value for the noise covariance matrices (point estimators), the VB gives a probability density function. The advantage of this method is that it mitigates the effect of over-trained filters [23]. As an example, the VB is applied in [24] in order to improve the measurements of inertial navigation systems. It shows robustness against anomalous measurements. As a counterpart, the VB assumes that the PNCM is accurately known. Thus, it can be only used for estimating the states and the MNCM [25]. This is an important drawback since, as reflected in [26,27], the filter performance will decrease with the uncertainties of the PNCM. The VB approach is extended in [26,28] including the calculation of the predicted error covariance matrix instead of the process covariance matrix seeking for higher accuracy. In the same way, in [29], the VB approach is enhanced by its combination with a Sage-Husa algorithm for the PNCM estimation achieving satisfactory results.

The aim of this work is to explore the use of adaptive algorithms with multibody based Kalman filters, solving the actual limitation of the unknown statistics of the noise covariance matrices. For that purpose, a Kalman filter based on multibody models has to be combined with an adaptive method. This implies to study how to combine the different equations involved, performing the required modifications. Most of the research made in adaptive Kalman filtering is related to navigation applications. However, multibody models present strong non-linearities that can affect the performance of the adaptive filters. To the authors' knowledge, adaptive Kalman filtering is a technique that has never been applied to multibody-based estimation and, at the state of the art of Kalman filtering in this field, it is the next natural step to be taken.

As novelty, this work presents a state and input estimator for multibody models, which includes an adaptive methodology in order to estimate the noise covariance matrices. The lack of knowledge on the statistics of the system is an actual limitation for industrial applications of multibody-based estimation. That is the case, for example, of automotive applications, where the maneuvers are continuously changing and adjusting the noise covariance matrices following a trial-and-error procedure is not viable. Following the presented approach, this limitation can be overcome, and the use of estimation based on multibody models can be increased in real industrial applications.

#### **2. Materials and Methods**

This work continues the development of an accurate and efficient Kalman filter for multibody models. Starting from the work presented in [9], the errorEKF with force estimation is extended with adaptive methods improving its accuracy and robustness. The errorEKF with force estimation is one of the best options in terms of accuracy and

computational efficiency for multibody-based Kalman filtering. Regarding the adaptive method, whereas the Variational Bayesian can only be used for estimating the measurement noise covariance matrix, the Sage-Husa and maximum likelihood can estimate the process and measurement noise covariance matrices. The former has shown lack of stability if both matrices are estimated simultaneously. Hence, the filter of this work combines the maximum likelihood method with the errorEKF with force estimation.

The adaptive techniques used in this work are evaluated when applied to mechanisms described by multibody models. In order to present a fair comparison with the previous version of the filter, the mechanisms employed in this work are the same as in [9]: a four bar linkage (Figure 1a) and a five bar linkage (Figure 1b). The dimensions of both mechanisms are summarized in Tables 1 and 2.

(**b**) Five-bar linkage.

**Figure 1.** Mechanisms employed in this work.

**Table 1.** Properties of the four-bar linkage.


**Table 2.** Properties of the five-bar linkage.


#### *2.1. Multibody Modeling*

The input and state observer that is used in this work, the errorEKF with force estimation, can be applied to any multibody formulation and integrator [8]. This is one of its main advantages, since it reduces the complexity of implementing estimators based on multibody models (MBS). For example, in [9], an augmented Lagrangian formulation of index 3 (ALI3P) using natural coordinates is employed. Meanwhile, in [10], the multibody model is based on a semi-recursive method using relative coordinates.

In this work, the ALI3P formulation is selected following the work in [9]. First presented in [30], this formulation is widely used in multibody applications due to its efficiency and robustness [16,31–34]. The formulation is briefly described next. The reader is referred to [30,31,35] for further details. The multibody models are simulated using MBDE, a MATLAB® Open Source toolbox (See https://github.com/MBDS/mbde-matlab (accessed on 12 May 2021)).

The dynamics of a multibody system can be described by an index-3 augmented Lagrangian formulation as,

$$\mathbf{M}\ddot{\mathbf{q}} + \boldsymbol{\Phi}\_{\mathbf{q}}\mathbf{^T\boldsymbol{\kappa}}\boldsymbol{\Phi} + \boldsymbol{\Phi}\_{\mathbf{q}}\mathbf{^T\boldsymbol{\lambda}}^\* = \mathbf{Q} \tag{1}$$

being **<sup>M</sup>** the mass matrix, **.. q** the accelerations of the natural coordinates, Φ the constraints vector, Φ**<sup>q</sup>** the Jacobian matrix of the constraint equations with respect to the generalized coordinates **q**, *α* the penalty factor, **Q** the vector of generalized forces, and λ∗ the vector of Lagrange multipliers obtained through the following iteration process [35],

$$
\lambda\_{i+1}^\* = \lambda\_i^\* + a \Phi\_{i+1} \qquad i = 0, 1, 2 \dots \tag{2}
$$

where *i* is the index of the iteration.

It should be noted that this method directly includes the constraint equations at position level as a dynamical system, penalized by a large factor, *α*. The larger the penalty factor, the better the constraints will be achieved. In order to avoid numerical ill conditioning, a factor between 10<sup>7</sup> and 10<sup>9</sup> (depending on the mass of the system) gives excellent results for double arithmetic [3].

In order to integrate Equation (1), the implicit single-step trapezoidal rule has been adopted. The corresponding difference equations in velocities and accelerations can be derived as,

$$\dot{\mathbf{q}}\_{n+1} = \frac{2}{\Delta t} \mathbf{q}\_{n+1} + \dot{\mathbf{q}}\_{n}^{\*}; \ \dot{\mathbf{q}}\_{n}^{\*} = -\left(\frac{2}{\Delta t} \mathbf{q}\_{n} + \dot{\mathbf{q}}\_{n}\right) \tag{3}$$

$$\ddot{\mathbf{q}}\_{n+1} = \frac{4}{\Delta t^2} \mathbf{q}\_{n+1} + \dot{\hat{\mathbf{q}}}\_n^\*; \ \dot{\hat{\mathbf{q}}}\_n^\* = -\left(\frac{4}{\Delta t^2} \mathbf{q}\_n + \frac{4}{\Delta t} \dot{\mathbf{q}}\_n + \ddot{\mathbf{q}}\_n\right) \tag{4}$$

where Δ*t* is the time-step. Introducing the previous equations in Equation (1) leads to a nonlinear system of equations, **f**, in which the positions at time-step *n* + 1 are the unknowns. Hence,

$$\mathbf{f}(\mathbf{q}\_{n+1}) = \mathbf{0} \tag{5}$$

Since Equation (5) is a nonlinear system of algebraic equations, the Newton–Raphson iteration can be used to find a solution. The residual vector is,

$$\mathbf{f}(\mathbf{q}) = \frac{\Delta t^2}{4} (\mathbf{M}\ddot{\mathbf{q}} + \boldsymbol{\Phi}\_{\mathbf{q}}{}^T \boldsymbol{a} \boldsymbol{\Phi} + \boldsymbol{\Phi}\_{\mathbf{q}}{}^T \boldsymbol{\lambda}^\* - \mathbf{Q}) \tag{6}$$

and the approximated tangent matrix,

$$\frac{\partial \mathbf{f}(\mathbf{q})}{\partial \mathbf{q}} = \mathbf{M} + \frac{\Delta t}{2} \mathbf{C} + \frac{\Delta t^2}{4} \left( \boldsymbol{\Phi}\_{\mathbf{q}} \prescript{\mathsf{T}}{}{a} \boldsymbol{\Phi}\_{\mathbf{q}} + \bar{\mathbf{K}} \right) \tag{7}$$

where **K¯** and **C** represent the contribution of damping and elastic forces of the system.

After converging to a solution, the positions satisfy the equations of motion and the constraint conditions, Φ = **0**. However, there is no guarantee on satisfying the constraint equations at velocity, **.** <sup>Φ</sup> <sup>=</sup> **<sup>0</sup>**, and acceleration level, **..** Φ = **0**, since they were not imposed. To overcome this issue, the velocities and accelerations are projected,

$$\frac{\partial \mathbf{f}(\mathbf{q})}{\partial \mathbf{q}} \dot{\mathbf{q}} = \left[ \mathbf{M} + \frac{\Delta t}{2} \mathbf{C} + \frac{\Delta t^2}{4} \mathbf{K} \right] \ddot{\mathbf{q}} - \frac{\Delta t^2}{4} \boldsymbol{\Phi}\_{\mathbf{q}} \prescript{\mathbf{T}}{}{\alpha} \boldsymbol{\Phi}\_{t} \tag{8}$$

$$\frac{\partial \mathbf{f}(\mathbf{q})}{\partial \mathbf{q}} \ddot{\mathbf{q}} = \left[ \mathbf{M} + \frac{\Delta t}{2} \mathbf{C} + \frac{\Delta t^2}{4} \mathbf{K} \right] \ddot{\mathbf{q}} - \frac{\Delta t^2}{4} \Phi\_\mathbf{q} \mathbf{r} \left( \dot{\Phi}\_\mathbf{q} \dot{\mathbf{q}} + \dot{\Phi}\_t \right) \tag{9}$$

where ˜ **. q** and ˜ **.. q** are the velocities and accelerations obtained from the integration process, i.e., non-compliant with the constraints equations.

#### *2.2. Adaptive Error-State Extended Kalman Filter with Force Estimation (AerrorEKF-FE)*

The errorEKF with force estimation is based on indirect estimation, also known as error-state estimation. It combines the efficiency of an EKF filter with the easy integration of the dynamical system in the UKF filters [36]. The coupling between the filter and model is limited to share information regarding the states, which are the error in the kinematics of the model. The multibody model can be used without modifications and, thus, any formulation or integrator can be employed.

The diagram of Figure 2 illustrates the procedure followed by the adaptive errorEKF with force estimation for a time step *k*. The method starts integrating the multibody equations, obtaining the predicted coordinates **<sup>q</sup>***MBS*, velocities **. <sup>q</sup>***MBS* and accelerations **.. q***MBS*. These coordinates are later used to obtain the *virtual* measurements **h**(**q***MBS*, **. q***MBS*, **.. q***MBS*). Later, the equations of the filter are used to perform the estimation.

**Figure 2.** Simplified flow diagram of a time step of the adaptive errorEKF with force estimation. MBS refers to the multibody system, whose output is the predicted position, velocities and accelerations of the system. H is the observation matrix, employed to obtain the *virtual* measurements, which are compared with the measurements from the sensors. **Σˆ** *<sup>P</sup>* and **Σˆ** *<sup>S</sup>* are the PNCM and MNCM, respectively, where the superscripts <sup>−</sup> and <sup>+</sup> refer to the a priori and *a posteriori* estimations. EKF represents the application of the extended Kalman filter equations, whose outputs are the estimated errors in the predicted state vector (MBS errors). ML refers to maximum likelihood and is where the noise covariance matrices are estimated.

#### 2.2.1. State and Input Estimation

For estimating the states and inputs of the system, the state vector consists in the *error* in position, velocity and acceleration of the degrees of freedom of the mechanism, ensuring a certain level of independence within the states. Hence,

$$\mathbf{x}^{\mathsf{T}} = \left[\boldsymbol{\Delta}\mathbf{z}^{\mathsf{T}}, \boldsymbol{\Delta}\dot{\mathbf{z}}^{\mathsf{T}}, \boldsymbol{\Delta}\ddot{\mathbf{z}}^{\mathsf{T}}\right] \tag{10}$$

where <sup>Δ</sup>**z**, <sup>Δ</sup>**. <sup>z</sup>**, <sup>Δ</sup>**.. z** are the errors in position, velocity and acceleration of the independent coordinates (coincident with the degrees of freedom), respectively, such that,

$$\mathbf{z} = \mathbf{z}\_{MBS} + \Delta \mathbf{z} \tag{11}$$

$$\mathbf{z} = \mathbf{z} \tag{12}$$

$$
\dot{\mathbf{z}} = \dot{\mathbf{z}}\_{MBS} + \Delta \dot{\mathbf{z}} \tag{12}
$$

$$
\ddot{\mathbf{z}} = \ddot{\mathbf{z}}\_{MBS} + \Lambda \ddot{\mathbf{z}} \tag{13}
$$

being **<sup>z</sup>***MBS*, **. z***MBS*, **.. z***MBS* the *virtual* positions, velocities and accelerations of the degrees of freedom (i.e., the coordinates predicted by the multibody model), and **<sup>z</sup>**, **. z**, **.. z** the *real* positions, velocities and accelerations of the degrees of freedom of the mechanism. It should be noted that estimating the accelerations is closely related with the estimation of the forces, as can be seen from Equations (22)–(24).

Combining the information from the *real* and *virtual* measurements into the Kalman filter, the state of the multibody model is corrected. Thus, the expected error for the next time step is null. As a consequence, the propagation phase takes the form of,

$$
\hat{\mathbf{x}}\_k^- = \mathbf{0} \tag{14}
$$

$$\mathbf{P}\_{k}^{-} = \mathbf{f}\_{\mathbf{x}k-1} \mathbf{P}\_{k-1}^{+} \mathbf{f}\_{\mathbf{x}k-1}^{\mathrm{T}} + \hat{\mathbf{z}}^{P^{+}} \mathbf{z}^{+} \tag{15}$$

where **P** is the covariance matrix of the estimation error, **fx** is the Jacobian matrix of the transition model and **Σˆ** *<sup>P</sup>* is the estimated covariance matrix of the plant or process (i.e., the multibody model), referred as PNCM. Note that since the filter is adaptive, the value of **Σˆ** *<sup>P</sup>* is estimated each time step. For the propagation phase, the best estimation of the **Σˆ** *<sup>P</sup>* is the one obtained in the previous time step.

Equations (14) and (15) are equivalent to the equations employed in the discrete extended Kalman filter (DEKF) [2,37]. Following [9], the Jacobian matrix of the transition model can be derived through a forward Euler integrator of the states and a random walk for modeling the acceleration error,

$$\mathbf{f}\_{\mathbf{x}} = \begin{bmatrix} \mathbf{I} + \frac{1}{2} \frac{\partial \Delta \tilde{\mathbf{z}}}{\partial \mathbf{z}} \Delta t^2 & \frac{1}{2} \mathbf{I} \Delta t + \frac{1}{2} \frac{\partial \Delta \tilde{\mathbf{z}}}{\partial \tilde{\mathbf{z}}} \Delta t^2 & \frac{1}{2} \mathbf{I} \Delta t^2 \\ \dots & \dots & \dots & \dots & \dots & \dots & \dots & \dots \\ \dots & \frac{\partial \Delta \tilde{\mathbf{z}}}{\partial \mathbf{z}} \Delta t & \vdots & \mathbf{I} + \frac{\partial \Delta \tilde{\mathbf{z}}}{\partial \tilde{\mathbf{z}}} \Delta t & \vdots & \mathbf{I} \Delta t \\ \dots & \dots & \dots & \dots & \dots & \dots \\ \mathbf{0} & \vdots & \mathbf{0} & \vdots & \mathbf{I} \end{bmatrix} \tag{16}$$

where the terms *<sup>∂</sup>*Δ**.. z** *<sup>∂</sup>***<sup>z</sup>** and *<sup>∂</sup>*Δ**.. z** *∂* **. <sup>z</sup>** reflect the effect of the acceleration error through the force models of the multibody model. Deriving expressions for these terms is not straightforward and is out of the scope of this work. The reader is referred to [9,38] for a comprehensive explanation on how to obtain these terms.

Following the equations defined for the DEKF [2,37], the corrections can be propagated through the states obtaining the *a posteriori* state vector, **x**ˆ+, yielding,

$$\mathbf{y}\_k = \mathbf{o}\_k - \mathbf{h}(\mathbf{q}\_{MBS}, \dot{\mathbf{q}}\_{MBS}, \ddot{\mathbf{q}}\_{MBS}) \tag{17}$$

$$
\boldsymbol{\Sigma}\_k = \mathbf{h}\_{\mathbf{x}k} \mathbf{P}\_k^- \mathbf{h}\_{\mathbf{x}k}^T + \boldsymbol{\mathsf{L}}^{\mathcal{S}^+}{}\_{k-1} \tag{18}
$$

$$\mathbf{K}\_k = \mathbf{P}\_k^{-} \mathbf{h}\_{\mathbf{x}\_k} \mathbf{E}\_k^{-1} \tag{19}$$

$$\mathbf{x}\_k^+ = \mathbf{0} + \mathbf{K}\_k \mathbf{y}\_k \tag{20}$$

$$\mathbf{P}\_k^+ = (\mathbf{I} - \mathbf{K}\_k \mathbf{h}\_{\mathbf{x}k}) \mathbf{P}\_k^- \tag{21}$$

being **y***<sup>k</sup>* the innovation sequence, **o***<sup>k</sup>* the measurements, **Σ***<sup>k</sup>* the innovation covariance matrix, **K***<sup>k</sup>* the Kalman gain, and **Σˆ** *<sup>S</sup>* the covariance matrix of the sensors or measurements (MNCM). Similarly to **<sup>Σ</sup><sup>ˆ</sup>** *<sup>P</sup>* in Equation (15), **<sup>Σ</sup><sup>ˆ</sup>** *<sup>S</sup>* can vary within time steps. Hence, **<sup>Σ</sup><sup>ˆ</sup>** *<sup>S</sup>*<sup>+</sup> *<sup>k</sup>*−<sup>1</sup> is the more accurate value for propagating the corrections. The state vector and covariance matrix are estimated in Equations (20) and (21), respectively.

From the *a posteriori* state vector, **x**ˆ+, and through Equation (13), the estimated position **z**ˆ, velocities ˆ **. z** and accelerations ˆ **.. z** for the time step *k* are obtained. Since the model relies on the set of dependent coordinates, it is required to compute the errors in these coordinates. Projecting the independent coordinates over the constraints manifold, the dependent coordinates can be obtained as stated in [9].

After correcting the states of the model, the forces (or inputs) can be estimated. The error in forces can be seen as the forces required to avoid the error in accelerations, which is referred as Δ**Q**. Since (in general) there are more dependent coordinates than degrees of freedom, there are infinite force combinations producing the same effect. Hence, depending on the application, different criteria should be considered in order to find a proper force distribution [10]. A common solution, valid for simple mechanisms, is to assume that the unknown forces are applied to the degrees of freedom. Therefore,

$$
\Delta \mathbf{Q}\_k^{\mathbf{q}} = \mathbf{0} \tag{22}
$$

$$
\Delta \mathbf{Q}\_k^{\mathbf{z}} = \Delta \mathbf{Q}\_{k-1}^{\mathbf{z}} + \mathbf{\bar{M}} \Delta \hat{\mathbf{z}} \tag{23}
$$

$$
\Delta \mathbf{Q} = \begin{bmatrix} (\Delta \mathbf{Q}^{\mathbf{z}})^{\mathrm{T}} & (\Delta \mathbf{Q}^{\mathbf{q}})^{\mathrm{T}} \end{bmatrix}^{\mathrm{T}} \tag{24}
$$

being Δ**Q<sup>q</sup>** and Δ**Q<sup>z</sup>** the force correction acting on the dependent and independent coordinates (equivalent to the degrees of freedom), respectively.

#### 2.2.2. Process and Measurement Covariance Matrices Estimation

As reflected in Section 1, knowing the covariance matrices of the process and measurements noises is critical for a proper behavior of the filter in terms of accuracy and robustness. For the particular case of multibody models, the PNCM is the most critical. The PNCM is completely unknown and can vary depending on the maneuver due to, for example, external perturbations which are not modeled. On the other hand, the MNCM can be approximated by the information provided by the sensor manufacturer. In addition, fluctuations of this noise is not expected within maneuvers. In order to address this issue, in this work, the previous errorEKF with force estimation is extended with adaptive strategies for the PNCM. The effect of a proper estimation of the MNCM is also addressed.

Following the work presented in [14], through the maximum likelihood technique, the PNCM and MNCM can be estimated. The ML has the attractive property of uniqueness and consistency of the solution. This ensures, in a probabilistic sense, a convergence to the true value of the covariance matrices. However, the ML relies on a sliding window of the innovation sequence, whose size directly affects its performance: small sample sizes would lead to biased estimations.

In addition, the solution proposed in [14] is developed for navigation purposes. It assumes that the filter transition matrix, **fx**, and the design matrix, **h**, are time invariant. This is not true when using the errorEKF. Thus, the algorithm should be adapted for the use-case of this work.

The detailed procedure for estimating the PNCM and MNCM through the ML method is presented in [14]. Before presenting the method, it should be noted the difference between probability and likelihood. Probability refers to the chances of an event to happen based on the statistical distribution of some data. Likelihood corresponds to finding the distribution that fits best to a given set of data.

The ML estimation aims to solve the question of, given the observed data, which are the model parameters that maximize the likelihood of the observed data occurring. The likelihood function can be defined as *L*(*α*|**Z**), where *α* are the parameters and **Z** the observed data. In innovation-based adaptive Kalman filtering, the parameters are the noise covariance matrices, and the observed data corresponds to the innovation sequence. In plain words, the ML estimates the noise covariance matrices searching for the values which give the maximum probability of obtaining a particular innovation sequence.

In order to calculate the likelihood, it is expressed as the probability of observing a certain dataset under an unknown statistical distribution, that is *P*(**Z**|*α*). Hence, the target is to find the value of *α*, which maximizes *P*(**Z**|*α*). If the sets of observed data, *N*, are independent, the likelihood of observing the data can be expressed as the product of the probability of observing each data point individually yielding,

$$L(\mathfrak{a}|\mathbf{Z}) = \prod\_{j=1}^{N} P(\mathbf{Z}\_j|\mathfrak{a}) \tag{25}$$

where *α*, for this particular case, corresponds to the elements of the noise covariance matrices, and **Z** are the innovations of the errorEKF with force estimation, **y**. Regarding *N* ∏ *j*=1 (·), it implies the product of the full set of innovations. However, storing the entire set

of past data is not viable for online estimation. Thus, a slide window is defined so that the filter only processes a fixed number of past events.

Assuming that the innovation sequence is Gaussian distributed, the likelihood function can be expressed as,

$$L(\mathfrak{a}|\mathbf{y}) = \prod\_{j=1}^{N} \frac{1}{\sqrt{(2\pi)^{m}|\Sigma\_{j}|}} \exp^{-\frac{1}{2}\mathbf{y}\_{j}^{T}\Sigma\_{j}^{-1}\mathbf{y}\_{j}}\tag{26}$$

being **y** the innovation sequence, **Σ** the covariance matrix, *m* the number of measurements per time step, |·| the determinant operator, and exp the natural base.

The value of *α* maximizing the likelihood can be obtained by deriving the likelihood function (Equation (25)) with respect to *α* and setting it to zero. However, differentiate Equation (25) is difficult. As an alternative, Equation (25) is simplified by taking the natural logarithm of the equation yielding,

$$L(\mathbf{a}|\mathbf{y}) = -\frac{1}{2} \sum\_{j=j\flat}^{k} \left[ \ln|\boldsymbol{\Sigma}\_{j}| + \mathbf{y}\_{j}^{T} \boldsymbol{\Sigma}\_{j}^{-1} \mathbf{y}\_{j} + \mathbf{c}\_{j} \right] \tag{27}$$

where *cj* is a constant term independent of the adaptive parameters, *α*. Maximizing the previous equation is equivalent to minimizing its negative version. Neglecting the constant terms, the maximum likelihood condition can be derived,

$$\sum\_{j=j\_0}^{k} \ln|\boldsymbol{\Sigma}\_j| + \sum\_{j=j\_0}^{k} \mathbf{y}\_j^T \boldsymbol{\Sigma}\_j^{-1} \mathbf{y}\_j = \min \tag{28}$$

where *k* is the time step in which the covariance matrix is estimated and *j* is the counter inside the estimating window. Equation (28) aims at finding the covariance matrix, which results in the smallest error norm, being complementary to the state estimation.

As stated before, the solution for Equation (28) can be constructed by taking its derivative with respect to the adaptation parameters and setting the results to zero. This yields,

$$\sum\_{j=j\_0}^{k} \left[ tr \left\{ \boldsymbol{\Sigma}\_j^{-1} \frac{\partial \boldsymbol{\Sigma}\_j^S}{\partial \boldsymbol{\alpha}\_k} \right\} - \mathbf{y}\_j \boldsymbol{\Sigma}\_j^{-1} \frac{\partial \boldsymbol{\Sigma}\_j^S}{\partial \boldsymbol{\alpha}\_k} \boldsymbol{\Sigma}\_j^{-1} \mathbf{y}\_j^T \right\} = 0 \tag{29}$$

In addition, using Equations (15) and (18), the covariance matrix, **Σ***k*, can be replaced by **Σ***<sup>P</sup>* and **Σ***S*. After some manipulation [14], the equation that maximizes the likelihood function for the adaptation parameters can be expressed as,

$$\sum\_{j=j\_0}^k tr\left\{ \left[ \boldsymbol{\Sigma}\_j^{-1} - \boldsymbol{\Sigma}\_j^{-1} \mathbf{y}\_j \mathbf{y}\_j^T \boldsymbol{\Sigma}\_j^{-1} \right] \left[ \frac{\partial \boldsymbol{\Sigma}\_j^S}{\partial \boldsymbol{\alpha}\_k} + \mathbf{h}\_{\mathbf{x}j} \frac{\partial \boldsymbol{\Sigma}\_{j-1}^P}{\partial \boldsymbol{\alpha}\_k} \mathbf{h}\_{\mathbf{x}j} \right] \right\} = 0 \tag{30}$$

From Equation (30), expressions for **Σ***<sup>P</sup>* and **Σ***<sup>S</sup>* can be obtained independently. For deriving the expression of **Σ***P*, it is assumed that **Σ***<sup>S</sup>* is known and independent of *α* (i.e., *∂***Σ***<sup>P</sup> j*−1 *∂α<sup>k</sup>* <sup>=</sup> **<sup>0</sup>**). Under this assumption, the adaptation relies on **<sup>Σ</sup>***P*. This means that the covariance noises of the process are the adaptive parameters (i.e., *α* = *σ*2). Considering **Σ***<sup>P</sup>* as a diagonal matrix leads to

$$\sum\_{j=j\_0}^k tr\left\{ \left[ \mathbf{E}\_j^{-1} - \mathbf{E}\_j^{-1} \mathbf{y}\_j \mathbf{y}\_j^T \mathbf{E}\_j^{-1} \right] \left[ 0 + \mathbf{h}\_{\mathbf{x}j} \mathbf{I} \mathbf{h}\_{\mathbf{x}j} \right] \right\} = 0\tag{31}$$

which can be reduced to

$$\sum\_{j=j\_0}^{k} \mathbf{h}\_{\mathbf{x}\_j^T}^T \left[ \boldsymbol{\Sigma}\_j^{-1} - \boldsymbol{\Sigma}\_j^{-1} \mathbf{y}\_j \mathbf{y}\_j^T \boldsymbol{\Sigma}\_j^{-1} \right] \mathbf{h}\_{\mathbf{x}\_j^T} = \mathbf{0} \tag{32}$$

Introducing now the expressions of the errorEKF, the covariance matrix **Σ** can be replaced by **Σ***P*, leading to

$$\Delta^{P}\_{\ \cdot \ \mathbf{x}} = \frac{1}{N} \sum\_{j=j\_0}^{k} \left[ \Delta \mathbf{x}\_j \Delta \mathbf{x}\_j^T + \mathbf{P}\_j^+ - \mathbf{f}\_{\mathbf{x}j} \mathbf{P}\_{j-1}^+ \mathbf{f}\_{\mathbf{x}\_j^T} \right] \tag{33}$$

where **P**<sup>+</sup> *<sup>j</sup>* <sup>−</sup> **fx***j***P**<sup>+</sup> *<sup>j</sup>*−1**fx** *T <sup>j</sup>* covers the change in the covariances between time steps, and Δ**x***<sup>k</sup>* is the state correction sequence. Since the state vector in the errorEKF, defined in Equation (10), is the error in the variables, and the a priori state vector is null,

$$
\Delta \mathbf{x}\_k = \hat{\mathbf{x}}\_k^+ - \mathbf{0} \tag{34}
$$

The same procedure can be followed to estimate the measurement noise covariance matrix. Thus, assuming that **Σ***<sup>P</sup>* is known and independent of *α*, and that **Σ***<sup>S</sup>* is diagonal,

$$\hat{\mathbf{2}}^S{}\_k = \frac{1}{N} \sum\_{j=j\_0}^k \left[ \mathbf{y}\_j \mathbf{y}\_j^T - \mathbf{h}\_{\mathbf{x}\_j} \mathbf{P}\_j^{-} \mathbf{h}\_{\mathbf{x}\_j}^T \right] \tag{35}$$

Note that both estimations (Equations (33) and (35)) are considering that the transition matrix, **fx**, and observation matrix, **hx**, are time-varying.

Furthermore, the shape of both matrices for the errorEKF with force estimation is known. The information of the matrices shape is useful to help the algorithm to reach the convergence. Thus, the estimated PNCM and MNCM are modified in order to adapt them to their theoretical shape. The MNCM is a diagonal matrix composed by the covariance of each sensor. The PNCM, according to [9], is

$$\mathfrak{L}^{P} = \begin{bmatrix} \mathbf{0} \ \vdots \ \mathbf{0} \ \vdots \ \mathbf{0} \\ \vdots \ \vdots \ \vdots \ \vdots \ \vdots \ \mathbf{0} \\ \mathbf{0} \ \vdots \ \mathbf{0} \ \vdots \ \mathbf{0} \\ \mathbf{0} \ \vdots \ \mathbf{0} \ \vdots \ diag} \end{bmatrix} \tag{36}$$

where *diag σ***ˆ** 2 is a diagonal matrix containing the variance of all the components of the discrete plant noise at acceleration level.

A last consideration is that the covariance matrices must be semi-definite positive. With this algorithm, this cannot be guaranteed. Having a negative definite matrix would lead the filter to failure. To ensure the semi-definite positiveness of the matrices, the negative values are set to their absolute value [39].
