Derivative-Free Nonlinear Kalman Filtering for MIMO Dynamical Systems: Application to Multi-DOF Robotic Manipulators

Abstract

The paper proposes derivative-free nonlinear Kalman Filtering for MIMO nonlinear dynamical systems. The considered nonlinear filtering scheme which is based on differential flatness theory extends the class of systems to which Kalman Filtering can be applied without the need for calculation of Jacobian matrices. To deduce if a dynamical system is differentially flat, the following should be examined: (i) the existence of the flat output, which is a variable that can be written as a function of the system's state variables (ii) the system's state variables and the input can be written as functions of the flat output and its derivatives. Nonlinear systems satisfying the differential flatness property can be written in the Brunovsky form via a transformation of their state variables and control inputs. After transforming the nonlinear system to the canonical form it is straightforward to apply the standard Kalman Filter recursion. The performance of the proposed derivative-free nonlinear filtering scheme is tested through simulation experiments on benchmark nonlinear multi-input multi-output dynamical systems, such as robotic manipulators.

1. Introduction

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

First, the continuous-time dynamical system of Eq. (1) is assumed (Rigatos Tzafestas 2007):

$\{\begin{array}{l}\dot{x}(t)=Ax(t)+Bu(t)+w(t)\mathrm{,}\hspace{0.17em}t\ge t_0\\ z(t)=Cx(t)+v(t)\mathrm{,}\hspace{0.17em}t\ge t_0\end{array}$ (1)

where $x\in R^{m\times 1}$ is the system's state vector, and $z\in R^{p\times 1}$ is the system's output. Matrices A, B and C can be time-varying and $w(t)$ , $v(t)$ are uncorrelated white Gaussian noises. The covariance matrix of the process noise $w(t)$ is $Q(t)$ , while the covariance matrix of the measurement noise is $R(t)$ . Then, the Kalman Filter is a linear state observer which is given by

$\{\begin{array}{l}\dot{\widehat{x}}=A\widehat{x}+Bu+K[z-C\widehat{x}]\mathrm{,}\hspace{0.17em}\widehat{x}(t_0)=0\\ K(t)=P{C}^T{R}^{-1}\\ \dot{P}=AP+P{A}^T+Q-P{C}^T{R}^{-1}CP\end{array}$ (2)

where $\widehat{x}(t)$ is the optimal estimation of the state vector $x(t)$ and $P(t)$ is the covariance matrix of the state vector estimation error with $P(t_0)=P_0$ . The Kalman Filter consists of the system's state equation plus a corrective term $K[z-C\widehat{x}]$ . The selection of gain K 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 K is calculated by $K=P{C}^T{R}^{-1}$ considering an optimal control problem for the dual system $(A^T\mathrm{,}{C}^T)$ , where the covariance matrix of the estimation error P is found by the solution of a continuous-time Riccati equation of the form

$\dot{P}=AP+P{A}^T+Q-P{C}^T{R}^{-1}CP$ (3)

where matrices Q and R stand for the process and measurement noise covariance matrices, respectively.

2.2. The discrete-time Kalman Filter for linear dynamical systems

In the discrete-time case a dynamical system is assumed to be expressed in the form of a discrete-time state model (Rigatos Tzafestas 2007), (Rigatos 2010b):

$\begin{array}{c}x(k+1)=A(k)x(k)+L(k)u(k)+w(k)\\ z(k)=Cx(k)+v(k)\end{array}$ (4)

where the state $x(k)$ is a m -vector, $w(k)$ is a m -element process noise vector and A is a $m\times m$ real matrix. Moreover the output measurement $z(k)$ is a p -vector, C is an $p\times m$ -matrix of real numbers, and $v(k)$ is the measurement noise. It is assumed that the process noise $w(k)$ and the measurement noise $v(k)$ are uncorrelated.

Now the problem of interest is to estimate the state $x(k)$ based on the sequence of output measurements $z(1)\mathrm{,}z(2)\mathrm{,}\cdots \mathrm{,}z(k)$ . The initial value of the state vector $x(0)$ , and the initial value of the error covariance matrix $P(0)$ is unknown and an estimation of it is considered, i.e. $\widehat{x}(0)$ = a guess of $E[x(0)]$ and $\widehat{P}(0)$ = a guess of $Cov[x(0)]$ .

For the initialization of matrix P one can set $\widehat{P}(0)=\lambda I$ , with $\lambda >0$ . The state vector $x(k)$ has to be estimated taking into account $\widehat{x}(0)$ , $\widehat{P}(0)$ and the output measurements $Z=[z(1)\mathrm{,}z(2)\mathrm{,}\cdots \mathrm{,}z(k{)]}^T$ , i.e. $\widehat{x}(k)={\alpha }_n(\widehat{x}(0))\mathrm{,}\widehat{P}(0)\mathrm{,}Z(k))$ . This is a linear minimum mean squares estimation problem (LMMSE) formulated as $\widehat{x}(k+1)=a_{n+1}(\widehat{x}(k)\mathrm{,}z(k+1))$ . The process and output noise are white and their covariance matrices are given by: $E[w(i)w^T(j)]=Q\delta (i-j)$ and $E[v(i)v^T(j)]=R\delta (i-j)$ .

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 $x(k)$ made before the output measurement $z(k)$ is available (a priori estimate). The second part estimates $x(k)$ after $z(k)$ has become available (a posteriori estimate).

• When the set of measurements $Z^{-}=\{z(1)\mathrm{,}\cdots \mathrm{,}z(k-1)\}$ is available. From $Z^{-}$ an a priori estimation of $x(k)$ is obtained which is denoted by ${\widehat{x}}^{-}(k)$ = the estimate of $x(k)$ given $Z^{-}$ .

• When $z(k)$ is available, the output measurements set becomes $Z=\{z(1)\mathrm{,}\cdots \mathrm{,}z(k)\}$ , where $\widehat{x}(k)$ = the estimate of $x(k)$ given Z.

The associated estimation errors are defined by $e^{-}(k)=x(k)-{\widehat{x}}^{-}(k)$ = the a priori error, and $e(k)=x(k)-\widehat{x}(k)$ = the a posteriori error. The estimation error covariance matrices associated with $\widehat{x}(k)$ and $\widehat{x}(k)$ are defined as $P^{-}(k)=Cov[e^{-}(k)]=E[e^{-}(k)e^{-}{(k)}^T]$ and $P(k)=Cov[e(k)]=E[e(k)e^T(k)]$ (Kamen Su 1999). From the definition of the trace of a matrix, the mean square error of the estimates can be written as $MSE({\widehat{x}}^{-}(k))=E[e^{-}(k)e^{-}{(k)}^T]=tr(P^{-}(k))$ and $MSE(x(k))=E[e(k)e^T(k)=tr(P(k))$ .

Finally, the linear Kalman filter equations in cartesian coordinates are

measurement update:

$\begin{array}{l}K(k)=P^{-}(k)C^T{\left[C\cdot P^{-}(k)C^T+R\right]}^{-1}\\ \widehat{x}(k)={\widehat{x}}^{-}(k)+K(k)[z(k)-C{\widehat{x}}^{-}(k)]\\ P(k)=P^{-}(k)-K(k)C{P}^{-}(k)\end{array}$ (5)

time update:

$\begin{array}{l}{P}^{-}(k+1)=A(k)P(k)A^T(k)+Q(k)\\ {\widehat{x}}^{-}(k+1)=A(k)\widehat{x}(k)+L(k)u(k)\end{array}$ (6)

2.3. The Extended Kalman Filter

State estimation can be also performed for nonlinear dynamical systems using the EKF recursion. The following nonlinear model is considered (Rigatos Tzafestas 2007), (Rigatos 2010b):

$\begin{array}{c}x(k+1)=\varphi (x(k))+L(k)u(k)+w(k)\\ z(k)=\gamma (x(k))+v(k)\end{array}$ (7)

where $x\in R^{m\times 1}$ is the system's state vector and $z\in R^{p\times 1}$ is the system's output, while $w(k)$ and $v(k)$ are uncorrelated, zero-mean, Gaussian zero-mean noise processes with covariance matrices $Q(k)$ and $R(k)$ respectively. The operators $\phi (x)$ and $\gamma (x)$ are vectors defined as $\phi (x)=[{\phi }_1(x)\mathrm{,}{\phi }_2(x)\mathrm{,}\cdots$ , ${\phi }_m(x{)]}^T$ , and $\gamma (x)=[{\gamma }_1(x)\mathrm{,}{\gamma }_2(x)\mathrm{,}\cdots \mathrm{,}{\gamma }_p(x{)]}^T$ , respectively. It is assumed that φ and γ are sufficiently smooth in x so that each one has a valid series Taylor expansion. Following a linearization procedure, φ is expanded into Taylor series about $\widehat{x}$ :

$\varphi (x(k))=\varphi (\widehat{x}(k))+J_{\varphi }(\widehat{x}(k))[x(k)-\widehat{x}(k)]+\cdots$ (8)

where $J_{\phi }(x)$ is the Jacobian of φ calculated at $\widehat{x}(k)$ :

$J_{\varphi }(x)={\frac{\partial \varphi }{\partial x}}_{{|}_{x=\widehat{x}(k)}}=\left(\begin{array}{cccc}\frac{\partial {\varphi }_1}{\partial x_1}& \frac{\partial {\varphi }_1}{\partial x_2}& \cdots & \frac{\partial {\varphi }_1}{\partial x_m}\\ \frac{\partial {\varphi }_2}{\partial x_1}& \frac{\partial {\varphi }_2}{\partial x_2}& \cdots & \frac{\partial {\varphi }_2}{\partial x_m}\\ ⋮& ⋮& ⋮& ⋮\\ \frac{\partial {\varphi }_m}{\partial x_1}& \frac{\partial {\varphi }_m}{\partial x_2}& \cdots & \frac{\partial {\varphi }_m}{\partial x_m}\end{array}\right)$ (9)

Likewise, γ is expanded about ${\widehat{x}}^{-}(k)$

$\gamma (x(k))=\gamma ({\widehat{x}}^{-}(k))+J_{\gamma }[x(k)-{\widehat{x}}^{-}(k)]+\cdots$ (10)

where ${\widehat{x}}^{-}(k)$ is the estimation of the state vector $x(k)$ before measurement at the k -th instant to be received and $\widehat{x}(k)$ is the updated estimation of the state vector after measurement at the k -th instant has been received. The Jacobian $J_{\gamma }(x)$ is

$J_{\gamma }(x)={\frac{\partial \gamma }{\partial x}}_{{|}_{x={\widehat{x}}^{-}(k)}}=\left(\begin{array}{cccc}\frac{\partial {\gamma }_1}{\partial x_1}& \frac{\partial {\gamma }_1}{\partial x_2}& \cdots & \frac{\partial {\gamma }_1}{\partial x_m}\\ \frac{\partial {\gamma }_2}{\partial x_1}& \frac{\partial {\gamma }_2}{\partial x_2}& \cdots & \frac{\partial {\gamma }_2}{\partial x_m}\\ ⋮& ⋮& ⋮& ⋮\\ \frac{\partial {\gamma }_m}{\partial x_1}& \frac{\partial {\gamma }_m}{\partial x_2}& \cdots & \frac{\partial {\gamma }_m}{\partial x_m}\end{array}\right)$ (11)

Figure 1.

Schematic diagram of the EKF loop

The resulting expressions create first order approximations of φ and γ. Thus the linearized version of the system is obtained:

$\begin{array}{l}x(k+1)=\varphi (\widehat{x}(k))+J_{\varphi }(\widehat{x}(k))[x(k)-\widehat{x}(k)]+w(k)\\ z(k)=\gamma ({\widehat{x}}^{-}(k))+J_{\gamma }({\widehat{x}}^{-}(k))[x(k)-{\widehat{x}}^{-}(k)]+v(k)\end{array}$ (12)

Now, the EKF recursion is as follows: First the time update is considered: by $\widehat{x}(k)$ the estimation of the state vector at time instant k is denoted. Given initial conditions ${\widehat{x}}^{-}(0)$ and $P^{-}(0)$ the recursion proceeds as:

• Measurement update. Acquire $z(k)$ and compute:

$\begin{array}{l}K(k)=P^{-}(k)J_{\gamma }^T({\widehat{x}}^{-}(k))\cdot {\left[J_{\gamma }({\widehat{x}}^{-}(k))P^{-}(k)J_{\gamma }^T({\widehat{x}}^{-}(k))+R(k)\right]}^{-1}\\ \widehat{x}(k)={\widehat{x}}^{-}(k)+K(k)[z(k)-\gamma ({\widehat{x}}^{-}(k))]\\ P(k)=P^{-}(k)-K(k)J_{\gamma }({\widehat{x}}^{-}(k))P^{-}(k)\end{array}$ (13)

• Time update. Compute:

$\begin{array}{l}{P}^{-}(k+1)=J_{\varphi }(\widehat{x}(k))·P(k)J_{\varphi }^T(\widehat{x}(k))+Q(k)\\ {\widehat{x}}^{-}(k+1)=\varphi (\widehat{x}(k))+L(k)u(k)\end{array}$ (14)

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. $S_i(w\mathrm{,}\dot{w}\mathrm{,}\ddot{w}\mathrm{,}\cdots \mathrm{,}{w}^{(i)})\mathrm{,}i=\mathrm{1,2,}\cdots \mathrm{,}q$ . The quantity w denotes the system variables (these variables are for instance the elements of the system's state vector and the control input) while $w^{(i)}$ , $i=\mathrm{1,2,}\cdots \mathrm{,}q$ are the associated derivatives. Such a system is said to be differentially flat if there exists a collection of m functions $y=(y_1\mathrm{,}\cdots \mathrm{,}{y}_m)$ of the system variables and of their time-derivatives, i.e. $y_i=\phi (w\mathrm{,}\dot{w}\mathrm{,}\ddot{w}\mathrm{,}\cdots \mathrm{,}{w}^{({\alpha }_i)})\mathrm{,}i=\mathrm{1,}\cdots \mathrm{,}m$ satisfying the following two conditions (Fliess Mounier 1999), (Rigatos 2008a):

1. There does not exist any differential relation of the form $R(y\mathrm{,}\dot{y}\mathrm{,}\cdots \mathrm{,}{y}^{(\beta )})=0$ 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 w and the control input) can be expressed using only the flat output y and its time derivatives $w_i={\psi }_i(y\mathrm{,}\dot{y}\mathrm{,}\cdots \mathrm{,}{y}^{({\gamma }_i)})\mathrm{,}i=\mathrm{1,}\cdots \mathrm{,}s$ . An equivalent definition of differentially flat systems is as follows:

Definition: The system $\dot{x}=f(x\mathrm{,}u)$ , $x\in R^n$ , $u\in R^m$ is differentially flat if there exist relations

$\begin{array}{l}h:\hspace{0.17em}{R}^n\times {\left(R^m\right)}^{r+1}\to R^m\mathrm{,}\\ \varphi :\hspace{0.17em}{\left(R^m\right)}^r\to R^n\hspace{0.17em}\text{and}\\ \psi :\hspace{0.17em}{\left(R^m\right)}^{r+1}\to R^m\end{array}$ (15)

such that

$\begin{array}{l}y=h(x\mathrm{,}u\mathrm{,}\dot{u}\mathrm{,}\cdots \mathrm{,}{u}^{(r)})\mathrm{,}\\ x=\varphi (y\mathrm{,}\dot{y}\mathrm{,}\cdots \mathrm{,}{y}^{(r-1)})\mathrm{,}\hspace{0.17em}\text{and}\\ u=\psi (y\mathrm{,}\dot{y}\mathrm{,}\cdots \mathrm{,}{y}^{(r-1)}\mathrm{,}{y}^{(r)})\mathrm{.}\end{array}$ (16)

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

$\begin{array}{l}x(t)=\varphi (y(t)\mathrm{,}\dot{y}(t)\mathrm{,}\cdots \mathrm{,}{y}^{(r)}(t))\mathrm{,}\hspace{0.17em}\text{and}\\ u(t)=\psi (y(t)\mathrm{,}\dot{y}(t)\mathrm{,}\cdots \mathrm{,}{y}^{(r+1)}(t))\end{array}$ (17)

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:

$\dot{x}=f(x)+{\displaystyle \sum _{i=1}^m}{g}_i(x)u_i$ (18)

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 $G=[g_1\mathrm{,}\cdots \mathrm{,}{g}_m]$ is of rank m. In case that the flat outputs of the aforementioned system are only functions of states x, then this class of dynamical systems is called 0 -flat. It has been proven that a dynamical affine system with n states and $n-1$ inputs is 0 -flat if it is controllable.

2. Driftless systems.

These are systems of the form

$\dot{x}={\displaystyle \sum _{i=1}^m}{f}_i(x)u_i$ (19)

For driftless systems with two inputs, i.e.

$\dot{x}=f_1(x)u_1+f_2(x)u_2$ (20)

the flatness property holds, if and only if the rank of matrix $E_k:=\{E_k\mathrm{,}[E_k\mathrm{,}{E}_k]\}\mathrm{,}k\ge 0$ with $E_0:=\left\{f_1\mathrm{,}{f}_2\right\}$ is equal to $k+2$ for $k=\mathrm{0,}\cdots \mathrm{,}n-2$ . It has been proven that a driftless system that is differentially flat, is also 0 -flat.

Moreover, for flat systems with n states and $n-2$ control inputs, i.e.

$\dot{x}={\displaystyle \sum _{i=1}^{n-2}}{u}_i{f}_i(x)\hspace{0.17em}x\in R^n$ (21)

the flatness property holds, if controllability also holds. Furthermore, the system is 0 -flat if n 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

$\dot{x}=f(x\mathrm{,}u)$ (22)

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)

$\dot{x}=f(x)+{\displaystyle {\sum }_{i=1}^m}{g}_i(x)u_i$ (23)

The following definitions are used (Rigatos 2011a), (Marino 1990), (Marino Tomei 1992):

(i) Lie derivative: $L_{f}h(x)$ stands for the Lie derivative $L_{f}h(x)=(\nabla h)f$ and the repeated Lie derivatives are recursively defined as $L_f^{0}h=h\text{for}i=0$ , $L_f^{i}h=L_f{L}_f^{i-1}h=\nabla L_f^{i-1}hf\text{for}i=\mathrm{1,2,}\cdots$ .

(ii) Lie Bracket: $a{d}_f^{i}g$ stands for a Lie Bracket which is defined recursively as $a{d}_f^{i}g=[f\mathrm{,}a{d}_f^{i-1}]g$ with $a{d}_f^{0}g=g$ and $a{d}_{f}g=[f\mathrm{,}g]=\nabla gf-\nabla fg$ .

If the system of Eq. (25) can be linearized by a diffeomorphism $z=\phi (x)$ and a static state feedback $u=\alpha (x)+\beta (x)v$ into the following form

$\begin{array}{c}{\dot{z}}_{i\mathrm{,}j}=z_{i+\mathrm{1,}j}\hspace{0.17em}\text{for}\hspace{0.17em}1\le j\le m\hspace{0.17em}\text{and}\hspace{0.17em}1\le i\le v_j-1\\ {\dot{z}}_{v_{i\mathrm{,}j}}=v_j\end{array}$ (24)

with ${\displaystyle {\sum }_{j=1}^m}{v}_j=n$ , then $y_j=z_{\mathrm{1,}j}$ for $1\le j\le m$ are the 0-flat outputs which can be written as functions of only the elements of the state vector x. 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:

$\begin{array}{l}\left(\text{i}\right)G_0=\text{span}[g_1\mathrm{,}\cdots \mathrm{,}{g}_m]\\ \left(\text{ii}\right)G_1=\text{span}[g_1\mathrm{,}\cdots \mathrm{,}{g}_m\mathrm{,}a{d}_f{g}_1\mathrm{,}\cdots \mathrm{,}a{d}_f{g}_m]\\ \dots \\ (\text{k})G_k=\text{span}\{a{d}_f^j{g}_i\text{for}0\le j\le k\mathrm{,1}\le i\le m\}\end{array}$ .

Then, the linearization problem for the system of Eq. (25) can be solved if and only if

• (1). The dimension of $G_i\mathrm{,}i=\mathrm{1,}\cdots \mathrm{,}k$ is constant for $x\in X\subseteq R^n$ and for $1\le i\le n-1$

• (2). The dimension of $G_{n-1}$ if of order n.

• (3). The distribution $G_k$ is involutive for each $1\le k\le n-2$ .

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:

$\begin{array}{l}{\dot{x}}_1=x_2\\ {\dot{x}}_2=x_3\\ \dots \\ {\dot{x}}_{r_1-1}=x_{r_1}\\ {\dot{x}}_{r_1}=f_1(x)+{\displaystyle {\sum }_{j=1}^p}{g}_{1_j}(x)u_j+d_1\\ \\ {\dot{x}}_{r_1+1}=x_{r_1+2}\\ {\dot{x}}_{r_1+2}=x_{r_1+3}\\ \dots \\ {\dot{x}}_{p-1}=x_p\\ {\dot{x}}_p=f_p(x)+{\displaystyle {\sum }_{j=1}^p}{g}_{p_j}(x)u_j+d_p\\ \\ y_1=x_1\\ y_2=x_2\\ \dots \\ y_p=x_{n-r_p+1}\end{array}$ (25)

where $x=[x_1\mathrm{,}\cdots \mathrm{,}{x}_n{]}^T$ is the state vector of the transformed system (according to the differential flatness formulation), $u=[u_1\mathrm{,}\cdots \mathrm{,}{u}_p{]}^T$ is the set of control inputs, $y=[y_1\mathrm{,}\cdots \mathrm{,}{y}_p{]}^T$ is the output vector, $f_i$ are the drift functions and $g_{i\mathrm{,}j}\mathrm{,}i\mathrm{,}j=\mathrm{1,2,}\cdots \mathrm{,}p$ are smooth functions corresponding to the control input gains, while $d_j$ is a variable associated to external disturbances. In holds that $r_1+r_2+\cdots +r_p=n$ . Having written the initial nonlinear system into the canonical (Brunovsky) form it holds

$y_i^{(r_i)}=f_i(x)+{\displaystyle {\sum }_{j=1}^p}{g}_{ij}(x)u_j+d_j$ (26)

Next the following vectors and matrices can be defined

$\begin{array}{l}f(x)={\left[f_1(x)\mathrm{,}\cdots \mathrm{,}{f}_n(x)\right]}^T\\ g(x)={\left[g_1(x)\mathrm{,}\cdots \mathrm{,}{g}_n(x)\right]}^T\hspace{0.17em}\hspace{0.17em}\text{with}\hspace{0.17em}{g}_i(x)={\left[g_{1i}(x)\mathrm{,}\cdots \mathrm{,}{g}_{pi}(x)\right]}^T\\ A=diag[A_1\mathrm{,}\cdots \mathrm{,}{A}_p]\mathrm{,}\hspace{0.17em}\hspace{0.17em}B=diag[B_1\mathrm{,}\cdots \mathrm{,}{B}_p]\\ C^T=diag[C_1\mathrm{,}\cdots \mathrm{,}{C}_p]\mathrm{,}\hspace{0.17em}\hspace{0.17em}d={\left[d_1\mathrm{,}\cdots \mathrm{,}{d}_p\right]}^T\end{array}$ (27)

where matrix A has the MIMO canonical form, i.e. with elements

$A_i={\left(\begin{array}{cccc}0& 1& \cdots & 0\\ 0& 0& \cdots & 0\\ ⋮& ⋮& \cdots & ⋮\\ 0& 0& \cdots & 1\\ 0& 0& \cdots & 0\end{array}\right)}_{r_i\times r_i}{B}_i={\left(\begin{array}{c}0\\ 0\\ \cdots \\ 0\\ 1\end{array}\right)}_{r_i\times 1}{C}_i={\left(\begin{array}{c}1\\ 0\\ \cdots \\ 0\\ 0\end{array}\right)}_{1\times r_i}$ (28)

Thus, Eq. (26) can be written in state-space form

$\begin{array}{c}\dot{x}=Ax+B[f(x)+g(x)u+\tilde{d}]\\ y=C^{T}x\end{array}$ (30)

which can be also written in the equivalent form:

$\begin{array}{c}\dot{x}=Ax+Bv+B\tilde{d}\\ y=C^{T}x\end{array}$ (31)

where the transformed control input is defined as $v=f(x)+g(x)u$ .

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

$u=g^{-1}(\widehat{x})[-f(\widehat{x})+y_m^{(r)}+K_c^{T}e+u_c]$ (32)

where $f(\widehat{x})$ and $g(\widehat{x})$ are the state estimation-based approximations of $f(x)$ and $g(x)$ , respectively, while $y_m^{(r)}$ is the setpoint vector for output derivatives, i.e. $y_m^{(r)}=[{\dot{x}}_{r_1\mathrm{,}m}\mathrm{,}\cdots \mathrm{,}{\dot{x}}_{p\mathrm{,}m}{]}^T$ . $u_c$ is a supervisory control term, e.g. $H_{\infty }$ control term that is used to compensate for the effects of modelling inaccuracies and external disturbances. Moreover, $K_c^T$ is the feedback gain matrix that assures that the characteristic polynomial of matrix $A-B{K}_c^T$ 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)

