key: cord-0918680-9qjv7djm
authors: Jaiswal, Suraj; Sanjurjo, Emilio; Cuadrado, Javier; Sopanen, Jussi; Mikkola, Aki
title: State estimator based on an indirect Kalman filter for a hydraulically actuated multibody system
date: 2022-02-22
journal: Multibody Syst Dyn
DOI: 10.1007/s11044-022-09814-3
sha: a1d47f6874d816394829a72eee1ebc7bc549839b
doc_id: 918680
cord_uid: 9qjv7djm

In multibody system dynamics, the equations of motion are often coupled with systems of other physical nature, such as hydraulics. To infer the real dynamical state of such a coupled multibody system at any instant of time, information fusing techniques, such as state estimators, can be followed. In this procedure, data is combined from the coupled multibody model and the physical sensors installed on the actual machine. This paper proposes a novel state estimator developed by combining a multibody model with an indirect Kalman filter in the framework of hydraulically driven systems. An indirect Kalman filter that utilizes the exact Jacobian matrix of the plant at position and velocity level is extended for hydraulically actuated systems. The structures of the covariance matrices of the plant and measurement noise are also studied. The multibody system, described using a semi-recursive formulation, and the hydraulic subsystem, described using lumped fluid theory, are coupled using a monolithic approach. As a case study, the state estimator is applied to a hydraulically actuated four-bar mechanism. The state estimator considers modeling errors in the force model because of its uncertainty in modeling. The measurements are obtained from a dynamic model which is considered as the ground truth, with an addition of white Gaussian noise to represent the noise properties of the actual sensors. The state estimator uses four sensor configurations with different sampling rates. For the presented case study, the state estimator can accurately estimate the work cycle and hydraulic pressures of the coupled multibody system. The results demonstrate the efficacy of the proposed state estimator.

Computer simulation of mechanical systems can be carried out using multibody system dynamics in which the equations of motion describe an equilibrium for the dynamic system under consideration. In multibody system dynamics, the equations of motion are often coupled with systems of other physical nature, such as hydraulic actuators and electric drives. If the forces are accurately defined, then such a coupled multibody system can accurately describe reality. Nevertheless, even such accurate models can deviate from reality over a Extended author information available on the last page of the article long simulation run because of the accumulation of small modeling errors over time. To infer the real dynamical state of a multibody system at any instant of time, information fusing techniques, such as state estimators or observers, can be followed to combine data from the coupled multibody model and the real sensors installed on the actual machine. A state estimator, in general, is a recursive Bayesian estimator [33] . It can find its application to control and monitor the operation of the machine [22] by estimating the state subject of control instead of measuring it. In many practical cases, measurement can be cumbersome or expensive to conduct.

In mobile working machinery, the use of the multibody system approach is often combined with a description of the hydraulic actuators. Modeling of hydraulics often leads to numerically stiff systems [24, 29] . For such systems, the efficiency requirements in realtime applications are high [15, 16] . The problem of numerical stiffness can be alleviated by a proper selection of the multibody approach, as shown in [17] . Furthermore, to control and monitor the operation of a mobile working machine, it may become cumbersome or expensive to obtain the measurements of all the required physical quantities. For example, it may not be feasible or possible to measure the hydraulic forces acting on the multibody system in such an application. In this situation, state estimators can provide the possibility of using virtual sensors to infer measurements from the readings of different sensors, which are more economical or easy to install and maintain than the replaced one.

In recent years, there have been a number of studies on multibody-based state estimators. As a consequence, a number of estimation techniques such as Kalman filters [13, 33] and particle filters [1, 6] have been used in the literature [4, 9] . The application of Kalman filters to general multibody systems is not a trivial task because of their different mathematical structures. The Kalman filter was originally formulated for the first order linear and unconstrained systems [21] . Multibody dynamic systems are, in general, second order nonlinear and constrained systems [20] . Therefore, it is an open field for additional studies in general and more specifically for the study of state estimators for multibody systems, which include hydraulic actuators [17, 29] .

The methods within the family of non-linear Kalman filters can be, in general, categorized into two groups, namely, independent coordinate methods and dependent coordinate methods [33] . In independent coordinate methods, the state vector of the filter comprises the independent coordinates only, which is the case for most probabilistic estimators presented in the estimation and control theory literature [13] . In this approach, the usual way to combine a multibody system with a Kalman filter is to use the independent coordinates and velocities of the multibody system as the state vector of the filter [35] . On the other hand, dependent coordinate methods [12, 34] can handle the case of intra-state vector dependencies, which is exactly the situation found in multibody dynamic systems. This approach can be applied to a multibody system by incorporating the constraints as perfect measurements or by projecting the unconstrained estimation over the constraints manifold [33] . For simplicity, only methods based on independent coordinates are covered in this work.

Among the methods in independent coordinates, a continuous extended Kalman filter (CEKF) was used to demonstrate a real-time state estimator for a four-bar mechanism [9] . In CEKF, the dynamic multibody equations are adapted to the structure of the Kalman filter in a continuous-time frame so that the state-transition and state-update stages are seamlessly fused together. In the rest of the estimators that work in discrete time steps, such as the discrete extended Kalman filter (DEKF) [35] , the unscented Kalman filter (UKF) [25] , and indirect Kalman filter [31] , the filter formulation considers these stages as two separate steps. Both stages constitute different equations for updating the state vector and the associated covariance matrix. In these formulations, the state-transition or prediction stage relies on the transition model of the dynamic system, while the state-update stage includes information from the sensors or observations [35] .

