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

Authors: Xiaosu Xu; Peijuan Li; Jian-juan Liu
Journal: Mathematical Problems in Engineering
 (2015)

---

## 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.


---

## Body

## 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 Section2, 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 by(1)
ϕ
˙
n
=
δ
ω
i
e
n
+
δ
ω
e
n
n
-
ω
i
e
n
+
ω
e
n
n
×
ϕ
n
+
C
b
n
ε
b
,where ϕ
n
=
[
ϕ
E
,
ϕ
N
,
ϕ
U
]
Τ is the orientation error vector of the calculated platform represented in the navigation coordinates frame, ω
i
e
n
=
[
0
,
ω
i
e
cos
⁡
L
,
ω
i
e
sin
⁡
L
]
Τ represents the rotation projection of the earth onto the ENV axes with ω
i
e
=
7.292115
×
1
0
-
5
r
a
d
/
s to be the angular velocity of the rotation of the earth, and ω
e
n
n
=
[
-
V
N
/
R
n
+
h
,
V
E
/
R
e
+
h
,
(
V
E
/
(
R
e
+
h
)
)
tan
⁡
L
]
Τ denotes the angular velocity of the rotation of a navigation coordinates frame relative to the earth with V
E, V
N, and V
U that represent the linear velocities, λ, L, and h that represent the position coordinates (longitude latitude and height, resp.), and R
n, R
e that represent the radii of the curvatures along the meridian and parallel, respectively (Table 2). Moreover, ε
b
=
[
ε
x
,
ε
y
,
ε
z
] is the gyro-drifts in the body coordinates frame with ε
˙
b
=
0. The last term C
b
n represents the attitude matrix, which can be expressed by the attitude angles ψ, γ, and θ.Given the definition[
s
ψ
,
c
ψ
,
s
γ
,
c
γ
,
s
θ
,
c
θ
]
Τ
=
[
sin
⁡
ψ
,
cos
⁡
ψ
,
sin
⁡
γ
,
cos
⁡
γ
,
sin
⁡
θ
,
cos
⁡
θ
]
Τ, C
b
n can be written as(2)
C
b
n
=
c
γ
c
ψ
+
s
γ
s
ψ
s
θ
s
ψ
c
θ
s
γ
c
ψ
-
c
γ
s
ψ
s
θ
-
c
γ
s
ψ
+
s
γ
c
ψ
s
θ
c
ψ
c
θ
-
s
γ
s
ψ
-
c
γ
c
ψ
s
θ
-
s
γ
c
θ
s
θ
c
γ
c
θ
.Velocity error is defined as(3)
δ
V
˙
n
=
f
n
×
ϕ
n
-
2
δ
ω
i
e
n
+
δ
ω
e
n
n
×
V
n
-
2
ω
i
e
n
+
ω
e
n
n
×
δ
V
n
+
C
b
n
∇
b
,where δ
V
n
=
[
δ
V
E
,
δ
V
N
,
δ
V
U
]
Τ viewed as the linear velocity error vector is determined by the specific force vector f
n
=
[
f
E
,
f
N
,
f
U
]
Τ and the bias vector ∇
b
=
[
∇
x
,
∇
y
,
∇
z
] of accelerometer in the body coordinates frame, along with δ
ω
i
e
n and δ
ω
e
n
n that represent the slow variations of ω
i
e
n and ω
e
n
n, respectively. Here, ∇
˙
b
=
0.Position error is given by(4)
δ
L
˙
=
δ
V
N
R
n
+
h
-
V
N
R
n
+
h
2
δ
h
,
δ
λ
˙
=
δ
V
E
R
e
+
h
s
e
c
L
+
V
E
R
e
+
h
s
e
c
L
t
a
n
L
δ
L
-
V
E
R
e
+
h
2
s
e
c
L
δ
h
,
δ
h
˙
=
δ
V
U
,where δ
λ, δ
L, and δ
h represent the position errors.(2) DVL Error Model. The velocity of DVL in the body coordinates frame V
~
d can be described as follows:(5)
V
~
d
=
I
+
δ
K
V
d
+
δ
V
d
.Here, δ
K
=
[
δ
K
d
x
,
δ
K
d
y
,
δ
K
d
z
]
Τ is the scale factor error assumed to be random constant and δ
K
˙
=
0 and δ
V
d
=
[
δ
V
d
x
,
δ
V
d
y
,
δ
V
d
z
]
Τ represents the linear velocity errors of DVL, which can be characterized by first-order Markov process as(6)
δ
V
˙
d
x
=
-
1
τ
d
x
δ
V
d
x
+
w
d
x
,
δ
V
˙
d
y
=
-
1
τ
d
y
δ
V
d
y
+
w
d
y
,
δ
V
˙
d
z
=
-
1
τ
d
z
δ
V
d
z
+
w
d
z
,where τ
d
=
[
τ
d
x
,
τ
d
y
,
τ
d
z
]
Τ is the time correlation coefficient of this Markov process and w
d
=
[
w
d
x
,
w
d
y
,
w
d
z
]
Τ is the input noise of DVL.(3) MCP Error Model. Heading attitude angle error of MCP δ
ψ
m can be represented as first-order Markov process with(7)
δ
ψ
˙
m
=
-
1
τ
m
δ
ψ
m
+
w
m
,where τ
m is the time correlation coefficient and w
m 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 by(8)
X
˙
=
F
·
X
+
Γ
·
W
,where matrices F and Γ can be established from the above error model and W is the input (system) noise:(9)
W
=
w
g
x
,
w
g
y
,
w
g
z
,
w
a
x
,
w
a
y
,
w
a
z
,
w
d
x
,
w
d
y
,
w
d
z
,
w
m
,where w
g
x, w
g
y, and w
g
z are the white noise of the gyros and w
a
x, w
a
y, and w
a
z are the white noise of the accelerometers. The state vector X of the integrated navigation system is defined as(10)
X
=
δ
V
E
,
δ
V
N
,
δ
V
U
,
ϕ
E
,
ϕ
N
,
ϕ
U
,
δ
L
,
δ
λ
,
δ
h
,
∇
x
,
∇
y
,
∇
z
,
ε
x
,
ε
y
,
ε
z
,
δ
V
d
x
,
δ
V
d
y
,
δ
V
d
z
,
δ
K
d
x
,
δ
K
d
y
,
δ
K
d
z
,
δ
ψ
m
.