$\begin{array}{l}\dot{x}=f(x)+q_0(x\mathrm{,}u)+{\displaystyle {\sum }_{i=1}^p}{\theta }_i{q}_i(x\mathrm{,}u)\mathrm{,}\hspace{0.17em}\text{or}\\ \dot{x}=f(x)+q_0(x\mathrm{,}u)+Q(x\mathrm{,}u)\theta \hspace{0.17em}x\in R^n\mathrm{,}\hspace{0.17em}u\in R^m\mathrm{,}\hspace{0.17em}\theta \in R^p\\ z=h(x)\mathrm{,}\hspace{0.17em}z\in R\end{array}$ (33)

with $q_i:R^n\times R^m\to R^n$ , $0\le i\le p$ , $f:R^n\to R^n$ , $h:R^n\to R$ , smooth functions, $h(x_0)=0$ , $q_0(x\mathrm{,0})=0$ for every $x\in R^n$ ; x is the state vector, $u(x\mathrm{,}t):R^{+}\to R^m$ is the control which is assumed to be known, θ is the parameter vector which is supposed to be constant and y 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 $R^n$

$\zeta =T(x)\mathrm{,}\hspace{0.17em}\hspace{0.17em}T(x_0)=0$ (34)

into the system

$\begin{array}{l}l\dot{\zeta }=A_c\zeta +{\psi }_0(z\mathrm{,}u)+{\displaystyle {\sum }_{i=1}^p}{\theta }_i{\psi }_i(z\mathrm{,}u)\Rightarrow \\ \dot{\zeta }=A_c\zeta +{\psi }_0(z\mathrm{,}u)+\mathrm{\Psi }(z\mathrm{,}u)\theta \\ \\ z=C_c\zeta \end{array}$ (35)

