# Multibody-Based Input and State Observers Using Adaptive Extended Kalman Filter

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Materials and Methods

#### 2.1. Multibody Modeling

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

#### 2.2.1. State and Input Estimation

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

#### 2.3. Methods

- Real mechanism: This model represents the real version of the mechanism, providing the ground truth. It is equivalent to the physical mechanism. The sensor measurements are obtained from this model. The sensor data is gathered from the kinematics of the real mechanism. Since the sensor data is obtained from a simulation, it is required to add white noise to the simulated measurements in order to represent the noise properties of real sensors.
- Model: This model represents the modeling of the real mechanism. The model can be affected by several uncertainties. Even though multibody modeling can represent with high accuracy a real mechanism, it is always subjected to modeling errors. While the geometry of a system can be accurately determined, the force models present a high level of uncertainty. Hence, this model is modified, creating a discrepancy between model and real mechanism. It should be mentioned that the errors in the mass or the mass distribution will lead to a similar accuracy level regarding kinematics magnitudes, since both force and mass errors result in acceleration errors. In addition, while the mass usually remains constant, force models are prone to change during a maneuver. Thus, it is of interest to test the estimator under errors in force models. The modeling error introduced is of $1\phantom{\rule{0.222222em}{0ex}}\mathrm{m}/{\mathrm{s}}^{2}$ in gravity acceleration. In addition, the initial value of the crank angle has an offset of $\pi /16\phantom{\rule{0.222222em}{0ex}}\mathrm{rad}$.
- Observer: The estimations are computed based on this model. Regarding the modeling, it is the same as the model. The difference is that it is combined with the filter. Thus, its motion and dynamics are corrected with the information of the sensors installed on the real mechanism.

- Encoder on the crank for measuring the crank angle, which is the degree of freedom (Figure 3a).
- Gyroscope on the crank for measuring the angular rates (Figure 3b).
- Gyroscope in the coupler of the mechanism (Figure 3c).
- Pair of accelerometers at the end of the crank, in such a manner that there is one sensitive axis parallel and another perpendicular to the crank (Figure 3d).

## 3. Results

#### 3.1. Accuracy Test

#### 3.2. Robustness Test

#### 3.3. Computational Cost

## 4. Discussion

## 5. Conclusions

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Data Availability Statement

## Conflicts of Interest

## References

