# A Particle Filtering Approach for Fault Detection and Isolation of UAV IMU Sensors: Design, Implementation and Sensitivity Analysis

^{1}

^{2}

^{3}

^{*}

## Abstract

**:**

## 1. Introduction

- fast detection of abnormal situations;
- isolation of faults;
- robustness to noise and uncertainties;
- low false alarm rate.

- sensor faults: a malfunction of the measurement subsystem;
- actuator faults: a malfunction of actuators acting on system dynamics;
- process faults: a strong change in system dynamics due to structural problems.

- in Section 2, the proposed hardware and software architecture are described;
- in Section 3, the attitude estimation model is defined, using quaternion-based relations;
- in Section 4, the particle filter algorithm is introduced;
- in Section 5, the SFDI algorithm is shown with details on the implementation;
- the effectiveness of the algorithm is shown by means of realistic simulations in Section 6, where an analysis against tunable parameters is discussed.

## 2. System Description

- two commercially available boards based on the STM32F103VCT CPU by ST, typically used on low-cost multi-rotors as flight controllers; such boards, called IMU-1 and IMU-2, weighing 17 grams, have a power consumption average below 1W and include the following inertial sensors:
- a Raspberry Pi 3B platform, a single-board computer based on the Broadcom BCM2837 64bit CPU, with wireless LAN, USB and GPIO connectivity. It weighs 42 grams and in our test had a power consumption of less than 2.5W.

## 3. UAV Attitude Estimation Problem

- a north-east-down (NED) reference frame, parallel to the Earth’s surface, with the ${X}_{E}$-axis pointed toward north, the ${Y}_{E}$-axis pointed toward east and the ${Z}_{E}$-axis oriented downwards;
- the body frame, centered in the UAV’s center of gravity ($CG$), with the ${X}_{B}$-axis pointed toward the UAV nose, the ${Z}_{B}$-axis downward and the ${Y}_{B}$-axis oriented to complete a right-handed system.

- the first rotation around the ${Z}_{E}$ axis by the yaw angle $\psi $, from $O{X}_{E}{Y}_{E}{Z}_{E}$ to $O{X}^{\prime}{Y}^{\prime}{Z}^{\prime}$;
- the second rotation around the ${Y}^{\prime}$ axis by the pitch angle $\theta $ from $O{X}^{\prime}{Y}^{\prime}{Z}^{\prime}$ to $O{X}^{\u2033}{Y}^{\u2033}{Z}^{\u2033}$;
- the third rotation around the ${X}^{\u2033}$ axis by the roll angle $\varphi $ from $O{X}^{\u2033}{Y}^{\u2033}{Z}^{\u2033}$ to $O{X}_{B}{Y}_{B}{Z}_{B}$.

## 4. Particle Filter Algorithm

- (i)
- the set of a priori particles $\mathbf{X}\left(k\right|k-1)$ at the k-th sampling step is obtained by propagating the set $\mathbf{X}(k-1|k-1)$ with the state Equation (19), considering the process noise $\mathbf{d}\left(k\right)$ equal to zero. Therefore, the generic particle ${\mathbf{x}}_{\delta}\left(k\right)\in \mathbf{X}\left(k\right|k-1)$ is computed as follows:$$\begin{array}{c}\hfill {\mathbf{x}}_{\delta}\left(k\right)=\mathbf{f}({\widehat{\mathbf{x}}}_{\delta}(k-1),{\omega}_{S}(k-1),\mathbf{0})\end{array}$$
- (ii)
- We compute the relative likelihood ${\tilde{w}}_{\delta}\left(k\right)$ of each particle ${\mathbf{x}}_{\delta}\left(k\right)$ conditioned on the measurement $\mathbf{y}\left(k\right)$., i.e., evaluate the PDF $p\left(\mathbf{y}\left(k\right)|{\mathbf{x}}_{\delta}\left(k\right)\right)$ on the basis of the nonlinear measurement equation and the PDF of the measurement noise $\mathbf{v}\left(k\right)$. Under the assumption of additive measurement noise, the weight ${\tilde{w}}_{\delta}\left(k\right)$ can be computed as follows:$${\tilde{w}}_{\delta}\left(k\right)=P\left[\mathbf{y}\left(k\right)|{\mathbf{x}}_{\delta}\left(k\right))\right]=P\left[\mathbf{v}\left(k\right)=\left(\mathbf{y}\left(k\right)-{\mathbf{y}}_{\delta}\left(k\right)\right)\right]$$$${\mathbf{y}}_{\delta}\left(k\right)=\mathbf{h}\left({\mathbf{x}}_{\delta}\left(k\right)\right)$$
- (iii)
- The weights ${\tilde{w}}_{\delta}\left(k\right)$ are normalized according to the following equation:$${w}_{\delta}\left(k\right)={\displaystyle \frac{{\tilde{w}}_{\delta}\left(k\right)}{{\sum}_{\delta =1}^{\Delta}{\tilde{w}}_{\delta}\left(k\right)}}$$
- (iv)
- A set of particles taking into account a posteriori knowledge $\tilde{\mathbf{X}}\left(k\right|k)$ is obtained by randomly drawing $\Delta $ samples from $\mathbf{X}\left(k\right|k-1)$$$\begin{array}{cc}\hfill \tilde{\mathbf{X}}\left(k\right|k)=\left\{{\mathbf{x}}_{\delta}\left(k\right)\in \mathbf{X}\left(k\right|k-1),\underset{{m}_{1}}{\underbrace{{\mathbf{x}}_{1}\left(k\right),\cdots ,{\mathbf{x}}_{1}\left(k\right)}},\cdots ,\right.& \\ \hfill \left.\underset{{m}_{\delta}}{\underbrace{{\mathbf{x}}_{\delta}\left(k\right),\cdots ,{\mathbf{x}}_{\delta}\left(k\right)}},\cdots ,\underset{{m}_{\Delta}}{\underbrace{{\mathbf{x}}_{\Delta}\left(k\right),\cdots ,{\mathbf{x}}_{\Delta}\left(k\right)}}\right\}\end{array}$$
- ${m}_{\delta}$ is a non-negative integer;
- ${\sum}_{\delta =1}^{\Delta}{m}_{\delta}=\Delta $;
- ${lim}_{\Delta \to \infty}{m}_{\delta}={w}_{\delta}\left(k\right)\xb7\Delta $.