with

$A_c=\left(\begin{array}{ccccc}0& 1& 0& \cdots & 0\\ 0& 0& 1& \cdots & 0\\ ⋮& ⋮& ⋮& ⋮& ⋮\\ 0& 0& 0& \cdots & 0\end{array}\right)$ (36) $C_c=\left(\begin{array}{ccccc}1& 0& 0& \cdots & 0\end{array}\right)$ (37)

and ${\psi }_i:R\times R^m\to R^n$ smooth functions for $i=\mathrm{0,}\cdots \mathrm{,}p$ . 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:

• (i) $\text{rank}\{dh(x)\mathrm{,}{d}_{L_f}h(x)\mathrm{,}\cdots \mathrm{,}{d}_{L_f^{n-1}}h(x)\}=n$ , $\forall x\in R^n$ (which implies local observability). It is noted that $L_{f}h(x)$ stands for the Lie derivative $L_{f}h(x)=(\nabla h)f$ and the repeated Lie derivatives are recursively defined as $L_f^{0}h=h\text{for}i=0$ , $L_f^{i}h=L_f{L}_f^{i-1}h=\nabla L_f^{i-1}hf\text{for}i=\mathrm{1,2,}\cdots$ .

• (ii) $[a{d}_f^{i}g\mathrm{,}a{d}_f^{j}g]=\mathrm{0,0}\le i\mathrm{,}j\le n-1$ . It is noted that $a{d}_f^{i}g$ stands for a Lie Bracket which is defined recursively as $a{d}_f^{i}g=[f\mathrm{,}a{d}_f^{i-1}]g$ with $a{d}_f^{0}g=g$ and $a{d}_{f}g=[f\mathrm{,}g]=\nabla gf-\nabla fg$ .

