1-A Fault Tolerant Integrated Navigation Scheme Realized through Online Tuning of Weighting Factors for Federated Kalman Filter

A Fault Tolerant Integrated Navigation Scheme Realized through Online
Tuning of Weighting Factors for Federated Kalman Filter
Muhammad Ushaq1,a*
and Fang Jian Cheng2,b
a
PhD Candidate, b
Professor
1,2
School of Instrumentation Science & Optoelectronics Engg, Beihang University, 37 Xue
Yuan Lu Haidian District Beijing, People Republic of China
*
ushaq71@yahoo.com (Phone: 0086-15011530142)
Keywords: Adaptive Kalman Filter, Integrated Navigation, Data Fusion, Federated Filter, Fault
Detection and Isolation
Abstract: Strapdown Inertial navigation (SINS) is a highly reliable navigation system for short
term applications. SINS functions continuously, less hardware failures, renders high speed
navigation solutions ranging from 50 Hz to 1000 Hz and exhibits low short-term errors. It provides
efficient attitude, angular rate, acceleration, velocity and position solutions. But, the accuracy of
SINS solution vitiates with time as the sensor (gyros & accelerometers) errors are integrated
through the navigation equations. Average navigation grade SINS are capable of providing effective
stand-alone navigation for shorter duration (few minutes) applications Stand-alone SINS capable of
providing solutions for applications exceeding 10 minutes duration, are generally highly expensive
($0.1M to $2.0M). To cope with this limitation, a cost effective solution is the integrated navigation
system wherein the unboundedly growing errors of SINS are contained with the help of external
non-inertial navigation aids like GPS, Celestial Navigation System (CNS), Odometer, Doppler
radars etc. The efficient methodology for integrated or multi-sensory navigation is the Federated
Kalman Filter (FKF) scheme. In FKF architecture, a reference SINS solution is integrated
independently with each of the aiding navigation systems in a bank of local Kalman filters. There
are a number of different ways in which the local filter outputs may be combined to produce an
integrated navigation solution. The no-reset, fusion-reset, zero-reset, and cascaded versions of
federated integration have been used by different researcher and navigators over the years. All
different schemes of FKF have certain pros and cons. Fusion-reset method although nearly optimal
is less fault tolerant while no-resent scheme renders highly fault tolerant solutions but with sub-
optimal solutions and compromised precision. To enhance the fault tolerance ability of fusion-reset
scheme of FKF, additional parameters called weighting factors are introduced to tune the
contribution of each local filter in the final data fusion. The presented scheme has been found nearly
optimal and expressively fault tolerant.
1. Introduction
Navigation system purely based on SINS renders erroneous solution if employed for longer
duration missions, as inertial navigation system errors are accumulated unboundedly over the time.
Integrated navigation is the remedy for this limitation of SINS[1–3]. Kalman filter algorithm is
invariably used for integration of SINS with other non-inertial navigation aids. The key function
performed by the Kalman filter is the statistical combination of navigation aids and SINS
information to track drifting parameters of the gyros and accelerometer used in SINS. As a result,
the SINS can provide navigation solutions with enhanced precision during periods even when
aiding signals may be lost, and the improved position and velocity estimates from the SINS can
then be used to make aiding signal reacquisition, happen much faster when the those signal
becomes available again[4]. In this research work GPS and CNS are used as external navigation
aids to contain the drifting behavior of inertial sensors used in SINS. The scheme is implemented
on a simulated trajectory of an aircraft for duration of 2300s. In a standard centralized Kalman
Filter (CKF) integration architecture, the systematic errors and noise sources of all of the navigation
sensors are modeled in one Kalman filter. This ensures that all error correlations are accounted for,

