International Journal of Non-Linear Mechanics ] (]]]]) ]]]]]]

Contents lists available at SciVerse ScienceDirect

International Journal of Non-Linear Mechanics

journal homepage: www.elsevier.com/locate/nlm

State estimation using multibody models and non-linear Kalman lters

Roland Pastorino a,n, Dario Richiedei b, Javier Cuadrado a, Alberto Trevisani b
a
b

bal s/n, 15403 Ferrol, Spain

a, Escuela Politecnica Superior, Mendiza
Mechanical Engineering Laboratory, University of La Corun
DTG Department of Management and Engineering, Universita degli Studi di Padova, Stradella S. Nicola 3, 36100 Vicenza, Italy

a r t i c l e i n f o

Keywords:
State estimation
Kalman lters
Multibody dynamics
Non-linear ltering

abstract
The aim of this work is to provide a thorough research on the implementation of some non-linear
Kalman lters (KF) using multibody (MB) models and to compare their performances in terms
of accuracy and computational cost. The lters considered in this study are the extended KF (EKF) in
its continuous form, the unscented KF (UKF) and the spherical simplex unscented KF (SSUKF).
The MB formulation taken into consideration to convert the differential algebraic equations (DAE) of
the MB model into the ordinary differential equations (ODE) required by the lters is a state-space
reduction method known as projection matrix-R method. Additionally, both implicit and explicit
integration schemes are used to evaluate the impact of explicit integrators over implicit integrators in
terms of accuracy, stability and computational cost. However, state estimation through KFs is a closedloop estimation correcting the model drift according to the difference between the predicted
measurement and the actual measurement, what limits the interest in using implicit integrators
despite being commonly employed in MB simulations. Performance comparisons of all the aforementioned non-linear observers have been carried out in simulation on a 5-bar linkage. The mechanism
parameters have been obtained from an experimental 5-bar linkage and the sensor characteristics from
off-the-shelf sensors to reproduce a realistic simulation. The results should highlight useful clues for
the choice of the most suitable lters and integration schemes for the aforementioned MB formulation.
& 2013 Elsevier Ltd. All rights reserved.

1. Introduction
In recent years, the interest in state estimation in mechanical
systems, and in particular in MB systems, has increased. A state
observer is a dynamical system that employs the model of a real
system in order to provide estimates of its internal states, given
measurements of the inputs and outputs of the real system. It is
typically a computer-implemented mathematical model. This allows
to extract otherwise unmeasurable variables of the real system [1]. In
practice, the knowledge of the system state allows synthesizing
effective state controllers, replacing expensive sensors with virtual
sensors, improving reliability by making the controlled system robust
to sensor fault. On the one hand, many works in the literature address
the synthesis of optimal observers for linear mechanical systems
through the KF [2,3]. On the other hand, when non-linear mechanical
systems are considered, only sub-optimal approaches based on the
LKF (linearized KF) have usually been adopted to ensure highfrequency and hard real-time estimation [46]. Indeed, up to now,
the use of other types of non-linear observers in MB systems has only

Corresponding author. Tel.: 34 981 33 74 00x3870.

E-mail addresses: roland.pastorino@gmail.com, roland@rolandpastorino.com,
rpastorino@udc.es (R. Pastorino).

been investigated marginally. This lack of use is mainly due to the

difculties in performing fast integration of the non-linear equations
of motion, which usually involve high frequency dynamics and severe
non-linearities. In Refs. [710], it is shown how the recent improvements in MB dynamics raise the possibility of employing complex
real-time models in state observers. In these papers, the estimation
was performed through the EKF in its continuous form [11]. Generally
speaking, the EKF is the most widely used algorithm for non-linear
estimation. However, when non-linearities are severe, EKF often gives
unreliable or divergent estimates. In addition, the linearization
requires a Jacobian matrix which could either be difcult to calculate
or not exist. Implementation difculties are particularly relevant if the
system model is represented by DAE, as it is common in the MB eld.
Recent developments in Kalman ltering algorithms make it
possible to overcome part of the aforementioned EKF shortcomings. The SPKFs (sigma-point Kalman lters), also called LRKFs
(linear regression Kalman lters), take advantage of a set of
deterministically calculated weighted samples (referred to as
sigma-points or regression points) to capture at least the rst
and second order moments of the actual state probability distribution. This approximation made by the SPKFs is therefore
more accurate than the EKF linearization, and it does not require
the calculation of Jacobian matrices. Different sigma-point set
denitions lead to different lter characteristics, allowing priority

0020-7462/$ - see front matter & 2013 Elsevier Ltd. All rights reserved.
http://dx.doi.org/10.1016/j.ijnonlinmec.2013.01.016

Please cite this article as: R. Pastorino, et al., State estimation using multibody models and non-linear Kalman lters, International
Journal of Non-Linear Mechanics (2013), http://dx.doi.org/10.1016/j.ijnonlinmec.2013.01.016i

R. Pastorino et al. / International Journal of Non-Linear Mechanics ] (]]]]) ]]]]]]

to be given to either estimation accuracy or computational