To compute the parameter ${m}_{\delta}$ the following sub-procedure is applied:- (a)
- define a series of $\Delta +1$ thresholds ${\sigma}_{\delta}$ as:$${\sigma}_{\delta}=\sum _{1}^{\delta}{w}_{\delta},\phantom{\rule{1.em}{0ex}}\mathrm{with}\phantom{\rule{4.pt}{0ex}}{\sigma}_{0}=0$$
- (b)
- initialize ${m}_{\delta}$ values to zero;
- (c)
- draw $\Delta $ uniformly distributed random numbers ${r}_{i}$ from the interval $[0,1]$, $\mathcal{P}=\{{r}_{i}\in [0,1],\mathrm{with}\phantom{\rule{4.pt}{0ex}}i=1,...,\Delta \}$;
- (d)
- for each ${r}_{i}\in \mathcal{P}$, find $\delta $ such that ${\sigma}_{\delta -1}<{r}_{i}\le {\sigma}_{\delta}$, then ${m}_{\delta}={m}_{\delta}+1$.

- (v)
- In order to counteract the so-called particle degeneracy problem, which can weaken algorithm convergence and robustness [78], the set of a posteriori particle $\mathbf{X}\left(k\right|k)$ is obtained by randomly scattering each particle of set $\tilde{\mathbf{X}}\left(k\right|k)$ in a given neighborhood.
- (vi)
- The state estimate $\widehat{\mathbf{x}}\left(k\right)$ is evaluated as the algebraic mean of a posteriori particles belonging to $\mathbf{X}\left(k\right|k)$:$$\widehat{\mathbf{x}}\left(k\right)=\frac{1}{\Delta}\sum _{\delta =1}^{\Delta}{\widehat{\mathbf{x}}}_{\delta}\left(k\right)$$

## 5. SFDI Algorithm for a Duplex IMU

- subscript A indicates accelerometers, subscript G is for gyroscopes and subscript M is for magnetometers;
- ${N}_{Z}$ is the normal state for sensor $Z\in \{A,\phantom{\rule{0.166667em}{0ex}}G,\phantom{\rule{0.166667em}{0ex}}M\}$;
- ${A}_{Z}$ is the alert state activated at the detection phase on the basis of a preliminary comparison between IMUs;
- ${F}_{j}$ is the fault state for the IMU unit $j\in \{1,2\}$.

**Step 1—FAULT DETECTION.**Under the assumption of a non-contemporary fault, at each time step k, the transition from normal $N(k-1)$ to alert $A\left(k\right)$ state (and vice versa from $A(k-1)$ to $N\left(k\right)$) is achieved by comparing measurement accelerometer, magnetometer and gyroscope measurements from IMUs.

**Step 2—FAULT ISOLATION.**Once the presence of a fault is detected, with $\tilde{k}$ the time instant of fault detection, then

**Step 3—FAULT RECOVERY.**When the fault condition is no longer held (${\mathcal{E}}_{Z2}$), the system is able to recover the faulted IMU by checking the consistency between attitude estimations from both PFs, through

## 6. Numerical Simulations Based on Experimental Data

- a backward movement, executed by applying a doublet (+/−) to the pitch angle;
- a lateral movement, with a doublet (+/−) applied to the roll angle;
- a doublet applied to the yaw angle.

#### 6.1. Fault Scenario Analyses

- F1: intermittent abrupt bias—a step disturbance of 0.2 g is added to ${a}_{y,S}^{\left(1\right)}\left(t\right)$ for $t\in [3,\phantom{\rule{0.166667em}{0ex}}13]$ s;
- F2: slow drift—a linearly increasing signal with a rate of $0.2$ rad/s${}^{2}$ is added to the measured yaw rate on IMU-2 starting from the time instant $t=3.5$ s;
- F3: abrupt freezing—magnetometer data ${M}_{x,S}^{\left(1\right)}\left(t\right)$ stops being updated at $t=3$ s;
- F4: oscillation—a sinusoidal signal with a frequency of 30 rad/s is added to the ${r}_{S}^{\left(1\right)}\left(t\right)$ measurement for $t\in [3,\phantom{\rule{0.166667em}{0ex}}28]$ s;
- F5: random walk—Gaussian noise, whose amplitude is smaller than the relevant detection threshold, is integrated and added to accelerometer data ${a}_{x,S}\left(t\right)$ for $t\in [23,\phantom{\rule{0.166667em}{0ex}}33]$ s.

#### 6.1.1. Scenario F1—Intermittent Abrupt Bias

#### 6.1.2. Scenario F2—Slow Drift

#### 6.1.3. Scenario F3—Abrupt Freezing

#### 6.1.4. Scenario F4—Oscillation

#### 6.1.5. Scenario F5—Random Walk

#### 6.2. Algorithm Parameter Sensitivity Analysis

