kalman_filter_for_INS_Gps

Centralized Kalman Filter with Adaptive Measurement Fusion: its Application to a GPS/SDINS Integration System with an Additional Sensor

Tae-Gyoo Lee

Abstract: An integration system with multi-measurement sets can be realized via combined

application of a centralized and federated Kalman filter. It is difficult for the centralized

Kalman filter to remove a failed sensor in comparison with the federated Kalman filter. All

varieties of Kalman filters monitor innovation sequence (residual) for detection and isolation of

a failed sensor. The innovation sequence, which is selected as an indicator of real time

estimation error plays an important role in adaptive mechanism design. In this study, the

centralized Kalman filter with adaptive measurement fusion is introduced by means of

innovation sequence. The objectives of adaptive measurement fusion are automatic isolation

and recovery of some sensor failures as well as inherent monitoring capability. The proposed

adaptive filter is applied to the GPS/SDINS integration system with an additional sensor.

Simulation studies attest that the proposed adaptive scheme is effective for isolation and

recovery of immediate sensor failures.

Keywords: Centralized Kalman filter, federated Kalman filter, innovation sequence, adaptive

measurement fusion, GPS/SDINS integration system with an additional sensor.

1. INTRODUCTION

The CKF (Centralized Kalman Filter) can be applied to a system with multi-measurement sets to determine an optimal estimation of global system states. Although the CKF provides an optimal solution to the estimation problem, the large number of states often requires processing data rates that cannot be maintained in practical real time applications. Moreover, the estimate contains the measurement history of all previous updates. If a sensor failure is detected, it is difficult to remove the failed sensor data from the estimate. For these reasons, parallel structures can often provide improved failure detection and correction, enhanced redundancy management, and decreased costs for system integration. As such, there has recently been considerable interest shown in decentralized Kalman filter architectures.

One architecture that has received considerable attention as a practical means of decentralization is the FKF (Federated Kalman Filter). FKF differs from the conventional Kalman filter because each measurement is processed in local filters, and the results are combined in a master filter. The local filters run completely independent of each other, providing isolation between filters in the instance of sensor failure. The primary disadvantage is that the FKF does not give performance equal to that of the CKF, even when local filters are based on true models of the system. Furthermore, the FKF with the existing system requires additional processor burden to implement the local filters [1, 2].

The accuracy of Kalman filters depends on a priori knowledge of system models and noise statistics. In practical applications, priori knowledge is somewhat inaccurate. The estimation accuracy will be degraded from the theoretical prediction. The purpose of an adaptive filter is to reduce or bound the gaps by modifying or adapting the Kalman filter. A number of approaches can be taken to adaptive filtering. Since the basic source of uncertainty is due to unknown priori statistics of noise, one can estimate them on-line from the observed data. Another approach is to estimate the optimal Kalman gain directly without estimating the covariance of the process and measurement noise [3-5].

In the navigation system, a number of researches are to integrate the GPS (global positioning system) into the INS (inertial navigation system) [6-10]. In addition, the GPS/INS integration system employing other navigation systems is designed to provide a high level of accuracy and fault detection/isolation. That can be realized via the application of a CKF, a FKF and an adaptive filter [1, 2, 7].

In this study, first, the CKF and the FKF are summarized. Secondly, the adaptive measurement

__________

M anuscript received November 15, 2002; revised April 30, 2003; accepted July 24, 2003. Recommended by Editorial Board member Taek Lyul Sang under the direction of Editor Chung Choo Chung.

Tae-Gyoo Lee is with the Inertial Navigation Laboratory, Agency for Defense Development, Yuseong P. O. BOX 35-5, Daejeon, Korea (e-mail: tglee@add.re.kr).

fusion scheme is derived through the innovation sequence based on the CKF. Lastly, the proposed adaptive filter is compared with the CKF and the FKF by simulations of the GPS/SDINS (strapdown inertial navigation system), which is another sensor integration system. The test results indicate that the suggested method performs well against abrupt sensor failure.

2. CENTRALIZED AND FERDERATED

KALMAN FILTER A linear system can be described as the space state model.