### 2.3. Measurement Space

Supposing that the error of the velocity of DVL in the navigation coordinates frameV
d
n is mainly caused by platform misalignment, the error ofV
~
d
n can be described as (11)
V
~
d
n
=
V
d
n
+
δ
V
d
n
=
C
b
n
C
d
b
V
d
+
δ
C
b
n
C
d
b
V
d
=
V
d
n
+
δ
C
b
n
C
d
b
V
d
,where C
d
b is a constant matrix which translates the velocity of DVL V
d from the instrumental frame to the body coordinates frame. The perturbation of the attitude matrix δ
C
b
n is defined as(12)
δ
C
b
n
=
I
-
C
n
p
C
b
n
,where p is the calculating navigation coordinate frame. Substituting (12) into (11), it yields(13)
V
~
d
n
=
V
d
n
+
I
-
C
n
p
C
b
n
C
d
b
V
d
.After differentiating the velocity of SINS and DVL, the measurement model can be written as(14)
Δ
V
n
=
V
~
n
-
V
~
d
n
+
η
d
=
V
n
+
δ
V
n
-
V
d
n
-
I
-
C
n
p
C
b
n
C
d
b
V
d
+
η
d
=
δ
V
n
-
I
-
C
n
p
C
b
n
C
d
b
V
d
+
η
d
,where the measurement velocity vector Δ
V
n
=
[
Δ
V
E
,
Δ
V
N
,
Δ
V
U
] and δ
V
d
n
=
(
I
-
C
n
p
)
C
b
n
C
d
b
V
d can be calculated from (15)
δ
V
d
E
=
ϕ
U
V
N
-
ϕ
N
V
U
+
C
b
n
1,1
δ
V
d
x
+
C
b
n
1,2
δ
V
d
y
+
C
b
n
1,3
δ
V
d
z
+
C
b
n
1,1
V
E
δ
K
d
x
+
C
b
n
1,2
V
N
δ
K
d
y
+
C
b
n
1,3
V
U
δ
K
d
z
,
δ
V
d
N
=
ϕ
E
V
U
-
ϕ
U
V
E
+
C
b
n
2,1
δ
V
d
x
+
C
b
n
2,2
δ
V
d
y
+
C
b
n
2,3
δ
V
d
z
+
C
b
n
2,1
V
E
δ
K
d
x
+
C
b
n
2,2
V
N
δ
K
d
y
+
C
b
n
2,3
V
U
δ
K
d
z
,
δ
V
d
U
=
ϕ
N
V
E
-
ϕ
E
V
N
+
C
b
n
3,1
δ
V
d
x
+
C
b
n
3,2
δ
V
d
y
+
C
b
n
3,3
δ
V
d
z
+
C
b
n
3,1
V
E
δ
K
d
x
+
C
b
n
3,2
V
N
δ
K
d
y
+
C
b
n
3,3
V
U
δ
K
d
z
.Similarly, after differentiating the heading angle of SINS and MCP, the measurement model is given as(16)
Δ
ψ
=
ψ
~
-
ψ
~
m
+
η
m
=
ψ
+
δ
ψ
-
ψ
m
-
δ
ψ
m
+
η
m
=
δ
ψ
-
δ
ψ
m
+
η
m
,where(17)
δ
ψ
=
-
C
b
n
1,2
·
C
b
n
3,2
C
b
n
1,2
2
+
C
b
n
2,2
2
ϕ
E
-
C
b
n
2,2
·
C
b
n
3,2
C
b
n
1,2
2
+
C
b
n
2,2
2
ϕ
N
+
ϕ
U
.Based on the previous analysis, the measurement model is defined as(18)
Z
k
=
H
k
X
k
+
η
k
,where Z
=
[
Δ
V
E
,
Δ
V
N
,
Δ
V
U
,
Δ
ψ
]
Τ is the observation vector, H
k is the observation matrix at the moment of k which can be established from (15) and (17), and vector η
k represents the Gaussian measurement noise with zero mean value and covariance matrix R.


