Two-stage approach to state and force estimation in rigid-link multibody systems

Abstract

A novel two-stage approach is presented for improving the estimates of both the kinematic state and the unknown external forces in rigid-link multibody systems with negligible joint clearance. The approach is said to be a two-stage one because the estimation process is carried out by two observers running simultaneously and only partially coupled in order to reduce model uncertainties. Nonlinear Kalman filters are employed at both stages. In the first stage, a kinematic observer estimates an augmented system state (i.e., positions, velocities and accelerations) by employing the kinematic constraint equations and some measurements of kinematic quantities as inputs and outputs. Therefore, it is unbiased by external forces and uncertainties on any dynamic parameters. In the second stage, a force observer estimates the external forces by employing dynamic models. The input of the force observer is the kinematic state, while the correction is performed through some direct or indirect measurements of the known forces. Numerical assessment of the theory developed is provided through a slider–crank mechanism. The results achieved through the proposed approach are compared with those yielded by traditional unknown input observers based on a single-stage dynamic estimation. An extensive statistical analysis is carried out at varying levels of measurement noise. Two different strategies are followed in the synthesis of the non-linear Kalman filters. The comparison clearly shows the advantages and the effectiveness of the new two-stage approach.

Appendix A: Implementation details

1.1 A.1 Two-stage approach

1.1.1 A.1.1 Kinematic observer

Starting from the definitions of the state and the inputs given in Table 1, it is possible to define the first-order ODEs representing the slider–crank mechanism in the kinematic estimation as follows:

$$\begin{aligned} &\dot{\bar{\mathbf{x}}}^{\mathrm{kin}}(t)= \begin{Bmatrix} \dot{ \theta } \\ \ddot{\theta } \end{Bmatrix} = \bar{\boldsymbol{f}}^{\mathrm{kin}}_{c}\bigl( \bar{\mathbf{x}}^{\mathrm{kin}}(t),\mathrm{u}^{\mathrm{kin}}(t)\bigr)= \begin{Bmatrix} \dot{\theta } \\ \frac{c\cdot \cos (\alpha -\theta ) +r\cdot K_{{\alpha \theta }}^{2}}{c\cdot \sin (\alpha -\theta )}\dot{\theta ^{2}} +\frac{ \cos (\alpha )}{c\cdot \sin (\alpha -\theta )}a \end{Bmatrix} \\ &\quad\mathrm{with} \ \alpha =a\sin \biggl( \frac{-c\cdot \sin (\theta )-h}{r} \biggr) \ \mathrm{and} \ K_{\alpha \theta }=-\frac{c\cdot \cos (\theta )}{r\cdot \cos ( \alpha )} \end{aligned}$$

(A.1)

The model in Eq. (A.1) has been discretized using the Runge–Kutta method of order 2, so the discrete-time model is

$$\begin{aligned} \bar{\mathbf{x}}^{\mathrm{kin}}_{k} = & \bar{\boldsymbol{f}}^{\mathrm{kin}}\bigl(\bar{\mathbf{x}} ^{\mathrm{kin}}_{k-1}, \mathrm{u}^{\mathrm{kin}}_{k-1}\bigr) \\ = &\bar{\mathbf{x}}^{\mathrm{kin}}_{k-1}+ \dfrac{\Delta t}{2}\bigl( \bar{\boldsymbol{f}} ^{\mathrm{kin}}_{c}\bigl( \bar{\mathbf{x}}^{\mathrm{kin}}_{k-1},\mathrm{u}^{\mathrm{kin}}_{k-1} \bigr)+ \bar{\boldsymbol{f}}^{\mathrm{kin}}_{c}\bigl(\bar{\mathbf{x}}^{\mathrm{kin}}_{k-1}+ \Delta t\cdot \bar{\boldsymbol{f}}^{\mathrm{kin}}_{c}\bigl( \bar{\mathbf{x}}^{\mathrm{kin}}_{k-1},\mathrm{u}^{\mathrm{kin}} _{k-1} \bigr),\mathrm{u}^{\mathrm{kin}}_{k-1} \bigr)\bigr) . \end{aligned}$$