- computation time—the time needed to execute the SFDI algorithm at each time instant on the embedded platform Raspberry Pi 3B;
- correct detection—the ratio between the correctly detected fault time interval and the real fault duration, computed as $CD=\mathrm{card}({\mathcal{T}}_{f}\cap {\mathcal{T}}_{0})/\mathrm{card}\left({\mathcal{T}}_{0}\right)$, where $\mathrm{card}\left(\mathcal{T}\right)$ denotes the cardinality of set $\mathcal{T}$;
- wrong detection—the ratio between the badly detected fault time interval and the real fault duration, computed as $WD=\mathrm{card}({\mathcal{T}}_{f}\cap {\overline{\mathcal{T}}}_{0})/\mathrm{card}\left({\mathcal{T}}_{0}\right)$, where ${\overline{\mathcal{T}}}_{0}$ is the complementary set of ${\mathcal{T}}_{0}$;
- detection time—the delay between the beginning of the fault and the detection, computed as $DT=(min{\mathcal{T}}_{f}-min{\mathcal{T}}_{0})$;
- recovery time—the delay between the end of the fault and the fault recovery, computed as $RT=(max{\mathcal{T}}_{f}-max{\mathcal{T}}_{0})$.

## 7. Conclusions

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Data Availability Statement

## Conflicts of Interest

## References

