# A Discrete-Time Extended Kalman Filter Approach Tailored for Multibody Models: State-Input Estimation

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Multi-Body Model and Time-Disretization

#### 2.1. The Multibody Equations of Motion

#### 2.2. The Differential-Algebraic form of the EOMs

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

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

## 4. State-Input Estimation for MB Models

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

#### 4.2. The Augmented Constraint Measurement Equations

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

#### 4.4. Augmented Discrete Extended Kalman Filter

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

- A-priori step: Assuming that the augmented states ${\tilde{x}}_{k}^{+}$ at the previous filter step and the input ${u}_{k+1}$ are known, the a-priori state prediction ${\tilde{x}}_{k+1}^{-}$ and generalized accelerations ${\dot{v}}_{k+1}^{-}$ can be computed solving the ID-DAEs of Equation (17):$${g}_{d}({\tilde{x}}_{k+1}^{-},{\tilde{x}}_{k}^{+},{u}_{k+1})={0}_{x}.$$Knowing the estimated state covariance matrix ${\mathcal{P}}_{k}^{+}$ for the previous timestep, the a-priori 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}+{\tilde{Q}}_{k}.$$The predicted measurement ${\tilde{y}}_{k+1}^{-}$ can then be evaluated from Equation (30) as:$${\tilde{y}}_{k+1}^{-}=\tilde{y}({\dot{v}}_{k+1}^{-},{x}_{k+1}^{-},{u}_{k+1}).$$The Kalman filter gain $\mathcal{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}+{\tilde{R}}_{k})}^{-1},$$
- A-posteriori step: When the real measurement ${y}_{k+1}^{*}$ becomes available together with the predicted measurement ${\tilde{y}}_{k+1}^{-}$, the a posteriori state vector ${\tilde{x}}_{k+1}^{+}$ is obtained as:$${\tilde{x}}_{k+1}^{+}={\tilde{x}}_{k+1}^{-}+{\mathcal{K}}_{k+1}({y}_{k+1}^{*}-{\tilde{y}}_{k+1}^{-})$$The inclusion of the actual measurements also affects the posterior covariance matrix ${\mathcal{P}}_{k+1}^{+}$ and can be evaluated as:$${\mathcal{P}}_{k+1}^{+}=({\mathcal{I}}_{x}-{\mathcal{K}}_{k+1}\mathcal{H}){\mathcal{P}}_{k+1}^{-}.$$

## 5. Validation: Joint State-Input Estimation

#### 5.1. The Slider-Crank System

#### 5.2. Results

#### 5.3. Kalman Filter Tuning

## 6. Discussion and Conclusions

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Data Availability Statement

## Conflicts of Interest

## Abbreviations

TPA | Transfer Path Analysis |

KF | Kalman Filter |

ADE-KF | Augmented Discrete Extended Kalman Filter |

MB | MultiBody |

FE | Finite Element |

EOM | Equation Of Motion |

(I-), (E-) DAE | (Implicit), (Explicit) Differential Algebraic Equation |

(I-), (E-) ODE | (Implicit), (Explicit) Ordinary Differential Equation |

MBRC | MultiBody Research Code |

FNCF | Flexible Natural Coordinates Formulation |

BDF | Backward Differentiation Formula |

VS | Virtual Sensor |

MEMS | Micro Electro-Mechanical Systems |

PID | Proportional Integrative Derivative |

$\mathbb{Z}$ | integer numbers set |

$\mathbb{R}$ | real numbers set |

$\mathbf{a}\in \mathbb{R}$ | scalar |

$a\in {\mathbb{R}}^{{n}_{a}}$ | column vector |

${\left[\begin{array}{cc}{a}_{1}^{T}& {a}_{2}^{T}\end{array}\right]}^{T}\in {\mathbb{R}}^{{n}_{a1}+{n}_{{a}_{2}}}$ | vertical vector concatenation |

$A\in {\mathbb{R}}^{{n}_{1}\times {n}_{2}}$ | matrix |

${I}_{a}\in {\mathbb{Z}}^{{n}_{a}\times {n}_{a}}$ | identity matrix |

${0}_{{a}_{1}}\in {\mathbb{Z}}^{{n}_{{a}_{1}}}$ | zero vector |

${0}_{{a}_{1},{a}_{2}}\in {\mathbb{Z}}^{{n}_{{a}_{1}}\times {n}_{{a}_{2}}}$ | zero matrix |

${\square}^{T}$ | transpose operator |

${\square}^{-1}$ | inverse matrix operator |

${\square}^{-}$ | a priori prediction |

${\square}^{+}$ | a posteriori prediction |

