IMU Sensor Fault Diagnosis and Estimation for Quadrotor UAVs

9th IFAC Symposium on Fault Detection, Supervision and 9th IFAC on Fault Detection, Supervision and Safety of Symposium Technical Processes 9th IFAC IFAC Symposium on Fault Fault Detection, Detection, Supervision Supervision and and 9th on Safety of Symposium Technical Processes Available online at www.sciencedirect.com September 2-4, 2015. Arts et Métiers ParisTech, Paris, France Safety of Technical Processes Safety of Technical Processes September 2-4, 2015. Arts et Métiers ParisTech, Paris, France September September 2-4, 2-4, 2015. 2015. Arts Arts et et Métiers Métiers ParisTech, ParisTech, Paris, Paris, France France

ScienceDirect

IFAC-PapersOnLine 48-21 (2015) 380–385

IMU Sensor Fault Diagnosis and IMU IMU Sensor Sensor Fault Fault Diagnosis Diagnosis and and Estimation for Quadrotor UAVs Estimation for Quadrotor UAVs Estimation for Quadrotor UAVs

∗∗∗ Avram ∗∗ Xiaodong Zhang ∗∗ ∗∗ Jacob Campbell ∗∗∗ Avram ∗ Xiaodong Zhang ∗∗ Jacob Campbell ∗∗∗ ∗∗∗ ∗∗∗∗ ∗ ∗∗ Avram Xiaodong Zhang Jacob Campbell Avram Jonathan Xiaodong Muse Zhang∗∗∗∗ Jacob Campbell Jonathan Muse ∗∗∗∗ ∗∗∗∗ Jonathan Muse Jonathan Muse ∗ ∗ Wright State University, Dayton, OH 45435, USA State University, Dayton, OH 45435, USA ∗ ∗ Wright Wright University, (e-mail:[email protected]) Wright State State University, Dayton, Dayton, OH OH 45435, 45435, USA USA (e-mail:[email protected]) ∗∗ (e-mail:[email protected]) University, Dayton, OH 45435, USA (e-mail:[email protected]) ∗∗ Wright State Wright State University, Dayton, OH 45435, USA ∗∗ ∗∗ Wright State Wright(e-mail:[email protected]) State University, University, Dayton, Dayton, OH OH 45435, 45435, USA USA (e-mail:[email protected]) ∗∗∗ Research Laboratory, WPAFB, OH 45433, USA (e-mail:[email protected]) ∗∗∗ Air Force(e-mail:[email protected]) Force Research Laboratory, WPAFB, OH 45433, USA ∗∗∗ ∗∗∗ Air Air Research Air Force Force (e-mail:[email protected]) Research Laboratory, Laboratory, WPAFB, WPAFB, OH OH 45433, 45433, USA USA (e-mail:[email protected]) ∗∗∗∗ (e-mail:[email protected]) Research Laboratory, WPAFB, OH 45433, USA ∗∗∗∗ Air Force(e-mail:[email protected]) Air Force Research Laboratory, WPAFB, OH 45433, USA ∗∗∗∗ ∗∗∗∗ Air Force(e-mail:[email protected]) Research Air Force(e-mail:[email protected]) Research Laboratory, Laboratory, WPAFB, WPAFB, OH OH 45433, 45433, USA USA (e-mail:[email protected]) (e-mail:[email protected])

Remus Remus Remus Remus

C. C. C. C.