efciency. The most relevant variants are the UKF [1214], the
CDKF (central difference KF) [15], the SSUKF [16,17] and their
respective numerically stable forms, the so-called square-root
forms [18,19]. Therefore, a natural approach to overcome the EKF
problems and to improve the estimation in MB systems would be
to use SPKFs. To the best of the authors knowledge, SPKFs have
never been applied to the estimation of MB models.
The aim of this work is therefore to discuss and investigate the
implementation of non-linear KFs using MB models and to
compare their performances in terms of accuracy and computational cost. In particular, the EKF in continuous-form, the UKF and
the SSUKF will be investigated and implemented with reference
to a rigid 5-bar linkage.

2. Non-linear Kalman lters

In the whole document, the following typesetting rules for the
mathematical notation have been adopted: vectors and matrices
are in upright bold types and scalars are in upright normal
font types.
The original formulation of the KF is intended only to linear
systems whose inputs are supposed to be known. To apply the KF
to non-linear systems, the process and measurement models have
to be approximated in some way. The differences between the KF
variants for non-linear systems arise mainly from distinct approximation approaches. In this section, two types of non-linear
Kalman lters are discussed: the EKF, which is the de facto KF
for non-linear systems, and the SPKFs. For the EKF, only the
continuous-time form is taken into account while only the
discrete-form is considered for the SPKFs. The advantages of
discrete-form lters over continuous-form ones are exemplied
in Section 5.
2.1. The Kalman lter
The estimation problem for this lter consists in estimating
the state of a linear stochastic system. Both continuous-time and
discrete-time forms of the equations are described below. First,
the continuous-time equations for the system dynamics and the
measurement model are presented in Eqs. (1) and (2) respectively. In this lter, the system dynamics are described by rst
order ordinary differential equations (ODE) with independent states.
For simplicitys sake, the process noises wt and the measurement noises vt have been considered additive white zero-mean
Gaussian noises
_ fxt,t wt,
xt
yt hxt vt,

wt  N 0,Qt,
vt  N 0,Rt,

1
2

_
where x(t) represents the states of the system, xt
is the time
derivative of the state vector, f(t) is the dynamics system function,
y(t) contains the measurements and h(t) is the measurement
sensitivity matrix. The differential equation for the recursive
estimation of the states is presented as follows:
^
^
x^_ Efxt,t
wt Ktytyt
^ Ktytyt
^
Fxt

^ Ehxt,t
^
^
yt
vt Hxt

where H(t) is the linearization of h(t) (here Ht ht as the latter

is linear) and P(t) is the covariance matrix of state estimation
uncertainty which is obtained using the differential equation (6),
also called Riccati equation
T
P_ FP PFT KRK GtQGT t

where G(t) is the coupling matrix between the process noises and
the states of the system. The case in which f(t) is a non-linear
function will be treated in Section 2.2.
The estimation problem can also be formulated in discretetime form which is the most suitable form for hardware implementation as the estimation is divided in two parts depending on
the availability of sensor information. The system nominal and
measurement models, which are now linear difference equations
with independent states, are shown in Eqs. (7) and (8). The
aforementioned assumptions have also been made for the process
and measurement noises
xk 1 /k xk wk ,
yk hk xk vk ,

wk  N 0,Qk

vk  N 0,Rk

7
8

where k represents the time step and /k is the state transition

matrix. The estimates of the states and the covariance matrix of
state estimation uncertainty are now calculated using the following equations:

x^ k 1 E/k x^ k wk  Uk x^ k

T
T
T
^
^
P
k 1 Exk 1 x k 1 xk 1 x k 1  Uk Pk Uk Gk Q k Gk


10

where Uk is the linearization of /k (here Uk /k as /k is linear),

 indicates the a priori values of the variables (i.e. before the
measurement information is used) and indicates the a posteriori values (i.e. after the measurement information is used).
To calculate the a posteriori estimates of the state, the estimation
is split in two distinct parts: the time-update and the
measurement-update parts. When no information is available
from the sensors, the estimation relies only on the systems
model, this is the time-update. In this case the posterior state
estimates and the posterior values of Pk are obtained using the
following equations:

x^ k 1 x^ k 1

11

Pk 1 P
k1

12

If some information is available from the sensors, the estimation

relies now on both the systems model and the sensors information; this is the measurement update, which provides a closed
loop correction of the estimates. The a posteriori state estimates
and the a posteriori covariance matrix are now obtained using the
following equations:



x^ k 1 x^ k 1 x^ k 1 K k 1 yk 1 y^ k 1

13


Pk 1 Pk 1 P
k 1 K k 1 Hk 1 Pk 1

14

The Kalman gain and the predicted measurements are calculated

with Eqs. (15) and (16) respectively

^ are
where E is the expectation, Kt is the Kalman gain matrix, yt
the predicted measurements and F(t) is the linearization of f(t)
(here Ft ft as the latter is linear). The Kalman gain and the
predicted measurements are calculated using the following equations:



K k 1 Exk 1 x^ k 1 yk 1 y^ k 1 T 

Kt PtHT tR1 t




y^ k 1 Ehk 1 x^ k 1 vk  Hk 1 x^ k 1

