Mathematical Problems in Engineering

Volume 2015, Article ID 581909, 12 pages

http://dx.doi.org/10.1155/2015/581909

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

^{1}Key Laboratory of Micro-Inertial Instrument and Advanced Navigation Technology, Ministry of Education, School of Instrument Science & Engineering, Southeast University, Nanjing 210096, China^{2}Industrial Center, Nanjing Institute of Technology, Nanjing 211167, China^{3}Henan University of Technology, Zhengzhou 450007, China

Received 3 July 2014; Revised 13 April 2015; Accepted 15 April 2015

Academic Editor: Filippo Ubertini

Copyright © 2015 Xiaosu Xu et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### Abstract

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 KF will degrade or even diverge, when using inaccurate model 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.

#### 1. 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–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–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 the accuracy of an integrated navigation system through inexact or unavailable observation data is important and urgent [15–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.

#### 2. Mathematical Model for Filter

##### 2.1. 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 bywhere is the orientation error vector of the calculated platform represented in the navigation coordinates frame, represents the rotation projection of the earth onto the ENV axes with to be the angular velocity of the rotation of the earth, and 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 . The last term represents the attitude matrix, which can be expressed by the attitude angles , , and .

Given the definition , can be written asVelocity error is defined aswhere viewed as the linear velocity error vector is determined by the specific force vector and the bias vector of accelerometer in the body coordinates frame, along with and that represent the slow variations of and , respectively. Here, .

Position error is given bywhere , , and represent the position errors.

*(2) DVL Error Model*. The velocity of DVL in the body coordinates frame can be described as follows:Here, is the scale factor error assumed to be random constant and and represents the linear velocity errors of DVL, which can be characterized by first-order Markov process aswhere is the time correlation coefficient of this Markov process and is the input noise of DVL.

*(3) MCP Error Model.* Heading attitude angle error of MCP can be represented as first-order Markov process withwhere is the time correlation coefficient and is the input noise of MCP.

##### 2.2. State Space

The error models given in (1), (3)-(4), and (6)-(7) have a standard form given bywhere matrices and can be established from the above error model and 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 of the integrated navigation system is defined as

##### 2.3. Measurement Space

Supposing that the error of the velocity of DVL in the navigation coordinates frame is mainly caused by platform misalignment, the error of can be described as where is a constant matrix which translates the velocity of DVL from the instrumental frame to the body coordinates frame. The perturbation of the attitude matrix is defined aswhere is the calculating navigation coordinate frame. Substituting (12) into (11), it yieldsAfter differentiating the velocity of SINS and DVL, the measurement model can be written aswhere the measurement velocity vector and can be calculated from Similarly, after differentiating the heading angle of SINS and MCP, the measurement model is given aswhereBased on the previous analysis, the measurement model is defined aswhere is the observation vector, 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 .

#### 3. AKF Algorithm to Integrated Navigation System

Given the differential equations, we can have a continuous state transition matrix 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 aswhere is chosen as 0.01 s. Thus, the state equations and observation equation can be written aswhere is the 22-dimensional state vector at the moment of , is the 4-dimensional observation vector at the moment of , is state-transfer-matrix from sample time to , is the system noise matrix at the moment of , and is system noise. Here, and are uncorrelated white noise sequences with zero mean. So we can have and . is the variance matrix of system noise and 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, 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 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.