Abstract: This paper presents a fault detection, isolation and estimation scheme for sensor bias Abstract: This paper presents a fault detection, isolation and estimation scheme for sensor bias Abstract: This paper a detection, isolation and scheme for bias fault in inertial units (IMUs) of quadrotor unmanned air vehicles (UAVs). By using Abstract: Thismeasurement paper presents presents a fault fault detection, isolation and estimation estimation scheme for sensor sensor bias fault in inertial measurement units (IMUs) of quadrotor unmanned air vehicles (UAVs). By fault in in inertial inertial measurement units (IMUs) using of quadrotor quadrotor unmanned air vehicles vehicles (UAVs). By using using estimated roll and pitch angles obtained a sliding-mode observer method, a diagnostic fault measurement units (IMUs) of unmanned air (UAVs). By using estimated roll and pitch angles obtained using a sliding-mode observer method, a diagnostic estimated roll and obtained using aa estimating sliding-mode observer method, aa diagnostic algorithm is developed forangles detecting, isolating and sensor bias fault in the gyroscope estimated roll and pitch pitch angles obtained using sliding-mode observer method, diagnostic algorithm is developed for detecting, isolating and estimating sensor bias fault in the gyroscope algorithm is developed for detecting, isolating and estimating sensor bias fault in the gyroscope and accelerometer measurements. The stability and estimation performance properties of the algorithm is developed for detecting, isolating and estimating sensor bias fault in the gyroscope and accelerometer measurements. The stability and estimation performance properties of the and accelerometer measurements. The stability and estimation performance properties of fault estimation algorithm are rigorously established. Flight data collected from a real-time and accelerometer measurements. The stability and estimation performance properties of the the fault estimation algorithm are rigorously established. Flight data collected from a real-time fault estimation estimation algorithm are are rigorously established. Flight data data collected from real-time quadrotor test environment is used to illustrate the effectiveness of the proposed method. fault algorithm rigorously established. Flight collected from aa real-time quadrotor test environment is used to illustrate the effectiveness of the proposed method. quadrotor quadrotor test test environment environment is is used used to to illustrate illustrate the the effectiveness effectiveness of of the the proposed proposed method. method. © 2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Sensor bias detection, isolation, estimation, attitude angle estimation. Keywords: Sensor Sensor bias detection, detection, isolation, estimation, estimation, attitude angle angle estimation. Keywords: Keywords: Sensor bias bias detection, isolation, isolation, estimation, attitude attitude angle estimation. estimation. 1. INTRODUCTION sensor bias. Additionally, nonlinear adaptive estimation 1. INTRODUCTION INTRODUCTION sensor bias. bias. Additionally, Additionally, nonlinear nonlinear adaptive adaptive estimation estimation 1. sensor methods are employed to provide a robust estimate of the 1. INTRODUCTION sensor bias. Additionally, nonlinear adaptive estimation methods are employed to provide a robust estimate of the the Unmanned Aerial Vehicles (UAVs) have attracted sig- methods are employed to provide aabias. robust estimate of unknown magnitude of the sensor The stability methods are employed to provide robust estimate of and the Unmanned Aerial Aerial Vehicles Vehicles (UAVs) (UAVs) have have attracted attracted sigsig- unknown magnitude of the sensor bias. The stability and Unmanned nificant attentions in recent years. In order to enhance unknown magnitude of the sensor bias. The stability and estimation performance properties of the fault estimation Unmanned Aerial Vehicles (UAVs) have attracted sigunknown magnitude of the sensor bias. The stability and nificant attentions attentions in in recent recent years. years. In In order order to to enhance enhance estimation performance properties of the fault estimation nificant the reliability, survivability autonomy UAVs, ad- estimation performance of estimation are rigorouslyproperties established. Thefault effectiveness of nificant attentions in recentand years. In orderof to enhance estimation performance of the the estimation the reliability, survivability and autonomy of UAVs, ad- algorithm algorithm are are rigorouslyproperties established. Thefault effectiveness of the reliability, autonomy of UAVs, advanced health survivability managementand technologies are required, algorithm rigorously established. The effectiveness of the proposed diagnostic scheme is illustrated using flight the reliability, survivability and autonomy of UAVs, adare diagnostic rigorously scheme established. The effectiveness of vanced health health management management technologies technologies are are required, required, algorithm the proposed is illustrated using flight vanced which will enable UAVs to have the capabilities of state the proposed scheme using flight data collecteddiagnostic from an experimental autonomous quadrovanced health management technologies are required, the proposed diagnostic scheme is is illustrated illustrated using flight which will enable UAVs to have the capabilities of state data collected from an experimental autonomous quadrowhich will enable UAVs the state awareness self-adaptation (Vachtsevanos et al.of (2005); collected from tor test environment. which will and enable UAVs to to have have the capabilities capabilities state data data collected from an an experimental experimental autonomous autonomous quadroquadroawareness and self-adaptation (Vachtsevanos et al. al.of (2005); tor test test environment. awareness and self-adaptation (Vachtsevanos et (2005); Zhang et al. (2013)). Quadrotors are often equipped with tor environment. awareness and self-adaptation (Vachtsevanos et al. (2005); tor test environment. Zhang et et al. al. (2013)). (2013)). Quadrotors Quadrotors are are often often equipped with with Zhang low-cost and lightweight micro-electro-mechanical systems 2. PROBLEM FORMULATION Zhang al. (2013)). Quadrotors are often equipped equipped with low-costetand and lightweight micro-electro-mechanical systems 2. PROBLEM PROBLEM FORMULATION low-cost lightweight micro-electro-mechanical systems (MEMS) inertial measurement units (IMU) including 32. low-cost and lightweight micro-electro-mechanical systems 2. PROBLEM FORMULATION FORMULATION (MEMS) inertial inertial measurement measurement units units (IMU) (IMU) including including 33(MEMS) axis gyroscope, accelerometer and magnetometer which (MEMS) inertial measurement units (IMU) including 3The quadrotor nominal system are derived from axis gyroscope, gyroscope, accelerometer accelerometer and and magnetometer magnetometer which which The quadrotor nominal system dynamics dynamics are are derived derived from from axis are bias faults as aand result of componentwhich dam- The quadrotor nominal system axissusceptible gyroscope,to accelerometer magnetometer the Newton-Euler equations of dynamics motion and given from by The quadrotor nominal system dynamics areare derived are susceptible to bias faults as a result of component damthe Newton-Euler equations of motion and are given by are susceptible to faults aa result of age, temperature variation, excessive etc. damSev- the are susceptible to bias bias faults as as result vibration, of component component damthe Newton-Euler Newton-Euler equations of of motion motion and and are are given given by by(1) p˙ E = vE equations age, temperature variation, excessive vibration, etc. Sevage, temperature variation, excessive vibration, etc. Sevp ˙ = v (1) eral researchers have investigated the problem of quadrotor E age, temperature variation, excessive vibration, etc. Sev     p ˙˙ E = v (1) E E eral researchers have investigated the problem of quadrotor p = v (1)      0 0 E E eral researchers have investigated the problem of IMU sensor fault diagnosis based on linearized quadrotor      1 eral researchers have investigated the problem of 0 0      IMU sensor fault diagnosis based on linearized quadrotor 1 0 0 = R (η) − c v + (2) v ˙ B IMU sensor fault diagnosis based on quadrotor dynamic models certain (see, 11 REB IMU sensor fault around diagnosis based equilibrium on linearized linearizedpoints quadrotor 00 − 0 ccd + (2) vv˙˙ E m E = EB (η) dv B dynamic models around certain equilibrium points (see, 0 R = (η) − v + (2) −U g E EB d B 0 0 m − cd vB + g (2) v˙ E = m REB (η) dynamic models around equilibrium points for instance, Sharifi et al.certain (2010)) and by assuming the dynamic models around certain equilibrium points (see, (see, −U   for instance, Sharifi et al. (2010)) and by assuming the m −U g −U g for instance, Sharifi et al. (2010)) and by assuming the   1 sin φ tan θ cos φ tan θ attitude angles are directly measured (see, for instance, for instance, Sharifi et al. (2010)) and by assuming the   1 sin φ tan θθ cos φ tan θ attitude angles angles are are directly directly measured measured (see, (see, for for instance, instance, φ tan attitude φ θ cos −φ φ θθ ω Younes etangles al. (2013); Avram measured et al. (2014)). the η˙ = 011 sin (3) sincos φ tan tan cos φsin tan attitude are directly (see, However, for instance, 0 cos φ − sin φ Younes et al. (2013); Avram et al. (2014)). However, the η ˙ = (3) cos φ θ cos −φ sin φθ ω Younes et al. (2013); Avram et al. (2014)). However, η˙˙ = = 0 sincos ω (3) φ sec sec dynamics of the quadrotor are highly nonlinear, and the φ − sin φ Younes et al. (2013); Avram et al. (2014)). However, the η ω (3) 00 sin φ sec θθ  cos φ sec θθ dynamics of of the the quadrotor quadrotor are are highly highly nonlinear, nonlinear, and and the the   sin φ sec cos φ sec dynamics  Jysin −Jzφ sec attitude angle measurements are not always available in 1 0 θ cos φ sec θ   dynamics of the quadrotor are highly nonlinear, and the  τφ  JyJ−Jz qr  attitude angle angle measurements measurements are are not always always available in in    x z qr  −J attitude many practical applications. are not  JJ111xx ττφφ   −J attitude angle measurements not always available available in  JJJzyyJJ−J z qr  x x many practical applications.  qr pr J + (4) ω ˙ =   x θ x τφ JzJ−J  many practical applications. J1 x x pr  x yτ   y   many practical applications. 1 JzJ−J −J + (4) ω ˙ = x θ      1 J J1y τθ  This paper extends the results in a previous paper Avram z −J pr y x J + (4) ω ˙ = x y     τ pr J1y ψ (4) ω˙ =  JxJJ−J θ y y pq  + This paper paper extends extends the the results results in in aa previous previous paper paper Avram Avram J yτ y z J This 1 z J −J et al. (2014), by removing the critical assumption that ψ x −Jy pq This paper extends the results in a previous paper Avram 1 J J τ z x y J ψ pq z et al. (2014), by removing the critical assumption that τ J ψ 3 3 z pq J et al.and (2014), removing the critical assumption that roll pitch by attitude angles directly measurable. pE ∈ R3 isJzz the inertialJz position, vE ∈ R3 is the et (2014), removing the are critical assumption that where rollal.and and pitch by attitude angles are directly measurable. ∈ the inertial position, vvE ∈ R where p roll attitude angles are directly measurable. ∈ R R33 is is in the inertial position, ∈ θ, R33ψ]is isTT the the where pE Based on pitch estimated roll and pitch angles obtained using where velocityp expressed theinertial Earth position, frame, η = are E roll and pitch attitude angles are directly measurable. ∈ R is the vE ∈ R is the E E[φ, Based on on estimated estimated roll roll and and pitch pitch angles angles obtained obtained using using velocity expressed in the Earth frame, ηη = [φ, θ, ψ] are T T and Based velocity expressed in the Earth frame, = [φ, θ, ψ] are aBased sliding-mode observer method, aangles nonlinear diagnostic the roll, expressed pitch andinyaw Euler frame, angles, ηrespectively, on estimated roll and pitch obtained using velocity the Earth = [φ, θ, ψ] are a sliding-mode observer method, a nonlinear diagnostic the roll, pitch and yaw Euler angles, respectively, and T aalgorithm sliding-mode observer method, a nonlinear diagnostic the roll, pitch and yaw Euler angles, respectively, and for detecting and isolating sensor bias faults in ω = [p , q , r] represents the angular rates, m is the mass of a sliding-mode observer method, a nonlinear diagnostic the roll, pitch and yaw Euler angles, respectively, and T algorithm for for detecting detecting and and isolating isolating sensor sensor bias bias faults faults in in ω = [p ,, qq ,, r] the angular rates, m is the mass of T T represents algorithm ω = [p r] represents the angular rates, m is the mass of accelerometer and gyroscope measurements is developed. the quadrotor, and g is the gravitational acceleration. The algorithm for detecting and isolating sensor bias faults in ω =quadrotor, [p , q , r] represents thegravitational angular rates,acceleration. m is the mass of accelerometer and gyroscope measurements is developed. the and g is the The accelerometer gyroscope is developed. the quadrotor, quadrotor, and is the the gravitational gravitational acceleration. The Two diagnosticand estimators aremeasurements designed to provide struc- the terms Jx , Jy and Jzggrepresent the quadrotor inertias about accelerometer and gyroscope measurements is developed. and is acceleration. The Two diagnostic estimators are designed to provide structerms J , J and J represent the quadrotor inertias about x y and Jz represent the quadrotor inertias about Two diagnostic estimators are provide structerms J tured fault detection and isolation (FDI)to residuals, body y- Jand z-axis, the respectively. that the y and z represent Two estimators are designed designed provide allowstruc- the terms Jxx ,, J Jx-, quadrotorNote inertias about y z tureddiagnostic fault detection detection and isolation isolation (FDI)toresiduals, residuals, allowthe body x-, yand z-axis, respectively. Note that the tured fault and (FDI) allowthe body x-, yand z-axis, respectively. Note the ing simultaneous diagnosis of gyroscope and accelerometer quadrotor is assumed to be symmetric about thethat xz and tured fault detection and isolation (FDI) residuals, allowthe body x-, yand z-axis, respectively. Note that the ing simultaneous diagnosis of gyroscope and accelerometer quadrotor is assumed to be symmetric about the xz and ing simultaneous diagnosis of gyroscope and accelerometer quadrotor is assumed to be symmetric about the xz and ing simultaneous diagnosis of gyroscope and accelerometer quadrotor is assumed to be symmetric about the xz and Copyright 2015 IFAC 380 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © 2015, IFAC (International Federation of Automatic Control) Copyright © 2015 IFAC 380 Copyright 2015 IFAC 380 Peer review© of International Federation of Automatic Copyright ©under 2015 responsibility IFAC 380Control. 10.1016/j.ifacol.2015.09.556

SAFEPROCESS 2015 September 2-4, 2015. Paris, France

Remus C. Avram et al. / IFAC-PapersOnLine 48-21 (2015) 380–385

yz planes (i.e. the product of inertias is zero). U represents the total thrust generated by the rotors, τφ , τθ , τψ are the torques acting on the quadrotor around the body x-, yand z-axis, respectively. The term cd vB represents the drag force acting on the vehicle frame, with cd being drag force coefficient and vB being the velocity of the UAV relative to the body frame. The transformation from the body frame to the Earth fixed frame is given by the rotation matrix REB which is defined based on a 3-2-1 rotation sequence as follows:   cθcψ sφsθcψ − cφsψ cφsθcψ + sφsψ REB (η) = cθsψ sφsθsψ + cφcψ cφsθsψ − sφcψ −sθ sφcθ cφcθ where s· and c· are short hand notations for the sin(·) and cos(·) functions, respectively. Alternatively, as in Leishman et al. (2014), by assuming that the nonlinear Coriolis terms are small enough to be negligible, the quadrotor velocity dynamics relative to the body frame are expressed as        u˙ 0 −g sin θ 1 v˙ = 0 − cd vB + g sin φ cos θ (5) m w˙ −U g cos φ cos θ

where vB = [u, v, w]T , represents the velocities along the body x−, y− and z−direction. The relation between the inertial velocity and body velocity is given by vE = REB vB .

MEMS sensors, such as accelerometers and gyroscopes, measure forces and moments acting in the body frame. The quantity expressed inside the parenthesis in the inertial velocity (2) represents all the significant forces acting on the body. By considering IMU measurement susceptibility to a sensor bias fault, the accelerometer and gyroscope sensor measurements are given by ya = a + βa (t − Ta )ba (6) (7) yω = ω + βω (t − Tω )bω

where ya ∈ R3 and yω ∈ R3 are the measured accelerometer and gyro quantities, respectively, ba ∈ R3 and bω ∈ R3 represent possible sensor faults in accelerometer and gyroscope measurements respectively, and a represents the nominal acceleration measurement without bias, that is,  1  a= [0 , 0 , −U ]T − cd vB . (8) m The fault time profile functions βa (·) and βω (·) are assumed to be step functions with unknown fault occurrence times Ta and Tω , respectively. Assumption 1. The bias in accelerometer and gyroscope measurements are assumed to be constant and bounded: |bu | ≤ ¯bu , |bv | ≤ ¯bv , |bw | ≤ ¯bw (9) ¯ ¯ ¯ (10) |bu | ≤ bu , |bv | ≤ bv , |bw | ≤ bw

where ba = [bu , bv , bw ]T are the biases in each of the three axis acceleration measurements, and bω = [bp , bq , br ]T represent the gyroscope biases. Remark: It is worth noting that, in practical applications, after the occurrence of an IMU sensor bias, its magnitude may be time-varying and grow slowly over time. However, the change in the bias is often small over a short time duration. Therefore, the bias may be assumed to be constant on the short time duration under consideration. In addition, it is assumed that position in the Earth frame and yaw angle are available for measurement. Hence, the 381

381

system model (1) - (7) is augmented by the following additional output equation: y = [pE , ψ]T . (11) The objective of this research includes: (1) detect and isolate possible sensor bias in accelerometer and gyroscope measurements, (2) provide an estimation of the accelerometer and gyroscope sensor fault after detection and isolation. 3. FAULT DETECTION, ISOLATION AND ESTIMATION ARCHITECTURE

Fig. 1. Fault detection, isolation and estimation architecture. As shown in Figure 1, the proposed fault detection, isolation and estimation architecture is divided into two main tasks: (1) fault detection and isolation, (2) estimation of fault size. Controller signal, sensor measurements, and estimated roll and pitch angles serve as inputs to the fault detection, isolation and estimation framework. Under normal operating conditions two FDI estimators monitor the system for detecting and isolating faults in the accelerometer and gyroscope measurements. Once a fault is detected and isolated, the corresponding nonlinear estimator is activated for the purpose of sensor bias estimation. The estimation of the sensor bias obtained from the proposed algorithm can be used to enhance fault-tolerance of the closed-loop flight control system. In the following sections, we describe the details of attitude estimation, fault detection and isolation, and fault size estimation. 4. ROLL AND PITCH ANGLE ESTIMATION The problem of aircraft attitude angle estimation has been investigated by many researchers under various conditions (John L. Crassidis (2007)). However, most of these approaches don’t take into account the effect of IMU sensor faults. In this paper, faults in both gyroscope and accelerometer measurements are considered. By using (6), the accelerations of the quadrotor in the body frame x− and y− directions are given by cd y u = − u + βa b u m (12) cd yv = − v + β a bv , m where yu and yv represent the translational acceleration measurements, and bu and bv are the constant biases associated with these measurements. By derivating (12), and using (5), it can be shown that:

SAFEPROCESS 2015 382 September 2-4, 2015. Paris, France

Remus C. Avram et al. / IFAC-PapersOnLine 48-21 (2015) 380–385

cd cd cd g yu + sin θ + βa bu (13) m m m cd cd cd g sin φ cos θ + βa bv . y˙ v = − yv − (14) m m m Equations (13) and (14) represent the nonlinear dynamics of the translational acceleration. For the above dynamics, it can be seen that the sinusoidal terms appearing in (13) and (14) can possibly be treated as unknown inputs in the system and estimated online. In this research, based on the sliding-mode observer technique (Edwards et al. (2000)), the following estimator is chosen cd (15) yˆ˙ u = σ y˜u − yu + ζu m cd yˆ˙ v = σ y˜v − yv + ζv , (16) m where σ is a positive design constant, yˆu and yˆv represent the estimated x- and y- accelerations, ζu , ζv are the nonlinear injection signals, and y˜u  yu − yˆu and y˜v  yv − yˆv are the acceleration estimation errors, respectively. The nonlinear injection signals are given by yu ) , ζv = ρv sgn(˜ yv ) , (17) ζu = ρu sgn(˜ where sgn(·) represents the signum function, and ρu , ρv are design constants satisfying bu cd g ρu ≥ | (sin θ + )| + ξu (18) m g bv cd g ρv ≥ | (sin φ cos θ + )| + ξv , (19) m g with ξu and ξv being some small positive constants. By choosing V = 12 y˜x2 + 12 y˜y2 as a Lyapunov function candidate and using similar reasoning logic as in Edwards et al. (2000), it can be shown that V = 0 in finite time, and consequently a sliding motion is achieved and maintained after some finite time ts > 0. Therefore, in the absence of accelerometer faults (i.e. for t < Ta ), an estimation of roll and pitch angles can be generated by: m m θˆ = sin−1 ( ζueq ) , φˆ = sin−1 (− ζveq ), (20) cd g cd g cos θˆ where ζueq and ζveq are the output equivalent error injection signals approximated by: y˜u y˜v , ζveq ≈ ρv . (21) ζueq ≈ ρu ˜ yu  + δ ˜ yv  + δ y˙ u = −

5. FAULT DETECTION AND ISOLATION This section presents the proposed diagnostic method for detecting and isolating sensor faults in accelerometer and gyroscope measurements. Substituting the sensor model given by (6)-(7) into the systems dynamics (1)-(4), we obtain (22) p˙E = vE   0 (23) v˙ E = REB (η)ya + 0 − REB (η)βa ba g η˙ = T (η)yω − T (η)βω bω (24)    Jy −Jz  1 Jx (yq − βω bq )(yr − βω br ) Jx τ φ   Jz −J 1 x  ω˙ =  Jy (yr − βω br )(yp − βω bp ) + Jy τθ  1 Jx −Jy Jz τψ Jz (yp − βω bp )(yq − βω bq ) (25) where T (η) is the rotation matrix relating angular rates to Euler angle rates (see (3)). As can be seen from (22)-(25), 382

position and velocity states are only affected by the bias in accelerometer measurements. Additionally, angular rates states are only affected by gyroscope measurements. Based on this observation, it follows naturally to also divide the fault diagnosis task of these two sensor faults. 5.1 Accelerometer Bias Diagnostic Estimator The UAV position and inertial velocity dynamics given by (22) and (23) can be put into the following model: x˙ = Ax + f (η, ya ) + Ga (η)βa ba (26) y = Cx , T T where x = [pTE vE ] , y = pE ,

A=





03×3 I3 , 03×3 03×3

   0   f (η, ya ) =  REB ya + 0  g 

03×1

(27)

T Ga (η) = [03×3 , −REB ]T , and C = [I3 , 03×3 ]. The following fault diagnostic observer is chosen : ˆ θ, ˆ ψ, ya ) + L(y − yˆ) x ˆ˙ = Aˆ x + f (φ, (28) yˆ = C x ˆ,

where x ˆ ∈ R6 represents the inertial position and velocity estimation, yˆ ∈ R3 are the estimated position outputs, φˆ and θˆ are the estimated roll and pitch angles given by (20), and L is a design matrix. From the definition of matrices A and C given by (27), it is straightforward to show that the system is observable. Therefore, the matrix L can be designed such that the matrix A¯  A − LC is stable. Let us define the state estimation error as x ˜  x−x ˆ and the quadrotor position estimation error as y˜p  y − yˆ. From (26) and (28), it follows that ¯x + ∆f · ya + Ga (η)βa ba x ˜˙ = A˜ (29) y˜p = C x ˜, ˆ θ, ˆ ψ). where ∆f  REB (φ, θ, ψ) − REB (φ, As shown in Section 4, in the absence of an accelerometer bias, the estimated roll and pitch angles converge to the true attitude angles in finite time, which implies that ∆f → 0 in finite time. Additionally, it can be seen from (29) that the residual y˜p is only sensitive to the bias ba . Therefore, if any component of the position estimation error y˜p deviates significantly from zero, we can conclude a fault in the accelerometer measurement has occurred. 5.2 Gyroscope Bias Diagnostic Estimator Expanding the dynamics of the angular rates described by (25), we obtain:       τφ p˙ yq yr −1 ¯ τθ q˙ = J yr yp + J yp yq τψ r˙ (30)      0 −yr −yq bp bq br ¯ ω −yr 0 −yp bq + br bp + Jβ −yq −yp 0 br bp b q J −J J −J where J¯ = diag{ y z , Jz −Jx , x y }. Based on adaptive Jx

Jy

Jz

estimation schemes (Ioannou and Sun (1996)), a gyroscope bias diagnostic estimator is designed as follows:

SAFEPROCESS 2015 September 2-4, 2015. Paris, France

Remus C. Avram et al. / IFAC-PapersOnLine 48-21 (2015) 380–385

        yˆ˙ p yq yr τφ yp − yˆp yˆ˙ q  = Ψ yq − yˆq + J¯ yr yp + J −1 τθ yr − yˆr y p yq τψ yˆ˙ r

(31)

where [ˆ yp , yˆq , yˆr ]T represents the estimations of the gyroscope measurements, and Ψ is a diagonal positive definite design matrix. Let the angular rate estimation error be defined as:     y˜p yp − yˆp y˜ω = y˜q  yq − yˆq . y˜r yr − yˆr Based on (30) and (31), it follows that          y˜˙ p y˜p 0 −yr −yq bp bq br ¯ ω y˜˙ q  = −Ψ y˜q +Jβ −yr 0 −yp bq + br bp y˜r −yq −yp 0 br bp bq y˜˙ r (32) As seen from (32), the angular rates estimation error converges exponentially to zero in the absence of gyroscope sensor bias. In addition, it can be seen that the residual y˜ω is only sensitive to the bias bω . Therefore, if any component of the state estimation error y˜ω is significantly different from zero, we can conclude that a fault in the gyroscope measurements has occurred. 5.3 Fault Detection and Isolation Decision Scheme As described in Sections 5.1 and 5.2, the two fault diagnostic residuals are designed such that each of them is only sensitive to one type of sensor faults. Based on this observation, the residuals y˜ω and y˜p generated by (32) and (29) can also be used as structured residuals for fault isolation. Specifically, Table 1 provides the isolation logic for these two fault types, where a “0” represents around zero, and a “1” represents significantly larger than zero. Table 1. Fault isolation decision truth table.

y˜p y˜ω

No Fault

Gyro Bias

Accel Bias

0 0

0 1

1 0

Accel & Gyro Bias 1 1

6. SENSOR BIAS ESTIMATION 6.1 Accelerometer Bias Estimation Once an accelerometer sensor fault has been detected, an estimation of the accelerometer bias is obtained based on the following algorithm: i) Obtain the inertial velocity estimate vˆE using the following estimator ˆ θ, ˆ ψ) + L(y − yˆ) x ˆ˙ = Aˆ x + H(φ, (33) yˆ = C x ˆ where   03×3 I3 A= , C = [I3 03×3 ] , 03×3 − cmd I3 

 H(φ, θ, ψ) = 

U −m

 03×1    cφsθcψ + sφsψ 0  , cφsθsψ − sφcψ + 0  cφcθ g



383

383

x = [pE , vE ]T and y represent the state and measurement vector, respectively, and L is a design matrix chosen such that the matrix A¯  A − LC is Hurwitz. ii) Obtain an estimate of the quadrotor velocity relative to the body frame, that is: ˆ θ, ˆ ψ)ˆ vˆB = RBE (φ, vE . (34) iii) Using (8) and (34) obtain an estimate of the acceleration acting on the quadrotor body, that is    0 1 0 − cd vˆB . a ˆ= (35) m −U iv) Use the accelerometer sensor measurement model given in (6) to obtain an estimate of the accelerometer bias as ˆba = ya − a ˆ. (36)