## 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 by(1)
ϕ
˙
n
=
δ
ω
i
e
n
+
δ
ω
e
n
n
-
ω
i
e
n
+
ω
e
n
n
×
ϕ
n
+
C
b
n
ε
b
,where ϕ
n
=
[
ϕ
E
,
ϕ
N
,
ϕ
U
]
Τ is the orientation error vector of the calculated platform represented in the navigation coordinates frame, ω
i
e
n
=
[
0
,
ω
i
e
cos
⁡
L
,
ω
i
e
sin
⁡
L
]
Τ represents the rotation projection of the earth onto the ENV axes with ω
i
e
=
7.292115
×
1
0
-
5
r
a
d
/
s to be the angular velocity of the rotation of the earth, and ω
e
n
n
=
[
-
V
N
/
R
n
+
h
,
V
E
/
R
e
+
h
,
(
V
E
/
(
R
e
+
h
)
)
tan
⁡
L
]
Τ denotes the angular velocity of the rotation of a navigation coordinates frame relative to the earth with V
E, V
N, and V
U that represent the linear velocities, λ, L, and h that represent the position coordinates (longitude latitude and height, resp.), and R
n, R
e that represent the radii of the curvatures along the meridian and parallel, respectively (Table 2). Moreover, ε
b
=
[
ε
x
,
ε
y
,
ε
z
] is the gyro-drifts in the body coordinates frame with ε
˙
b
=
0. The last term C
b
n represents the attitude matrix, which can be expressed by the attitude angles ψ, γ, and θ.Given the definition[
s
ψ
,
c
ψ
,
s
γ
,
c
γ
,
s
θ
,
c
θ
]
Τ
=
[
sin
⁡
ψ
,
cos
⁡
ψ
,
sin
⁡
γ
,
cos
⁡
γ
,
sin
⁡
θ
,
cos
⁡
θ
]
Τ, C
b
n can be written as(2)
C
b
n
=
c
γ
c
ψ
+
s
γ
s
ψ
s
θ
s
ψ
c
θ
s
γ
c
ψ
-
c
γ
s
ψ
s
θ
-
c
γ
s
ψ
+
s
γ
c
ψ
s
θ
c
ψ
c
θ
-
s
γ
s
ψ
-
c
γ
c
ψ
s
θ
-
s
γ
c
θ
s
θ
c
γ
c
θ
.Velocity error is defined as(3)
δ
V
˙
n
=
f
n
×
ϕ
n
-
2
δ
ω
i
e
n
+
δ
ω
e
n
n
×
V
n
-
2
ω
i
e
n
+
ω
e
n
n
×
δ
V
n
+
C
b
n
∇
b
,where δ
V
n
=
[
δ
V
E
,
δ
V
N
,
δ
V
U
]
Τ viewed as the linear velocity error vector is determined by the specific force vector f
n
=
[
f
E
,
f
N
,
f
U
]
Τ and the bias vector ∇
b
=
[
∇
x
,
∇
y
,
∇
z
] of accelerometer in the body coordinates frame, along with δ
ω
i
e
n and δ
ω
e
n
n that represent the slow variations of ω
i
e
n and ω
e
n
n, respectively. Here, ∇
˙
b
=
0.Position error is given by(4)
δ
L
˙
=
δ
V
N
R
n
+
h
-
V
N
R
n
+
h
2
δ
h
,
δ
λ
˙
=
δ
V
E
R
e
+
h
s
e
c
L
+
V
E
R
e
+
h
s
e
c
L
t
a
n
L
δ
L
-
V
E
R
e
+
h
2
s
e
c
L
δ
h
,
δ
h
˙
=
δ
V
U
,where δ
λ, δ
L, and δ
h represent the position errors.(2) DVL Error Model. The velocity of DVL in the body coordinates frame V
~
d can be described as follows:(5)
V
~
d
=
I
+
δ
K
V
d
+
δ
V
d
.Here, δ
K
=
[
δ
K
d
x
,
δ
K
d
y
,
δ
K
d
z
]
Τ is the scale factor error assumed to be random constant and δ
K
˙
=
0 and δ
V
d
=
[
δ
V
d
x
,
δ
V
d
y
,
δ
V
d
z
]
Τ represents the linear velocity errors of DVL, which can be characterized by first-order Markov process as(6)
δ
V
˙
d
x
=
-
1
τ
d
x
δ
V
d
x
+
w
d
x
,
δ
V
˙
d
y
=
-
1
τ
d
y
δ
V
d
y
+
w
d
y
,
δ
V
˙
d
z
=
-
1
τ
d
z
δ
V
d
z
+
w
d
z
,where τ
d
=
[
τ
d
x
,
τ
d
y
,
τ
d
z
]
Τ is the time correlation coefficient of this Markov process and w
d
=
[
w
d
x
,
w
d
y
,
w
d
z
]
Τ is the input noise of DVL.(3) MCP Error Model. Heading attitude angle error of MCP δ
ψ
m can be represented as first-order Markov process with(7)
δ
ψ
˙
m
=
-
1
τ
m
δ
ψ
m
+
w
m
,where τ
m is the time correlation coefficient and w
m 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 by(8)
X
˙
=
F
·
X
+
Γ
·
W
,where matrices F and Γ can be established from the above error model and W is the input (system) noise:(9)
W
=
w
g
x
,
w
g
y
,
w
g
z
,
w
a
x
,
w
a
y
,
w
a
z
,
w
d
x
,
w
d
y
,
w
d
z
,
w
m
,where w
g
x, w
g
y, and w
g
z are the white noise of the gyros and w
a
x, w
a
y, and w
a
z are the white noise of the accelerometers. The state vector X of the integrated navigation system is defined as(10)
X
=
δ
V
E
,
δ
V
N
,
δ
V
U
,
ϕ
E
,
ϕ
N
,
ϕ
U
,
δ
L
,
δ
λ
,
δ
h
,
∇
x
,
∇
y
,
∇
z
,
ε
x
,
ε
y
,
ε
z
,
δ
V
d
x
,
δ
V
d
y
,
δ
V
d
z
,
δ
K
d
x
,
δ
K
d
y
,
δ
K
d
z
,
δ
ψ
m
.