In [32] , the accuracy and efficiency of CEKF, DEKF, UKF and indirect Kalman filter applied to four-bar and five-bar mechanisms, were compared. The indirect Kalman filter outperformed the others and was less affected by the increase in the size of the system. It even finds its application in automobiles [26] with real-time capability [30] . In an indirect Kalman filter, the position and velocity errors of the independent coordinates of a dynamic system are considered as the state vector of the filter. Thus, it is also referred to as the errorstate extended Kalman filter (eEKF). However, this method assumes a simplified form of the transition model of the dynamic system, that is, the Jacobian matrix of its plant. This can lead to incorrect estimation in certain examples, as presented in [32] . Nevertheless, this short-come of eEKF has been addressed in [31] using an exact Jacobian matrix of the plant, referred to as the eEKF-EJ method. In [31] , another variant of eEKF was proposed that additionally considers the acceleration errors of the independent coordinates in the state vector. This variant of eEKF can estimate the input force for the system and by its nature provides better accuracy than the variants without force estimation. Note that the indirect Kalman filter and its variants are independent of the type of integrator used in the dynamic system.

Despite the previous research efforts explained above, the limitations in the existing literature are twofold. First, even though there are various studies on multibody-based state estimators, their application in the field of hydraulically actuated multibody systems has been overlooked. Second, even though the literature on hydraulic machinery covers various aspects of modeling and simulation, their detailed investigation using extended or unscented Kalman filters has been neglected. Therefore, this study claims to cover this research gap by introducing a state estimator based on an indirect Kalman filter for hydraulically actuated multibody systems.

The objective of this paper is to propose a novel state estimator developed by combining a multibody model with an indirect Kalman filter in the framework of hydraulically driven systems. The eEKF-EJ method that utilizes the exact Jacobian matrix of the plant at position and velocity level, introduced in [31] , is extended for hydraulically actuated systems. To complement the above objective, the structures of the covariance matrices of the plant and measurement noise are also explained. The multibody system, described using a semirecursive formulation [2, 8, 10] , and the hydraulic subsystem, described using the lumped fluid theory [37] , are combined using a monolithic approach. As a case study, the state estimator is applied to a hydraulically actuated four-bar mechanism. Although a case study of a planar mechanism is shown in this study, the method presented is general and applicable to three-dimensional mechanisms as well. The state estimator considers modeling errors in the force model because of its uncertainty in modeling [31] . Measurements are obtained from a dynamic model, which is considered as a ground truth, with an addition of white Gaussian noise, to represent the noise properties of the actual sensors. The state estimator considers four sensor configurations at four different sampling rates of the sensors. For the presented case study, the state estimator is evaluated based on the accuracy of the work cycle and hydraulic pressures.

In this study, the dynamics of a constrained mechanical system is described using a semirecursive formulation. In this formulation, the motion of a system is described using the dynamics of the open-loop system and then incorporating the loop-closure constraints by means of the penalty-based approach [2, 8, 10] . The dynamics of the hydraulic actuators are modeled using the lumped fluid theory [37] and coupled with the dynamics of the multibody system in a monolithic approach. It should be noted that the hydraulic actuators are not considered as separate bodies, instead, the hydraulic forces are computed and fed into the multibody system as external forces. In this study, a semi-recursive formulation and lumped fluid theory are considered because they lead to a computationally efficient method for a unified simulation of multibody and hydraulic dynamics as presented in [28] , [29] and [17] .

Consider a system as an open-loop system with N b bodies, which may require temporary cutting of certain joints. The Cartesian velocities Z j ∈ R 6×1 and the Cartesian accelera-tionsŻ j ∈ R 6×1 of a reference point on body j can be written as

Here,ṡ j ands j are, respectively, the velocity and the acceleration of the reference point attached to body j that instantaneously coincides with the origin of the inertial reference frame, and ω j andω j are, respectively, the angular velocity and angular acceleration of the body j . The Cartesian velocities, Z j , and the Cartesian accelerations,Ż j , can be recursively expressed in terms of the previous bodies as [19] :

where the scalarsż j andz j are, respectively, the time and double time derivatives of the relative joint coordinate z j , and the vectors b j and d j depend on the type of joint [11] that connects the bodies j − 1 and j . The vector b j are the Cartesian velocities Z j of body j when all relative joint velocities are made zero except forż j = 1; and the vector d j is the difference of the Cartesian accelerationsŻ j −Ż j −1 when all the relative joint accelerations are made zero [7, 8] . Note that the indexes j − 1 and j may not be successive as the system may branch. The mapping of the vector of Cartesian velocities,

into the vector of relative joint velocities,ż = ż 1 ,ż 2 , . . . ,ż N b T , can be achieved using a velocity transformation matrix R ∈ R 6N b ×N b as [11, 19] :

where T ∈ R 6N b ×6N b is the constant path matrix that describes the topology of the open-loop system, and R d ∈ R 6N b ×N b is a diagonal matrix whose elements are the vectors b j arranged in an ascending order. Note that the termṘż in Eq. (4) can be expressed in terms of the vectors d j using Eq. (2) [11] .

Using the principle of virtual work, the equations of motion for the open-loop system can be written as [19] :

wherez ∈ R N b is the vector of relative joint accelerations, and the matricesM ∈ R 6N b ×6N b andQ ∈ R 6N b are, respectively, a diagonal matrix that consists of the mass matricesM j and 

where m j is the mass of body j , I 3 is a (3 × 3) identity matrix, g j is the position vector of the centre of mass of body j , the skew-symmetric matrix of a vector is denoted by a tilde (~), J j is the inertia tensor of body j , f j is the vector of external forces applied on body j , and τ j is the vector of external moments with respect to the center of mass of body j . The equations of motion for the closed-loop system can be written by incorporating a set of m loop-closure constraint equations, = 0, in Eq. (5) as [10, 27] :

where z is the Jacobian matrix of (z) = 0, α is the penalty factor that can be set the same for all constraints, λ is the vector of iterated Lagrange multipliers, M = R T d T TM TR d , and Q = R T d T T Q −MTṘ dż . Note that for simplicity, the constraint equations are assumed to be holonomic and scleronomic. In this formulation, λ are iterated at each time-step k as [10] :