- Adams, S.M.; Friedland, C.J. A survey of unmanned aerial vehicle (UAV) usage for imagery collection in disaster research and management. In Proceedings of the 9th International Workshop on Remote Sensing for Disaster Response, Stanford, CA, USA, 15–16 September 2011; Volume 8. [Google Scholar]
- Danilov, A.; Smirnov, U.D.; Pashkevich, M. The system of the ecological monitoring of environment which is based on the usage of UAV. Russ. J. Ecol.
**2015**, 46, 14–19. [Google Scholar] [CrossRef] - Furci, M.; Paoli, A.; Naldi, R. A supervisory control strategy for robot-assisted search and rescue in hostile environments. In Proceedings of the 2013 IEEE 18th Conference on Emerging Technologies & Factory Automation (ETFA), Cagliari, Italy, 10–13 September 2013; pp. 1–4. [Google Scholar]
- Shakhatreh, H.; Sawalmeh, A.H.; Al-Fuqaha, A.; Dou, Z.; Almaita, E.; Khalil, I.; Othman, N.S.; Khreishah, A.; Guizani, M. Unmanned aerial vehicles (UAVs): A survey on civil applications and key research challenges. IEEE Access
**2019**, 7, 48572–48634. [Google Scholar] [CrossRef] - Peng, Z.; Kato, T.; Takahashi, H.; Kinoshita, T. Intelligent home security system using agent-based IoT devices. In Proceedings of the 2015 IEEE 4th Global Conference on Consumer Electronics (GCCE), Osaka, Japan, 27–30 October 2015; pp. 313–314. [Google Scholar]
- Marchese, M.; Moheddine, A.; Patrone, F. IoT and UAV integration in 5G hybrid terrestrial-satellite networks. Sensors
**2019**, 19, 3704. [Google Scholar] [CrossRef] [PubMed] [Green Version] - Schaefer, R. Unmanned Aerial Vehicle Reliability Study; Office of the Secretary of Defense: Washington, DC, USA, 2003.
- Hwang, I.; Kim, S.; Kim, Y.; Seah, C.E. A survey of fault detection, isolation, and reconfiguration methods. IEEE Trans. Control Syst. Technol.
**2010**, 18, 636–653. [Google Scholar] [CrossRef] - Marzat, J.; Piet-Lahanier, H.; Damongeot, F.; Walter, E. Model-based fault diagnosis for aerospace systems: A survey. Proc. Inst. Mech. Eng. Part G J. Aerosp. Eng.
**2012**, 226, 1329–1360. [Google Scholar] [CrossRef] [Green Version] - Zhang, Y.; Jiang, J. Bibliographical review on reconfigurable fault-tolerant control systems. Annu. Rev. Control
**2008**, 32, 229–252. [Google Scholar] [CrossRef] - Fekih, A. Fault Diagnosis and Fault Tolerant Control Design for aerospace Systems: A Bibliographical Review. In Proceedings of the American Control Conference (ACC), Portland, OR, USA, 4–6 June 2014; Volume 32. [Google Scholar]
- Chen, H.; Wang, X.; Li, Y. A Survey of Autonomous Control for UAV. In Proceedings of the 2009 International Conference on Artificial Intelligence and Computational Intelligence, Shanghai, China, 7–8 November 2009; Volume 2, pp. 267–271. [Google Scholar]
- Zhang, Y.; Jiang, J. Issues on Integration of Fault Diagnosis and Reconfigurable Control in Active Fault-Tolerant Control Systems. IFAC Proc. Vol.
**2006**, 39, 1437–1448. [Google Scholar] [CrossRef] - Hansen, S.; Blanke, M.; Adrian, J. Fault Diagnosis and Fault Handling for Autonomous Aircraft. Ph.D. Thesis, Department of Electrical Engineering, Hong Kong, China, 2012. [Google Scholar]
- Schulte, P.Z.; Spencer, D.A. State Machine Fault Protection Architecture for Aerospace Vehicle Guidance, Navigation, and Control. J. Aerosp. Inf. Syst.
**2020**, 17, 70–85. [Google Scholar] [CrossRef] - Boskovic, J.; Jackson, J.A. An Innovative Approach to Air Data Sensor FDIR for Commercial Aircraft. In Proceedings of the AIAA Guidance, Navigation, and Control Conference, San Diego, CA, USA, 4–8 January 2016; p. 0375. [Google Scholar]
- Blanke, M.; Hansen, S. Towards self-tuning residual generators for UAV control surface fault diagnosis. In Proceedings of the 2013 Conference on Control and Fault-Tolerant Systems (SysTol), Nice, France, 9–11 October 2013; pp. 37–42. [Google Scholar]
- Suarez, A.; Heredia, G.; Ollero, A. Cooperative Virtual Sensor for Fault Detection and Identification in Multi-UAV Applications. J. Sensors
**2018**, 2018, 1–19. [Google Scholar] [CrossRef] - Leung, T.; Rife, J.; Seiler, P.; Venkataraman, R. Comparison of fault-tree models for fault detection, isolation, and recovery algorithms. J. Aerosp. Inf. Syst.
**2017**, 14, 517–522. [Google Scholar] [CrossRef] - Crassidis, J.; Markley, L.; Cheng, Y. Survey of Nonlinear Attitude Estimation Methods. J. Guid. Control Dyn.
**2007**, 30, 12–28. [Google Scholar] [CrossRef] - Gośliński, J.; Giernacki, W.; Królikowski, A. A Nonlinear Filter for Efficient Attitude Estimation of Unmanned Aerial Vehicle (UAV). J. Intell. Robot. Syst.
**2019**, 95, 1079–1095. [Google Scholar] [CrossRef] [Green Version] - Zhong, Y.; Zhang, W.; Zhang, Y.; Zuo, J.; Zhan, H. Sensor Fault Detection and Diagnosis for an Unmanned Quadrotor Helicopter. J. Intell. Robot. Syst.
**2019**, 96, 555–572. [Google Scholar] [CrossRef] - Goupil, P. AIRBUS state of the art and practices on FDI and FTC in flight control system. Control Eng. Pract.
**2011**, 19, 524–539. [Google Scholar] [CrossRef] - Rudin, K.; Ducard, G.J.; Siegwart, R.Y. A sensor fault detection for aircraft using a single Kalman filter and hidden Markov models. In Proceedings of the 2014 IEEE Conference on Control Applications (CCA), Juan Les Antibes, France, 8–10 October 2014; pp. 991–996. [Google Scholar]
- Berdjag, D.; Cieslak, J.; Zolghadri, A. Fault diagnosis and monitoring of oscillatory failure case in aircraft inertial system. Control Eng. Pract.
**2012**, 20, 1410–1425. [Google Scholar] [CrossRef] - Rosa, P.; Silvestre, C. Fault detection and isolation of LPV systems using set-valued observers: An application to a fixed-wing aircraft. Control Eng. Pract.
**2013**, 21, 242–252. [Google Scholar] [CrossRef] - Notaro, I.; Ariola, M.; D’Amato, E.; Mattei, M. HW VS SW sensor redundancy: Fault detection and isolation observer based approaches for inertial measurement units. In Proceedings of the 29th Congress of International Council of Aeronautical Sciences, St. Peterburg, Russia, 7–12 September 2014. [Google Scholar]
- Ariola, M.; Corraro, F.; Mattei, M.; Notaro, I.; Sollazzo, A. An SFDI observer-based scheme for a general aviation aircraft. In Proceedings of the 2013 Conference on Control and Fault-Tolerant Systems (SysTol), Nice, France, 9–11 October 2013; pp. 152–157. [Google Scholar]
- Alwi, H.; Edwards, C. Robust sensor fault estimation for tolerant control of a civil aircraft using sliding modes. In Proceedings of the American Control Conference, Minneapolis, MN, USA, 14–16 June 2006; pp. 5704–5709. [Google Scholar]
- Varga, A.; Ossmann, D.; Joos, H. A fault diagnosis based reconfigurable longitudinal control system for managing loss of air data sensors for a civil aircraft. In Proceedings of the 19th IFAC World Congress 2014, Cape Town, South Africa, 24–29 August 2014; Volume 47, pp. 3489–3496. [Google Scholar]
- Mattei, M.; Ollio, L.; Scordamaglia, V. A set based approach for SFDI on small commercial aircraft. In Proceedings of the 2016 3rd Conference on Control and Fault-Tolerant Systems (SysTol), Barcelona, Spain, 7–9 September 2016; pp. 678–683. [Google Scholar]
- Mattei, M.; Paviglianiti, G.; Scordamaglia, V. Nonlinear observers with H
_{∞}performance for sensor fault detection and isolation: A linear matrix inequality design procedure. Control Eng. Pract.**2005**, 13, 1271–1281. [Google Scholar] [CrossRef] - Mattei, M.; Paviglianiti, G.; Scordamaglia, V. Nonlinear H¥ identity observers for sensor FDI: An application example. In Proceedings of the ICM’04—IEEE International Conference on Mechatronics, Istanbul, Turkey, 5 June 2004. [Google Scholar]
- Doucet, A.; Godsill, S.; Andrieu, C. On sequential Monte Carlo sampling methods for Bayesian filtering. Stat. Comput.
**2000**, 10, 197–208. [Google Scholar] [CrossRef] - Cheng, Y.; Crassidis, J. Particle Filtering for Sequential Spacecraft Attitude Estimation. In Proceedings of the AIAA Guidance, Navigation, and Control Conference and Exhibit, Providence, RI, USA, 16–19 August 2004. [Google Scholar]
- Oshman, Y.; Carmi, A. Attitude Estimation from Vector Observations Using a Genetic-Algorithm-Embedded Quaternion Particle Filter. J. Guid. Control Dyn.
**2006**, 29, 879–891. [Google Scholar] [CrossRef] - Carmi, A.; Oshman, Y. Adaptive Particle Filtering for Spacecraft Attitude Estimation from Vector Observations. J. Guid. Control Dyn.
**2009**, 32, 232–241. [Google Scholar] [CrossRef] - Jiang, X.-Y.; Ma, G.-F. Spacecraft attitude estimation from vector measurements using particle filter. In Proceedings of the 2005 International Conference on Machine Learning and Cybernetics, Guangzhou, China, 18–21 August 2005; Volume 2, pp. 682–687. [Google Scholar]
- Chagas, R.A.J.; Waldmann, J. Rao-Blackwellized particle filter with vector observations for satellite three-axis attitude estimation and control in a simulated testbed. Sba: Controle Automaç Ao Soc. Bras. De Autom.
**2012**, 23, 277–293. [Google Scholar] [CrossRef] [Green Version] - Chagas, R.; Waldmann, J. Nonlinear filtering in a simulated three-axis satellite attitude estimation and control tested. J. Aerosp. Eng. Sci. Appl.
**2010**, 2, 37–49. [Google Scholar] [CrossRef] [Green Version] - Kadirkamanathan, V.; Li, P.; Jaward, M.H.; Fabri, S.G. A sequential Monte Carlo filtering approach to fault detection and isolation in nonlinear systems. In Proceedings of the 39th IEEE Conference on Decision and Control (Cat. No.00CH37187), Sydney, NSW, Australia, 12–15 December 2000; Volume 5, pp. 4341–4346. [Google Scholar]
- Kadirkamanathan, V.; Li, P.; Jaward, M.H.; Fabri, S.G. Particle filtering-based fault detection in non-linear stochastic systems. Int. J. Syst. Sci.
**2002**, 33, 259–265. [Google Scholar] [CrossRef] - Dearden, R.; Clancy, D. Particle Filters for Real-Time Fault Detection in Planetary Rovers; Technical Report; National Aeronautics and Space Administration: Washington, DC, USA, 2002. [Google Scholar]
- Wang, E.; Jia, C.; Tong, G.; Qu, P.; Lan, X.; Pang, T. Fault detection and isolation in GPS receiver autonomous integrity monitoring based on chaos particle swarm optimization-particle filter algorithm. Adv. Space Res.
**2018**, 61, 1260–1272. [Google Scholar] [CrossRef] - Wei, T.; Huang, Y.; Chen, C.L.P. Adaptive Sensor Fault Detection and Identification Using Particle Filter Algorithms. IEEE Trans. Syst. Man, Cybern. Part C Appl. Rev.
**2009**, 39, 201–213. [Google Scholar] [CrossRef] - Duan, Z.; Cai, Z.; Yu, J. Adaptive Particle Filter for Unknown Fault Detection of Wheeled Mobile Robots. In Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing, China, 9–15 October 2006; pp. 1312–1315. [Google Scholar]
- Yu, J. A particle filter driven dynamic Gaussian mixture model approach for complex process monitoring and fault diagnosis. J. Process Control
**2012**, 22, 778–788. [Google Scholar] [CrossRef] - Zhang, Q.; Campillo, F.; Cerou, F.; Legland, F. Nonlinear system fault detection and isolation based on bootstrap particle filters. In Proceedings of the 44th IEEE Conference on Decision and Control, Seville, Spain, 15 December 2005; pp. 3821–3826. [Google Scholar]
- Yin, S.; Zhu, X. Intelligent Particle Filter and Its Application to Fault Detection of Nonlinear System. IEEE Trans. Ind. Electron.
**2015**, 62, 3852–3861. [Google Scholar] [CrossRef] - Hamzah, N.H.; Yaacob, S.; Muthusamy, H.; Hamzah, N. Comparative Study of Extended Kalman Filter and Particle Filter for Attitude Estimation in Gyroless Low Earth Orbit Spacecraft. In Advanced Computational Methods for Knowledge Engineering; Le Thi, H.A., Nguyen, N.T., Do, T.V., Eds.; Springer International Publishing: Cham, Switzerland, 2015; pp. 95–106. [Google Scholar]
- Zhang, C.; Taghvaei, A.; Mehta, P.G. Attitude estimation with feedback particle filter. In Proceedings of the 2016 IEEE 55th Conference on Decision and Control (CDC), Las Vegas, NV, USA, 12–14 December 2016; pp. 5440–5445. [Google Scholar]
- Hernáez, F.; Giribet, J.I. Comparison between linearized Kalman filters and particle filters applied to integrated navigation systems. In Proceedings of the 6th International ESA Conference on Guidance, Navigation and Control Systems, Loutraki, Greece, 17–20 October 2005. [Google Scholar]
- Ahmed, H.; Tahir, M. Accurate Attitude Estimation of a Moving Land Vehicle Using Low-Cost MEMS IMU Sensors. IEEE Trans. Intell. Transp. Syst.
**2017**, 18, 1723–1739. [Google Scholar] [CrossRef] - Hua, M.D.; Ducard, G.; Hamel, T.; Mahony, R. Introduction to Nonlinear Attitude Estimation for Aerial Robotic Systems. Aerosp. Lab
**2014**, AL08-04. [Google Scholar] - D’Amato, E.; Notaro, I.; Mattei, M.; Tartaglione, G. Attitude and position estimation for an UAV swarm using consensus Kalman filtering. In Proceedings of the 2015 IEEE Metrology for Aerospace (MetroAeroSpace), Benevento, Italy, 4–5 June 2015; pp. 519–524. [Google Scholar]
- Hyyti, H.; Visala, A. A DCM Based Attitude Estimation Algorithm for Low-Cost MEMS IMUs. Int. J. Navig. Obs.
**2015**, 2015, 503814. [Google Scholar] [CrossRef] [Green Version] - Costanzi, R.; Fanelli, F.; Monni, N.; Ridolfi, A.; Allotta, B. An attitude estimation algorithm for mobile robots under unknown magnetic disturbances. IEEE/ASME Trans. Mechatronics
**2016**, 21, 1900–1911. [Google Scholar] [CrossRef] - De Marina, H.G.; Espinosa, F.; Santos, C. Adaptive UAV attitude estimation employing unscented Kalman filter, FOAM and low-cost MEMS sensors. Sensors
**2012**, 12, 9566–9585. [Google Scholar] [CrossRef] [Green Version] - Chiella, A.C.; Teixeira, B.O.; Pereira, G.A. Quaternion-based robust attitude estimation using an adaptive unscented Kalman filter. Sensors
**2019**, 19, 2372. [Google Scholar] [CrossRef] [Green Version] - Cheng, Y.; Crassidis, J.L. Particle filtering for attitude estimation using a minimal local-error representation. J. Guid. Control. Dyn.
**2010**, 33, 1305–1310. [Google Scholar] [CrossRef] - Bar-Itzhack, I.Y. REQUEST-A recursive QUEST algorithm for sequential attitude determination. J. Guid. Control. Dyn.
**1996**, 19, 1034–1038. [Google Scholar] [CrossRef] [Green Version] - Psiaki, M.L. Attitude-determination filtering via extended quaternion estimation. J. Guid. Control. Dyn.
**2000**, 23, 206–214. [Google Scholar] [CrossRef] - Euston, M.; Coote, P.; Mahony, R.; Kim, J.; Hamel, T. A complementary filter for attitude estimation of a fixed-wing UAV. In Proceedings of the 2008 IEEE/RSJ international conference on intelligent robots and systems, Nice, France, 22–26 September 2008; pp. 340–345. [Google Scholar]
- Wu, J.; Zhou, Z.; Fourati, H.; Li, R.; Liu, M. Generalized linear quaternion complementary filter for attitude estimation from multisensor observations: An optimization approach. IEEE Trans. Autom. Sci. Eng.
**2019**, 16, 1330–1343. [Google Scholar] [CrossRef] - Madgwick, S. An efficient orientation filter for inertial and inertial/magnetic sensor arrays. Rep. X-Io Univ. Bristol (UK)
**2010**, 25, 113–118. [Google Scholar] - Blanco, J.L. A tutorial on se (3) transformation parameterizations and on-manifold optimization. Univ. Malaga, Tech. Rep
**2010**, 3, 6. [Google Scholar] - Valenti, R.G.; Dryanovski, I.; Xiao, J. Keeping a good attitude: A quaternion-based orientation filter for IMUs and MARGs. Sensors
**2015**, 15, 19302–19330. [Google Scholar] [CrossRef] [Green Version] - Park, S.; Horowitz, R.; Tan, C.W. Dynamics and control of a MEMS angle measuring gyroscope. Sensors Actuators A Phys.
**2008**, 144, 56–63. [Google Scholar] [CrossRef] - Vitali, A. Noise Analysis and Identification in MEMS Sensors, Allan, Time, Hadamard, Overlapping, Modified, Total Variance; Technical Report DT0064; STMicroelectronics: Geneva, Switzerland, 2016. [Google Scholar]
- Hoffmann, F.; Goddemeier, N.; Bertram, T. Attitude estimation and control of a quadrocopter. In Proceedings of the 2010 IEEE/RSJ International Conf. on Intelligent Robots and Systems, Taipei, Taiwan, 18–22 October 2010. [Google Scholar]
- D’Amato, E.; Mattei, M.; Notaro, I.; Scordamaglia, V. UAV Sensor FDI in Duplex Attitude Estimation Architectures Using a Set-Based Approach. IEEE Trans. Instrum. Meas.
**2018**, 67, 2465–2475. [Google Scholar] [CrossRef] - D’Amato, E.; Mattei, M.; Mele, A.; Notaro, I.; Scordamaglia, V. Fault tolerant low cost IMUS for UAVs. In Proceedings of the 2017 IEEE International Workshop on Measurement and Networking (MN), Naples, Italy, 27–29 September 2017; pp. 1–6. [Google Scholar]
- Simon, D. Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches; John Wiley & Sons: Hoboken, NJ, USA, 2006. [Google Scholar]
- Gordon, N.J.; Salmond, D.J.; Smith, A.F. Novel approach to nonlinear/non-Gaussian Bayesian state estimation. IEEE Proc. Radar Signal Process.
**1993**, 140, 107–113. [Google Scholar] [CrossRef] [Green Version] - Georgy, J.; Noureldin, A.; Korenberg, M.; Bayoumi, M. Modeling the Stochastic Drift of a MEMS-Based Gyroscope in Gyro/Odometer/GPS Integrated Navigation. Intell. Transp. Syst. IEEE Trans.
**2011**, 11, 856–872. [Google Scholar] [CrossRef] - Thrun, S.; Burgard, W.; Fox, D. Probabilistic Robotics; MIT Press: Cambridge, MA, USA, 2005. [Google Scholar]
- Dellaert, F.; Fox, D.; Burgard, W.; Thrun, S. Monte Carlo localization for mobile robots. In Proceedings of the 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C), Detroit, MI, USA, 10–15 May 1999; Volume 2, pp. 1322–1328. [Google Scholar]
- Li, T.; Sun, S.; Sattar, T.P.; Corchado, J.M. Fight sample degeneracy and impoverishment in particle filters: A review of intelligent approaches. Expert Syst. Appl.
**2014**, 41, 3944–3954. [Google Scholar] [CrossRef] [Green Version] - D’Amato, E.; Di Francesco, G.; Notaro, I.; Tartaglione, G.; Mattei, M. Nonlinear Dynamic Inversion and Neural Networks for a Tilt Tri-Rotor UAV. IFAC-PapersOnLine
**2015**, 48, 162–167. [Google Scholar] [CrossRef] - Rogne, R.H.; Johansen, T.A.; Fossen, T.I. Observer and IMU-based detection and isolation of faults in position reference systems and gyrocompasses with dual redundancy in dynamic positioning. In Proceedings of the 2014 IEEE Conference on Control Applications (CCA), Juan Les Antibes, France, 8–10 October 2014; pp. 83–88. [Google Scholar]
- Lu, P.; Van Eykeren, L.; van Kampen, E.; de Visser, C.C.; Chu, Q.P. Aircraft Inertial Measurement Unit Fault Identification with Application to Real Flight Data. J. Guid. Control. Dyn.
**2015**, 38, 2467–2475. [Google Scholar] [CrossRef] - Kolcio, K.O. Model-Based Fault Detection and Isolation System for Increased Autonomy. In Proceedings of the AIAA SPACE 2016, AIAA SPACE, Long Beach, CA, USA, 13–16 September 2016. [Google Scholar]
- Fourlas, G.K.; Karras, G.C.; Kyriakopoulos, K.J. Sensors fault diagnosis in autonomous mobile robots using observer—Based technique. In Proceedings of the 2015 International Conference on Control, Automation and Robotics, Singapore, 20–22 May 2015; pp. 49–54. [Google Scholar]
- Bhatti, U.I.; Ochieng, W.Y. Failure Modes and Models for Integrated GPS/INS Systems. J. Navig.
**2007**, 60, 327–348. [Google Scholar] [CrossRef]

