当前位置：文档库 › kalman_filter_for_INS_Gps

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

()(/1)

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

?()

()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

()

(),N

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

?()

()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

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

where

?,N

T n n c i i i

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

?()().N

T c

c i i i

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

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

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.

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.

-20

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

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

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

-20

100

120

140

160

180

-20

020406080100120140160180-20

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