where h is the iteration step and λ (0) k is the final value of λ k−1 from the previous time-step. Figure 1 shows an example of a closed-loop system.

The system is integrated using the implicit single-step trapezoidal rule [10] . In this formulation, the relative joint velocities,ż, and accelerations,z, are corrected using the massdamping-stiffness-orthogonal projections as [3, 8] :

whereż andz are, respectively, the relative joint velocities and accelerations obtained from the Newton-Raphson iteration, t is the time-step, W = M + t 2 C + t 2 4 K, where C and K represent the damping and stiffness contributions in the system, t is the partial derivative of the constraints with respect to time t , and the term ˙ zż can be calculated from the chain rule of differentiation by using as intermediate variables, the coordinates of points and the components of unit vectors, as those shown in Fig. 1 .

In this study, hydraulic pressures within a hydraulic circuit are described using the lumped fluid theory [37] . According to the lumped fluid theory, a hydraulic circuit can be divided into discrete volumes, where pressures are assumed to be evenly distributed. Thus, the effects of acoustic waves are ignored. The pressure build-up,ṗ s , in a hydraulic section S can be expressed as:ṗ

where V s is the volume of the section, B es is its effective bulk modulus, Q sk is the incoming or outgoing flows of the section, and n f is the total number of volume flows going in or out of the section. In Eq. (12), B es can be written as:

where B oil is the bulk modulus of oil, B k is the bulk modulus of volume V k , and n c is the total number of sub-volumes V k , such as pipes and hoses, that form the volume V s .

The valves in a hydraulic circuit can be described using a semi-empirical modeling method [14] . In this method, the volume flow rate, Q t , through a simple throttle valve can be expressed as:

where C vt is the semi-empirical flow rate coefficient of the throttle valve and p is the pressure difference over the valve. In Eq. (14), C vt can be written as:

where A t is the area of the throttle valve, C d is its flow discharge coefficient and ρ is the oil density. The value of C d can range between 0 and 1, signifying how much the actual flow differs from a reference flow for the same restriction and geometry of the valve. In this study, the value of C d is considered to be 0.8, as in [17, 29] . Similarly, the volume flow rate, Q d , through a directional control valve can be expressed as: where C v d is the semi-empirical flow rate constant of the directional control valve that can be procured from manufacturers' catalogues, and U is the relative spool position that can be computed as:U

where U ref is the reference voltage signal for the reference spool position and τ is the time constant, which can be procured from the Bode-diagram of the valve that describes the valve spool dynamics. In this study, the value of C v d is computed for a nominal flow (flow at full opening) of 24 l/min over a 35 bar pressure difference. Note that for a pressure difference of less than 2 bar, the volume flow is assumed laminar, and Eqs. (14) and (16) are modified so that the volume flow and the pressure difference follow a linear relation.

A hydraulic cylinder is shown in Fig. 2 , whose motion produces volume flows that can be written as:

where Q in and Q out are, respectively, the incoming and outgoing volume flow rates of the hydraulic cylinder, A 1 and A 2 are, respectively, the piston and piston-rod side areas of the cylinder, andẋ is the piston velocity. The force F c produced by the cylinder can be expressed as:

where p 1 and p 2 are, respectively, the pressures on the piston and piston-rod sides, and F μ is the total friction force [5] caused by the seal. The friction force from sealing can be computed using various friction models as presented by the authors in [18] . In this study, a continuous static friction model, namely, Brown-McPhee friction model [5] , is utilized because it can describe the usual friction characteristics with a computationally efficient approach, as presented by the authors in [18] .

The multibody system, described in Sect. 2.1, is extended to incorporate the dynamics of the hydraulic actuators, described in Sect. 2.2, in a monolithic approach [17, 24, 29] . The force vector, Q, in Eq. (8) is extended with the pressure variation equations, which can lead to a combined system of equations as in [29] :

whereṗ is the time derivative of the vector of pressures, p, in the hydraulic subsystem, and v (p, z,ż) are the pressure variation equations. The dependencies of Q and the function v with respect to z,ż, and p are assumed to be known. In this study, the coupled system is integrated using the implicit single-step trapezoidal rule [23] . By initially applying the predictors, z k+1 = z k +ż k t + 1 2z k t 2 and p k+1 = p k +ṗ k t, respectively, to the relative joint coordinates, z k+1 , and the pressures, p k+1 , the solution for the relative joint velocities,ż k+1 , the relative joint accelerations,z k+1 , and pressure derivatives,ṗ k+1 , at time-step (k + 1) can, respectively, be written as:

By introducing Eq. (21) into Eq. (20), the dynamic equilibrium for the coupled system, at time-step (k + 1), can be written as:

Equation (23) is a nonlinear system of equations that can be denoted asf (x k+1 ) = 0, wherex = z T , p T T . Such nonlinear system of equations can be iteratively solved using the Newton-Raphson method with numerical derivatives. Here, the numerical derivatives are performed using the forward differentiation rule as in [17] . After every integration step, z andz are corrected using the mass-damping-stiffness-orthogonal projections [3, 8] , as shown in Eqs. (10) and (11) .

In this study, the state estimator is obtained by combining a coupled multibody system, described in Sect. 2.3, with the indirect Kalman filter [13, 33] . Here, the coupled multibody system is used without any modifications in its formulation. Thus, any multibody and hydraulic formulations or integrators can be used, including implicit integrators, projections and dependent coordinates, as is the case in this study. The eEKF-EJ method, described in [31] , is extended to incorporate the pressure terms of a hydraulically actuated system.

In this indirect Kalman filter, the filter estimates the error in the coupled multibody system, explained in Sect. 2.3, at each time-step of the simulation and the error is corrected with measurements of the real system. Figure 3 illustrates a simplified scheme of this formulation.