## 2.3. Measurement Space

Supposing that the error of the velocity of DVL in the navigation coordinates frameV
d
n is mainly caused by platform misalignment, the error ofV
~
d
n can be described as (11)
V
~
d
n
=
V
d
n
+
δ
V
d
n
=
C
b
n
C
d
b
V
d
+
δ
C
b
n
C
d
b
V
d
=
V
d
n
+
δ
C
b
n
C
d
b
V
d
,where C
d
b is a constant matrix which translates the velocity of DVL V
d from the instrumental frame to the body coordinates frame. The perturbation of the attitude matrix δ
C
b
n is defined as(12)
δ
C
b
n
=
I
-
C
n
p
C
b
n
,where p is the calculating navigation coordinate frame. Substituting (12) into (11), it yields(13)
V
~
d
n
=
V
d
n
+
I
-
C
n
p
C
b
n
C
d
b
V
d
.After differentiating the velocity of SINS and DVL, the measurement model can be written as(14)
Δ
V
n
=
V
~
n
-
V
~
d
n
+
η
d
=
V
n
+
δ
V
n
-
V
d
n
-
I
-
C
n
p
C
b
n
C
d
b
V
d
+
η
d
=
δ
V
n
-
I
-
C
n
p
C
b
n
C
d
b
V
d
+
η
d
,where the measurement velocity vector Δ
V
n
=
[
Δ
V
E
,
Δ
V
N
,
Δ
V
U
] and δ
V
d
n
=
(
I
-
C
n
p
)
C
b
n
C
d
b
V
d can be calculated from (15)
δ
V
d
E
=
ϕ
U
V
N
-
ϕ
N
V
U
+
C
b
n
1,1
δ
V
d
x
+
C
b
n
1,2
δ
V
d
y
+
C
b
n
1,3
δ
V
d
z
+
C
b
n
1,1
V
E
δ
K
d
x
+
C
b
n
1,2
V
N
δ
K
d
y
+
C
b
n
1,3
V
U
δ
K
d
z
,
δ
V
d
N
=
ϕ
E
V
U
-
ϕ
U
V
E
+
C
b
n
2,1
δ
V
d
x
+
C
b
n
2,2
δ
V
d
y
+
C
b
n
2,3
δ
V
d
z
+
C
b
n
2,1
V
E
δ
K
d
x
+
C
b
n
2,2
V
N
δ
K
d
y
+
C
b
n
2,3
V
U
δ
K
d
z
,
δ
V
d
U
=
ϕ
N
V
E
-
ϕ
E
V
N
+
C
b
n
3,1
δ
V
d
x
+
C
b
n
3,2
δ
V
d
y
+
C
b
n
3,3
δ
V
d
z
+
C
b
n
3,1
V
E
δ
K
d
x
+
C
b
n
3,2
V
N
δ
K
d
y
+
C
b
n
3,3
V
U
δ
K
d
z
.Similarly, after differentiating the heading angle of SINS and MCP, the measurement model is given as(16)
Δ
ψ
=
ψ
~
-
ψ
~
m
+
η
m
=
ψ
+
δ
ψ
-
ψ
m
-
δ
ψ
m
+
η
m
=
δ
ψ
-
δ
ψ
m
+
η
m
,where(17)
δ
ψ
=
-
C
b
n
1,2
·
C
b
n
3,2
C
b
n
1,2
2
+
C
b
n
2,2
2
ϕ
E
-
C
b
n
2,2
·
C
b
n
3,2
C
b
n
1,2
2
+
C
b
n
2,2
2
ϕ
N
+
ϕ
U
.Based on the previous analysis, the measurement model is defined as(18)
Z
k
=
H
k
X
k
+
η
k
,where Z
=
[
Δ
V
E
,
Δ
V
N
,
Δ
V
U
,
Δ
ψ
]
Τ is the observation vector, H
k is the observation matrix at the moment of k which can be established from (15) and (17), and vector η
k represents the Gaussian measurement noise with zero mean value and covariance matrix R.