(A.2)

A discretization time of $5\cdot 10^{-3}\mbox{ s}$ has been employed.

The augmented discrete-time kinematic model used in the state prediction takes the following form:

$$\begin{aligned} \mathbf{x}^{\mathrm{kin}}_{k} = & \boldsymbol{f}^{\mathrm{kin}}\bigl(\mathbf{x}^{\mathrm{kin}}_{k-1}, \mathrm{u}^{\mathrm{kin}}_{k-1}\bigr) \\ {} = & \begin{Bmatrix} \bar{\mathbf{x}}^{\mathrm{kin}}_{k} \\ \ddot{\theta }_{k} \end{Bmatrix} = \begin{Bmatrix} \bar{\boldsymbol{f}}^{\mathrm{kin}}\bigl(\bar{\mathbf{x}}^{\mathrm{kin}}_{k-1}, \mathrm{u}^{\mathrm{kin}}_{k-1}\bigr) \\ \frac{4}{3 \Delta t} (\dot{\theta }_{k}-\dot{\theta }_{k-1})- \frac{1}{3} \bigl( \frac{c\cdot \cos (\alpha_{k-1}-\theta_{k-1}) +r\cdot K_{{\alpha \theta }_{k-1}}^{2}}{c\cdot \sin (\alpha _{k-1}-\theta_{k-1})}\dot{\theta _{k-1}^{2}} +\frac{\cos ( \alpha_{k-1})}{c\cdot \sin (\alpha_{k-1}-\theta_{k-1})}a_{k-1} \bigr) \end{Bmatrix} . \end{aligned}$$

(A.3)

The measurement model employed by the kinematic observer is

$$ \mathrm{y}^{\mathrm{kin}}_{k}= \begin{bmatrix} 1 & 0 & 0 \end{bmatrix} \mathbf{{x}}^{\mathrm{kin}}_{k}. $$

(A.4)

1.1.2 A.1.2 Force observer

Equation (12) has been employed in order to formulate the model used for the state prediction in the force observer:

$$\begin{aligned} &\mathbf{{x}}^{\mathrm{for}}_{k}= \begin{Bmatrix} F_{k} \\ T_{k} \end{Bmatrix} = \begin{Bmatrix} F_{k-1} \\ \mathrm{I}(\theta_{k-1})\cdot \ddot{\theta }_{k-1}+ \mathrm{C}(\theta _{k-1},\dot{\theta }_{k-1})+\mathrm{P}( \theta_{k-1})-F_{k-1}\cdot \mathrm{K}_{{s\theta }_{k-1}} \end{Bmatrix} \\ &\quad\mathrm{with} \ \mathrm{K}_{s\theta }=c\dfrac{\sin (\alpha -\theta )}{ \cos (\alpha )}. \end{aligned}$$

(A.5)

In Eq. (A.5), $\mathrm{I}(\theta_{k-1})$, $\mathrm{P}( \theta_{k-1})$, and $\mathrm{C}(\theta_{k-1},\dot{\theta }_{k-1})$ are, respectively, the total inertia, gravity, and centrifugal effects of the mechanism reflected to the crank. All the data needed to compute such values are provided in Table 3.

The measurement model employed by the force observer is

$$ \mathrm{y}^{\mathrm{for}}_{k}= \begin{bmatrix} 0 & 1 \end{bmatrix} \mathbf{{x}}^{\mathrm{for}}_{k} . $$

(A.6)

1.2 A.2 Benchmark approach

The continuous-time first-order dynamic model augmented with a random walk model to represent the force acting on the slider $F$ can be write as follows:

$$\begin{aligned} &\begin{aligned} \dot{\bar{\mathbf{x}}}^{\mathrm{ben}}(t) & = \bar{\boldsymbol{f}}^{\mathrm{ben}}_{c}\bigl( \bar{\mathbf{x}}^{\mathrm{ben}}(t), \mathrm{u}^{\mathrm{ben}}(t)\bigr) \\ & = \left \{ \textstyle\begin{array}{c} \dot{\theta } \\ \ddot{\theta } \\ \dot{F} \end{array}\displaystyle \right \} = \left \{ \textstyle\begin{array}{c} \dot{\theta } \\ \frac{T+F\cdot \mathrm{K}_{s\theta }-\mathrm{C}(\theta , \dot{\theta })-\mathrm{P}(\theta )}{\mathrm{M}(\theta )} \\ 0 \end{array}\displaystyle \right \} \end{aligned} \\ &\quad\mathrm{with} \ \mathrm{K}_{s\theta }=\dfrac{c\cdot \sin (\alpha -\theta )}{ \cos (\alpha )}. \end{aligned}$$

(A.7)

The model in Eq. (A.7) has been discretized using the Runge–Kutta method of order 2, so the discrete-time model is

$$\begin{aligned} \bar{\mathbf{x}}^{\mathrm{ben}}_{k} = & \bar{\boldsymbol{f}}^{\mathrm{ben}}\bigl(\bar{\mathbf{x}} ^{\mathrm{ben}}_{k-1}, \mathrm{u}^{\mathrm{ben}}_{k-1}\bigr) \\ = &\bar{\mathbf{x}}^{\mathrm{ben}}_{k-1}+ \dfrac{\Delta t}{2} \bigl( \bar{\boldsymbol{f}} ^{\mathrm{ben}}_{c}\bigl( \bar{\mathbf{x}}^{\mathrm{ben}}_{k-1},\mathrm{u}^{\mathrm{ben}}_{k-1} \bigr)+ \bar{\boldsymbol{f}}^{\mathrm{ben}}_{c}\bigl(\bar{\boldsymbol{x}}^{\mathrm{ben}}_{k-1}+ \Delta t\cdot \bar{\boldsymbol{f}}^{\mathrm{ben}}_{c}\bigl( \bar{\mathbf{x}}^{\mathrm{ben}}_{k-1},\mathrm{u}^{\mathrm{ben}} _{k-1} \bigr),\mathrm{u}^{\mathrm{ben}}_{k-1} \bigr)\bigr) . \end{aligned}$$

(A.8)

A discretization time of $5\times 10^{-3}\mbox{ s}$ has been employed.

The discrete-time model in Eq. (A.8) has been augmented with the Newmark’s first-order interpolation, in order to estimate also the crank acceleration. Finally, the discrete-time dynamic model used by the benchmark observer is the following one:

$$\begin{aligned} \mathbf{{x}}^{\mathrm{ben}}_{k} = & \boldsymbol{{f}}^{\mathrm{ben}}\bigl(\mathbf{{x}}^{\mathrm{ben}}_{k-1}, \mathrm{u}^{\mathrm{ben}}_{k-1}\bigr) \\ = & \begin{Bmatrix} \bar{\mathbf{x}}^{\mathrm{ben}}_{k} \\ \ddot{\theta }_{k} \end{Bmatrix} = \begin{Bmatrix} \bar{\boldsymbol{f}}^{\mathrm{ben}}(\bar{\mathbf{x}}^{\mathrm{ben}}_{k-1}, \mathrm{u}^{\mathrm{ben}}_{k-1}) \\ \frac{4}{3\Delta t}(\dot{\theta }_{k}-\dot{\theta }_{k-1})- \frac{1}{3}\ddot{\theta }_{k-1} \end{Bmatrix} . \end{aligned}$$

(A.9)

The measurement model employed by the benchmark observer is

$$ \mathbf{y}^{\mathrm{ben}}_{k}= \begin{Bmatrix} \theta_{k} \\ a_{k} \end{Bmatrix} =\boldsymbol{g}^{\mathrm{ben}}\bigl(\mathbf{x}^{\mathrm{ben}} \bigr)= \begin{Bmatrix} \theta_{k} \\ \frac{c\cdot \sin (\alpha_{k}-\theta_{k})\cdot \ddot{\theta } _{k} -(c\cdot \cos (\alpha_{k}-\theta_{k})+r\cdot \mathrm{K} _{\alpha \theta }^{2})\cdot \dot{\theta }_{k}^{2}}{\cos ( \alpha )} \end{Bmatrix} . $$

1.3 A.3 Filter tuning