Eyk 1 y^ k 1 yk 1 y^ k 1 T 1

T

T
1
P
k 1 Hk 1 Hk 1 Pk 1 Hk 1 R

15
16

Please cite this article as: R. Pastorino, et al., State estimation using multibody models and non-linear Kalman lters, International
Journal of Non-Linear Mechanics (2013), http://dx.doi.org/10.1016/j.ijnonlinmec.2013.01.016i

R. Pastorino et al. / International Journal of Non-Linear Mechanics ] (]]]]) ]]]]]]

2.2. The extended Kalman lter

The EKF is the de facto KF for non-linear systems. This lter
propagates Gaussian random variables (GRVs) through the system
dynamics represented by either the non-linear continuous-time
equation (1) or the non-linear discrete-time equation (7). Unlike
in the KF, here f(t), h(t), /k and hk can be non-linear functions.
While the optimal quantities can be calculated in Eqs. (3), (5),
(9) and (16) for linear systems, this is not the case for non-linear
systems. As a consequence, the optimal terms have to be
approximated in some way. The EKF approximates rst the
continuous-time equation (3) by Eq. (17) where the derivative
of the state vector is the function of the latter and then Eq. (5) by
Eq. (18), where the predicted measurements are the function
of the estimates. As for the KF, the Kalman gain is obtained using
Eq. (4) but the covariance matrix of the state estimation uncertainty given by Eq. (6) is approximated by means of linearizations
F and H as shown in Eq. (19)

Fig. 1. UKF: weighted sigma-points for a 2-dimensional Gaussian random variable.

17

After that, each sigma-point is propagated through the system

dynamics equation (7) as illustrated in Eq. (21). There are as many
function evaluations as sigma-points. It is worth pointing out that
these function evaluations are independent from each other
allowing to parallelize the computations using several cores for
example

^ Ehxt,t
^
^
yt
vt C hxt

18

vk 1 /k vk

_ CFtPt PFT tKRK T GQGT

Pt

19

^
^
^
^
x^_ Efxt,t
wt Ktytyt
Cfxt
Ktytyt

2.3. The sigma-point Kalman lters

The SPKFs are KFs for non-linear systems with a linearization
approach substantially different from the one of the EKF [14].
Instead of rst approximating the estimates as function of the
prior estimates propagated through the non-linear system function and then linearizing the dynamic equations to determine the
covariances, a set of deterministically chosen weighted sample
points is propagated through the non-linear system function.
The sample points, called sigma-points, capture at least the rst
two moments (mean and covariance) of the prior and posterior
(i.e. after propagation through the non-linear function) random
variables. Two SPKF versions are briey reminded hereafter: the
UKF and the SSUKF. All the equations and the most important
characteristics of these lters when used with MB models are
presented.
2.3.1. The unscented Kalman lter
As previously mentioned, the time-update equations of the
UKF differ substantially from the ones of the EKF. Firstly, the set
of nsp sigma-points for the UKF has to be calculated, where
nsp 2L 1 points (L is the dimension of the state vector).
The zeroth point is the unchanged state estimate while the rest
of points is calculated using the zeroth point and the square-root
decomposition of the covariance matrix of state estimation
uncertainty as demonstrated below
8
x^
i0
>
< k
p
^
x

P
i 1, . . . ,L
vk i
k
k i
20

>
: x^ gp
P
i L 1, . . . ,2L

k
k i
p
where vk i is the i-th sigma-point, g L l, l a2 L k, a and
p
k are user-dened tuning parameters,  is the matrix squareroot using lower triangular Cholesky decomposition and i
represents the i-th column. 0 o a r 1 is a scaling factor dening
the extension of the spread of the sigma-points around the mean
of the estimates. k is another scaling factor, usually set to 0. Fig. 1
exemplies the sigma-point set for a 2-dimensional Gaussian
random variable (nsp 5). The size of the points represents their
possible weights.

21

The a priori state estimates and covariance matrix of state

estimation uncertainty are approximated by taking the weighted
mean and covariances of the propagated sigma-points as shown
in the following equations:

x^ k 1 E/k x^ k wk  C

nX
sp 1


wm
i vk 1 i

22

i0



P^ k 1 Exk 1 x^ k 1 xk 1 x^ k 1 T 

nX
sp 1

T

^
^
wci v
k 1 ix k 1 vk 1 ix k 1

23

i0
c
m
2
c
m
where wm
0 l=L l, w0 w0 1a b, wi wi 1=2L l
for i 1 . . . nsp 1 and b is a scaling factor used to control the
weighting of the zeroth sigma-point. Then, if no information is
available from the sensors, Eqs. (11) and (12) are used.
If some information is available, the measurement-update
equations, which also differ substantially from the ones of the
EKF, are employed. The Kalman gain is obtained using the
weighted covariances as shown below


K k 1 Exk 1 x^ k 1 yk 1 y^ k 1 T 


Eyk 1 y^ k 1 yk 1 y^ k 1 T 1

Pxk yk Pyk yk

nX
sp 1

T

^
^
wci v
k 1 ix k 1 Y k 1 iy k 1

i0

nX
sp 1

T

