Embed Size (px)
Citation preview
A robust data fusion scheme for integrated navigation systems
employing fault detection methodology augmented with fuzzy adaptive
filtering
Muhammad Ushaq*, a
and Fang Jianchengb
a,b School of Instrumentation Science & Optoelectronics Engineering, Beihang University, 37 Xue
Yuan Lu Haidian District Beijing, China
ABSTRACT
Integrated navigation systems for various applications, generally employs the centralized Kalman filter (CKF) wherein
all measured sensor data are communicated to a single central Kalman filter. The advantage of CKF is that there is a
minimal loss of information and high precision under benign conditions. But CKF may suffer computational
overloading, and poor fault tolerance. The alternative is the federated Kalman filter (FKF) wherein the local estimates
can deliver optimal or suboptimal state estimate as per certain information fusion criterion. FKF has enhanced
throughput and multiple level fault detection capability. The Standard CKF or FKF require that the system noise and the
measurement noise are zero-mean and Gaussian. Moreover it is assumed that covariance of system and measurement
noises remain constant. But if the theoretical and actual statistical features employed in Kalman filter are not compatible,
the Kalman filter does not render satisfactory solutions and divergence problems also occur. To resolve such problems,
in this paper, an adaptive Kalman filter scheme strengthened with fuzzy inference system (FIS) is employed to adapt the
statistical features of contributing sensors, online, in the light of real system dynamics and varying measurement noises.
The excessive faults are detected and isolated by employing Chi Square test method. As a case study, the presented
scheme has been implemented on Strapdown Inertial Navigation System (SINS) integrated with the Celestial Navigation
System (CNS), GPS and Doppler radar using FKF. Collectively the overall system can be termed as
SINS/CNS/GPS/Doppler integrated navigation system. The simulation results have validated the effectiveness of the
presented scheme with significantly enhanced precision, reliability and fault tolerance. Effectiveness of the scheme has
been tested against simulated abnormal errors/noises during different time segments of flight. It is believed that the
presented scheme can be applied to the navigation system of aircraft or unmanned aerial vehicle (UAV).
Keywords: Integrated Navigation, Strapdown, GPS, Celestial, Doppler, Fuzzy Adaptive, Kalman, Fault Detection
1. INTRODUCTION
Integrated Navigation System is regarded as the process of blending and combining the navigation solutions from
principal navigations system, customarily Strapdown inertial navigation system (SINS), with the solutions from external
sources like GPS, CNS, and Doppler radars etc., to accomplish enhanced performance1–4
. This technique is believed to
achieve more explicit and precise solutions than those could be achieved by the use of any stand-alone contributing
system5. Fusion of data from various sensors renders substantial advantages over single source data. The Kalman
filtering theory for the synthesis of multi-sensor data has been widely applied in integrated navigation systems in various
applications like aircraft, UAVs, land vehicle, satellites and missile system etc. When multiple sensors measure the state
for the same stochastic system, generally two different types of Kalman filtering algorithms are used to process and
combine the measurements. The first method is the centralized Kalman filter (CKF) where all measured sensor data are
passed along to the central processing filter. The advantage of CKF is that there is a minimal loss of information6.
However it can result in excessive computational overloading. Therefore, generally CKF is considered as unreliable as it
suffers from poor accuracy and stability especially on occurrence of fault in system or measurements. The alternative
method is the distributed or federated Kalman filter (FKF) wherein the local estimates from all local sensors can deliver
optimal or suboptimal state estimate as per certain information fusion criterion. The advantages of FKF are flexible
implementation to support a variety of system requirements and facilitate performance trades. It provides globally
optimal estimation accuracy in single-step mode with fusion resets to local filters and subset-optimal estimation accuracy
in multi-step mode with periodic fusion resets to the local filters7,8
. FKF has enhanced throughput thanks to the parallel
and local processing, multiple level fault detection, isolation and recovery capability.
*[email protected] (Phone: 0086-15011530142)
Fault tolerance of FKF can be further enhanced employing no reset mode, on expense of precision to some extent9. The
Standard Kalman filter, both CKF and FKF, require that all the system dynamics and noise statistics are known10
. But if
the theoretical and actual statistical features employed in Kalman filter are not compatible, the Kalman filter render
unsatisfactory solutions and divergence problems also occur. In order to resolve the divergence due to modelling errors,
un-modelled states must be estimated. However, it adds to the complexity of the filter and one can never be sure that all
the likely unstable states are indeed model states. Moreover standard KF requires that the measurement noise covariance
be known and remain constant throughout the mission time. But in real systems this demand is a farfetched assumption
indeed. To resolve such problems, in this paper, an adaptive Kalman filter scheme strengthened with fuzzy inference
system (FIS) is employed to regulate and adapt the system noise covariance and measurement noise covariance online in
accordance with real system dynamics and varying measurement errors from aiding source. Chi square test method has
also been used for detection of excessive random or step errors in measurement noise. The proposed scheme has been
implemented on SINS integrated with CNS, GPS and Doppler radar. The SINS provides solutions for attitude, position
and velocity. GPS provides the solution for velocity and position. CNS-1 provides solution for attitude, CNS-2 provide
solution for latitude and longitude and Doppler radar render the solution for velocity. The simulation results have
validated the effectiveness of the presented scheme with significantly enhanced precision as compared to standard CKF
or standard FKF. Effectiveness and fault tolerance of the scheme has been tested by introducing deliberate measurement
noise of different nature at different time segments of flight.
2. STANDARD KALMAN FILTERING ALGORITHM
The standard linear Kalman filter algorithm in discrete form is elucidated as follows11,12
:
Time update equations:
1
ˆ ˆk k kx x
(1)
1 1
T T
k k k k kP P Q
(2)
1( )T T
k k k kK P H HP H R (3)
Measurement equation:
k k k kz H x v (4)
Measurement updates equations:
ˆ ˆ ˆ( )k k k k k kx x K z H x (5)
( ) ( )T T
k k k k k k k k kP I K H P I K H K R K (6)
Whereas x is the error state vector. k is the state transition matrix.
( )T
kP E xx is estimation state error covariance matrix, [ ]T
kQ E ww is the system noise covariance matrix,
[ ]T
kR vv is the measurement noise covariance matrix. It is worth mentioning that the Kalman filter is based on the
assumption that the error state vector x̂ , estimation error vector ˆx x x , system noise vector w and measurement
errors vector v (to be defined in following) are all white in nature and uncorrelated with each other and can be expressed
by following statistical expressions: T
k j kE x x P for k j , 0 for k j
0T
k jE w v for all values of k and j
T
k j kE w w Q for k j , 0 for k j
T
k j kE v v R for k j , 0 for k j
p
kQ R and m
kR R are positive definite matrices. p and m are the sizes of the vectors w and v respectively.
3. FEDERATED KALMAN FILTER (FKF)
The FKF is a decentralized estimation algorithm that employs a two-stage information processing methodology wherein
the outputs of sensor related local filters (LF) are fused by a large master filter (MF). The scheme implemented in this
research work is depicted in Figure 1. Each LF is dedicated to a distinct sensor subsystem, and also gets data from the
main reference SINS. The SINS is the kernel sensor, and its data is the measurement input for the MF. The data from the
GPS, CNS-1, CNS-2 and Doppler radar are dedicated to the corresponding LFs. Each LF after implementation; provides
its solutions to the MF for the master update.
Time update equations for the LFs and MF are13–15
:
, | 1 , , 1
ˆ ˆ , 1,2, , ,i k k i k i kx x i l m (7)
1 1
, | 1 , 1 ,[ ] , 1,2, , ,T
i k k k i k k i kP P Q i l m
(8)
Measurement update equations for the LFs are:
1 1 1
, , | 1 , , 1,2, ,T
i k i k k i i k iP P H R H i l
(9)
1 1
, , , | 1 , | 1 , ,ˆ ˆ T
i k i k i k k i k k i i k i kx P P x H R z
(10)
Whereas: , | 1ˆ and i in n
i k k kx x are the priori and posteriori estimates of kx respectively. i in n
iQ
is the
covariance matrix of system noise. , | 1i in n
i k kP
is the priori covariance matrix of estimation errors, 1
,i in n
i kP is the
inverse posteriori covariance of estimation error also known as information matrix. In the presented integrated
navigation system, 1,2,3,4,i m , where m stands for the MF. l is the number of local filters.
Where , , | 1
ˆ ˆm k m k kx x and
, | 1 , | 1ˆ ˆ
m k k f k kx x ,1
,
f fn n
f kP is the inverse of the fused covariance
fP , ,ˆ fn
f kx is the
fused state estimate. In fact, the estimates of the LFs are correlated because these are based on a common system
dynamics. In order to circumvent the problem related with this correlation, the process noise and the state vector
covariance are set to their upper bounds as follows16
:
1
, , , 1,2, , ,i k i kQ Q i l m (11)
1
, , , , 1,2, , ,i k i k f kP P i l m (12)
Where ( 0)i are information sharing coefficients satisfying the following condition:
1 2 1l m (13)
Information from all local filters and main filter are fused as follow:
1 1 1
, , ,
1
l
f k m k j k
j
P P P
(14)
1 1
, , , , , ,
1
ˆ ˆ ˆl
f k f k m k m k j k j k
j
x P P x P x
(15)
Whereas:1 1
, , 1m k m k kP P
;
1 1
,, 1 , 1m km k k f k kP P
; , , 1
ˆ ˆm k m k k
x x
;, 1 , 1
ˆ ˆm k k f k k
x x ; and ,
f fn n
f kP
is the fused
covariance of error state vector; ,ˆ fn
f kx is the fused state estimate; m in the subscript stands for master filter; and f in
the subscript represents federated solution.
Having obtained global solution, it can be (optionally) fed-back to the local KFs, known as the reset operation, by the
following algorithm17
.
1
, ,
, ,
,, 1, 2... ,
ˆ ˆ
j k j f k
j k f k
P Pj l m
x x
(16)
Furthermore, the total process information, represented by the matrix 1Q shared among the LFs, must add-up to the true
net process information as 1 1
, ,i k i kQ Q or 1
, ,i k i kQ Q .
Master
FilterSINS
GPS
CNS-1
Fault Detection&
Adaption
KF-1For
SINS/GPS
CNS-2 +
Altimeter
KF-2For
SINS/CNS-1
KF-3For
SINS/CNS-2+Alt
Doppler
Radar
KF-4For
SINS/Doppler
Observation For KF-4
Time
Update
Measurement
Update
1
1ˆ , f fx P
1
2ˆ , f fx P
1
3ˆ , f fx P
1
4ˆ , f fx P
1 1ˆ , x P
2 2ˆ , x P
3 3ˆ , x P
4 4ˆ , x P
ˆ , f fx P
Co
rrection
to S
INS
Corrected Solution for Position, Velocity and Attitude
+
-
+
-
+
-
+
-
Fault Detection&
Adaption
Fault Detection&
Adaption
Fault Detection&
Adaption
Observation For KF-3
Observation For KF-2
Observation For KF-1
Figure 1 SINS/GPS/CNS/Doppler Integrated Navigation
4. THE SYSTEM MODEL FOR SINS/GPS/CNS/DOPPLER INTEGRATION
The SINS provides position, velocity and attitude solutions, the CNS-1 provides attitude information, CNS-2 + Baro-
Altimeter provides the position (Latitude, longitude & altitude), the GPS renders position and velocity solutions and
Doppler radar give the velocity information. The aim of integrated navigation system it to blend & fuse the navigation
information provided by different sensors in an optimal mode as to get more precise navigation solution with high
reliability and fault tolerance at minimum computational cost.
4.1 Error State Vector
Following is the error state vector:
T
g g g
x y z x y z bx by bz bx by bzx V V V h (17)
x , y , z :attitude errors g
xV , g
yV , g
zV : Velocity Errors.
, , h : Latitude, Longitude and Altitude Errors bx , by , bz : Fixed drifts in gyros.
bx , by , bz : Fixed biases in accelerometers.
4.2 Process Noise Vector and Process Noise Covariance Matrix
Process noise vector consisting of gyro’s and accelerometers random errors is given as follows
T
rx ry rz rx ry rzW (18)
The corresponding process noise covariance matrix composed of the covariance of gyros and accelerometers is given as:
2 2 2 2 2 2
gx gy gz ax ay azQ diag (19)
The system dynamic equation is given as follows:
( ) ( ) ( ) ( ) ( )x t F t x t G t w t (20)
The dynamic equations for the elements of error state vector are expressed in detail in references17,18
: The system dynamic matrix is given as follows:
6 9 6 6 15 15
0 0
N S
X
F FF
(21)
FN is the 9 x 9 matrix obtained from the coefficient of system dynamic equations. The matrices SF and G (the system
noise transformation matrix) are given as follows:
3 3 3 3
3 3 3 3
3 3 3 3 9 3 9 3
0 0
0 , 0
0 0 0 0
n n
b b
n n
S b b
C C
F C G C
(22)
Matrices F and G should be discretized to be used in digital computation, as follows:
2 2 3 3 1 1
.....2! 3! 1!
n n
k
F F FI F
n
(23)
2 2 3 3 1 1
.....2! 3! 4! !
n n
k
F F F FI G
n
(24)
State error covariance matrix is given as:
2 2 2 2 2 2 2 2 2 2 2 2 2 2 2
E N U E N U bx by bz bx by bzV V V hP diag (25)
5. MEASUREMENT MODEL FOR THE LOCAL FILTERS
The measurement models for each local filter are elaborated as follows:
5.1 SINS/GPS Integration (KF-1)
Measurement equation for SINS/GPS Integration is the difference between the navigation information (latitude,
longitude, altitude and velocity) provided by SINS and that provided by GPS given as follows:-
1ˆ ˆˆ ˆ ˆ ˆˆ ˆ ˆ ˆ ˆ ˆ( ) ( )
T Tg g g g g g
i i i i ix iy iz G G G G Gx Gy Gzz Rm h R h Cos h V V V Rm h R h Cos h V V V
(26)
1 1 1ˆ
kz H x v (27)
Whereas: 1
T
Vx Vy Vz hv v v v v v v is the random noise vector in velocity and position measurement of GPS
The corresponding measurement matrix 1H (which maps the measurement vector into the state vector) is given as
follow:
1
0 0 0 1 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 1 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 Rm h 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 Rn h cos lat 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0
H
(28)
The measurement noise covariance matrix 1R is given as follows:
2 2 2 2 2 2
1 Gx Gy Gz G G GV V V hR diag
(29)
Random errors in velocity measurement are 0.1m/S and in position measurement 15m.
5.2 SINS/CNS-1 Integration (KF-2)
Measurement equation for SINS/CNS-1 Integration is the difference between the attitude (pitch, roll and yaw) provided
by SINS and that provided by CNS-1 transformed into navigation frame: as follows:
Attitude error vector in body frame is represented as
ˆ ˆ ˆ ˆ ˆ ˆTT
I C I C I C
(30)
This attitude error when transformed into the navigation frame is the misalignment angles i.e. E ,.
N and U . The
attitude error vector (for small angle errors) in navigation frame (ENU) is defined as follows:
T
E N U (31)
The corresponding transformation matrix from navigation frame (n) to erroneous navigation frame(c) is given as follow:
1
1
1
U N
c
n U E
N E
C
(32)
While the transformation matrices from body frame to true navigation frame and estimated navigation frame are:
n
b
Cos Cos Sin Sin Sin Cos Sin Cos Cos Cos Sin Sin
C Cos Sin Sin Sin Cos Cos Cos Sin Sin Cos Sin Cos
Sin Cos Sin Cos Cos
(33)
ˆ ˆ ˆˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ
ˆ ˆ ˆ ˆˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ
ˆ ˆ ˆˆ ˆ
n
b
Cos Cos Sin Sin Sin Cos Sin Cos Cos Cos Sin Sin
C Cos Sin Sin Sin Cos Cos Cos Sin Sin Cos Sin Cos
Sin Cos Sin Cos Cos
(34)
ˆ ˆ ˆT T
(35)
ˆ n c c n
b b n bC C C C (36)
Using equations (32), (34), (35) and (36) and considering platform misalignment errors (in ENU frame) and attitude
errors (in body frame) as very small angles, we can arrive at following relation:
0
0
0 1
E
N
U
Cos Cos Sin
Sin Cos Cos
Sin
(37)
Therefore considering Eq (17), (30) and (37) the measurement matrix in body frame for KF-2 will be as follows:
2 3 3 3 120X XH I (38)
While considering the measurement vector as:
2 2 2ˆ
kz H x v (39)
2v is random noise vector in attitude measurement of CNS-1 in navigation frame.
2
ˆ ˆ0
ˆ ˆ0
ˆ ˆ0 1
I C
I C
I C
Cos Cos Sin
z Sin Cos Cos
Sin
(40)
The attitude information provided by SINS and CNS, both are in body frame. Therefore the measurement matrix in
navigation frame (ENU) will be as follows:
/
2 3 12
3 15
0
0 0
0 1
X
X
Cos Cos Sin
H Sin Cos Cos
Sin
(41)
The measurement noise covariance matrix 2R is given as follows:
2 2 2
2 E N UR (42)
Random errors in attitude measurement are taken as 10 arc second
5.3 SINS/ (CNS-2+Altimeter) Integration (KF-3)
Measurement equation for SINS/(CNS-2+Altimeter) integration is the difference between latitude, longitude, altitude
provided by SINS and that provided by (CNS-2+Altimeter) given as follows:-
3ˆ ˆˆ ˆ ˆ ˆ( ) ( )i i i i C C C Bz Rm h R h Cos h Rm h R h Cos h
(43)
3 3 3ˆ
kz H x v (44)
Whereas: 3
T
c c bhv v v v is random noise vector in position measurement from (CNS-2+Altimeter)
The corresponding 3H matrix is given as follow:
3
0 0 0 0 0 0 Rm h 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 Rn h cos lat 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0
H
(45)
The measurement noise covariance matrix 3R is given as follows:
2 2 2
3 c c chR (46)
The random measurement noise for this local filter is 5m.
5.4 SINS/ Doppler radar Integration (KF-4)
The Doppler radar provides velocity information in the body frame19
. For fusion with the velocity provided by SINS,
Doppler velocity needs to be transformed into navigation frame (ENU) using n
bC . SINS provide the estimate of
Strapdown ˆ n
bC matrix which is in fact c
bC due to inherent errors in the system. The ideal frame transformation matrix
from body to navigation frame is given as follows
n n c
b c bC C C (47)
n
cC is the transformation matrix from erroneous computation frame to ideal navigation frame representing
misalignments. Doppler radar provides velocity in body frame as follows:
ˆ b b
D DV V (48)
D is the measurement noise error in Doppler velocity, which is a random noise in nature. For comparison with SINS
velocity, Doppler velocity is required to be transformed into navigation frame.
ˆˆ ˆn n b
D b DV C V (49)
ˆˆ ( )n n b
D b DV C V (50)
ˆˆ n n b n
D b DV C V (51)
As: n n c
b c bC C C ˆ n c c n
b b n bC C C C n
cC is defined as ( )c
nC I
is the skew symmetric matrix related with attitude errors mentioned in state vector
0
0
0
z y
z x
y x
(52)
1
1
1
z y
c
n z x
y x
C I
(53)
Therefore
ˆ ( )n n
b bC I C (54)
By putting this value of ˆ n
bC and rearranging we have following
ˆ n n n n
D DV V V (55)
Now let us define the velocity provided by SINS as follows:
ˆ n n n
I IV V V (56)
For observation/measurement update we take the difference between the velocity provided by SINS and that provided by
Doppler radar as follows
4
ˆ ˆn n
I Dz V V (57)
4
n n n n n
I Dz V V V V (58)
4 ( )*n n n
I Dz V skewsym V (59)
By rearranging and manipulation we have
4 3 3 3 9ˆ( ) I 0n n
Dz SkewSym V x (60)
Therefore H4 matrix is given as follows:
4
0 1 0 0 0 0 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0 0 0 0
n n
z y
n n
z x
n n
y x
V V
H V V
V V
(61)
Measurement noise covariance matrix 4R is given as follows:
4
2
2
2
ˆ ˆ
0 0
0 0
0 0
D
D
D
n b
b n
bx
by
bz
C CR
(62)
6. ADAPTIVE FILTERING AND FAULT DETECTION IN LOCAL KF
The real measurement characteristics of the inertial sensors as well as other non-inertial navigation aids are highly
dependent on the vehicle dynamics and variant environmental effects. Therefore the statistical features of the
measurement and system noises cannot be predefined as required by the standard CKF or standard FKF. The navigation
sensors are prone to faults and environmental effects in real applications20–22
. Incorrect a priori information of the sensor
statistics can seriously worsen the standard filter performance. Hence, for dependable navigation of the vehicles, it is
required to design the adaptive and fault-tolerant navigation filtering algorithm that can tune itself to the varying
statistics hence can deal with sensor failures22,23
. Here, we present a fault-tolerant navigation algorithm, which has the
capability of system level fault detection, isolation and accommodation. The presented algorithm is mainly composed of
two parts; firstly the detection of excessive faults in LF based on a relative degree of mismatch between the actual
innovation covariance and the theoretical innovation covariance and secondly adaption of measurement noise covariance
matrix based on FIS system or maximum likelihood (ML) based adaption24
or the adaption based on innovation/residual
information.
6.1 Innovation/Residual Based Adaptive Filtering
As mentioned earlier innovation vector is the difference between actual measurement kz and the estimated measurement
ˆk k kE z H x given as follows
25
ˆk k k ke z H x (63)
In case there is no fault in the measurement other than bounded random noise, innovation ke and the measurement
vector kz are orthogonal to each other
26. The nominal covariance matrix of innovation sequence is given as
24:
T
en k kC HP H R (64)
Computation of adaptive R and Q is based on ML estimation criterion25,27
.
For taking innovation sequence inside an estimation window of size M the expression for the estimated covariance
matrix of the innovation sequence is:
1
1ˆ ˆ
kT
ek i i i i i i
i k M
C z H x z H xM
(65)
Expression for adaptive measurement noise covariance matrix is given as follows:
ˆˆ T
k ek kR C HP H (66)
Equation(66) is used to adapt the measurement noise covariance online in the light of varying measurement noises.
Using residual sequence ( ˆk k k kr z H x ) instead of innovation sequence ( ˆ
k k k ke z H x ) and noting that 1
k k ek kr R C e
we arrive at a more stable expression for the adaptive R as follows:
ˆˆ T
k rk kR C HP H (67)
Following the similar reasoning we have following expression for adaptive system noise covariance matrix 9.
ˆ ˆ T
k k ek kQ K C K (68)
6.2 Fuzzy Adaptive Filtering
As the innovation sequence is regarded as the most relevant source of information to adjust the measurement covariance
matrix online. That is, to ensure filter estimation performance, it is required to tune the covariance of the innovation to
make it consistent with its theoretical value. If a discrepancy between the actual ekC covariance of and its theoretical
value enC is found, proper measures are taken to adjust the diagonal elements of ˆkR . To ascertain the discrepancy
between theoretical and actual covariance, which may be termed as extent of mismatch (EOM) is computed as:
( , ) ( , )en ekEOM C j j C j j (69)
Or taking relative extent of mismatch REOM for each diagonal element we have:
( , )
( )( , )
ek
en
C j jREOM j
C j j (70)
To tune or adjust the each diagonal element of ˆkR a fuzzy inference systems (FIS) is designed
28,29. Each diagonal
element of EOM or that of REOM is a scalar quantity, which has specific significance and one-to-one correspondence
with the diagonal element of ˆkR .Each element of REOM is a relative measure that represent the extent of deviation or
mismatch of the theoretical covariance of innovation from its actual value. The adjustment of the each element of ˆkR is
made in accordance with the corresponding element of REOM by employing fuzzy logic system using FIS. When
( )REOM j is selected as the input to fuzzy inference system, only a FIS with a single-input ( )REOM j and a single
output (scaling factor) is required for each element. The FIS can be employed to iteratively generate the correction
scaling factors for each diagonal elements of ˆkR as follows:
1
ˆ ˆk kR R (71)
The FIS is implemented by adopting three input membership functions for the ( )REOM j i.e. Low (L), Normal (N) and
High (H), and three output membership functions for namely Decrease (D), Maintain (M), and Increase (I). The
fuzzy reasoning rules are expressed according to the following deliberation: To realize covariance matching, if
( )REOM j is less than 1 (i.e., the actual estimated covariance is less than the theoretical value), the value the
corresponding element of ˆkR should be decreased, and vice versa.
6.3 Detection of excessive fault in local filters
If the measurement sensor in the ith
LF is functioning normally, the mean value of its innovation or residual is zero, and
its covariance is given by(64). The residual-based test for the failure detection can be devised by comparing the test
statistic, which is the theoretical normalized innovation covariance. It is a scalar value
1
ˆ ˆ( )T
T
i k k k k k k k kk z H x HP H R z H x
(72)
The value given in Equation (72) has 2 distribution with m degree of freedom, whereas m is the number of
elements in measurement vector. The fault detection criteria for the ith local filter can be versed as:
: Faulty Observation , : Normal Observationi d i dT T (73)
Value of dT can be selected from 2 table for the desired confidence level and length of measurement vector.
The value can also be chosen by Matlab function. For example for a confidence level of 95 % and
measurement vector with 6 elements dT is given as: chi2inv(0.95 , 6)=12.5916dT
On detection of fault the measurement update of that particular local filter for that particular epoch is skipped.
7. SIMULATION UNDER VARIOUS CONDITIONS
To evaluate the performance of the adaptive and fault-tolerant integrated navigation scheme presented in this paper, a
series of representative simulations have been carried out. SINS/GPS/CNS/Doppler integrated navigation has been
evaluated using CKF, FKF without adaption, with adaption based on simple innovation or residual method and with
adaption based on FIS method. The fault tolerance and adaption strength has been tested by introducing deliberate errors
of different types (excessive random errors, fixed step errors, slowly growing errors) in different measurement sensors at
different time segment of trajectory. The algorithm has been tested on a simulated trajectory of vehicle having different
maneuvering like taxying, climbing, turning, straight line motion with acceleration etc. as shown in Figure 2. It may be
noted that in following simulation results navigation errors of attitude (pitch errors, roll errors, and yaw errors), velocity
errors (east velocity errors, north velocity errors, vertical velocity errors) and position errors (latitude errors, longitude
errors and altitude errors) are depicted. In following figures simulation results of estimation errors for following three
schemes are depicted simultaneously and compared with each other.
a) Standard Federated Kalman Filter without any excessive errors (green line).
b) Standard Federated Kalman Filter with excessive errors (blue line).
c) Fault Tolerant Federated Kalman Filter with excessive errors (red line).
From simulation results it can be seen that the estimation errors of fault tolerant scheme (red line), even on occurrence of
abnormal noise in the GPS measurements are very close to the normal situation (green line). Hence the presented scheme
has effectively contained/controlled the excessive estimation errors provided by standard FKF as depicted by blue lines
in the figures.
Figure 3 to Figure 5 depict the errors in navigation solution for a situation when excessive random errors are introduced in
GPS position and velocity measurement equal to 15 times the standard deviation in position and velocity measurement
during time segment of 2200 to 2300 Sec.
39
39.2
39.4
39.6
39.8
40
40.2
40.4
115.7
115.8
115.9
116
116.1
116.2
116.3
116.4
116.5
116.6
116.7
0
2
4
6
8
10
12
14
x 104
Latitude (deg)
Longitude (deg)
Alt
itu
de (
m)
Actual Trajectory
Estimated Trajectory
Figure 2 Trajectory of Vehicle for 3600 sec
0 500 1000 1500 2000 2500 3000 3500 4000-30
-20
-10
0
10
20
30
Attitude Error (Arc Sec)
z [
]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
0 500 1000 1500 2000 2500 3000 3500 4000-60
-40
-20
0
20
40
x [
]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
0 500 1000 1500 2000 2500 3000 3500 4000-100
-50
0
50
y [
]
Time [sec]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
Figure 3 Attitude Errors
0 500 1000 1500 2000 2500 3000 3500 4000-0.04
-0.02
0
0.02
0.04
0.06
Velocity Error (m/s)
V
E [
m/s
]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
0 500 1000 1500 2000 2500 3000 3500 4000-0.04
-0.02
0
0.02
0.04
0.06
V
N [
m/s
]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
0 500 1000 1500 2000 2500 3000 3500 4000-0.1
-0.05
0
0.05
0.1
V
U [
m/s
]
Time [sec]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
Figure 4 Velocity Errors
0 500 1000 1500 2000 2500 3000 3500 4000-200
-100
0
100
200
Position Error (m)
(L
at)
[m
]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
0 500 1000 1500 2000 2500 3000 3500 4000-150
-100
-50
0
50
(L
on
g)
[m]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
0 500 1000 1500 2000 2500 3000 3500 4000-4
-3
-2
-1
0
1
2
(H
) [m
]
Time [sec]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
Figure 5 Position Errors
Figure 6 to Figure 8 depicts the errors in navigation solutions provided by the proposed scheme under a situation when
linearly growing errors in GPS Position and velocity as expressed by equations 10 t , 10 t ,
10 hh t and 10 VV t are introduced, during time segment 2500 to 2650 sec.
0 500 1000 1500 2000 2500 3000 3500 4000-30
-20
-10
0
10
20
30
Attitude Error (Arc Sec)
z [
]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
0 500 1000 1500 2000 2500 3000 3500 4000-50
0
50
x [
]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
0 500 1000 1500 2000 2500 3000 3500 4000-60
-40
-20
0
20
40
y [
]
Time [sec]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
Figure 6 Attitude Errors
0 500 1000 1500 2000 2500 3000 3500 4000-100
0
100
200
300
400
Position Error (m)
(L
at)
[m]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
0 500 1000 1500 2000 2500 3000 3500 4000-100
0
100
200
300
400
500
(L
on
g)
[m]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
0 500 1000 1500 2000 2500 3000 3500 4000-2
0
2
4
6
8
(H
) [m
]
Time [sec]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
Figure 7 Position Errors
0 500 1000 1500 2000 2500 3000 3500 4000-0.02
0
0.02
0.04
0.06
Velocity Error (m/s)
V
E [
m/s
]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
0 500 1000 1500 2000 2500 3000 3500 4000-0.02
0
0.02
0.04
0.06
V
N [
m/s
]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
0 500 1000 1500 2000 2500 3000 3500 4000-0.02
0
0.02
0.04
0.06
V
U [
m/s
]
Time [sec]
Standard FKF
Adaptive FKF
FKF Without Excessive Errors
Figure 8 Velocity Errors
8. CONCLUSION
A fault tolerant integrated navigation methodology is presented in this paper. The aim was to develop a highly reliable
navigation system that renders the safe functioning of the vehicle in complex situations. The presented scheme employs
a hybrid federated filtering architecture and combines multiple redundant sensors, including SINS, GPS, CNS and
Doppler radar. The integrated navigation model is developed, which renders the fault-tolerant potential to the navigation
system. To tune the measurement noise covariance of various contributing sensors, two methods have been used; first
the ML based adaptive Kalman filter and other one is the adaptive Kalman filter based on fuzzy adaption of covariance.
The excessive random or step faults are detected at sensor level using 2 detection method. A variety of simulations
have been carried out to evaluate the performance of the navigation. The presented methodology is substantiated to
efficiently detect, isolate, and accommodate a diverse type of sensor faults, which can satisfy highly reliable navigation
requirements for automated vehicles in non-benign environments. Furthermore, the presented scheme can also withstand
the simultaneous failures in various sensors. The fault-tolerant navigation scheme presented in this paper can be applied
to aircrafts and/or unmanned aerial vehicle (UAV) efficiently and effectively.
REFERENCES
1. Farrell, J. & Barth, M. The global positioning system and inertial navigation [M]. 61, (McGraw-Hill New York,
NY, USA:: 1999).
2. Groves, P. D. Principles of GNSS, inertial, and multisensor integrated navigation systems [M]. (Artech House:
2008).
3. Siouris, G. M. Aerospace avionics systems: A Modern Synthesis [M]. 466, (Academic press Inc: London, UK,
1993).
4. Grewal, M. S., Weill, L. R. & Andrews, A. P. Global positioning systems, inertial navigation, and integration
[M]. (Wiley-Interscience: 2007).
5. Brown, R. G. & HWANG, P. Introduction to random signals and applied Kalman filtering [M]. (1992).
6. Zarchan, P. & Musoff, H. Fundamentals of Kalman filtering: a practical approach [M]. 208, (AIAA: 2005).
7. Carlson, N. A. Federated filter for fault-tolerant integrated navigation systems [C]. Position Location and
Navigation Symposium, Navigation into the 21st Century 110–119 (1988).
8. Carlson, N. A. Federated filter for distributed navigation and tracking applications [C]. Proceedings of the 58th
Annual Meeting of The Institute of Navigation 340–353 (2001).
9. Bar-Shalom, Y., Li, X. R. & Kirubarajan, T. Estimation with applications to tracking and navigation: theory
algorithms and software [M]. (Wiley-Interscience: 2001).
10. Li, X. & Zhang, W. An Adaptive Fault-Tolerant Multisensor Navigation Strategy for Automated Vehicles. 59,
2815–2829 (2010).
11. MOHINDER S. GREWAL, A. P. A. Kalman filtering: theory and practice using MATLAB [M]. (John Wiley &
Sons Inc: 2008).
12. Simon, D. Optimal State Estimation Kalman, H-infinity and Nonlinear Approaches [M]. (JOHN WILEY &
SONS, INC., PUBLICATION: 2006).
13. Study on Federated Architecture for GPS INS TRN Integrated Navigation System.pdf.
14. Mu, R., Rong, S. & Cui, N. Federated filter with strengthened FDIR capability for multiple sensor navigation
system [C]. 1st International Symposium on Systems and Control in Aerospace and Astronautics ISSCAA 4–pp
(2006).
15. Cong, L., Qin, H. & Tan, Z. Intelligent fault-tolerant algorithm with two-stage and feedback for integrated
navigation federated filtering [J]. Journal of Systems Engineering and Electronics 22, 274–282 (2011).
16. Carlson, N. A. Federated square root filter for decentralized parallel processors [J]. Aerospace and Electronic
Systems 26, 517–525 (1990).
17. Quan, W. & Liu, B. INS/CNS/GNSS Integrated Navigation Technology (Chinese Book) [M]. (2011).
18. Ushaq, M. Research on SINS Based Navigation Techniques [D]. (2003).
19. Ushaq, M. & Fang, J. An Improved and Efficient Algorithm for SINS/GPS/Doppler Integrated Navigation
Systems [J]. Applied Mechanics and Materials 245, 323–329 (2013).
20. Shi, J., Miao, L., Ni, M. & Shen, J. Optimal robust fault-detection filter for micro-electro-mechanical system-
based inertial navigation system/global positioning system [J]. Control Theory & Applications, IET 6, 254–260
(2012).
21. Lingxia, Z., Ming, C. & Hua, H. A Method Research on Robust Fault Diagnosis of Integrated Navigation
Systems 2 Relative Measurement Error Models of Navigation Systems 3 Robust Fault Diagnosis Method (
signed by R-FD ). 326–330 (2007).
22. Yun-feng, M. A. Application of Fault Tolerant and Federated Strong Tracking Kalman Algorithm in Integrated
Navigation System. 3946–3949 (2011).
23. Xin, Z., ShiCheng, W., Yang, G., JinSheng, Z. & ZhiGuo, L. SINS/SAR/GPS navigation system intelligent fault
diagnosis method [C]. 2nd International Conference on Intelligent Control and Information Processing (ICICIP)
2, 754–759 (2011).
24. Mohamed, A. H. Optimizing the Estimation Procedure in INS/GPS Integration for Kinematic Applications [D].
(1999).
25. Mehra, R. Approaches to adaptive filtering [J]. IEEE Transactions on Automatic Control 17, 693–698 (1972).
26. Cui, Y. Y. X. Adaptive Integrated Navigation for Multisensor Adjustment Outputs [J]. THE JOURNAL OF
NAVIGATION 57, 287, C295 (2004).
27. Mohamed, A. H. & Schwarz, K. P. Adaptive Kalman filtering for INS/GPS [J]. Journal of Geodesy 73, 193–203
(1999).
28. Sasiadek, J. Z., Wang, Q. & Zeremba, M. B. Fuzzy adaptive Kalman filtering for INS/GPS data fusion [C].
International Symposium on Intelligent Control 181–186 (2000).
29. Castillo, I., Edgar, T. F. & Dunia, R. Nonlinear model-based fault detection with fuzzy set fault isolation [C].
36th Industrial Electronics Conference, IECON 2010 174–179 (2010).