all measurements optimally weighted, and the maximum information used to calibrate each error.
CKF architecture provides the optimal navigation solution in terms of accuracy. With all of the
error sources modeled in one place, the principal disadvantage of CKF is a high processor load.
With no independent subsystem navigation solutions available, processor-intensive parallel filters
are required for applications with fault-tolerant requirements. FKF avoids the theoretical and
practical limitations of CKF. This is realized by means of information sharing methodology[5][6].
The observations from the different aiding sensors are generally independent of each other therefore
any error in one sensor will not contaminate the solution from other sensor. This particular feature
can be observed from Figure 6 where attitude solutions provided by SINS/CNS filter are not
affected by errors in velocity and position measurements from GPS; furthermore, the redundant
observations can be used to detect or accommodate the outliers. Moreover, the throughput is
significantly improved due to the parallel processing. Each local filter is dedicated to a separate
sensor subsystem, and also uses data from the common reference SINS. The SINS acts as a central
sensor in the architecture, and its data is the measurement input for the master filter. In this research
work, the data from the CNS and GPS is dedicated to the local KF-1 and KF-2 respectively as
shown in Figure 1.
2. Fault Tolerant Federated Kalman Filtering Architecture
The presented integrated navigation architecture realized through FKF is shown in Figure 1. As
elucidated each KF is dedicated to a distinct sensor subsystem, and also uses data from the central
system SINS. The data from the GPS and CNS is dedicated to the corresponding KFs which after
implementation provide their solutions to the MF for the master update, yielding a global solution
[6], [7]. Moreover a fault detection and adaption of statistical features has been implemented for
each local filter.
Master
Filter
SINS
GPS
CNS
Adaption
of
R
KF-1
For
SINS/GPS
KF-2
For
SINS/CNS
Time
Update
Measurement
Update
1
1
ˆ ,f fx P 
1
2
ˆ ,f fx P 
1 1
ˆ ,x P
2 2
ˆ ,x P
Corrected Solution for Position, Velocity and Attitude
+
-
+
-
Adaption
of
R
Observation For KF-2
Observation For KF-1
Computation
of Weighting
Factors
&
Final Data
Fusion
Corrections
Figure 1 SINS/GPS/CNS Navigation Scheme Based on FKF
Time update equations for the state vector and the state error covariance for LFs and MF are:
, | 1 , , 1
ˆ ˆ ,i k k i k i kx x   , 1 1
, | 1 , 1 ,[ ] , 1,2, ,T
i k k k i k k i kP P Q i n 
       (1)
Kalman gain for each local filter is computed as:

1
/ 1 / 1 1( )T T
k k k k k k k k kK P H H P H R 
    (2)
Measurement update equations for the LFs are:
1 1 1
, , | 1 ,
T
i k i k k i i k iP P H R H  
  ,  1 1
, , , | 1 , | 1 , ,
ˆ ˆ , 1,2, ,T
i k i k i k k i k k i i k i kx P P x H R z i n 
     (3)
Fusion update is performed as follows:
1 1
, ,
1
l
f k i k
i
P P 

 
,
1
, , , ,
1
ˆ ˆ
l
f k f k i k i k
i
x P P x

 
  
 
 (4)
Where 1
,
f fn n
f kP

 is the inverse of the fused covariance, ,
ˆ fn
f kx  is the fused state estimate.
In order to suppress the effect of correlation, the process noise and the state vector covariance are
set to their upper bounds as per Eqs (5).
1
, ,i k i kQ Q

,
1
, , , , 1,2, ,i k i k f kP P i n
   (5)
Where ( 0)i  are information sharing coefficients satisfying the following condition[5]:
1 2 1n     (6)
When the fusion reset mode is realized, the covariance matrices of LFs and MF are reset by using
Eqs (5). The state estimates of the LFs are also reset by the fused solution as shown in Eq (7)
, ,
ˆ ˆ , 1,2,i k f kx x i n   (7)
The measurement noise covariance matrix kR is made adaptive using following equation
ˆ
k kR R (8)
 is computed using Fuzzy Inference System (FIS) taking relative degree of mismatch (RDOM)
given byEq (9) as input function and  as output function.
  
 
1
/ 1 1
1
ˆ ˆ
k
T
k k k k k k
i k M
i T
k k k k k
trace z H x z H x
M
RDOM
trace H P H R
 
  
 
 
  
 


(9)
In equation   
1
1
ˆ ˆ
k T
k k k k k k
i k M
z H x z H x
M
 
  
  the actual innovation covariance and
/ 1 1
T
k k k k kH P H R  is the theoretical innovation covariance[8], [9].
3. Measurement Model for Local Filters
The measurement models for local filters, SINS/GPS and SINS/CNS are represented by the
corresponding measurement matrices 1H and 2H respectively are given as follows[10], [11]:
 
   
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
 
 
 
 
  
 
 
 
 
(10)

Whereas: 2
1 2 3( )M eR R e Sine    and 2
R 1( )N eR Sine  
2 3 12
3 15
0
0 0
0 1
X
X
Cos Cos Sin
H Sin Cos Cos
Sin
  
  

  
   
  
(11)
In Eq (11)  and  are heading and pitch of the vehicle respectively.
4. Weighted and Scaled Data Fusion
The conventional scheme for detection and isolation of faults in local filters is a 2
 test wherein a
test statistic
1
ˆ ˆ( )
T T
i k k k k k k k kk z H x HP H R z H x
  
              , a scalar having 2
 distribution is
tested against a determined threshold. If ( )i k is found exceeding the threshold the measurement
update for that particular epoch is skipped. But here in this work we have introduced the scaled
weighting factors, i which adjust the weights or contributing factor for each local filter estimates
and fused estimates. Finally based on all weighting factors a global solution is obtained. The
deriving variable in computation of weighting factors is the iRDOM . For each value of iRDOM a
corresponding confidence factor  is computed as shown in Figure 2.  is equal to 1 for a range of
 0.7 1.3iRDOM  indicating normal operation, value of  grows linearly for range
 0.1 0.7iRDOM  while  decreases linearly for  1.3 10iRDOM  and finally 0  for
   0.1 , 10i iRDOM RDOM  . These computation of  Vs RDOM can be made using FIS or