^
^
wci Y 
k 1 iy k 1 Y k 1 iy k 1

24

i0

Finally the predicted measurements are approximated by the

weighted means of the estimates propagated through the measurement sensitivity matrix hk , as can be seen in Eqs. (25) and
(26). The a posteriori covariance matrix is now obtained using
Eq. (27) and the a posteriori state estimates using Eq. (13)


y^ k 1 Ehk 1 x^ k 1 vk  C

nX
sp 1

wci Y 
k 1 i

25

i0

Y
k 1 hk 1 vk 1

26
T

Pk 1 P
k 1 K k 1 Pyk yk K k 1

27

Please cite this article as: R. Pastorino, et al., State estimation using multibody models and non-linear Kalman lters, International
Journal of Non-Linear Mechanics (2013), http://dx.doi.org/10.1016/j.ijnonlinmec.2013.01.016i

R. Pastorino et al. / International Journal of Non-Linear Mechanics ] (]]]]) ]]]]]]

2.3.2. The spherical simplex unscented Kalman lter

The structure and the equations of the SSUKF are similar to
those of the UKF [17]. The rst difference between the two lters
is the rule adopted for the selection of the sigma-point set, and
consequently their number: nsp L 2 for the SSUKF and
nsp 2L 1 for the UKF. As a consequence, the number of
function evaluations is smaller for this lter, meaning that the
computational cost is reduced. The sigma-point set for the SSUKF
is calculated using the following equation:
8 " j1 #
>
v0
>
>
for i 0
>
>
>
0
>
>
>
2
3
>
>
>
vj1
>
>
i
>
6
7
>6
<
7 for i 1, . . . ,j
1
28
vji 4 p 5
>
jj 1w1
>
>
>
>
2
3
>
>
0j1
>
>
>
>
6
7
>
1
for i j 1
> 4 p
5
>
>
>
:
jj 1w1
where j 2, . . . ,n, w0 is the weight of the zeroth sigma-point and
also a user-dened parameter that affects the fourth and higher
moments of the sigma-points set (0 r w0 r 1), wi are the weights
of the rest of sigma-points (wi 1w0 =n 1) and nally the
initial values
the
set of sigma-points are: v10 0,
pto calculate p

v11 1= 2w1 and v12 1= 2w1 . All the points (apart from the
zeroth
point) lie on a hypersphere, the radius of which is
p
L=1w0 , as it is exemplied in Fig. 2. The second difference
with respect to the UKF is related to the weights used in (22)(24)
where only one set of weights is employed: wi for i 0, . . . ,nsp 1.

independent from each other; in other words the equations of

motion have to be ODEs. In this research, a MB formulation has been
employed to convert the DAEs into ODEs: the state-space reduction
method known as projection matrix-R method [20]. This formulation eliminates the Lagrange multipliers related to the restrictions in
such a way that the variables behave as if they were independent.
3.1. Matrix-R methodA state-space reduction method
This method transforms the DAEs expressed in dependent
variables into ODEs expressed in independent variables. Therefore, the ODEs dimension ni is equal to the number of degrees of
freedom (DOF) of the system. Eq. (30), the rst base equation of
this method, relates the vector of dependent velocities q_ (dimension nd ) to the independent vector z_ (dimension ni , ni o nd )
q_ Rz_

30

Matrix R is calculated rst using the derivative of the constraint

equations, as shown in Eq. (31) and then expressing the dependent velocities as linear combinations of the independent ones

Uq q_ 0

31

The second base equation, Eq. (32), is the time-derivative of Eq. (30)
q Rz R_ z_

32

Finally, the equations of motion, shown in Eq. (33), are derived by

premultiplying Eq. (29) by RT , by then substituting Eq. (32) and
nally by removing the Lagrange multipliers using Eq. (31)
1
_ M Q
z RT MR1 RT Q MR_ z

33

The dynamics of an MB system can be represented by Lagranges

equation (29) which lead to a set of second order differential algebraic
equations (DAE) of index 3

The formulations in independent coordinates, like the one obtained

through the matrix-R method, have the advantage of reducing
considerably the number of equations to be integrated (ni). However, this imposes an increase in the computational cost since the
position and velocity problems have to be solved after each function
evaluation.

Mq UTq l Q

3.2. Integration schemes

3. MB formulation and integration schemes

U0

29

where M is the positive semidenite mass matrix, q is the vector of

dependent accelerations, U is the vector of constraints, Uq is the
Jacobian matrix of the constraints, k are the Lagrange multipliers
and Q contains the external forces, the velocity-dependent inertia
forces and those obtained from a potential. These equations are
not independent from each other as they are linked by the
constraint equations. However, with a view to employ MB models
in state estimation, the KF approach requires the equations to be

Two integration schemes have been considered in order to

integrate the equations of motion: the trapezoidal rule (TR),
an implicit scheme shown in Eq. (34), and the second order
explicit RungeKutta method (RK2) presented in Eq. (35)
vk 1 vk
vk 1 vk

Dt
2

Dt
2

v_ k v_ k 1

34

k1 k2

35

with
k1 ftk ,vk ,

k2 ftk Dt,vk Dtk1