For each time-step of this filter, one step of the coupled multibody system is simulated to obtain its relative joint coordinates, z, relative joint velocities,ż, and pressures, p. The state vector for the coupled multibody system can be written as

Here, z i and ż i are respectively the errors in the relative joint coordinates and velocities of the degrees of freedom of the multibody system and p is the error in the pressures of the hydraulic subsystem. Next, the state vector,x, is estimated such that:

where z i andż i are, respectively, the values of the relative joint coordinates and velocities of the degrees of freedom of the mechanism predicted by the coupled multibody system prior to the correction phase;ẑ i andẑ i are respectively their estimated values after the correction phase; and p is the value of the pressures predicted by the coupled multibody system prior to the correction phase andp is its estimated value after the correction phase. It is assumed that a linearized estimator can be well suited to a problem of estimating small displacements in a linearized neighborhood or tangent space of the nonlinear manifold of the state space of the mechanism. The propagation phase or the prediction stage can be performed as follows:

wherex − k is the predicted mean of the state, also knows as 'a priori' state estimation, and P − k is its associated covariance matrix, P + k−1 is the covariance matrix associated with the corrected mean of the state,x + k−1 , from the previous time-step, f x is the Jacobian matrix of the discrete transition model, f (·), of the system with respect to the estimated statex, and Σ P k−1 is the covariance matrix of the plant noise, which stands for the additional uncertainty of the new state. The value of Σ P k−1 is physically attributed to the incorrect forces and errors in modeling the mechanism, such as inertia values, initial conditions, and many more. Note that Eqs. (25) and (26) are the equations obtained when one applies the conventional Kalman filter to the tracking error of a model [13, 33] . The matrix f x in Eq. (26) , which follows a forward Euler integration, can be written as:

where I f is a (f × f ) identity matrix in which f is the number of degrees of freedom of the multibody system, I np is a n p × n p identity matrix in which n p is the number of pressures modeled in the hydraulic subsystem, t is the time-step, and 0 np ×f is a n p × f zero matrix. In Eq. (27), the blocks at positions (3, 1) and (3, 2) are set to 0 because the evolution of pressure with respect to position and velocity is unknown. For simplicity, it is assumed that ∂ ṗ ∂p = 0. In the case of complex force models, the partial derivatives in Eq. (27) are difficult to calculate [31] , and for this reason they are obtained numerically in this study using the forward differentiation rule [17] . As this method operates in the transformed statespace of errors, the predicted mean of the state,x − k , is always zero, as shown in Eq. (25) . In other words, the filter initially assumes that the dynamic model made a perfect work in tracking the real system.

The correction phase or the state-update stage is similar to the ones found in a conventional DEKF and can be written as follows [32] :

where y k is the error or innovation between virtual measurements h (·) and their actual measurements o k . Note that the virtual measurements h (z k ,ż k , p k ) are built using the coordinates and pressures of the coupled multibody system instead of the states of the filter because the states of the filter are the errors, and the errors are always set to zero until after correction [31] . In Eq. (29), S k is the covariance matrix of the innovation that represents the uncertainty of the system state projected by the sensor function (h x ) k P − k (h x ) T k and an additional sensor Gaussian noise, Σ S k , known as the covariance matrix of the measurement noise. Small values of S k imply that actual measurements or observations introduce valuable information that constrains the estimation of the system state. Here, h x is the Jacobian matrix of the measurement model, h (·), with respect to the state x. Note that the expression of h x is obtained similarly to an equivalent conventional Kalman filter because the partial derivatives with respect to the errors in the states have the same value as the partial derivatives with respect to the states. In Eq. (30), K k is the Kalman gain, which is a temporary term, to update or correct the predicted mean of the state and its covariance. In Eqs. (31) and (32) ,x + k is the corrected mean of the state, also known as 'a posteriori' state estimation, and P + k is its associated covariance matrix. The covariance matrix P + k is used as input for the next iteration of this iterative filter at the next time-step.

After the correction phase, the estimated errors, ẑ i , ẑ i and p are used in Eq. (24) to solve forẑ i ,ẑ i , andp. As the corrections are expected to be small, a linearization of the position problem can be solved using z z = 0. As a consequence, the estimated error of the dependent relative joint coordinates, ẑ d , of the multibody system can be written in terms of ẑ i as:

where d z and i z are, respectively, the dependent and independent columns of the Jacobian matrix z . It is assumed that d z is invertible. The complete set of estimated errors of the relative joint coordinates, ẑ = ẑ i T , ẑ d T T , can be used to estimate the relative joint coordinates,ẑ, as:ẑ

where z is the value of the relative joint coordinates predicted by the coupled multibody system prior to the correction phase. Note that this approach to solving the position problem is an approximation to avoid solving it iteratively, thus, a perfect fulfillment of the constraints at position level is not expected. The estimator corrects the states of the multibody model whenever measurements are available and this may lead to imperfect fulfillment of the constraints. However, the multibody formulation imposes the fulfillment of constraints at every time-step, and therefore the errors are usually acceptable for most applications [31] . Nevertheless, when the highest possible accuracy is required, then the exact position problem must be solved. Once the correction of the position estimation is applied, the correction of the velocity estimation is solved using the coordinate partitioning method as in [20] : . Once the dynamic model is corrected usingẑ,ẑ, andp, then the expected error becomesx + k = 0

In the application of the Kalman filter, the tuning of parameters, such as the covariance matrices of the plant and measurement noise, that is, Σ P and Σ S , is crucial. If Σ P and Σ S are not properly defined, then a nonlinear system can become unstable even though all other parameters of the filter are suitably tuned.