The stability performance of the above accelerometer bias estimation scheme can be summarized as follows: Theorem 1. In the presence of an accelerometer bias, the estimation scheme described by (34), (35), (36) and (33) guarantees that (1) the state estimation error x ˜ is bounded, (2) the accelerometer bias estimation error satisfies cd cd ˜ BE  RBE (µ0 e−β0 (t−t0 ) |˜ vE |R x(t0 )| + |ˆ |˜ba | ≤ m m  t

+

t0

µ0 e−β0 (t−τ ) |∆H|dτ ).

The detailed proof of the above theorem can be found in Avram (2015). Theorem 1 ensures the boundness of all signals in the estimation scheme. Additionally, it can be seen that the bias estimation performance is limited by the uncertainties entering the fault estimation problem, including unknown initial conditions x ˜(t0 ) and the attitude ˜ EB and ∆H. It angle estimation error represented by R is worth noting that the occurrence of an accelerometer bias will affect the accuracy of attitude angle estimation. However, as can be seen from (18) and (19), this effect is significantly reduced due to the division of the bias by the gravitational constant g. In addition, after a reasonable estimate of the sensor bias is obtained, it can possibly be used to to adjust the sensor measurement, hence improving attitude angle estimation. 6.2 Gyroscope Bias Estimation Once the occurrence of a gyroscope sensor fault is determined, an adaptive estimator is activated in order to estimate the bias in the gyroscope sensor. Specifically, based on (24), the following adaptive estimator is chosen: ˙ ηˆ˙ e = −Λ(ˆ η − ηe ) + T (ηe )yω − T (ηe )ˆbω + Ωˆbω (37) ˙Ω = −ΛΩ − T (ηe ) (38)

