**Fault Tolerant Depth Control of the MARES AUV**

Bruno Ferreira, Aníbal Matos and Nuno Cruz *INESC Porto and University of Porto Portugal*

## **1. Introduction**

48 Challenges and Paradigms in Applied Robust Control

Rollins L. (1999), Robust Control Theory, Carnegie Mellon University, 18-849b Dependable

Russo, G., et al. (2007), Unmanned Space Vehicle Program: DTFT in Flight Experiments, 18th

Spillman, Mark S. (2000), Robust Longitudinal Flight Control Design Using Linear

Tempo R. and Dabbene F. (1999), Probabilistic Robustness Analysis and Design of Uncertain

Van Soest, W. R., Chu, Q. P. and Mulder, J. A. (2006), Combined Feedback Linearization and

Vincent, James H., Abbas Emami-Naeinjt and Nasser M. Khraishi (1994), Case Study

Guidance, Control, And Dynamics, Vol. 17, No. 5, September-October 1994 Xing, L. (2003), Comparison of Pole Assignment & LQR Design Methods for Multivariable

Control of Uncertain Systems ; New York: Springer-Verlag, 2005.

Zhou , K., and Doyle, John C. (1998), Essential Of Robust Control, Prentice Hal, 1998

Control, and Dynamics, Vol. 29, No. 2, March–April 2006

ESA Symposium on European Rocket and Balloon Programmes and Related

Parameter-Varying Feedback, Journal of Guidance, Control, and Dynamics, Vol. 23,

Systems, in Dynamical Systems Control, Coding, Computer Vision – New Trends, Interfaces, and Interplay, ed. by Picci G. and Gilliam D.S., Birkhauser Verlag, 1999 Tempo, R., Calafiore, G., Dabbene, F. (2005), Randomized Algorithms for Analysis and

Constrained Model Predictive Control for Entry Flight, Journal of Guidance,

Comparison of Linear Quadratic Regulator and H∞ Control Synthesis, Journal Of

Control for STATCOM, Thesis submitted to the Department of Mechanical

Embedded Systems, Spring 1999

Research, Visby, Sweden, 2007.

No. 1, January–February 2000

Engineering, Florida State University

Control theory has been applied to several domains where practical considerations are relevant. Robotics is a notable example of this. In most cases, mobile robotic systems are governed such that their behavior obeys to a defined motion. However, during their operations, it is conceivable that faults could occur. Indeed, this assumption has to be made in order to predict a possible malfunction and to take an appropriate action according to the fault, improving the robustness and the reliability of the system. This work tackles the problem of fault detection, identification and automatic reconfiguration of an autonomous underwater vehicle (AUV). Although our emphasis will be directed to an AUV, the methods and the tools that are employed in this chapter can be easily extended to other engineering problems beyond robotics.

In this work, we will consider the MARES (Modular Autonomous Robot for Environment Sampling) (Fig. 1) Cruz & Matos (2008); Matos & Cruz (2009), a small-sized (1.5 meters long), torpedo shaped AUV weighting 32 kg, able to move at constant velocities up to 2.5 m/s. Its four thrusters provide four degrees of freedom (DOF), namely surge, heave, pitch and yaw. One of its main particularities is the capability to dive independently of the forward motion. The vertical through-hull thrusters provide heave and pitch controllability, while the horizontal ones ensure the surge and the yaw DOFs. The heave and pitch DOFs make the vertical plane control redundant when the vehicle is moving with surge velocities different from zero. In other words, the vehicle remains controllable if only one of these two DOFs is available. Such characteristic will be explored along this chapter in which the control of the nonlinear dynamics of the AUV Ferreira, Matos, Cruz & Pinto (2010); Fossen (1994) constitutes a challenging problem.

By taking advantage of the distribution of the actuators on the vehicle, it is possible to decouple the horizontal and the vertical motion. A common approach in such systems is to consider reduced models in order to simplify the analysis and the derivation of the control law (see Ferreira, Matos, Cruz & Pinto (2010); Teixeira et al. (2010) or Fossen (1994), for example). In general, for topedo-shaped vehicles, coupling effects due to composed motions (e.g., simultaneous sway and heave motions) are clearly smaller than the self effects of decomposed motion (e.g., effect of the heave motion on the heave dynamics) and can therefore be considered disturbances in the reduced model in which they are not included. Thus, a reduced model will be considered to deal with the vertical motion taking surge, heave and pitch rate as state components.

In order to make the detection and identification of possible faults, we present a method based on process monitoring by estimating relevant state variables of the system. See Frank & Ding (1997) for an overview on several techniques andZhang & Jiang (2002) for an application to a particular linear system. Wu et al. (2000) have developed an algorithm based on the two-stage

when operating with only one thruster. The method considers an integral component in the

Fault Tolerant Depth Control of the MARES AUV 51

Beyond the derivation and the particularization of the mathematical tools used here, we demonstrate our approach by illustrating the work with real experiments, voluntarily inducing faults on the system, and analyze the behavior of the dynamic system under such

The organization of the chapter is as follow: the section 2 describes the MARES AUV and presents the main mathematical models under consideration here, namely the kinematic and the dynamic models. In section 3, we introduce the relevant concepts and formulate the ASEKF from a reduced model of the vertical dynamics and describe how to identify the fault. In section 4, we derive the control law to drive the depth error to zero making use of the tools described above. Finally, in 5, our solution is demonstrated through real experiments.

The MARES autonomous underwater vehicle was developed in 2006 at the Faculty of Engineering of the University of Porto (FEUP). Typical operations have been performed in the ocean and fresh water, collecting relevant data for surveys and environmental monitoring during several tens of missions to date. Its configuration was specially designed to dive vertically in the water column while its horizontal motion is controlled independently, resulting in truly decoupled vertical and horizontal motions. Such characteristic is particularly appreciated in missions where the operation area is restricted or precise positioning is required. Parallel to the missions to collect data, the MARES AUV has also been used as a testbed for intensive research being performed in several problems related to robotics, specially on localization and control. Thus, besides the typical applications, several

Before presenting our method to detect a possible fault and to control the vehicle under such situation, let us first introduce the kinematics and dynamics concepts and equations. We assume an inertial earth-fixed frame {*I*} <sup>=</sup> {*xI*, *yI*, *zI*}, where *xI*, *yI*, *zI* <sup>∈</sup> **<sup>R</sup>**<sup>3</sup> are orthonormal vectors (in the marine literature, they are often made coincident with north, east and down directions, respectively), and a body-fixed frame {*B*} = {*xB*, *yB*, *zB*}, where *xB*, *yB*, *zB* <sup>∈</sup> **<sup>R</sup>**<sup>3</sup> are orthonormal vectors frequently refered to as surge, sway and heave directions, respectively (see Fig. 2). The absolute position and orientation of the vehicle is

coordinate of the frame {*B*} expressed in {*I*}. The vehicle's velocity, expressed in the body

*xB*, *yB* and *zB*, respectively. The velocities in both referentials are related through the kinematic

where *η*˙*<sup>c</sup>* denotes the time derivative of *η* and *J*(*η*2) = *block diag*[*J*1, *J*2] represents the rotation matrix, with *<sup>J</sup>*1, *<sup>J</sup>*<sup>2</sup> <sup>∈</sup> **<sup>R</sup>**3×3. Although this transformation is common in the literature to map vectors from a referential frame to another, it is not the only one. An alternative can be found in quaternions (see Zhang (1997), for an introduction and useful results), avoiding the singularity problems of the matrix *J*2. However, in this chapter, we will assume that the values of the angles that make the matrix *J*<sup>2</sup> singular (and *J*, consequently) are not reached. Moreover, since the water currents present in the ocean and in the rivers do not influence the

development of the present work, they will not be considered for simplicity.

*<sup>T</sup>* is the vector of euler angles with respect to *xI*, *yI* and *zI*, and [*x*, *y*, *z*] are the

*<sup>T</sup>* = [*x*, *y*, *z*, *φ*, *θ*, *ψ*]

*<sup>T</sup>*, where *p*, *q* and *r* are the angular velocities along

*η*˙*<sup>c</sup>* = *J*(*η*2)*νc*, (1)

*<sup>T</sup>*, where

missions have been conducted to test and to verify implemented algorithms.

expressed in the inertial frame {*I*} through the vector *η<sup>c</sup>* = [*η*1, *η*2]

frame {*B*}, is given by *ν<sup>c</sup>* = [*u*, *v*, *w*, *p*, *q*,*r*]

control law derived in the backstepping first step.

conditions.

**2. MARES**

*η*<sup>2</sup> = [*φ*, *θ*, *ψ*]

equation Fossen (1994)

## Fig. 1. MARES starting a typical mission in the ocean

Kalman filter to estimate deviations from expected input actuation for a linear system. Their approach consists in estimating the *loss of control effectiveness factors* that are added as entries of the state estimate, while guaranteeing that the corresponding estimate covariance lies in a defined interval. By imposing boundaries on the corresponding eigenvalues, it is possible to avoid impetuous corrections or to be insensitive to measurements. Inspired on the work by Zhang & Jiang (2002) and Wu et al. (2000), the present paper describes the implementation of an augmented state extended Kalman filter (ASEKF) to estimate the effectiveness of the control commands, detect and identify the possible faults.

The present work focuses on the vertical motion considering faults on the vertical thrusters. The method for accomodation of the faults consists in three main steps: fault detection, fault diagnosis and decision. Fault detection is responsible for creating a warning whenever an abrupt or an incipient fault happens, while fault diagnosis distinguishes and identifies the fault. In the presence of faults, a decision must be taken, adopting a suitable control law to stabilize the vertical motion. In the presence of faults in one of the vertical thrusters, the heave motion will no longer be controllable. Consequently, a control law derived for normal operation could be inadequate or even turn the feedback system unstable when such a fault occurs. An algorithm has to be developed in order to make the behavior of the robotic system tolerant to faults.

Making use of the pitch angle controllability, we will derive two control laws to drive the vehicle to a depth reference, possibly time variant. To achieve so, we make use of the Lyapunov theory, adopting the backstepping method Khalil (2002). Nevertheless, the presence of biases in steady state shifts the error at equilibrium away from zero. Those biases are commonly induced by unmodeled, neglected effects or external disturbances whose values are hard to observe or to estimate. The introduction of an integral term, under some assumptions, would solve the problem allowing the error to converge to zero as time goes to infinity. Based on the conditional integrators, extended by Singh & Khalil (2005) to more general control framework beyond sliding mode control Seshagiri & Khalil (2005), we present a control law that makes it possible to achieve asymptotic regulation of the vehicle depth error when operating with only one thruster. The method considers an integral component in the control law derived in the backstepping first step.

Beyond the derivation and the particularization of the mathematical tools used here, we demonstrate our approach by illustrating the work with real experiments, voluntarily inducing faults on the system, and analyze the behavior of the dynamic system under such conditions.

The organization of the chapter is as follow: the section 2 describes the MARES AUV and presents the main mathematical models under consideration here, namely the kinematic and the dynamic models. In section 3, we introduce the relevant concepts and formulate the ASEKF from a reduced model of the vertical dynamics and describe how to identify the fault. In section 4, we derive the control law to drive the depth error to zero making use of the tools described above. Finally, in 5, our solution is demonstrated through real experiments.

#### **2. MARES**

2 Will-be-set-by-IN-TECH

Kalman filter to estimate deviations from expected input actuation for a linear system. Their approach consists in estimating the *loss of control effectiveness factors* that are added as entries of the state estimate, while guaranteeing that the corresponding estimate covariance lies in a defined interval. By imposing boundaries on the corresponding eigenvalues, it is possible to avoid impetuous corrections or to be insensitive to measurements. Inspired on the work by Zhang & Jiang (2002) and Wu et al. (2000), the present paper describes the implementation of an augmented state extended Kalman filter (ASEKF) to estimate the effectiveness of the

The present work focuses on the vertical motion considering faults on the vertical thrusters. The method for accomodation of the faults consists in three main steps: fault detection, fault diagnosis and decision. Fault detection is responsible for creating a warning whenever an abrupt or an incipient fault happens, while fault diagnosis distinguishes and identifies the fault. In the presence of faults, a decision must be taken, adopting a suitable control law to stabilize the vertical motion. In the presence of faults in one of the vertical thrusters, the heave motion will no longer be controllable. Consequently, a control law derived for normal operation could be inadequate or even turn the feedback system unstable when such a fault occurs. An algorithm has to be developed in order to make the behavior of the robotic system

Making use of the pitch angle controllability, we will derive two control laws to drive the vehicle to a depth reference, possibly time variant. To achieve so, we make use of the Lyapunov theory, adopting the backstepping method Khalil (2002). Nevertheless, the presence of biases in steady state shifts the error at equilibrium away from zero. Those biases are commonly induced by unmodeled, neglected effects or external disturbances whose values are hard to observe or to estimate. The introduction of an integral term, under some assumptions, would solve the problem allowing the error to converge to zero as time goes to infinity. Based on the conditional integrators, extended by Singh & Khalil (2005) to more general control framework beyond sliding mode control Seshagiri & Khalil (2005), we present a control law that makes it possible to achieve asymptotic regulation of the vehicle depth error

Fig. 1. MARES starting a typical mission in the ocean

control commands, detect and identify the possible faults.

tolerant to faults.

The MARES autonomous underwater vehicle was developed in 2006 at the Faculty of Engineering of the University of Porto (FEUP). Typical operations have been performed in the ocean and fresh water, collecting relevant data for surveys and environmental monitoring during several tens of missions to date. Its configuration was specially designed to dive vertically in the water column while its horizontal motion is controlled independently, resulting in truly decoupled vertical and horizontal motions. Such characteristic is particularly appreciated in missions where the operation area is restricted or precise positioning is required. Parallel to the missions to collect data, the MARES AUV has also been used as a testbed for intensive research being performed in several problems related to robotics, specially on localization and control. Thus, besides the typical applications, several missions have been conducted to test and to verify implemented algorithms.

