A Fault-Tolerant Filtering Algorithm for SINS / DVL / MCP Integrated Navigation System

The Kalman filter (KF), which recursively generates a relatively optimal estimate of underlying system state based upon a series of observed measurements, has been widely used in integrated navigation system. Due to its dependence on the accuracy of system model and reliability of observation data, the precision of KFwill degrade or even diverge, when using inaccuratemodel or trustless data set. In this paper, a fault-tolerant adaptive Kalman filter (FTAKF) algorithm for the integrated navigation system composed of a strapdown inertial navigation system (SINS), a Doppler velocity log (DVL), and a magnetic compass (MCP) is proposed. The evolutionary artificial neural networks (EANN) are used in self-learning and training of the intelligent data fusion algorithm. The proposed algorithm can significantly outperform the traditional KF in providing estimation continuously with higher accuracy and smoothing the KF outputs when observation data are inaccurate or unavailable for a short period.The experiments of the prototype verify the effectiveness of the proposed method.


Introduction
Integration technology has become one of the key issues in navigation because of its capability of overcoming the drawbacks of single strapdown inertial navigation system (SINS), such as error accumulation and error swing over a period of time.The most commonly used navigation systems are radio and satellite based system, including global positioning system (GPS) [1] and Doppler velocity log (DVL) [2,3]; their performance is fundamentally limited by propagation of signals [4,5].The integrated navigation system proposed in this work is an economic navigating algorithm of high precision, in which the SINS is integrated with a DVL and a magnetic compass (MCP).
The Kalman filter (KF) is an effective optimal estimation algorithm and has been widely used in integrated navigation system since 1970s [6][7][8][9].On one hand, in order to achieve a better performance with a KF, an accurate system model and reliable observation data are indispensable.Moreover, the KF fails to estimate the state vector exactly when the observations noise increases [10][11][12].However, in real engineering applications, it is usually difficult to satisfy the above conditions.For example, undersea DVL's output will be interfered or broken owing to the cliffy sea bottom and other vessel reflections [13,14].In order to solve this problem, the following method is often adopted: if the observation data from DVL or MCP are temporarily inaccurate or inaccessible, the integrated navigation system can be compromised to work under pure inertial mode.But, it is evident that the measuring accuracy will deteriorate remarkably and the system error will accumulate or swing over time in pure inertial mode.After the system returns to the integration mode when the observation data are recovered, the position error will definitely not be compensated although the errors of velocities and attitude angles can be reduced.Therefore, a fault-tolerant filtering algorithm that can maintain 2 Mathematical Problems in Engineering the accuracy of an integrated navigation system through inexact or unavailable observation data is important and urgent [15][16][17].
Evolutionary programming, put forward by Fogel [18], is essential for random searching in that it boosts the survival of the fittest and organic evolution mechanism and searches for the optimal point in feasible space via population search strategy [19,20].Moreover, the artificial neural network (ANN), optimized by evolutionary programming, not only overcomes the obstacle of conventional ANN which may easily plunge into local minimum and requires long training time, but also avoids the problem in genetic algorithms caused by binary-coded and cross operation.Thanks to its superiorities, ANN has been applied in many fields such as financial forecasting [21], breast cancer diagnosis [22], and electricity supply industry [23], and so forth [24].
In this paper, a fault-tolerant integrated navigation system with an adaptive Kalman filter (AKF) based on evolutionary artificial neural networks (EANN) is proposed, in which the EANN is used in self-learning and training of the intelligent data fusion algorithm.The proposed fault-tolerant adaptive Kalman filter (FTAKF) algorithm is able to maintain a smooth output under the condition when observation data are inaccurate or unavailable.The prototype experiment indicates that the algorithm can efficiently outperform the traditional KF with a higher accuracy when observation data are inaccurate or unavailable.
This paper is organized as follows.In Section 2, the inertial error model is introduced and a corresponding mathematical model in state and measurement space is described.In Section 3, we propose the AKF algorithm and compare its characteristics and properties with those of traditional KF algorithm.A fast ANN based on evolutionary programming is proposed in Section 4. In Sections 5 and 6, applications of algorithm in SINS/DVL/MCP are presented and several tests are conducted to evaluate the performance of the intelligent navigation system with FTAKF.Conclusions are made in Section 7.