()(1)(1)(1),

()()(),

x k k x k w k z k H x k v k =Φ???+?=?+ (1)

where

()n x k ∈\ :state vector, ()n n

k ×Φ∈\

:transition matrix,

()n w k ∈\ :system noise with zero mean,

and known covariance ~(0,),Q ()p z k ∈\ :measurement vector,

p n H ×∈\ :measurement matrix,

()p v k ∈\ :measurement noise with zero mean

and known covariance ~(0,).R

A linear optimal estimation (Kalman filter) of the state ()x k can be designed as follows [7, 11].

?(/1)(1)x

k k k ?=Φ???(1),x k ? (/1)(1)(1)(1),T P k k k P k k Q ?=Φ????Φ?+ (2) 111()(/1),T P k P k k H R H ???=?+??

1?()()P k x k ??=11?(/1)(/1)(),T P k k x k k H R z k ?????+??

where

?(/1)n x k k ?∈\ :priori estimate of (),x k ?()n x

k ∈\ :posteriori estimate of (),x k

(/1)n n

P k k ×?∈\

:priori covariance matrix

of estimation errors,

()n n P k ×∈\ :posteriori covariance

of estimation errors.

Multi-measurement sets are available as follows:

()()()i i i i z k H x k v k =?+,1,2,...,i N = (3)

where

()i p i z k ∈\ :i-th measurement vector,

i p n i H ×∈\ :i-th measurement matrix,

()i p i v k ∈\ :i-th measurement noise ~(0,).i R

The CKF and the FKF can be constructed as follows [1, 2]:

Centralized Kalman Filter (CKF)

?(/1)c x

k k ?=?(1)(1),c c k x k Φ??? (/1)(1)(1)(1),T c c c c c P k k k P k k Q ?=Φ????Φ?+(4) 11

11

()(/1)

,N

T c c i i i i P k P k k H R H ???==?+??∑

1

?()

()c c P k x

k ??=1?(/1)(/1)c c P k k x k k ???? 11(),N

T i i i i H R z k ?=+??∑

where

?(/1)c n c x k k ?∈\:priori estimate of ()x k in CKF, c c n n c Q ×∈\:covariance matrix of system noise

in CKF,

?()c n c x

k ∈\:posteriori estimate of ()x k in CKF,

(/1)c c n n c P k k ×?∈\:priori covariance matrix of estimation errors in CKF,

()c c n n c P k ×∈\:posteriori covariance of estimation errors in CKF.

Federated Kalman Filter (FKF) with no-reset mode

Local Filter (1,2,...,i N =)

?(/1)i x k k ?=?()(1),i i k x k Φ?? (/1)()(1)(),T i i i i i P k k k P k k Q ?=Φ???Φ+ (5) 111()(/1),T i i i i i P k P k k H R H ???=?+?? 1?()()i i P k x

k ??=1?(/1)(/1)i i P k k x k k ???? 1().T i i i H R z k ?+??

Master Filter

1

11

()

(),N

f i i P k P k ??==∑ (6)

1

?()

()f f P k x

k ??11

?()(),N

i i i P k x k ?==?∑

where

?(/1)i n i x

k k ?∈\:priori estimate of ()x k in the i -th local filter,

i i n n i Q ×∈\:covariance matrix of system noise in the i -th local filter,

?()i n i x

k ∈\:posteriori estimate of ()x k in the i -th local filter,

(/1)i i n n i P k k ×?∈\:priori covariance matrix

of estimation errors in the i -th local filter,

()i i n n i P k ×∈\:posteriori covariance of estimation errors in the i -th local filter,

()f f

n n f P k ×∈\:fused covariance in the master filter,

?()f

n f x

k ∈\:fused estimate in the master filter.

In the FKF, the information is not shared between

the local filters. If a failure is detected at the local filter, it is simply dropped from the master filter solution.

3. ADAPTIVE MEASUREMENT FUSION

In this study, the adaptive measurement fusion method is derived by the estimation of measurement noise covariance.

The difference between the current measurement and the priori estimate is called innovation sequence ()p c k ∈\.