Before presenting our method to detect a possible fault and to control the vehicle under such situation, let us first introduce the kinematics and dynamics concepts and equations. We assume an inertial earth-fixed frame {*I*} <sup>=</sup> {*xI*, *yI*, *zI*}, where *xI*, *yI*, *zI* <sup>∈</sup> **<sup>R</sup>**<sup>3</sup> are orthonormal vectors (in the marine literature, they are often made coincident with north, east and down directions, respectively), and a body-fixed frame {*B*} = {*xB*, *yB*, *zB*}, where *xB*, *yB*, *zB* <sup>∈</sup> **<sup>R</sup>**<sup>3</sup> are orthonormal vectors frequently refered to as surge, sway and heave directions, respectively (see Fig. 2). The absolute position and orientation of the vehicle is expressed in the inertial frame {*I*} through the vector *η<sup>c</sup>* = [*η*1, *η*2] *<sup>T</sup>* = [*x*, *y*, *z*, *φ*, *θ*, *ψ*] *<sup>T</sup>*, where *η*<sup>2</sup> = [*φ*, *θ*, *ψ*] *<sup>T</sup>* is the vector of euler angles with respect to *xI*, *yI* and *zI*, and [*x*, *y*, *z*] are the coordinate of the frame {*B*} expressed in {*I*}. The vehicle's velocity, expressed in the body frame {*B*}, is given by *ν<sup>c</sup>* = [*u*, *v*, *w*, *p*, *q*,*r*] *<sup>T</sup>*, where *p*, *q* and *r* are the angular velocities along *xB*, *yB* and *zB*, respectively. The velocities in both referentials are related through the kinematic equation Fossen (1994)

$$
\dot{\eta}\_{\mathcal{C}} = f(\eta\_{\mathcal{D}}) \nu\_{\mathcal{C}} \tag{1}
$$

where *η*˙*<sup>c</sup>* denotes the time derivative of *η* and *J*(*η*2) = *block diag*[*J*1, *J*2] represents the rotation matrix, with *<sup>J</sup>*1, *<sup>J</sup>*<sup>2</sup> <sup>∈</sup> **<sup>R</sup>**3×3. Although this transformation is common in the literature to map vectors from a referential frame to another, it is not the only one. An alternative can be found in quaternions (see Zhang (1997), for an introduction and useful results), avoiding the singularity problems of the matrix *J*2. However, in this chapter, we will assume that the values of the angles that make the matrix *J*<sup>2</sup> singular (and *J*, consequently) are not reached. Moreover, since the water currents present in the ocean and in the rivers do not influence the development of the present work, they will not be considered for simplicity.