_
and f(t) is a system of rst-order ODE
where v t qtqt
equations coming from reshaping the second-order ODE equations
obtained with the matrix-R method and using variable duplication. Eq. (36) demonstrates how f(t) is calculated
_
q F t,q, q
_ 
) vt

Fig. 2. SSUKF: weighted sigma-points for a 2-dimensional Gaussian random variable.

"

_
wt
wt

"

#
F t,q,w
 fvt,t
q_

36

If an MB formulation in independent coordinates is used, q has to

be replaced by z in the previous equations. Implicit integrators
are often employed in MB simulations to overcome stability
problems and energy loss while explicit integrators are commonly used in applications requiring hard real-time.

Please cite this article as: R. Pastorino, et al., State estimation using multibody models and non-linear Kalman lters, International
Journal of Non-Linear Mechanics (2013), http://dx.doi.org/10.1016/j.ijnonlinmec.2013.01.016i

R. Pastorino et al. / International Journal of Non-Linear Mechanics ] (]]]]) ]]]]]]

4. State estimation using non-linear Kalman lters with

MB models
In the previous sections, several types of non-linear Kalman
lters have been reminded as well as the MB formulation known
as matrix-R method. In order to employ MB models with these
lters, the analytical equations of motion have to be reshaped or
integrated before using them as system dynamics equations of
the lters. Various approaches will be treated in this section.
4.1. The extended Kalman lter with MB models
In previous researches, the EKF in its continuous-time form
has been employed with MB models [710,21,22]. The corresponding equations are briey reminded in order to be compared
with the other approaches presented in this paper. Eq. (33) can be
reshaped using variable duplication when dening the state
vector (shown in Eq. (37) in order to convert the second order
ODE (Eq. (33)) into a rst order ODE (Eq. (38)). As a consequence,
the latter equation and Eq. (1) have now the same form meaning
that the EKF equations in continuous-time can be applied
"
#
zt
_
xt
where wt zt
37
wt
"
_ 
xt

# "
# "
#
wt
_
wt
zt


 fxt,t
1
_
_
RT MR1 RT Q MR_ z
wt
M Q
38

Taking into account the chosen state vector, Eq. (17) can be
written as shown in Eq. (39)
# " z#
" # "
^
w
z^_
K
^

39

T
T
1
w yy
^
R MR R Q MR_ w
^_
w
K
z

where K and K are the two blocks of K. It is worth pointing out

that all the elements of the state vector are independent by
denition. Therefore, when the state vector contains positions
and velocities, the Kalman corrections are applied to velocities
and accelerations independently. This implies that the timederivative relation between the position and the velocity as well
as between the velocity and the acceleration is not thoroughly
respected. This is not a problem since the lter tries to provide
the best estimate of the velocity vector, which should overcome
the effect of the derivation of the position estimation error. The
same comment applies also to the estimate of the acceleration.
The resulting equations are then integrated by means of the TR.

reducing the overall computational cost. In this research, the state

vector has been chosen equal to the independent acceleration
In this way, the time-derivative relation between
vector z.
positions, velocities and accelerations is preserved. The equations
of motion (Eq. (7)) and the a posteriori state estimates (Eq. (13))
corresponding to the new state vector are presented in Eqs. (40)
and (41). A direct physical signicance can be given to the Kalman

corrections, namely K k 1 yk 1 y^ k 1 in Eq. (41). Indeed as

xk z k , the Kalman corrections are also accelerations, meaning

that these corrections can be understood as forces introduced to
guide the system towards its real motion (partially given by the
sensors information) after the information from a sensor is
available
z k 1 /k z k wk

40


z^ k 1 z^ k 1 z^ k 1 K k 1 yk 1 y^ k 1

41

Once the state vector has been dened, the lters equations can
be applied. First, the set of sigma-point is calculated using Eq. (20)
for the UKF or Eq. (28) for the SSUKF. Then, each sigma-point is
propagated through the non-linear discrete-time system function
(Eq. (21)), in other words, a function evaluation of the MB model
is performed for each sigma-point. The posterior independent
acceleration vector z^ k 1 is obtained using Eq. (22). Then, it is used
to assess the posterior independent position (zk 1 ) and velocity
(z_ k 1 ) vectors by means of the TR. If no information from the
sensors is available, the covariance matrix is obtained through
Eq. (10) and the time-update equations (11) and (12) are applied.
If any sensor data are available, the measurement-update equations (24)(27) and (41) are applied.

5. Observer performance comparisons using a 5-bar linkage

5.1. Test case description
A 5-bar linkage has been employed to exemplify the implementation and the performances of all the aforementioned nonlinear observers. The mechanism parameters have been obtained
from the experimental 5-bar linkage shown in Fig. 3 and the
sensor characteristics from off-the-shelf sensors to reproduce a
realistic simulation. A scheme of the mechanism is shown in Fig. 4
where the points A and E are xed points. The mechanism
has been modeled using mixed coordinates [20] with the vector
of dependent Cartesian coordinates presented in the following
equation:

4.2. The unscented Kalman lters with MB models

qT xB yB xC yC xD yD f1 f2 

The approach to employ the projection matrix-R method with

the UKF and the SSUKF is described hereafter. In order to match
the non-linear difference equations of the lter (Eq. (7)), the
equations of motion (Eq. (33)) have to be integrated using either
the TR (Eq. (34)) or the RK2 method (Eq. (35)). Subsequently, the
choice of the variables to include in the state vector is a crucial
point that will condition the implementation of the remaining
equations of the lter. The rst important comment is that, unlike
in the EKF, the variable duplication is not imposed in this lter.
As a consequence, there are no constraints for the selection of the
states apart from the independence of the equations. As the
projection matrix-R method is a formulation in independent
coordinates, the state vector could be made for instance of some
of the following vectors: zk , z_ k or z k . Nevertheless, another
important issue when choosing the state vector is to attempt to
maintain the dimension of the state vector as small as possible in
order to calculate the minimum number of sigma-points, thus

As a rst step in this research, the motion of the real

mechanism has been simulated but the magnitudes that correspond to sensor data are passed to the observers with their
respective noises and sample rates. After that, some known errors
(lengths, mass, inertia measuring errors for example) have to be
considered for the system nominal model of the lters, in order to
allow the simulated real mechanism and the lters model to have
similar but different behavior. Employing a simulated real
mechanism allows a comparison of all the states, including those
that cannot be measured experimentally. As a consequence, all
the variables of the lters model (i.e. the virtual sensors and the
variables estimated through the state observer) can be compared
to their exact magnitudes. In this way, comparisons of the lters
performances are more genuine and comprehensive.
A free motion under the effect of gravity has been simulated,
both cranks starting from the horizontal conguration. Figs. 57
show this motion for the real mechanism, the system nominal
model used in the lters (i.e. an open-loop estimation) and the

42

Please cite this article as: R. Pastorino, et al., State estimation using multibody models and non-linear Kalman lters, International
Journal of Non-Linear Mechanics (2013), http://dx.doi.org/10.1016/j.ijnonlinmec.2013.01.016i

R. Pastorino et al. / International Journal of Non-Linear Mechanics ] (]]]]) ]]]]]]