## 3. AKF Algorithm to Integrated Navigation System

Given the differential equations, we can have a continuous state transition matrixF 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(19)
Φ
=
I
+
F
Δ
T
,where Δ
T is chosen as 0.01 s. Thus, the state equations and observation equation can be written as(20)
X
k
=
Φ
k
,
k
-
1
X
k
-
1
+
Γ
k
-
1
W
k
-
1
,
Z
k
=
H
k
X
k
+
η
k
,where X
k is the 22-dimensional state vector at the moment of k, Z
k is the 4-dimensional observation vector at the moment of k, Φ
k
,
k
-
1 is state-transfer-matrix from sample time k
-
1 to k, Γ
k
-
1 is the system noise matrix at the moment of k
-
1, and W
k
-
1 is system noise. Here, {
W
k
} and {
η
k
} are uncorrelated white noise sequences with zero mean. So we can have E
{
W
k
W
j
}
=
Q
k
δ
k
j and E
{
η
k
η
j
}
=
R
k
δ
k
j. Q
k is the variance matrix of system noise and R
k is the variance matrix of observation noise.The equations of AKF based on the new information sequence are written as(21)
X
^
k
/
k
-
1
=
ϕ
k
,
k
-
1
X
^
k
-
1
,
V
k
=
Z
k
-
H
k
X
^
k
/
k
-
1
,
P
k
/
k
-
1
=
K
k
-
1
V
k
V
k
T
K
k
-
1
T
+
P
k
-
1
,
K
k
=
P
k
/
k
-
1
H
k
T
H
k
P
k
/
k
-
1
H
k
T
+
R
k
-
1
,
X
^
k
=
X
^
k
/
k
-
1
+
K
k
V
k
,
P
k
=
I
-
K
k
H
k
P
k
/
k
-
1
.Compared with traditional KF equations, there are two differences. Firstly,Q
k 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.Figure 1
AKF scheme of SINS/DVL/MCP.