?()()(/1)?()()(/1)?[()(/1)]()c k z k H x

k k H x k v k H x

k k H x k x

k k v k =???=?+???=???+ (7)

The covariance of the innovation sequence ()C k ∈ p p ×\can be defined as follows [4, 7]:

()[()()]

(/1)T T

C k E c k c k H P k k H R

=?=???+ (8)

The innovation sequence plays an important role in the adaptive algorithm design and can be selected as an indicator of the actual estimation errors. The Kalman filter determines within its calculation procedure the covariance matrix of estimation errors that can be considered as a theoretical idea of estimation accuracy. In practical applications, a gap can be observed between actual estimation errors and theoretically predicted ones. This can be explained by the fact that the applied system models are never exactly correct. Moreover, in all types of Kalman filters, the monitoring of the innovation sequence can be employed for fault detection, which relies on the statistics of the noise sequences. The calculated standard deviation is determined by the square root of the diagonal elements of (8). Therefore, (7) may be statistically bounded by three times the standard deviation. This monitoring tends to work well with

failures that cause immediate and catastrophic

malfunction. However, since a common and slowly changing bias is introduced to both the measurements and the estimates, these failures can be undetected by (7).

In order to provide information from actual estimation errors, the innovation sequence can be used. For stationary systems and noise, an estimation of ()C k can be determined as follows:

11?()()()k T i C k c k c k k ==?∑ or ?()C

k =11?(1)()().T k C k c k c k k k

??+? (9)

In this study, the innovation sequences are derived from the measurement sets and the priori estimates based on the CKF, as follows:

?()()(/1)?[()(/1)]()i i i c i c i c k z k H x

k k H x k x

k k v k =???=???+ (10)

1,2,...,.i N =

Using (8,9) and the covariance of (10) the real time estimates can be determined as follows:

()(/1),T i i c i i C k H P k k H R =???+ (11) ?()i C k 11?(1)()()T i i i

k C k c k c k k k

?=?+? (12) 1,2,...,.i N =

Substituting (12) into (11), the measurement noise covariance can be estimated as follows:

??[()(/1)]T i i i c i

R diag C k H P k k H =???? (13) 1,2,...,.i N =

At the beginning of the estimation process the

evaluation of ?i

R can be a negative definite due to the lack of statistics for the estimation of ()i C k . The normalization procedure can be introduced as follows:

If diag(?i

R

)<0 , then diag(?i

R )=ε. (14) 1,2,...,.i N =

where ε is a small positive. The measurement update of the CKF can be represented as follows:

111(/)(/1),c c c P k k P k k R ???=?+ (15) 1?(/)(/)c c P k k x

k k ??=1?(/1)(/1)c c P k k x k k ???? 1(),

c c R z k ?+?

Table 1. CKF with adaptive measurement fusion.

kalman_filter_for_INS_Gps

where

1

11

?,N

T n n c i i i

i R H R H ??×==??∈∑\ 1

11

?()().N

T c

c i i i

i R z k H R z k ??=?=??∑ (16)

kalman_filter_for_INS_Gps

(16) is the case in which posteriori covariance and estimates of the local filter in (6) are replaced by the measurement covariance and measurements of the sensors. When the CKF is similar to the Kalman filter with one measurement, such as (2), it is difficult to isolate the failed sensor.

kalman_filter_for_INS_Gps

Consequently, the fusion equation (16) or (6) has a clear physical meaning. When the arbitrary sensor shows high magnitude covariance of measurement noise or estimation error in comparison with others, the fusion mechanism doesn’t rely on these estimates or measurements. In the opposite situation, the fusion scheme does rely on the sensor. However, since the conventional Kalman filter has fixed noise covariance, the fusion equations are constructed by initial conditions. For this reason, the adaptive scheme (13) is useful. Generally, when the sensor output fails or is inconsistent, (13) causes the covariance measurement to increase. As a result, the estimates are less affected by the failed sensor. That signifies the conventional Kalman filter with (13)

obtains the fault tolerance function. In the FKF, these