Velocity (rad/s)

4
2
0
2
4
6
0

0.5

1.5
Time (s)

2.5

Fig. 6. Angular velocity of the left crank. (For interpretation of the references to
color in this gure legend, the reader is referred to the web version of this article.)

20
Acceleration (rad/s2)

Fig. 3. 5-bar linkage used for parameter identication.

10
0
10
20
0

Fig. 4. Scheme of the 5-bar linkage.

1.5
Time (s)

2.5

Fig. 7. Angular acceleration of the left crank. (For interpretation of the references
to color in this gure legend, the reader is referred to the web version of this
article.)

Position (rad)

0.5

Table 1
Color code for the gures of the 5-bar linkage.

0
1
2
3
0

0.5

1.5
Time (s)

2.5

Fig. 5. Angle of the left crank. (For interpretation of the references to color in this
gure legend, the reader is referred to the web version of this article.)

observers. The color code for the gures is shown in Table 1. The
behavior discrepancies between the real mechanism and the
system nominal model without observer (green) are clearly
demonstrated. It is worth pointing out that all the observers
follow the motion of the real mechanism with accuracy.
5.2. Comparison of observer performances
As previously mentioned, the motion of the real mechanism is
completely known as it is simulated. To provide a clear evaluation
of the observer accuracy, the estimation errors and their rst and
second time-derivatives are compared. The sensors for all the
lters are the two crank encoders. Figs. 810 show the errors in
positions, velocities and accelerations of the left crank for an
integration time step Dti of 2 ms and an update time step of the
sensors Dts of 2 ms. This means that data from each sensor
is available at each integration time step. In a similar way,
Figs. 1113 show the same errors but for an update time step of

Color

Filter

MB formu.

Integ.

Note

Gray
Green
Black
Red
Brown
Blue

No
EKF
UKF
SSUKF
SSUKF

Matrix-R
Matrix-R
Matrix-R
Matrix-R
Matrix-R

TR
TR
TR
TR
RK2

Encoder noise
System model

the sensors Dts of 6 ms. The CPU time for all the mentioned lters
with respect to the lter with the lower computational cost is
presented in Table 2. The RMSE (root-mean-square error), where
the error is the difference between the predicted measurements
and the actual measurements, is also presented in Table 2. It is
worth pointing out that, when the update time step of the sensors
is equal to the integration time step, the most accurate lter is
the EKF [10], with an RMSE slightly superior to the noise standard
deviation (103 rad). The rest of lters present also very similar
results, while correcting only the independent acceleration
vector. However, when the update time step of the sensors is
greater than the integration time step, the RMSE of the EKF in its
continuous form increases dramatically compared to the RMSE of
the other lters. When the update time step of the sensors
increases, the corrections of the lter are less frequent meaning
that less information on the behavior of the real mechanism is
available. Therefore, the model of the lter has more difculties to
follow accurately the motion of the real mechanism and the RMSE
increases. The bad behavior of the continuous-form EKF in
multi-rate situations is in part due to the assumptions made in
Section 2.2. Indeed, in the theory of the EKF in continuous form, it

