Introduction

In the recent years there has been significant effort in the design of intelligent autonomous vehicles capable of operating in variable conditions. The precise modeling of the vehicles’ dynamics improves the efficiency of vehicles controllers in adverse cases, for example in high velocity, when performing abrupt maneuvers, under mass and loads changes or when moving on rough terrain. Using model-based control approaches it is possible to design a nonlinear controller that maintains the vehicle’s motion characteristics within desirable ranges [1–6]. When the vehicle’s dynamics is subject to modeling uncertainties or when there are unknown forces and torques exerted on the vehicle it is important to be in position to estimate in real-time disturbances and unknown dynamics so as to compensate them through the control input and to maintain the satisfactory performance of the vehicle’s automated steering system. In this direction, estimation for the unknown dynamics of the vehicle and state estimation-based control schemes have been developed [7–10].

The objective of this research work is two-fold. On the one side it analyzes the design of a controller for autonomous navigation of automatic ground vehicles (AGVs). On the other side it proposes a solution to the problem of four-wheel vehicle control under model uncertainties and external disturbances. Considering, that only under ideal conditions the dynamic model of the vehicle is precisely known (e.g. there may be variations in the transported mass, or in the cornering stiffness coefficients characterizing the interaction of the tires with the ground, or in the position of the vehicle’s center of gravity) and that in several cases there is uncertainty about the forces and torques developed on the vehicle (e.g. traction and braking torques on the wheels, forces due to traction of implements, or lateral forces which generate torques affecting the yaw stability of the vehicle) the need for designing robust controllers of the autonomous vehicles becomes obvious [11–14]. By compensating efficiently such disturbances forces and torques safety features of the vehicle are improved and its autonomous functioning remains reliable even under adverse road conditions.

Dynamic analysis for the 4-wheel vehicle is provided. A 3-DOF model is introduced having as elements the vehicle’s velocity along the horizontal and vertical axis of an inertial reference frame as well as the rate of change of its orientation angle (this is the angle defined by the vehicle’s longitudinal axis and the horizontal axis of the frame). Lateral forces are shown to affect the vehicle’s motion and to be dependent on the longitudinal and lateral velocity of the vehicle, on the yaw rate and on the cornering stiffness coefficients for the front and rear tires. The control inputs to the vehicles’ dynamic model are the traction/bracking wheel torque and the turn angle of the steering wheel. Since the parameters of the dynamic model of the vehicle cannot always be known with precision or may be time-varying (e.g. cornering stiffness coefficients, transported mass) and since there may be unmodeled external forces and torques exerted on the vehicle (e.g. due to road condition, disturbances in traction forces) it is important to design a control loop with robustness to the aforementioned sources of uncertainty and disturbances, as well as to be in position to estimate in real-time such disturbances through the processing of measurements from a small number of on-board sensors.

Next, it is shown how a nonlinear controller for the aforementioned vehicle’s model can be obtained through the application of the differential flatness theory [15–18]. The flat output for the vehicle’s model is a vector comprising the x-axis velocity and a second variable based on a linear relation between the y-axis velocity and the rate of change of the orientation angle [1, 2]. By expressing all state variables and the control input of the four-wheel vehicle model as functions of the flat output and its derivatives the system’s dynamic model is transformed into the linear Brunovksy (canonical) form [19, 20]. For the latter model it is possible to design a state feedback controller that enables accurate tracking of the vehicle’s velocity set-points.

By exploiting the vehicle’s exactly linearized model and its transformation into a canonical form it is possible to design a linear state estimator for approximating the system’s state vector through the processing of measurements coming from a small number of on-board sensors. To this end the concept of Derivative-free nonlinear Kalman Filtering is introduced. Unlike the Extended Kalman Filter, the proposed filtering method provides estimates of the state vector of the nonlinear system without the need for derivatives and Jacobians calculation [21–23]. By avoiding linearization approximations, the proposed filtering method improves the accuracy of estimation of the system’s state variables. Moreover, it is shown that it is possible to redesign the Kalman Filter in the form of a disturbance observer and using the estimation of the disturbance to develop an auxiliary control input that compensates for their effects. In this way the vehicle’s control and autonomous navigation system can become robust with respect to uncertainties in the model’s parameters or uncertainties about external forces and torques. It is also noted that in terms of computation speed the proposed Kalman Filter-based disturbance estimator for the vehicle is faster than disturbance estimators that may be based on other nonlinear filtering approaches (e.g. Extended Kalman Filter, Unscented Kalman Filter or Particle Filter) thus becoming advantageous for the real-time estimation of the unknown vehicle dynamics [24, 25].

The efficiency of the proposed nonlinear control and Kalman Filter-based disturbances estimation scheme is evaluated through numerical simulation tests. It has been shown that by accurately estimating disturbance forces and torques the control loop succeeds elimination of the tracking error for all state variables of the vehicle. The structure of the paper is as follows: in Sect. 2 the dynamic model of the 4-wheel vehicle is analyzed. In Sect. 3 it is proven that the 4-wheel vehicle dynamical model satisfies the differential flatness properties and the stages for designing a flatness-based control for the vehicle’s model are analyzed. In Sect. 4 state estimation with the use of state observers and with the use of the Kalman Filter is explained. In Sect. 5 the concept of disturbance observers is introduced as a solution to the problem of state estimation under model uncertainties and external disturbances. In Sect. 6 it is shown how the Derivative-free nonlinear Kalman Filter can be redesigned in the form of a disturbance observer so as to estimate the disturbance torques and forces that affect the vehicle’s dynamic model. In Sect. 7 evaluation tests are provided about the performance of the vehicle’s nonlinear control scheme and about the performance of the state estimator that aims at real-time identification of uncertainty in the vehicle’s dynamics. It is shown that by applying an additional control input that is based on the estimated disturbance variables the stability of the vehicle’s control loop is preserved and elimination of velocity tracking error is succeeded. Finally in Sect. 8 concluding remarks are provided.

Dynamic Model of the Vehicle

Definition of Parameters in 4-Wheel Vehicle Dynamic Model

With reference to Fig. 1 (where the lateral forces applied on the wheels are considered to define the vehicle’s motion) one has the following parameters: \(\beta \) is the angle between the velocity and the vehicle’s transversal angle, V is the velocity vector of the vehicle, \(\psi \) is the yaw angle (rotation round the z axis), \(f_x\): is the aggregate force along the x axis, \(f_y\) is the aggregate force along the y axis, \(T_z\) is the aggregate torque round the z axis and \(\delta \) is the steering angle of the front wheels [1, 4, 17].

Fig. 1
figure 1

Nonlinear 4-wheeled vehicle model

Full size image

The motion of the vehicle is described by the following set of equations:

  1. 1.

    Longitudinal motion

    $$\begin{aligned} -mV(\dot{\beta }+\dot{\psi })sin(\beta )+m\dot{V}cos(\beta )=f_x \end{aligned}$$
    (1)
  2. 2.

    Lateral motion

    $$\begin{aligned} mV(\dot{\beta }+\dot{\psi })cos(\beta )+m\dot{V}sin(\beta )=f_y \end{aligned}$$
    (2)
  3. 3.

    Yaw turn

    $$\begin{aligned} I\ddot{\psi }=T_z \end{aligned}$$
    (3)

The above described vehicle dynamics can be also written in matrix form

$$\begin{aligned} \begin{pmatrix} -sin(\beta ) &{} cos(\beta ) &{} 0 \\ cos(\beta ) &{} sin(\beta ) &{} 0 \\ 0 &{} 0 &{} 1 \end{pmatrix} \begin{pmatrix} mV(\dot{\beta }+\dot{\psi }) \\ m\dot{V} \\ I\ddot{\psi } \end{pmatrix}= \begin{pmatrix} f_x \\ f_y\\ T_z \end{pmatrix} \end{aligned}$$
(4)

Finally a matrix relation is provided about the transformation of forces on a tire into forces and torques along the vehicle’s axes:

$$\begin{aligned} \begin{pmatrix} f_x \\ f_y \\ T_z \end{pmatrix}= \begin{pmatrix} -sin(\delta ) &{} 0 \\ cos(\delta ) &{} 1 \\ {L_1}cos(\delta ) &{} -L_2 \end{pmatrix} \begin{pmatrix} F_f \\ F_r \end{pmatrix} \end{aligned}$$
(5)

Vehicle Dynamical Model with Longitudinal and Lateral Forces

The previous model of Fig. 1 is rexamined considering that \(\dot{\beta }=0\) and that \(\psi \) is the yaw angle formed between the vehicle’s longitudinal axis and the horizontal axis of an inertial reference frame. Moreover, it is assumed that apart from the lateral forces there are traction torques transferred from the engine to the front wheels as well as braking torques on the rear and front wheels. Due to the distance between the wheels axes and the vehicle’s center of gravity, torques are also generated along the vehicle’s z-axis. With reference to Fig. 2 the model of the vehicle’s dynamics is formulated as follows [1, 4, 17]:

$$\begin{aligned} m{\alpha _x}= & {} m(\dot{V}_x-\dot{\psi }\dot{V}_y)=F_{x_1}+F_{x_2}\nonumber \\ m{\alpha _y}= & {} m(\dot{V}_y+\dot{\psi }\dot{V}_x)=F_{y_1}+F_{y_2}\\ {I_z}\ddot{\psi }= & {} T_{z_1}+T_{z_2}\nonumber \end{aligned}$$
(6)
Fig. 2
figure 2

Vehicle model with longitudinal and lateral forces