**Figure 1.**Architecture of the proposed duplex IMU. On top, the Raspberry Pi 3B platform, running the SFDI algorithm and data logging. Below, Flip32-based IMU boards, with MPU6050 and HMC5883L MEMS sensors.

**Figure 2.**Inertial (E) and body-fixed (B) reference frames. Transition from the NED frame to the body frame is obtained with a sequence of three ordered rotations around the ${Z}_{E}$ axis, the ${Y}^{\prime}$ axis and the ${X}^{\u2033}$ axis of angles $\psi $, $\theta $ and $\varphi $, respectively.

**Figure 3.**Geometrical meaning of quaternion: a rotation around the $\zeta $ axis by a given angle $\alpha $ is defined by the unit quaternion $\tilde{\mathbf{q}}={[{\tilde{q}}_{0},{\tilde{q}}_{1},{\tilde{q}}_{2},{\tilde{q}}_{3}]}^{T}$, whose components are $\tilde{{q}_{1}}={\zeta}_{x}$, $\tilde{{q}_{2}}={\zeta}_{y}$, $\tilde{{q}_{3}}={\zeta}_{z}$ and $\tilde{{q}_{0}}=cos\alpha $.

**Figure 4.**State transition graph for SFDI. Normal state (green) $N={N}_{A}\cap {N}_{M}\cap {N}_{G}$, alert state (yellow) $A={A}_{A}\cap {A}_{M}\cap {A}_{G}$, fault state (red) $F=\left({\bigcap}_{i=1}^{2}{F}_{i}\right)\cap \left({\bigcap}_{i=1}^{2}{R}_{i}\right)$. Arrows denote event-driven state transitions.