In case of a multibody model only, the geometrical properties can be precisely modeled, whereas, the accurate modeling of the forces and the actual distribution of the masses are complex in practice. As a consequence, the system deviates from the ideal behavior and errors occur at the acceleration level. Furthermore, the integration process and the multibody formulation may cause additional errors, however, they are negligible compared to the previous error. Therefore, in this study, only the acceleration terms are included in the covariance matrix of the plant noise for the multibody system, as in [31, 32] . Furthermore, since the states considered in the state estimator do not contain acceleration terms, Σ P must be calculated from its continuous-time counterpart. For example, it can be computed using Van Loan's method [36] of integration as in [32] . Note that if acceleration errors were considered in the proposed estimator, then the acceleration noise could be used in its discrete form as in [31] . The structure of Σ P for the multibody system with position and velocity estimation can be written as [32] :

where σz is the variance of all components of the continuous plant noise at acceleration level.

In this study, a hydraulic subsystem is coupled with the multibody system, therefore the pressure level noise coming from the hydraulics can be directly incorporated in Σ P as:

where σ p,D is the variance of all components of the discrete plant noise at the pressure level. Note that both σz and σ p,D are tuned by trial and error. However, σz is independent of the simulation time-step, as it is a continuous variance, whereas, σ p,D should be modified proportionally to the simulation time-step as it is a discrete variance.

In this section, the structure of the covariance matrix of the measurement noise is presented. It should be noted that the structure presented here is equally applicable to most real sensors currently in use. Nevertheless, the measurements in this study are built from a dynamic model of the coupled multibody system that has zero modeling error and acts as a real system, thus providing the ground truth. White Gaussian noise is generated and added to the measurements to represent the noise properties of real sensors. Therefore, the measurement noise properties are already known and are used to obtain the covariance matrix of the measurement noise. For example, the structure of Σ S with position and pressure sensors takes the form as:

where σ z and σ p are the standard deviations of the measurement noise at the position and pressure levels, respectively. Note that a similar sequence of pseudo-random noise is used in different combinations of sensors to ensure a fair comparison.

State estimator based on an indirect Kalman filter. . .

In this study, a hydraulically actuated four-bar mechanism, as shown in Fig. 4 , is used to demonstrate the state estimator explained in Sect. 3. The four-bar mechanism is modeled using the semi-recursive formulation, explained in Sect. 2.1. It has three bodies: crank, coupler and rocker and four revolute joints, where the joint between the rocker and the ground is a cut-joint for which two loop-closure constraint equations are introduced. Note that although a planar mechanism is presented in this study, the implementation of the methods is carried out in the Matlab environment in three-dimensions. The system has one degree of freedom. Note that the crank, coupler, and rocker are assumed to be rectangular beams, whose lengths are L 1 = 2 m, L 2 = 8 m, and L 3 = 5 m, and masses are m 1 = 2 kg, m 2 = 8 kg, and m 3 = 5 kg, respectively. The moment of inertia of the beams is considered as mL 2 12 , where m is its mass and L is its length. In the inertial reference frame, the positions of the bodies are represented by z 1 , z 2 and z 3 , respectively. The position vector of points E, F and G are, respectively, r E = [10, 0, 0] T m, r F = L 1 2 cos (z 1 ), L 1 2 sin (z 1 ), 0 T m and r G = − L 1 2 , 0, 0 T m, and point F is located at the center of mass of the crank.

The actuation of the four-bar mechanism is carried out using the hydraulic circuit shown in Fig. 4 . The hydraulic circuit consists of a pump with a constant pressure source p P , a tank with a constant pressure p T , a directional control valve with a control signal U , a throttle valve, a double-acting hydraulic cylinder and connecting hoses. It is assumed that the hydraulic circuit is ideal, that is, it has no leakage. According to the lumped fluid theory, the hydraulic circuit is divided into three control volumes, V 1 , V 2 , and V 3 , whose respective pressures, p 1 , p 2 , and p 3 , are computed by using Eq. (12) as:

where B e1 , B e2 , and B e3 are the effective bulk modulus of the respective sections calculated from Eq. (13), Q t is the volume flow rate calculated from Eq. (14), Q d1 and Q 3d are the volume flow rates calculated from Eq. (16), A 2 and A 3 are, respectively, the areas of the piston side and the piston-rod side within a cylinder, andṡ is the actuator velocity. In Eq. (39), V 1 , V 2 , and V 3 are calculated as:

where V h 1 , V h 2 , and V h 3 are the volumes of the respective hoses, and l 2 and l 3 are, respectively, the length of the piston side and the piston-rod side, that are calculated as:

where l 2 0 and l 3 0 are, respectively, the values of l 2 and l 3 at t = 0, | s | is the actuator length of the hydraulic cylinder, and s 0 is its value at t = 0. Note that the length of the hydraulic cylinder is l = l 2 + l 3 . The values ofṡ in Eq. (39) and | s | in Eq. (41) are computed by using z 1 andż 1 as:

whereṙ F is the velocity vector of point F . For simplicity, the force F c obtained from Eq. (19) is expressed in the form of Eq. (7) as:

where s X , s Y and s Z are the components of vector s along the axes of the inertial reference frame. The value of F c at t = 0 can be computed from the static configuration as: F c 0 = , where z 1 0 , z 2 0 , and z 3 0 are the values of z 1 , z 2 , and z 3 at t = 0. In the static configuration, p 1 0 = p 2 0 , and from Eq. (19) ,

where p 1 0 , p 2 0 , and p 3 0 are the respective values of p 1 , p 2 , and p 3 at t = 0. Note that the friction is neglected in static configuration. The four-bar structure is hydraulically actuated for 6 s, such that:

t < 1 s, 2.5 s ≤ t < 3.5 s, t ≤ 6 s 10 1 s ≤ t < 2.5 s −10 3.5 s ≤ t < 5 s (43) where t is the simulation run time. The set of variables used inside the trapezoidal integration scheme to solve the coupled multibody system can be written asx = z T , p T T = [z 1 , z 2 , z 3 , p 1 , p 2 , p 3 ] T . In the trapezoidal integration scheme, the error tolerance for position is 1 × 10 −10 rad and for pressure is 1 × 10 −2 Pa. The voltage, which corresponds to the spool position is integrated using the trapezoidal method and its error tolerance is 1 × 10 −10 V. The penalty factor, α, used in the study (Eq. (8)) is 1 × 10 9 . The parameters of the hydraulic circuit are shown in Table 1 .

In this study, dynamic models are used to provide a fair comparison for the implemented state estimator. The first reference model represents the actual mechanism that is modeled without any modeling error and is referred to as the "real system", as in [31, 32] , thus, providing the ground truth. The "real system" in this study can also be referred to as the "ground truth" or "reference model". Measurements are obtained from this model with an addition of white Gaussian noise to represent the noise properties of the actual sensors. The second model represents an imperfect representation of the "real system" or "reference model", with some parameters modified to simulate modeling errors, and is referred to as the "simulation model". The "simulation model" in this study can also be referred to as the "model", as in [31, 32] , or the "imperfect model". The modeling errors introduced in the simulation model can be seen in Table 2 . Furthermore, the implementation of the indirect Kalman filter on the simulation model described above is referred to as the "state estimator". The "state estimator" combines the "imperfect model" with noisy measurements obtained from the "reference model" to achieve the best possible estimations of the true state of the "reference model", which is unknown. The "state estimator" in this study can also be referred to as the "state observer". In the state estimator, the simulation model is corrected using the measurements of the real system, described above.

The errors introduced in the simulation model compared to the real system are in the force model. Note that when modeling a coupled multibody system, the geometry and mass can be accurately defined. However, the force models might have uncertainties in the modeling [31] . Therefore, incorrect values for gravity, as in [31] , and pump pressures, are considered in the simulation model compared to the real system, as shown in Table 2 . This introduces an incorrect force model such that the dynamics of the "simulation model" or "imperfect model" are affected throughout the simulation. Consequently, the "simulation model" will be out of synchronization compared to the "real system" or "reference model", just like any unmodeled force would affect. Furthermore, the ability to correct for the initial position and pressure errors in the simulation model provides a non-formal demonstration of the system observability.

For the present case study, the state vector considered for the state estimator is x =

That is, z 1 is selected as the independent relative joint coordinate and assumed to be valid throughout the simulation, as is the case with a hydraulically actuated machinery. The initial values of z 2 and z 3 are derived from the initial value of z 1 shown in Table 2 . To maintain the stability of the simulation process, the simulations are executed from static equilibrium. Note that gravity acts in the negative Y-direction. The initial covariance, P 0 , of the state estimator is a diagonal matrix whose first two diagonal elements are, respectively, 0.76 × 10 −2 rad 2 and 0.76 × 10 −2 rad 2 /s 4 , and the last three elements are 22.5 × 10 7 Pa 2 . In this study, the standard deviation of the measurement noise at the position, velocity and pressure levels are σ z = 1.745 × 10 −2 rad, σ ż = 9.839 × 10 −4 rad/s, and σ p = 0.15 × 10 5 Pa, respectively. Whereas, the values of the plant noise are σ 2 z = 11.163 × 10 −2 rad 2 /s 4 and σ 2 p,D = 259.81 × 10 7 Pa 2 , which are obtained by trial and error [31] . In this study, the simulations are run with a time-step of 1 ms, providing information about the coupled system at 1000 Hz.

In this study, a combination of position, velocity and pressure sensors is used, as shown in Fig. 4 . The advantage of using a coupled multibody system inside a Kalman filter is that it provides a mean to obtain the Jacobian matrix of the measurement model, h x , in a systematic way. All coordinates, velocities and pressures are available from the coupled multibody system, so that building the model of the sensors and obtaining their Jacobian is quite straightforward, as shown in the following subsections.

The devices used to measure the angular position of a body are encoders, which are commonly used in all kinds of machines to monitor angular magnitudes. Thus, in this study, an encoder is used as a position sensor at the location of the crank of the four-bar mechanism, as shown in Fig. 4 . It measures the angle of the crank, that is, z 1 , such that the measurement model is h (x) = [z 1 ]. As the states considered in the state estimator are x = [ z 1 , ż 1 , p 1 , p 2 , p 3 ] T , thus, h x can be written as: 

The state estimator developed in this study is also tested by incorporating a velocity sensor. The most common devices used to measure the angular velocity of a body are micro-electromechanical systems (MEMS) gyroscopes. MEMS gyroscopes are used in many applications such as cell phones, robots and autonomous vehicles. For the presented case study, a gyroscope is installed on the crank, as shown in Fig. 4 . Thus, it will measure the angular velocity of the crank, that is, ω 1 =ż 1 , such that the measurement model is h (x) = [ż 1 ] and h x can be written as:

The pressures in a hydraulic circuit can be measured using pressure sensors. There are many pressure sensors that can be used, such as gauge pressure sensors, which measure pressure relative to atmospheric pressure. In this study, pressure sensors can be installed at three hydraulic control volumes in the hydraulic circuit, as shown in Fig. 4 . When the pressure sensors are installed at control volumes V 1 and V 3 , for example, then they measure pressures p 1 and p 3 , such that the measurement model is h (x) = [p 1 , p 3 ] T , and h x can be written as:

This section presents the results of the state estimator applied to the hydraulically actuated four-bar mechanism, presented in Sect. 5. The position of the bodies of the simulation model at different instants of time is shown in Fig. 5 . In this study, a non-formal demonstration of the system observability is provided by the initial position error of the crank angle and the initial pressure errors of the hydraulic control volumes. The system is observable only if the position error (i.e. the initial 11 o error of the crank angle) and the pressure errors (i.e. the initial 0.5 MPa error of pressure p 3 , and 0.41 MPa error of pressures p 1 and p 2 , each) are corrected with a set of sensors. Accordingly, four sets of sensor combinations are used, as shown in Table 3 .