Error Model of Integrated Navigation System
(1) Inertial Error Model.Here we define the navigation coordinates frame as an east-north-vertical (ENV) geography coordinates frame.The attitude angle, velocity, and position errors are given in the following.Attitude angle error is determined by where   = [  ,   ,   ] Τ is the orientation error vector of the calculated platform represented in the navigation coordinates frame,    = [0,   cos ,   sin ] Τ represents the rotation projection of the earth onto the ENV axes with   = 7.292115×10 −5 rad/s to be the angular velocity of the rotation of the earth, and    = [−  /(  + ℎ),   /(  + ℎ), (  /(  + ℎ)) tan ] Τ denotes the angular velocity of the rotation of a navigation coordinates frame relative to the earth with   ,   , and   that represent the linear velocities, , , and ℎ that represent the position coordinates (longitude latitude and height, resp.), and   ,   that represent the radii of the curvatures along the meridian and parallel, respectively (Table 2).Moreover,   = [  ,   ,   ] is the gyro-drifts in the body coordinates frame with ε  = 0.The last term C   represents the attitude matrix, which can be expressed by the attitude angles , , and .
Given ] . ( Velocity error is defined as where V  = [  ,   ,   ] Τ viewed as the linear velocity error vector is determined by the specific force vector f  =[  ,   ,   ] Τ and the bias vector ∇  = [∇  , ∇  , ∇  ] of accelerometer in the body coordinates frame, along with    and    that represent the slow variations of    and    , respectively.Here, ∇ = 0. Position error is given by where , , and ℎ represent the position errors. (2) DVL Error Model.The velocity of DVL in the body coordinates frame Ṽ can be described as follows: Here, K = [  ,   ,   ] Τ is the scale factor error assumed to be random constant and  K = 0 and V  = [  ,   ,   ] Τ represents the linear velocity errors of DVL, which can be characterized by first-order Markov process as where (3) MCP Error Model.Heading attitude angle error of MCP   can be represented as first-order Markov process with where   is the time correlation coefficient and   is the input noise of MCP.

State Space.
The error models given in ( 1), ( 3)-( 4), and ( 6)-( 7) have a standard form given by where matrices F and Γ can be established from the above error model and W is the input (system) noise: where   ,   , and   are the white noise of the gyros and   ,   , and   are the white noise of the accelerometers.The state vector X of the integrated navigation system is defined as

Measurement Space.
Supposing that the error of the velocity of DVL in the navigation coordinates frame V   is mainly caused by platform misalignment, the error of Ṽ  can be described as where C   is a constant matrix which translates the velocity of DVL V  from the instrumental frame to the body coordinates frame.The perturbation of the attitude matrix C   is defined as where  is the calculating navigation coordinate frame.Substituting ( 12) into (11), it yields After differentiating the velocity of SINS and DVL, the measurement model can be written as where the measurement velocity vector Similarly, after differentiating the heading angle of SINS and MCP, the measurement model is given as where Based on the previous analysis, the measurement model is defined as where Z = [Δ  , Δ  , Δ  , Δ] Τ is the observation vector, H  is the observation matrix at the moment of  which can be established from ( 15) and ( 17), and vector   represents the Gaussian measurement noise with zero mean value and covariance matrix R.

AKF Algorithm to Integrated Navigation System
Given the differential equations, we can have a continuous state transition matrix F for error states.The continuous transition matrix can be discretized for KF application by using the first-order Taylor series approximation.The corresponding discrete transition matrix can be computed as where Δ is chosen as 0.01 s.Thus, the state equations and observation equation can be written as where X  is the 22-dimensional state vector at the moment of , Z  is the 4-dimensional observation vector at the moment of , Φ ,−1 is state-transfer-matrix from sample time  − 1 to , Γ −1 is the system noise matrix at the moment of  − 1, and W −1 is system noise.Here, {W  } and {  } are uncorrelated white noise sequences with zero mean.So we can have {W  W  } = Q    and {    } = R    .Q  is the variance matrix of system noise and R  is the variance matrix of observation noise.The equations of AKF based on the new information sequence are written as Compared with traditional KF equations, there are two differences.Firstly, Q  does not appear in above filter equations.That is to say, the statistical properties of the system noise are not needed in AKF, which is a highlight of this algorithm.Secondly, the initial value K 0 is needed, and it can be obtained by applying the traditional KF in the first loop.The AKF scheme of the integrated navigation system is shown in Figure 1.

Evolutionary Artificial Neural Networks
In recent years, ANN has been widely used in various fields, especially the backpropagation (BP) training [25].However, drawbacks like poor convergence and failure in obtaining the global optimum are the bottleneck in the practical applications [26].Articles [27][28][29] use genetic algorithms to optimize the parameters of neural networks.But there exist some imperfections.In this work, evolutionary programming is used to optimize the structure and connection weights in neural networks.

Artificial Neural Networks Model
. ANN theory has proved that three-layer feedforward networks containing only one hidden layer can be a nonlinear mapping of any system with any precision [30].According to this, the ANN model used in this paper consists of an input layer, a hidden layer, and an output layer.The network structure shown in Figure 2 treats the output navigation parameters of SINS as the input of the networks and the measurement errors as the output of the networks.Considering the limitation of underwater experimental environment, tests are conducted on a land vehicle, and the experimental area is flat and the speed of the vertical direction   can be regarded as zero.Hence, there are 5 nodes in input layer while there are 3 in output layer.
Based on the assumption that the number of hidden layer nodes is , we have x  ∈ R  , x  = (  0 ,   1 , . . .,   −1 ) T , and the connection weights between input and hidden layers are   ( = 0, 1, . . ., 4;  = 0, 1, . . .,  − 1), with thresholds to be Θ  .The connection weights between output and hidden layers are represented as   ( = 0, 1, 2), with thresholds to be Θ  .Then the output of each layer can be written as where  1 and  2 are transfer functions.
In order to study the nonlinear properties of biological neurons, usually  1 is chosen as a sigmoid function  1 () = 1/(1 +  − ) and  2 is chosen as a linear function  2 () = .During optimizing, the threshold value is treated as import power with value of −1.

Artificial Neural Networks Optimized by Evolutionary
Programming.Darwin's evolutionary programming is a new search algorithm which is largely dependent on the mutation operator, while the genetic algorithms rely greatly on the crossover operator.When using the evolutionary programming as an optimization algorithm, users need not to be constrained to a particular code structure (representation) or a mutation strategy (evolution operator).Therefore, the flexibility and convenience of evolutionary programming make it surpass the conventional genetic algorithm.The steps for ANN optimization with the network structure and parameters using evolutionary programming are described as follows.
(1) Initialization.Here the population of neural networks is set by an initial quantity and the number of hidden layer neurons and the connection weights are initialized randomly within a certain range.Population can be expressed as P = { 1 ,  2 , . . .,  2 }, where   ( = 1, 2, . . ., 2) is the weight distribution of the network and defined as   = [ 1 ,  2 , . . .,   ] with   ( = 1, 2, . . ., ) to be the weight between every two connecting nodes of the network.
(2) Performance Calculation and Categorization.The training procedure of neural networks uses the overall mean square deviation as a metric.Training will be terminated if the networks' overall mean square deviation is less than a preset value.Here, the set error is defined as the individual survival environment, which is the goal of every individual evolutionary.For each individual, the fitness function is defined as where  is the number of the imported samples and   and   are the desired and actual outputs of the node  of the output layer.Each individual is sorted in a descending order according to fitness.
(3) Termination Condition Check.When the best individual meets the requirement or number of loops exceeds the limitation of evolutionary generation, turn to Step ( 5); otherwise continue.
(4) New Population Generation.Firstly,  individuals in the current generation are picked as part of the next generation, while removing the rest.Secondly,  new individuals are generated from individuals selected by mutation.These 2 individuals gather to form the new population.Here Gaussian variation is used as the mutation operator.Moreover, according to the characteristics of BP neural networks, the actual error of network is introduced as the enlightening knowledge in the formula; namely, where   ( + 1) and   () are the weights of the ( + 1)th generation and ()th generation, respectively, with subscript to be  = 1, 2, . . ., , (0, 1) represents a standard normal distribution,  and  are definition coefficients,  max is the maximum training cycles of the network, and  is the number of the individual survival generations.When the new population is generated, jump to Step (2).
(5) Termination Cycle.Terminate the evolution procedure and then train the optimal neural networks using BP algorithm until all requirements are met.

Applications of Algorithm in SINS/DVL/MCP
On account of the previous algorithm, the FTAKF method based on EANN technology can be applied to the SINS/DVL/ MCP integrated system.The integrated navigation system with an EANN based AKF is shown in Figure 3.
When the observation data are diagnosed as available and reliable, EANN works at training state and uses SINS velocity and attitude information as input while the measurement errors are treated as the desired output.In this case switch 1 is connected and switch 2 is off.When the observation data is unavailable or unreliable, EANN works at forecasting state, with switch 1 to be off and switch 2 to be connected.Here, we use  2 detecting method to judge whether the observation data is available [31].
When no fault occurs, the residual of filter belongs to white noise, where The mean of V  is 0 and its variance is While the auxiliary navigation equipment fails, the statistical characteristic of measurements noise changes.Then, the residual V  no longer belongs to the white noise.So the mean of the residual V  does not equal 0, which can be used to judge whether the observation data is available.
Firstly, we define the detection function as where   obeys the  2 distribution and its degree of freedom is ; that is,   ∼  2 (), and  is the dimension of observations vector Z  .Then, the detection criteria are as follows: if   ≥   , observation data are unavailable, if   <   , observation data are available, where the threshold   determines the performance of the detection method, which should consider the leakage alarm rate and the false alarm rate.From ( 25), (26), and ( 27), we can conclude that the fault information needs to be great enough; then the value of the detection function   will be bigger than   .So this method is suitable for the mutant fault detection.The noise sudden increasing fault and gradual fault detection method will be specially introduced in another manuscript of the authors.

Experiments
Limited by the experimental condition, on-board experiments were carried out to instead of underwater tests.
The defect of the on-board experiments was that the movement of autonomous underwater vehicle (AUV) in the direction of depth cannot be simulated, but the other conditions are similar.
(  4. For the outputs of PHINS are used as the benchmark, the IMU which we used is fixed on a platform with the PHINS.The installation diagram is as Figure 5.The working scene of vehicle test inside the car is shown in Figure 6.(2) Experiment Route.The experiment was conducted on the campus of Southeast University.Experiment 1 (for a traditional KF algorithm).The experiment consists of three phases.
Phase 1. From start time as the yellow arrow on the map to 1000 seconds, this phase marked by yellow line on the map, the tested system works in the integration mode, and the observation data are available.
Phase 2. From 1000 seconds to 1600 seconds marked by black line on the map, the tested system works in the pure inertial mode, and the observation data are unavailable.
Phase 3. From 1600 seconds to 2800 seconds marked by light green line on the map, the tested system recovers from the pure inertial mode and works in the integration mode, while the observation data are renewed again.Figures 7-9 show the system's navigation errors of Experiment 1. Figure 10 shows the navigation route of Experiment 1.
According to Figures 7 to 9, during Phase 2, the horizontal attitude errors are bigger than the attitude errors when the system is working in the integrated mode.For lack of outside velocity and yaw information, the yaw error, velocity error, and the position errors are increased fast as time grows when the system works in the pure inertial mode, and the precision of the system depends highly on IMU's accuracies.From 1600 seconds, the observation data are available again and the system returns to the integration mode.The attitude and velocity errors can be lessened due to correction effects of KF, while the position errors cannot be eliminated.

(3) Experiment Results
Experiment 2 (for the FTAKF algorithm based on EANN).The experiment also consists of three phases like Experiment 1.In order to verify the effects of evolutionary neural networks, the observation data will be unavailable during Phase 2 from 1000 to 1600 seconds; the tested system will work in the fault-tolerant filtering mode instead of the pure inertial mode.
Experiment results shown in Figures 11-13 indicate that when the observation data are unavailable, the evolutionary neural network, which has completed training process, works on the forecasting mode and provides KF with required inputs.Thus, the KF can keep operating in a continuous manner and the system precision can be maintained stably.Figure 14 shows the navigation route of Experiment 2.

Conclusion
In this paper, aimed at addressing the problems of inaccurate model and unreliable observation data for KF in practical applications of AUV (autonomous underwater vehicle), a FTAKF algorithm for integrated navigation system based on EANN is proposed and tested.The conclusions are listed as follows: (1) The performance of a traditional KF is vulnerable due to its dependence on precision of the system model, such as state equation, observation equation, and noise statistic characters.In order to reduce the dependence on the precision of the system model, an AKF algorithm is designed.
(2) The FTAKF algorithm based on EANN is capable of maintaining a smooth filter output even under the severe condition when observation data are inaccurate or unavailable and of enhancing the precision and reliability of integrated navigation system.

Figure 3 :
Figure 3: Integrated navigation system blocks diagram based on evolutionary neural networks.

Figure 5 :
Figure 5: Experimental equipment: tested system and comparison unit.

Table 1 :
IMU parameters of tested system.

Table 2 :
Statistical results of horizontal position errors.