**Figure 5.**A picture of the custom tri-rotor aircraft used in the flight test with the duplex hardware platform on top.

**Figure 6.**Data gathered from both IMUs during the flight test in the absence of faults. Black lines indicate data gathered by IMU-1 while red lines are for IMU-2.

**Figure 7.**Scenario F1. Black lines denote measurements/estimates from IMU-1, red lines are for measurements/estimates from IMU-2. Green lines are for SFDI output, gray areas highlight the time interval in which the SFDI algorithm is in the fault state. Vertical red dashed lines denote the time instants when the fault occurs and ends.

**Figure 8.**Scenario F2. Black lines denote measurements/estimates from IMU-1, red lines are for measurements/estimates from IMU-2. Green lines represent the output of the system, gray areas highlight the time interval in which the SFDI algorithm is in the fault state. Vertical red dashed lines denote the time instants when the fault occurs and ends.

**Figure 9.**Scenario F3. Black lines denote measurements/estimates from IMU-1, red lines are for measurements/estimates from IMU-2. Green lines represent the output of the system, gray areas highlight the time interval in which the SFDI algorithm is in the fault state. Vertical red dashed lines denote the time instants when the fault occurs and ends.

**Figure 10.**Scenario F4. Black lines denote measurements/estimates from IMU-1, red lines are for measurements/estimates from IMU-2. Green lines represent the output of the system, gray areas highlight the time interval in which the SFDI algorithm is in the fault state. Vertical red dashed lines denote the time instants when the fault occurs and ends.