Several approaches for fault identification and detection have been proposed, commonly based on observation of the residuals Frank & Ding (1997) either by using state observers or by using accurate models. Our approach makes use of an extended Kalman filter to detect the faults, indirectly exploiting the residuals to estimate actuation bias variables additionally incorporated in the state. A similar approach was already carried out by Zhang & Jiang (2002) where they used a two stage Kalman filter to identify the faults on actuation of a linear system. The method presented here makes it possible not only to detect faults but also to identify the faulty actuator. By taking advantage of the cyclic predictions and corrections, the main idea behind our approach is to estimate the biases on the actuation (or deviation from the nominal value, commonly referred to as loss of control effectiveness factor in Wu et al. (2000); Zhang & Jiang (2002) whose values should theoretically equal zero when no fault is occuring.

Fault Tolerant Depth Control of the MARES AUV 53

The implementation of the extended Kalman filter assumes the use of a reasonably accurate dynamics model that recreates mathematically the behavior of the system for the prediction step. As it was pointed out in the previous section, the use of the complete model of the vehicle dynamics is complex and computationally expensive. Thus, from 2, we derive the

> ⎡ ⎢ ⎣

⎤ ⎥ <sup>⎦</sup> , **ft**(*t*) =

The authors recommend Fossen (1994); Triantafyllou & Hover (2002) for details about the parameters above and their derivation, and Hoerner (1993); White (2008) for further details. Notice that **ft**(*t*) is the vector of forces applied by the thrusters that are generated according to a given control law, and *fp*, *fr*, *fs* and *fb* are scalars that represents the force applied by the port, starboard, stern and bow thrusters, respectively. We assume that such forces can be directly measured during operation. The inclusion of the surge velocity is required in this reduced order model due to the nonnegligible influence it has on the vertical plane dynamics.

Our final goal in this section is to detect and to identify a fault occuring on one of the vertical thrusters. To this end, one aims to quantify the loss of control effectiveness of the referred actuators: The effective force applied by the vertical thrusters may differ from the commanded one. We will consider that *fs* and *fb* are the commanded forces, which may not correspond

⎤ ⎥

<sup>⎦</sup> , *<sup>g</sup>*(*η*2) = <sup>−</sup>

⎡ ⎢ ⎢ ⎣

*fp*(*t*) *fr*(*t*) *fs*(*t*) *fb*(*t*)

⎤ ⎥ ⎥ ⎦ .

*Mν*˙ = −*C*(*ν*)*ν* − *D*(*ν*)*ν* − *g*(*η*2) + *Pf***ft**(*t*) (3)

0 −*mq* −*Zw*˙ *w* − *Zq*˙*q* −*mq* 0 *Xu*˙*u* + *Xq*˙*q*

> (*W* − *B*) sin *θ* (*B* − *W*) cos *θ* −*zCBB* sin *θ*

⎤ ⎥ ⎦ , ⎤ ⎥ ⎦ ,

*Zw*˙ *w* + *Zq*˙*q* −*Xu*˙*u* − *Xq*˙*q* 0

⎡ ⎢ ⎣

reduced model for the vertical motion, considering that cross-terms are negligible:

⎤ ⎥ <sup>⎦</sup> , *<sup>C</sup>*(*ν*) =

*<sup>X</sup>*|*u*|*u*|*u*| <sup>0</sup> *<sup>X</sup>*|*q*|*q*|*q*| <sup>0</sup> *<sup>Z</sup>*|*w*|*w*|*w*| *<sup>Z</sup>*|*q*|*q*|*q*| *<sup>M</sup>*|*u*|*u*|*u*| *<sup>M</sup>*|*w*|*w*|*w*| *<sup>M</sup>*|*q*|*q*|*q*|

> 11 0 0 00 1 1 0 0 *xts xtb*

⎡ ⎢ ⎣

The parameters used in the reduced model are listed in the table 1.

**3.2 Augmented state extended Kalman filter formulation**

*Pf* =

**3.1 Vertical plane dynamics**

*<sup>T</sup>* and

*m* − *Xu*˙ 0 −*Xq*˙ 0 *m* − *Zw*˙ −*Zq*˙ −*Mu*˙ −*Mw*˙ −*Mq*˙

> ⎡ ⎢ ⎣

where *ν* = [*u*, *w*, *q*]

⎡ ⎢ ⎣

*D*(*ν*) = −

*M* =

Fig. 2. Body frame and inertial frame referentials

As it is well known, rigid bodies moving in the three dimensional space are governed by nonlinear equations. For the particular case of underwater vehicles, such equations include the effect of the added mass, viscous damping, restoring and actuation forces and moments. Following the notation in Fossen (1994), the nonlinear second order, six dimensions equation is written as:

$$M\_{\mathcal{C}} \dot{\nu}\_{\mathcal{C}} = -\mathsf{C}\_{\mathcal{C}}(\nu\_{\mathcal{C}}) \nu\_{\mathcal{C}} - D\_{\mathcal{C}}(\nu\_{\mathcal{C}}) \nu\_{\mathcal{C}} - g\_{\mathcal{C}}(\eta\_{\mathcal{C}}) + \mathsf{r}\_{\mathcal{C}} \tag{2}$$

where *Mc* <sup>∈</sup> **<sup>R</sup>**6×<sup>6</sup> is the sum of the body inertia and added mass matrices, *Cc* <sup>∈</sup> **<sup>R</sup>**6×<sup>6</sup> results from the sum of the Coriolis and centrifugal terms from body inertia and added mass, *Dc* ∈ **<sup>R</sup>**6×<sup>6</sup> is the viscous damping matrix, *gc* <sup>∈</sup> **<sup>R</sup>**<sup>6</sup> is the vector of restoring forces and moments and *<sup>τ</sup><sup>c</sup>* <sup>∈</sup> **<sup>R</sup>**<sup>6</sup> is the vector of actuation forces and moments.

Such system is complex and the task of deriving control laws that ensure stability is not trivial, having led to order reduction in several works (see Ferreira, Matos, Cruz & IEEE (2010); Fossen (1994); Teixeira et al. (2010), for example). By taking advantage of the body shape symmetries and of the configuration of the actuators on the body, it is usual to decouple the complex motion in more elementary ones. However, this has consequences since some cross-coupling terms are eliminated but their influence is often small and can be neglected or considered disturbances. Such approach has been implemented in the MARES AUV and the corresponding performances were already demonstrated in Ferreira, Matos, Cruz & Pinto (2010). The current thruster configuration on the MARES makes it possible to decouple the motion into the vertical and the horizontal plane. Since the roll angle is stable (and *φ* ≈ 0) two reduced order models are extracted. See Ferreira, Matos, Cruz & Pinto (2010), for further details.

#### **3. Fault detection and identification**

Under normal operation, the vertical thrusters of MARES provide the capability to control almost independently the pitch and heave degrees of freedom (DOF). In the same way, the horizontal thrusters make possible the control on the surge and yaw DOFs. As it will be exposed later, the vehicle remains controllable if one of the vertical thrusters fails. As an aside, note that the same is not verified if one the two horizontal thrusters fails since the surge and the yaw motion can no longer be decoupled. The derivation of the control laws is left to the next section.

Several approaches for fault identification and detection have been proposed, commonly based on observation of the residuals Frank & Ding (1997) either by using state observers or by using accurate models. Our approach makes use of an extended Kalman filter to detect the faults, indirectly exploiting the residuals to estimate actuation bias variables additionally incorporated in the state. A similar approach was already carried out by Zhang & Jiang (2002) where they used a two stage Kalman filter to identify the faults on actuation of a linear system. The method presented here makes it possible not only to detect faults but also to identify the faulty actuator. By taking advantage of the cyclic predictions and corrections, the main idea behind our approach is to estimate the biases on the actuation (or deviation from the nominal value, commonly referred to as loss of control effectiveness factor in Wu et al. (2000); Zhang & Jiang (2002) whose values should theoretically equal zero when no fault is occuring.

#### **3.1 Vertical plane dynamics**

4 Will-be-set-by-IN-TECH

As it is well known, rigid bodies moving in the three dimensional space are governed by nonlinear equations. For the particular case of underwater vehicles, such equations include the effect of the added mass, viscous damping, restoring and actuation forces and moments. Following the notation in Fossen (1994), the nonlinear second order, six dimensions equation

where *Mc* <sup>∈</sup> **<sup>R</sup>**6×<sup>6</sup> is the sum of the body inertia and added mass matrices, *Cc* <sup>∈</sup> **<sup>R</sup>**6×<sup>6</sup> results from the sum of the Coriolis and centrifugal terms from body inertia and added mass, *Dc* ∈ **<sup>R</sup>**6×<sup>6</sup> is the viscous damping matrix, *gc* <sup>∈</sup> **<sup>R</sup>**<sup>6</sup> is the vector of restoring forces and moments

Such system is complex and the task of deriving control laws that ensure stability is not trivial, having led to order reduction in several works (see Ferreira, Matos, Cruz & IEEE (2010); Fossen (1994); Teixeira et al. (2010), for example). By taking advantage of the body shape symmetries and of the configuration of the actuators on the body, it is usual to decouple the complex motion in more elementary ones. However, this has consequences since some cross-coupling terms are eliminated but their influence is often small and can be neglected or considered disturbances. Such approach has been implemented in the MARES AUV and the corresponding performances were already demonstrated in Ferreira, Matos, Cruz & Pinto (2010). The current thruster configuration on the MARES makes it possible to decouple the motion into the vertical and the horizontal plane. Since the roll angle is stable (and *φ* ≈ 0) two reduced order models are extracted. See Ferreira, Matos, Cruz & Pinto (2010), for further

Under normal operation, the vertical thrusters of MARES provide the capability to control almost independently the pitch and heave degrees of freedom (DOF). In the same way, the horizontal thrusters make possible the control on the surge and yaw DOFs. As it will be exposed later, the vehicle remains controllable if one of the vertical thrusters fails. As an aside, note that the same is not verified if one the two horizontal thrusters fails since the surge and the yaw motion can no longer be decoupled. The derivation of the control laws is left to

*Mcν*˙*<sup>c</sup>* = −*Cc*(*νc*)*ν<sup>c</sup>* − *Dc*(*νc*)*ν<sup>c</sup>* − *gc*(*ηc*) + *τ<sup>c</sup>* (2)

Fig. 2. Body frame and inertial frame referentials

and *<sup>τ</sup><sup>c</sup>* <sup>∈</sup> **<sup>R</sup>**<sup>6</sup> is the vector of actuation forces and moments.

**3. Fault detection and identification**

is written as:

details.

the next section.

The implementation of the extended Kalman filter assumes the use of a reasonably accurate dynamics model that recreates mathematically the behavior of the system for the prediction step. As it was pointed out in the previous section, the use of the complete model of the vehicle dynamics is complex and computationally expensive. Thus, from 2, we derive the reduced model for the vertical motion, considering that cross-terms are negligible:

$$M\dot{\nu} = -\mathbf{C}(\nu)\nu - D(\nu)\nu - \mathbf{g}(\eta\_2) + P\_f \mathbf{f}\_{\mathbf{f}}(t) \tag{3}$$

where *ν* = [*u*, *w*, *q*] *<sup>T</sup>* and

$$M = \begin{bmatrix} m - X\_{\vec{\mathsf{u}}} & 0 & -X\_{\vec{\mathsf{q}}} \\ 0 & m - Z\_{\vec{\mathsf{w}}} & -Z\_{\vec{\mathsf{q}}} \\ -M\_{\vec{\mathsf{u}}} & -M\_{\vec{\mathsf{w}}} & -M\_{\vec{\mathsf{q}}} \end{bmatrix}, \mathsf{C}(\nu) = \begin{bmatrix} 0 & -mq & -Z\_{\vec{\mathsf{u}}}w - Z\_{\vec{\mathsf{u}}}q \\ -mq & 0 & X\_{\vec{\mathsf{u}}}u + X\_{\vec{\mathsf{q}}}q \\ Z\_{\vec{\mathsf{w}}}w + Z\_{\vec{\mathsf{q}}}q - X\_{\vec{\mathsf{u}}}u - X\_{\vec{\mathsf{q}}}q & 0 \end{bmatrix},$$

$$D(\nu) = -\begin{bmatrix} X\_{|u|,|\mu|}|u| & 0 & X\_{|q|}q| |q| \\ 0 & Z\_{|w|w}|w| & Z\_{|q|}q|q| \\ M\_{|u|,|u|}|u| & M\_{|w|w}|w| & M\_{|q|}q| \end{bmatrix}, \mathcal{J}(\eta\_{2}) = -\begin{bmatrix} (W - B)\sin\theta \\ (B - W)\cos\theta \\ -z\_{\mathsf{C}B}B\sin\theta \end{bmatrix},$$

$$P\_{f} = \begin{bmatrix} 1 \ 1 & 0 & 0 \\ 0 \ 0 & 1 & 1 \\ 0 \ 0 & x\_{\mathsf{t}} & x\_{\mathsf{t}} \end{bmatrix}, \ \mathbf{f}\_{\mathbf{t}}(t) = \begin{bmatrix} f\_{p}(t) \\ f\_{f}(t) \\ f\_{\vec{\mathsf{s}}}(t) \\ f\_{\vec{\mathsf{s}}}(t) \end{bmatrix}.$$

The authors recommend Fossen (1994); Triantafyllou & Hover (2002) for details about the parameters above and their derivation, and Hoerner (1993); White (2008) for further details. Notice that **ft**(*t*) is the vector of forces applied by the thrusters that are generated according to a given control law, and *fp*, *fr*, *fs* and *fb* are scalars that represents the force applied by the port, starboard, stern and bow thrusters, respectively. We assume that such forces can be directly measured during operation. The inclusion of the surge velocity is required in this reduced order model due to the nonnegligible influence it has on the vertical plane dynamics. The parameters used in the reduced model are listed in the table 1.

#### **3.2 Augmented state extended Kalman filter formulation**

Our final goal in this section is to detect and to identify a fault occuring on one of the vertical thrusters. To this end, one aims to quantify the loss of control effectiveness of the referred actuators: The effective force applied by the vertical thrusters may differ from the commanded one. We will consider that *fs* and *fb* are the commanded forces, which may not correspond

assuming *φ* = 0, and *f*(·) results from the dynamics model 3 as

<sup>⎦</sup> <sup>=</sup> *<sup>S</sup>ν*˙ <sup>=</sup> *SM*−1�

*S* =

depth *z*, the pitch angle *θ* and the pitch rate *q*. Thus it results

*y* = *Chx* + *vn*(*t*),

*<sup>T</sup>*, we rewrite 5 on the form

*y* = *Css* + *vn*(*t*).

⎡ ⎢ ⎣

*h*(**x**) = *Ch***x**, *Ch* =

⎤ ⎥ <sup>⎦</sup> , *Pv* <sup>=</sup>

Fault Tolerant Depth Control of the MARES AUV 55

Regarding the output *y* of 4, the dimension of the function *h*(**x**) depends on the measurements and consequently on the on-board sensors. Here, we will assume we are able to observe the

The vector *vn* <sup>∈</sup> **<sup>R</sup>**<sup>3</sup> is the output noise, assumed to be zero-mean, Gaussian noise with

Notice that **x2** is the vector containing the last two entries of the velocity vector *ν*, i.e., *ν* = [*u*, **x2**]. For simplicity of notation, in the expressions above we wrote *ν* instead of [*u*, **x2**]. Recall that we assumed that *u* is a measured variable, or at least, it can be accurately estimated. Indeed, it could be included in the state in 4 but the complexity of this latter would increase

vector of loss of control effectiveness factors, adopting the same notation as in Wu et al. (2000).

**x**˙ = *Al*(**x**)**x** + *fu*(**x**, *u*) + *f*(**x**, *u*,**fv**) + *E*(**fv**)*γ* + *wx*(*t*)

where *<sup>w</sup><sup>γ</sup>* <sup>∈</sup> **<sup>R</sup>**<sup>2</sup> is a zero-mean, Gaussian noise vector with autocorrelation matrix *<sup>Q</sup>γ*,

*E*(**fv**) = *SM*−1*Pv* diag(**fv**). As it can be seen in 5, *γ* is assumed to be driven only by the noise *wγ*. This comes from the fact that, in real scenarios, it is impossible to predict how the fault and, consequently, how *γ* evolve. In such situation, the most appropriate is to model the evolution with a noise vector *w<sup>γ</sup>* with a sufficiently large autocorrelation (see Wu et al. (2000)), whose entries can play an important role in the design of the augmented state estimator, as it will be seen later on.

*s*˙ = *As*(*s*)*s* + *fus*(*s*, *u*) + *fs*(*s*, *u*,**fv**) + *Es*(**fv**)*s* + *ws*(*t*)

In order to model the possible loss of control effectiveness, let us define *γ* = [*γs*, *γb*]

Introducing these multiplicative factors in 4, the augmented state model results in

⎡ ⎣

1000 0100 0001

*γ*˙ = *wγ*(*t*) (5)

⎤ ⎦ .

− *C*(*ν*)*ν* − *D*(*ν*)*ν* − *g*(**x2**) + *Pv***fv**(*t*)

⎤

⎦ , **fv** =

� *fs*(*t*) *fb*(*t*) � .

⎡ ⎣

� ,

*<sup>T</sup>* as the

(6)

*f*(**x**, *u*,**fv**) =

autocorrelation matrix *Rv*.

uncorrelated with *wx*, and

Making *s* = [**x***T*, *γT*]

without advantages in the approach.

⎡ ⎢ ⎣

0 0 *w*˙ *q*˙

⎤ ⎥


Table 1. Reduced model terms

to the effective applied force. Like in many other problems in robotics, it is often difficult or even impossible to measure such forces. Measuring relative or absolute motion variables then becomes an alternative and the choice of the state to be observed is directly influenced by the variables that can be actually measured. Therefore, we propose the following model for the fault free ideal system:

$$\begin{aligned} \dot{\mathbf{x}} &= \begin{bmatrix} \dot{\mathbf{x}}\_1 \\ \dot{\mathbf{x}}\_2 \end{bmatrix} = \begin{bmatrix} \dot{z} \\ \dot{\theta} \\ \dot{w} \\ \dot{\theta} \end{bmatrix} = A\_l(\mathbf{x})\mathbf{x} + f\_{\mu}(\mathbf{x}, u) + f(\mathbf{x}, u, \mathbf{f}\_{\mathbf{V}}) + w^{\chi}(t) \\\\ y &= h(\mathbf{x}) + v\_n(t) \end{aligned} \tag{4}$$

where *<sup>w</sup><sup>x</sup>* <sup>∈</sup> **<sup>R</sup>**<sup>4</sup> is a zero-mean Gaussian noise vector with autocorrelation matrix *<sup>Q</sup>w*(*t*), **x1** = [*z*, *θ*] *<sup>T</sup>*, **x2** = [*w*, *q*] *<sup>T</sup>*, *Al* and *fu*(·) are easily derived from the kinematics model in 1 as

$$A\_{l}(\mathbf{x}) = \begin{bmatrix} 0 \ 0 \ \cos \theta \ 0 \\ 0 \ 0 \ \mathbf{0} & 1 \\ 0 \ 0 \ \mathbf{0} & 0 \\ 0 \ 0 \ \mathbf{0} & 0 \end{bmatrix}, \quad f\_{l}(\mathbf{x}, \mu) = \begin{bmatrix} -u \sin \theta \\ 0 \\ 0 \\ 0 \end{bmatrix}.$$

6 Will-be-set-by-IN-TECH

*Xu*˙ <sup>−</sup>1.74 · 100 *kg* Added mass longitudinal term

*xts* <sup>−</sup>3.21 · <sup>10</sup>−<sup>1</sup> *m xB* of stern vertical thruster w.r.t CG *xtb* 5.34 · <sup>10</sup>−<sup>1</sup> *m xB* of bow vertical thruster w.r.t CG

to the effective applied force. Like in many other problems in robotics, it is often difficult or even impossible to measure such forces. Measuring relative or absolute motion variables then becomes an alternative and the choice of the state to be observed is directly influenced by the variables that can be actually measured. Therefore, we propose the following model for the

where *<sup>w</sup><sup>x</sup>* <sup>∈</sup> **<sup>R</sup>**<sup>4</sup> is a zero-mean Gaussian noise vector with autocorrelation matrix *<sup>Q</sup>w*(*t*),

<sup>⎦</sup> , *fu*(**x**, *<sup>u</sup>*) =

⎤ ⎥

<sup>⎦</sup> <sup>=</sup> *Al*(**x**)**<sup>x</sup>** <sup>+</sup> *fu*(**x**, *<sup>u</sup>*) + *<sup>f</sup>*(**x**, *<sup>u</sup>*,**fv**) + *<sup>w</sup>x*(*t*)

*<sup>T</sup>*, *Al* and *fu*(·) are easily derived from the kinematics model in 1 as

⎡ ⎢ ⎣ −*u* sin *θ* 0 0 0

⎤ ⎥ ⎦ , (4)

*Xq*˙ <sup>−</sup>3.05 · <sup>10</sup>−<sup>2</sup> *kg* · *<sup>m</sup>* Added mass cross-term *Zw*˙ <sup>−</sup>4.12 · 101 *kg* Added mass heave term *Zq*˙ <sup>−</sup>1.23 · <sup>10</sup>−<sup>1</sup> *kg* · *<sup>m</sup>* Added mass cross-term *Mu*˙ <sup>−</sup>3.05 · <sup>10</sup>−<sup>2</sup> *kg* · *<sup>m</sup>* Added mass cross-term *Mw*˙ <sup>−</sup>1.23 · <sup>10</sup>−<sup>1</sup> *kg* · *<sup>m</sup>* Added mass cross-term *Mq*˙ <sup>−</sup>6.07 · 100 *kg* · *<sup>m</sup>*<sup>2</sup> Added mass pitch term *<sup>X</sup>*|*u*|*<sup>u</sup>* <sup>−</sup>1.04 · 101 *kg* · *<sup>m</sup>*−<sup>1</sup> Drag longitudinal term

*<sup>X</sup>*|*q*|*<sup>q</sup>* 4.84 · <sup>10</sup>−<sup>2</sup> *kg* · *<sup>m</sup>* Drag cross term *<sup>Z</sup>*|*w*|*<sup>w</sup>* <sup>−</sup>1.16 · 102 *kg* · *<sup>m</sup>*−<sup>1</sup> Drag heave term *<sup>Z</sup>*|*q*|*<sup>q</sup>* <sup>−</sup>5.95 · 100 *kg* · *<sup>m</sup>* Drag cross-term *<sup>M</sup>*|*u*|*<sup>u</sup>* <sup>−</sup>2.11 · <sup>10</sup>−<sup>1</sup> *kg* Drag cross-term *<sup>M</sup>*|*w*|*<sup>w</sup>* <sup>−</sup>8.26 · 100 *kg* Drag cross-term *<sup>M</sup>*|*q*|*<sup>q</sup>* <sup>−</sup>1.56 · 101 *kg* · *<sup>m</sup>*<sup>2</sup> Drag pitch term

Table 1. Reduced model terms

**x**˙ = � **x˙ 1 x˙ 2** � =

*<sup>T</sup>*, **x2** = [*w*, *q*]

⎡ ⎢ ⎢ ⎣

*y* = *h*(*x*) + *vn*(*t*),

*Al*(**x**) =

⎡ ⎢ ⎣

0 0 cos *θ* 0 00 0 1 00 0 0 00 0 0

*z*˙ ˙ *θ w*˙ *q*˙

⎤ ⎥ ⎥

fault free ideal system:

**x1** = [*z*, *θ*]

Parameter Value units Description *<sup>m</sup>* 3.20 · 101 *kg* Vehicle's mass *<sup>W</sup>* 3.14 · 102 *<sup>N</sup>* Vehicle's weight *<sup>B</sup>* 3.16 · 102 *<sup>N</sup>* Vehicle's bouyancy *zCB* <sup>−</sup>4.40 · <sup>10</sup>−<sup>3</sup> *m zB* of CB w.r.t CG

assuming *φ* = 0, and *f*(·) results from the dynamics model 3 as

$$f(\mathbf{x}, u, \mathbf{f}\_{\mathbf{V}}) = \begin{bmatrix} 0 \\ 0 \\ \dot{w} \\ \dot{q} \end{bmatrix} = S\dot{\nu} = SM^{-1}\left(-\mathbf{C}(\nu)\nu - D(\nu)\nu - g(\mathbf{x}\_{2}) + P\_{\mathbf{v}}\mathbf{f}\_{\mathbf{V}}(t)\right),$$

$$S = \begin{bmatrix} 0 \ 0 \ 0 \\ 0 \ 0 \\ 0 \ 1 \\ 0 \ 0 \ 1 \end{bmatrix}, \quad P\_{\mathbf{v}} = \begin{bmatrix} 0 & 0 \\ 1 & 1 \\ \mathbf{x}\_{\text{ts}} \ \mathbf{x}\_{\text{tb}} \end{bmatrix}, \quad \mathbf{f}\_{\mathbf{V}} = \begin{bmatrix} f\_{\mathbf{s}}(t) \\ f\_{\mathbf{b}}(t) \end{bmatrix}.$$

Regarding the output *y* of 4, the dimension of the function *h*(**x**) depends on the measurements and consequently on the on-board sensors. Here, we will assume we are able to observe the depth *z*, the pitch angle *θ* and the pitch rate *q*. Thus it results

$$h(\mathbf{x}) = \mathbf{C}\_{\hbar}\mathbf{x}\_{\prime} \quad \mathbf{C}\_{\hbar} = \begin{bmatrix} 1 & 0 \ 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 0 \end{bmatrix}.$$

The vector *vn* <sup>∈</sup> **<sup>R</sup>**<sup>3</sup> is the output noise, assumed to be zero-mean, Gaussian noise with autocorrelation matrix *Rv*.

Notice that **x2** is the vector containing the last two entries of the velocity vector *ν*, i.e., *ν* = [*u*, **x2**]. For simplicity of notation, in the expressions above we wrote *ν* instead of [*u*, **x2**]. Recall that we assumed that *u* is a measured variable, or at least, it can be accurately estimated. Indeed, it could be included in the state in 4 but the complexity of this latter would increase without advantages in the approach.

In order to model the possible loss of control effectiveness, let us define *γ* = [*γs*, *γb*] *<sup>T</sup>* as the vector of loss of control effectiveness factors, adopting the same notation as in Wu et al. (2000). Introducing these multiplicative factors in 4, the augmented state model results in

$$\begin{aligned} \dot{\mathbf{x}} &= A\_l(\mathbf{x})\mathbf{x} + f\_{l\iota}(\mathbf{x}, \boldsymbol{\mu}) + f(\mathbf{x}, \boldsymbol{\mu}, \mathbf{f}\_{\mathbf{v}}) + E(\mathbf{f}\_{\mathbf{v}})\boldsymbol{\gamma} + w^{\chi}(t) \\ \dot{\boldsymbol{\gamma}} &= w^{\gamma}(t) \\ \dot{y} &= \mathbf{C}\_{\hbar}\mathbf{x} + v\_{\iota}(t), \end{aligned} \tag{5}$$

where *<sup>w</sup><sup>γ</sup>* <sup>∈</sup> **<sup>R</sup>**<sup>2</sup> is a zero-mean, Gaussian noise vector with autocorrelation matrix *<sup>Q</sup>γ*, uncorrelated with *wx*, and

$$E(\mathbf{f}\_{\mathbf{v}}) = S M^{-1} P\_{\upsilon} \operatorname{diag}(\mathbf{f}\_{\mathbf{v}}).$$

As it can be seen in 5, *γ* is assumed to be driven only by the noise *wγ*. This comes from the fact that, in real scenarios, it is impossible to predict how the fault and, consequently, how *γ* evolve. In such situation, the most appropriate is to model the evolution with a noise vector *w<sup>γ</sup>* with a sufficiently large autocorrelation (see Wu et al. (2000)), whose entries can play an important role in the design of the augmented state estimator, as it will be seen later on. Making *s* = [**x***T*, *γT*] *<sup>T</sup>*, we rewrite 5 on the form

$$\begin{aligned} \dot{s} &= A\_s(s)s + f\_{\text{lsf}}(s, \mu) + f\_{\text{s}}(s, \mu, \mathbf{f\_V}) + E\_{\text{s}}(\mathbf{f\_V})s + w\_{\text{s}}(t) \\\\ \dot{y} &= C\_{\text{s}}s + v\_{\text{lsf}}(t). \end{aligned} \tag{6}$$

factors. For the sake of clarity, from 10 and 13 we can decompose *Pk* as

Thus, from 10 and 13, we can conclude

**3.3 Fault identification**

from the eigenvalues *λ<sup>γ</sup>* of *P<sup>γ</sup>*

**4. Control of MARES**

detection:

of *P<sup>γ</sup>* as measures, we propose the following function

*Q<sup>γ</sup>* =

*Pk* =

*<sup>k</sup>*+1|*<sup>k</sup>* <sup>=</sup> *<sup>P</sup><sup>γ</sup>*

*diag*(*qγ*, *<sup>q</sup>γ*) , if max(*λ<sup>γ</sup>*

where *q<sup>γ</sup>* is the autocorrelation of *γi*, *i* = 1, 2 and *λmax* is a preset maximum constant.

0 , if max(*λ<sup>γ</sup>*

The loss control effectiveness factors provide an estimate of the performances of the actuators. Ideally, a fault would be identified whenever the absolute value of one of the factors would rise above a preset threshold. However, model uncertainties will be directly reflected in these factors. Even in normal operation, with the actuators working perfectly, the loss of control effectiveness factors may diverge from zero, reflecting, for example, the effect of a damping force greater than the modeled. As these errors are frequently commited on the overall model, their effects are verified on all actuators either by increasing or decreasing the loss of control effectiveness factors. Hence, for the present case, a reasonable measure of the malfunction of one of the thrusters is given by the difference of the corresponding loss of control effectiveness factor estimate. On the other hand, taking a decision about the malfunction of a given thruster should also be based on the confidence of the factor estimate, which can be indirectly taken

corrections on the state are being performed. Thus, we propose the following measure for fault

Whenever *δ* is greater than a preset threshold, a fault is detected and the identification is made according to the greater *λ*, i.e., if *γ<sup>s</sup>* > *γ<sup>b</sup>* then the stern thruster is faulty and vice-versa.

In the presence of a faulty vertical thruster, the reconfiguration of the actuation is required. Otherwise, keeping the same actuation will likely lead to instability or to other pratical problems such as thruster dammage or large battery consumption, for example. Therefore, the control law for normal operation could be inadequate and another control law must take

<sup>1</sup> <sup>+</sup> *<sup>λ</sup><sup>γ</sup>* 2 )

*<sup>δ</sup>* <sup>=</sup> <sup>|</sup>*γ<sup>s</sup>* <sup>−</sup> *<sup>γ</sup>b*<sup>|</sup> *<sup>f</sup>λ*(*λ<sup>γ</sup>*

where *f<sup>λ</sup>* is a monotically increasing function of its argument.

*<sup>k</sup>*+1|*k*+<sup>1</sup> <sup>≤</sup> *<sup>P</sup><sup>γ</sup>*

that *P<sup>γ</sup>* lies in an interval, preventing state corrections to be excessive, when *P<sup>γ</sup>* is too large, or to be insufficient with slow convergence, when *P<sup>γ</sup>* is too small. Taking the eigenvalues *λ<sup>γ</sup>*

*Pγ*

*Pγ*

where we used the fact that *Pk*<sup>+</sup>1|*<sup>k</sup>* <sup>&</sup>gt; 0. Hence, the autocorrelation matrix *<sup>Q</sup><sup>γ</sup>*

 *P<sup>x</sup> <sup>k</sup> <sup>P</sup>γ<sup>x</sup> k*

Fault Tolerant Depth Control of the MARES AUV 57

*Px<sup>γ</sup> <sup>k</sup> <sup>P</sup><sup>γ</sup> k*

> *<sup>k</sup>*|*<sup>k</sup>* <sup>+</sup> *<sup>Q</sup><sup>γ</sup> k*

> > *k*+1|*k*

<sup>1</sup> , *<sup>λ</sup><sup>γ</sup>*

<sup>1</sup> , *<sup>λ</sup><sup>γ</sup>*

*<sup>k</sup>* , avoiding taking decisions on transient state, while considerable

. (15)

<sup>2</sup> ) < *λmax*

<sup>2</sup> ) ≥ *λmax*

*<sup>k</sup>* can be set such

(14)

where

$$\begin{aligned} A\_s &= \begin{bmatrix} A\_l & 0\_{4 \times 2} \\ 0\_{2 \times 4} & 0\_{2 \times 2} \end{bmatrix}, \; f\_{\text{us}} = \begin{bmatrix} f\_u \\ 0\_{2 \times 1} \end{bmatrix}, \; f\_s = \begin{bmatrix} f \\ 0\_{2 \times 1} \end{bmatrix}, \\\ E\_s &= \begin{bmatrix} 0\_{4 \times 4} & E \\ 0\_{2 \times 4} & 0\_{2 \times 2} \end{bmatrix}, \; w\_{\text{s}} = \begin{bmatrix} w^x \\ w^{\gamma} \end{bmatrix} \; \mathbb{C}\_{\text{s}} = \begin{bmatrix} \mathbb{C}\_h \; 0\_{3 \times 2} \end{bmatrix}. \end{aligned}$$

The discrete time representation of 6 follows

$$\mathbf{s}\_{k+1} = A\_{\rm sk}(\mathbf{s}\_k)\mathbf{s}\_k + f\_{\rm usk}(\mathbf{s}\_k, \mathbf{u}\_k) + f\_{\rm sk}(\mathbf{s}\_k, \mathbf{u}\_k, \mathbf{f}\_{\mathbf{v}k}) + E\_{\rm sk}(\mathbf{f}\_{\mathbf{v}k})\mathbf{s}\_k + w\_{\rm sk} \tag{7}$$
 
$$\mathbf{s}\_{\bullet} = \dots$$

$$y\_{k+1} = \mathbb{C}\_{\mathbf{s}} s\_{k+1} + v\_{k+1} \nu$$

where *β<sup>k</sup>* represents the discrete time equivalent vector, or matrix, *β* at time *tk*. We assume that the process noise *wsk* and the output noise *vk* are uncorrelated, i.e., *<sup>E</sup>*{*wskv<sup>T</sup> <sup>k</sup>* } = 0. The autocorrelation of the process noise and of the output are respectively given by

$$E\{w\_{sk}w\_{sk}^T\} = \mathcal{Q}\_k = \begin{bmatrix} \mathcal{Q}\_k^\mathbf{x} & \mathbf{0} \\ \mathbf{0} & \mathcal{Q}\_k^\mathbf{y} \end{bmatrix}, \quad E\{v\_kv\_k^T\} = \mathcal{R}\_k. \tag{8}$$

The formulation of a Kalman filter assumes the use of a model of the process which is a mathematical representation of the dynamics. However, the mathematical translation of the dynamics of a given system may be inaccurate or may not describe entirely its behavior. This is the case in hydrodynamics, where the models are complex, difficult to extract. Moreover, there is no complete theory that allows for determining an accurate model and calculations of parameters mostly rely on empirical or semi-empirical formulas.

Hence, we define *β*ˆ as the estimate of the generic vector, or matrix, *β*. The augmented state extended Kalman filter formulation follows now directly from Gelb (1974). During the prediction stage, the state estimate and the covariance matrix evolve according to

$$\mathfrak{s}\_{\mathbf{k}+1|\mathbf{k}} = \hat{A}\_{\mathfrak{sl}}(\mathfrak{s}\_{\mathbf{k}})\mathfrak{s}\_{\mathbf{k}} + \hat{f}\_{\text{usk}}(\mathfrak{s}\_{\mathbf{k}\prime}\mathfrak{a}\_{\mathbf{k}}) + \hat{f}\_{\text{sk}}(\mathfrak{s}\_{\mathbf{k}\prime}\mathfrak{a}\_{\mathbf{k}\prime}\mathfrak{f}\_{\mathbf{v}\cdot\mathbf{k}}) + \hat{E}\_{\mathfrak{sl}}(\mathfrak{f}\_{\mathbf{v}\cdot\mathbf{k}})\mathfrak{s}\_{\mathbf{k}} \tag{9}$$

$$P\_{k+1|k} = F\_k P\_k F\_k^T + Q\_{k'} \tag{10}$$

.

where *Fk* stands for the Jacobian of *s*˙ evaluated at *s*ˆ*k*:

$$F\_k = \frac{\partial \dot{\mathbf{s}}}{\partial \mathbf{s}}\_{|\mathbf{s} = \mathbf{s}\_k}$$

The so-called Kalman gain and the updates of the estimate and of the covariance matrix are respectively given by

$$K\_{k+1} = P\_{k+1|k} \mathbb{C}\_s^T \left( \mathbb{C}\_s P\_{k+1|k} \mathbb{C}\_s^T + R\_k \right)^{-1} \tag{11}$$

$$
\hat{s}\_{k+1|k+1} = s\_{k+1|k} + K\_{k+1}(y\_{k+1} - \mathbb{C}\_s s\_{k+1|k}) \tag{12}
$$

$$P\_{k+1\mid k+1} = (I - K\_{k+1}C\_s)P\_{k+1\mid k}.\tag{13}$$

From the state estimate, it is now possible to extract the vector *γk*, whose entries constitute the base to determine whether a fault has occured or not.

As it was stated earlier, the autocorrelation matrix *Q<sup>γ</sup> <sup>k</sup>* can play a significant role to avoid divergence or guarantee faster convergence of the estimate of the loss of control effectiveness 8 Will-be-set-by-IN-TECH

 *fu* 02×<sup>1</sup>

 *wx w<sup>γ</sup>* 

*sk*<sup>+</sup><sup>1</sup> = *Ask*(*sk*)*sk* + *fusk*(*sk*, *uk*) + *fsk*(*sk*, *uk*,**fv***k*) + *Esk*(**fv***k*)*sk* + *wsk*

We assume that the process noise *wsk* and the output noise *vk* are uncorrelated, i.e.,

The formulation of a Kalman filter assumes the use of a model of the process which is a mathematical representation of the dynamics. However, the mathematical translation of the dynamics of a given system may be inaccurate or may not describe entirely its behavior. This is the case in hydrodynamics, where the models are complex, difficult to extract. Moreover, there is no complete theory that allows for determining an accurate model and calculations of

Hence, we define *β*ˆ as the estimate of the generic vector, or matrix, *β*. The augmented state extended Kalman filter formulation follows now directly from Gelb (1974). During the

*fusk*(*s*ˆ*k*, *<sup>u</sup>*ˆ*k*) + <sup>ˆ</sup>

*Fk* <sup>=</sup> *<sup>∂</sup>s*˙

*∂s*|*s*=*s*ˆ*<sup>k</sup>* .

*<sup>s</sup>* (*CsPk*<sup>+</sup>1|*kC<sup>T</sup>*

The so-called Kalman gain and the updates of the estimate and of the covariance matrix are

From the state estimate, it is now possible to extract the vector *γk*, whose entries constitute the

divergence or guarantee faster convergence of the estimate of the loss of control effectiveness

prediction stage, the state estimate and the covariance matrix evolve according to

 *Qx <sup>k</sup>* 0 0 *Q<sup>γ</sup> k* 

*<sup>k</sup>* } = 0. The autocorrelation of the process noise and of the output are respectively

 , *fs* =

*Cs* =

, *<sup>E</sup>*{*vkv<sup>T</sup>*

*fsk*(*s*ˆ*k*, *u*ˆ*k*,**fv***k*) + *E*ˆ

*<sup>s</sup>*ˆ*k*+1|*k*+<sup>1</sup> <sup>=</sup> *sk*<sup>+</sup>1|*<sup>k</sup>* <sup>+</sup> *Kk*<sup>+</sup>1(*yk*<sup>+</sup><sup>1</sup> <sup>−</sup> *Cssk*<sup>+</sup>1|*k*) (12) *Pk*<sup>+</sup>1|*k*+<sup>1</sup> = (*<sup>I</sup>* <sup>−</sup> *Kk*<sup>+</sup>1*Cs*)*Pk*<sup>+</sup>1|*k*. (13)

*<sup>k</sup>* + *Qk*, (10)

 *f* 02×<sup>1</sup>

*Ch* 03×<sup>2</sup> .

 ,

*<sup>k</sup>* } = *Rk*. (8)

*sk*<sup>ˆ</sup> (**fv***k*)*s*ˆ*<sup>k</sup>* (9)

*<sup>s</sup>* + *Rk*)−<sup>1</sup> (11)

*<sup>k</sup>* can play a significant role to avoid

(7)

, *fus* =

, *ws* =

where

*<sup>E</sup>*{*wskv<sup>T</sup>*

given by

*As* =

*Es* = 

*yk*<sup>+</sup><sup>1</sup> = *Cssk*<sup>+</sup><sup>1</sup> + *vk*+1,

*<sup>E</sup>*{*wskw<sup>T</sup>*

*<sup>s</sup>*ˆ*k*+1|*<sup>k</sup>* <sup>=</sup> *<sup>A</sup>*<sup>ˆ</sup>

respectively given by

*Pk*<sup>+</sup>1|*<sup>k</sup>* <sup>=</sup> *FkPkF<sup>T</sup>*

where *Fk* stands for the Jacobian of *s*˙ evaluated at *s*ˆ*k*:

base to determine whether a fault has occured or not. As it was stated earlier, the autocorrelation matrix *Q<sup>γ</sup>*

The discrete time representation of 6 follows

 *Al* <sup>04</sup>×<sup>2</sup> 02×<sup>4</sup> 02×<sup>2</sup>

04×<sup>4</sup> *E* 02×<sup>4</sup> 02×<sup>2</sup> where *β<sup>k</sup>* represents the discrete time equivalent vector, or matrix, *β* at time *tk*.

*sk*} = *Qk* =

parameters mostly rely on empirical or semi-empirical formulas.

*sk*<sup>ˆ</sup> (*s*ˆ*k*)*s*ˆ*<sup>k</sup>* + <sup>ˆ</sup>

*Kk*<sup>+</sup><sup>1</sup> <sup>=</sup> *Pk*<sup>+</sup>1|*kC<sup>T</sup>*

factors. For the sake of clarity, from 10 and 13 we can decompose *Pk* as

$$P\_k = \begin{bmatrix} P\_k^{\chi} & P\_k^{\gamma \times} \\ P\_k^{x \gamma} & P\_k^{\gamma} \end{bmatrix}$$

Thus, from 10 and 13, we can conclude

$$P\_{k+1|k}^{\gamma} = P\_{k|k}^{\gamma} + Q\_k^{\gamma}$$

$$P\_{k+1|k+1}^{\gamma} \le P\_{k+1|k}^{\gamma}$$

where we used the fact that *Pk*<sup>+</sup>1|*<sup>k</sup>* <sup>&</sup>gt; 0. Hence, the autocorrelation matrix *<sup>Q</sup><sup>γ</sup> <sup>k</sup>* can be set such that *P<sup>γ</sup>* lies in an interval, preventing state corrections to be excessive, when *P<sup>γ</sup>* is too large, or to be insufficient with slow convergence, when *P<sup>γ</sup>* is too small. Taking the eigenvalues *λ<sup>γ</sup>* of *P<sup>γ</sup>* as measures, we propose the following function

$$Q^{\gamma} = \begin{cases} \operatorname{diag}(q\_{\gamma \prime} q\_{\gamma}) \text{, if } \max(\lambda\_1^{\gamma}, \lambda\_2^{\gamma}) < \lambda\_{\max} \\ 0 & \text{, if } \max(\lambda\_1^{\gamma}, \lambda\_2^{\gamma}) \ge \lambda\_{\max} \end{cases} \tag{14}$$

where *q<sup>γ</sup>* is the autocorrelation of *γi*, *i* = 1, 2 and *λmax* is a preset maximum constant.

#### **3.3 Fault identification**

The loss control effectiveness factors provide an estimate of the performances of the actuators. Ideally, a fault would be identified whenever the absolute value of one of the factors would rise above a preset threshold. However, model uncertainties will be directly reflected in these factors. Even in normal operation, with the actuators working perfectly, the loss of control effectiveness factors may diverge from zero, reflecting, for example, the effect of a damping force greater than the modeled. As these errors are frequently commited on the overall model, their effects are verified on all actuators either by increasing or decreasing the loss of control effectiveness factors. Hence, for the present case, a reasonable measure of the malfunction of one of the thrusters is given by the difference of the corresponding loss of control effectiveness factor estimate. On the other hand, taking a decision about the malfunction of a given thruster should also be based on the confidence of the factor estimate, which can be indirectly taken from the eigenvalues *λ<sup>γ</sup>* of *P<sup>γ</sup> <sup>k</sup>* , avoiding taking decisions on transient state, while considerable corrections on the state are being performed. Thus, we propose the following measure for fault detection:

$$\delta = \frac{|\gamma\_s - \gamma\_b|}{f\_\lambda(\lambda\_1^\gamma + \lambda\_2^\gamma)}.\tag{15}$$

where *f<sup>λ</sup>* is a monotically increasing function of its argument. Whenever *δ* is greater than a preset threshold, a fault is detected and the identification is made according to the greater *λ*, i.e., if *γ<sup>s</sup>* > *γ<sup>b</sup>* then the stern thruster is faulty and vice-versa.

#### **4. Control of MARES**

In the presence of a faulty vertical thruster, the reconfiguration of the actuation is required. Otherwise, keeping the same actuation will likely lead to instability or to other pratical problems such as thruster dammage or large battery consumption, for example. Therefore, the control law for normal operation could be inadequate and another control law must take

Fig. 4. Depth control by actuating either *fs* or *fb*

*<sup>θ</sup>* <sup>=</sup> *<sup>θ</sup>d*(*ez*) = arcsin

where *ϕ*(·) : **R** → **R** is the continuous infinitely differentiable sigmoid function

take *ς* = 2, which will make *ϕ*(·) equal to the hyperbolic tangent function.

− 1 *u* 

Assuming *zd* sufficiently smooth, *u* > 0 and imposing

the time derivative of the Lyapunov function in 18 results

us define *<sup>ε</sup>* <sup>∈</sup> (0, 1) and take *<sup>μ</sup>* <sup>=</sup> <sup>1</sup>

*V*˙ <sup>1</sup> = *ez* 

*<sup>ϕ</sup>*(*x*) = <sup>2</sup>

virtual control law

Inspired by Singh & Khalil (2005), let us suppose we are able to handle *θ* directly through the

Fault Tolerant Depth Control of the MARES AUV 59

− 1 *u* 

which verifies *xϕ*(*x*) > 0, *x* �= 0, and *α*(·) > 0 is a continuous function left to be determined later. Of course, handling *θ* directly and instantaneously is not realistic and such assumption will be lifted next. In opposition to Singh & Khalil (2005), we have selected a sigmoid function *ϕ* instead of a saturated linear function due to the differentiability characteristic. Let us take

*z*˙*<sup>d</sup>* − *α*(*ez*)*ϕ*(

− *α*(*ez*)*ϕ*(

≤ −*ezα*(*ez*)*ϕ*(

tanh−<sup>1</sup> (*ε*) *ez μ* ) 

*ez*

*ez*

where we used the fact that | cos *θ*| ≤ 1 and *w* be bounded. By choosing appropriately *α*(·) and *ϕ*(·), the system can now be made pratically stable (see Singh & Khalil (2005)). Hence, let

*<sup>μ</sup>* ) + *<sup>w</sup>* cos *<sup>θ</sup>*

, then choosing *<sup>α</sup>*(*ez*) = *Kz*, *Kz* <sup>∈</sup> ( *wmax*

*z*˙*<sup>d</sup>* − *α*(*ez*)*ϕ*(

*ez μ* ) 

<sup>1</sup> <sup>+</sup> *<sup>e</sup>*−*ς<sup>x</sup>* <sup>−</sup> 1, (20)

≤ 1, (21)

*<sup>ε</sup>* , *u* + *z*˙*d*], such

*<sup>μ</sup>* ) + <sup>|</sup>*ez*|*wmax*, (22)

, (19)

Fig. 3. Operation of the fault overall fault detection and recovery algorithm

over. In this section, we first start by deriving such controller and present the main concepts behind the derivation of the control for normal operation in order to make the result section clear.

#### **4.1 Control under fault**

We consider now the scenario in which only one of the vertical thrusters is available to control the motion of the vehicle. Under such situation, the heave DOF is no longer controllable but the depth is still controllable by manipulating pitch. Based on the Lyapunov theory we will derive a controller that makes it possible to control the vehicle's depth, while assuming that the absolute value of the surge velocity is sufficiently large to compensate the vehicle's flotation. The derivation of the controller employs the well know backstepping method as well as conditional integrators to achieve asymptotic regulation.

As the final goal in this section is to control the vehicle depth, we will assume that roll angle is null (*φ* = 0), resulting:

$$
\dot{z} = -\mu \sin \theta + w \cos \theta \tag{16}
$$

Let us introduce the error variable *ez* = *z* − *zd*, which we want to drive to zero, and the quadratic Lyapunov function:

$$V\_1 = \frac{1}{2}e\_{z'}^2\tag{17}$$

whose time derivative results

$$
\dot{V}\_1 = \varepsilon\_2 \dot{\varepsilon}\_2 = \varepsilon\_2 (-\mu \sin \theta + w \cos \theta - \dot{z}\_d). \tag{18}
$$

Although *u*, *θ* and *z* are measured by sensors or estimated, it is hard to accurately compute *w* due to model uncertainties and measurement noise. Thus we will assume that it constitutes a disturbance acting on the system, shifting the equilibrium point *ez* = 0 to an uncertain value. Throughout the following developments, we will consider that the surge velocity is maintained constant in order to simplify our approach. Indeed, in most missions the surge velocity is intended to be constant along the trajectory. Moreover, the limited actuation on the vertical thruster makes the pitch angular velocity to lie in a bounded interval. Hence, from the vertical dynamics, we can assume that there exists an upper bound on the absolute value of *w* ∈ [−*wmax*, *wmax*].

10 Will-be-set-by-IN-TECH

over. In this section, we first start by deriving such controller and present the main concepts behind the derivation of the control for normal operation in order to make the result section

We consider now the scenario in which only one of the vertical thrusters is available to control the motion of the vehicle. Under such situation, the heave DOF is no longer controllable but the depth is still controllable by manipulating pitch. Based on the Lyapunov theory we will derive a controller that makes it possible to control the vehicle's depth, while assuming that the absolute value of the surge velocity is sufficiently large to compensate the vehicle's flotation. The derivation of the controller employs the well know backstepping method as

As the final goal in this section is to control the vehicle depth, we will assume that roll angle

Let us introduce the error variable *ez* = *z* − *zd*, which we want to drive to zero, and the

Although *u*, *θ* and *z* are measured by sensors or estimated, it is hard to accurately compute *w* due to model uncertainties and measurement noise. Thus we will assume that it constitutes a disturbance acting on the system, shifting the equilibrium point *ez* = 0 to an uncertain value. Throughout the following developments, we will consider that the surge velocity is maintained constant in order to simplify our approach. Indeed, in most missions the surge velocity is intended to be constant along the trajectory. Moreover, the limited actuation on the vertical thruster makes the pitch angular velocity to lie in a bounded interval. Hence, from the vertical dynamics, we can assume that there exists an upper bound on the absolute value

*<sup>V</sup>*<sup>1</sup> <sup>=</sup> <sup>1</sup> 2 *e* 2

*z*˙ = −*u* sin *θ* + *w* cos *θ* (16)

<sup>1</sup> = *eze*˙*<sup>z</sup>* = *ez*(−*u* sin *θ* + *w* cos *θ* − *z*˙*d*). (18)

*<sup>z</sup>* , (17)

Fig. 3. Operation of the fault overall fault detection and recovery algorithm

well as conditional integrators to achieve asymptotic regulation.

*V*˙

clear.

**4.1 Control under fault**

is null (*φ* = 0), resulting:

quadratic Lyapunov function:

whose time derivative results

of *w* ∈ [−*wmax*, *wmax*].

Fig. 4. Depth control by actuating either *fs* or *fb*

Inspired by Singh & Khalil (2005), let us suppose we are able to handle *θ* directly through the virtual control law

$$\theta = \theta\_d(e\_z) = \arcsin\left[ -\frac{1}{\mu} (\dot{z}\_d - \mathfrak{a}(e\_z)\varrho(\frac{e\_z}{\mu})) \right],\tag{19}$$

where *ϕ*(·) : **R** → **R** is the continuous infinitely differentiable sigmoid function

$$\varphi(\mathbf{x}) = \frac{2}{1 + e^{-\xi \mathbf{x}}} - 1,\tag{20}$$

which verifies *xϕ*(*x*) > 0, *x* �= 0, and *α*(·) > 0 is a continuous function left to be determined later. Of course, handling *θ* directly and instantaneously is not realistic and such assumption will be lifted next. In opposition to Singh & Khalil (2005), we have selected a sigmoid function *ϕ* instead of a saturated linear function due to the differentiability characteristic. Let us take take *ς* = 2, which will make *ϕ*(·) equal to the hyperbolic tangent function. Assuming *zd* sufficiently smooth, *u* > 0 and imposing

$$-\frac{1}{\mu} \left( \dot{z}\_d - a(e\_z)\varphi(\frac{e\_z}{\mu}) \right) \le 1,\tag{21}$$

the time derivative of the Lyapunov function in 18 results

$$\dot{W}\_1 = e\_z \left( -\kappa(e\_z)\varphi(\frac{e\_z}{\mu}) + w\cos\theta \right)$$

$$\leq -e\_z \kappa(e\_z)\varphi(\frac{e\_z}{\mu}) + |e\_z| w\_{\max} \tag{22}$$

where we used the fact that | cos *θ*| ≤ 1 and *w* be bounded. By choosing appropriately *α*(·) and *ϕ*(·), the system can now be made pratically stable (see Singh & Khalil (2005)). Hence, let us define *<sup>ε</sup>* <sup>∈</sup> (0, 1) and take *<sup>μ</sup>* <sup>=</sup> <sup>1</sup> tanh−<sup>1</sup> (*ε*) , then choosing *<sup>α</sup>*(*ez*) = *Kz*, *Kz* <sup>∈</sup> ( *wmax <sup>ε</sup>* , *u* + *z*˙*d*], such

error variable as well as the new augmented Lyapunov function:

<sup>2</sup> + *eq*(*Sqν*˙ − *qd*)

Considering 3, the time derivative results

= *V*˙

⎡ ⎣

decoupled form of *Pi*, we can rewrite 28 as

*fi* = (*SqM*−<sup>1</sup>*Pvi*)−1�

⎤

*<sup>h</sup>* , *<sup>f</sup> <sup>T</sup> i* ]

on the pitch dynamics (see the entries of *Ps* and *Pb*).

the time derivative of the Lyapunov function 28 becomes

⎦ , *Pb* =

⎡ ⎣

Finally, defining the proportional gain *Kq* > 0 and choosing the control law

*V*˙ <sup>3</sup> = *V*˙

actuating with only stern or bow thruster, being different on the entries of *Pvi* only.

The controller was derived using common backstepping with no integral terms.

*e* � *<sup>p</sup>* = � *z* − *zd θ* − *θ<sup>d</sup>*

*V*˙ <sup>3</sup> = *V*˙

*Ps* =

input vector into *fpi* = [ *f <sup>T</sup>*

*V*˙ <sup>3</sup> = *V*˙

**4.2 Control without fault**

respectively given by

*V*<sup>3</sup> = *V*<sup>2</sup> +

Fault Tolerant Depth Control of the MARES AUV 61

where *Pi* and *fpi*, *i* = {*s*, *b*}, are given as functions of the actuator configuration. When the vehicle is operating with only one thruster, either stern or bow thruster, *Pi* and *fpi* are

⎤

Note that *Pi* takes the form *<sup>P</sup>* = [*Ph*|*Pvi*], where *Ph* <sup>∈</sup> **<sup>R</sup>**3×<sup>2</sup> is the submatrix composed by the first two columns of *Pi* and *Pvi* <sup>∈</sup> **<sup>R</sup>**3×<sup>1</sup> is the last column of *Pi*. Further, let us decouple the

and *fi* is the last entry of this latter, which we can manipulate directly. By considering the

Clearly *SqM*−<sup>1</sup>*Ph fh* = 0, which means that the horizontal thrusters have no direct influence

<sup>2</sup> <sup>+</sup> *eq*(*SqM*−1(−*C*(*ν*)*<sup>ν</sup>* <sup>−</sup> *<sup>D</sup>*(*ν*)*<sup>ν</sup>* <sup>−</sup> *<sup>g</sup>*(*η*2) + *Ph fh* <sup>+</sup> *Pvi fi*) <sup>−</sup> *<sup>q</sup>*˙*d*).

*SqM*−1(*C*(*ν*)*<sup>ν</sup>* <sup>+</sup> *<sup>D</sup>*(*ν*)*<sup>ν</sup>* <sup>+</sup> *<sup>g</sup>*(*η*)) + *<sup>q</sup>*˙*<sup>d</sup>* <sup>−</sup> *Kqeq*

<sup>2</sup> − *Kqe* 2

Therefore, the convergence of the error *ez* to zero is then guaranteed by setting the input *fi* according to the control law 29. Note that Equation 29 gives the two control laws for either

Under normal operation, the two through-hull thrusters provide controllability on the heave and the pitch DOFs. We will not give emphasis to the derivation of this controller since it was previously derived in Ferreira, Matos, Cruz & Pinto (2010). We aim at exposing the main concepts that led to the control law, in order to better understand the results of the next section.

In opposition to the previous subsection, the errors considered for the control with the two thrusters are bidimensional vectors. Naturally, the error vector for vertical position comes

> � ,

⎦ , *fps* =

<sup>2</sup> <sup>+</sup> *eq*(*SqM*−1(−*C*(*ν*)*<sup>ν</sup>* <sup>−</sup> *<sup>D</sup>*(*ν*)*<sup>ν</sup>* <sup>−</sup> *<sup>g</sup>*(*η*2) + *Pi fpi*) <sup>−</sup> *<sup>q</sup>*˙*d*). (28)

⎡ ⎣ *fp fr fs*

⎤

*<sup>T</sup>*, where *fh* <sup>∈</sup> **<sup>R</sup>**<sup>2</sup> is composed by the first two entries of *fpi*

⎦ , *fpb* =

*<sup>q</sup>*. (27)

⎡ ⎣ *fp fr fb*

�

*<sup>q</sup>*. (30)

, *i* = {*s*, *b*}, (29)

⎤ ⎦ .

that it satisfies 21, it comes

$$\begin{aligned} \dot{V}\_1 &< 0, \ ||\boldsymbol{e}\_z|| > 1\\ \dot{V}\_1 &\le -\boldsymbol{e}\_{\overline{z}}\boldsymbol{\alpha}(\boldsymbol{e}\_{\overline{z}})\boldsymbol{\varrho}(\frac{\boldsymbol{e}\_z}{\mu}) + |\boldsymbol{e}\_{\overline{z}}|\boldsymbol{w}\_{\max}, \ ||\boldsymbol{e}\_{\overline{z}}|| \le 1. \end{aligned}$$

Thus, the system is made pratically stable and the invariant set for which the error tends to can be made arbitrarily small by handling *μ*. However, a too small *μ* induces chattering phenomena which are intended to be minimal.

Taking into account that *V*<sup>1</sup> is a strictly increasing function of *ez* from the last inequalities, we can state that the error enters a positively invariant set Ω = {*ez* ≤ 1}. However, due to non-null disturbances considered above, asymptotic stability can not be achieved. Therefore, following the same idea as in Singh & Khalil (2005), the conditional integrator is now introduced to obtain asymptotic convergence to the origin *ez* = 0. Modifiyng the control law in 19 to include an integral component, it results

$$\theta\_d(e\_z) = \arcsin\left[-\frac{1}{\mu}(\dot{z}\_d - \mathfrak{a}(e\_z)\varrho(\frac{e\_z + \sigma}{\mu}))\right],\tag{23}$$

where

$$\dot{\sigma} = -\gamma \sigma + \mu \varrho (\frac{e\_z + \sigma}{\mu}), \quad \gamma > 0, \ \sigma(t\_0) = 0.$$

Since <sup>|</sup>*φ*(*x*)<sup>|</sup> <sup>&</sup>lt; 1, <sup>∀</sup>*<sup>x</sup>* <sup>∈</sup> **<sup>R</sup>**, it is easy to check that *<sup>σ</sup>* <sup>≤</sup> *<sup>μ</sup> <sup>γ</sup>* . In order to guarantee convergence to zero, one has to set *γ* and *μ* such that the maximum absolute value of the integral satisfies|*σ*| > *<sup>μ</sup>*|*ϕ*−<sup>1</sup> *wmax Kz* |. Although conservative, this will allow the integral component to compensate the disturbance effect. By applying theorem 1 in Singh & Khalil (2005), convergence to *ez* = 0 as *t* → ∞ is ensured.

So far, we have considered that we are able to handle *θ* directly, which is not true, as it was stated before. Thus, based on the backstepping method Khalil (2002), let us introduce the new error variable *e<sup>θ</sup>* = *θ* − *θ<sup>d</sup>* and the new augmented Lyapunov function as follows

$$V\_2 = V\_1 + \frac{1}{2}e\_{\theta'}^2\tag{24}$$

whose time derivative results

$$
\dot{V}\_2 = \dot{V}\_1 + e\_\theta (\dot{\theta} - \dot{\theta}\_d)\_\prime \tag{25}
$$

with

$$\dot{\theta}\_d = -\frac{\ddot{z}\_d - K\_z \frac{\partial}{\partial \mathbf{f}} \varphi(\frac{\varepsilon\_z + \sigma}{\mu})}{\left(\mu^2 - \left(\dot{z}\_d - K\_z \varphi(\frac{\varepsilon\_z + \sigma}{\mu})\right)^2\right)^{1/2}}.$$

Then by imposing

$$
\dot{\theta} = q\_d = \dot{\theta}\_d - \mathcal{K}\_\theta e\_\theta, \quad \mathcal{K}\_\theta > 0,\tag{26}
$$

the time derivative of the augmented Lyapunov function satisfies *V*˙ <sup>2</sup> <sup>≤</sup> *<sup>V</sup>*˙ <sup>1</sup> <sup>−</sup> *<sup>K</sup><sup>θ</sup> <sup>e</sup>*<sup>2</sup> *<sup>θ</sup>* . Taking into account the previous result about the convergence of *ez* to zero and the fact that *V*<sup>1</sup> is a class K<sup>∞</sup> function, we can deduce that *V*<sup>2</sup> → 0 as *t* → ∞.

Nevertheless, we are not able to handle ˙ *θ* directly and, as it can be seen from 1 and 2, a last step is required. Hence, we define *eq* = *q* − *qd* = *Sqν* − *qd*, with *Sq* = [0, 0, 1], as the pitch rate 12 Will-be-set-by-IN-TECH

Thus, the system is made pratically stable and the invariant set for which the error tends to can be made arbitrarily small by handling *μ*. However, a too small *μ* induces chattering

Taking into account that *V*<sup>1</sup> is a strictly increasing function of *ez* from the last inequalities, we can state that the error enters a positively invariant set Ω = {*ez* ≤ 1}. However, due to non-null disturbances considered above, asymptotic stability can not be achieved. Therefore, following the same idea as in Singh & Khalil (2005), the conditional integrator is now introduced to obtain asymptotic convergence to the origin *ez* = 0. Modifiyng the control

*ez*

*V*˙

<sup>1</sup> < 0, ||*ez*|| > 1

*<sup>μ</sup>* ) + <sup>|</sup>*ez*|*wmax*, ||*ez*|| ≤ 1.

*z*˙*<sup>d</sup>* − *α*(*ez*)*ϕ*(


*<sup>θ</sup>* <sup>−</sup> ˙

*<sup>∂</sup><sup>t</sup> <sup>ϕ</sup>*( *ez*+*<sup>σ</sup> <sup>μ</sup>* )

*<sup>μ</sup>* ) 2 1/2 .

*<sup>z</sup>*˙*<sup>d</sup>* <sup>−</sup> *Kzϕ*( *ez*+*<sup>σ</sup>*

*<sup>μ</sup>* ), *<sup>γ</sup>* <sup>&</sup>gt; 0, *<sup>σ</sup>*(*t*0) = 0.

*ez* + *σ <sup>μ</sup>* ) 

, (23)

*<sup>γ</sup>* . In order to guarantee convergence to

*<sup>θ</sup>* , (24)

*θd*), (25)

*θ<sup>d</sup>* − *K<sup>θ</sup> e<sup>θ</sup>* , *K<sup>θ</sup>* > 0, (26)

*θ* directly and, as it can be seen from 1 and 2, a last

<sup>2</sup> <sup>≤</sup> *<sup>V</sup>*˙

<sup>1</sup> <sup>−</sup> *<sup>K</sup><sup>θ</sup> <sup>e</sup>*<sup>2</sup>

*<sup>θ</sup>* . Taking into

that it satisfies 21, it comes

where

*<sup>μ</sup>*|*ϕ*−<sup>1</sup> *wmax Kz* 

with

as *t* → ∞ is ensured.

Then by imposing

whose time derivative results

*V*˙

law in 19 to include an integral component, it results

Since <sup>|</sup>*φ*(*x*)<sup>|</sup> <sup>&</sup>lt; 1, <sup>∀</sup>*<sup>x</sup>* <sup>∈</sup> **<sup>R</sup>**, it is easy to check that *<sup>σ</sup>* <sup>≤</sup> *<sup>μ</sup>*

˙

K<sup>∞</sup> function, we can deduce that *V*<sup>2</sup> → 0 as *t* → ∞.

Nevertheless, we are not able to handle ˙

*θd*(*ez*) = arcsin

*σ*˙ = −*γσ* + *μϕ*(

 − 1 *u* 

*ez* + *σ*

zero, one has to set *γ* and *μ* such that the maximum absolute value of the integral satisfies|*σ*| >

the disturbance effect. By applying theorem 1 in Singh & Khalil (2005), convergence to *ez* = 0

So far, we have considered that we are able to handle *θ* directly, which is not true, as it was stated before. Thus, based on the backstepping method Khalil (2002), let us introduce the new

<sup>1</sup> + *<sup>e</sup><sup>θ</sup>* ( ˙

account the previous result about the convergence of *ez* to zero and the fact that *V*<sup>1</sup> is a class

step is required. Hence, we define *eq* = *q* − *qd* = *Sqν* − *qd*, with *Sq* = [0, 0, 1], as the pitch rate

*V*<sup>2</sup> = *V*<sup>1</sup> +

error variable *e<sup>θ</sup>* = *θ* − *θ<sup>d</sup>* and the new augmented Lyapunov function as follows

*V*˙ <sup>2</sup> = *V*˙

 *<sup>u</sup>*<sup>2</sup> <sup>−</sup>

˙ *<sup>θ</sup>* = *qd* = ˙

the time derivative of the augmented Lyapunov function satisfies *V*˙

*<sup>θ</sup><sup>d</sup>* <sup>=</sup> <sup>−</sup> *<sup>z</sup>*¨*<sup>d</sup>* <sup>−</sup> *Kz <sup>∂</sup>*

phenomena which are intended to be minimal.

<sup>1</sup> ≤ −*ezα*(*ez*)*ϕ*(

error variable as well as the new augmented Lyapunov function:

$$V\_3 = V\_2 + \frac{1}{2}e\_q^2.\tag{27}$$

Considering 3, the time derivative results

$$\begin{split} \dot{\mathcal{V}}\_{3} &= \dot{\mathcal{V}}\_{2} + \varepsilon\_{q} (\mathbb{S}\_{q} \dot{\boldsymbol{\nu}} - q\_{d}) \\ &= \dot{\mathcal{V}}\_{2} + \varepsilon\_{q} (\mathbb{S}\_{q} \boldsymbol{M}^{-1} (-\mathbb{C}(\boldsymbol{\nu}) \boldsymbol{\nu} - \boldsymbol{D}(\boldsymbol{\nu}) \boldsymbol{\nu} - \boldsymbol{g}(\boldsymbol{\eta}\_{2}) + \boldsymbol{P}\_{i} \boldsymbol{f}\_{pi}) - \dot{\boldsymbol{q}}\_{d}). \end{split} \tag{28}$$

where *Pi* and *fpi*, *i* = {*s*, *b*}, are given as functions of the actuator configuration. When the vehicle is operating with only one thruster, either stern or bow thruster, *Pi* and *fpi* are respectively given by

$$P\_s = \begin{bmatrix} 1 \ 1 & 0 \\ 0 \ 0 & 0 \\ 0 \ 0 \ \mathbf{x}\_{ts} \end{bmatrix} \quad P\_b = \begin{bmatrix} 1 \ 1 & 0 \\ 0 \ 0 & 0 \\ 0 \ 0 \ \mathbf{x}\_{tb} \end{bmatrix} \quad f\_{ps} = \begin{bmatrix} f\_p \\ f\_r \\ f\_s \end{bmatrix} \quad f\_{pb} = \begin{bmatrix} f\_p \\ f\_r \\ f\_b \end{bmatrix}.$$

Note that *Pi* takes the form *<sup>P</sup>* = [*Ph*|*Pvi*], where *Ph* <sup>∈</sup> **<sup>R</sup>**3×<sup>2</sup> is the submatrix composed by the first two columns of *Pi* and *Pvi* <sup>∈</sup> **<sup>R</sup>**3×<sup>1</sup> is the last column of *Pi*. Further, let us decouple the input vector into *fpi* = [ *f <sup>T</sup> <sup>h</sup>* , *<sup>f</sup> <sup>T</sup> i* ] *<sup>T</sup>*, where *fh* <sup>∈</sup> **<sup>R</sup>**<sup>2</sup> is composed by the first two entries of *fpi* and *fi* is the last entry of this latter, which we can manipulate directly. By considering the decoupled form of *Pi*, we can rewrite 28 as

$$
\dot{V}\_3 = \dot{V}\_2 + e\_q(\mathcal{S}\_q M^{-1} (-\mathcal{C}(\nu)\nu - D(\nu)\nu - g(\eta\_2) + P\_h f\_h + P\_{\text{vi}} f\_i) - \dot{q}\_d).
$$

Clearly *SqM*−<sup>1</sup>*Ph fh* = 0, which means that the horizontal thrusters have no direct influence on the pitch dynamics (see the entries of *Ps* and *Pb*).

Finally, defining the proportional gain *Kq* > 0 and choosing the control law

$$f\_i = \left(\mathcal{S}\_q M^{-1} P\_{\bar{v}i}\right)^{-1} \left(\mathcal{S}\_q M^{-1} (\mathcal{C}(\nu)\nu + D(\nu)\nu + \mathcal{g}(\eta)) + \dot{q}\_d - \mathcal{K}\_q \mathbf{e}\_q\right), \quad \mathbf{i} = \{\mathbf{s}\_\prime b\}, \tag{29}$$

the time derivative of the Lyapunov function 28 becomes

$$
\dot{V}\_3 = \dot{V}\_2 - K\_q e\_q^2. \tag{30}
$$

Therefore, the convergence of the error *ez* to zero is then guaranteed by setting the input *fi* according to the control law 29. Note that Equation 29 gives the two control laws for either actuating with only stern or bow thruster, being different on the entries of *Pvi* only.

#### **4.2 Control without fault**

Under normal operation, the two through-hull thrusters provide controllability on the heave and the pitch DOFs. We will not give emphasis to the derivation of this controller since it was previously derived in Ferreira, Matos, Cruz & Pinto (2010). We aim at exposing the main concepts that led to the control law, in order to better understand the results of the next section. The controller was derived using common backstepping with no integral terms.

In opposition to the previous subsection, the errors considered for the control with the two thrusters are bidimensional vectors. Naturally, the error vector for vertical position comes

$$e'\_p = \begin{bmatrix} z - z\_d \\ \theta - \theta\_d \end{bmatrix} \prime$$

fact, the vertical thrusters are mainly compensating the difference between the weight and the bouyancy of the vehicle, which is less than 1.5 Newton, well below the maximum actuation (24N in each thruster). Although it is not shown in the graph, the eigenvalues would stabilize

Fault Tolerant Depth Control of the MARES AUV 63

In Fig. 6, the graphs show the evolution of the variables during the operation for which 1.5N and 2.5N of force are opposing the vertical bow and stern thrusters, respectively. It can be seen that the eigenvalues (Fig. 6(c)) reach their "steady stade" after 40 seconds. In this case, the controller compensates the effects of the opposition force by increasing the actuation, which, in turn, makes the observability better than in the previous case (compare Fig. 6(c) with Fig. 5(c)). Regarding the loss of control effectiveness factors, one can observe that the bow thruster is more affected by the added forces than the stern one when compared with Fig. 5(a). This behavior is explained by the fact that the opposition forces were applied asymetrically with

In Fig. 7 and Fig. 8, we present the same variables as in the previous figures, but now with a disturbance occuring at time *t* = 60*s*, approximately. The graphs in Fig. 7 are related to the disturbance applied in the stern thruster while the graphs in Fig. 8 corresponds to the disturbance in the bow thruster. Comparing the results in Fig. 5 and Fig. 6 with those in Fig. 7 and Fig. 8, the occurence of faults in the last two experiences is evident through simple

By comparing the results exposed in Fig. 5(d), 6(d), 7(d), 8(d), we can conclude that setting the threshold *δth* = 7 would adequatly detect the occurence of a fault, while ensuring a sufficiently large margin to avoid false dectections. The choice of such threshold has to be made according to the sensitivity desired for the detections of faults which must be mainly

The identification is made according to the stated before, being the thruster with bigger |*γ*| the one that is identified as faulty. In both situations (Fig. 7(a) and Fig. 8(a)), the faulty thruster is easily identifiable. Still, practical considerations have to be made at this stage: One can observe that *δ* reaches larger values in the initial transient instants due to small eigenvalues, which could lead to false detections (depending on the threshold). To avoid so, a possibility would be integrating the value of *δ* when it is above the threshold, while using a forgetting factor, and defining another threshold for the value of the integral. Nonetheless, the simple method with the threshold defined above would be sufficient for the present work, as it can

On the other hand, the relation between the actuation amplitude and the eigenvalues of the matrix *P<sup>γ</sup>* is now notable in Fig. 7(c) and Fig. 8(c). With the application of the disturbance, the controller increases the actuation and, as a consequence, the observability is made greater

The control laws derived in section 4 were implemented in MARES and tested in real conditions. The results of the operations are shown in this section, being the analysis of the performances of the controllers the main topic. The vehicle missions were programed such that it navigates at a constant depth reference with a given constant orientation. We must highlight that several unconsidered disturbances have acted on the vehicle during operations: The vehicle was subject to more bouyancy than the assumed in the mathematical model; the fedback depth measurement is actually performed in the nose of MARES instead of the vehicle's center of gravity. Such disturbances induce undesired effects on the controllers. However, the following figures show the robustness of our approach. The variables shown in the graphs have the following units correspondence: Depth is expressed in meters, the pitch angle is expressed in radians while the surge velocity is expressed in meters per second.

after some more time of operation.

analysis of Fig. 7(b) and Fig. 8(b).

related to the accuracy of the model.

be seen from the experiments.

(see 7(c) and 8(c)).

**5.2 Control**

respect to the center of gravity of the vehicle.

assuming that *θ*, *θ<sup>d</sup>* � *π*/2. Following the same method as previously, a first Lyapunov function is defined as a quadratic function of the error *ep* and its time derivative is made negative definite by adequately choosing *z*˙ and ˙ *θ* as virtual control variables to achieve asymptotic stability.

A new augmented Lyapunov function is then introduced by adding a quadratic term of the error

$$\mathcal{e}'\_{\vee} = \begin{bmatrix} w - w\_d \\ q - q\_d \end{bmatrix} \wedge$$

and, from the reduced dynamics model 3, the control law for the two vertical thrusters *fpv* = *χ*(*e*� *p*,*e*� *<sup>ν</sup>*, *<sup>u</sup>*, *<sup>η</sup>*2) <sup>∈</sup> **<sup>R</sup>**<sup>2</sup> is determined such that the time derivative of the augmented Lyapunov function is made negative definite.

## **5. Experiments and results**

To validate the method described in the previous sections, several experimental tests were conducted. The results will be presented in a decoupled way in order to facilitate the exposition and the analysis. First, the results obtained from the ASEKF will be exposed for the system under normal operation with induced faults. Then the performances of the controller derived in the previous section will be presented.

#### **5.1 Fault detection**

Several tests were performed in order to verify the behavior of the fault detection and identification algorithm. Under normal operation, we have intentionally induced faults in the thrusters with the aim of analyzing the behavior of our approach. The following graphs expose the evolution of some of the most relevant variables referred in section 3. The model errors and uncertainties were not corrected so that we could observe behaviors similar to those occuring with real faults. Although not explicitly written, the units of the loss of control effectiveness factors, their difference, the eigenvalues of the corresponding covariance matrix as well as the fault measure are dimensionless, while angles and linear distances are expressed in radians and meters, respectively.

The fault in one of the thruster can be simulated by a conteracting force in the same axis of force application, with an opposite direction. The following tests were carried out such that a force with an opposite direction was applied in an axis near the axis of force application along all the operation or along part of it. The fault measure computed and exposed in the graphs below was set, according to 15, equal to

$$\delta = \frac{|\gamma\_s - \gamma\_b|}{\sqrt{\lambda\_1^\gamma + \lambda\_2^\gamma}} \cdot$$

Fig. 5 presents the evolution of the state estimate in normal operation without conteracting forces. It can be seen in Fig. 5(a), however, that the loss of control effectiveness factors are non-null, in opposition to what would be expected. Indeed, such behavior is due to errors of model parameters, neglected dynamics effects, discretization errors as well as linearization in the extended Kalman filter formulation. Nevertheless, it can be seen they are limited and their differences in Fig. 5(b) are confined to a well defined interval. This effect is unavoidable since deviations of the mathematical model will be directly reflected in the loss of control effectiveness factors.

On the other hand, the eigenvalues of the submatrix *P<sup>γ</sup>* (Fig. 5(c)) are monotically increasing along the operation due to the reduced actuation, and consequently poor observability. In 14 Will-be-set-by-IN-TECH

assuming that *θ*, *θ<sup>d</sup>* � *π*/2. Following the same method as previously, a first Lyapunov function is defined as a quadratic function of the error *ep* and its time derivative is made

A new augmented Lyapunov function is then introduced by adding a quadratic term of the

and, from the reduced dynamics model 3, the control law for the two vertical thrusters *fpv* =

To validate the method described in the previous sections, several experimental tests were conducted. The results will be presented in a decoupled way in order to facilitate the exposition and the analysis. First, the results obtained from the ASEKF will be exposed for the system under normal operation with induced faults. Then the performances of the controller

Several tests were performed in order to verify the behavior of the fault detection and identification algorithm. Under normal operation, we have intentionally induced faults in the thrusters with the aim of analyzing the behavior of our approach. The following graphs expose the evolution of some of the most relevant variables referred in section 3. The model errors and uncertainties were not corrected so that we could observe behaviors similar to those occuring with real faults. Although not explicitly written, the units of the loss of control effectiveness factors, their difference, the eigenvalues of the corresponding covariance matrix as well as the fault measure are dimensionless, while angles and linear distances are expressed

The fault in one of the thruster can be simulated by a conteracting force in the same axis of force application, with an opposite direction. The following tests were carried out such that a force with an opposite direction was applied in an axis near the axis of force application along all the operation or along part of it. The fault measure computed and exposed in the graphs

> *<sup>δ</sup>* <sup>=</sup> <sup>|</sup>*γ<sup>s</sup>* <sup>−</sup> *<sup>γ</sup>b*<sup>|</sup> *λγ* <sup>1</sup> <sup>+</sup> *<sup>λ</sup><sup>γ</sup>* 2 .

Fig. 5 presents the evolution of the state estimate in normal operation without conteracting forces. It can be seen in Fig. 5(a), however, that the loss of control effectiveness factors are non-null, in opposition to what would be expected. Indeed, such behavior is due to errors of model parameters, neglected dynamics effects, discretization errors as well as linearization in the extended Kalman filter formulation. Nevertheless, it can be seen they are limited and their differences in Fig. 5(b) are confined to a well defined interval. This effect is unavoidable since deviations of the mathematical model will be directly reflected in the loss of control

On the other hand, the eigenvalues of the submatrix *P<sup>γ</sup>* (Fig. 5(c)) are monotically increasing along the operation due to the reduced actuation, and consequently poor observability. In

*<sup>ν</sup>*, *<sup>u</sup>*, *<sup>η</sup>*2) <sup>∈</sup> **<sup>R</sup>**<sup>2</sup> is determined such that the time derivative of the augmented Lyapunov

 ,

*e* � *<sup>ν</sup>* = *w* − *wd q* − *qd*

*θ* as virtual control variables to achieve

negative definite by adequately choosing *z*˙ and ˙

derived in the previous section will be presented.

asymptotic stability.

function is made negative definite.

in radians and meters, respectively.

below was set, according to 15, equal to

**5. Experiments and results**

**5.1 Fault detection**

effectiveness factors.

error

*χ*(*e*� *p*,*e*� fact, the vertical thrusters are mainly compensating the difference between the weight and the bouyancy of the vehicle, which is less than 1.5 Newton, well below the maximum actuation (24N in each thruster). Although it is not shown in the graph, the eigenvalues would stabilize after some more time of operation.

In Fig. 6, the graphs show the evolution of the variables during the operation for which 1.5N and 2.5N of force are opposing the vertical bow and stern thrusters, respectively. It can be seen that the eigenvalues (Fig. 6(c)) reach their "steady stade" after 40 seconds. In this case, the controller compensates the effects of the opposition force by increasing the actuation, which, in turn, makes the observability better than in the previous case (compare Fig. 6(c) with Fig. 5(c)). Regarding the loss of control effectiveness factors, one can observe that the bow thruster is more affected by the added forces than the stern one when compared with Fig. 5(a). This behavior is explained by the fact that the opposition forces were applied asymetrically with respect to the center of gravity of the vehicle.

In Fig. 7 and Fig. 8, we present the same variables as in the previous figures, but now with a disturbance occuring at time *t* = 60*s*, approximately. The graphs in Fig. 7 are related to the disturbance applied in the stern thruster while the graphs in Fig. 8 corresponds to the disturbance in the bow thruster. Comparing the results in Fig. 5 and Fig. 6 with those in Fig. 7 and Fig. 8, the occurence of faults in the last two experiences is evident through simple analysis of Fig. 7(b) and Fig. 8(b).

By comparing the results exposed in Fig. 5(d), 6(d), 7(d), 8(d), we can conclude that setting the threshold *δth* = 7 would adequatly detect the occurence of a fault, while ensuring a sufficiently large margin to avoid false dectections. The choice of such threshold has to be made according to the sensitivity desired for the detections of faults which must be mainly related to the accuracy of the model.

The identification is made according to the stated before, being the thruster with bigger |*γ*| the one that is identified as faulty. In both situations (Fig. 7(a) and Fig. 8(a)), the faulty thruster is easily identifiable. Still, practical considerations have to be made at this stage: One can observe that *δ* reaches larger values in the initial transient instants due to small eigenvalues, which could lead to false detections (depending on the threshold). To avoid so, a possibility would be integrating the value of *δ* when it is above the threshold, while using a forgetting factor, and defining another threshold for the value of the integral. Nonetheless, the simple method with the threshold defined above would be sufficient for the present work, as it can be seen from the experiments.

On the other hand, the relation between the actuation amplitude and the eigenvalues of the matrix *P<sup>γ</sup>* is now notable in Fig. 7(c) and Fig. 8(c). With the application of the disturbance, the controller increases the actuation and, as a consequence, the observability is made greater (see 7(c) and 8(c)).

#### **5.2 Control**

The control laws derived in section 4 were implemented in MARES and tested in real conditions. The results of the operations are shown in this section, being the analysis of the performances of the controllers the main topic. The vehicle missions were programed such that it navigates at a constant depth reference with a given constant orientation. We must highlight that several unconsidered disturbances have acted on the vehicle during operations: The vehicle was subject to more bouyancy than the assumed in the mathematical model; the fedback depth measurement is actually performed in the nose of MARES instead of the vehicle's center of gravity. Such disturbances induce undesired effects on the controllers. However, the following figures show the robustness of our approach. The variables shown in the graphs have the following units correspondence: Depth is expressed in meters, the pitch angle is expressed in radians while the surge velocity is expressed in meters per second.

(a) Control effectiveness factors (b) Difference of control effectiveness factors

Fault Tolerant Depth Control of the MARES AUV 65

(c) Eigenvalues (d) Fault measure

(e) Depth (f) Pitch angle

Fig. 6. MARES with constant disturbances: bouyancy added in the nose and the tail

asymetrically

Fig. 5. MARES under normal operation (no faults)

16 Will-be-set-by-IN-TECH

(a) Control effectiveness factors (b) Difference of control effectiveness factors

(c) Eigenvalues (d) Fault measure

(e) Depth (f) Pitch angle

Fig. 5. MARES under normal operation (no faults)

Fig. 6. MARES with constant disturbances: bouyancy added in the nose and the tail asymetrically

(a) Control effectiveness factors (b) Difference of control effectiveness factors

Fault Tolerant Depth Control of the MARES AUV 67

(c) Eigenvalues (d) Fault measure

(e) Depth (f) Pitch angle

Fig. 8. MARES with disturbance in the bow thruster at *t* = 60*s* approximately

Fig. 7. MARES with disturbance in the stern thruster at *t* = 60*s* approximately

18 Will-be-set-by-IN-TECH

(a) Control effectiveness factors (b) Difference of control effectiveness factors

(c) Eigenvalues (d) Fault measure

(e) Depth (f) Pitch angle

Fig. 7. MARES with disturbance in the stern thruster at *t* = 60*s* approximately

Fig. 8. MARES with disturbance in the bow thruster at *t* = 60*s* approximately

and one of the controllers for degraded mode with only one vertical thruster starts operating.

Fault Tolerant Depth Control of the MARES AUV 69

The Fig. 9 shows the variables directly measured from the sensors for a mission with a depth reference *zd* = 0.7m, only with bow thruster. One can verify that the depth (9(a)) is reasonably close the reference and the small oscillation is due to natural disturbances that the vehicle

(a) Depth

(b) Pitch angle

(c) Surge velocity

Fig. 10. MARES controlling depth with bow and horizontal thrusters only (*zd* = 0.6 m)

The figures show the results for different surge velocities and for the two controllers.

Figures 9 to 11 show the results obtained during the operation of the controllers. The missions were set so that the vehicle starts diving with the two vertical thrusters simultaneously controlling pitch and depth, with surge velocity *u* = 0. At time *t* = 20*s*, a fault is simulated

Fig. 9. MARES controlling depth with bow and horizontal thrusters only (*zd* = 0.7 m)

20 Will-be-set-by-IN-TECH

Figures 9 to 11 show the results obtained during the operation of the controllers. The missions were set so that the vehicle starts diving with the two vertical thrusters simultaneously controlling pitch and depth, with surge velocity *u* = 0. At time *t* = 20*s*, a fault is simulated

(a) Depth

(b) Pitch angle

(c) Surge velocity

Fig. 9. MARES controlling depth with bow and horizontal thrusters only (*zd* = 0.7 m)

and one of the controllers for degraded mode with only one vertical thruster starts operating. The figures show the results for different surge velocities and for the two controllers.

The Fig. 9 shows the variables directly measured from the sensors for a mission with a depth reference *zd* = 0.7m, only with bow thruster. One can verify that the depth (9(a)) is reasonably close the reference and the small oscillation is due to natural disturbances that the vehicle

Fig. 10. MARES controlling depth with bow and horizontal thrusters only (*zd* = 0.6 m)

more actuation is needed for the same pitch angle, in steady state. Such behavior is originated by the smaller distance at which the stern thruster is placed relatively to the center of gravity,

Fault Tolerant Depth Control of the MARES AUV 71

Figures 9 to 11 show the robustness of the control laws, independently of the surge velocity. Even in the presence of unconsidered disturbances naturally induced by the environment, the graphs 9(a), 10(a) and 11(a) demonstrate that the controllers provide satisfactory results in real

In this work, we have presented a complete method for fault detection, identification and automatic reconfiguration of the MARES AUV. Although we have focused on this particular system, the concepts and ideas can easily be extended to other problems, even beyond

Based on the dynamics and kinematics models, we have formulated a filter to estimate possible actuation biases. The augmented state extended Kalman filter was chosen to handle the problem, contemplating a reduced order model to simplify the analysis and the formulation. Bias variables were introduced in the state to be estimated as loss of control effectiveness factors whose values reflect the commanded underactuation/overactuation. Along with these estimated variables, the eigenvalues of the corresponding submatrix in the state estimate covariance matrix were taken to define a fault measure. Such measure was then used to generate a fault detection warning through comparison of its value with a given threshold. Finally, the faulty thruster is identified through analysis of the biases amplitude. When a fault occurs and the corresponding thruster is set off, a suitable control law has to take over to ensure that the on-going mission succeed. To achieve so, we have defined two control laws for which we have based the derivation on Lyapunov theory and on backstepping method and further applied conditional integrators in order to drive the vehicle depth to a

At last, we demonstrated the performances of the developed method through real experiments in which we verified the operation of both estimator and controllers. Even in the presence of unconsidered disturbances, naturally induced by the environment, we have demonstrated that the controllers provide satisfactory results for several surge velocities and

Bruno Ferreira was supported by the Portuguese Foundation for Science and Technology

Cruz, N. A. & Matos, A. C. (2008). The MARES auv, a modular autonomous robot

Ferreira, B., Matos, A., Cruz, N. & IEEE (2010). Single beacon navigation: Localization and control of the mares auv, *Oceans 2010*, Oceans-IEEE, IEEE, New York. Ferreira, B., Matos, A., Cruz, N. & Pinto, M. (2010). Modeling and control of the MARES autonomous underwater vehicle, *Marine Technology Society Journal* 44(2): 19–36.

Frank, P. M. & Ding, X. (1997). Survey of robust residual generation and evaluation methods in observer-based fault detection systems, *Journal of Process Control* 7(6): 403–424.

Fossen, T. I. (1994). *Guidance and Control of Ocean Vehicles*, John Whiley & Sons Ltd.

for environment sampling, *Oceans 2008, Vols 1-4*, Oceans-IEEE, IEEE, New York,

comparing to the bow thruster (see Table 1).

given reference with a null error in steady state.

through the Ph.D. grant SFRH/BD/60522/2009.

different thruster configurations.

pp. 1996–2001.

**7. Acknowledgement**

**8. References**

operation.

robotics.

**6. Conclusions**

founds in practical operations. Moreover, the commanded forces are affected by delays and thruster model is subject to uncertainties, which certainly influence the behavior. On the other hand, the Fig. 11(a) shows that the oscillation amplitude is bigger than in Fig. 9(a) and 9(a), having been induced by more disturbances. Moreover, for stern-only control,

Fig. 11. MARES controlling depth with stern and horizontal thrusters only (*zd* = 0.8 m)

more actuation is needed for the same pitch angle, in steady state. Such behavior is originated by the smaller distance at which the stern thruster is placed relatively to the center of gravity, comparing to the bow thruster (see Table 1).

Figures 9 to 11 show the robustness of the control laws, independently of the surge velocity. Even in the presence of unconsidered disturbances naturally induced by the environment, the graphs 9(a), 10(a) and 11(a) demonstrate that the controllers provide satisfactory results in real operation.

## **6. Conclusions**

22 Will-be-set-by-IN-TECH

founds in practical operations. Moreover, the commanded forces are affected by delays and

On the other hand, the Fig. 11(a) shows that the oscillation amplitude is bigger than in Fig. 9(a) and 9(a), having been induced by more disturbances. Moreover, for stern-only control,

(a) Depth

(b) Pitch angle

(c) Surge velocity

Fig. 11. MARES controlling depth with stern and horizontal thrusters only (*zd* = 0.8 m)

thruster model is subject to uncertainties, which certainly influence the behavior.

In this work, we have presented a complete method for fault detection, identification and automatic reconfiguration of the MARES AUV. Although we have focused on this particular system, the concepts and ideas can easily be extended to other problems, even beyond robotics.

Based on the dynamics and kinematics models, we have formulated a filter to estimate possible actuation biases. The augmented state extended Kalman filter was chosen to handle the problem, contemplating a reduced order model to simplify the analysis and the formulation. Bias variables were introduced in the state to be estimated as loss of control effectiveness factors whose values reflect the commanded underactuation/overactuation. Along with these estimated variables, the eigenvalues of the corresponding submatrix in the state estimate covariance matrix were taken to define a fault measure. Such measure was then used to generate a fault detection warning through comparison of its value with a given threshold. Finally, the faulty thruster is identified through analysis of the biases amplitude.

When a fault occurs and the corresponding thruster is set off, a suitable control law has to take over to ensure that the on-going mission succeed. To achieve so, we have defined two control laws for which we have based the derivation on Lyapunov theory and on backstepping method and further applied conditional integrators in order to drive the vehicle depth to a given reference with a null error in steady state.

At last, we demonstrated the performances of the developed method through real experiments in which we verified the operation of both estimator and controllers. Even in the presence of unconsidered disturbances, naturally induced by the environment, we have demonstrated that the controllers provide satisfactory results for several surge velocities and different thruster configurations.

## **7. Acknowledgement**

Bruno Ferreira was supported by the Portuguese Foundation for Science and Technology through the Ph.D. grant SFRH/BD/60522/2009.

## **8. References**


Fossen, T. I. (1994). *Guidance and Control of Ocean Vehicles*, John Whiley & Sons Ltd.

Frank, P. M. & Ding, X. (1997). Survey of robust residual generation and evaluation methods in observer-based fault detection systems, *Journal of Process Control* 7(6): 403–424.

**4**

*Germany*

**Robust Control Design for Automotive**

The steady rise in fuel prices and the increased awareness on climate issues led and still lead to considerable efforts in the development of automotive engines and drivetrains (Guzzella & Sciarretta (2005)). Thus, fuel savings and emission reduction are of general interest and

> **Memory size**

> > **Calibration parameters**

obviously as important as improved riding comfort or driveability.

**1996 2001 2003 2005 2009**

an automotive engine control unit from 1996 to 2009 (ETAS GmbH (2010))

**Computing power**

Fig. 1. Evolution of memory size, computing power and number of calibration parameters of

However, it is hard to find a suitable trade-off between all of these requirements and many resulting solutions lead to increased complexity of the vehicle systems. This is in particular true for common automotive combustion engines where the number of free calibration parameters of the corresponding electronic control unit (ECU) software has been increased up to five times during the last fifteen years (see Figure 1). From today's state of the art it takes up to five calibration engineers one whole year to finish all the calibration work on a series-production engine (Reif (2007)). Consequently, this time consuming calibration results in considerable development cost. Since the complexity of future drivetrains (e.g. battery electric vehicles or hybrid electric vehicles) will be drastically increased (Ehsani et al. (2010)) an ongoing rise on development cost is inevitable. However, with this effect cars may become

**1. Introduction**

**45.000 40.000 35.000 30.000 25.000 20.000 15.000 10.000 5.000** **Applications: A Variable Structure**

**Control Approach**

**[MIPS] [MByte]**

Benedikt Alt and Ferdinand Svaricek *University of the German Armed Forces Munich*

Gelb, e. A. (1974). *Optimal applied estimation*, MIT Press.