• (iii) $[q_i\mathrm{,}a{d}_f^{i}g]=\mathrm{0,0}\le i\le p\mathrm{,0}\le j\le n-2\forall u\in R^m$ .

• (iv) the vector fields $a{d}_f^{i}g\mathrm{,0}\le i\le n-1$ are complete, in which g is the vector field satisfying

$<\left(\begin{array}{c}dh\\ ⋮\\ d(L_f^{n-1}h)\end{array}\right)\mathrm{,}g>=\left(\begin{array}{c}0\\ ⋮\\ 1\end{array}\right)$ (38)

Then for every parameter vector θ, the system

$\begin{array}{l}\widehat{\zeta }=A_c\widehat{\zeta }+{\psi }_0(z\mathrm{,}u)+{\displaystyle \sum _{i=1}^p}{\theta }_i{\psi }_i(z\mathrm{,}u)+K(z-C_c\widehat{\zeta })\\ \widehat{x}=T^{-1}(\widehat{\zeta })\end{array}$ (39)

is an asymptotic observer for a suitable choice of K provided that the state $x(t)$ is bounded, with estimation error dynamics

$\dot{e}=(A_c-K{C}_c)e=\left(\begin{array}{ccccc}-k_1& 1& 0& \cdots & 0\\ -k_2& 0& 1& \cdots & 0\\ \cdots & \cdots & \cdots & \cdots & \cdots \\ -k_{n-1}& 0& 0& \cdots & 1\\ -k_n& 0& 0& \cdots & 0\end{array}\right)e$ (40)