**Figure 11.**Scenario F5. Black lines denote measurements/estimates from IMU-1, red lines are for measurements/estimates from IMU-2. Green lines represent the output of the system, gray areas highlight the time interval in which the SFDI algorithm is in the fault state. Vertical red dashed lines denote the time instants when the fault occurs and ends.

Name | MPU6050 Acc. | MPU6050 Gyro | HMC5883l Mag. |
---|---|---|---|

Dynamic range | $\pm 4$ g | $\pm 500$ deg s${}^{-1}$ | $\pm 2.5$ Gs |

Digital resolution | $0.122$ mg | $0.015$ deg s${}^{-1}$ | $2.095$ mGs |

Total RMS noise | n.d. | $0.05$ deg s${}^{-1}$ | n.d |

Noise density | $400\phantom{\rule{1.em}{0ex}}$gHz${}^{-2}$ | $0.005$ deg s${}^{-1}$ Hz${}^{-2}$ | n.d. |

Noise floor | n.d. | n.d. | 0.002 mGs |

**Table 2.**Parameters used in numerical results (${\mathcal{I}}_{4\times 4}$ represents the identity matrix of size $4\times 4$).

Parameter | Value |
---|---|

$\Delta $ | 4096 |

$\mathbf{v}$ | ${\left[\begin{array}{cccccc}0.0633& 0.3291& 0.5291& 0.2658& 0.2658& 0.2658\end{array}\right]}^{T}$ |