## 4. 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–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.


### 4.1. 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 V
U can be regarded as zero. Hence, there are 5 nodes in input layer while there are 3 in output layer.Figure 2
Artificial neural networks model.Based on the assumption that the number of hidden layer nodes ism, we have x
′
∈
R
m, x
′
=
(
x
0
′
,
x
1
′
,
…
,
x
m
-
1
′
)
T, and the connection weights between input and hidden layers are ϖ
i
j
(
i
=
0,1
,
…
,
4
;
j
=
0,1
,
…
,
m
-
1
), with thresholds to be Θ
j. The connection weights between output and hidden layers are represented as ϖ
j
k
(
k
=
0
,
1
,
2
), with thresholds to be Θ
k. Then the output of each layer can be written as (22)
x
j
′
=
f
1
∑
i
=
0
4
ϖ
i
j
x
i
-
Θ
j
,
y
k
=
f
2
∑
j
=
0
m
-
1
ϖ
j
k
x
j
′
-
Θ
k
,where f
1 and f
2 are transfer functions.In order to study the nonlinear properties of biological neurons, usuallyf
1 is chosen as a sigmoid function f
1
(
x
)
=
1
/
(
1
+
e
-
x
) and f
2 is chosen as a linear function f
2
(
x
)
=
x. During optimizing, the threshold value is treated as import power with value of −1.