Full size image

where \(a_x\) and \(a_y\) are accelerations along the axes of the inertial reference frame and \(\dot{V}_x\), \(\dot{V}_y\) in a reference frame that rotates with the yaw rate \(\dot{\psi }\). The forces \(F_{x_i}, \ i=1,2\) on the vehicle’s longitudinal axis and \(F_{y_i}, \ i=1,2\) on the vehicle’s transversal axis are computed from the horizontal and vertical forces applied on the vehicle’s wheels as follows:

$$\begin{aligned} \begin{aligned} F_{x_1}&={F_{x_f}}cos(\delta )-{F_{y_f}}sin(\delta ) \\ F_{x_2}&=F_{x_r} \\ F_{y_1}&={F_{y_f}}sin(\delta )+{F_{y_f}}cos(\delta ) \\ F_{y_2}&=F_{y_r} \\ T_{z_1}&={L_f}({F_{y_f}}cos(\delta )+{F_{x_f}}sin(\delta )) \\ T_{z_2}&=-{L_r}F_{y_r} \end{aligned} \end{aligned}$$
(7)

About the longitudinal and the lateral forces applied to the vehicle one has:

  1. 1.

    Longitudinal force on the front wheel

    $$\begin{aligned} F_{x_f}=\left( {1 \over R}\right) ({I_r}\dot{\omega }_f+{T_m}-T_{b_f}) \end{aligned}$$
    (8)
  2. 2.

    Longitudinal force on the rear wheel

    $$\begin{aligned} F_{x_r}=-\left( {1 \over R}\right) (T_{b_r}+{I_r}\dot{\omega }_r) \end{aligned}$$
    (9)
  3. 3.

    Lateral force on the front wheel (taking that the angle \(\beta \) between the vehicle’s longitudinal axis and the wheel’s velocity vector is approximated by \(\beta ={{V_y+\dot{\psi }{L_f}} \over {V_x}}\))

    $$\begin{aligned} F_{y_f}={C_f}\left( \delta -{{V_y+\dot{\psi }{L_f}} \over {V_x}}\right) \end{aligned}$$
    (10)
  4. 4.

    Lateral force on the rear wheel (taking that for the rear wheel the steering angle is \(\delta =0\) and that the angle \(\beta \) between the vehicle’s longitudinal axis and the wheel’s velocity vector is approximated by \(\beta ={{V_y-\dot{\psi }{L_r}} \over {V_x}}\)).

    $$\begin{aligned} F_{y_r}=-{C_r}{{V_y-\dot{\psi }{L_r}} \over {V_x}} \end{aligned}$$
    (11)

where \(C_f\) and \(C_r\) are the cornering stiffness coefficients for the front and rear tires respectively. Nominal values of these cornering stiffness coefficients can be estimated through identification procedures. The substitution of Eq. (8) to Eq. (11) into Eq. (6) results into

$$\begin{aligned} \begin{aligned} m\dot{V}_x&=m\dot{\psi }{V_y}-{I_r \over R}(\dot{\omega }_r+\dot{\omega }_f)+{1 \over R}(T_m-T_{b_f}-T_{b_r})\\&\quad +{C_f}\left( {{V_y+\dot{\psi }{L_f}} \over {V_x}}\right) \delta -{C_f}{\delta ^2}\\ m\dot{V}_y&=\!-\!m\dot{\psi }{V_x}\!-\!{C_f}\left( {{{V_y\!+\! \dot{\psi }{L_f}}} \over {V_x}}\right) \!-\!{C_r}\left( {{V_y\!-\! \dot{\psi }{L_f}} \over {V_x}}\right) \\&\quad +\left( {1 \over R}\right) (T_m-T_{b_f})\delta +\left( C_f-{I_r \over R}\dot{\omega }_f\right) \delta \\ {I_z}\ddot{\psi }&=-{L_f}{C_f}\left( {{V_y+\dot{\psi }{L_f}} \over {V_x}}\right) +{L_r}{C_r}\left( {{V_y-\dot{\psi }{L_f}} \over {V_x}}\right) \\&\quad +{L_f \over R}(T_m-T_{b_f})\delta +{L_f}\left( T_m-{I_r \over R}\right) \delta \end{aligned} \end{aligned}$$
(12)

The motion of the vehicle along its longitudinal axis is controlled by the traction or braking wheel torque \(T_{\omega }={T_m}-{T_b}\) with \(T_b=T_{b_f}+T_{b_r}\) and the lateral movement via the steering angle \(\delta \). The two control inputs of the four wheel vehicle model are

$$\begin{aligned} u_1= & {} T_{\omega }\nonumber \\ u_2= & {} \delta \end{aligned}$$
(13)

A first form of the vehicle’s dynamic model is

$$\begin{aligned} \dot{x}=f(x,t)+g(x,t)u+{g_1}{u_1}{u_2}+{g_2}{u_2^2} \end{aligned}$$
(14)

where

$$\begin{aligned} f(x,t)= & {} \begin{pmatrix} {I_r \over {mR}}(\dot{\omega }_r+\dot{\omega }_f) \\ \dot{\psi }{V_x}+{1 \over m}\left( -C_f{(V_y+L_f\dot{\psi }) \over {V_x}}-C_r{(V_y-L_f\dot{\psi }) \over {V_x}}\right) \\ {1 \over {I_z}}\left( -{L_f}C_f{(V_y+L_f\dot{\psi }) \over {V_x}}+{L_r}C_r{(V_y-L_f\dot{\psi }) \over {V_x}}\right) \end{pmatrix}\nonumber \\ \end{aligned}$$
(15)
$$\begin{aligned} g(x,t)= & {} \begin{pmatrix} {1 \over {mR}} &{} {C_f \over m}\left( {{V_y+L_f\dot{\psi }} \over V_x}\right) \\ 0 &{} \left( {{{C_f}R-{I_r}\dot{\omega }_f} \over {mR}}\right) \\ 0 &{} ({{L_f}{C_f}R-{L_f}{I_r}\dot{\omega }_f)} \over {{I_z}R} \end{pmatrix} \end{aligned}$$
(16)
$$\begin{aligned} g_1= & {} \begin{pmatrix} 0 \\ {1 \over {mR}} \\ {{L_f} \over {{I_z}R}} \end{pmatrix} g_2= \begin{pmatrix} -{C_f \over m} \\ 0 \\ 0 \end{pmatrix} x=\begin{pmatrix} V_x \\ V_y \\ \dot{\psi } \end{pmatrix} u=\begin{pmatrix} u_1 \\ u_2 \end{pmatrix}\nonumber \\ \end{aligned}$$
(17)

The previously analyzed nonlinear model of the vehicle’s dynamics can be simplified if the control inputs \({u_1}{u_2}\) and \(u_2^2\) are not taken into account. In the latter case the dynamics of the vehicle takes the form

$$\begin{aligned} \dot{x}=f(x,t)+g(x,t)u \end{aligned}$$
(18)

Flatness-Based Controller for the 3-DOF Vehicle Model

Differential Flatness Theory

Differential flatness theory can be applied to the generic class of systems \(\dot{x}=f(x,u)\). In this study, the interest is in dynamic models of the form of Eq. (18).

The principles of the differential flatness theory have been extensively studied in the relevant bibliography [16, 17, 23]: A finite dimensional system is considered. This can be written in the form of an ordinary differential equation (ODE), i.e. \(S_i(w,\dot{w},\ddot{w},\ldots ,w^{(i)}), \ \ i=1,2,\ldots ,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=1,2,\ldots ,q\) are the associated derivatives. Such a system is said to be differentially flat if there exists a set of m functions \(y=(y_1,\ldots ,y_m)\) of the system variables and of their time-derivatives, i.e. \(y_i=\phi (w,\dot{w},\ddot{w},\ldots ,w^{(\alpha _i)}), \ i=1,\ldots ,m\) satisfying the following two conditions [15, 21]:

  1. 1.

    There does not exist any differential relation of the form \(R(y,\dot{y},\ldots ,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. 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,\dot{y},\ldots ,y^{(\gamma _i)}), \ i=1,\ldots ,s \). An equivalent definition of differentially flat systems is as follows:

Definition

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

$$\begin{aligned}&h: \ {R^n}{\times }{{(R^m)}^{r+1}}{\rightarrow }{R^m},\nonumber \\&\phi : \ {(R^m)^r}{\rightarrow }{R^n} \ \text {and}\nonumber \\&\psi : \ (R^m)^{r+1}{\rightarrow }{R^m} \end{aligned}$$
(19)

such that

$$\begin{aligned} y= & {} h\left( x,u,\dot{u},\ldots ,u^{(r)}\right) , \nonumber \\ x= & {} \phi \left( y,\dot{y},\ldots ,y^{(r-1)}\right) , \ \text {and} \nonumber \\ u= & {} \psi \left( y,\dot{y},\ldots ,y^{(r-1)},y^{(r)}\right) . \end{aligned}$$
(20)

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{aligned} x(t)= & {} \phi \left( y(t),\dot{y}(t),\ldots ,y^{(r)}(t)\right) , \text {and} \nonumber \\ u(t)= & {} \psi \left( y(t),\dot{y}(t),\ldots ,y^{(r+1)}(t)\right) \end{aligned}$$
(21)

Classes of Differentially Flat Systems

