This section starts with a summary of the multirotor dynamics model. Subsequently, a specific control scheme is assumed and the stability conditions for the fault-free system are shown analytically. Finally, the fault diagnosis scheme and two control sub-laws are proposed.

#### 3.1. Multirotor Dynamics

The mathematical model of the multirotor is deduced by introducing a world-fixed coordinate system

$\left\{W\right\}$ and a body-fixed coordinate system

$\left\{B\right\}$ (see

Figure 1). By using either the Newton-Euler equations or the Euler-Lagrange formalism, the model of the multirotor can be obtained [

34,

35,

36]. We selected the following model to describe the multirotor unmanned helicopter:

where

${[x,y,z]}^{T}\in {\mathbb{R}}^{3}$ is the position vector from the origin of the body frame to the origin of the world frame,

${[\varphi ,\theta ,\psi ]}^{T}\in {\mathbb{R}}^{3}$ is the vector of Euler angles (roll, pitch, and yaw),

$m\in \mathbb{R}$ is the mass of the vehicle,

$\mathrm{diag}({J}_{x},{J}_{y},{J}_{z})\in {\mathbb{R}}^{3\times 3}$ is the inertia matrix,

$g\in \mathbb{R}$ is the acceleration of gravity and

${[{u}_{z},{u}_{\varphi},{u}_{\theta},{u}_{\psi}]}^{T}\in {\mathbb{R}}^{4}$ is the vector of control inputs.

Assuming that the rotorcraft moves around the hovering state, the model (

1) reduces to:

From (

2), it is clear that the translational axes are decoupled. Thus, the

z-axis dynamic model of the multirotor can be separated as follows:

This model implies that any possible gravity is subsumed under the

${u}_{v}$ term:

where

${u}_{z}\left(t\right)=m[\ddot{z}\left(t\right)+g]$ is the actual upward thrust generated by the rotorcraft in Newtons. In control system theory, it is common to compensate factors such as gravity and the mass into

${u}_{v}$, as the calculation of

${u}_{z}$ is straightforward and does not depend on any state variables.

#### 3.2. Altitude Control

Before developing the fault-free control scheme, the following assumption is required. Define ${z}_{ref}\left(t\right)\in \mathbb{R}$ as the trajectory of reference. This trajectory satisfies that ${\dot{z}}_{ref}\left(t\right)\in \mathbb{R}$ and ${\ddot{z}}_{ref}\left(t\right)\in \mathbb{R}$ exist for all $t\ge 0$. This assumption is reasonable since, in practice, the trajectory that a multirotor can track is limited by the physical attributes of the multirotor.

We follow a cascade control scheme, where a low-level controller is present as the internal loop and a trajectory tracking controller is running as the external loop [

37]. The inner loop controls the velocity at a high frequency and the outer loop controls the position at a low frequency. When there are no sensor faults, the formulation of the latter is:

where

${k}_{p,1}\in \mathbb{R}$ and

${k}_{d,1}\in \mathbb{R}$ are controller parameters to be designed. The reference velocity for the inner loop controller is obtained simply as

${v}_{ref}={u}_{p}$. Then, the formulation of this loop’s controller is: title = Active Fault-Tolerant Control for a Quadrotor with Sensor Faults,

where

${k}_{p,2}\in \mathbb{R}$,

${k}_{i,2}\in \mathbb{R}$, and

${k}_{d,2}\in \mathbb{R}$ are controller parameters and

${v}_{z}\left(t\right)\in \mathbb{R}$ is the

z-axis velocity of the robot.

Define the position and velocity errors as ${e}_{z}={z}_{ref}-z$ and ${e}_{v}={v}_{ref}-{v}_{z}$, respectively. The following two propositions provide the stability conditions for the outer and inner controllers.

**Proposition** **1.** The position error ${e}_{z}$, resulting from the application of the external controller (5) to the model (3), converges asymptotically to zero if ${k}_{p,1}/(1+{k}_{d,1})>0$. **Proof.** Given that the internal loop is faster than the external one, we can regard the model from

${u}_{p}$ to

$\dot{z}$ as a proportion. Therefore, recalling the form of

${e}_{z}$, we have that:

The characteristic equation of (

7) is:

Since

${k}_{p,1}/(1+{k}_{d,1})>0$., the only root of (

8) is negative, which implies that (

7) is stable and

${e}_{z}\left(t\right)$ reaches zero asymptotically. □

**Proposition** **2.** The velocity error ${e}_{v}$, resulting from the application of the internal controller (6) to the model (3), converges asymptotically to zero if ${k}_{p,2}/(1+{k}_{d,2})>0$ and ${k}_{i,2}/(1+{k}_{d,2})>0$. **Proof.** For small time steps, it is reasonable to assume constant acceleration between time steps, which means that