problems can be solved through the isolation of local filters. But since the performance of (13) for fault tolerance is degraded with respect to time k , the moving data or instantaneous value of the innovation sequence can be used. However, these methods can cause the global estimation accuracy to worsen

because the adaptive mechanism is incapable of exactly reflecting noise statistics.

Table 1 summarizes the adaptive scheme based on the CKF.

4. SIMULATION STUDY The proposed adaptive scheme is applied to the GPS/SDINS integration system with an additional position sensor (such as radar) and compared with both the CKF and the FKF. The structures of the FKF and CKF with adaptive measurement fusion are shown in Fig. 1. These filters are realized using the factored covariance (U/D) form of the Kalman update equation. The configurations of filters are consistent with the indirect feedforward structure. The advantage of the indirect feedforward scheme is that sensors are totally independent on the integration filters [8, 11]. In designing integration systems with INS, one of the main concerns is the INS error model because it plays an important role in implementing the filter, accomplishing superior performance, and analyzing the characteristics of the navigation error propagation [6, 9]. In this study, the SDINS error model is derived by the perturbation method with respect to horizontal

(a) Centralized filter and adaptive measurement fusion.

(b) Federated filter (no-reset mode).

Fig. 1. Configurations of filter.

kalman_filter_for_INS_Gps

T

kalman_filter_for_INS_Gps

The flight distance to east direction[m]

h e f l i g h t d i s t a n c e t o n o r t h d i r e c t i o n [m ]

Fig. 2. Navigation scenario for simulation.

P

-20

20

40

60

80

100

120

140

160

180

-30

0306090120150180210240

270

300o s i t i o n e r r o r [m ]Time[sec]

Fig. 3. Pure navigation errors (RER) of SDINS (3

times).

position, horizontal velocity and attitude. Accelerometer and gyro biases are included. Therefore, the error model is designed by the 13th order. Consider the vertical channel of SDINS is compensated by a baro-altimeter. Accelerometer and gyro biases are modeled as random constants. As a result, the CKF for the proposed scheme is constructed as the 13th order filter. The local filter in the FKF can be designed by reduced order in comparison with the CKF. But in these tests, the local filters are also modeled as the 13th order.

Fig. 2 shows the navigation scenario. The flight time is 160 seconds. This trajectory is indicated in [12] and [13]. In simulations, consider that SDINS with the grade of 0.01[deg/h] provides navigation solution with 50 [Hz]. Fig. 3 shows the pure navigation error (RER, radial position error) of SDINS in this trajectory. Suppose GPS has 10 [m] C.E.P. (with instantaneous errors plotted in Fig. 4) and gives 1[Hz] of navigation data. The errors and the output frequencies of the applied additional sensor are assumed as 1, 10, 100 [m] C.E.P. and 0.1, 1[Hz], respectively. Assume the error of the

P

o s i t i o n e r r o r [m ]Time[sec]

Fig. 4. The errors (RER) of GPS (3 times).

P

-20

20

40

60

80

100

120

140

160

180

-20

020406080100120140160180-20

20

40

60

80

100

120

140

160

180

-200204060

80100120140160180o s i t i o n E r r o r [m ]Time[sec]

(A) Bias Type Fault

(B) Jump Type Fault

Fig. 5. Fault shapes.

Table 2. Simulation results in normal cases (Monte

Carlo 30 times).

additional sensor is similar in shape to GPS. In order to analyze the filter with the failed sensor, abrupt jump error (100 [m], 30 [sec]) and bias type error of the additional sensor are defined in Fig. 5.

Simulations are evaluated through two cases. First, when the navigation systems are under normal conditions, the statistics are analyzed by means of 30

Additional Sensor Output [Hz]

Additional Sensor Error [C.E.P .]

Centralized Kalman Filter [C.E.P .]

Federated Kalman Filter [C.E.P .]

Centralized Kalman Filter With Adaptive Fusion [C.E.P .]

1.360209

8.0025111

10.6656278.502861

10.2690050.1

10.723402

kalman_filter_for_INS_Gps

kalman_filter_for_INS_Gps

P

o s i t i o n E r r o r [m ]Time[sec]

(a) Estimate of filters and jump fault.

(The failed sensor is not isolated.)