For certain classes of dynamical systems it has been proven that they satisfy differential flatness properties. The following classes of nonlinear differentially flat systems are defined [26]:

  1. 1.

    Affine in-the-input systems: The dynamics of such systems is given by:

    $$\begin{aligned} \dot{x}=f(x)+{\sum \limits _{i=1}^m}{g_i(x)}{u_i} \end{aligned}$$
    (22)

    From Eq. (22) it can be seen that the above state equation can also describe MIMO dynamical systems. Without out loss of generality it is assumed that \(G=[g_1,\ldots ,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. 2.

    Driftless systems: These are systems of the form

    $$\begin{aligned} \dot{x}={\sum \limits _{i=1}^m}{f_i(x)}u_i \end{aligned}$$
    (23)

For driftless systems with two inputs, i.e.

$$\begin{aligned} \dot{x}={f_1(x)}{u_1}+{f_2(x)}{u_2} \end{aligned}$$
(24)

the flatness property holds, if and only if the rank of matrix \(E_{k+1}:=\{E_k,[E_k,E_k]\}, \ k{\ge }0\) with \(E_0:=\{f_1,f_2\}\) is equal to \(k+2\) for \(k=0,\ldots ,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.

$$\begin{aligned} \dot{x}={\sum \limits _{i=1}^{n-2}}{u_i}{f_i}(x) \ x{\in }R^n \end{aligned}$$
(25)

the flatness property holds, if controllability also holds. Furthermore, the system is 0-flat if n is even.

Conditions for Applying the Differential Flatness Theory to MIMO Systems

Application of the differential flatness theory to multi-input multi-output systems is of particular importance for the AGVs because the latter stand also for MIMO systems. In order to demonstrate that a MIMO system satisfies the differential flatness properties, the flat outputs of the 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

$$\begin{aligned} \dot{x}=f(x,u) \end{aligned}$$
(26)

Such a system can be transformed to the form of an affine in the input system by adding an integrator to each input [27]

$$\begin{aligned} \dot{x}=f(x)+{\sum \limits _{i=1}^m}g_i(x){u_i} \end{aligned}$$
(27)

The following definitions are used [28–30]:

  1. (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}h}f \ \ \text {for} \ i=1,2,\ldots \).

  2. (ii)

    Lie Bracket: \({ad_f^i}g\) stands for a Lie Bracket which is defined recursively as \({ad_f^i}g=[f,{ad_f^{i-1}}g]\) with \({ad_f^0}g=g\) and \({ad_f}g=[f,g]={{\nabla }g}f-{{\nabla }f}g\).

If the system of Eq. (27) 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{aligned} \dot{z}_{i,j}= & {} z_{i+1,j} \quad \text {for} \ 1{\le }j{\le }m \; \text {and} \; 1{\le }i{\le }v_j-1\nonumber \\ \dot{z}_{v_{i,j}}= & {} {v_j} \end{aligned}$$
(28)

with \({\sum \nolimits _{j=1}^m}{v_j}=n\), then \(y_j=z_{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. (27) into the canonical form described in Eq. (28) the following theorem holds [27]

Theorem

For the nonlinear systems described by Eq. (27) the following variables are defined:

  1. (i)

    \(G_0=\text {span}[g_1,\ldots ,g_m]\).

  2. (ii)

    \(G_1=\text {span}[g_1,\ldots ,g_m,ad_f{g_1},\ldots ,ad_f{g_m}]\).

\(\cdots \)

(k) \(G_k=\text {span}\{{ad_f^j}{g_i} \ \text {for} \ 0{\le }j{\le }k, \ 1{\le }i{\le }m \}\). Then, the linearization problem for the system of Eq. (27) can be solved if and only if

  1. (1)

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

  2. (2)

    The dimension of \(G_{n-1}\) if of order n.

  3. (3)

    The distribution \(G_k\) is involutive for each \(1{\le }k{\le }{n-2}\).

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{aligned}&\dot{x}_1=x_2\nonumber \\&\dot{x}_2=x_3 \nonumber \\&\cdots \nonumber \\&\dot{x}_{r_1-1}=x_{r_1} \nonumber \\&\dot{x}_{r_1}=f_1(x)+{\sum \limits _{j=1}^p}{g_{1_j}(x)}u_j+d_1\nonumber \\&\dot{x}_{r_1+1}=x_{r_1+2} \nonumber \\&\dot{x}_{r_1+2}=x_{r_1+3} \nonumber \\&\cdots \\&\dot{x}_{p-1}=x_{p} \nonumber \\&\dot{x}_{p}=f_p(x)+{\sum \limits _{j=1}^p}{g_{p_j}(x)}u_j+d_p\nonumber \\&y_1=x_1 \nonumber \\&y_2=x_2 \nonumber \\&\cdots \nonumber \\&y_p=x_{n-r_p+1}\nonumber \end{aligned}$$
(29)

where \(x=[x_1,\ldots ,x_n]^T\) is the state vector of the transformed system (according to the differential flatness formulation), \(u=[u_1,\ldots ,u_p]^T\) is the set of control inputs, \(y=[y_1,\ldots ,y_p]^T\) is the output vector, \(f_i\) are the drift functions and \(g_{i,j}, \ i,j=1,2,\ldots ,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

$$\begin{aligned} y_i^{(r_i)}=f_i(x)+{\sum \limits _{j=1}^p}g_{ij}(x)u_j+d_j \end{aligned}$$
(30)

Next the following vectors and matrices can be defined

$$\begin{aligned} \begin{aligned} f(x)&=[f_1(x),\ldots ,f_n(x)]^T\\ g(x)&=[g_1(x),\ldots ,g_n(x)]^T \quad \\&\text {with} \ g_i(x)=[g_{1i}(x),\ldots ,g_{pi}(x)]^T\\ A&=diag[A_1,\ldots ,A_p], \ \ B=diag[B_1,\ldots ,B_p] \\ C^T&=diag[C_1,\ldots ,C_p], \ \ d=[d_1,\ldots ,d_p]^T \end{aligned} \end{aligned}$$
(31)

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

$$\begin{aligned} A_i=\begin{pmatrix} 0 &{} 1 &{} \cdots &{} 0 \\ 0 &{} 0 &{} \cdots &{} 0 \\ \vdots &{} \vdots &{} \cdots &{} \vdots \\ 0 &{} 0 &{} \cdots &{} 1 \\ 0 &{} 0 &{} \cdots &{} 0 \end{pmatrix}_{{r_i}\times {r_i}} B_i=\begin{pmatrix} 0 \\ 0 \\ \cdots \\ 0 \\ 1 \end{pmatrix}_{{r_i}{\times }1} C_i=\begin{pmatrix} 1\\ 0\\ \cdots \\ 0\\ 0 \end{pmatrix}_{1{\times }{r_i}} \end{aligned}$$
(32)

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

$$\begin{aligned} \dot{x}= & {} Ax+B[f(x)+g(x)u+\tilde{d}] \nonumber \\ y= & {} {C^T}x \end{aligned}$$
(33)

which can be also written in the equivalent form:

$$\begin{aligned} \dot{x}= & {} Ax+Bv+B\tilde{d}\nonumber \\ y= & {} {C^T}x \end{aligned}$$
(34)

where the transformed control input is defined as \(v=f(x)+g(x)u\). By demonstrating differential flatness for the vehicle’s model it is anticipated to express its dynamics in the canonical form defined by Eq. (32) to Eq. (33).

Flatness-Based Controller for the 4-Wheel Vehicle

To show that the four-wheel vehicle is differentially flat the following flat outputs are defined [1, 2]:

$$\begin{aligned} y_1= & {} V_x \nonumber \\ y_2= & {} {L_f}m{V_y}-{I_z}\dot{\psi } \end{aligned}$$
(35)

Then it holds that all elements of the system’s state vector can be written as functions of the flat outputs and their derivatives. Indeed, for \(x=[{V_x}, \ {V_y}, \ \dot{\psi }]^T\) it holds

$$\begin{aligned} V_x= & {} y_1 \end{aligned}$$
(36)
$$\begin{aligned} V_y= & {} {{y_2} \over {{L_f}m}}-\left( {{I_z} \over {{L_f}m}}\right) \nonumber \\&\times \left( {{{L_f}m{y_1}{\dot{y}_2}+{C_r}(L_f+L_r){y_2}} \over {C_r(L_f+L_r)}(I_z-{L_f}{L_r}m)+({L_f}m{y_1})^2}\right) \end{aligned}$$
(37)
$$\begin{aligned} \dot{\psi }= & {} {{{L_f}m{y_1}{\dot{y}_2}+{C_r}(L_f+L_r){y_2}} \over {C_r(L_f+L_r)}(I_z-{L_f}{L_r}m)+({L_f}m{y_1})^2} \end{aligned}$$
(38)

Expressing the system’s state variables as functions of the flat outputs one has the following state-space description for the system

$$\begin{aligned} \begin{pmatrix} \dot{y}_1 \\ \ddot{y}_2 \end{pmatrix}=\Delta (y_1,y_2,\dot{y}_2) \begin{pmatrix} u_1 \\ u_2 \end{pmatrix}+ \Phi (y_1,y_2,\dot{y}_2) \end{aligned}$$
(39)

where

$$\begin{aligned} \Delta (y_1,y_2,\dot{y}_2)= \begin{pmatrix} \Delta _{11}(y_1,y_2,\dot{y}_2) &{}\quad \Delta _{12}(y_1,y_2,\dot{y}_2) \\ \Delta _{21}(y_1,y_2,\dot{y}_2) &{} \quad \Delta _{22}(y_1,y_2,\dot{y}_2) \end{pmatrix} \end{aligned}$$
(40)

with

$$\begin{aligned}&\Delta _{11}(y_1,y_2,\dot{y}_2)={1 \over {mR}} \end{aligned}$$
(41)
$$\begin{aligned}&\Delta _{12}(y_1,y_2,\dot{y}_2)={{C_f \over m}}\left( {{{V_y}+{L_f}\dot{\psi }} \over {y_1}}\right) \end{aligned}$$
(42)
$$\begin{aligned}&\Delta _{21}(y_1,y_2,\dot{y}_2)\nonumber \\&\quad ={{{C_r}(L_f+L_r)(V_y-{L_r\dot{\psi }})-{L_f}m\dot{\psi }{y_1^2}} \over {mR{y_1^2}}} \end{aligned}$$
(43)
$$\begin{aligned}&\Delta _{22}(y_1,y_2,\dot{y}_2)\nonumber \\&\quad =\left( -{L_f}m{y_1}+{{{L_r}{C_r}(L_f+L_r)} \over {y_1}}\right) {({{L_f}{C_f}R-{L_f}{I_r}\dot{\omega }_f)} \over {{I_z}R}}\nonumber \\&\qquad +\,{{({{(C_r(L_f+L_r))(V_y-L_r\dot{\psi })-{L_f}m\dot{\psi }{y_1^2}})} \over {y_1^2}}}\\&\qquad \times {{{C_f}(V_y+{L_f}\dot{\psi }})\over {m{y_1}}}\nonumber \\&\qquad -\,{ {C_r({L_f+L_r})}\over y_1}{{R{C_f}-{I_r}\dot{\omega }_f} \over {mR}}\nonumber \end{aligned}$$
(44)

Moreover about matrix \(\Phi (y_1,y_2,\dot{y}_2)\) it holds

$$\begin{aligned} \Phi (y_1,y_2,\dot{y}_2)= \begin{pmatrix} \Phi _1(y_1,y_2,\dot{y}_2) \\ \Phi _2(y_1,y_2,\dot{y}_2) \end{pmatrix} \end{aligned}$$
(45)

with elements

$$\begin{aligned}&\Phi _1(y_1,y_2,\dot{y}_2)=\dot{\psi }{V_y}-{{I_r} \over {mR}}(\dot{\omega }_r+\dot{\omega }_f) \end{aligned}$$
(46)
$$\begin{aligned}&\Phi _2(y_1,y_2,\dot{y}_2)\nonumber \\&\quad =-{L_f}m{y_1}{f_3}(x,t)-{{{C_r}(L_f+L_r)} \over {y_1}}f_2(x,t)+\nonumber \\&\qquad +{ {{C_f}(L_f+L_r)(V_y-L_r\dot{\psi })-{L_f}m\dot{\psi }y_1^2} \over {y_1^2}}f_1(x,t)\nonumber \\ \\&\qquad +{{{L_r}{C_r}(L_f+L_r)} \over {y_1}}f_3(x,t)\nonumber \end{aligned}$$
(47)

According to the above the system’s control input can be also written as a function of the flat output and its derivatives. Thus one has

$$\begin{aligned} \begin{pmatrix} \dot{y}_1 \\ \ddot{y}_2 \end{pmatrix}=\Delta (y_1,y_2,\dot{y}_2) \begin{pmatrix} u_1 \\ u_2 \end{pmatrix}+ \Phi (y_1,y_2,\dot{y}_2) \end{aligned}$$
(48)

i.e.

$$\begin{aligned} \begin{pmatrix} u_1 \\ u_2 \end{pmatrix}={\Delta ^{-1}}(y_1,y_2,\dot{y}_2)^{-1} \Bigg (\begin{pmatrix} \dot{y}_1 \\ \dot{y}_2 \end{pmatrix}-\Phi (y_1,y_2,\dot{y}_2) \Bigg ) \end{aligned}$$
(49)

which means that provided that matrix \(\Delta (y_1,y_2,\dot{y}_2)\) is invertible, the control input \(u=[u_1,u_2]^T\) can be written as a function of the flat output and its derivatives. The non-singularity of matrix \(\Delta (y_1,y_2,\dot{y}_2)\) depends on the determinant

$$\begin{aligned} \begin{aligned}&det(\Delta (y_1,y_2,\dot{y}_2))\\&\quad ={{({I_r}{\dot{\omega }_f}-{C_f}R)({L_f^2}{y_1^2}{m^2}-C_r(L_f+L_r){L_r}{L_f}m+{C_r}{I_z}L_r)} \over {{I_z}{R^2}{y_1}{m^2}}} \end{aligned} \end{aligned}$$
(50)

This determinant has non-zero values because it holds:

  1. (i)

    \(({I_r}{\dot{\omega }_f}-{C_f}R){\ne }0\) since for the wheels rotational acceleration one has \({\dot{\omega }_f}<{{C_f}R \over {I_r}}\), and also

  2. (ii)

    \(({L_f^2}{y_1^2}{m^2}-C_r(L_f+L_r){L_r}{L_f}m+{C_r}{I_z}L_r){\ne }0\) when \({I_z}>{L_f}m\).

The differentially flat model of the vehicle can be also written in a canonical form after defining the new control input vector

$$\begin{aligned} \begin{pmatrix} v_1 \\ v_2 \end{pmatrix}=\Delta (y_1,y_2,\dot{y}_2) \begin{pmatrix} u_1 \\ u_2 \end{pmatrix}+\Phi (y_1,y_2,\dot{y}_2) \end{aligned}$$
(51)

thus one obtains a MIMO system description into canonical form, i.e.

$$\begin{aligned} \begin{pmatrix} \dot{y}_1 \\ \dot{y}_2 \\ \ddot{y}_2 \end{pmatrix}= \begin{pmatrix} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 1 \\ 0 &{} 0 &{} 0 \end{pmatrix} \begin{pmatrix} y_1 \\ y_2 \\ \dot{y}_2 \end{pmatrix}+ \begin{pmatrix} 1 &{} 0 \\ 0 &{} 0 \\ 0 &{} 1 \end{pmatrix} \begin{pmatrix} v_1 \\ v_2 \end{pmatrix} \end{aligned}$$
(52)

Once the vehicle’s model is written in the differentially flat form the controller that enables tracking of a desirable trajectory defined by \(y_1^{ref},y_2^{ref},\dot{y}_2^{ref}\) is given by

$$\begin{aligned} v_1= & {} \dot{y}_1^{ref}-k_{p_1}\left( y_1-y_1^{ref}\right) \nonumber \\ v_2= & {} \ddot{y}_2^{ref}-k_{d_2}\left( \dot{y}_2-\dot{y}_2^{ref}\right) -k_{p_2}\left( y_2-y_2^{ref}\right) \end{aligned}$$
(53)

and defining the error variables \(e_1=y_1-y_1^{ref}\) and \(e_2=y_2-y_2^{ref}\) one has the following tracking error dynamics for the closed-loop system

$$\begin{aligned}&\dot{e}_1+k_{p_1}{e_1}=0\nonumber \\&\ddot{e}_2+k_{d_2}{\dot{e}_2}+k_{p_2}{e_2}=0 \end{aligned}$$
(54)

Therefore, the suitable selection of gains \(k_{p_1>0}\) and \(k_{p_2}>0,k_{d_2}>0\) assures the asymptotic elimination of the tracking errors, i.e. \(lim_{t{\rightarrow }\infty }e_1(t)=0\) and \(lim_{t{\rightarrow }\infty }e_2(t)=0\).

The control input that is finally applied for the vehicle’s steering is given by

$$\begin{aligned} \begin{pmatrix} u_1 \\ u_2 \end{pmatrix}={\Delta }(y_1,y_2,\dot{y}_2)^{-1} \Bigg (\begin{pmatrix} v_1 \\ v_2 \end{pmatrix}-\Phi (y_1,y_2,\dot{y}_2) \Bigg ) \end{aligned}$$
(55)

or equivalently

$$\begin{aligned}&\begin{pmatrix} u_1 \\ u_2 \end{pmatrix}={\Delta }(y_1,y_2,\dot{y}_2)^{-1}\nonumber \\&\quad \times \left[ \begin{pmatrix} \dot{y}_1^{ref}-k_{p_1}(y_1-y_1^{ref}) \\ \ddot{y}_2^{ref}-k_{d_2}(\dot{y}_2-\dot{y}_2^{ref})-k_{p_2}(y_2-y_2^{ref}) \end{pmatrix}-\Phi (y_1,y_2,\dot{y}_2) \right] \nonumber \\ \end{aligned}$$
(56)

The transformation of the vehicle’s model into a canonical form, through the application of the differential flatness theory, facilitates not only the design of a feedback controller for trajectory tracking but also the design of filters for the estimation of the state vector of the vehicle out of a limited number of sensor measurements.

State Estimation with the Kalman Filter

The Continuous-Time Kalman Filter for the Linear State Estimation Model

In the dynamic model of the 4-wheel vehicle described in Eq. (52) it is assumed that the measurable elements of the state vector are \(y_1\) and \(y_2\), which in turn can be obtained through measurements of the vehicle’s velocities \(V_x\), \(V_y\) and \(\dot{\psi }\). Therefore, to implement the state feedback control of Eq. (56) it is necessary to estimate the non-measurable state vector elements through some filtering/estimation procedure. Moreover, the filtering/estimation will be useful for identifying unknown forces and torques exerted on the vehicle.

In the continuous-time representation of the system’s dynamics, the continuous-time Kalman Filter stands for a state estimator of optimal accuracy. The following continuous-time dynamical system is assumed:

$$\begin{aligned} {\left\{ \begin{array}{ll} \dot{x}(t)=Ax(t)+Bu(t)+w(t), \quad t{\ge }{t_0} \\ z(t)=Cx(t)+v(t), \quad t{\ge }{t_0} \end{array}\right. } \end{aligned}$$
(57)

where again \(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 again a linear state observer which is given by

$$\begin{aligned} {\left\{ \begin{array}{ll} \dot{{\hat{x}}}=A{{\hat{x}}}+Bu+K[z-C{{\hat{x}}}], \quad {\hat{x}}(t_0)=0 \\ K(t)=P{C^T}R^{-1}\\ \dot{P}=AP+PA^T+Q-P{C^T}{R^{-1}}CP \end{array}\right. } \end{aligned}$$
(58)

where \({\hat{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\). It can be seen that as in the case of the Luenberger observer, the Kalman Filter consists of the system’s state equation plus a corrective term \(K[z-C{\hat{x}}]\). The associated Riccati equation for calculating the covariance matrix P(t) has the form given in the last row of Eq. (58).

The Discrete-Time Kalman Filter for the Linear State Estimation Model

In the discrete-time case the dynamical system is assumed to be expressed in the form of a discrete-time state model:

$$\begin{aligned} {\left\{ \begin{array}{ll} x(k+1)={A(k)}x(k)+B(k)u(k)+w(k) \\ z(k)=Cx(k)+v(k) \end{array}\right. } \end{aligned}$$
(59)

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 measurements \(z(1),z(2), \ldots ,z(k)\). The initial value of the state vector x(0), the initial value of the error covariance matrix P(0) is unknown and an estimation of it is considered, i.e. \({\hat{x}}(0)=\) a guess of E[x(0)] and \(\hat{P}(0)\)=a guess of Cov[x(0)].

For the initialization of matrix P one can set \(\hat{P}(0)={\lambda }I\), with \(\lambda >0\). The state vector x(k) has to be estimated taking into account \({\hat{x}}(0)\), \(\hat{P}(0)\) and the output measurements \(Z=[z(1),z(2),\ldots ,z(k)]^T\), i.e. there is a function relationship:

$$\begin{aligned} {\hat{x}}(k)={\alpha _n}({\hat{x}}(0),\hat{P}(0),Z(k)) \end{aligned}$$
(60)

Actually, this is a linear minimum mean squares estimation problem (LMMSE) which is solved recursively, through the function relationship

$$\begin{aligned} {\hat{x}}(k + 1)=a_{n+1}({\hat{x}}(k), z(k + 1)) \end{aligned}$$
(61)

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, and ii) measurement update. 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), \ldots ,z(k-1)\}\) is available. From \(Z^{-}\) an a priori estimation of x(k) is obtained which is denoted by \({\hat{x}}^{-}(k)=\) the estimate of x(k) given \(Z^{-}\).

  • When z(k) becomes available, the set of the output measurements becomes \(Z=\{z(1),\ldots ,z(k)\}\), where \({\hat{x}}(k)=\) the estimate of x(k) given Z.

The associated estimation errors are defined by

$$\begin{aligned} e^{-}(k)= & {} x(k)-{\hat{x}}^{-}(k)=\text {the a priori error}\nonumber \\ e(k)= & {} x(k)-{\hat{x}}(k)= \text {the a posteriori error} \end{aligned}$$
(62)

The estimation error covariance matrices associated with \({\hat{x}}(k)\) and \({\hat{x}}^{-}(k)\) are defined as [31]

$$\begin{aligned} P^{-}(k)= & {} Cov[e^{-}(k)]=E[e^{-}(k){e^{-}(k)^T}]\\ P(k)= & {} Cov[e(k)]=E[e(k)e^{T}(k)] \end{aligned}$$

From the definition of the trace of a matrix, the mean square error of the estimates can be written as

$$\begin{aligned} MSE({\hat{x}}^{-}(k))= & {} E[{e^{-}(k)}{e^{-}(k)^T}]=tr(P^{-}(k)) \\ MSE(x(k))= & {} E[e(k){e^T}(k)]=tr(P(k)) \end{aligned}$$

Finally, the linear Kalman filter equations in cartesian coordinates are measurement update:

$$\begin{aligned} K(k)= & {} {P^{-}}(k){C^T}[C{\cdot }{P^{-}(k)}{C^T}+R]^{-1}\nonumber \\ {\hat{x}}(k)= & {} {{\hat{x}}^{-}}(k)+K(k)[z(k)-C{{\hat{x}}^{-}}(k)]\\ P(k)= & {} P^{-}(k)-K(k)CP^{-}(k)\nonumber \end{aligned}$$
(63)

time update:

$$\begin{aligned} P^{-}(k + 1)= & {} A(k)P(k){A^T}(k)+Q(k)\nonumber \\ {\hat{x}}^{-}(k+1)= & {} {A}(k){\hat{x}}(k)+B(k)u(k) \end{aligned}$$
(64)

The schematic diagram of the KF loop is given in Fig. 3.

Fig. 3
figure 3

Schematic diagram of the Kalman filter loop

Full size image

The Extended Kalman Filter

State estimation can be also performed for nonlinear dynamical systems using the Extended Kalman Filter (EKF) recursion. The following nonlinear model is considered [23, 32]:

$$\begin{aligned} x(k+1)= & {} \phi (x(k))+L(k)u(k)+w(k)\nonumber \\ z(k)= & {} \gamma (x(k))+v(k) \end{aligned}$$
(65)

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

$$\begin{aligned} \phi (x)= & {} [{\phi _1}(x),{\phi _2}(x),\ldots ,{\phi _m}(x)]^T, \quad \text {and}\nonumber \\ \gamma (x)= & {} [{\gamma _1}(x),{\gamma _2}(x),\ldots ,{\gamma _p}(x)]^T, \end{aligned}$$
(66)

respectively. It is assumed that \(\phi \) and \(\gamma \) are sufficiently smooth in x so that each one has a valid series Taylor expansion. Following a linearization procedure, \(\phi \) is expanded into Taylor series about \({\hat{x}}\):

$$\begin{aligned} \phi (x(k))=\phi ({\hat{x}}(k))+J_{\phi }({\hat{x}}(k))[x(k)-{\hat{x}}(k)]+\cdots \end{aligned}$$
(67)

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

$$\begin{aligned} J_{\phi }(x)={\partial {\phi } \over \partial {x}}{\vert _{x=\hat{x}(k)}}= \begin{pmatrix} {\partial {\phi _1} \over \partial {x_1}} &{} {\partial {\phi _1} \over \partial {x_2}} &{} \cdots &{} {\partial {\phi _1} \over \partial {x_m}} \\ {\partial {\phi _2} \over \partial {x_1}} &{} {\partial {\phi _2} \over \partial {x_2}} &{} \cdots &{} {\partial {\phi _2} \over \partial {x_m}} \\ \vdots &{} \vdots &{} \vdots &{} \vdots \\ {\partial {\phi _m} \over \partial {x_1}} &{} {\partial {\phi _m} \over \partial {x_2}} &{} \cdots &{} {\partial {\phi _m} \over \partial {x_m}} \end{pmatrix} \end{aligned}$$
(68)

Likewise, \(\gamma \) is expanded about \({\hat{x}}^{-}(k)\)

$$\begin{aligned} \gamma (x(k))=\gamma ({\hat{x}}^{-}(k))+J_{\gamma }[x(k)-{\hat{x}}^{-}(k)]+\cdots \end{aligned}$$
(69)

where \({\hat{x}}^{-}(k)\) is the estimation of the state vector x(k) before measurement at the k-th instant to be received and \({{\hat{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

$$\begin{aligned} J_{\gamma }(x)={\partial {\gamma } \over \partial {x}} {\vert _{x={\hat{x}}^{-}(k)}} = \begin{pmatrix} {\partial {\gamma _1} \over \partial {x_1}} &{} {\partial {\gamma _1} \over \partial {x_2}} &{} \cdots &{} {\partial {\gamma _1} \over \partial {x_m}} \\ {\partial {\gamma _2} \over \partial {x_1}} &{} {\partial {\gamma _2} \over \partial {x_2}} &{} \cdots &{} {\partial {\gamma _2} \over \partial {x_m}} \\ \vdots &{} \vdots &{} \vdots &{} \vdots \\ {\partial {\gamma _p} \over \partial {x_1}} &{} {\partial {\gamma _p} \over \partial {x_2}} &{} \cdots &{} {\partial {\gamma _p} \over \partial {x_m}} \end{pmatrix} \end{aligned}$$
(70)

The resulting expressions create first order approximations of \(\phi \) and \(\gamma \). Thus the linearized version of the system is obtained:

$$\begin{aligned}&x(k+1)=\phi ({\hat{x}}(k))+J_{\phi }({\hat{x}}(k))[x(k)-{\hat{x}}(k)]+w(k) \nonumber \\&z(k)={\gamma }({\hat{x}}^{-}(k))+J_{\gamma }({\hat{x}}^{-}(k))[x(k)-{\hat{x}}^{-}(k)]+v(k)\nonumber \\ \end{aligned}$$
(71)

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

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

    $$\begin{aligned} K(k)= & {} P^{-}(k)J^{T}_{\gamma }({\hat{x}}^{-}(k)){\cdot }[J_{\gamma }({\hat{x}}^{-}(k))\nonumber \\&P^{-}(k)J^T_{\gamma }({\hat{x}}^{-}(k))+R(k)]^{-1}\nonumber \\ {\hat{x}}(k)= & {} {\hat{x}}^{-}(k)+K(k)[z(k)-{\gamma }({\hat{x}}^{-}(k))]\nonumber \\ P(k)= & {} P^{-}(k)-K(k)J_{\gamma }({\hat{x}}^{-}(k)){P^{-}(k)} \end{aligned}$$
    (72)

  • Time update. Compute:

    $$\begin{aligned} P^{-}(k+1)= & {} J_{\phi }({\hat{x}}(k))P(k){J_{\phi }^T({\hat{x}}(k))}+Q(k)\nonumber \\ {\hat{x}}^{-}(k+1)= & {} {\phi }({\hat{x}}(k))+L(k)u(k) \end{aligned}$$
    (73)

State Estimation Under Model Uncertainties and External Disturbances

Unknown Input Observers

To account for model uncertainties and external disturbances, observer-based estimation has been proposed, enabling to solve the problem of model accuracy in reverse [33–37]. This is done by modeling the mechatronic or robotic system with an equivalent input disturbance that includes unmodeled dynamics. An observer is then designed to estimate the disturbance in real time and provide feedback to cancel it.

The Unknown Input observer is applied to dynamical systems of the form

$$\begin{aligned} \dot{x}= & {} Ax+B(u+w_e)\nonumber \\ z= & {} Cx \end{aligned}$$
(74)

while the disturbance dynamics is given by

$$\begin{aligned}&\dot{d}={A_f}d \nonumber \\&{w_e}={C_f}d \end{aligned}$$
(75)

Then, the unknown input observer provides a state estimate of the extended state vector

$$\begin{aligned} \begin{pmatrix} \dot{{\hat{x}}}\\ \dot{\hat{d}} \end{pmatrix}= \begin{pmatrix} A &{} B{C_f} \\ 0 &{} A_f \end{pmatrix} \begin{pmatrix} {\hat{x}} \\ \hat{d} \end{pmatrix}+ \begin{pmatrix} B \\ 0 \end{pmatrix}u+ K(z-C{\hat{x}}) \end{aligned}$$
(76)

In the generic case one can assume that the disturbances vector \(w_e\) varies dynamically in time. However, in several cases it suffices to assume a constant or piecewise constant disturbance \(\dot{w}_e(z)=0\) where \(A_f=0\) and \(C_f=1\). The observer’s gain can be obtained through the standard Kalman Filter recursion.

Perturbation Observer

The perturbation observer is an extension of the unknown inputs observer which takes into account not only external disturbances but also parametric uncertainties. In the discrete-time form, the system dynamics is given by

$$\begin{aligned} x_{k+1}= & {} A{x_k}+B{u_k}+w_f \nonumber \\ z= & {} C{x_k} \end{aligned}$$
(77)

while the disturbance dynamics is given by

$$\begin{aligned}&d_k={A_f}d_{k-1}+{B_f}(B^{+}({\hat{x}}_k-A{\hat{x}}_{k-1})-u_{k-1})\nonumber \\&{\hat{w}}_{f_k}={C_f}{d_k}\\&{\hat{x}}_{k+1}=A{{\hat{x}}_k}+B(u_k+{\hat{w}}_{f_k})+L(z_k-C{\hat{x}}_k)\nonumber \end{aligned}$$
(78)

where \(B^{+}\) is the Moore-Penrose pseudo-inverse of matrix B. The unknown input can represent traditional external disturbances and model uncertainties, i.e. \(w_f=w_e+{\Delta }A{x_k}+{\Delta }B{u_k}\).

Extended State Observer

The Extended State Observer uses a canonical form so the unmodelled dynamics appear at the disturbance estimation part. The system’s description in the canonical form is given by

$$\begin{aligned}&x_1^{(n)}=f(x,t,u,w_f)+{b_m}u\nonumber \\&z=x_1 \\&x=\begin{pmatrix} x_1&\dot{x}_1&\cdots&x_1^{(n-1)} \end{pmatrix}^T\nonumber \end{aligned}$$
(79)
$$\begin{aligned}&\begin{pmatrix} \dot{{\hat{x}}}_1 \\ \cdots \\ \dot{{\hat{x}}}_{n-1} \\ \dot{{\hat{x}}}_n \\ \dot{\hat{f}} \end{pmatrix}= \begin{pmatrix} {\hat{x}}_2 \\ \cdots \\ {\hat{x}}_{n} \\ \hat{f}+{b_m}u \\ 0 \end{pmatrix}+ K(x_1-{\hat{x}}_1) \end{aligned}$$
(80)

The Extended State Observer can be also modified to take into account derivatives of the disturbance

$$\begin{aligned} \begin{aligned}&x_1^{(n)}=f(x,t,u,w_f)+{b_m}u\\&z=x_1 \\&x=\begin{pmatrix}&x_1&\dot{x}_1&\cdots&x_1^{(n-1)} \end{pmatrix}^T\\&F=\begin{pmatrix} f&\dot{f}&\cdots&f^{(h-1)} \end{pmatrix}^T \end{aligned} \end{aligned}$$
(81)

and now the state and disturbance observer takes the form

$$\begin{aligned} \begin{pmatrix} \dot{{\hat{x}}}_1 \\ \cdots \\ \dot{{\hat{x}}}_{n-1} \\ \dot{{\hat{x}}}_n \\ \dot{\hat{F}}_1 \\ \cdots \\ \dot{\hat{F}}_{h-1} \\ \dot{\hat{F}}_{h} \end{pmatrix}= \begin{pmatrix} {\hat{x}}_2 \\ \cdots \\ {\hat{x}}_{n} \\ \hat{f}+{b_m}u \\ \hat{F}_2 \\ \cdots \\ \hat{F}_h \\ 0 \end{pmatrix}^T+ K(x_1-{\hat{x}}_1) \end{aligned}$$
(82)

The latter form of the Extended State Observer described in Eq. (82) enables to track various types of disturbances. For example, \(h=1\) allows estimation of disturbance dynamics defined by its first order derivative, and \(h=2\) allows estimation of disturbance dynamics defined by its second order derivative.

Estimation of Vehicle Disturbance Forces with Kalman Filtering

State Estimation with the Derivative-Free Nonlinear Kalman Filter

It was shown that the initial nonlinear model of the vehicle can be written in the MIMO canonical form

$$\begin{aligned} \begin{pmatrix} \dot{y}_1 \\ \dot{y}_2 \\ \ddot{y}_2 \end{pmatrix}= \begin{pmatrix} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 1 \\ 0 &{} 0 &{} 0 \end{pmatrix} \begin{pmatrix} y_1 \\ y_2 \\ \dot{y}_2 \end{pmatrix}+ \begin{pmatrix} 1 &{} 0 \\ 0 &{} 0 \\ 0 &{} 1 \end{pmatrix} \begin{pmatrix} v_1 \\ v_2 \end{pmatrix} \end{aligned}$$
(83)

Thus one has a MIMO linear model of the form

$$\begin{aligned} \dot{y}_f= & {} A_f{y_f}+{B_f}v\nonumber \\ z_f= & {} {C_f}y_f \end{aligned}$$
(84)

where \(y_f=[y_1,y_2,\dot{y}_2]^T\) and matrices \(A_f\),\(B_f\),\(C_f\) are in the MIMO canonical form

$$\begin{aligned} A_f=\begin{pmatrix} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 1 \\ 0 &{} 0 &{} 0 \end{pmatrix} B_f=\begin{pmatrix} 1 &{} 0 \\ 0 &{} 0 \\ 0 &{} 1 \end{pmatrix} C_f^T=\begin{pmatrix} 1 &{} 0 \\ 0 &{} 1 \\ 0 &{} 0 \end{pmatrix} \end{aligned}$$
(85)

where the measurable variables \(y_1=V_x\), \(y_2={L_f}m{V_y}-{I_z}{\dot{\psi }}\) are associated with the linear velocity of the vehicle \(V_x,V_y\) and with its angular velocity \(\dot{\psi }\). For the aforementioned model, and after carrying out discretization of matrices \(A_f\), \(B_f\) and \(C_f\) with common discretization methods one can perform linear Kalman filtering using Eq. (63) and Eq. (64). This is Derivative-free nonlinear Kalman filtering for the model of the vehicle which, unlike EKF, is performed without the need to compute Jacobian matrices and does not introduce numerical errors due to approximate linearization with Taylor series expansion.

Kalman Filter-Based Estimation of Disturbances

It is assumed that disturbance forces affect the nonlinear vehicle model along its longitudinal and transversal axis and that disturbance torques affect the nonlinear vehicle model on its z axis. For example disturbance forces be due to a force vector that coincides with the vehicle’s longitudinal axis (e.g. traction disturbance) or disturbance torques can be due to unmodeled lateral forces. These disturbance forces and torques change dynamically in time and their dynamics is given by

$$\begin{aligned} {\tilde{d}}_x= & {} f_{d_x}(V_x,V_y,\dot{\psi })\nonumber \\ {\tilde{d}}_y= & {} f_{d_y}(V_x,V_y,\dot{\psi })\\ {\tilde{d}}_\psi= & {} T_{d_\psi }(V_x,V_y,\dot{\psi })\nonumber \end{aligned}$$
(86)

Since the state variables of the vehicle’s dynamic model can be written as functions of the flat outputs \(y_1\) and \(y_2\) and of their derivatives it also holds

$$\begin{aligned} {\tilde{d}^{(i)}}_x= & {} f_{d_x}^{(i)}(y_1,y_2,\dot{y}_2)\nonumber \\ {\tilde{d}^{(i)}}_y= & {} f_{d_y}^{(i)}(y_1,y_2,\dot{y}_2)\\ {\tilde{d}^{(i)}}_\psi= & {} T_{d_\psi }^{(i)}(y_1,y_2,\dot{y}_2)\nonumber \end{aligned}$$
(87)

where \(i=1,2,\ldots \) stands for the i-th order derivative of the disturbance variable.

Considering the effect of disturbance functions on the initial nonlinear state equation of the vehicle and the linear relation between the initial state variables \([V_x,V_y]\) and the state variables of the flat system description \([y_1, y_2]\) one has the appearance of the disturbance terms in the canonical form model of Eq. (52)

$$\begin{aligned} \begin{pmatrix} \dot{y}_1 \\ \dot{y}_2 \\ \ddot{y}_2 \end{pmatrix}= \begin{pmatrix} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 1 \\ 0 &{} 0 &{} 0 \end{pmatrix} \begin{pmatrix} y_1 \\ y_2 \\ \dot{y}_2 \end{pmatrix}+ \begin{pmatrix} 1 &{} 0 \\ 0 &{} 0 \\ 0 &{} 1 \end{pmatrix} \begin{pmatrix} v_1 \\ v_2 \end{pmatrix}+ \begin{pmatrix} {1 \over m}\tilde{d}_x \\ 0 \\ {L_f}\dot{\tilde{d}}_y-\dot{\tilde{d}}_\psi \end{pmatrix} \end{aligned}$$
(88)

Next, the state vector of the model of Eq. (88) is extended to include as additional state variables the disturbance forces \(\tilde{d}_x\), \(\tilde{d}_y\) and \(\tilde{d}_{psi}\). Then, in the new state-space description one has \(z_1=y_1\), \(z_2=y_2\), \(z_3=\dot{y}_2\), \(z_4={\tilde{f}}_a={1 \over m}\tilde{d}_x\), \(z_5=\dot{{\tilde{f}}}_a\), \(z_6=\dot{{\tilde{f}}}_b={L_f}\dot{\tilde{d}}_y-\dot{\tilde{d}}_\psi \), \(z_7=\ddot{{\tilde{f}}}_b\), which takes the form of matrix equations

$$\begin{aligned} \dot{z}={\tilde{A}}{\cdot }z+{\tilde{B}}{\cdot }{\tilde{v}} \end{aligned}$$
(89)

where the control input is

$$\begin{aligned} \tilde{v}= & {} \begin{pmatrix} v_1&v_2&{1 \over m}\ddot{\tilde{d}}_x&{L_f}{\tilde{d}^{(3)}}_y-{\tilde{d}^{(3)}}_\psi \end{pmatrix}^T \ \text {or}\nonumber \\ {\tilde{v}}= & {} \begin{pmatrix} v_1&v_2&\ddot{{\tilde{f}}}_a&{{\tilde{f}}^{(3)}}_b\end{pmatrix}^T \end{aligned}$$
(90)

with

$$\begin{aligned} {\tilde{A}}=\begin{pmatrix} 0 &{} 0 &{} 0 &{} 1 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 1 &{} 0 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 1 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 &{} 1 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 1 \\ 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0 \end{pmatrix} {\tilde{B}}=\begin{pmatrix} 1 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 \\ 0 &{} 1 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 1 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 1 \end{pmatrix}&{\tilde{C}}^T=\begin{pmatrix} 1 &{} 0 \\ 0 &{} 1 \\ 0 &{} 0 \\ 0 &{} 0 \\ 0 &{} 0 \\ 0 &{} 0 \\ 0 &{} 0 \end{pmatrix}\nonumber \\ \end{aligned}$$
(91)

where the measurable state variables are \(z_1\) and \(z_2\). Since the dynamics of the disturbance terms \({\tilde{f}}_a\) and \({\tilde{f}}_b\) are taken to be unknown in the design of the associated disturbances’ estimator one has the following dynamics:

$$\begin{aligned} \dot{z}_o={\tilde{A}}_o{\cdot }z+{\tilde{B}}_o{\cdot }{{\tilde{v}}}+K({C_o}z-{C_o}\hat{z}) \end{aligned}$$
(92)

where \(K{\in }R^{7{\times }2}\) is the state estimator’s gain and

$$\begin{aligned} {\tilde{A}}_o=\begin{pmatrix} 0 &{} 0 &{} 0 &{} 1 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 1 &{} 0 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 1 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 &{} 1 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 1 \\ 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0 \end{pmatrix} {\tilde{B}}_o=\begin{pmatrix} 1 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 \\ 0 &{} 1 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 \end{pmatrix} {\tilde{C}}_o^T=\begin{pmatrix} 1 &{} 0 \\ 0 &{} 1 \\ 0 &{} 0 \\ 0 &{} 0 \\ 0 &{} 0 \\ 0 &{} 0 \\ 0 &{} 0 \end{pmatrix} \end{aligned}$$
(93)

Defining as \({\tilde{A}}_d\), \({\tilde{B}}_d\), and \({\tilde{C}}_d\), the discrete-time equivalents of matrices \({\tilde{A}}_o\), \({\tilde{B}}_o\) and \({\tilde{C}}_o\) respectively, a Derivative-free nonlinear Kalman Filter can be designed for the aforementioned representation of the system dynamics [24, 25]. The associated Kalman Filter-based disturbance estimator is given by

Fig. 4
figure 4

Control loop for the autonomous vehicle comprising a flatness-based nonlinear controller and a Kalman Filter-based disturbances estimator

Full size image

measurement update

$$\begin{aligned} K(k)= & {} {P^{-}(k)}{{\tilde{C}}_d^T}[{\tilde{C}}_d{\cdot }P^{-}(k){{\tilde{C}}_d^T}+R]^{-1}\nonumber \\ {{\hat{x}}(k)}= & {} {{\hat{x}}^{-}(k)}+K(k)[z(k)-{\tilde{C}}_d{{\hat{x}}^{-}(k)}]\\ P(k)= & {} P^{-}(k)-K(k){{\tilde{C}}_d}P^{-}(k)\nonumber \end{aligned}$$
(94)

time update

$$\begin{aligned} P^{-}(k+1)= & {} {{\tilde{A}}_d(k)}P(k){{\tilde{A}}_d^T}(k)+Q(k)\nonumber \\ {{\hat{x}}^{-}(k+1)}= & {} {\tilde{A}}_d(k){\hat{x}}(k)+{\tilde{B}}_d(k){\tilde{v}}(k) \end{aligned}$$
(95)

To compensate for the effects of the disturbance forces it suffices to use in the control loop the modified control input vector

$$\begin{aligned} v=\begin{pmatrix} v_1-\hat{{\tilde{f}}}_a \\ v_2-\hat{\dot{{\tilde{f}}}}_b \end{pmatrix} \ \text {or} \ v=\begin{pmatrix} v_1-\hat{z}_4\\ v_2-\hat{z}_6 \end{pmatrix} \end{aligned}$$
(96)

Simulation Tests

To evaluate for the performance of the proposed nonlinear control scheme, as well as about the performance of the Kalman Filter-based disturbances estimator simulation experiments have been carried out. Different velocity setpoints have been assumed (for velocity along the horizontal and vertical axis of the inertial reference frame, as well as for angular velocity round the vehicle’s z axis). Moroever, different disturbances forces and torques have been assumed to affect the vehicles’ dynamic model. Using the representation of the vehicle’s dynamics given in Eq. (88) two generalized disturbance forces/torques have been considered: the first denoted as \({\tilde{f}}_a\) was associated with state variable \(y_1\), while the second one denoted as \({\tilde{f}}_b\) was associated with the state variable \(y_2\). It was also assumed that the change in time of the generalized forces and torques was defined by the second derivative of the associated variable, i.e. \(\ddot{{\tilde{f}}}_a\) and \(\ddot{{\tilde{f}}}_b\). The disturbances dynamics was completely unknown to the controller and their identification was performed in real time by the disturbance estimator. The control loop used in the vehicle’s autonomous navigation is given in Fig. 4

The measurable variables used by the control and disturbances’ estimation scheme were the vehicle’s velocity \(V_x\) along the longitudinal axis, the vehicle’s velocity \(V_y\) along the lateral axis and the vehicle’s yaw rate \(\dot{\psi }\). The first two variables can be obtained with the use of onboard accelerometers while the third variable can be obtained with the use of a gyrocompass. The longitudinal axis of the vehicle is denoted as x-axis, while the lateral axis of the vehicle is denoted as y-axis. As it can be seen in Figs. 5, 6, 7, 8, 9, 10, 11, and 12 the proposed nonlinear controller succeeded accurate tracking of velocity setpoints. Moreover, the efficient estimation of disturbance forces and torques that was succeeded by the Kalman Filter-based disturbance estimator enabled their compensation through the inclusion of an additional control term in the loop.

Fig. 5
figure 5

Vehicle control under disturbances profile 1: a convergence of x-axis velocity \(V_x\) (blue line) to the desirable setpoint (red line), b convergence of the y-axis velocity \(V_y\) (blue line) to the desirable setpoint (red line)

Full size image
Fig. 6
figure 6

Vehicle control under disturbances profile 1: a convergence of yaw rate \(\dot{\psi }\) (blue line) to the desirable setpoint (red line), b estimation of the disturbance terms and of their rate of change (red line) and the associated real values (blue line)

Full size image
Fig. 7
figure 7

Vehicle control under disturbances profile 2: a convergence of x-axis velocity \(V_x\) (blue line) to the desirable setpoint (red line), b convergence of the y-axis velocity \(V_y\) (blue line) to the desirable setpoint (red line)

Full size image
Fig. 8
figure 8

Vehicle control under disturbances profile 2: a convergence of yaw rate \(\dot{\psi }\) (blue line) to the desirable setpoint (red line), b estimation of the disturbance terms and of their rate of change (red line) and the associated real values (blue line)

Full size image
Fig. 9
figure 9

Vehicle control under disturbances profile 3: a convergence of x-axis velocity \(V_x\) (blue line) to the desirable setpoint (red line), b convergence of the y-axis velocity \(V_y\) to the desirable setpoint (blue line) and the associated real values (red line)

Full size image
Fig. 10
figure 10

Vehicle control under disturbances profile 3: a convergence of yaw rate \(\dot{\psi }\) (blue line) to the desirable setpoint (red line) , b estimation of the disturbance terms and of their rate of change (red line) and the associated real values (blue line)

Full size image
Fig. 11
figure 11

Vehicle control under disturbances profile 4: a convergence of x-axis velocity \(V_x\) (blue line) to the desirable setpoint (red line) , b convergence of the y-axis velocity \(V_y\) (blue line) to the desirable setpoint (red line)

Full size image
Fig. 12
figure 12

Vehicle control under disturbances profile 4: a convergence of yaw rate \(\dot{\psi }\) (blue line) to the desirable setpoint (red line) , b Estimation of the disturbance terms and of their rate of change (red line) and the associated real values (blue line)

Full size image

Remark 1

In the AGV state estimation and control problem, random variables are the ones describing measurement noise. The Kalman Filter provides an optimal estimate about the state variables of the vehicle in the presence of Gaussian measurement noise in sensors. Besides, there is lack of knowledge about the mathematical description of disturbance forces and torques that affect the vehicle’s motion. These perturbation terms are not necessarily random but their mathematical description is unknown and time-varying. By redesigning the Derivative-free nonlinear Kalman Filter as a disturbance observer it becomes possible to estimate such disturbance inputs and to compensate for them.

Remark 2

By applying differential flatness theory and the associated change of state variables (diffeomorphisms) the state vector of the vehicle is transformed into the linear canonical form, as described by Eq. (83). For the latter description of the system, state estimation can be performed with the use of the standard Kalman Filter recursion. Besides, by exploiting the inverse transformation that is described in Eqs. (36)–(38) it becomes possible to obtain estimates of the state variables of the initial nonlinear model of the vehicle, which has been been described in Eq. (1)–(3). Unlike state estimation for nonlinear dynamical models with the use of the Extended Kalman Filter, in the proposed filtering approach (Derivative-free nonlinear Kalman Filter) there is no need to compute Jacobian matrices and the associated partial derivatives. A detailed oomparison between the Derivative-free nonlinear Kalman Filter and the Extended Kalman Filter can be found in [24] and [25]. The Derivative-free nonlinear Kalman Filter outperforms the Extended Kalman Filter in two aspects: (i) it is computationally faster because it does not require the online computation of Jacobian matrices, (ii) it provides more accurate state estimates because it avoids the cumulative linearization errors which appear in the case of the Extended Kalman Filter, and which are due to the truncation of higher order terms in the Taylor series expansion of the vehicle’s model.

Remark 3

The application of the disturbance estimation and compensation scheme that is described in the manuscript is not constrained to a specific type of perturbation inputs (forces and torques) that may distort the vehicle’s motion. Therefore, there is no limitation about the mathematical description of the perturbation inputs, apart from the requirement that they should be bounded. The disturbance terms may even exhibit discontinuities and jumps and despite such abrupt changes the Derivative-free nonlinear Kalman Filter can track them accurately. Finally, in case that the sensors’ measurement noise (accelerometers and gyrocompasses) are subjected to white Gaussian noise, the estimation that is provided by the Derivative-free nonlinear Kalman Filter, about both the state variables of the model and the perturbation inputs, is an optimal one.

Remark 4

Flatness-based control is a global linearization-based control method which makes use of the linearized state-space model of the system that is obtained after a state variables transformation (diffeomorphism). Using the linearized equivalent description of the system one can design a stabilizing feedback controller with pole placement methods. The stability of flatness-based control loops is analytically proven. On the other hand, in PID control the selection of the controller gains follows in most cases an empirical procedure and remains valid only round local operating points. This means that a change in the operating conditions of the robotic vehicle, will also cause the loss of stability of the control loop. PID control is output feedback-based and performs differentiation of the tracking error. This may also incur undesirable transients and abrupt variations of the control signal. Finally, taking into account the nonlinear dynamics of the robotic vehicle no proof of global asymptotic stability of the PID control can be demonstrated.

Remark 5

Singularities in the vehicle’s control are avoided. The proposed control scheme is valid provided that the vehicle’s velocity satisfies the constraint \(v_x\,{\ne }\,0\) and \(v_y\,{\ne }\,0\). Apart from this, the non-singularity conditions of the control method have been defined in Eqs. (49) and (50). By assuring that matrix \(\Delta \) is invertible then singularity is avoided. Necessary and sufficient conditions for the avoidance of such a singularity have been provided. The article solves both the stabilization and tracking problem for the velocity state vector of the vehicle, according to the dynamic model defined in Eqs. (1)–(3). The vehicle’s velocity expressed in an inertial reference frame and comprising the linear velocity state variables \(\dot{x}\),\(\dot{y}\) and the angular velocity state variable \(\dot{\psi }\) can be made to convergence to any time-invariant setpoint (stabilization) or can be made to converge to any time-varying setpoint (tracking). By extending the state vector of the vehicle so as to comprise as state variable the cartesian coordinates of the AGV as well as its heading angle one can also arrive at the formulation of the control problem for the vehicle’s position. This can be also solved in the context of flatness-based control.

Remark 6

The objective of the article is not only to control and stabilize the dynamics of the autonomous vehicles but also to estimate its state vector from indirect measurements. Thus one has to solve the joint nonlinear control and estimation (filtering) problem for the AGV’s model, which is not possible with other techniques, such as sliding mode control or backstepping control. First about sliding mode control it is noted that its application to the MIMO model of the vehicle is not straightforward for the following reasons (i) the sliding surface should have a vector form, (ii) uncertainty ranges for the model of the vehicle and for external perturbations are not known, (iii) the switching control term of sliding mode control can cause undesirable oscillations and unacceptable transients for the vehicle’s state vector, (iv) the solution of the state estimation problem with the use of a sliding mode observer will be also of inferior performance comparing to Kalman Filtering due to chattering phenomena, (v) there is no direct and easy to implement stability proof for the joint sliding-mode controller and sliding-mode observer scheme. Second, about backstepping control it is noted that its application to the model of the autonomous vehicle is not possible because this model is not found in the backstepping integral (triangular) form. For the AGV model to be brought to such a form a prior transformation is needed, but this falls again to the problem of writing the vehicle’s model in the canonical (Brunovsky) form through differential flatness diffeomorphisms. Similarly, in the backstepping approach there is no direct solution to the state and disturbances estimation problem for the AGV.

Conclusions

Two different problems in the design of AGVs have been treated. The first has to do with the design of a nonlinear controller for autonomous navigation of the vehicle according to specific velocity profiles. The second has to do with the real time estimation and disturbances due to forces or torques affecting the vehicle’s motion and with the estimation of the unknown parts of the vehicle’s dynamics. Once such disturbances have been identified with the use of a nonlinear filtering algorithm, that is redesigned in the form of a disturbance observer, it is possible to include an additional element in the vehicle’s controller that compensates for the disturbances effects.

The proposed nonlinear controller is based on differential flatness theory. It is shown that the vehicle’s model is a differentially flat one, which means that all its state variables and control inputs can be written as functions of the flat output and its derivatives. Once this is done it is possible to write the system in a linear canonical form (Brunovsky form) for which one can easily design a state feedback controller. The flatness-based controller eliminates velocity tracking error for all state variables of the vehicle.

The transformation into the linear canonical (Brunovsky) form is also used to obtain an estimator of the vehicle’s state vector through the processing of measurements from on-board sensors. To this end the Derivative-free nonlinear Kalman Filter is used, which stands for a linear Kalman Filter recursion operating on the exactly linearized canonical model of the vehicle, as obtained after the application of the differential flatness transformation. The proposed Derivative-free nonlinear Kalman Filter is significantly faster than all other nonlinear state estimators (e.g. EKF or UKF) while also succeeding very satisfactory accuracy in state estimation. Moreover, comparing to the Extended Kalman Filter it avoids linearization errors due to approximative linearization and it does not require the computation of Jacobian matrices.

By redesigning the Kalman Filter algorithm in the form of a disturbance observer it is also possible to estimate in real-time the effects of disturbance forces and torques that are exerted on the vehicle’s model and of terms representing unknown system dynamics. By knowing disturbances in the vehicle’s model their compensation is also possible through the inclusion of an additional term in the system’s controller. The efficiency of the proposed scheme for nonlinear control of the 4-wheel vehicle’s dynamic model and for estimation of disturbance’s or uncertainties with the use of a Kalman Filter-based disturbance observer was confirmed through simulation experiments.