In this study, the simulations are run with a time-step of 1 ms, providing information about the coupled system at 1000 Hz. Furthermore, the sampling rates considered for the sensors are 1000 Hz (one measurement per simulation time-step); 500 Hz (one measurement available every two simulation time-steps), 200 Hz (one measurement every five simulation time-steps), and 100 Hz (one measurement every 10 simulation time-steps). The states of the estimator are not corrected when no measurements are available. Note that the simulation Table 2 . It should be noted that this study focuses only on the estimation accuracy and not on the computational efficiency of the estimator. The computational efficiency is ignored in this study because numerical methods (for Eqs. (23) and (27)) are employed in the Matlab environment and thus computational efficiency is expected to be low. Figure 6a presents the root mean square error (RMSE) of the different tests on the position level (the crank angle error), the velocity level (the crank angular rate error) and the acceleration level (the crank angular acceleration error). The RMSEs of the hydraulic pressures are provided in Fig. 6b . Note that the RMSEs are measured in % with respect to the absolute maximum value of the real system. It can be observed that sensor set-2 provides relatively better estimation accuracy compared to the other sensor sets because it utilizes all sensors at position, velocity and pressure levels. Furthermore, the accuracy of the estimation degrades when the sampling rate of the sensors is reduced. Fig. 7 Comparison of the crank angle with sensor set-1 providing data at 1000 Hz Note that sensor set-1 is unable to provide a result at a sampling rate of 100 Hz. This is because the terms of the covariance matrix grow, resulting in higher corrections, which is more difficult for the multibody model to handle, thus, reaching a maximum number of iterations. To stabilize the multibody simulation at 100 Hz sampling rate with sensor combination-1, the values of the covariance matrix of the plant noise can be reduced depending on the application. Even though the plant covariance noise may not be optimal, it can help to have smaller corrections by compromising on accuracy that will not unstabilize the multibody simulation. However, this was not done in this study so that different sensor combinations can be tested under the same conditions.

To better understand the behavior of the state estimator, the crank angle for the entire work cycle is shown in Fig. 7 . Here, the hydraulic cylinder tilts the four-bar structure outwards between 1-2.5 s; holds it in this position between 2.5-3.5 s; tilts it inwards between 3.5-5 s; and holds it in this position to complete the work cycle. In all the subsequent figures, the hydraulic actuation regions are highlighted in purple. In Fig. 7 , the RMSE of the crank angle is 0.23% with respect to the absolute maximum value of the real system, at 1000 Hz sampling rate of the sensors. Whereas the RMSE of the encoder measurement is 1.67%. Thus, the state estimator provides a more accurate estimation of the crank angle than the encoder measurement, which is also evident from Fig. 7 . Furthermore, the difference between the state estimator and the real system is almost indistinguishable in Fig. 7 . Therefore, the actual position and velocity errors, that is, the crank angle and angular rate errors, with respect to the real system are shown in Fig. 8 . Here the 95% confidence interval is consistent with the actual estimation errors, that is, it shrinks as the error decreases and grows as the error increases. Plots of this kind provide useful information about the system observability without the need for more formal observability analysis. Note that the 95% confidence interval is computed as ±1.96σ , where σ is the standard deviation calculated from the corresponding value of the covariance matrix, P + , associated with the state estimation.

In sensor set-1, the RMSE on the position and velocity levels are, respectively, 0.23% and 12.02% with respect to the absolute maximum value of the real system at 1000 Hz sampling rate of the sensors. Whereas, they are, respectively, 0.10% and 0.28% in case of sensor set-2 at 1000 Hz sampling rate. Thus, the overall estimation quality is improved by adding a velocity sensor, that is, a gyroscope, to a working sensor set, which is also shown Fig. 8b . Note that the RMSE of the gyroscope measurement is 0.29% in sensor set-2 at 1000 Hz sampling rate.

Furthermore, Fig. 9 shows the position and velocity estimation errors when the sampling rate of the sensors is 200 Hz in sensor set-1. The saw-tooth shape of the 95% confidence interval shows the evolution of the covariance matrix, that is, the covariance grows when there is no measurement and shrinks when a measurement is available. The tracking error follows a similar trend.

The estimations of the hydraulic pressures are shown in Fig. 10 , where the pressure estimation is almost indistinguishable from the real system. The RMSEs of the estimated pressures are, respectively, 0.34%, 0.33%, and 0.28% with respect to the absolute maximum value of the real system, at 1000 Hz sampling rate of the sensors. Whereas, the RMSEs of the pressure sensors are, respectively, 0.35%, 0.34%, and 0.29%, at 1000 Hz sampling rate. Thus, the estimation of the hydraulic pressures is slightly improved in comparison with the pressure sensor measurements. Furthermore, for the plots in Fig. 10 , the pressure errors are shown in Fig. 11a .

Note that in sensor set-1 (Fig. 10) the measurements for all pressures, p 1 , p 2 , and p 3 , are used because they are assumed to be independent inside the filter. However, if an additional velocity sensor, that is a gyroscope, is used, then it enables one to remove one of the pressure sensors from either end of the hydraulic cylinder (Eq. (39)), such as for sensor sets 3 and 4. Otherwise, removing a pressure sensor without adding a velocity sensor would result either in a maximum number of iterations of the state estimator or a gradually increasing covariance of the pressure estimation. Note that the limit on the maximum number of iterations is defined within the implicit integrator used in this study. Moreover, the lack of convergence within the integrator is caused by the growth of the terms of the covariance matrix that leads Fig. 9 Position and velocity estimation errors with respect to the real system using sensor set-1 providing data at 200 Hz Fig. 10 Comparison of hydraulic pressures with sensor set-1 providing data at 1000 Hz to too large corrections, thus demonstrates the lack of observability. The pressure estimation error using sensor set-4 is shown in Fig. 11b .