P

o s i t i o n E r r o r [m ]Time[sec]

(b) Estimate of local filters in federated filter.

M

Time[sec]

e a s u r e m e n t N o i s e [m ]

(c) Estimate of measurement noise variance.

(east channel.)

Fig. 6. Performance of filters with jump fault. (GPS 1[Hz], Additional another sensor 1[Hz])

P

o s i t i o n E r r o r [m ]Time[sec]

(a) Estimate of filters and bias fault. (The failed sensor is not isolated.)

P

o s i t i o n E r r o r [m ]Time[sec]

(b) Estimate of local filters in federated filter.

M

Time[sec]

e a s u r e m e n t N o i s e [m ]

(c) Estimate of measurement noise variance.

(east channel.)

Fig. 7. Performance of filters with bias fault. (GPS 1[Hz], Additional another sensor 1[Hz])

kalman_filter_for_INS_Gps

P

o s i t i o n E r r o r [m ]Time[sec]

(a) Estimate of filters and jump fault.

(The failed sensor is not isolated.)

P

o s i t i o n E r r o r [m ]Time[sec]

(b) Estimate of local filters in federated filter.

kalman_filter_for_INS_Gps

kalman_filter_for_INS_Gps

M

Time[sec]

e a s u r e m e n t N o i s e [m ]

(c) Estimate of measurement noise variance.

(east channel.)

Fig. 8. Performance of filters with jump fault. (GPS 1[Hz], Additional another sensor 0.1[Hz])

P

o s i t i o n E r r o r [m ]Time[sec]

(a) Estimate of filters and bias fault. (The failed sensor is not isolated.)

P

o s i t i o n E r r o r [m ]Time[sec]

(b) Estimate of local filters in federated filter.

M

Time[sec]

e a s u r e m e n t N o i s e [m ]

(c) Estimate of measurement noise variance.

(east channel.)

Fig. 9. Performance of filters with bias fault. (GPS 1[Hz], Additional another sensor 0.1[Hz])

simulations. Secondly, when an additional sensor has 10[m] C.E.P. with faults (Fig. 5) and is not isolated, the performances of filters are investigated.

Table 2 shows the proposed filter, the CKF and the FKF under the normal conditions. On the whole, all of the filters demonstrate similar performances. When GPS and the additional sensor have the same error level and information period, the rate of performance improvement is higher than in other cases. This suggests that the outputs of integration filters are affected by the accuracy difference between sensors. When sensors cannot be synchronized, the local filter in the FKF is aided by the pure SDINS solutions or time update of the Kalman filter. The CKF and the proposed filter can only use the high frequency output sensor. The proposed adaptive filter gives inferior performance to others and generates near the high frequency output sensor. One reason is that the FKF and the CKF are well tuned through many simulations. On the contrary, the adaptive mechanism may not exactly reflect in noise statistics because the flight time is short. And the estimate of measurement noises is constructed by the high rate sensor (see Fig. 8 (c) and 9 (c)), because the proposed filter frequently uses the high frequency sensor.

The proposed method does have the following advantage. When another sensor is failed and is not isolated, RERs of the filter and estimates of noise covariance are shown in Fig. 6 and Fig. 7. In the CKF and the FKF, when the sensor experiences jump fault, the estimate is also increased. Once the fault disappears, the output slowly converges to a reasonable value. That means that the fault in the past affects the estimate for a long time because of the inherent performance of the Kalman filter. When the fault is abrupt bias, the filter outputs always converge toward near bias level. However, the filter employing the adaptive fusion is insensitive to abrupt failures. As such, the filter depends upon the sensor not failed and the estimate maintains the reasonable value such as indicated by the local filter with normal sensor in the FKF. Consequently, the adaptive mechanism causes the filter to isolate and recover the abrupt failed signal.

In the given faults, the CKF, the FKF and the proposed filter can isolate and recover by means of innovation sequence monitoring in (7). The FKF has the local filter outputs in Figs. 6 and 7. Therefore, the FKF may choose appropriately regarding faults. And the advantage of the CKF with adaptive measurement fusion is the self isolation and recovery of the abrupt failed sensors.

