Particle Filter for Fault Diagnosis and Robust Navigation of Underwater Robot

Transcription

1 Downloaded fro orbit.dtu.d on: May 2, 28 Particle Filter for Fault Diagnosis and Robust Navigation of Underwater Robot Zhao, Bo; Sjetne, Roger; Blane, Mogens; Duan, Fredri Published in: I E E E Transactions on Control Systes Technology Lin to article, DOI:.9/TCST Publication date: 24 Docuent Version Early version, also nown as pre-print Lin bac to DTU Orbit Citation (APA: Zhao, B., Sjetne, R., Blane, M., & Duan, F. (24. Particle Filter for Fault Diagnosis and Robust Navigation of Underwater Robot. I E E E Transactions on Control Systes Technology, 22(6, DOI:.9/TCST General rights Copyright and oral rights for the publications ade accessible in the public portal are retained by the authors and/or other copyright owners and it is a condition of accessing publications that users recognise and abide by the legal requireents associated with these rights. Users ay download and print one copy of any publication fro the public portal for the purpose of private study or research. You ay not further distribute the aterial or use it for any profit-aing activity or coercial gain You ay freely distribute the URL identifying the publication in the public portal If you believe that this docuent breaches copyright please contact us providing details, and we will reove access to the wor iediately and investigate your clai.

2 ACCEPTED BY IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, JANUARY 24 Particle Filter for Fault Diagnosis and Robust Navigation of Underwater Robot Bo Zhao, Roger Sjetne, Meber, IEEE, Mogens Blane, Senior Meber, IEEE, Fredri Duan Abstract A particle filter based robust navigation with fault diagnosis is designed for an underwater robot, where failure odes of sensors and thrusters are considered. The noinal underwater robot and its anoaly are described by a switchingode hidden Marov odel. By extensively running a particle filter on the odel, the fault diagnosis and robust navigation are achieved. Closed-loop full-scale experiental results show that the proposed ethod is robust, can diagnose faults effectively, and can provide good state estiation even in cases where ultiple faults occur. Coparing with other ethods, the proposed ethod can diagnose all faults within a single structure, it can diagnose siultaneous faults, and it is easily ipleented. Index Ters Fault diagnosis, fault tolerance, particle filter, switch-ode hidden Marov odel, ROV, underwater navigation. I. INTRODUCTION DUE TO the increasing requireents on safety, reliability, and availability, fault tolerant control systes design has drawn significant attention. This ethodology ais to prevent that a inor fault in a coponent leads to loss of syste functionality. Since fail-operational architectures are costly, a fault tolerant architecture is a natural choice for syste design []. Towards realization of a fault tolerant syste, the first step is to diagnose faults, where the ter fault diagnosis (FD includes fault detection, isolation, and estiation. Model-based analytical FD is discussed in detail in [], [2] and [3]. Residual signals in FD are functions of the inputs and easureents of the syste and a fault is detected whenever the residual exceeds a pre-designed threshold. This ethod is practical and has any applications, such as a detailed fault tolerant design for ship propulsion in [4] and fault tolerant design for station-eeping in [5]. Classical techniques in cobined state and paraeter estiation include the Kalan filter and the Extended Kalan Filter; see [6] for linear systes, and extended to a class of nonlinear systes in [7]. FD based on cobined state and paraeter estiation were discussed in [8]. Observers for nonlinear systes in using estiated faults as states and sliding ode techniques were suggested in [9]. An unscented B. Zhao, R. Sjetne, and F. Duan are with the Centre for Ships and Ocean Structures (CeSOS and the Departent of Marine Technology, Norwegian University of Science and Technology (NTNU, Trondhei, 749 Norway e-ails: Bo.Zhao@ntnu.no, Roger.Sjetne@ntnu.no, and Fredri.Duan@ntnu.no. M. Blane is with the Departent of Electrical Engineering, Technical University of Denar (DTU and with AMOS CoE, Institute of Technical Cybernetics, Norwegian University of Science and Technology (NTNU. (b@eletro.dtu.d Manuscript received March 2, 23; revised June 23, accepted January 2, 24. Kalan filter (UKF-based ethod was developed in [] to detect and isolate both teperature sensor and valve faults. Particle filters (PFs [], [2] inherited fro the Bayesian estiation and the Monte Carlo ethod, can also be used for this purpose. In [3] the authors developed a ethod that cobined state estiation by a PF in a ultiple odel environent and lielihood ratio approach to detect and isolate faults in stochastic nonlinear systes. Another approach using PFs for fault diagnosis was presented in [4], suggesting a hidden Marov odel (HMM with variable transition probabilities that were estiated online fro data and applied to ulti-sensor fusion for land vehicle positioning. The underwater vehicle proble is quite different. Artifacts in sensor signals, such as outliers and teporal dropouts of signals occur frequently and are essential to consider to obtain robust navigation. Another application of using a PF in fault detection is [5]. In this paper, the fault detection proble for a space rover was studied, and the so-called ris sensitive PF and variable resolution PF were reported. PFs have also been used for failure prognosis; see [6] that dealt with crac growth prediction. This paper eploys a switching-ode HMM as syste description. Failure odes are included in the odel, and a ethod is suggested using a PF to solve the FD proble on this odel, hereafter called a FDPF design. The coputational burden of the approach by [4] is eased significantly by forulating a odel with prior and fixed probabilities of failure odes. The paper discusses how to design for a tradeoff between false alar and detection probabilities, and it describes how this is ipleented. The resulting algorith is shown to be copact, easy to ipleent, and resulting in oderate coputational coplexity that aes the algorith run with ease in real-tie. Having introduced the PF design, the paper focus on the realization of a robust FDPF design for a reotely operated underwater vehicle (ROV. A prototype ipleentation is described together with results fro experients in full-scale at sea where the algorith was used in closed loop control of the ROV Minerva belonging to the Applied Underwater Robotics Lab (AUR-Lab at NTNU. ROVs are widely used in various safety critical operations, where accurate positioning and control of the ROVs are required. It is necessary to realize high-precision and fault tolerant ROV navigation. Applications of ROV integrated navigation are reported in [7], [8]. Sensor and actuator faults are encountered frequently in practice [9]; hence, fault detection and fault handling are essential for ROV reliability. For instance, the update intervals of ROV position easureent are relatively long and uneven. This phenoenon cobined

3 ACCEPTED BY IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, JANUARY 24 2 N E D y b sway pitch HPR eas. [] index of easureent (a x b roll surge yaw z b heave Fig.. ROV Minerva with ineatics notations. There are one lateral thruster, two longitudinal thrusters and two vertical thrusters. Photo: Johanna Jarnegren. DOF TABLE I NOTATIONS FOR ROV MODEL. Force and oent Linear and angular velocities otion in x b direction (surge X u N otion in y b direction (sway Y v E otion in z b direction (heave Z w D rotation about x b axis (roll K p φ rotation about y b axis (pitch M q θ rotation about z b axis (yaw N r ψ Positions and Euler angles with frequent outliers aes the navigation proble ore coplex. Detection of actuator faults and faults in navigation sensors are considered as a cobined proble in this paper, and experiental data are used to assess the occurrence of different fault types. Actuator reconfiguration possibilities are rather liited on the vessel considered and are not within the scope of this brief paper. The paper is organized as follows: Section II describes the odel of the ROV used in the experient and its failure odes. Section III, presents the switched Hidden Marov Model, the Particle Filter and the navigation syste design. Results fro ROV sea trials are presented in Section IV. II. PROBLEM DESCRIPTION A. ROV and ROV control syste The ROV Minerva [8], shown in Figure, is a SUBfighter 75 ROV. It is powered fro and counicates with a surface supply vessel through a 6 ubilical cable. Minerva is equipped with five thrusters and various navigation sensors. A hydroacoustic positioning reference (HPR syste, is used to easure the position of the ROV relative to a transducer on the surface vessel. A Doppler velocity log (DVL is installed to easure the ROV velocity. An inertial easureent unit (IMU provides turn rate and heading easureents. Depth is provided by the HPR and also by a pressure gauge. Kineatics: Adopting the notations of [2], the ineatics is described by the degrees-of-freedo (DOFs in Table I. The ROV is designed to be passive and stable in roll and pitch, the dynaics in these DOFs are ignored. The ineatic odel of the ROV is then given by the 4-DOF odel, η = R (ψ ν, ( where η = [N E D ψ] is the ROV position and heading in the NED (North-East-Down reference frae, ν = [u v w r] frequency easureent update interval [sec] (b Fig. 2. (a A segent of the HPR easureent with the easureent index along the horizontal axis. (b The histogra of the update intervals of the HPR easureent. is the body-fixed velocity and yaw rate vector, and R (ψ is the rotation atrix, which transfors a vector in the ROV body frae to NED frae coordinates. 2 Kinetics: Following [2] and [8], the ROV dynaics is, M ν = C (ν ν D (ν r ν r g (η + τ + w ν (2 ν c = T c ν c + w c, (3 where M is the cobined rigid body and added ass atrix, C (ν ν is the Coriolis and centripetal force, D (ν r ν r is the nonlinear and linear hydrodynaic daping, g (η is the restoring force, τ = [X Y Z N] is the control force and oent, and w ν is process noise. All the atrices are in R 4 4, and vectors are in R 4. Moreover, ν r = ν ν c is the relative velocity vector with respect to the current. The current velocity ν c can be odeled as (3, where T c is a diagonal atrix, and w c is process noise. B. Sensor odeling and anoaly analysis Hydroacoustic Position Reference (HPR syste: The HPR syste deterines the position of an underwater target. The HPR easureent is a vector with North and East positions and follows a ultivariate Gaussian distribution, p ( p A, η = N ([ I ] η, Σ A,, (4 where N denotes the Gaussian distribution function and Σ A, is the covariance of the easureent noise. When the ROV dives down to deeper water, the HPR update rate becoes nonunifor and lower than noinal. This phenoenon is naed HPR dropout and is given the sybol HPR,. In the HPR data logs, as shown in Figure 2, the HPR update intervals were uneven and larger than seconds in general. This is uch lower than the sapling tie of the control syste. The HPR dropout is odeled as p ( p A, η = N ( [ ], σ 2 A,dI, (5 The pressure gauge depth sensor gives ore reliable easureents than the HPR easureent in depth, so the depth coponent in the HPR easureent was not used.

4 ACCEPTED BY IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, JANUARY 24 3 where σ A,d is assigned to a very large positive nuber representing that the current easureent is noninforative. Figure 2 also indicates that the HPR easureent frequently suffers fro outliers, which is given the sybol HPR,2. The outliers are seen as saples fro p ( p A, η = N ([ I ] η, σ 2 A,oI, (6 where σa,o 2 I is the covariance of the distribution of the outliers, conceptually chosen as σa,o 2 I Σ A. 2 Doppler Velocity Log (DVL: The DVL is used to easure the velocity of the ROV in 3D space with respect to sea botto. We used only the two horizontal coponents in its easureent. The DVL easureent is transfored into the ROV body frae according to v D = R ROV DVL ( v DVL D + rb ROV DVL, (7 where v DVL D is the velocity easureent in the DVL instruent frae, R ROV DVL is the rotation atrix fro the instruent frae to the body frae, b ROV DVL is the lever ar between the two fraes expressed in body frae, and v D is the resulting ROV velocity easureent in the body frae. The DVL easureent is siply expressed as v D = [u D v D ]. Assuing the DVL easureent noise in each direction is independently identically norally distributed, the resulting DVL easureent v D, is p (v D, ν = N ([ I ] ν, σ 2 DI. (8 The DVL dropout proble, which is given the sybol DVL,, happens when the DVL looses sea botto tracing. It reports a sentinel ax velocity that indicates a lost velocity easureent. The DVL dropout is p (v D, ν = N ( [ ], σ 2 D,dI, (9 where σ 2 D,d is a large positive nuber. The DVL easureent ay suffer fro a bias. For instance, an alignent error in the instruent frae can cause a bearing and offset error of the easured velocity. This failure is given the sybol DVL,2. The size of this error is unnown and tie-varying, and a st-order Marov process is used to odel DVL bias, ḃ DVL = T DVL b DVL + w DVL, ( where b DVL R 2 is the bias, T DVL is a diagonal tie constant atrix, and w DVL R 2 is the driving noise. It follows that v D is biased fro the ROV velocity according to p (v D, ν, b DVL = N ([ I ] ν + b DVL, σ 2 DI. ( 3 IMU and depth sensors: The ROV is also equipped with an IMU as heading sensor and a depth sensor, such that ψ M, = [ ] η, (2 D P, = [ ] η. (3 C. Thruster control and thruster faults Thruster control: The Minerva ROV has five thrusters, lateral (l, vertical port (vp, vertical starboard (vs, longitudinal port (p, and longitudinal starboard (s. The thruster control, described in [8], odels the achieved thrust by τ = T Ku (4 where T R 4 5 is a thrust allocation atrix that reflects thruster position and orientation, K R 5 5 is a diagonal gain atrix, and u R 5 is the thrust rotational speed coand fro the controller. Since the thruster speed control is openloop due to lacing rotational speed sensors, an unnown error between the coanded and actual speed of each thruster could be present. We categorize the thruster anoalies into the following failure odes, The actual rotational speed is slightly lower than coanded. Such a fault is typically negligible, as it is handled by integral action. 2 The actual speed is uch lower than the coanded value. We assign it ode THR,t, where t {l, vp, vs, p, s} is the thruster index. 3 The actual speed is zero, e.g., due to blocing the propeller. We nae this failure ode zero thrust and assign it ode THR,t,2. We augent the state space with the vector α = [ al a vp a vs a p a s ] [, ] 5 to odel the thrust loss, whose entries represent the ratio between the desired thrust and the actual thrust for the 5 thrusters. It follows for the fault-free case that all entries of α are. Inserting this into the thruster odel (4 yields, D. Resulting ROV odel τ = diag {α} T Ku. (5 Collecting the ROV ineatics (, inetics (2, current (3, the easureents (4, (8, (2 (3, and thruster control (4 (5, we obtain the ROV odel as η =R (ψ ν M ν = C (ν ν D (ν r ν r g (η (6a (6b + diag {α} T Ku + w ν (6c ν c = T c ν c + w c, (6d ḃ DVL = T b DVL b DVL + w bdvl p (a t, = p (p A η = p (v D ν = { ρ(at,,[ THR,t, THR,t,2 ]=[ ] U(,,[ THR,t, THR,t,2 ]=[ ]. ρ(a t,,[ THR,t, THR,t,2 ]=[ ] { N ([I ]η,σa,[ HPR, HPR,2 ]=[ ] N ([ ]η,σ 2 A,d I,[ HPR, HPR,2 ]=[ ] N ([I ]η,σ 2 A,o I,[ HPR, HPR,2 ]=[ ] { N ([I ]ν+bdvl,σ 2 D I, DVL, = N ([ ]η,σ 2 D,d I, DVL, = where ρ( is the Dirac delta function. (6e (6f (6g (6h

5 ACCEPTED BY IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, JANUARY 24 4 III. PARTICLE FILTER BASED ROV ROBUST NAVIGATION Inherit fro the last cycle A FDPF-based ROV robust navigation syste is outlined in this section, introducing first soe eleents of the theory of fault diagnosis of switching-ode hodden Marov chain odels, and then focusing on the particular ipleentation for navigation filter design, which has soe novel eleents. A. A generalized odel for the syste with possible faults We describe a syste with possible failure odes by a switching-ode HMM. This is a cobined odel of a storder Marov chain representing the ode transitions, and a ban of HMMs representing the fault-free syste odel and the odels associated with different failure odes, that is, xt cycle x s s, s, s, 2 N, x x x s, i i. x Mode Mode Mode Tie update p x p p f, x, u p i j Pr p ji i w Prior PDF x s s, x, x Mode Mode Measureent update Posterior PDF p x p i w s, s, 2 N x s i, i x Mode i i w w p y i 2 N p x p i w 2 N p x p i w N x s N s x Pr ( + = δ j = δ i = p ij, (7a X + (X = x, u, = δ p (x + x, u, δ = f (x, u, δ (7b Y (X = x, u, = δ p (y x, u, δ = h (x, u, δ. (7c Mode Mode Resapling Mode Mode Mode Mode 2 N 2 N Equation (7a is the ode transition Marov chain to transfer the syste between syste odes, where = [ f ( f (2 f (N ] is a discrete rando variable defining the syste ode. The coponents f (p {, } (p {,..., N } denote whether the fault f (p occurs in the syste, Equation (7b is the process equation, and X R Nx is the state vector. Its realization is x. u R Nu is the input, p ( is a probability density function (PDF on R Nx, and f ( : R Nx R Nu {, } N R + is the state transition apping fro the states, input, and syste ode at tie to the PDF of the states for tie +. Equation (7c is called the easureent equation, where Y R Ny is a rando vector representing the easureent and y is its realization. p ( is a PDF on R Ny and h ( : R Nx R Nu {, } N R + is the easureent apping, which aps the states, input, and syste ode at current tie to the PDF of the easureent. Define an augented syste state vector to consist of the syste state and syste ode by 2 ξ = [δ x ]. Then the syste can be written as p ( ξ + ξ, u = Pr ( + = δ i = δ j p (x + ξ, u = p ji, f (ξ, u (8 p (y ξ, u = h (ξ, u. (9 B. The PF algorith of the switching-ode HMM Solving the FD proble in (8 and (9 includes to estiate the syste ode sequence δ. Assuing for instance the sequence δ has already been estiated and that f (p i is for i = l,,, then it can be concluded that the fault 2 Fro now on we do not distinguish a rando variable fro its realization. Both of the will be denoted by lowercase letters. Fig. 3. One cycle of the PF, divided into 3 steps by chain lines. These steps corresponding to the inherit, tie update and easureent update, respectively. In the inherit step of this figure, the particles are conceptually ordered N s fro left to right, for the purpose to show the replaceent of the particles in the tie update steps. f (p happened at tie l. The advantage of this approach is that the state estiation proble and the FD proble are solved at the sae tie within a single structure. We eploy a PF to solve this estiation proble. The proposed PF algorith, shown in Figure 3, is adapted fro the sapling iportance resaple PF (SIR-PF in []. SIR-PF is eployed in this wor because of its ease of ipleentation. However other PF algoriths could also be eployed. In the following, x (i and δ (i denote the state vector and syste ode of the ith particle, w (i is the corresponding weight, and N s is the nuber of particles. Inheriting fro the last cycle: The PF wors in a recursive anner. At tie it inherits the posteriori estiation p(ξ y :. The posterior density should be understood as a cobination of 2 N scaled distributions subjected to different syste odes. For instance, the posteriori distribution of x in ode δ (q is, p ( x = δ (q, y : Ns i= w(i ρ( x x (i 2 N q= w (i ρ δ (q,δ (i ρδ (q,δ (i, (2 where ρ ( again is the Dirac delta function, ρ s,t is the Kronecer delta function, and the denoinator in (2 should be non-zero as long as there is at least one particle in this ode.

6 ACCEPTED BY IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, JANUARY Tie update: The tie update process is to obtain the a priori estiation of the states as q ( ξ ξ:, y :. In the PF context, this is done by drawing saples fro an iportance density. The SIR-PF uses the ost convenient distribution p ( ξ ξ (i, u as the iportance density. We deterine the new syste state by drawing saples fro p ( ξ ξ (i, u = p ( x [x (i, δ(i ], u Pr ( δ δ (i. (2 At the end of the tie update process, the new positions ξ (i of the particles are obtained. 3 Measureent update: At this step the weights of the particles are updated according to the observation y. Given the current observation y and the iportance density p ( ξ ξ (i, this yields w (i w(i p( y ξ (i, u, (22 where p ( y ξ (i, u is defined by (9. 4 Resapling: To counteract the degeneracy proble, resapling is applied following [2]. C. Fault diagnosis using a particle filter It is concluded that the syste is in ode δ (q when = δ (q, tered the significant ode, has the largest arginal probability ass. This is given by ax Pr ( { = δ (i y : = q, i,..., 2 N }, i where the probability ass is obtained by arginalizing the distribution p (ξ y :, according to Pr ( = δ (q y : Ns i= w(j ρ. (23 δ (q,δ (i When the significant ode is other than fault-free, a fault is detected. The particular failure is deterined by the estiated syste ode, eaning that the fault is isolated, and its size is obtained fro the PF state estiate. An alternative ethod is otivated by the CUSUM algorith []. The tie cuulation of the arginal asses of the odes are used as indicators, and the detection of faults is done by assessing the behavior of these indicators. This is applied to diagnose the thruster fault in Section IV-F. D. ROV robust navigation syste The heave DOF can be controlled and observed separately fro other DOFs, since they are not coupled [2]. In addition, the corresponding sensor are reliable, so they are not included in our robust navigation design. Consequently, the failure odes regarding the two vertical thrusters are not considered in this design for siplicity. Since the failure odes are induced by different echaniss, it is reasonable to assue that their occurrence are independent fro each other. Hence, the ode transition Marov chain can also be designed independently for each equipent and then assebled. Table II shows the ode transition probabilities for the HPR failure odes, using Pr ( [ HPR, + HPR,2 + ] = δ (n [ HPR, HPR,2 ] = δ ( TABLE II THE MARKOV CHAIN FOR THE TRANSITION OF MODES HPR, AND HPR,2. δ ( HPR, p δ ( δ (n HPR,2 HPR, HPR, δ (n - N/A N/A N/A TABLE III THE MARKOV CHAIN FOR THE TRANSITION OF MODES THR,t, AND THR,t,2, t {l, p, s}. δ ( THR,t, p δ ( δ (n THR,t,2 THR,t, THR,t, δ (n = p δ ( δ (n. (24 The transition probabilities for [ HPR, HPR,2] = [ ] is not considered in this Marov chain since the syste adopts the HPR dropout ode whenever the HPR easureent is not available in the last sapling interval. The DVL dropout is handled as the HPR dropout, that is, the DVL easureent adopts (9 whenever its easureent is not available. The DVL bias, on the other hand, has been odeled as an additional state of the syste. Hence, there is no probabilistic ode switching for the DVL. The ode transition probabilities for the thruster odes are given in Table III, using Pr ( [ THR, + THR,2 + ] = δ (n [ THR, THR,2 ] = δ ( = p δ ( δ (n. (25 We then construct the syste ode vector = [ HPR, HPR,2 DVL, THR, THR,2 ]. The ode transition Marov chain subjects to the cobination of (24 and (25 is Pr ( + = δ j + = δ i = p ij. (26 The total switching-ode HMM for the ROV is obtained by cobining (26 and the ROV odel (6. There are 3 odes for the HPR, 3 odes for the DVL, and we consider the 3 odes for the 3 thrusters in the horizontal plane, resulting in = 243 odes for the PF. IV. FULL-SCALE TEST CAMPAIGN The full-scale test was perfored on October 7-8, 22, in the Trondheisfjord, Norway. The test was focused on the perforance of the proposed FDPF-based navigation filter in a real environent, real sensor easureents, and its cooperation with the ROV control syste. Besides the FDPFbased navigation filter, a Kalan-based navigation filter [8]

7 ACCEPTED BY IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, JANUARY 24 6 setpoint Guidance syste, d d Controller Thruster faults u TABLE VI HPR OUTLIERS DETECTION ˆ, ˆ PF based observer Sensor faults, Fig. 4. Bloc diagra of the control syste. Note that functionality has been ipleented to trigger the thruster and sensors failure odes to ae the failure testing ore predictable and practical. TABLE IV PARTICLE FILTER PARAMETERS IN THE EXPERIMENT A D dropout tie before the outlier [s] X 3 X X X X X X.6 X X X X X X. X X X X X outliers.6 X X X X size [] 2.4 X X X 2.8 X X 3.6 X 4. Paraeter Value notation unit w v [/s] N (,.2I T c [s].i w c [/s] N (,.I T bdvl [s].i w bdvl [/s] N (,.I [ σ D 2 /s 2] (.2 [ 2 Σ A 2 ] (.2I [ 2 Σ A,o 2 ] (3I 2 TABLE V NOTATIONS IN THE EXPERIMENTAL RESULTS FIGURES Notation Meaning N, E North and East position. u, v Surge and sway velocity. subscript e FDPF estiate. subscript d Desired position/velocity fro the guidance syste. subscript o Measureent without triggered failure odes. subscript off Estiation fro the sector heading KF in open-loop. upper bound of the FDPF estiate. lower bound of the FDPF estiate. A. Basic navigation The results of using the FDPF for state estiation of the ROV are shown in Figure 5a. In this test the ROV was controlled to ove along a triangular path with heading along the path. It is seen that the HPR easureent suffers fro outliers and low update rate. The state estiate by the FDPF is generally good, and this verifies its success as a state observer exposed to a nonunifor easureent update rate and easureent outliers. The state estiation perforance of the PF was close to the Kalan filter, but a sall highfrequency oscillation was observed due to tuning. B. Outliers Outliers islead the state estiation, for instance the estiation of the offline observer in Figure 5b jups after soe outliers. Therefore, they have to be detected. The principle of detecting outliers ebedded in the PF is just as the following hypothesis testing. Define { H, : HPR easureent at tie is fault free; was running in parallel and open-loop for coparison. Functionality has been ipleented to anually set the relevant thruster and sensor failure odes according to Figure 4 and: HPR outliers: A rando nuber taen fro a 2- diensional ultivariate noral distribution with zero ean and tunable variance is added to the current HPR easureent. HPR dropout is triggered by blocing the easureent. DVL dropout is triggered by blocing the easureent. DVL bias: A tunable constant bias is added to the DVL easureent. Loss of thrust: The thruster failure odes are activated by setting the gain α. In the test trial 2 particles were used in the PF. The ode transition Marov chain probabilities are shown in Table II and Table III. Other paraeters were set according to Table IV. Figures 5 to 9 show results fro the ROV sea trial. The annotation refers to Table V. In the design, syste noise is exaggerated to attenuate effects of uncertainties in hydrodynaic paraeters that constitute the C(ν and D(ν r ters in the inetics. This is coon practice when designing observers for arine systes. Uncertainties are also attenuated by the easureent update in the PF. H, : HPR easureent at tie is an outlier. It can be derived Pr ( H, p A,, ˆη Pr ( = Pr ( pa, H,, ˆη Pr (H, H, p A,, ˆη Pr ( p A, H,, ˆη Pr (H,, (27 where ˆη is the prior estiation, Pr (H, and Pr (H, are the transition probabilities defined in the Marov chain, and Pr ( p A, H,, ˆη and Pr ( pa, H,, ˆη are deterined by the easureent relation (6g. In the PF the sae process is iplicitly done by the iportance sapling (2 and the easureent update (22. This hypothesis testing is influenced by the variance of the prior estiation. Table VI shows the detectability of an outlier based on the size of the outlier and the dropout tie before the outlier easureent is ade. Naturally it is ore difficult to detect an outlier the longer the dropout tie is, and outliers with large aplitude are ore easily detected. To test outlier detection, outliers of nown aplitudes were injected during an position-eeping test. The statistics of the outlier detection is presented in Figure 5c, which coincides copares with Table VI. 3 X eans a outlier is deterined to be fault-free, and eans a outlier is deterined to be outlier.

8 ACCEPTED BY IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, JANUARY 24 7 North position [] surge velocity [] sway velocity [] East position [] Tie [s] (a N HPR N e N off E HPR E e E off u DVL u e u off North pos. [] East pos. [] 5 5 Dropout Outliers v DVL OK v e Outliers v No fault off Tie [s] (b No Noff Eo Eoff triggering detection frequency % 35.2% 53.3% 87.5% % >8 outliers size [] Fig. 5. (a Measureents and state estiation when the ROV was oving along a triangular path. (b Measureents and state estiation using HPR even during dropout. (c State estiation after detection and reoval of outliers. (c Total Detected North pos. [] East pos. [] surge vel. [/s] sway vel. [/s] HPR drop HPR OK 2 4 Tie [s] (a No Noff Eo Eoff udvl ue vdvl ve North pos. [] East pos. [] surge vel. [/s] sway vel. [/s] DVL drop DVL OK Tie [s] (b udvl ue uo vdvl ve vo North pos. [] East pos. [] surge vel. [/s] sway vel. [/s] Tie [s] Fig. 6. (a Measureents and state estiation subject to HPR dropouts. (b Measureents and state estiation subject to DVL dropout. (c Measureents and state estiation subject to DVL bias. DVL bias and estiation [/s] (c udvl ue uo vdvl ve vo ub vb ub,e vb,e C. HPR dropout Figure 6a shows the perforance of the FDPF when the HPR drops out for about 3sec while the ROV was oving straight in an Eastern direction. As discussed earlier, the HPR dropout does not have to be diagnosed since it is handled within the nonunifor sapling interval echanis, even for such a long interval, all the while the FDPF outputs a steady strea of position estiates. In state-of-the-art observers this is typically achieved by entering a dead-reconing ode [22], such that the position is estiated open-loop based on the thrust force and, possibly, velocity easureent. However, such an observer ay fail to estiate the ROV position correctly when then HPR drops out for too long tie, as seen in the large deviation between the ROV position and the offline filter estiation in Figure 6a. When the position easureent drops out here, the variance of the estiation cannot be reduced by new position inforation and the uncertainty of the estiation grows due to syste noise. This is observed by the increasing distance between the upper and lower bounds. The estiated position during the dropout is shown to be close to the original fault-free easureent, and this confirs the good dead-reconing capability of the FDPF. D. DVL dropout When the DVL drops out, the state estiation is based on the thrust force coand and the HPR easureent. The perforance of the FDPF is for this case presented in Figure 6b, showing that the estiated velocity is satisfactorily close to the original fault-free easured velocity during the DVL dropout. Siilar to the HPR dropout case, the variance of the velocity estiation increases during the DVL dropout. However, this increase only lasts for about 3 seconds until

9 ACCEPTED BY IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, JANUARY 24 8 probability.5 TABLE VII ANALYSIS OF THRUSTER FAULT DETECTION RESULT Thruster ode Percentage of estiated ode THR, THR, THR,2 THR, 9.% 5.5% 4.4% THR, not enough saples THR,2 77.2%.2%.6% h threshold P{D >h OK thr. } P{D >h zero thr.} P{D >h zero thr.} P{D >h OK thr. } P{D 2 >h OK thr. } P{D 2 >h zero thr.} P{D 2 >h zero thr.} P{D2 >h OK thr. } Fig. 7. Probabilities of detection P D and false alar P F versus value of threshold. The difference P D P F is an indicator for the tradeoff between P D and P F Fig. 8. North pos. [] Thrust output percentage East pos. [] % 75% 5% 25% % zero thr insuf. thr OK thr 5 Cuulated syste ode 5 Tie [s] Nd Noff Ed Eoff X Y N ode est. insuf. thr. zero thr. thresholds Measureents and state estiate when injecting thruster faults. it settles at a stationary value. This can be explained by the Bayesian properties of the PF: The particles with estiated velocity that are significantly different fro the actual velocity will not be supported by the observations since these will also yield a large difference between the estiated position and the easured position. This is an inforation bac propagation feature of PFs. E. DVL bias In the sea trial, a DVL bias was injected in the ROV surge direction while the ROV was in position-eeping operation. The bias was first increased slowly in steps before decreased bac to. The corresponding experiental responses are shown in Figure 6c. The bias estiate is close to the value of the anually triggered fault, especially if taing the variances of the DVL easureent noise and the syste noise into account. This indicates that the DVL bias is well diagnosed by the algorith. F. Insufficient thrust and zero thrust The reduced thrust failure ode was tested by decreasing the thrust fro the two surge-directed thrusters, as shown in the third graph in Fig. 9a, in such a way as to avoid spin of the ROV. The KF estiates rapidly diverged fro the true state when the fault was triggered. In contrast, the FDPF provides good state estiates during presence of these failures. When the thrust failure is present, the FDPF shows ore frequent confiration of zero thrust and insufficient thrust odes than in fault-free conditions. Table VII shows the thruster ode and estiated thruster ode during the period in Figure 9a, where the thruster odes are referring to the two thrusters in the surge direction. Thus, we obtain the probabilities Pr{ˆδ = THR,i δ = THR,j }, (i, j {,, 2}. Defining two statistics D i = j= n ρ(ˆδ j, THR,i (i =, 2 as the tie cuulations of the estiated syste odes, where n is a oving window length, we can calculate the probabilities of detection P {D i > h {δ n,, δ }}, where h is a threshold to be decided by exaining the false-alar-rate and tie-to-detection. As an exaple, Figure 7 shows the probability of detection and false alar against the threshold when the window size is (which is 5sec. This suggests to use the thresholds 2 for THR, and for THR,2 to restrain the false alars. To this end, the fourth graph in Figure 9a shows the D, D2 with oving window size, and the threshold. The statistic D exceeds the corresponding threshold in 6sec after the fault happens. Figure 8 shows the result when applying this ethod and thresholds to another set of experients, where a thruster fault was injected. The result validates the proposed diagnosis ethod where the two statistics helps to ae the fault detection robust to odel uncertainty. G. Multiple failure odes Two cobinations of siultaneous failures were tested. Fig. 9b shows the state estiation for a DVL drop-out during a HPR dropout. At the end of the 3sec HPR dropout, the position estiate has deviated about fro the easureent, while the velocity estiate is intact. The other ultiple failure ode test was assessing the syste response to HPR outliers during a DVL dropout period, and the results are shown in Figure 9c. When HPR outliers occur during a DVL dropout, the variance of the position estiate becoes large, and this aes detection of outliers increasingly difficult. The position and velocity estiation is again good. In this case one should also notice that the variance of the velocity estiation did not increase as uch as in the previous ultiple failure case (but ore than the fault-free case since the inforation fro the HPR is bac propagated to the velocity estiation through the syste odel.

10 ACCEPTED BY IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, JANUARY 24 9 North pos. [] East pos. [] Thrust output percentage % 75% 5% 25% % zero thr insuf. thr OK thr Cuulated syste ode Tie [s] (a Nd Noff Ed Eoff X Y N ode est. insuf. thr. zero thr. thresholds North pos. [] East pos. [] surge vel. [/s] sway vel. [/s] DVL drop DVL OK HPR drop HPR OK 2 2 Tie [s] (b No Eo udvl ue uo vdvl ve vo North pos. [] East pos. [] surge vel. [/s] sway vel. [/s] HPR drop Outliers Fault free Outliers No fault Tie [s] Fig. 9. (a Measureents and state estiate when thruster faults are injected. (b Measureents and state estiate subject to HPR dropout and DVL dropout. (c Measureents and state estiate subjected to HPR outliers and DVL dropout. (c No Eo udvl ue uo vdvl ve vo triggering detection V. CONCLUSION In this paper, we have proposed a PF-based algorith for fault diagnosis (FDPF built on a switching-ode hidden Marov odel. The algorith was applied to robustify the navigation of an ROV, where the navigation sensors and thrusters are vulnerable and fault diagnosis is essential. The design was tested in a full-scale ROV sea trial, for which the design process and the test responses have been presented and discussed in detail. The experiental results confir that the perforance of the fault diagnosis was generally good and that the proposed algorith provided robust and efficient state estiation for the ROV under different cobinations of failure odes, signal artifacts and disturbances. ACKNOWLEDGMENT We gratefully acnowledge funding fro the Research Council of Norway, project Arctic DP with consortiu partners Kongsberg Maritie, Statoil, and Det Norse Veritas. Experients were supported by AUR-Lab at NTNU. REFERENCES [] M. Blane, M. Kinnaert, J. Lunze, and M. Staroswieci, Diagnosis and Fault-Tolerant Control. Springer Berlin Heidelberg, 26. [2] Y. Zhang and J. Jiang, Bibliographical review on reconfigurable fault-tolerant control systes, Annual Reviews in Control, vol. 32, pp , 28. [3] I. Hwang, S. Ki, Y. Ki, and C. E. Seah, A survey of fault detection, isolation, and reconfiguration ethods, IEEE Trans. Autoatic Control, vol. 8, no. 3, pp , 2. [4] M. Blane, R. Izadi-Zaanabadi, and T. F. Lootsa, Fault onitoring and re-configurable control for a ship propulsion plant, Int. J. of Adaptive Control and Signal Proc., vol. 2, no. 8, pp , 998. [5] M. Blane, Diagnosis and fault-tolerant control for ship station eeping, in Intelligent Control, 25. Proc. IEEE Int. Syp., Mediterrean Conf. Control and Autoation, 25, pp [6] L. Ljung, Asyptotic behavior of the extended alan filter as a paraeter estiator for linear systes, IEEE Trans. Autoatic Control, vol. 24, pp. 36 5, 979. [7] W.-W. Zhou and M. Blane, Identification of a class of nonlinear statespace odels using rpe techniques, IEEE Trans. Autoatic Control, vol. 34, no. 3, pp , 989. [8] N. E. Wu, S. Thavaani, Y. Zhang, and M. Blane, Sensor fault asing of a ship propulsion syste, Control Eng. Practice, vol. 4, no., pp , 26. [9] C. Edwards, S. Spurgeon, and R. Patton, Sliding ode observers for fault detection and isolation, Autoatica, vol. 36 (4, pp , 2. [] L. Shang and G. Liu, Sensor and actuator fault detection and isolation for a high perforance aircraft engine bleed air teperature control syste, IEEE Trans. Control Systes Technology, vol. 9, no. 5, pp , 2. [] M. Arulapala, S. Masell, N. Gordon, and T. Clapp, A tutorial on particle filters for online nonlinear/non-gaussian bayesian tracing, IEEE Trans. Signal Processing, vol. 5, pp , 22. [2] A. Doucet and A. Johansen, The Oxford Handboo of Nonlinear Filtering. Cabridge University Press, 29, ch. A tutorial on particle filtering and soothing: Fifteen years later. [3] P. Li and V. Kadiraanathan, Particle filtering based lielihood ratio approach to fault diagnosis in nonlinear stochastic systes, IEEE Trans. Systes, Man, and Cybernetics - Part C: Applications and Reviews, vol. 3, pp , 2. [4] F. Caron, M. Davy, E. Duflos, and P. Vanheeghe, Particle filtering for ultisensor data fusion with switching observation odels: Application to land vehicle positioning, IEEE Trans. Signal Processing, vol. 55, no. 6, pp , 27. [5] V. Vera, G. Gordon, R. Sions, and S. Thrun, Real-tie fault diagnosis [robot fault diagnosis], IEEE Robotics Autoation Magazine, vol., no. 2, pp , 24. [6] M. E. Orchard and G. J. Vachtsevanos, A particle-filtering approach for on-line fault diagnosis and failure prognosis, Transactions of the Institute of Measureent and Control, vol. 3, pp , 29. [7] J. C. Kinsey and L. L. Whitcob, Preliinary field experience with the dvlnav integrated navigation syste for oceanographic subersibles, Control Eng. Practice, vol. 2, no. 2, pp , 24. [8] F. Duan, M. Ludvigsen, and A. Sørensen, Dynaic positioning syste for a sall size rov with experiental results, in OCEANS, 2 IEEE, 2. [9] G. Antonelli, A survey of fault detection/tolerance strategies for auvs and rovs, in Fault Diagnosis and Fault Tolerance for Mechatronic Systes: Recent Advances, F. Caccavale and L. Villani, Eds. Springer Berlin Heidelberg, 23, vol., pp [2] T. I. Fossen, Handboo of Marine Craft Hydrodynaics and Motion Control. John Wiley & Sons Ltd, UK, 2. [2] J. Liu, Metropolized independent sapling with coparisons to rejection sapling and iportance sapling, Statistics and Coputing, vol. 6, pp. 3 9, 996. [22] A. Sørensen, Marine Control Systes: Propulsion and Motion Control Systes of Ships and Ocean Structures, 2nd ed. Trondhei, Norway: Departent of Marine Technology at the Norwegian University of Science and Technology, 22.