At a lower sampling rate, 200 Hz, of the sensors, the pressure errors are shown in Fig. 12 . At this sampling rate of the sensors, the 95% confidence interval of the pressure estimations follows a similar saw-tooth shape evolution as in the case of the position and velocity estimations.

This paper introduced a novel state estimator developed by combining a multibody model and an indirect Kalman filter in the framework of hydraulically driven systems. An indirect Kalman filter that utilizes the exact Jacobian matrix of the state transition matrix at the position and velocity levels found in the literature has been extended for hydraulically actuated systems. The structures of the covariance matrices of the plant and measurement noise were also proposed. The multibody system was described using a semi-recursive formulation and the hydraulic subsystem using the lumped fluid theory. These two models were combined using a monolithic approach. The state estimator considered modeling errors in the force model, such as incorrect values of gravity and pump pressure, that affected the plant throughout the simulation. A non-formal demonstration of the system observability was provided by the introduction of errors in the initial values of position and pressure. The measurements in this study were obtained from a dynamic model, which was considered as the ground truth, with an addition of white Gaussian noise that represented the noise properties of the actual sensors.

The developed state estimator was illustrated on a hydraulically actuated four-bar mechanism using four sensor configurations with four different sampling rates. The state estimator provided a more accurate estimation, especially at the position level, compared to the Fig. 12 Pressure estimation errors with respect to the real system using sensor set-1 providing data at 200 Hz measurements. Moreover, the confidence interval was consistent with the actual estimation errors. Furthermore, the overall estimation quality is improved by adding a velocity sensor, that is, a gyroscope, to a working sensor set. Moreover, the addition of a gyroscope enabled one to remove one of the pressure sensors from either end of the hydraulic cylinder, which is otherwise difficult under the given set of conditions. It has been observed that the accuracy of the estimation degrades when the sampling rate of the sensors is reduced. At a lower sampling rate, the evolution of the covariance matrix follows a saw-tooth shape, that is, the covariance grows when there is no measurement and shrinks when there is measurement available. The results demonstrated the efficacy of the proposed state estimator.

In future works, the proposed method will be tested on real world systems, such as a hydraulic crane, where the size of the system and its complexity are higher. The studies may also be directed towards reducing the number of pressure sensors, as the pressure build-up in the hydraulic sections is inter-dependent. The idea can be to protect the pressure sensors from damage caused by impacts from other parts of the working machinery in a limited working space. The estimation of hydraulic forces acting on a coupled multibody system will also be considered in future work. first author also acknowledges the immense support of Ms. Heena Bhatia Jaiswal during the outbreak caused by the COVID-19 pandemic.

Funding Note Open Access funding provided by LUT University (previously Lappeenranta University of Technology (LUT)).

The authors declare that they have no conflict of interest.

Open Access This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article's Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article's Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http://creativecommons.org/licenses/by/4.0/.

A tutorial on particle filters for online nonlinear/non-gaussian Bayesian tracking

A modified Lagrangian formulation for the dynamic analysis of constrained mechanical systems

Augmented Lagrangian and mass-orthogonal projection methods for constrained multibody dynamics

Multibody dynamic systems as Bayesian networks: applications to robust state estimation of mechanisms

A continuous velocity-based friction model for dynamics and control with physically meaningful parameters

Improved particle filter for nonlinear problems

Modeling and solution methods for efficient real-time simulation of multibody dynamics

Intelligent simulation of multibody dynamics: space-state and descriptor methods in sequential and parallel computing environments

Real-time state observers based on multibody models and the extended Kalman filter

A combined penalty and recursive real-time formulation for multibody dynamics

Real-time multibody dynamics and applications

A smoothly constrained Kalman filter

Kalman Filtering: Theory and Practice with MATLAB

Flexible semi-empirical models for hydraulic flow control valves

Gamification procedure based on real-time multibody simulation

Deformable terrain model for the real-time multibody simulation of a tractor with a hydraulically driven front-loader

Comparing double-step and penaltybased semirecursive formulations for hydraulically actuated multibody systems in a monolithic approach

Efficiency comparison of various friction models of a hydraulic cylinder in the framework of multibody system dynamics

A fast and simple semi-recursive formulation for multi-rigid-body systems

Kinematic and Dynamic Simulation of Multibody Systems: The Real-Time Challenge

A new approach to linear filtering and prediction problems

Control of the nonlinear dynamics of a truck and trailer combination

Programming for Computations-MATLAB/Octave: A Gentle Introduction to Numerical Simulations with MATLAB/Octave

An efficient unified method for the combined simulation of multibody and hydraulic dynamics: comparison with simplified and co-integration approaches

State estimation using multibody models and non-linear Kalman filters

Validation of a real-time multibody model for an x-by-wire vehicle prototype through field testing

Application of an augmented Lagrangian approach to multibody systems with equality motion constraints

Computationally efficient approach for simulation of multibody and hydraulic dynamics

Combined semi-recursive formulation and lumped fluid method for monolithic simulation of multibody and hydraulic dynamics

State observers based on detailed multibody models applied to an automobile

State and force observers based on multibody models and the indirect Kalman filter

Accuracy and efficiency comparison of various nonlinear Kalman filters applied to multibody models

Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches

Kalman filtering with state equality constraints

Towards benchmarking of state estimators for multibody dynamics

Computing integrals involving the matrix exponential

Fluid Power Systems: Modeling, Simulation, Analog, and Microcomputer Control

Publisher's Note Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.

Suraj Jaiswal 1 · Emilio Sanjurjo 2 · Javier Cuadrado 2 · Jussi Sopanen 1 · Aki Mikkola 1 S. Jaiswal suraj.jaiswal@lut.fi