The eigenvalues of $A_c-K{C}_c$ can be arbitrarily placed by choosing the vector K, since they coincide with the roots of the polynomial $s^n+k_1{s}^{n-1}+\cdots +k_n$ .

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

$x^{(n)}=f(x\mathrm{,}t)+g(x\mathrm{,}t)u(x\mathrm{,}t)$ (41)

where $z=x$ is the system's output, and $f(x\mathrm{,}t)$ , $g(x\mathrm{,}t)$ 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 ${\zeta }_i=x^{(i-1)}\mathrm{,}i=\mathrm{1,}\cdots \mathrm{,}n$ , and $x^{(n)}=f(x\mathrm{,}t)+g(x\mathrm{,}t)u(x\mathrm{,}t)=v(\zeta \mathrm{,}t)$ , i.e. ${\dot{\zeta }}_n=v(\zeta \mathrm{,}t)$ , one obtains the linearized system of the form

$\begin{array}{l}{\dot{\zeta }}_1={\zeta }_2\\ {\dot{\zeta }}_2={\zeta }_3\\ \cdots \hspace{0.17em}\cdots \\ {\dot{\zeta }}_{n-1}={\zeta }_n\\ {\dot{\zeta }}_n=v(\zeta \mathrm{,}t)\end{array}$ (42)