simple piecewise linear & constant function. Here application of probability set theory is made to
explain the relative confidence factors and weighting factors related with individual local filters and
the fused filter using FKF. Let 1C , 2C and 3C are the subsets of confidence for which the local
filters SINS/GPS (KF-1), SINS/CNS and FKF are effectual respectively[12]. It may be noted that
3C is the subset of confidence for which the data fusion using FKF is valid. Another subset 0C can
be defined as the subset of confidence for which the solutions from both local filters (KF-1 and KF-
2) becomes ineffectual. These subsets of confidence can be defined by following Eqs
1 1( )P C  , 2 2( )P C  and 3 1 2 1 2( ) ( )P C P C C  
Let us define a confidence space as follows:
 1 1 1, , 0.1 10iS C C C RDOM     (12)
In the light of above-mentioned subsets of confidence and the overall confidence space we can
compute the Probability of validity or relative weighting factor corresponding to each local filter as
in following Eqs.
Weighting factor for SINS/GPS validity:
1 1 1 2 1 1 2( ) ( ) ( )P C P C P C      (13)
Weighting factor for SINS/CNS validity:
2 2 1 2 2 1 2( ) ( ) ( )P C P C P C      (14)
Weighting factor for SINS/SNS/GPS Integration Using FKF validity (or when both local filters are
valid hence FKF based data fusion is valid):
3 1 2 1 2( ) ( )P C P C   (15)

Finally Weighting factor for the probability that both SINS/GPS and SINS/CNS local filters are
invalid hence data fusion based FKF is also invalid given by
0 1 2 1 2 31 ( ) 1P C C         (16)
Having computed all of weighting factors final data fusion at epoch k is done as follows:
Time update of state vector and state vector covariance is given as:
| 1 1
ˆ ˆk k k kx x   (17)
| 1 1
T T
k k k k k kP P Q      (18)
The measurement update for the presented scheme is given as follows:
0 / 1 1 1, 2 2, 3 ,
ˆ ˆ ˆ ˆ ˆk k k k k f kx x x x x       (19)
       
   
0 / 1 1 1, 1, 1, 2 2, 2, 2,
3 , , ,
ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ
ˆ ˆ ˆ ˆ
T T
k k k k k k k k k k k k k
T
f k k f k k f k
P P P x x x x P x x x x
P x x x x
  

        
   
(20)
Figure 2 RDOMiVs Confidence Factor (η)
Time update to be used for next epoch is computed as:
1| 1
ˆ ˆk k k kx x   (21)
1| 1 1
T T
k k k k k kP P Q       (22)
It may be noted that if the confidence factor of SINS/GPS or SINS/CNS filter of FKS is 1, it makes
0 0  meaning that time update prediction component in Eq (19) is zero or time update is not used
in final data fusion. Otherwise if confidence factors of SINS/GPS and SINS/CNS both are 1, it
makes 0 1 2, and   all equal to 0 and 3 1  meaning that in this case final data fusion fall back
to simple FKF given by Eq (4) or:
, ,
ˆ ˆ ,k f k k f kx x P P  (23)
0 1 2 3 4 5 6 7 8 9 10
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
ConfidenceFactor()
RDOM
i

(0.1,0)
(0.7,1)
(1.3,1)

If the confidence factor of SINS/GPS and SINS/CNS are less than 1, then 0 0  meaning that the
contribution of time update in the final data fusion will be existent, and solutions of KF-1,KF-2 and
FKF will be scaled accordingly. If the confidence factors of both local filters are equal to 0 making
0 1  , meaning that in the final data fusion step, only time update will be used and the
measurement update and hence FKF fusion will be skipped.
5. Simulation and Results
To validate the performance of the scheme presented in this paper, a variation of simulations have
been performed employing CKF, standard FKF and FKF with weighted factors etc. The fault
tolerant strength has been tested by introducing deliberate errors of different types in the
measurement of aiding sensors at different time segments. For brevity of the paper the simulation
results for excessive random errors in GPS Position and Velocity measurement (10 times the
standard deviations) during time segment of 1800 to 1900s, are shown in Figure 4 to Figure 6. It
can be seen that the presented scheme has efficiently contained the errors as compared to standard
FKF scheme. The simulation results indicate that the excessive random error in KF-1 do not affect
the attitude solutions provided by KF-2.
Figure 3 Trajectory of the Vehicle
Figure 4 Velocity Errors
40
40.1
40.2
40.3
40.4
40.5
40.6
40.7
40.8
40.9
114.8
115
115.2
115.4
115.6
115.8
116
116.2
116.4
-0.5
0
0.5
1
1.5
2
2.5
3
x 10
4
Latitude (deg)
Trajectory
Longitude (deg)
Altitude(m)
Actual Trajectory
Estimated Trajectory
0 500 1000 1500 2000 2500
-0.03
-0.02
-0.01
0
0.01
Velocity Error (m/s)
VE
[m/s]
Standard FKF with Errors
FKF With Weighting Factor with Errors
Standard FKF Without Errors
0 500 1000 1500 2000 2500
-0.03
-0.02
-0.01
0
0.01
0.02
VN
[m/s]
0 500 1000 1500 2000 2500
-0.02
0
0.02
0.04
0.06
0.08
VU
[m/s]
Time [sec] 