${\square}_{k}=\square (t={t}_{k})$ | $k$th time step |

${\square}_{n}$ | natural coordinates |

$\dot{\square}=\frac{d\square}{dt}$, $\ddot{\square}=\frac{{d}^{2}\square}{d{t}^{2}}$ | time derivatives |

$\frac{d{a}_{1}}{d{a}_{2}}\in {\mathbb{R}}^{{n}_{{a}_{1}}\times {n}_{{a}_{2}}}$ | total derivative |

$\frac{\partial {a}_{1}}{\partial {a}_{2}}\in {\mathbb{R}}^{{n}_{{a}_{1}}\times {n}_{{a}_{2}}}$ | partial derivative |

$\frac{{\partial}^{2}{a}_{1}}{\partial {a}_{2}\partial {a}_{3}}\in {\mathbb{R}}^{{n}_{{a}_{1}}\times {n}_{{a}_{2}}\times {n}_{{a}_{3}}}$ | second partial derivative |

${\left|\right|\square \left|\right|}_{2}$ | 2-norm operator |

## Appendix A. Influence of the Forward Differentiation Scheme to the Linearization of the EOMs

**Figure A1.**Graphical interpretation of the forward and backward linearization for a generic state-time evolution x.

## References

- Forrier, B.; Naets, F.; Desmet, W. Broadband Load Torque Estimation in Mechatronic Powertrains Using Nonlinear Kalman Filtering. IEEE Trans. Ind. Electron.
**2018**, 65, 2378–2387. [Google Scholar] [CrossRef] - Van der Seijs, M.V.; De Klerk, D.; Rixen, D.J. General framework for transfer path analysis: History, theory and classification of techniques. Mech. Syst. Signal Process.
**2016**, 68–69, 217–244. [Google Scholar] [CrossRef] - Naets, F.; Croes, J.; Desmet, W. An online coupled state/input/parameter estimation approach for structural dynamics. Comput. Methods Appl. Mech. Eng.
**2015**, 283, 1167–1188. [Google Scholar] [CrossRef] - Naets, F.; Cuadrado, J.; Desmet, W. Stable force identification in structural dynamics using Kalman filtering and dummy-measurements. Mech. Syst. Signal Process.
**2015**, 50–51, 235–248. [Google Scholar] [CrossRef] - Shabana, A.A. Dynamics of Multibody Systems; Cambridge University Press: Cambridge, UK, 2013. [Google Scholar] [CrossRef]
- Géradin, M.; Cardona, A. Flexible Multibody Dynamics: A Finite Element Approach; John Wiley & Sons: New York, NY, USA, 2001; Volume 4. [Google Scholar]
- Bauchau, O.A. Flexible Multibody Dynamics; Springer Science and Business Media: Berlin/Heidelberg, Germany, 2011; Volume 176. [Google Scholar] [CrossRef]
- Hiller, M.; Kecskeméthy, A. Dynamics of Multibody Systems with Minimal Coordinates. In Computer-Aided Analysis of Rigid and Flexible Mechanical Systems; Seabra Pereira, M.F.O., Ambrósio, J.A.C., Eds.; Springer: Dordrecht, The Netherlands, 1994; pp. 61–100. [Google Scholar] [CrossRef]
- Cuadrado, J.; Dopico Dopico, D.; Perez, J.; 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 filters. Int. J. Non-Linear Mech.
**2013**, 53, 83–90. [Google Scholar] [CrossRef] - Sanjurjo, E.; Dopico, D.; Luaces, A.; Naya, M.Á. State and force observers based on multibody models and the indirect Kalman filter. Mech. Syst. Signal Process.
**2018**, 106, 210–228. [Google Scholar] [CrossRef] - Sanjurjo, E.; Naya, M.; Blanco, J.L.; Moreno, J.L.; Gimenez, A. Accuracy and efficiency comparison of various nonlinear Kalman filters applied to multibody models. Nonlinear Dyn.
**2017**, 88. [Google Scholar] [CrossRef] - Palomba, I.; Richiedei, D.; Trevisani, A. Kinematic state estimation for rigid-link multibody systems by means of nonlinear constraint equations. Multibody Syst. Dyn.
**2017**, 40, 1–22. [Google Scholar] [CrossRef] - Angeli, A.; Desmet, W.; Naets, F. Deep learning for model order reduction of multibody systems to minimal coordinates. Comput. Methods Appl. Mech. Eng.
**2021**, 373, 113517. [Google Scholar] [CrossRef] - Angeli, A.; Desmet, W.; Naets, F. Deep learning of multibody minimal coordinates for state and input estimation with Kalman filtering. Multibody Syst. Dyn.
**2021**, 1–19. [Google Scholar] [CrossRef] - Risaliti, E.; Tamarozzi, T.; Vermaut, M.; Cornelis, B.; Desmet, W. Multibody model based estimation of multiple loads and strain field on a vehicle suspension system. Mech. Syst. Signal Process.
**2019**, 123, 1–25. [Google Scholar] [CrossRef] - Vermaut, M.; Tamarozzi, T.; Naets, F.; Desmet, W. Development of a flexible multibody simulation package for in-house benchmarking. In Proceedings of the ECCOMAS Thematic Conference on Multibody Dynamics, Barcelona, Spain, 29 June–2 July 2015; pp. 1560–1571. [Google Scholar]
- De Jalon, J.G.; Bayo, E. Kinematic and Dynamic Simulation of Multibody Systems: The Real-Time Challenge; Springer: New York, NY, USA, 2012. [Google Scholar] [CrossRef]
- Vermaut, M.; Naets, F.; Desmet, W. A flexible natural coordinates formulation (FNCF) for the efficient simulation of small-deformation multibody systems. Int. J. Numer. Methods Eng.
**2018**, 115, 1353–1370. [Google Scholar] [CrossRef] - Pechstein, A.; Reischl, D.; Gerstmayr, J. A Generalized Component Mode Synthesis Approach for Flexible Multibody Systems With a Constant Mass Matrix. J. Comput. Nonlinear Dyn.
**2012**, 8, 011019. [Google Scholar] [CrossRef] - Brenan, K.E.; Campbell, S.L.; Petzold, L.R. Numerical Solution of Initial-Value Problems in Differential-Algebraic Equations; Siam: Philadelphia, PA, USA, 1996; Volume 14. [Google Scholar] [CrossRef]
- Blockmans, B. Model Reduction of Contact Problems in Flexible Multibody Dynamics. Ph.D. Thesis, KU Leuven, Leuven, Belgium, 2018. [Google Scholar]
- Simon, D. Kalman filtering with state constraints: A survey of linear and nonlinear algorithms. IET Control Theory Appl.
**2010**, 4, 1303–1318. [Google Scholar] [CrossRef] [Green Version] - Kirchner, M.; Croes, J.; Cosco, F.; Desmet, W. Exploiting input sparsity for joint state/input moving horizon estimation. Mech. Syst. Signal Process.
**2018**, 101, 237–253. [Google Scholar] [CrossRef] - Simon, D. Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches; John Wiley & Sons: Hoboken, NJ, USA, 2006. [Google Scholar]
- Tamarozzi, T.; Risaliti, E.; Rottiers, W.; Desmet, W. Noise, ill-conditioning and sensor placement analysis for force estimation through virtual sensing. In Proceedings of the International Conference on Noise and Vibration Engineering (ISMA2016), Leuven, Belgium, 19–21 September 2016; KU Leuven: Leuven, Belgium, 2016; pp. 1741–1756. [Google Scholar]
- Pacejka, H. The Wheel Shimmy Phenomenom: A Theoretical and Experimental Investigation with Particular Reference to the Nonlinear Problem (Analysis of Shimmy in Pneumatic Tires due to Lateral Flexibility for Stationary and Nonstationary Conditions). Ph.D. Thesis, Delft University of Technology, Delft, The Netherlands, 1966. [Google Scholar]
- Hansen, P.C.; O’Leary, D.P. The use of the L-curve in the regularization of discrete ill-posed problems. SIAM J. Sci. Comput.
**1993**, 14, 1487–1503. [Google Scholar] [CrossRef]

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