which in turn can be written in state-space equations as

$\left(\begin{array}{c}{\dot{\zeta }}_1\\ {\dot{\zeta }}_2\\ \cdots \\ {\dot{\zeta }}_{n-1}\\ {\dot{\zeta }}_n\end{array}\right)=\left(\begin{array}{ccccc}0& 1& 0& \cdots & 0\\ 0& 0& 1& \cdots & 0\\ \cdots & \cdots & \cdots & \cdots & \cdots \\ 0& 0& 0& \cdots & 1\\ 0& 0& 0& \cdots & 0\\ & & & & \end{array}\right)\left(\begin{array}{c}{\zeta }_1\\ {\zeta }_2\\ \cdots \\ {\zeta }_{n-1}\\ {\zeta }_n\end{array}\right)+\left(\begin{array}{c}0\\ 0\\ \cdots \\ 0\\ 1\end{array}\right)v(\zeta \mathrm{,}t)$ (43) $z=\left(\begin{array}{ccccc}1& 0& 0& \cdots & 0\end{array}\right)\zeta$ (44)

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 K appearing in Eq. (38) can be found using either linear observer design methods (in that case the elements of the observation error gain matrix K 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

$\left(\begin{array}{cc}{M}_{11}& M_{12}\\ M_{21}& M_{22}\end{array}\right)\left(\begin{array}{c}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right)+\left(\begin{array}{c}{F}_1(\theta \mathrm{,}\dot{\theta })\\ F_2(\theta \mathrm{,}\dot{\theta })\end{array}\right)+\left(\begin{array}{c}{G}_1(\theta )\\ G_2(\theta )\end{array}\right)=\left(\begin{array}{c}{T}_1\\ T_2\end{array}\right)$ (45)

where $M\in R^{2\times 2}$ is the inertia matrix, $F\in R^{2\times 1}$ is the Coriolis and centripetal forces vector, $G\in R^{2\times 1}$ is the vector of gravitational forces and $T\in R^{2\times 1}$ is the vector of control input torques. Equivalently, one has

$\begin{array}{c}\left(\begin{array}{c}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right)=-{\left(\begin{array}{cc}{M}_{11}& M_{12}\\ M_{21}& M_{22}\end{array}\right)}^{-1}\left(\begin{array}{c}{F}_1(\theta \mathrm{,}\dot{\theta })\\ F_2(\theta \mathrm{,}\dot{\theta })\end{array}\right)\\ -{\left(\begin{array}{cc}{M}_{11}& M_{12}\\ M_{21}& M_{22}\end{array}\right)}^{-1}\left(\begin{array}{c}{G}_1(\theta )\\ G_2(\theta )\end{array}\right)+{\left(\begin{array}{cc}{M}_{11}& M_{12}\\ M_{21}& M_{22}\end{array}\right)}^{-1}\left(\begin{array}{c}{T}_1\\ T_2\end{array}\right)\end{array}$ (46)

Denoting the inverse of the inertia matrix as

$\left(\begin{array}{cc}{M}_{11}& M_{12}\\ M_{21}& M_{22}\end{array}\right)=\left(\begin{array}{cc}{N}_{11}& N_{12}\\ N_{21}& N_{22}\end{array}\right)$ (47)

then one obtains

$\begin{array}{c}\left(\begin{array}{c}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right)=-\left(\begin{array}{cc}{N}_{11}& N_{12}\\ N_{21}& N_{22}\end{array}\right)\left(\begin{array}{c}{F}_1(\theta \mathrm{,}\dot{\theta })\\ F_2(\theta \mathrm{,}\dot{\theta })\end{array}\right)\\ -\left(\begin{array}{cc}{N}_{11}& N_{12}\\ N_{21}& N_{22}\end{array}\right)\left(\begin{array}{c}{G}_1(\theta )\\ G_2(\theta )\end{array}\right)+\left(\begin{array}{cc}{N}_{11}& N_{12}\\ N_{21}& N_{22}\end{array}\right)\left(\begin{array}{c}{T}_1\\ T_2\end{array}\right)\end{array}$ (48)

Figure 2.

A 2-DOF rigid-link robotic manipulator or equivalently

$\left(\begin{array}{c}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right)=\left(\begin{array}{c}-N_{11}{F}_1(\theta \mathrm{,}\dot{\theta })-N_{12}{F}_2(\theta \mathrm{,}\dot{\theta })-N_{11}{G}_1(\theta )-N_{12}{G}_2(\theta )+N_{11}{T}_1+N_{12}{T}_2\\ -N_{21}{F}_2(\theta \mathrm{,}\dot{\theta })-N_{22}{F}_2(\theta \mathrm{,}\dot{\theta })-N_{21}{G}_1(\theta )-N_{22}{G}_2(\theta )+N_{21}{T}_1+N_{22}{T}_2\end{array}\right)$ (49)

which can be also written as

$\begin{array}{c}{\ddot{\theta }}_1=-N_{11}{F}_1(\theta \mathrm{,}\dot{\theta })-N_{12}{F}_2(\theta \mathrm{,}\dot{\theta })-N_{11}{G}_1(\theta )\\ -N_{12}{G}_2(\theta )+\left(\begin{array}{cc}{N}_{11}& N_{12}\end{array}\right)\left(\begin{array}{c}{T}_1\\ T_2\end{array}\right)\end{array}$ (50) $\begin{array}{c}{\ddot{\theta }}_2=-N_{21}{F}_1(\theta \mathrm{,}\dot{\theta })-N_{22}{F}_2(\theta \mathrm{,}\dot{\theta })-N_{21}{G}_1(\theta )\\ -N_{22}{G}_2(\theta )+\left(\begin{array}{cc}{N}_{21}& N_{22}\end{array}\right)\left(\begin{array}{c}{T}_1\\ T_2\end{array}\right)\end{array}$ (51)

The following state variables are defined

$x_1={\theta }_1\hspace{0.17em}{x}_2={\dot{\theta }}_1\hspace{0.17em}{x}_3={\theta }_2\hspace{0.17em}{x}_4={\dot{\theta }}_2$ (52)

It holds that

$\begin{array}{l}{\ddot{x}}_1=f_1(x)+g_1(x)u\\ {\ddot{x}}_3=f_2(x)+g_2(x)u\end{array}$ (53)

where

$\begin{array}{c}{f}_1(x)=-N_{11}{F}_1(\theta \mathrm{,}\dot{\theta })-N_{12}{F}_2(\theta \mathrm{,}\dot{\theta })\\ -N_{11}{G}_1(\theta )-N_{12}{G}_2(\theta )\hspace{0.17em}\in R^{1\times 1}\\ g_1(x)=[N_{11}\hspace{0.17em}{N}_{12}]\hspace{0.17em}\in R^{1\times 2}\\ \\ f_2(x)=-N_{21}{F}_1(\theta \mathrm{,}\dot{\theta })-N_{22}{F}_2(\theta \mathrm{,}\dot{\theta })\\ \\ -N_{21}{G}_1(\theta )-N_{22}{G}_2(\theta )\hspace{0.17em}\in {\text{R}}^{1\times 1}\\ g_2(x)=[N_{21}\hspace{0.17em}{N}_{22}]\hspace{0.17em}\in R^{2\times 2}\end{array}$ (54)

The flat output is defined as

$y=[{\theta }_1\mathrm{,}{\theta }_2]=[x_1\mathrm{,}{x}_3]$ (55)

It holds that

$\begin{array}{c}{\dot{x}}_1=x_2\\ {\dot{x}}_2=f_1(x)+g_1(x)u\\ {\dot{x}}_3=x_4\\ {\dot{x}}_4=f_2(x)+g_2(x)u\end{array}$ (56)

therefore all system state variables can be written as functions of the flat output y and its derivatives. The same holds for the control input u. Indeed, one has

$\begin{array}{l}{x}_1=[1\hspace{0.17em}0]y^T\hspace{0.17em}{x}_2=[1\hspace{0.17em}0]{\dot{y}}^T\\ x_3=[0\hspace{0.17em}1]y^T\hspace{0.17em}{x}_2=[0\hspace{0.17em}1]{\dot{y}}^T\end{array}$ (57)

Moreover, from Eq. (58) it holds

$\begin{array}{c}\left(\begin{array}{c}{\ddot{x}}_1\\ {\ddot{x}}_3\end{array}\right)=\left(\begin{array}{c}{f}_1(x)\\ f_2(x)\end{array}\right)+\left(\begin{array}{c}{g}_1(x)\\ g_2(x)\end{array}\right)u\hspace{0.17em}\hspace{0.17em}\\ \text{i.e.}\hspace{0.17em}\hspace{0.17em}u={\left(\begin{array}{c}{g}_1(x)\\ g_2(x)\end{array}\right)}^{-1}\{\left(\begin{array}{c}{\ddot{x}}_1\\ {\ddot{x}}_3\end{array}\right)-\left(\begin{array}{c}{f}_1(x)\\ f_2(x)\end{array}\right)\}\end{array}$ (58)

Knowing that $x=h(y\mathrm{,}\dot{y})$ one finally obtains

$u={\left(\begin{array}{c}{g}_1(h(y\mathrm{,}\dot{y}))\\ g_2(h(y\mathrm{,}\dot{y}))\end{array}\right)}^{-1}\{\left(\begin{array}{c}[1\hspace{0.17em}0]{\ddot{y}}^T\\ [1\hspace{0.17em}0]{\ddot{y}}^T\end{array}\right)-\left(\begin{array}{c}{f}_1(h(y\mathrm{,}\dot{y}))\\ f_2(h(y\mathrm{,}\dot{y}))\end{array}\right)\}$ (59)

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

$\begin{array}{l}{\ddot{x}}_1=f_1(x\mathrm{,}t)+g_1(x\mathrm{,}t)u+{\tilde{d}}_1\\ {\ddot{x}}_3=f_2(x\mathrm{,}t)+g_2(x\mathrm{,}t)u+{\tilde{d}}_2\end{array}$ (60) $\left(\begin{array}{c}{\ddot{x}}_1\\ {\ddot{x}}_3\end{array}\right)=\left(\begin{array}{c}{f}_1(x\mathrm{,}t)\\ f_2(x\mathrm{,}t)\end{array}\right)+\left(\begin{array}{c}{g}_1(x\mathrm{,}t)\\ g_2(x\mathrm{,}t)\end{array}\right)u+\left(\begin{array}{c}{\tilde{d}}_1\\ {\tilde{d}}_2\end{array}\right)$ (61)

Moreover, the following control input is defined

$u={\left(\begin{array}{c}{g}_1(\widehat{x}\mathrm{,}t)\\ g_2(\widehat{x}\mathrm{,}t)\end{array}\right)}^{-1}\{\left(\begin{array}{c}{\ddot{x}}_1^d\\ {\ddot{x}}_3^d\end{array}\right)-\left(\begin{array}{c}{f}_1(\widehat{x}\mathrm{,}t)\\ f_2(\widehat{x}\mathrm{,}t)\end{array}\right)-\left(\begin{array}{c}{K}_1^T\\ K_2^T\end{array}\right)e+\left(\begin{array}{c}{u}_{c_1}\\ u_{c_2}\end{array}\right)\}$ (62)

where ${[u_{c_1}{u}_{c_2}]}^T$ is a supervisory control term that is used for the compensation of the model's uncertainties as well as of the external disturbances and $K_i^T=[k_1^i\mathrm{,}{k}_2^i\mathrm{,}\cdots \mathrm{,}{k}_{n-1}^i\mathrm{,}{k}_n^i]$ 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 $x\in R^{4\times 1}$ , with the state variables defined in Eq. (51), the following matrices are defined

$A=\left(\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right)\mathrm{,}\hspace{0.17em}B=\left(\begin{array}{cc}0& 0\\ 1& 0\\ 0& 0\\ 0& 1\end{array}\right)\mathrm{,}\hspace{0.17em}C=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right)$ (63)

