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

aPhD 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

FilterSINS

GPS

CNS

AdaptionofR

KF-1For

SINS/GPS

KF-2For

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

+

-

+

-

AdaptionofR

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ˆ ˆ

kT

k k k k k k

i k M

i T

k k k k k

trace z H x z H xM

RDOMtrace H P H R

(9)

In equation 1

1ˆ ˆ

kT

k k k k k k

i k M

z H x z H xM

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: 21 2 3( )M eR R e Sine and 2R 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 statistic1

ˆ ˆ( )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.3 iRDOM 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 100

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Co

nfi

den

ce F

acto

r (

)

RDOMi

(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

4040.1

40.240.3

40.440.5

40.640.7

40.840.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 104

Latitude (deg)

Trajectory

Longitude (deg)

Alt

itu

de

(m)

Actual Trajectory

Estimated Trajectory

0 500 1000 1500 2000 2500-0.03

-0.02

-0.01

0

0.01Velocity Error (m/s)

V

E [

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

V

N [

m/s

]

Standard FKF with Errors

FKF With Weighting Factor with Errors

Standard FKF Without Errors

0 500 1000 1500 2000 2500-0.02

0

0.02

0.04

0.06

0.08

V

U [

m/s

]

Time [sec]

Standard FKF with Errors

FKF With Weighting Factor with Errors

Standard FKF Without Errors

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

100Position Error (m)

(L

at)

[m

]

Standard FKF with Errors

FKF With Weighting Factor with Errors

Standard FKF Without Errors

0 500 1000 1500 2000 2500-100

-50

0

50

100

(L

on

g)

[m]

Standard FKF with Errors

FKF With Weighting Factor with Errors

Standard FKF Without Errors

0 500 1000 1500 2000 2500-2

-1

0

1

2

3

4

(H

) [m

]

Time [sec]

Standard FKF with Errors

FKF With Weighting Factor with Errors

Standard FKF Without Errors

0 500 1000 1500 2000 2500-100

0

100

200

300

400Attitude Error (Arc Sec)

z [

]

Standard FKF with Errors

FKF With Weighting Factor with Errors

Standard FKF Without Errors

0 500 1000 1500 2000 2500-400

-300

-200

-100

0

100

x [

]

Standard FKF with Errors

FKF With Weighting Factor with Errors

Standard FKF Without Errors

0 500 1000 1500 2000 2500-100

0

100

200

300

400

y [

]

Time [sec]

Standard FKF with Errors

FKF With Weighting Factor with Errors

Standard FKF Without Errors

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.