Figure 5 Position Errors
Figure 6 Attitude Errors
6. Conclusions
A fault tolerant integrated navigation algorithm based on FKF is presented in this paper. The
scheme employs a fault tolerant federated filtering architecture and combines multiple sensors
providing navigation information, including SINS, GPS and CNS. A scheme for online fault
detection, isolation and accommodation in implemented through tuning/adapting of weighting
factors of local filters. To adapt the measurement noise covariance of various contributing sensors
the innovation/residual based adaptive scheme has also been employed. The presented scheme
0 500 1000 1500 2000 2500
-50
0
50
100
Position Error (m)(Lat)[m]
0 500 1000 1500 2000 2500
-100
-50
0
50
100
(Long)[m]
0 500 1000 1500 2000 2500
-2
-1
0
1
2
3
4
(H)[m]
Time [sec] 
0 500 1000 1500 2000 2500
-100
0
100
200
300
400
Attitude Error (Arc Sec)

z
[]
0 500 1000 1500 2000 2500
-400
-300
-200
-100
0
100

x
[]
0 500 1000 1500 2000 2500
-100
0
100
200
300
400

y
[]
Time [sec] 

enables the system to detect and isolate the excessive random or step faults at sensor level. A
variety of simulations have been carried out to evaluate the performance of the presented scheme.
The presented scheme can be extended to any number of redundant/aiding navigation sensors. The
scheme can be applied to UAV or aircraft navigation systems efficiently and effectively.
Reference
[1] R. M. Rogers, Applied Mathematics in Integrated Navigation Systems [M], 3rd ed. 2007.
[2] P. D. Groves, Principles of GNSS, inertial, and multisensor integrated navigation systems
[M]. Artech House, 2008.
[3] J. A. Farrell, AIDED NAVIGATION GPS with High Rate Sensors. McGraw-Hill, 2008.
[4] M. S. Grewal, L. R. Weill, and A. P. Andrews, Global positioning systems, inertial
navigation, and integration [M]. Wiley-Interscience, 2007.
[5] N. A. Carlson, “Federated square root filter for decentralized parallel processors [J],”
Aerospace and Electronic Systems, vol. 26, no. 3, pp. 517–525, 1990.
[6] N. A. Carlson, “Federated filter for fault-tolerant integrated navigation systems [C],” in
Position Location and Navigation Symposium, Navigation into the 21st Century, 1988, pp.
110–119.
[7] J. Ali, “Augmented Performance Navigation Solution Realized Through Advanced Filtering
Methods for Multisensor Data Fusion[D],” 2006.
[8] R. Mehra, “Approaches to adaptive filtering [J],” IEEE Transactions on Automatic Control,
vol. 17, no. 5, pp. 693–698, 1972.
[9] A. H. Mohamed, “Optimizing the Estimation Procedure in INS/GPS Integration for
Kinematic Applications [D],” 1999.
[10] M. Ushaq and J. Fang, “An Improved and Efficient Algorithm for SINS/GPS/Doppler
Integrated Navigation Systems [J],” Applied Mechanics and Materials, vol. 245, no. 4, pp.
323–329, 2013.
[11] M. Ushaq, “Research on SINS Based Navigation Techniques [D].” Beihang University, 2003.
[12] C. Pingyuan and X. Tianlai, “Data fusion algorithm for INS/GPS/Odometer integrated
navigation system [C],” in 2nd IEEE Conference on Industrial Electronics and Applications,
2007, pp. 1893–1897.

1-A Fault Tolerant Integrated Navigation Scheme Realized through Online Tuning of Weighting Factors for Federated Kalman Filter

More Related Content

What's hot (16)

Similar to 1-A Fault Tolerant Integrated Navigation Scheme Realized through Online Tuning of Weighting Factors for Federated Kalman Filter (20)

1-A Fault Tolerant Integrated Navigation Scheme Realized through Online Tuning of Weighting Factors for Federated Kalman Filter