Please cite this article as: R. Pastorino, et al., State estimation using multibody models and non-linear Kalman lters, International
Journal of Non-Linear Mechanics (2013), http://dx.doi.org/10.1016/j.ijnonlinmec.2013.01.016i

R. Pastorino et al. / International Journal of Non-Linear Mechanics ] (]]]]) ]]]]]]

0.1

x 103

Error (rad/s)

0.05
Error (rad)

2
0

0
0.05

0.1

4
0

0.5

1.5
Time (s)

2.5

0.5

1.5
Time (s)

2.5

Fig. 12. Errors of the lters for the angular velocity of the left crank.

Fig. 8. Errors of the lters for the angle of the left crank.

1
0.5

Error (rad/s2)

Error (rad/s)

0.05

0
0.5

0.05

0.5

1.5
Time (s)

2.5

1.5
Time (s)

2.5

Table 2
CPU times and RMSE for Dti 2 ms.

0.5

Error (rad/s2)

0.5

Fig. 13. Errors of the lters for the angular acceleration of the left crank.

Fig. 9. Errors of the lters for the angular velocity of the left crank.

0.5
0

0.5

1.5
Time (s)

2.5

0.01
0.005
0
0.005
0.01
0.015
0

0.5

1.5
Time (s)

2.5

Filter

MB formu.

Integ.

CPU (%)

RMSE

Dts ms

EKF

Matrix-R

TR

SSUKF

Matrix-R

RK2

SSUKF

Matrix-R

TR

UKF

Matrix-R

TR

100
103
115
109
176
169
209
203

1.36  10  3
9.22  10  3
2.16  10  3
3.17  10  3
2.18  10  3
5.35  10  3
1.98  10  3
3.1  10  3

2
6
2
6
2
6
2
6

Fig. 10. Errors of the lters for the angular acceleration of the left crank.

Error (rad)

Fig. 11. Errors of the lters for the angle of the left crank.

is assumed that information of the sensors is available at any

instant. As it is not the case in multi-rate situations, a zero order
hold (ZOH) is applied to yt and the calculation of the Kalman
corrections in Eq. (17) is not correct at each integration time step.

Subsequently, high frequency noise appears due to the incorrect

corrections, as shown in Fig. 13. The use of the discrete form of
the EKF should solve this problem as the time-update and
measurement-update equations are separated. Then, it can be
seen in Table 2 that, in multi-rate situations, all the SPKFs have a
slightly lower computational cost as less calculations of the
measurement-update equations have to be performed, and the
EKF has a slightly greater computational cost due to convergence
difculties.
In light of the results, the continuous form of the EKF is not
suitable for multi-rate situations. Filters in discrete-form like the
one used for the SPKFs yield much better accuracy in these
situations. Consequently, the EKF in discrete-form should be
considered to handle multi-rate situations with this type of lter.
Then, the SPKFs have proved to have a slightly lower accuracy
than the EKF (except in multi-rate situations). However, the EKF
has Kalman corrections in accelerations and velocities, while the
SPKFs only have corrections in accelerations. Therefore, it can be
said that the SPKFs would be much more precise than the EKF if
velocities and accelerations were estimated, at the expense of an
additional computational cost. This additional cost is undesirable
in this research.

Please cite this article as: R. Pastorino, et al., State estimation using multibody models and non-linear Kalman lters, International
Journal of Non-Linear Mechanics (2013), http://dx.doi.org/10.1016/j.ijnonlinmec.2013.01.016i

R. Pastorino et al. / International Journal of Non-Linear Mechanics ] (]]]]) ]]]]]]

6. Conclusions
This work presents the theoretical development and practical
implementation of non-linear state observers using MB models.
Several types of Kalman lters have been considered: the EKF in
its continuous form and some SPKFs (UKF, SSUKF). Although these
SPKFs outperform the accuracy of the EKF in its continuous form
by using better approximations of system non-linearities, the
sigma-points have brought about an additional computational
cost. However, they present several advantages over the latter:
easy implementation, parallel computing structure that helps to
reach real-time. Implicit integrators have proven to improve
observer accuracy insignicantly when used in closed-loop state
estimation so explicit integrators are used to help reducing the
computational costs. Indeed, the difference between the predicted
measurement and the actual measurement (i.e. the innovation) is
adopted to correct the estimates and allows the prevention of
numerical energy loss due to the integration. In light of the
results, the choice of the most suitable set of lter, MB formulation
and integrator depends on the application requirements and is a
trade-off between estimation accuracy and computational efciency. Future works will be devoted to deeper investigations on
wider classes of MB formulations and lters, and to the experimental validation of the proposed observers.