ˆb˙ ω = ΓΩT (ηe − ηˆe ) (39) T where ηe  [φˆ , θˆ , ψ] , with φˆ and θˆ generated by (20), ηˆe represents an estimate of ηe , Λ and Γ are positive definite diagonal design matrices and the filter (38) related to Ω ∈ R3×3 , is needed to ensure the stability of the adaptive algorithm (37)(Bastin and Gevers (1988)). Without loss of generality, we let Λ = diag{λ, λ, λ} , where −λ < 0 is the

SAFEPROCESS 2015 384 September 2-4, 2015. Paris, France

Remus C. Avram et al. / IFAC-PapersOnLine 48-21 (2015) 380–385

filter pole. The adaptive law (39) for updating the term ˆbω is derived using Lyapunov synthesis approach (Ioannou and Sun (1996)). The stability and estimation performance properties of the adaptive scheme given by (37) - (39) are described as follows: Theorem 2. In the presence of a gyroscope bias, if there exists constants α0 > 0, T0 > 0 and α1 > 0 such that  t+T0 1 α1 I ≥ ΩT Ωdτ ≥ α0 I, (40) T0 t then the adaptive estimation scheme described by (37) (39) guarantees that:

Fig. 2. Experimental System Architecture Setup

(1) all the signals in the adaptive estimator remain bounded, (2) the gyroscope bias parameter estimation error satisfies  t |˜bω (t)| ≤ γe−α(t−t1 ) |˜bω (t1 )| + λe−α(t−τ ) ΓΩT χ(t) dτ t1