**Figure 5.**Diagram of the coupled state-input estimation scheme and signal comparisons. $\theta $ and $\dot{\theta}$ are the motor angle and angular motor velocity respectively; $\ddot{Y}$ is the translational slider acceleration; T is the motor torque.

**Figure 6.**Comparison of the measured and estimated motor angle $\theta $ (

**top**), motor angular velocity $\dot{\theta}$ (

**middle**) and translational slider acceleration $\ddot{Y}$ (

**bottom**) for the full time series.

**Figure 7.**Comparison of the measured and estimated motor angle $\theta $ (

**top row**), motor angular velocity $\dot{\theta}$ (

**middle row**) and translational slider acceleration $\ddot{Y}$ (

**bottom row**). Zoom-in per column on the velocity transitions.

**Figure 8.**Comparison of the measured and estimated motor torque; on the left, full time series; on the right, the zoom-in on the velocity transitions are shown.

**Figure 9.**The L-curve plot for different process variance ${Q}_{d}$ (

**left figure**) and zoom-in comparison on the velocity transition of the measured and estimated motor torque using two different values of process variance ${Q}_{d}$ (

**right figure**).

**Figure 10.**Comparison of the estimated motor torque using two different values of process covariance ${Q}_{d}$ of the augmented state.

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

Body | m [kg] | ${\mathit{J}}_{\mathit{xx}}$ [kg · m${}^{2}$] | ${\mathit{J}}_{\mathit{yy}}$ [kg · m${}^{2}$] | ${\mathit{J}}_{\mathit{zz}}$ [kg · m${}^{2}$] | |
---|---|---|---|---|---|