${\ddot{v}}_{ref}\left(t\right)=0$. Then, recalling the form of

${e}_{v}$, we have that:

Then, the system can be rewritten as:

The characteristic equation of (

10) is:

Since

${k}_{p,2}/(1+{k}_{d,2})>0$ and

${k}_{i,2}/(1+{k}_{d,2})>0$, the real parts of the roots of (

11) are negative, which implies that the system (

10) is stable and

${e}_{v}\left(t\right)$ reaches zero asymptotically. □

In addition to the conditions in Propositions 1 and 2, the parameters of the external and internal controllers should be determined according to the real circumstances of the multirotor under consideration. Particularly, it is common to use only PI control in the inner loop of multirotors [

38]. This choice is justified in the stability analysis, where the derivative gain appears, dividing the others, meaning that it slows down the internal closed-loop dynamics.

#### 3.3. Metric Monocular SLAM

External sensors for localization of the drone are not adequate for complex environments. To overcome this restriction, we can use a visual SLAM method. Given a monocular onboard camera, ORB-SLAM2 can be employed to obtain the camera pose and 3D point estimates without metric [

39]. To address the scale problem, assuming planar ground and knowing the camera angle and distance to the ground, we can obtain a synthetic depth image by resolving the ray-ground intersection geometry. This synthetic image can be coupled with incoming RGB images and then fed to the RGB-D version of ORB-SLAM2, which generates metric pose estimates [

2].

Figure 2 illustrates the side view of the ray-ground geometric configuration. Knowing the height above ground of the camera

h and the camera angle

$\alpha $, we can define a vector

n perpendicular to the ground. Therefore, a vector

l departing from the camera’s optical center

$({x}_{0},{y}_{0})$ and passing through a pixel

$(x,y)$ will intersect the planar ground for some scalar

d. The value of

d is obtained with the ray-plane intersection equations:

where

f is the focal length. This approach has been evaluated using the Vicon motion capture system in indoor environments. It was found that the pose error is 2% on average [

2].

Figure 3 shows examples of the system carrying out the metric mapping. In this figure, it is noticed that all map points (red and black, obtained from the synthetic depth map) are on a three dimensional plane, which corresponds to the ground.

The height above ground of the camera can be obtained from the altimeter of the drone. The camera angle can be set a priori either via hardware or via software if its field of view can be foveated. We propose using the metric monocular SLAM system because it offers the following advantages. First, the metric monocular SLAM system is suitable for autonomous navigation, which means that such a system is likely to be already present in the control architecture. Second, if correctly initialized, it is robust against erroneous measurements of altitude (see

Section 4.1). This property holds since the original SLAM algorithm at startup creates a keyframe with the first frame, sets its pose as the origin and creates an initial map from all keypoints with depth information [

39]. The system optimizes the camera pose by finding features matches to the local map. It also optimizes the entire map by keeping the origin keyframe fixed. Therefore, the initial synthetic depth image should be generated with faultless altitude readings, which are easily obtained while hovering far from horizontal and vertical surfaces.

#### 3.4. Fault-tolerant Control

We utilize a fault-tolerant control architecture.

Figure 4 illustrates this architecture applied to our problem. First, we will describe the interior of the fault detection unit. We assume that there are available three measurements related to the altitude of the drone—range-based (

${y}_{r}$), inertial-based (

${y}_{i}$), and vision-based (

${y}_{v}$). The first measurement can be obtained from a combination of a range sensor (ultrasonic, infrared, laser) and a barometer. The second is commonly obtained from the IMU. The last is from the vision algorithms which use an onboard camera. These measurements relate to the state as:

in which

$\delta \left(t\right)\in \mathbb{R}$ is the sensor fault. Let us consider

$r\left(t\right)\in {\mathbb{R}}^{2}$ as the residual vector defined as follows:

Next, we define the residual evaluation function

${J}_{eval}\left(t\right)\in \mathbb{R}$ with the formulation below:

where

$W\in {\mathbb{R}}^{2\times 2}$ is a real symmetric positive definite weighting matrix. In the literature, the fault diagnosis relies only on the norm of the residual [

40]. We propose the weighting matrix to select the relative importance of the different components of the residual vector. Thus, the fault detection unit is operated by a logical process:

where

${J}_{th}\in \mathbb{R}$ is the threshold of the fault detection unit and its value can be designed according to the experience of experts, considering uncertainties and disturbances present in the system.