- Kalman, R. A New Approach to Linear Filtering and Prediction Problems. J. Basic Eng.
**1960**, 82, 35–45. [Google Scholar] [CrossRef] [Green Version] - Simon, D. Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches; John Wiley & Sons: Hoboken, NJ, USA, 2006. [Google Scholar]
- García de Jalón, J.; Bayo, E. Kinematic and Dynamic Simulation of Multibody Systems: The Real Time Challenge; Springer: Berlin/Heidelberg, Germany, 1994. [Google Scholar]
- Pastorino, R. Model-Based System Testing; Technical Report; Siemens Digital Industries Software: Plano, TX, USA, 2019. [Google Scholar]
- Cuadrado, J.; Dopico, D.; Barreiro, A.; Delgado, E. Real-time state observers based on multibody models and the extended Kalman filter. J. Mech. Sci. Technol.
**2009**, 23, 894–900. [Google Scholar] [CrossRef] - Cuadrado, J.; Dopico, D.; Perez, J.A.; Pastorino, R. Automotive observers based on multibody models and the extended Kalman filter. Multibody Syst. Dyn.
**2012**, 27, 3–19. [Google Scholar] [CrossRef] [Green Version] - Pastorino, R.; Richiedei, D.; Cuadrado, J.; Trevisani, A. State estimation using multibody models and non-linear Kalman filter. Int. J. Non-Linear Mech.
**2013**, 53, 83–90. [Google Scholar] [CrossRef] - Sanjurjo, E.; Naya, M.Á.; Blanco-Claraco, J.L.; Torres-Moreno, J.L.; Giménez-Fernández, A. Accuracy and efficiency comparison of various nonlinear Kalman filters applied to multibody models. Nonlinear Dyn.
**2017**, 88, 1935–1951. [Google Scholar] [CrossRef] - Sanjurjo, E.; Dopico, D.; Luaces, A.; Naya, M.A. State and force observers based on multibody models and the indirect Kalman filter. Mech. Syst. Signal Process.
**2018**, 106, 210–228. [Google Scholar] [CrossRef] - Rodríguez, A.J.; Sanjurjo, E.; Pastorino, R.; Naya, M.A. State, parameter and input observers based on multibody models and Kalman filters for vehicle dynamics. Mech. Syst. Signal Process.
**2021**, 155, 107544. [Google Scholar] [CrossRef] - Mehra, R. Approaches to adaptive filtering. IEEE Trans. Autom. Control
**1972**, 17, 693–698. [Google Scholar] [CrossRef] - Fraser, C.; Ulrich, S. Adaptive extended Kalman filtering strategies for spacecraft formation relative navigation. Acta Astronaut.
**2021**, 178, 700–721. [Google Scholar] [CrossRef] - Filho, J.O.A.L.; Fortaleza, E.L.F.; Silva, J.G.; Campos, M.C.M.M. Adaptive Kalman filtering for closed-loop systems based on the observation vector covariance. Int. J. Control
**2020**, 1–16. [Google Scholar] [CrossRef] - Mohamed, A.H.; Schwarz, K.P. Adaptive Kalman Filtering for INS/GPS. J. Geod.
**1999**, 73, 193–203. [Google Scholar] [CrossRef] - Karasalo, M.; Hu, X. An optimization approach to adaptive Kalman filtering. Automatica
**2011**, 47, 1785–1793. [Google Scholar] [CrossRef] [Green Version] - Rodríguez, A.J.; Pastorino, R.; Carro-Lagoa, Á.; Janssens, K.; Naya, M.Á. Hardware acceleration of multibody simulations for real-time embedded applications. Multibody Syst. Dyn.
**2021**, 55, 455–473. [Google Scholar] [CrossRef] - Woo, R.; Yang, E.J.; Seo, D.W. A Fuzzy-Innovation-Based Adaptive Kalman Filter for Enhanced Vehicle Positioning in Dense Urban Environments. Sensors
**2019**, 19, 1142. [Google Scholar] [CrossRef] [PubMed] [Green Version] - Liu, Y.; Fan, X.; Lv, C.; Wu, J.; Li, L.; Ding, D. An innovative information fusion method with adaptive Kalman filter for integrated INS/GPS navigation of autonomous vehicles. Mech. Syst. Signal Process.
**2018**, 100, 605–616. [Google Scholar] [CrossRef] [Green Version] - Wu, Q.; Jia, Q.; Shan, J.; Meng, X. Angular velocity estimation based on adaptive simplified spherical simplex unscented Kalman filter in GFSINS. Proc. Inst. Mech. Eng. Part G J. Aerosp. Eng.
**2014**, 228, 1375–1388. [Google Scholar] [CrossRef] - Sage, A.P.; Husa, G.W. Algorithms for sequential adaptive estimation of prior statistics. In Proceedings of the 1969 IEEE Symposium on Adaptive Processes (8th) Decision and Control, University Park, PA, USA, 17–19 November 1969; p. 61. [Google Scholar] [CrossRef]
- Wang, J.; Xu, T.; Wang, Z. Adaptive Robust Unscented Kalman Filter for AUV Acoustic Navigation. Sensors
**2020**, 20, 60. [Google Scholar] [CrossRef] [Green Version] - Luo, Z.; Fu, Z.; Xu, Q. An Adaptive Multi-Dimensional Vehicle Driving State Observer Based on Modified Sage–Husa UKF Algorithm. Sensors
**2020**, 20, 6889. [Google Scholar] [CrossRef] - Sarkka, S.; Nummenmaa, A. Recursive Noise Adaptive Kalman Filtering by Variational Bayesian Approximations. IEEE Trans. Autom. Control
**2009**, 54, 596–600. [Google Scholar] [CrossRef] - Davari, N.; Gholami, A. Variational Bayesian adaptive Kalman filter for asynchronous multirate multi-sensor integrated navigation system. Ocean Eng.
**2019**, 174, 108–116. [Google Scholar] [CrossRef] - Wang, S.Y.; Yin, C.; Duan, S.K.; Wang, L.D. A Modified Variational Bayesian Noise Adaptive Kalman Filter. Circuits Syst. Signal Process.
**2017**, 36, 4260–4277. [Google Scholar] [CrossRef] - Huang, Y.; Zhang, Y.; Wu, Z.; Li, N.; Chambers, J. A Novel Adaptive Kalman Filter with Inaccurate Process and Measurement Noise Covariance Matrices. IEEE Trans. Autom. Control
**2018**, 63, 594–601. [Google Scholar] [CrossRef] [Green Version] - Pan, C.; Gao, J.; Li, Z.; Qian, N.; Li, F. Multiple fading factors-based strong tracking variational Bayesian adaptive Kalman filter. Measurement
**2021**, 176, 109139. [Google Scholar] [CrossRef] - Dong, X.; Chisci, L.; Cai, Y. An adaptive variational Bayesian filter for nonlinear multi-sensor systems with unknown noise statistics. Signal Process.
**2021**, 179, 107837. [Google Scholar] [CrossRef] - Shan, C.; Zhou, W.; Yang, Y.; Jiang, Z. Multi-Fading Factor and Updated Monitoring Strategy Adaptive Kalman Filter-Based Variational Bayesian. Sensors
**2021**, 21, 198. [Google Scholar] [CrossRef] [PubMed] - Cuadrado, J.; Gutiérrez, R.; Naya, M.A.; Morer, P. A comparison in terms of accuracy and efficiency between a MBS dynamic formulation with stress analysis and a non-linear FEA code. Int. J. Numer. Methods Eng.
**2001**, 51, 1033–1052. [Google Scholar] [CrossRef] - Cuadrado, J.; Dopico, D.; Naya, M.A.; Gonzalez, M. Real-Time Multibody Dynamics and Applications. In Simulation Techniques for Applied Dynamics; Springer: Vienna, Austria, 2008; Volume 507, pp. 247–311. [Google Scholar] [CrossRef]
- Dopico, D.; Luaces, A.; Saura, M.; Cuadrado, J.; Vilela, D. Simulating the anchor lifting maneuver of ships using contact detection techniques and continuous contact force models. Multibody Syst. Dyn.
**2019**, 46, 147–179. [Google Scholar] [CrossRef] - Parra, A.; Rodríguez, A.J.; Zubizarreta, A.; Pérez, J. Validation of a Real-Time Capable Multibody Vehicle Dynamics Formulation for Automotive Testing Frameworks Based on Simulation. IEEE Access
**2020**, 8, 213253–213265. [Google Scholar] [CrossRef] - Jaiswal, S.; Rahikainen, J.; Khadim, Q.; Sopanen, J.; Mikkola, A. Comparing double-step and penalty-based semirecursive formulations for hydraulically actuated multibody systems in a monolithic approach. Multibody Syst. Dyn.
**2021**, 52, 169–191. [Google Scholar] [CrossRef] - Cuadrado, J.; Dopico, D.; Gonzalez, M.; Naya, M.A. A Combined Penalty and Recursive Real-Time Formulation for Multibody Dynamics. J. Mech. Des.
**2004**, 126, 602. [Google Scholar] [CrossRef] - Sanjurjo, E.; Blanco, J.L.; Torres, J.L.; Naya, M.A. Testing the efficiency and accuracy of multibody-based state observers. In Proceedings of the ECCOMAS Thematic Conference on Multibody Dynamics, Barcelona, Spain, 29 June–2 July 2015; pp. 1595–1606. [Google Scholar]
- Grewal, M.; Andrews, A. Kalman Filtering: Theory and Practice Using MATLAB®; John Wiley & Sons: Hoboken, NJ, USA, 2008. [Google Scholar]
- Dopico, D.; Zhu, Y.; Sandu, A.; Sandu, C. Direct and Adjoint Sensitivity Analysis of Ordinary Differential Equation Multibody Formulations. J. Comput. Nonlinear Dyn.
**2014**, 10, 011012. [Google Scholar] [CrossRef] - Myers, K.; Tapley, B. Adaptive sequential estimation with unknown noise statistics. IEEE Trans. Autom. Control
**1976**, 21, 520–523. [Google Scholar] [CrossRef]