### 4.2. 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
=
{
X
1
,
X
2
,
…
,
X
2
N
}, where X
i
(
i
=
1,2
,
…
,
2
N
) is the weight distribution of the network and defined as X
i
=
[
W
1
,
W
2
,
…
,
W
M
] with W
k
(
k
=
1,2
,
…
,
M
) 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(23)
f
=
1
1
+
E
,
E
=
1
2
∑
l
=
1
p
∑
k
=
1
3
d
k
-
o
k
2
,where p is the number of the imported samples and d
k and o
k are the desired and actual outputs of the node k 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 toStep (5); otherwise continue.(4) New Population Generation. Firstly, n individuals in the current generation are picked as part of the next generation, while removing the rest. Secondly, n new individuals are generated from individuals selected by mutation. These 2n 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,(24)
W
k
t
+
1
=
W
k
t
+
Δ
W
k
,
Δ
W
k
=
L
1
F
X
i
N
0,1
,
L
=
C
exp
⁡
n
n
max
⁡
,where W
k
(
t
+
1
) and W
k
(
t
) are the weights of the (
t
+
1
)th generation and (
t
)th generation, respectively, with subscript to be k
=
1,2
,
…
,
M, N
(
0,1
) represents a standard normal distribution, L and C are definition coefficients, n
m
a
x is the maximum training cycles of the network, and n is the number of the individual survival generations. When the new population is generated, jump toStep (2).(5) Termination Cycle. Terminate the evolution procedure and then train the optimal neural networks using BP algorithm until all requirements are met.


## 4.1. 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 V
U can be regarded as zero. Hence, there are 5 nodes in input layer while there are 3 in output layer.Figure 2
Artificial neural networks model.Based on the assumption that the number of hidden layer nodes ism, we have x
′
∈
R
m, x
′
=
(
x
0
′
,
x
1
′
,
…
,
x
m
-
1
′
)
T, and the connection weights between input and hidden layers are ϖ
i
j
(
i
=
0,1
,
…
,
4
;
j
=
0,1
,
…
,
m
-
1
), with thresholds to be Θ
j. The connection weights between output and hidden layers are represented as ϖ
j
k
(
k
=
0
,
1
,
2
), with thresholds to be Θ
k. Then the output of each layer can be written as (22)
x
j
′
=
f
1
∑
i
=
0
4
ϖ
i
j
x
i
-
Θ
j
,
y
k
=
f
2
∑
j
=
0
m
-
1
ϖ
j
k
x
j
′
-
Θ
k
,where f
1 and f
2 are transfer functions.In order to study the nonlinear properties of biological neurons, usuallyf
1 is chosen as a sigmoid function f
1
(
x
)
=
1
/
(
1
+
e
-
x
) and f
2 is chosen as a linear function f
2
(
x
)
=
x. During optimizing, the threshold value is treated as import power with value of −1.


## 4.2. 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
=
{
X
1
,
X
2
,
…
,
X
2
N
}, where X
i
(
i
=
1,2
,
…
,
2
N
) is the weight distribution of the network and defined as X
i
=
[
W
1
,
W
2
,
…
,
W
M
] with W
k
(
k
=
1,2
,
…
,
M
) 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(23)
f
=
1
1
+
E
,
E
=
1
2
∑
l
=
1
p
∑
k
=
1
3
d
k
-
o
k
2
,where p is the number of the imported samples and d
k and o
k are the desired and actual outputs of the node k 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 toStep (5); otherwise continue.(4) New Population Generation. Firstly, n individuals in the current generation are picked as part of the next generation, while removing the rest. Secondly, n new individuals are generated from individuals selected by mutation. These 2n 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,(24)
W
k
t
+
1
=
W
k
t
+
Δ
W
k
,
Δ
W
k
=
L
1
F
X
i
N
0,1
,
L
=
C
exp
⁡
n
n
max
⁡
,where W
k
(
t
+
1
) and W
k
(
t
) are the weights of the (
t
+
1
)th generation and (
t
)th generation, respectively, with subscript to be k
=
1,2
,
…
,
M, N
(
0,1
) represents a standard normal distribution, L and C are definition coefficients, n
m
a
x is the maximum training cycles of the network, and n is the number of the individual survival generations. When the new population is generated, jump toStep (2).(5) Termination Cycle. Terminate the evolution procedure and then train the optimal neural networks using BP algorithm until all requirements are met.