${\tau}_{a}$ | $0.2411$ |

${\tau}_{g}$ | $0.2853$ |

${\tau}_{m}$ | $0.0591$ |

$\mathsf{\Omega}$ | $24.213\xb7{\mathcal{I}}_{4\times 4}$ |

Number of Particles | Computation Time (s) | |
---|---|---|

Average | Standard Deviation | |

1024 | 0.0138 | 0.0012 |

2048 | 0.0303 | 0.0022 |

4096 | 0.0682 | 0.0030 |

8192 | 0.1573 | 0.0050 |

16,384 | 0.3401 | 0.0071 |

Number of Particles | Correct Detection (%) | Wrong Detection (%) | Detection Time (s) | Recovery Time (s) |
---|---|---|---|---|

1024 | 89.81 | 0.00 | 0.27 | 11.56 |

4096 | 99.50 | 0.00 | 0.27 | 6.96 |

16,384 | 92.81 | 0.30 | 0.27 | 4.07 |

32,768 | 92.81 | 0.00 | 0.27 | 4.04 |

$\mathit{\alpha}\xb7{\mathit{\tau}}_{\mathit{a}}$ | Correct Detection (%) | Wrong Detection (%) | Detection Time (s) | Recovery Time (s) |
---|---|---|---|---|

$\alpha =0.25$ | 99.40 | 24.36 | 0.00 | 18.67 |

$\alpha =0.75$ | 99.90 | 0.06 | 0.00 | 6.96 |

$\mathit{\alpha}\mathbf{=}\mathbf{1}$ | 99.50 | 0.00 | 0.27 | 6.96 |

$\alpha =1.5$ | 45.75 | 1.03 | 0.27 | 6.96 |

$\mathit{\alpha}\xb7\mathbf{\Omega}$ | Correct Detection (%) | Wrong Detection (%) | Detection Time (s) | Recovery Time (s) |
---|---|---|---|---|

$\alpha =0.25$ | 97.30 | 0.00 | 0.27 | 10.28 |

$\alpha =0.5$ | 97.30 | 0.00 | 0.27 | 7.61 |

$\alpha =0.75$ | 97.30 | 0.00 | 0.27 | 7.44 |

$\mathit{\alpha}\mathbf{=}\mathbf{1}$ | 99.50 | 0.00 | 0.27 | 6.96 |

$\alpha =4$ | 35.26 | 0.84 | 0.27 | 0.00 |

$\mathit{\alpha}\xb7\mathit{v}$ | Correct Detection (%) | Wrong Detection (%) | Detection Time (s) | Recovery Time(s) |
---|---|---|---|---|

$\alpha =0.25$ | 88.11 | 0.00 | 0.27 | 32.45 |

$\alpha =0.5$ | 85.71 | 0.00 | 0.27 | 4.43 |

$\alpha =0.75$ | 43.36 | 0.00 | 0.27 | 0.00 |

$\mathit{\alpha}\mathbf{=}\mathbf{1}$ | 99.50 | 0.00 | 0.27 | 6.96 |

$\alpha =4$ | 94.31 | 0.00 | 0.27 | 4.08 |

PF + SFDI | Previous Work [72] | |||
---|---|---|---|---|

$\mathit{X}$ | ${\mathit{t}}_{\mathit{d}}$ (s) | ${\mathit{e}}_{\mathit{max}}$ (rad) | ${\mathit{t}}_{\mathit{d}}$ (s) | ${\mathit{e}}_{\mathit{max}}$ (rad) |

F1 | 0.270 | 0.018 (on $\varphi $ angle) | 0.094 | 0.073 (on $\varphi $ angle) |

F2 | 0.960 | 0.024 (on $\psi $ angle) | 2.075 | 0.026 (on $\psi $ angle) |

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. |

© 2021 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

D’Amato, E.; Nardi, V.A.; Notaro, I.; Scordamaglia, V.
A Particle Filtering Approach for Fault Detection and Isolation of UAV IMU Sensors: Design, Implementation and Sensitivity Analysis. *Sensors* **2021**, *21*, 3066.
https://doi.org/10.3390/s21093066

**AMA Style**

D’Amato E, Nardi VA, Notaro I, Scordamaglia V.
A Particle Filtering Approach for Fault Detection and Isolation of UAV IMU Sensors: Design, Implementation and Sensitivity Analysis. *Sensors*. 2021; 21(9):3066.
https://doi.org/10.3390/s21093066

**Chicago/Turabian Style**

D’Amato, Egidio, Vito Antonio Nardi, Immacolata Notaro, and Valerio Scordamaglia.
2021. "A Particle Filtering Approach for Fault Detection and Isolation of UAV IMU Sensors: Design, Implementation and Sensitivity Analysis" *Sensors* 21, no. 9: 3066.
https://doi.org/10.3390/s21093066