$BB$ | $3.2175\times {10}^{3}$ | $2.9225\times {10}^{2}$ | $6.274\times {10}^{2}$ | $8.714\times {10}^{2}$ | |

$MH$ | $1.420\times {10}^{1}$ | $2.48647\times {10}^{-2}$ | $9.366\times {10}^{-2}$ | $9.366\times {10}^{-2}$ | |

$MR$ | $6.670\times {10}^{-1}$ | $2.947\times {10}^{-3}$ | $2.001\times {10}^{-3}$ | $2.001\times {10}^{-3}$ | |

$MS$ | $1.350$ | $6.052\times {10}^{-3}$ | $3.897\times {10}^{-3}$ | $2.346\times {10}^{-3}$ | |

C | $1.830\times {10}^{-1}$ | $5.742\times {10}^{-4}$ | $4.930\times {10}^{-4}$ | $1.522\times {10}^{-5}$ | |

$CSh$ | $4.960\times {10}^{-1}$ | $1.644\times {10}^{-4}$ | $7.068\times {10}^{-4}$ | $7.068\times {10}^{-4}$ | |

$CS$ | $8.4058\times {10}^{-1}$ | $2.678\times {10}^{-3}$ | $1.954\times {10}^{-3}$ | $8.047\times {10}^{-4}$ | |

${B}_{C-CR}$ | $2.820\times {10}^{-1}$ | - | - | - | |

$CR$ | $2.540\times {10}^{-1}$ | $1.185\times {10}^{-2}$ | $1.1840\times {10}^{-2}$ | $3.654\times {10}^{-5}$ | |

${B}_{CR-S}$ | $2.820\times {10}^{-1}$ | - | - | - | |

S | $2.562\times {10}^{-1}$ | $2.665\times {10}^{-4}$ | $1.131\times {10}^{-4}$ | $3.{29510}^{-4}$ | |

$SA$ | $2.200\times {10}^{-2}$ | - | - | - | |

$TR$ | $1.206$ | $2.800\times {10}^{-1}$ | $9.424\times {10}^{-3}$ | $2.800\times {10}^{-1}$ | |

$TS$ | $4.206$ | $1.392\times {10}^{-2}$ | $1.256\times {10}^{-2}$ | $4.963\times {10}^{-3}$ |

${\mathbf{k}}_{\mathit{c}}$ [N/m] | ${\mathbf{c}}_{\mathit{c}}$ [Ns/m] | b [s/m] | $\mathbf{c}[-]$ | $\mathbf{d}[-]$ | $\mathbf{e}[-]$ |
---|---|---|---|---|---|

$9.7854\times {10}^{6}$ | $1.196$ | $5.036\times {10}^{2}$ | $1.5708$ | $2.653\times {10}^{-2}$ | $-9.8534$ |

$\mathit{\theta}$ [rad] | $\dot{\mathit{\theta}}$ [rad/s] | $\ddot{\mathit{Y}}$ [m/s${}^{2}$] | T [Nm] | |
---|---|---|---|---|

$Erro{r}_{RMS}$ | $0.005$ | $0.051$ | $13.651$ | $0.334$ |

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

Adduci, R.; Vermaut, M.; Naets, F.; Croes, J.; Desmet, W.
A Discrete-Time Extended Kalman Filter Approach Tailored for Multibody Models: State-Input Estimation. *Sensors* **2021**, *21*, 4495.
https://doi.org/10.3390/s21134495

**AMA Style**

Adduci R, Vermaut M, Naets F, Croes J, Desmet W.
A Discrete-Time Extended Kalman Filter Approach Tailored for Multibody Models: State-Input Estimation. *Sensors*. 2021; 21(13):4495.
https://doi.org/10.3390/s21134495

**Chicago/Turabian Style**

Adduci, Rocco, Martijn Vermaut, Frank Naets, Jan Croes, and Wim Desmet.
2021. "A Discrete-Time Extended Kalman Filter Approach Tailored for Multibody Models: State-Input Estimation" *Sensors* 21, no. 13: 4495.
https://doi.org/10.3390/s21134495