The filter implementation requires the knowledge of the statistics of the process and the measurement noise, i.e., the covariance matrices $\mathbf{Q}$ and $\mathbf{R}$, and an estimate of the initial state $\mathbf{x}_{0}$ and its covariance $\mathbf{P}_{0}$. In this paper the following choices have been adopted.

As far as $\mathbf{x}_{0}$ is concerned, the state variables that are not directly measured have been set to zero, while the rest have been set to their measured values ($\theta_{0}$, $T_{0}$):

$$ \mathbf{x}^{\mathrm{kin}}_{0}= \begin{Bmatrix} \theta_{0} \\ 0 \\ 0 \end{Bmatrix} , \qquad \mathbf{x}^{\mathrm{for}}_{0}= \begin{Bmatrix} 0 \\ T_{0} \end{Bmatrix} , \qquad \mathbf{x}^{\mathrm{ben}}_{0}= \begin{Bmatrix} \theta_{0} \\ 0 \\ 0 \\ 0 \end{Bmatrix} . $$

(A.10)

The initial state covariances have been set on the basis of the sensor covariances (see Table 2) and by manual tuning of the remaining values. In particular, the following diagonal matrices have been selected:

$$ \mathbf{P}_{0}^{\mathrm{kin}}= \begin{bmatrix} \sigma_{\theta }^{2} & 0 & 0 \\ 0 & 10^{-2} & 0 \\ 0 & 0 & 10^{-2} \end{bmatrix} , \qquad \mathbf{P}_{0}^{\mathrm{for}}= \begin{bmatrix} 10^{-2} & 0 \\ 0 & \sigma_{T}^{2} \\ \end{bmatrix} , \qquad \mathbf{P}_{0}^{\mathrm{ben}}= \begin{bmatrix} \sigma_{\theta }^{2} & 0 & 0 & 0 \\ 0 & 10^{-2} & 0 & 0 \\ 0 & 0 & 10^{-2} & 0 \\ 0 & 0 & 0 &10^{-2} \end{bmatrix} . $$

(A.11)

The process noise covariance matrices have been set by analyzing the propagation of the parameter uncertainties on the state variables through the model, i.e., by comparing the model outcomes with different values of the uncertain parameters (see Table 3). The entries related to the force applied to the slider (the first one on the main diagonal in $\mathbf{Q}^{\mathrm{for}}$ and the third one in $\mathbf{Q}^{\mathrm{ben}}$) have been manually tuned, since they are modeled as random walks and hence do not depend on model parameters. The following matrices have been obtained:

$$\begin{gathered} \mathbf{Q}^{\mathrm{kin}}= \begin{bmatrix} 2.831\times 10^{-6} & 0 & 0 \\ 0 & 2.300\times 10^{-3} & 0 \\ 0 & 0 & 9.155\times 10^{-1} \end{bmatrix} , \\ \mathbf{Q}^{\mathrm{for}}= \begin{bmatrix} 2.000\times 10^{0} & 0 \\ 0 & 4.820\times 10^{-1} \\ \end{bmatrix} , \\ \mathbf{Q}^{\mathrm{ben}}= \begin{bmatrix} 2.827\times 10^{-6} & 0 & 0 & 0 \\ 0 & 2.200\times 10^{-3} & 0 & 0 \\ 0 & 0 & 2.000\times 10^{0} & 0 \\ 0 & 0 & 0 & 8.838\times 10^{-1} \end{bmatrix} . \end{gathered}$$

(A.12)

Finally, the measurement noise covariance matrices have been determined by standard deviations of the sensors (see Table 2), and have been set as diagonal matrices:

$$ \mathrm{R}^{\mathrm{kin}}=\sigma^{2}_{\theta }, \qquad \mathrm{R}^{\mathrm{for}}=\sigma^{2}_{T}, \qquad \mathbf{R}^{\mathrm{ben}}= \begin{bmatrix} \sigma_{\theta }^{2} & 0 \\ 0 & \sigma_{a}^{2} \end{bmatrix} . $$

(A.13)

The same filter statistics have been used for both the SS-UKF and EKF to ensure a fair comparison.