## 5. 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 Figure3.Figure 3
Integrated navigation system blocks diagram based on evolutionary neural networks.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(25)
V
k
~
N
0
,
A
k
.The mean of V
k is 0 and its variance is (26)
A
k
=
H
k
P
k
/
k
-
1
H
k
T
+
R
k
.While the auxiliary navigation equipment fails, the statistical characteristic of measurements noise changes. Then, the residualV
k no longer belongs to the white noise. So the mean of the residual V
k does not equal 0, which can be used to judge whether the observation data is available.Firstly, we define the detection function as(27)
γ
k
=
V
k
T
A
k
-
1
V
k
,where γ
k obeys the χ
2 distribution and its degree of freedom is m; that is, γ
k
~
χ
2
(
m
), and m is the dimension of observations vector Z
k.Then, the detection criteria are as follows:(28)
if
γ
k
≥
T
D
,  observation  data  are  unavailable,
if
γ
k
<
T
D
,  observation  data  are  available,where the threshold T
D 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 γ
k will be bigger than T
D. 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.


## 6. 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.(1) Experiment Equipment. To evaluate the performance of the proposed fault-tolerant filtering algorithm, several tests are conducted on a land vehicle platform equipped with integrated navigation system (tested system) and PHINS III (comparison unit).(i)
Tested System: Integrated Navigation System (Strapdown Inertial Navigation System, Speed Log, and Magnetic Compass). The inertial measurement unit (IMU) parameters of the tested system are shown in Table 1.
(ii)
Comparison Unit. PHINS III is produced by IXSEA cooperation which worked on SINS/GPS mode.
(iii)
Vehicle Test Environment. The vehicle test structure is shown as Figure 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.Table 1
IMU parameters of tested system.
Terms
Parameters
Accelerometer biases (g)
<80 × 10−5
Accelerometer nonlinearity (% FS)
<30 × 10−3
Accelerometer resolution (mg)
0.5
Gyro biases (°/hr)
<0.1
Gyro nonlinearity (% FS)
<20 × 10−2
Random walk (°/hr1/2)
<0.1Table 2
Statistical results of horizontal position errors.
Longitude error (unit: m)
Latitude error (unit: m)
Experiment1
141
798
Experiment2
1.5
21Figure 4
Vehicle test structure.Figure 5
Experimental equipment: tested system and comparison unit.Figure 6
Working scene.(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.
Figures7–9 show the system’s navigation errors of Experiment 1. Figure 10 shows the navigation route of Experiment 1.
According to Figures7 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.Figure 7
Experiment1, attitude errors.Figure 8
Experiment1, velocity errors.Figure 9
Experiment1, position errors.Figure 10
Experimental route map with traditional KF algorithm.(3) Experiment ResultsExperiment 2 (for the FTAKF algorithm based on EANN).
The experiment also consists of three phases like Experiment1. 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 Figures11–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.Figure 11
Experiment2, attitude errors.Figure 12
Experiment2, velocity errors.Figure 13
Experiment2, position errors.Figure 14
Experimental route map with FTAKF algorithm.


## 7. 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.
(3)
The results of experiments show that the FTAKF algorithm based on EANN proposed in this paper is effective and practical for engineering application.
(4)
It should be noted that, because of the utilization of the evolutionary neural network, the prerequisite for this system is that the training time of neural networks must be longer than the period that observation data is unavailable.



---

*Source: 581909-2015-10-07.xml*