Using the matrices of Eq. (62) one obtains the Brunovsky form of the MIMO robot model dynamics

$\begin{array}{c}\dot{x}=Ax+Bv\\ y=Cx\end{array}$ (64)

where the new input v is given by

$v=\left(\begin{array}{c}{f}_1(x\mathrm{,}t)\\ f_2(x\mathrm{,}t)\end{array}\right)+\left(\begin{array}{c}{g}_1(x\mathrm{,}t)\\ g_2(x\mathrm{,}t)\end{array}\right)u+\left(\begin{array}{c}{\tilde{d}}_1\\ {\tilde{d}}_2\end{array}\right)$ (65)

For the robotic model of Eq. (65) state estimation can be performed using the standard Kalman Filter recursion, as described in Eq. (5) and Eq. (6).

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 $y_1=x_1$ and $y_2=x_3$ . 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 $[y_1\mathrm{,}{y}_2]$ and of the associated derivatives.

Two different set-points were studied: i) a sinusoidal signal of amplitude $1.0$ and period $T=10sec$ , ii) a see-saw set-point of amplitude $0.30$ and period $T=10sec$ . At the beginning of the second half of the simulation time an additive sinusoidal disturbance of amplitude $A=0.5$ and period $T=10$ sec was applied to the system. The approximations $\widehat{f}$ and $\widehat{g}$ 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.