Acknowledgments
The authors would like to thank the Spanish Ministry of Science
and Innovation and ERDF funds through the Grant TRA2009-09314
for its support in this research.
References
[1] A. Radke, Z. Gao, A survey of state and disturbance observers for practitioners,
in: Proceedings of the American Control Conference, Minneapolis, Minnesota,
USA, 2006, pp. 51835188.
[2] R.E. Kalman, A new approach to linear ltering and prediction problems,
Transactions of the ASMEJournal of Basic Engineering 82 (Series D) (1960)
3545.
[3] M.S. Grewal, A.P. Andrews, Kalman FilteringTheory and Practice Using
MATLAB, 3rd ed., John Wiley & Sons, Inc, 2008.
[4] R. Caracciolo, D. Richiedei, A. Trevisani, Experimental validation of a modelbased robust controller for multi-body mechanisms with exible links,
Multibody System Dynamics 20 (2) (2008) 129145, http://dx.doi.org/
10.1007/s11044-008-9113-7.

[5] R. Caracciolo, D. Richiedei, A. Trevisani, Robust piecewise-linear state observers for exible link mechanisms, Journal of Dynamic Systems, Measurement
and Control 130(3) (2008), 031011, http://dx.doi.org/10.1115/1.2909600.
[6] R. Caracciolo, D. Richiedei, A. Trevisani, Design and experimental validation of
piecewise-linear state observers for exible link mechanisms, Meccanica 41
(2006) 623637.
[7] A. Barreiro, E. Delgado, J. Cuadrado, D. Dopico, Extended-Kalman-lter
observers for multibody dynamical systems, in: Sixth EUROMECH Nonlinear
Dynamics Conference ENOC, Saint Petersburg, Russia, 2008.
[8] J. Cuadrado, D. Dopico, A. Barreiro, E. Delgado, Real-time state observers
based on multibody models and the extended Kalman lter, in: Proceedings
of the Fourth Asian Conference on Multibody Dynamics, no. 127, Jeju, Korea,
2008.
[9] J. Cuadrado, D. Dopico, J.A. Perez, R. Pastorino, Inuence of the sensored
magnitude in the performance of observers based on multibody models and
the extended Kalman lter, in: Proceedings of the Multibody Dynamics
ECCOMAS Thematic Conference, Warsaw, Poland, 2009.
[10] J. Cuadrado, D. Dopico, A. Barreiro, E. Delgado, Real-time state observers based
on multibody models and the extended Kalman lter, Journal of Mechanical
Science and Technology 23 (4) (2009) 894900, http://dx.doi.org/10.1007/
s12206-009-0308-5.
[11] R.E. Kalman, R.S. Bucy, New results in linear ltering and prediction theory,
Transactions of the ASME. Series D, Journal of Basic Engineering 83 (1961)
95107.
[12] S.J. Julier, J.K. Uhlmann, H.F. Durrant-Whyte, A new approach for ltering
nonlinear systems, in: Proceedings of the American Control Conference, vol.
3, 1995, pp. 16281632, http://dx.doi.org/10.1109/ACC.1995.529783.
[13] S.J. Julier, J.K. Uhlmann, A new extension of the Kalman lter to nonlinear
systems, in: The Eleventh International Symposium on Aerospace/Defense
Sensing, Simulation and Controls, Orlando, FL, USA, 1997, pp. 182193.
[14] S.J. Julier, J.K. Uhlmann, Unscented ltering and nonlinear estimation, in:
Proceedings of the IEEE, vol. 92, 2004, pp. 401422.
[15] M. Nrgaard, N.K. Poulsen, O. Ravn, New developments in state estimation for
nonlinear systems, Automatica 36 (11) (2000) 16271638, http://dx.doi.org/
10.1016/S0005-1098(00)00089-3.
[16] S. Julier, J. Uhlmann, Reduced sigma point lters for the propagation of means
and covariances through nonlinear transformations, in: Proceedings of the
American Control Conference, vol. 2, 2002, pp. 887892.
[17] S.J. Julier, The spherical simplex unscented transformation, in: Proceedings of
the American Control Conference, vol. 3, 2003, pp. 24302434.
[18] R. Van der Merwe, E. Wan, The square-root unscented Kalman lter for state
and parameter-estimation, in: IEEE International Conference on Acoustics,
Speech, and Signal Processing, vol. 6, Salt Lake City, USA, 2001, pp. 34613464.
[19] R. Van der Merwe, E.A. Wan, S. Julier, Sigma-point Kalman lters for
nonlinear estimation and sensor-fusion-applications to integrated navigation, in: AIAA Guidance, Navigation, and Control Conference and Exhibit,
Rhode Island, USA, 2004.
[20] J. Garca de Jalon, E. Bayo, Kinematic and Dynamic Simulation of Multibody
Systems: The Real-Time Challenge, Springer-Verlag, 1994.
[21] J. Cuadrado, D. Dopico, M. A. Naya, R. Pastorino, Automotive observers based
on multibody models and the extended Kalman lter, in: The First Joint
International Conference on Multibody System Dynamics, Lappeenranta,
Finland, 2010.
[22] J. Cuadrado, D. Dopico, J.A. Perez, R. Pastorino, Automotive observers based
on multibody models and the extended Kalman lter, Multibody System
Dynamics 27 (1) (2011) 319, http://dx.doi.org/10.1007/s11044-011-9251-1.

Please cite this article as: R. Pastorino, et al., State estimation using multibody models and non-linear Kalman lters, International
Journal of Non-Linear Mechanics (2013), http://dx.doi.org/10.1016/j.ijnonlinmec.2013.01.016i