So far, the detection or diagnosis scheme has been presented. In what follows, we will describe the general form of the fault-tolerant control law. Then, we will derive two control sub-laws that will be activated when a sensor fault is identified; the first deals with the sensor fault directly and the second deals with it as an actuator fault.

In general, the objective is to find a fault-tolerant control law for the external loop like the following equation:

Inserting the measurement,

${u}_{p}^{n}$ will always remain as:

The idea is to design

${u}_{p}^{f}$ such that the multirotor stays close to the trajectory of the reference whenever a sensor fault is identified. If the magnitude of the sensor fault is small, the fault will not be detected by our fault detection unit. In this case, the control law (

18) still guarantee the stability of the multirotor though the tracking error will not stay close to zero.

For the first control sub-law, suppose the inner loop uses a measurement with the form of the range sensor measurement in (

13):

For compensating disturbances produced by sensor faults of the inner loop, we will design an additive term

${u}_{a}\in \mathbb{R}$ for the outer loop. Taking into account the sensor fault and the additive term,

${u}_{v}$ becomes:

From the previous equation, it is clear that

${u}_{a}$ should be designed to be equal to

$\dot{\delta}$. However, the time derivative of

$\delta $ can be computed either with respect to the external loop or to the internal loop. To justify the selection, we will analyze the perturbation term

$p={u}_{a}-\dot{\delta}$. Let

${T}_{p}$ and

${T}_{v}$ be the sampling periods of the external and internal loops, respectively. We assume that

$\mu ={T}_{p}/{T}_{v}\in {\mathbb{Z}}^{+}$. In discrete form, after one step of

${T}_{p}$, it yields:

where the subscript of

$\delta $ denote the discrete-time index, in essence,

${\delta}_{k}=\delta \left(k{T}_{v}\right)$. The expansion of the summation results in:

The last form of

p implies that the additive term should be

${u}_{a}=({\delta}_{k}-{\delta}_{k-\mu})/{T}_{p}$. This means that internal sensor faults can be countered with information available at the time-steps of the external loop, no matter the values in between (at the cost of an approximation error). This is useful if the external loop receives little information about the internal one, which is likely the case when working with off-the-shelf drones. In reality,

$\delta $ cannot be measured directly. For this reason, we propose the following expression:

If both loops were to run at the same frequency, we would have nearly total fault cancellation with

${k}_{a}=1$ since

${r}_{2}\approx \dot{\delta}$. However, it is important to remember that the inner loop executes faster than the outer loop, so

${u}_{a}$ remains constant until a new outer loop command is computed. In this case, choosing unitary

${k}_{a}$ will result in an overcompensation of the fault. Moreover, the value of

${r}_{2}$ could be contaminated with noise. Therefore, we propose the range of this parameter to be

$0<{k}_{a}<1$. Furthermore, this parameter could take different values for positive and negative disturbances,

${k}_{a}=\{{k}_{a}^{+},{k}_{a}^{-}\}$. In the end, the first control sub-law is given by following equation:

Some literature has examined converting sensor faults into actuator faults through a transformation [

41,

42]. For comparison purposes, we consider sensor faults as actuator faults or disturbances and propose a second control sub-law based on Sliding Mode Control (SMC). Before going into detail, some remarks are in order. The time response of the internal loop is much smaller than that of the external loop, which means that the control of velocity is faster than that of the position. Therefore, we can neglect the response time of the velocity control and consider the model from

${u}_{v}$ to

$\ddot{z}$ as a proportion. Thence, the model from

${u}_{p}$ to

$\ddot{z}$ can be regarded as a double integrator. Also, we propose to activate the SMC only when a fault is detected, so

${u}_{p}\to {u}_{p}^{f}$. Considering disturbances or the actuator fault, the model of the

z axis results in:

where

$c\left(t\right)$ is a composite disturbance term. We assume that

$\left|c\right(t\left)\right|\le C$ for some known

$C>0$. Define the sliding surface

s with:

The second control sub-law becomes:

**Proposition** **3.** The control (27) drives the position error to the sliding surface (26) and keeps the error on the surface thereafter in the presence of the bounded disturbance $c\left(t\right)$. **Proof.** Recalling the position error

${e}_{z}$, it follows that:

Let us select a candidate Lyapunov function as:

Now, let us take the derivative of

V as in the usual Lyapunov method:

Taking the absolute value of the second term and considering the disturbance bound, it results in:

Suppose ${k}_{s}$ is chosen appropriately (${k}_{s}>C$), it yields that $\dot{V}<0$ for $s\ne 0$. Therefore, the region $s=0$ must be invariant. □

Throughout the next section, we will refer as (Fault-Tolerant Controller) FTC 1 to the strategy that adopts (

24), and as FTC 2 to the one that uses (

27).