*2.3. Methods*

All the experiments of this work are performed in a simulation environment in MATLAB®, using the Open Source toolbox MBDE (See https://github.com/MBDS/mbdematlab (accessed on 12 May 2021)). The simulations are executed in a single core of a PC with an Intel Core i5 at 3.50 GHz and 8 Gb of RAM. Since there is not a physical prototype, three multibody models of each mechanism are employed in order to replicate a real scenario through simulation. All the simulations are launched with a time step of 5 milliseconds, that is a frequency of 200 Hz. They are detailed hereafter:


For the estimation, different sensors are considered. Although there are multiple possible configurations, for a reliable comparative with the results of [9], the same four combinations are considered. The four-bar and five-bar linkage share the same configurations for the sensors, duplicating the number of sensors in the case of the five-bar linkage, since it has two degrees of freedom. The sensor models are provided in [9]. The characteristics of each sensor are presented in Table 3. They are obtained from the technical data sheet of similar sensors. The four considered configurations are listed next:


**Table 3.** Characteristics of the sensors considered in this work.


**Figure 3.** Sensor configurations considered in the four-bar linkage.

The performance of the proposed estimator is evaluated in terms of accuracy and robustness. For the accuracy, a set of tests is executed under different initial noise covariance matrices. It is expected that the filter converges to a similar accuracy level independently from the initial noise covariance matrices, due to the adaptive equations. With respect to the robustness, it is of interest that the estimator can cope with unexpected events during the simulations without causing a divergence in the estimations.

The numerical results of each test are analyzed using two different metrics: the rootmean square error (RMSE) and the confidence interval of the estimation errors. The RMSE is used to measure the accuracy of the estimations in position, velocity, acceleration and force acting in the degree of freedom of each mechanism. The RMSE can be obtained following Equation (37).

$$RMSE = \sqrt{\frac{\sum\_{i=0}^{N} (Virtual\_i - Real\_i)^2}{N}} \tag{37}$$

being *N* the number of samples gathered during the simulation. *Virtual* refers to the estimated value of the variable and *Real* to the reference value obtained from the *real mechanism*.

The confidence intervals of the errors give a different insight with respect to the RMSE on the accuracy of the estimator. For being reliable, the error of the estimations must be inside the confidence interval. Thus, the confidence interval of the estimation errors in position, velocity and acceleration are also provided. A confidence interval is calculated as a function of the standard deviation of a variable, which is related to the diagonal elements of the covariance matrix, **P** (Equation (21)). In this work, a confidence interval of 95% is selected. It can be calculated through Equation (38).

$$CI = E \pm 1.96\sqrt{\sigma\_{\bar{l}}} \tag{38}$$

where *<sup>E</sup>*¯ is the mean estimation error of the *<sup>i</sup>*th variable and <sup>√</sup>*σ<sup>i</sup>* the standard deviation of the *i*th variable.

#### **3. Results**

To asses the performance of the proposed adaptive Kalman filter, different tests are launched. During the first test, the mechanisms are in the vertical plane and they are only affected by the gravity force. A batch of tests is executed under these conditions for different initial values of the PNCM. The results of each simulation are compared against the results obtained from the conventional errorEKF with force estimation. In a second test, the mechanisms are modified with the addition of a torsional spring in the crank. During the simulation, the spring is removed from the *real mechanism*, simulating a failure on a real machine. The AerrorEKF-FE is expected to overcome this new modeling error (together with the error in the gravity acceleration and initial position) in the *observer* and adapt the covariance matrices to the new situation.

The simulations are executed with a time step of 5 milliseconds, that is a frequency of 200 Hz. The measurements are gathered at 200 Hz. Hence, there is one measurement available at each time step. The errors introduced in the model for each test are of 1 m/s<sup>2</sup> in gravity acceleration and of an offset in the initial value of the crank angle *π*/16 rad. In addition, for testing the robustness, a torsional spring is introduced in the crank. During the simulation, this spring is removed only from the *real mechanism*. The *observer* should detect this event through the sensor measurements and perform the required corrections.

#### *3.1. Accuracy Test*

For testing the convergence of the adaptive filter, different initial values of the PNCM are selected. While the MNCM can be approximated by testing the sensors, an initial value for the process noise is more difficult to obtain. Due to this inconvenience, it is of interest to test the robustness of the proposed filter under different initial values of the PNCM. The results are compared with the ones obtained from the errorEKF-FE. This would show the error reduction when using the AerrorEKF-FE if the initial PNCM is not close to the true value.

Following Equation (36), for the four-bar linkage, only one value is required. For the five-bar linkage, since it has two degrees of freedom, two elements of the PNCM matrix should be instantiated. For simplicity, both elements are set to the same value. The different initial values are summarized in Table 4.

**Table 4.** Initial values of the PNCM.


Note that the initial values of both noise covariance matrices are used for the errorEKF-FE and AerrorEKF-FE. For the errorEKF-FE, these values are constant during the simulation, since it assumes that the PNCM is constant. Meanwhile, for the AerrorEKF-FE, these are the initial values of the PNCM, which are modified by the adaptive method. Hence, a total of six simulations for each sensor configuration are launched for each filter in order to compare their performance.

From the initial tests, an undesired behavior is detected. If both the PNCM and MNCM are estimated at the same time, the filter shows a trend to diverge. In Figure 4, the evolution of the estimated PNCM in the four-bar linkage is represented and compared with its evolution when the MNCM is not included in the estimation. Except for the case of the accelerometers, there is a trend to a continuous growth of the PNCM. This implies that the filter is reducing its trust on the model in exchange of the measurements.

The different behavior within configuration relies on the nature of the sensors. In the scenarios without accelerometers, there is a trend of divergence in the acceleration error. In addition, when using only an encoder, both the velocity and acceleration error diverge. Once that the estimator detects that the error is increasing through the innovation sequence, it starts to reduce the confidence in the model and trusting more in the sensors. However, correcting the acceleration error from measurements in position or velocity is not accurate. Hence, the error keeps growing causing the filter to diverge. At some point, this issue will lead to a failure of the filter. This, together with the non-linearities of the mechanisms, can result in unpredictable behaviors as the case of the gyroscope in the crank. Hence, for the rest of the experiments, only the PNCM is estimated.

**Figure 4.** Estimated PNCM element in the four-bar linkage test for the case of estimating PNCM and MNCM simultaneously and estimating only the PNCM.

Regarding the size of the sliding window for the innovation sequence, it should be noted that a large window leads to low mean errors in smooth maneuvers. However, a large window also implies slow corrections to the covariance matrix. Hence, in some tests, the delay in the correction can result in an error which is not corrected. This is the case shown in Figure 5. It corresponds to a simulation of the four-bar linkage with a gyroscope on the coupler and an initial process covariance noise of *σ*<sup>2</sup> *ii*,0 = 10, and a window length of 100 time steps. The same maneuver is performed with a window length of 50 samples. The innovation sequences are shown in Figure 5a. The value of the crank angle for each slide window size is shown in Figure 5b.

**Figure 5.** Comparison of innovation and crank angle for the simulation of the four-bar linkage with a gyroscope on the coupler for different window lengths.

As can be seen in Figure 5a, with a window length of 100 samples, there are a notable difference between the estimated and measured angular rate of the coupler at particular moments of the maneuvers. In these points of the maneuver, the mechanism reaches a position where the crank and the rocker are parallel. In this situation, the possible velocities of the coupler are limited. Thus, for an error in position, the error in velocity can be incorrigible. This is a consequence of the non-linearity of the system.

Comparing the results between different window sizes, it is possible to appreciate the effects of the samples considered for the estimation. With a large window, the filter cannot correct with immediacy the error. Although it is able to correct the error in terms of crank angular rate, as is shown in the innovation sequence, a permanent offset appears in the position of the crank angle. Meanwhile, with a short window length, the influence of previous values is reduced and the filter can perform the correction properly. Hence, the tests of this work are executed with a window of 50 samples for the innovation sequence.

There is no a general criteria for selecting the length of this window. It should be adjusted depending on the nature of the maneuver aiming to include only the relevant information of past events. The main idea is to avoid the effect of past events which do not have relation with the actual state of the mechanism. If a maneuver is close to steady state, a large window can be selected. On the other hand, a short window length is adequate in cases where the system changes.

In order to analyze the performance of the proposed adaptive filter, the RMSE for the position, velocity, acceleration and force acting in the degree of freedom of each mechanism is evaluated. From Figures 6–9, the RMSE between the AerrorEKF-FE and errorEKF-FE are compared for the four-bar mechanism. Results show that the adaptive version of the errorEKF-FE leads in most of the test to a lower error in all the measured magnitudes. In fact, in some configurations, the RMSE is almost constant. This behavior is what could be initially expected: despite the different initial PNCM, the AerrorEKF-FE is capable of finding an optimal covariance that minimizes the errors in all tests.

**Figure 6.** RMSE in the crank angle (position) provided by the observers for the four-bar linkage.

**Figure 7.** RMSE in the crank angular rate (velocity) provided by the observers for the four-bar linkage.

**Figure 9.** RMSE in the forces acting on the crank provided by the observers for the four-bar linkage.

The confidence intervals of the error in position, velocity and acceleration are shown in Figure 10 for one of the simulations. It can be seen how the errors converge to the true value and that the confidence intervals are consistent with the evolution of the error. Considering that the confidence interval is of 95%, it can be asserted that only the 5% of the estimations will have an error higher than 1.96 times the standard deviation of the variable (i.e., the square root of the diagonal elements of the covariance matrix, **P**, of Equation (21)). These plots also provide useful information about the observability of the system. Since the confidence interval is based on the PNCM elements, if the system were not observable, it could be expected an unlimited growth of the covariance of the states.

**Figure 10.** Error and confidence interval of the position, velocity and acceleration of the crank angle in the configuration which considers a gyroscope on the coupler.

With respect to the five-bar linkage, a similar trend can be appreciated from Figures 11–14. Except for the crank angle and angular rate when using gyroscopes in couplers, the rest of the experiments show that the AerrorEFK-FE converges to accurate solutions in every tested case. Thus, it is able to estimate the value of the PNCM which minimizes the error.

**Figure 11.** RMSE of the norm of the two crank angles (position) provided by the observers for the five-bar linkage.

**Figure 12.** RMSE of the norm of the two crank angular rates (velocity) provided by the observers for the five-bar linkage.

**Figure 13.** RMSE of the norm of the two crank angular accelerations provided by the observers for the five-bar linkage.

**Figure 14.** RMSE of the norm of the forces acting on the two cranks provided by the observers for the five-bar linkage.
