State estimation of nonlinear dynamical systems with the use of filters is a significant topic in the area of mechatronics since it can provide improved methods for sensorless control and fault diagnosis of actuators and electromechanical systems. For nonlinear systems, and under the assumption of Gaussian noise, the Extended Kalman Filter (EKF) is frequently applied for estimating the non-measurable state variables through the processing of input and output sequences (Kamen Su 1999), (Basseville Nikiforov 1993), (Ng 2003), (Harris et al. 2002). The EKF is based on linearization of the nonlinear system dynamics using a first order Taylor expansion (Xiong 2008), (Rigatos Tzafestas 2007), (Rigatos and Zhang 2001), (Rigatos 2008a), (Rigatos 2009). Although EKF is efficient in several estimation problems, it is characterized by cumulative errors due to the local linearization assumption and this may affect the accuracy of the state estimation or even risk the stability of the observer-based control loop.
Aiming at finding more efficient implementations of nonlinear Kalman Filtering, in this paper a derivative-free approach to Kalman filtering is introduced and applied to state estimation-based control of a class of MIMO nonlinear dynamical systems. In the proposed derivative-free Kalman Filtering method the system is first subject to a linearization transformation that is based on the differential flatness theory and next state estimation is performed by applying the standard Kalman Filter recursion to the linearized model. Unlike EKF, the proposed method provides estimates of the state vector of the nonlinear system without the need for derivatives and Jacobians calculation. By avoiding linearization approximations, the proposed filtering method improves the accuracy of estimation of the system state variables, and results in smooth control signal variations and in minimization of the tracking error of the associated control loop. The paper extends the results of (Rigatos 2010a), (Rigatos 2011a), (Rigatos 2011b) towards multi-input multi-output (MIMO) nonlineal dynamical systems.
Differential flatness theory is currently a main direction in the analysis of nonlinear dynamical systems (Rudolph 2003), (Sira-Ramirez and Agrawal 2004), (Rigatos 2011c). To deduce if a dynamical system is differentially flat, the following should be examined: (i) the existence of the so-called flat output, i.e. a new variable which is expressed as a function of the system’s state variables. It should hold that the flat output and its derivatives should not be coupled in the form of an ordinary differential equation, (ii) the components of the system (i.e. state variables and control input) should be expressed as functions of the flat output and its derivatives (Fliess Mounier 1999), (Làine 2011), (Martin Rouchon 1999), (Rigatos 2010b), (Villagra et al. 2007), (Laroche et al. 2007). In certain cases the differential flatness theory enables transformation to a linearized form (canonical Brunovsky form) for which the design of the controller becomes easier. In other cases by showing that a system is differentially flat one can easily design a reference trajectory as a function of the so-called flat output and can find a control law that assures tracking of this desirable trajectory (Fliess Mounier 1999)), (Villagra et al. 2007). This paper is concerned with differentially flat MIMO nonlinear dynamical systems which after applying the differential flatness theory can be written in the Brunovksy (canonical) form (Martin Rouchon 1999).
The structure of the paper is as follows: In Section 2 state estimation for nonlinear dynamical systems with the use of the Extended Kalman Filter is explained. In Section 3 the concept of differential flatness for MIMO nonlinear dynamical systems is analyzed. In Section 4 derivative-free Kalman Filtering for nonlinear dynamical systems is introduced. In Section 5 the use of the differential flatness theory for transforming MIMO nonlinear dynamical systems in the canonical (Brunovsky) form and for designing MIMO derivative-free Kalman Filters is explained. As a case study the model of a 2-DOF rigid-link robotic manipulator is used. In Section 6 simulation tests are carried out to evaluate the performance of the derivative-free nonlinear Kalman Filter in the model of a 2-DOF rigid-link robotic manipulator. Finally, in Section 7 concluding remarks are stated.
2. Extended Kalman Filtering for nonlinear dynamical systems
2.1. The continuous-time Kalman Filter for the Linear State Estimation Model
where is the system’s state vector, and is the system’s output. Matrices , and can be time-varying and , are uncorrelated white Gaussian noises. The covariance matrix of the process noise is , while the covariance matrix of the measurement noise is . Then, the Kalman Filter is a linear state observer which is given by
where is the optimal estimation of the state vector and is the covariance matrix of the state vector estimation error with . The Kalman Filter consists of the system’s state equation plus a corrective term . The selection of gain corresponds actually to the solution of an optimization problem. This is expressed as the minimization of a quadratic cost functional and is performed through the solution of a Riccati equation. In that case the observer’s gain is calculated by considering an optimal control problem for the dual system , where the covariance matrix of the estimation error is found by the solution of a continuous-time Riccati equation of the form
where matrices and stand for the process and measurement noise covariance matrices, respectively.
2.2. The discrete-time Kalman Filter for linear dynamical systems
where the state is a -vector, is a -element process noise vector and is a real matrix. Moreover the output measurement is a -vector, is an -matrix of real numbers, and is the measurement noise. It is assumed that the process noise and the measurement noise are uncorrelated.
Now the problem of interest is to estimate the state based on the sequence of output measurements . The initial value of the state vector , and the initial value of the error covariance matrix is unknown and an estimation of it is considered, i.e. = a guess of and = a guess of .
For the initialization of matrix one can set , with . The state vector has to be estimated taking into account , and the output measurements , i.e. . This is a linear minimum mean squares estimation problem (LMMSE) formulated as . The process and output noise are white and their covariance matrices are given by: and .
Using the above, the discrete-time Kalman filter can be decomposed into two parts: i) time update (prediction stage), and ii) measurement update (correction stage). The first part employs an estimate of the state vector made before the output measurement is available (a priori estimate). The second part estimates after has become available (a posteriori estimate).
When the set of measurements is available. From an a priori estimation of is obtained which is denoted by = the estimate of given .
When is available, the output measurements set becomes , where = the estimate of given .
The associated estimation errors are defined by = the a priori error, and = the a posteriori error. The estimation error covariance matrices associated with and are defined as and (Kamen Su 1999). From the definition of the trace of a matrix, the mean square error of the estimates can be written as and .
Finally, the linear Kalman filter equations in cartesian coordinates are
2.3. The Extended Kalman Filter
where is the system’s state vector and is the system’s output, while and are uncorrelated, zero-mean, Gaussian zero-mean noise processes with covariance matrices and respectively. The operators and are vectors defined as , , and , respectively. It is assumed that and are sufficiently smooth in so that each one has a valid series Taylor expansion. Following a linearization procedure, is expanded into Taylor series about :
where is the Jacobian of calculated at :
Likewise, is expanded about
where is the estimation of the state vector before measurement at the -th instant to be received and is the updated estimation of the state vector after measurement at the -th instant has been received. The Jacobian is
The resulting expressions create first order approximations of and . Thus the linearized version of the system is obtained:
Now, the EKF recursion is as follows: First the time update is considered: by the estimation of the state vector at time instant is denoted. Given initial conditions and the recursion proceeds as:
The schematic diagram of the EKF loop is given in Fig. 1.
Indicative applications of the Extended Kalman Filter in mechatronics and robotics have been presented in (Chinniah et al. 2006) where the EKF has been used in state estimation-based control of electrohydraulic actutators, in (Seguritan Rotuno 2002) where the EKF has been used as a disturbance observer resulting in improved control of a direct current motor, in (Lin et al. 2001) where the EKF has been used in sensorless control of nonlinear brushless DC motor models, in (Liu et al. 2006) where the EKF has been used for estimating the electromagnetic torque of direct-torque controlled brushless DC motors and for implementing a sensorless control scheme, and in (Terzic Jardic 2001) where the EKF has been applied for rotor resistance estimation of a DC motor, for position and velocity estimation and finally for state estimation-based control. Examples on nonlinear filtering for fault diagnosis and condition monitoring tasks have been given in (Efimov et al. 2010) where an EKF-based fault diagnosis scheme for actuators has been studied, in (Yang 2002) where the EKF has been proposed for residual generation and diagnosis of incipient faults in DC electric motors, and in (An Sepehri 2004) where the EKF has been used in fault diagnosis of an electrohydraulic actuation system.
3. Differential flatness for nonlinear dynamical systems
3.1. Definition of differentially flat systems
The principles of the differential flatness theory have been extensively studied in the relevant bibliography (Rudolph 2003), (Rigatos 2010b), (Villagra et al. 2007): A finite dimensional system is considered. This can be written in the form of an ordinary differential equation (ODE), i.e. . The quantity denotes the system variables (these variables are for instance the elements of the system’s state vector and the control input) while , are the associated derivatives. Such a system is said to be differentially flat if there exists a collection of functions of the system variables and of their time-derivatives, i.e. satisfying the following two conditions (Fliess Mounier 1999), (Rigatos 2008a):
1. There does not exist any differential relation of the form which implies that the derivatives of the flat output are not coupled in the sense of an ODE, or equivalently it can be said that the flat output is differentially independent.
2. All system variables (i.e. the elements of the system’s state vector and the control input) can be expressed using only the flat output and its time derivatives . An equivalent definition of differentially flat systems is as follows:
Definition: The system , , is differentially flat if there exist relations
This means that all system dynamics can be expressed as a function of the flat output and its derivatives, therefore the state vector and the control input can be written as
It is noted that for linear systems the property of differential flatness is equivalent to that of controllability.
3.2. Classes of differentially flat systems
The following classes of nonlinear differentially flat systems are defined (Martin Rouchon 1999):
1. Affine in-the-input systems.
The dynamics of such systems is given by:
From Eq. (20) it can be deduced that the above state equation can also describe MIMO dynamical systems. Without out loss of generality it is assumed that is of rank . In case that the flat outputs of the aforementioned system are only functions of states , then this class of dynamical systems is called -flat. It has been proven that a dynamical affine system with states and inputs is -flat if it is controllable.
2. Driftless systems.
These are systems of the form
For driftless systems with two inputs, i.e.
the flatness property holds, if and only if the rank of matrix with is equal to for . It has been proven that a driftless system that is differentially flat, is also -flat.
Moreover, for flat systems with states and control inputs, i.e.
the flatness property holds, if controllability also holds. Furthermore, the system is -flat if is even.
3.3. Conditions for applying the differential flatness theory
In order to apply the concept of differential flatness, the flat outputs of the nonlinear system have to be defined first. For nonlinear systems it is still an open problem to construct flat outputs. The following generic class of nonlinear systems is considered
Such a system can be transformed to the form of an affine in the input system by adding an integrator to each input (Bououden et al. 2011)
(i) Lie derivative: stands for the Lie derivative and the repeated Lie derivatives are recursively defined as , .
(ii) Lie Bracket: stands for a Lie Bracket which is defined recursively as with and .
If the system of Eq. (25) can be linearized by a diffeomorphism and a static state feedback into the following form
with , then for are the 0-flat outputs which can be written as functions of only the elements of the state vector . To define conditions for transforming the system of Eq. (25) into the canonical form described in Eq. (26) the following theorem holds (Bououden et al. 2011)
Theorem: For the nonlinear systems described by Eq. (23) the following variables are defined:
Then, the linearization problem for the system of Eq. (25) can be solved if and only if
3.4. Transformation of MIMO nonlinear systems into the Brunovsky form
It is assumed now that after defining the flat outputs of the initial MIMO nonlinear system, and after expressing the system state variables and control inputs as functions of the flat output and of the associated derivatives, the system can be transformed in the Brunovsky canonical form:
where is the state vector of the transformed system (according to the differential flatness formulation), is the set of control inputs, is the output vector, are the drift functions and are smooth functions corresponding to the control input gains, while is a variable associated to external disturbances. In holds that . Having written the initial nonlinear system into the canonical (Brunovsky) form it holds
Next the following vectors and matrices can be defined
where matrix has the MIMO canonical form, i.e. with elements
Thus, Eq. (26) can be written in state-space form
which can be also written in the equivalent form:
where the transformed control input is defined as .
If the state variables of the system are estimated with the use of a nonlinear filter then a state-feedback control law can be formulated as
where and are the state estimation-based approximations of and , respectively, while is the setpoint vector for output derivatives, i.e. . is a supervisory control term, e.g. control term that is used to compensate for the effects of modelling inaccuracies and external disturbances. Moreover, is the feedback gain matrix that assures that the characteristic polynomial of matrix will be a Hurwitz one.
4. Application of derivative-free Kalman Filtering to SISO nonlinear systems
4.1. Conditions for derivative-free Kalman Filtering in SISO nonlinear systems
It will be shown that through a nonlinear transformation it is possible to design a state estimator for a class of nonlinear systems, which can substitute for the Extended Kalman Filter. The results will be generalized towards derivative-free Kalman Filtering for nonlinear systems. The following continuous-time nonlinear single-output system is considered (Rigatos 2011a), (Marino 1990), (Marino Tomei 1992)
with , , , , smooth functions, , for every ; is the state vector, is the control which is assumed to be known, is the parameter vector which is supposed to be constant and is the scalar output.
The first main assumption on the class of systems considered is the linear dependence on the parameter vector . The second main assumption requires that systems of Eq.(32) are transformable by a parameter independent state-space change of coordinates in
into the system
and smooth functions for . The necessary and sufficient conditions for the initial nonlinear system to be transformable into the form of Eq.(34) have been given in (Rigatos 2011a), (Marino 1990), (Marino Tomei 1992), and are summarized in the following:
, (which implies local observability). It is noted that stands for the Lie derivative and the repeated Lie derivatives are recursively defined as , .
. It is noted that stands for a Lie Bracket which is defined recursively as with and .
the vector fields are complete, in which is the vector field satisfying
Then for every parameter vector , the system
is an asymptotic observer for a suitable choice of provided that the state is bounded, with estimation error dynamics
The eigenvalues of can be arbitrarily placed by choosing the vector , since they coincide with the roots of the polynomial .
4.2. State estimation for SISO systems with the derivative-free Kalman Filter
Since Eq. (38) provides an asympotic observer for the initial nonlinear system of Eq. (32) one can consider a case in which the observation error gain matrix can be provided by the Kalman Filter equations given initially in the continuous-time KF formulation, or in discrete-time form by Eq. (5) and Eq. (6). The following single-input single-output nonlinear dynamical system is considered
where is the system’s output, and , are nonlinear functions. It can be noticed that the system of Eq. (40) belongs to the general class of systems of Eq. (32). Assuming the transformation , and , i.e. , one obtains the linearized system of the form
which in turn can be written in state-space equations as
The system of Eq. (41) and Eq. (43) has been written in the form of Eq. (34), which means that Eq. (38) is the associated asymptotic observer. Therefore, the observation gain appearing in Eq. (38) can be found using either linear observer design methods (in that case the elements of the observation error gain matrix have fixed values), or the recursive calculation of the continuous-time Kalman Filter gain described in subsection 2.2. If the discrete-time Kalman Filter is to be used then one has to apply the recursive formulas of Eq. (5) and Eq. (6) on the discrete-time equivalent of Eq. (42) and Eq. (43).
5. Derivative-free Kalman Filtering for MIMO nonlinear systems
The results of Section 4 will be generalized towards derivative-free Kalman Filtering for MIMO nonlinear dynamical systems. The proposed method for derivative-free Kalman Filtering for MIMO nonlinear systems will be analyzed through an application example. A 2-DOF rigid link robotic manipulator is considered. The dynamic model of the robot is given by
where is the inertia matrix, is the Coriolis and centripetal forces vector, is the vector of gravitational forces and is the vector of control input torques. Equivalently, one has
Denoting the inverse of the inertia matrix as
then one obtains
which can be also written as
The following state variables are defined
It holds that
The flat output is defined as
It holds that
therefore all system state variables can be written as functions of the flat output and its derivatives. The same holds for the control input . Indeed, one has
Moreover, from Eq. (58) it holds
Knowing that one finally obtains
Therefore, the considered robotic system is a differentially flat one. Next, taking also into account the effects of additive disturbances to the joints of the robotic manipulator the dynamic model becomes
Moreover, the following control input is defined
where is a supervisory control term that is used for the compensation of the model’s uncertainties as well as of the external disturbances and are the rows of the error feedback gain matrix (Rigatos 2008b), (Rigatos Tzafestas 2006). It is also noted that to perform efficient state estimation under such model uncertainties and external disturbances one can consider results on disturbance observers within a Kalman Filter framework (Rigatos 2011c).
Finally, the differentially flat robotic model is written in the Brunovsky (canonical) form. Considering the state vector , with the state variables defined in Eq. (51), the following matrices are defined
Using the matrices of Eq. (62) one obtains the Brunovsky form of the MIMO robot model dynamics
where the new input is given by
6. Simulation tests
The performance of the proposed derivative-free nonlinear Kalman Filter was tested in the benchmark problem of state estimation-based control for a 2-DOF rigid-link robotic manipulator (Fig. 2). The differentially flat model of the robot and its transformation to the Brunovksy form has been analyzed in Section 4. The flat outputs were taken to be the robot’s joint angles and . It has been proven that all state variables of the robotic model and the associated control inputs, i.e. the torques applied by the motors to the links’ joints can be written as functions of the flat output and of the associated derivatives.
Two different set-points were studied: i) a sinusoidal signal of amplitude and period , ii) a see-saw set-point of amplitude and period . At the beginning of the second half of the simulation time an additive sinusoidal disturbance of amplitude and period sec was applied to the system. The approximations and were used in the derivation of the control law, given by Eq. (31).
The position and velocity variations for the sinusoidal set-point for the first joint of the robotic manipulator are depicted in Fig. 3. For the second joint of the 2-DOF robot the tracking of the position and velocity setpoints is depicted in Fig. 4. The control inputs (motor torques) applied to the first and second joint of the robotic manipulator are shown in Fig. 5.
The performance of the state estimation-based control has been also tested in the tracking of a see-saw set-point. The position and velocity variation of the first joint are demonstrated in Fig. 6. Similarly, the tracking of the position and velocity reference setpoints for the second joint are depicted in Fig. 7. The control signal in the case of tracking of a tracking a see-saw setpoint by the two joints of the robotic manipulator is shown in Fig. 8. The estimated state variables were denoted as green line whereas the real state variables were denoted as blue lines.
Moreover, the derivative-free nonlinear Kalman Filter was tested against the Extended Kalman Filter which is among the most commonly used estimation methods for nonlinear dynamical systems. In the latter case, the position and velocity variations for the sinusoidal set-point for the first joint of the robotic manipulator are depicted in Fig. 9. For the second joint of the 2-DOF robot the tracking of the position and velocity setpoints is depicted in Fig. 10. The control inputs (motor torques) applied to the first and second joint of the robotic manipulator are shown in Fig. 11.
The performance of the state estimation-based control is also tested in the tracking of a see-saw set-point. The position and velocity variation of the first joint are demonstrated in Fig. 12. Similarly, the tracking of the position and velocity reference setpoints for the second joint are depicted in Fig. 13. The control signal in the case of tracking of a tracking a see-saw setpoint by the two joints of the robotic manipulator is shown in Fig. 14.
Comparing the estimation performed by the derivative-free MIMO nonlinear Kalman Filter with the one performed by the Extended Kalman Filter it can be noticed that the derivative-free filtering approach results in more accurate state estimates. Moreover, comparing the associated state estimation-based control loop that was based on the derivative-free MIMO nonlinear Kalman Filter to the control that was based on the Extended Kalman Filter it was observed that the first control scheme was significantly more robust and capable of tracking with better accuracy the desirable trajectories. These findings show the suitability of the considered derivative-free MIMO nonlinear Kalman Filter for tracking, control and fault diagnosis tasks.
The paper has examined the problem of derivative-free Kalman Filtering for MIMO nonlinear dynamical systems. The nonlinear system is first subject to a linearization transformation that is based on the differential flatness theory and next state estimation is performed by applying the Kalman Filter to the linearized model. It was mentioned that a system is considered to be differentially flat if (i) the so-called flat-output can be defined, which is a function of the system’s state variables. For the flat output it is assured that it is not coupled to its derivatives through an ordinary differential equation), (ii) the system’s state variables and the control inputs can be all expressed as functions of the differentially flat output and its derivatives. The paper was concerned with multi-input multi-output (MIMO) differentially flat systems. Such systems can be brought to a Brunovsky (canonical form) through a nonlinear transformation applied to the state variables and to the control input, thus making possible the application of the standard Kalman Filter recursion.
The proposed derivative-free Kalman Filter for nonlinear MIMO systems provides improved the estimation accuracy and has better convergence properties than the Extended Kalman Filter. The derivative-free Kalman Filter, has the following features: (i) it is not based on local linearization of the controlled system dynamics, (ii) it does not assume truncation of higher order Taylor expansion terms, (iii) it does not require the computation of Jacobian matrices. In the proposed filtering method, the system is first subject to a linearization transformation and next state estimation is performed by applying local Kalman Filters to the linearized model. The class of MIMO systems to which the derivative-free Kalman Filter can be applied has been also defined.