Figure 3.

Derivative-free Kalman Filtering: (a) Tracking of a sinusoidal position set-point (dashed line) by the angle of the first joint of the robot (continuous line) (b) Tracking of a sinusoidal velocity set-point (dashed line) by the angular velocity of the first joint of the robot (continuous line)

Figure 4.

Derivative-free Kalman Filtering: (a) Tracking of a sinusoidal position set-point (dashed line) by the angle of the second joint of the robot (continuous line) (b) Tracking of a sinusoidal velocity set-point (dashed line) by the angular velocity of the second joint of the robot (continuous line)

Figure 5.

Tracking of a sinusoidal setpoint by the 2-DOF robotic manipulator with the use of the derivative-free Kalman Filter: (a) Control input (motor torque) applied to the first joint (b) Control input (motor torque) applied to the second joint

Figure 6.

Derivative-free Kalman Filtering: (a) Tracking of a seesaw position set-point (dashed line) by the angle of the first joint of the robot (continuous line) (b) Tracking of a seesaw velocity set-point (dashed line) by the angular velocity of the first joint of the robot (continuous line)

Figure 7.

Derivative-free Kalman Filtering: (a) Tracking of a seesaw position set-point (dashed line) by the angle of the second joint of the robot (continuous line) (b) Tracking of a seesaw velocity set-point (dashed line) by the angular velocity of the second joint of the robot (continuous line)

Figure 8.

Tracking of a seesaw setpoint by the 2-DOF robotic manipulator with the use of the derivative-free Kalman Filter: (a) Control input (motor torque) applied to the first joint (b) Control input (motor torque) applied to the second joint

Figure 9.

Extended Kalman Filtering: (a) Tracking of a sinusoidal position set-point (dashed line) by the angle of the first joint of the robot (continuous line) (b) Tracking of a sinusoidal velocity set-point (dashed line) by the angular velocity of the first joint of the robot (continuous line)

Figure 10.

Extended Kalman Filtering: (a) Tracking of a sinusoidal position set-point (dashed line) by the angle of the second joint of the robot (continuous line) (b) Tracking of a sinusoidal velocity set-point (dashed line) by the angular velocity of the second joint of the robot (continuous line)

Figure 11.

Tracking of a sinusoidal setpoint by the 2-DOF robotic manipulator with the use of the Extended Kalman Filter: (a) Control input (motor torque) applied to the first joint (b) Control input (motor torque) applied to the second joint

Figure 12.

Extended Kalman Filtering: (a) Tracking of a seesaw position set-point (dashed line) by the angle of the first joint of the robot (continuous line) (b) Tracking of a seesaw velocity set-point (dashed line) by the angular velocity of the first joint of the robot (continuous line)

Figure 13.

Extended Kalman Filtering: (a) Tracking of a seesaw position set-point (dashed line) by the angle of the second joint of the robot (continuous line) (b) Tracking of a seesaw velocity set-point (dashed line) by the angular velocity of the second joint of the robot (continuous line)

Figure 14.

Tracking of a seesaw setpoint by the 2-DOF robotic manipulator with the Extended Kalman Filter: (a) Control input (motor torque) applied to the first joint (b) Control input (motor torque) applied to the second joint

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.

7. Conclusions

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.