where

ηe (t0 )| + χ(t) = e−λ(t−t0 ) |¯



t t0

e−λ(t−τ ) ∆T (|yω | + ¯bω )dτ. (41)

For the sake of space limitation, the proof of the above theorem is purposely omitted. Interested readers can contact the corresponding authors for details (Avram (2015)). Theorem 2 ensures the boundness of all signals in the adaptive gyroscope bias estimation scheme. Additionally, (41) shows that the estimation performance is limited by various uncertainties entering the fault estimation problem, including the attitude angle estimation error repre˜ EB and ∆T , unknown gyroscope bias (bω ) and sented by R unknown initial conditions (˜bω (t1 )).

Fig. 3. Roll/Pitch estimation in the case of accelerometer bias. method, we log approximately 4 minutes of autonomous flight data. Sensor bias is then artificially injected into the accelerometer and gyroscope measurements, respectively. In the following sections, we present the evaluation results of the diagnosis method using the collected data. 7.2 Evaluation Results

7. EXPERIMENTAL RESULTS 7.1 Experimental Setup A block diagram of the experimental system setup is shown in Figure 7.1. During flight tests, quadrotor position and attitude information is obtained from a Vicon motion capture camera system. Position and Euler angle measurements are collected every 10ms and relayed from a Vicon dedicated PC via TCP/IP connection to a to a ground station computer. The fault diagnosis method is evaluated using data from autonomous flight of a quadrotor built in-house with off-the-shelf components. The quadrotor is equipped with the Qbrain embedded control module Qbrain from Quanser Inc. The control module consist of a HiQ acquisition card providing real-time IMU measurements, and a Gumstix DuoVero microcontroller running the real-time control software. An IEEE802.11 connection between the ground station PC and the gumstix allows for fast and reliable wireless data transmission and online parameter tuning. Position and attitude information obtained from the Vicon system along with trajectory commands generated by the ground station are sent to the quadrotor in order to achieve real-time autonomous flight. The control software executes on-board at 500Hz, and accelerometer and gyroscope measurement are logged at 200Hz. In order to evaluate the proposed diagnosis 384