**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. ${\widehat{\Sigma}}^{P}$ and ${\widehat{\Sigma}}^{S}$ are the PNCM and MNCM, respectively, where the superscripts ${}^{-}$ and ${}^{+}$ 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.

**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.

**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.

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

**Figure 8.**RMSE in the crank angular acceleration 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.

**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.

**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.

**Figure 15.**Torque estimation for the four-bar linkage for the spring failure scenario. Obs refers to the estimated torque and Ref refers to the theoretical value.

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

Crank | Coupler | Rocker | Ground Element | |
---|---|---|---|---|

Mass (kg) | 2 | 8 | 5 | - |

Length (m) | 2 | 8 | 5 | 10 |

Left Crank | Left Coupler | Right Coupler | Right Crank | Ground Element | |
---|---|---|---|---|---|

Mass (kg) | 3 | 1 | 2 | 3 | - |

Length (m) | 0.5 | 2.062 | 3.202 | 0.5 | 3 |

Encoder | Gyroscope | Accelerometers | |
---|---|---|---|

Standard deviation | 1.745 × 10${}^{-2}$ rad | 9.839 × 10${}^{-4}$ rad/s | 5.638 × 10${}^{-2}$ m/s${}^{2}$ |

Sampling frequency (Hz) | 200 | 200 | 200 |

Test | 1 | 2 | 3 | 4 | 5 | 6 |
---|---|---|---|---|---|---|

${\sigma}_{0}^{2}$ | 0 | 0.0001 | 0.0023 | 0.1 | 1 | 10 |

**Table 5.**Computational cost analysis of the AerrorEKF-FE. The simulations tested correspond to the use of position sensors.

Simulated Time (s) | Computing Time (s) | ||
---|---|---|---|

errorEKF | AerrorEKF | ||

Four-bar linkage | 10 | 1.5931 | 3.1955 |

Five-bar linkage | 10 | 2.2551 | 4.7234 |

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

© 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/).

## Share and Cite

**MDPI and ACS Style**

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

**AMA Style**

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

**Chicago/Turabian Style**

Rodríguez, Antonio J., Emilio Sanjurjo, Roland Pastorino, and Miguel Á. Naya.
2021. "Multibody-Based Input and State Observers Using Adaptive Extended Kalman Filter" *Sensors* 21, no. 15: 5241.
https://doi.org/10.3390/s21155241