Figs. 8 and 9 show the performances of filters with different frequency of measurements, which is similar to Figs. 6 and 7. However, because the failed sensor has low frequency, the error level is lower than in previous cases. And figures show that the local filter has the SDINS pure solutions and time update in the FKF.

5. CONCLUSIONS

The FKF makes better choices relating to sensor failure than the CKF. In this study, the adaptive measurement fusion in the CKF is proposed for improving the isolation and recovery ability of some sensor failures. Via the development of algorithm and simulations of the GPS/SDINS/additional sensor integration system, the main advantages and disadvantages of the suggested adaptive filter are as follows.

(1) The abruptly failed (not failed) sensor can self-isolate (recover).

(2) The modification (by calculated covariance, moving data, instantaneous data and so on) is simple for desired applications.

(3) The global estimation accuracy can be degraded.

(4) If multi-measurement sets do not show the same rate output, then the estimate can depend upon the high rate output.

Remark 1: The adaptive scheme can be applied to one measurement system in (1) and (2). In this case, the abrupt jump signal can be isolated. However, the recovery time is proportional to the jump time. Moreover, the recovery cannot be accomplished. If the abrupt jump is the bias type (Fig. 5(a)), then the estimate will follow the fault signal. Therefore, the multi-measurement sets are useful. This is indicated in [7] and [8].

Remark 2: The suggested adaptive method can be directly developed for local filters in the FKF. Remark 3: To apply the master filter in the FKF, the fusion of priori estimates is required.

Remark 4: Both GPS and the additional sensor can fault simultaneously, but the SDINS may be available. INS does not require any external devices or signals and is not affected by any external environments. If the outage of sensors is short, the time update of the Kalman filter can be used, but the performance may be degraded.

REFERENCES

[1] N. A. Carlson, “Federated filter for computer-

efficient, near-optimal GPS integration,” IEEE

Trans. on Aerospace and Electronic Systems, pp.

306-314, 1996.

[2] K. Fried, Avoinics Navigation Systems, A Wiley-

Interscience Publication, 1997.

[3] P. S. Maybeck, Stochastic Models, Estimation

and Control volume 2, Academic Press, 1979. [4] R. K. Metra, “Approaches to adaptive filtering,”

IEEE Trans. on Automatic control, pp. 693-698,

October 1972.

[5] A. Gelb, Applied Optimal Estimation, M.I.T.

PRESS, 1974.

[6] G. M. Siouris, Aerospace Avionics Systems,

Academic Press, 1993.

[7] O. Salychev, Inertial Systems in Navigation and

Geophysics, Bauman MSTU Press Moscow,

1998.

[8] T. G. Lee, K. J. Kim, C. H. Je, and H. W. Park,

“Preprocessing scheme for stable cascaded

GPS/INS integration system,” The 4th Asian

Control Conference, Singapore, Sep. 2002. [9] D. H. Titterton and J. L. Weston, Strapdown

inertial navigation technology, Peper Peregrinus

Ltd., 1997.

Tae-Gyoo Lee received the B.S., M.S.

and Ph.D. degrees in Electrical

Engineering from Inha University in

1987, 1989 and 1996, respectively.

Since 1997, he has been a Senior

Researcher in the Inertial Navigation

Laboratory at the Agency for Defense

Development. His areas of interest include Integrated Navigation System, INS, GPS, etc.[10] E. D. Kaplan, Understanding GPS Principles

and Applications, ARTECH HOUSE, INC.,

1996.

[11] P. S. Maybeck, Stochastic Models, Estimation

and Control volume 1, Academic Press, 1979. [12] R. Braham, D. E. Mosher, and G. E. Foren,

“Ballistic missile defence: It’s back,” IEEE

Spectrum, September 1997.

[13] R. E. Stubbs, W. H. Huang, and E. Schmitz,

“Integration of GPS into a ballistic missle

navigation solution,” American Institute of

Aeronautics and Astronautics, Inc., pp. 1220-

1239, 1997.

kalman_filter_for_INS_Gps

相关推荐
相关主题
热门推荐