Figure 3 shows the estimation of roll and pitch angles during the quadrotor flight. At time t = 135s, a constant bias of ba = [0.15, −0.2, 0.25]T m/s2 is injected into the accelerometer measurements. As can be seen, the roll and pitch estimates closely track the true attitude angles prior to the accelerometer fault and only slightly degraded after the occurrence of the accelerometer fault. It is worth noting that the time delay introduced by the sliding-mode observer, and filtering of the accelerometer measurements and the observer outputs is taken into account in the algorithm implementation. Figure 4 shows the FDI residuals generated by the two diagnostic estimators described by (29) and (32), respectively. In order to enhance the diagnostic decision based on the FDI logic given by Table 1, the two-sided cumulative sum (CUSUM) test is applied to process the diagnostic residuals (Gustafsson (2000)). Figure 5 shows the statistic property generated by the CUSUM test. A fixed FDI detection threshold can easily be chosen. As can be seen, shortly after the occurrence of the fault, at least one component of the test statistic corresponding to the residuals generated by the accelerometer diagnostic estimator exceeds the detection threshold. Conversely, the test statistic corresponding to the gyroscope bias remains well below its detection threshold. Based on the detection and isolation logic given in Table 1, we can conclude that a fault has occurred in the accelerometer measurement. In

SAFEPROCESS 2015 September 2-4, 2015. Paris, France

Remus C. Avram et al. / IFAC-PapersOnLine 48-21 (2015) 380–385

385

pitch angles provided by a sliding-mode observer, two FDI estimators are designed to generate structured residuals for detecting and isolating the sensor faults. In addition, nonlinear adaptive estimation algorithms are developed to provide an estimate of the unknown magnitude of the sensor bias. The effectiveness of the method is demonstrated using flight data collected from a real-time quadrotor UAV test environment. An interesting direction for future research is to develop and demonstrate a systematic diagnostic method for quadrotor actuator faults REFERENCES

Fig. 4. Accelerometer bias fault detection and isolation.

Fig. 5. Accelerometer fault diagnosis using CUSUM.

Fig. 6. Accelerometer bias fault estimation. addition, Figure 6 shows the estimation of the bias in the accelerometer for each axis, respectively. As can be seen, the estimate of accelerometer bias is reasonably close to its actual value. The case of gyroscope bias has also been evaluated using experimental data and satisfactory results have been obtained. However, due to space limitations, the details of the experimental results is omitted. 8. CONCLUSION In this paper, we present the design and analysis of a nonlinear fault detection, isolation, and estimation scheme for sensor bias faults in accelerometer and gyroscope measurements of quadrotor UAVs. Using estimated roll and 385

Avram, R., Zhang, X., and Campbell, J. (2014). Sensor fault diagnosis in quadrotors using nonlinear adaptive estimators. In Annual Conference of the Prognostics and Health Management Society, volume 7, 217–224. Fort Worth, Texas. Avram, R.C. (2015). Fault Diagnosis and Fault Tolerant Control of Quadrotor UAVs. Technical report, Wright State University, Dayton, OH. Bastin, G. and Gevers, M. (1988). Stable adaptive observers for nonlinear time-varying systems. IEEE Transactions on Automatic Control, 33(7), 650–658. Edwards, C., Spurgeon, S.K., and Patton, R.J. (2000). Sliding mode observers for fault detection and isolation. Automatica, 36(4), 541–553. Gustafsson, F. (2000). Adaptive Filtering and Change Detection. Wiley, West Sussex, England. Ioannou, P.A. and Sun, J. (1996). Robust Adaptive Control. Dover Publications, Inc., Mineola, New York. John L. Crassidis, F. Landis Markley, Y.C. (2007). Survey of nonlinear attitude estimation methods. Journal of Guidance, Control and Dynamics, 30(1). Leishman, Robert, C., MacDonald, John, C., Beard, Randal, W., and McLain, T. (2014). Quadrotors and accelerometers: state estimation with an improved dynamic model. IEEE Control Systems Magazine, 34(1). Sharifi, F., Mirzaei, M., Gordon, B.W., and Zhang, Y. (2010). Fault tolerant control of a quadrotor UAV using sliding mode control. In 2010 Conference on Control and Fault Tolerant Systems, 239–244. Nice, France. Vachtsevanos, G., Tang, L., Drozeski, G., and Gutierrez, L. (2005). From mission planning to flight ocntrol of unmanned aerial vehicles: Strategies and implementation tools. Anual Reviews in Control, 29, 101–115. Younes, Y.A., Noura, H., Rabhi, A., Hajjajji, A.E., and Hussein, N.A. (2013). Sensor fault detection and isolation in the quadrotor vehicle using nonlinear identity observer approach. In 2013 Conference on Control and Fault-Tolerant Systems (SysTol), 486–491. Nice, France. Zhang, Y., Chamseddine, A., C.A., R., Gordon, B., Su, C., Rakheja, S., Fulford, C., Apkaraian, J., and Gosselin, P. (2013). Development of advanced FDD and FTC techniques with application to an unmanned quadrotor helicopter testbed. Journal of the Franklin Institue, 350(9).