Journal of Gloal Positioning Systems (26 Vol. 5 No. -2:62-69 Adaptive wo-stage EKF for INS-GPS Loosely Coupled System with Unnown Fault Bias Kwang Hoon Kim Jang Gyu Lee School of Electrical Engineering Computer Science Seoul National University 33-dong ASRI #6 San 56- Sillim-dong Gwana-gu Seoul 5-742 South Korea Chan Goo Par School of Mechanical Aerospace Engineering Seoul National University San 56- Sillim-dong Gwana-gu Seoul 5-744 South Korea Astract. his paper proposes an adaptive two-stage etended Kalman filter (AEKF for estimation of unnown fault ias in an INS-GPS loosely coupled system. he Kalman filtering technique requires complete specifications of oth dynamical statistical model parameters of the system. However in a numer of practical situations these models may contain parameters which may deviate from their nominal values y unnown rom ias. his unnown rom ias may seriously degrade the performance of the filter or cause a divergence of the filter. he two-stage etended Kalman filter (EKF which considers this prolem in nonlinear system has received considerale attention for a long time. he EKF suggested until now assumes that the information of a rom ias is nown. But the information of a rom ias is unnown or partially nown in general. o solve this prolem this paper firstly proposes a new adaptive fading etended Kalman filter (AFEKF that can e used for nonlinear system with incomplete information. Secondly it proposes the AEKF that can estimate unnown rom ias y using the AFEKF. he proposed AEKF is more effective than the EKF for the estimation of the unnown rom ias. he AEKF is applied to the INS-GPS loosely coupled system with unnown fault ias. Keywords. adaptive two-stage etended Kalman filter; covariance rescaling; unnown fault ias Introduction he well-nown Kalman filtering has een widely used in many industrial areas. his Kalman filtering technique requires complete specifications of oth dynamical statistical model parameters of the system. However in a numer of practical situations these models contain parameters which may deviate from their nominal values y unnown constant or unnown rom ias. hese unnown iases may seriously degrade the performance of the filter or even generate the divergence of the filter. o solve this prolem a new procedure for estimating the dynamic states of a linear stochastic system in the presence of unnown constant ias vector was suggested y Friedl (Friedl 969. his filter is called the two-stage Kalman filter (KF. Many researchers have contriuted to this prolem. Recently the KF to consider not only a constant ias ut also a rom ias has een suggested through several papers (Ignagni 99; Alouani 993; Keller 997; Hsieh 999; Ignagni 2. Several researchers performed the etension of the KF to nonlinear system (Shreve 974; Mendel 976; Caglayan 983. Unnown rom ias of nonlinear system may also generate the large prolem in the EKF ecause the EKF may e diverged if the initial estimates are not sufficiently good. So the EKF for nonlinear system with a rom ias has to assume that the dynamic equation the noise covariance of unnown rom ias are nown although these are unnown in general. o solve these prolems an adaptive filter can e used. his paper proposes an adaptive two-stage etended Kalman filter (AEKF y using an adaptive fading etended Kalman filter (AFEKF. As a result unnown rom ias can e estimated y the AEKF.
Kim et al.: Adaptive wo-stage EKF for INS-GPS Loosely Coupled System with Unnown Fault Bias 63 o verify the performance of the AEKF the proposed AEKF is applied to the INS-GPS loosely coupled navigation system with unnown fault ias. In general the INS-GPS loosely coupled system uses the EKF which cannot effectively trac time-varying parameters (Ljung 979. So when the navigation system has unnown fault ias the EKF cannot estimate unnown fault ias cannot eliminate this. his paper shows that the AEKF can estimate eliminate unnown fault ias in the INS-GPS loosely coupled system with unnown fault ias. 2 Prolem Formulation Consider the following nonlinear discrete-time stochastic system represented y ( = f + w (a + = A + w ( ( z = h + v (c where is the n state vector z is the m measurement vector is the p ias vector of unnown magnitude. All matrices have the appropriate dimensions. he noise sequence w w v are zero mean uncorrelated Gaussian rom sequences with w w Q E w w Q δ = j v v R (d where Q > Q > R > δ is the Kronecer j delta. he initial states are assumed to e uncorrelated with the white noise processes w w v. Assume that are Gaussian rom variales with * [ ] = * * ( ( E E P = > * [ ] = * * ( ( E * * ( ( E P E P = = > he function f ( ( h can e eped y aylor series epansion as f f ( ( = ( + ( + + F ( ( + ( + ( ( + + B ( ( + ( + ( ( + + ϕ ( + ( + f ( ( ( ( ( ( ( ( ( ( ( ( h( ( ( ( = ( ( h h where ( ( + H + D + ϕ (2 (3 are the state estimate the ias estimate respectively ϕ f ϕ mean the higher order h terms F ( ( + ( + = ( B f = + = ( + ( ( + ( + = ( H f = + = ( + ( ( ( = ( D h = = ( ( ( ( = ( h = = ( he prolem is to design an adaptive two-stage etended Kalman filter (AEKF to give a solution for nonlinear stochastic system with a rom ias on the assumption that the stochastic information of a rom ias is unnown or partially nown. 3 wo-stage Etended Kalman Filter in the Presence of a Rom Bias In this section we etend the optimal KF to nonlinear stochastic system models in order to otain a suoptimal EKF. Until now the several researchers have proposed the EKF (Mendel 976; Caglayan 983. But the proposed EKF was ased on the suoptimal KF. Recently several researchers have proposed the optimal KF for linear stochastic system with unnown rom ias (Keller 997; Hsieh 999; Ignagni 2. hus we newly propose the EKF ased on the optimal KF as Lemma 3.. Lemma 3.: A discrete-time two-stage etended Kalman filter is given y the following coupled difference.
64 Journal of Gloal Positioning Systems equations when the information of nonlinear stochastic system ( is perfectly nown. = + U + + (4a ( ( ( ( ( ( + = + + V + ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( U ( + ( + (4 P = P + U + + P (5a ( ( ( ( P + = P + + V P + (5 V where A Q are nown. he two-stage etended Kalman filter can e decomposed into two filters such as the modified ias free filter the ias filter. he modified ias free filter which is designed on the assumption that the iases are identically zero gives the state estimate (. On the other h the ias filter gives the ias estimate (. Finally the corrected state estimate ( of the EKF is otained from the estimates of two filters the coupling equation U V. he modified ias free filter is = F + + + + C + u (6a ( ( ( ( ( P F P (6 = ( + + ( + F ( + + + Q ( ( ( = ( ( ( ( ( ( ( ( ( ( ( + ( + = ( ( ( ( ( ( ( ( ( ( ( K P H (6c H P H R P I K H P ( η = z H E ( ( ( ( ( ( (6d (6e + = + K η (6f the ias filter is ( A ( = + (7a ( ( P = A P + A + Q (7 K ( ( ( = P ( N H ( ( P H + R + N P ( N ( ( ( ( ( (7c P ( + = I K ( ( N P ( ( ( (7d η = η N (7e ( ( + = + K η (7f with the coupling equations N = H ( ( ( U ( ( + ( + (8a + D ( ( ( U ( ( + ( + = U I Q P ( (8 F ( ( + ( + V ( ( ( U = + ( ( + ( + V ( ( ( = U ( ( + ( + (8d K N A (8c B ( ( ( ( + + ( ( ( ( u = U U + + A + + + (8e Q = Q + U Q U (8f Here the initial conditions: * * * ( + = ( V + = (9a ( + = P ( P P V P V where V P ( P =. + = P (9 he equations of the EKF are similar to the equations of the optimal KF. But although the structure of this EKF is possile for nonlinear stochastic system the coupling etween the modified ias free filter the ias filter in the nonlinear case eists all linearizations aout the state ias estimate have to e evaluated. hus a designer has to give attention to the linearization point when the EKF is used. In general the EKF may give iased estimates e diverged if the initial estimates are not sufficiently good. So the incomplete information of nonlinear system may generate the large prolem in the EKF. he EKF has also the same prolem. hus the EKF of Lemma 3. assumes that A Q are nown. However in most case these are unnown. If this information is incomplete the performance of the EKF may e degraded or the EKF may e diverged. o solve this prolem the EKF has to e adapted to environment of incomplete rom ias information. hus an adaptive two-stage etended Kalman filter is needed
Kim et al.: Adaptive wo-stage EKF for INS-GPS Loosely Coupled System with Unnown Fault Bias 65 4 Adaptive wo-stage Etended Kalman Filter in the Presence of a Rom Bias 4. Adaptive Fading Etended Kalman Filter using Innovation Covariance Consider the following nonlinear discrete-time stochastic system represented y ( + = f + + w ( 2 z = h + + v where measurement vector. Moreover (a ( is the n state vector z is the m w v denote sequences of uncorrelated Gaussian rom vectors with zero means. Each covariance matri is E w w j = Qδ E v v Rδ E w v j j = = j j ( where δ is the Kronecer delta. he variale j means the uncertainty term in the dynamic equation that is generated y incomplete process noise covariance unnown input ias or incomplete model coefficients. On the other h the variale 2 means the uncertainty term in the measurement equation that is generated y incomplete measurement noise covariance or unnown measurement ias. Also are independent of 2 w v respectively. If the system information is perfectly nown are all zero. 2 he function f ( ( h can e eped y aylor series epansion as f = f + + F + + ϕ + (2 ( ( ( ( ( ( ( f h h H ( = ( ( + ( ( + ϕ ( ( h f where F = H = ( + = ( (3 h =. he variale ϕ f ϕ h mean the higher order terms. Definition 4.: Several equations related to the innovation are arranged as follows. η = z h (4a ( ( ( C = E ηη H P H R = + (4 C = ηη i i (4c M i= M+ where η is the innovation of filter C is the calculated innovation covariance C is the estimated innovation covariance. Here M is a window size. P ( is a calculated error covariance of the EKF. he equations of the EKF are similar to those of the linear Kalman filter. hus first we derive an optimal property from the linear Kalman filter secondly ep this result in terms of nonlinear system. One of the important properties of the optimal linear Kalman filter is that the innovation is a white sequence when the optimal filtering gain is used. Although the EKF is a suoptimal filter this property can e used to improve the performance of the EKF in nonlinear system (. he following equation (5 is the auto-covariance of the innovation in the EKF: E η + η j = H + F j + j I K + j H + j LF [ I K H ] F P ( H K C + + + (5 j = 23 L ( S = P H K C (6 he white sequence has to satisfy a condition that the auto-covariance is zero ecause whiteness means uncorrelated in time. If the common term (6 of the auto-covariance (5 for all j = 2 3 L is zero then the innovation sequence is a white sequence. If we insert the error covariance the Kalman gain of the EKF into equation (5 the auto-covariance of the innovation is identically zero since S is zero for all j = 2 3 L. Actually the Kalman gain of the EKF is easily otained when S = in the equation (6. In case of the linear Kalman filter if the auto-covariance equation is zero we can say that the filter is optimal. But in case of the EKF the real innovation sequence is not a white sequence due to higher order terms to e neglected the linearization error. Nonetheless if the auto-covariance equation (5 of the innovation in the EKF is zero then the performance of the EKF will e improved. When the system model is incomplete the real covariance of the innovation is different from a theoretical one given in equation (4. Hence the real auto-covariance of the innovation may not e identically zero although S =. Under this acground this section newly proposes a new AFEKF. Definition 4.2: For a design of a new AFEKF let s define several variales. Assume that the system information is incomplete. From now P ( K C are defined as an error covariance a Kalman gain an innovation covariance for system with incomplete information. Here C is calculated from the model equation of system with incomplete information. hus C is also called as the calculated innovation covariance. On the other h P ( K C are defined as an error covariance an optimal Kalman gain an
66 Journal of Gloal Positioning Systems innovation covariance for system with complete information. We have to otain these P ( K C y the appropriate method ecause the information of system to e considered is incomplete. Here C can e otained from (4c. hus C is also called as the estimated innovation covariance. We assume P ( = λ P ( where λ is a forgetting factor. If P ( can e otained from P (. can e otained y λ then the Kalman gain Let s thin the case that the information of system is unnown or partially nown. If P ( can e otained y using the forgetting factor C can e eactly estimated y (4c then the Kalman gain K maes that S = where ( S = P H K C (7 However in many cases the difficulty is to get P ( K. herefore the filter is suoptimal for system with incomplete information in general. As a result the prolem is to see how to eactly otain P ( K. Definition 4.3: Scalar variale α = ma trace( C C m trace ( C or α = ma trace( C α is defined y where the AFEKF uses the relationship C = α C. K (8 In general the innovation covariance shows the effect of the present error ecause the innovation is easily affected y the error. Let s thin for system with incomplete dynamic equation. he real innovation covariance of the present is estimated as C. hen C is represented y ( λ ( C = α C = H P H + R = H P H + R (9 he increase of the innovation covariance y α is due to the increase of the error covariance y λ ecause the system has incomplete dynamic equation. On the other h R remains unchanged. If the increased error covariance is larger than R we can thin that α is almost equal to λ. Remar 4.: Sometimes we only partially now the dynamic equation of nonlinear stochastic system. hen the estimation error may e increased y the effect of unnown information. his means that the error increase is due to incomplete process noise covariance unnown input ias or incomplete model coefficients. he effects of incomplete information in dynamic equation can e compensated through the increase of the magnitude of P (. If we assume the relationship λ = α then the error covariance is P ( α P ( =. Definition 4.4: A discrete-time adaptive fading etended Kalman filter is given y the following coupled difference equations when the information of nonlinear stochastic system ( is partially nown. f ( = ( ( + ( ( (2a P = λ F P + F + Q (2 ( ( K = P H H P H R + (2c ( ( ( P + = I K H P (2d K z h where λ = α. ( + = ( + ( ( (2e 4.2 Adaptive wo-stage Etended Kalman Filter using the AFEKF he AEKF can e easily designed y the proposed AFEKF. his AEKF is used when the information of A Q are incomplete. For a compensation of the effects of incomplete information in the ias filter of the EKF the calculated innovation covariance the estimated innovation covariance are defined y (2 (2c. Definition 4.5: Several equations related to the innovation are arranged as follows. ( η = η N (2a C = H ( ( ( P ( H ( ( ( (2 + R + N P N C ( M = ηη i i i= M+ (2c he α is equal to the forgetting factor λ where C = α C. By the forgetting factor calculated from (2 (2c the state error covariance equation (7 is changed into ( ( P = λ A P + A + Q (22
Kim et al.: Adaptive wo-stage EKF for INS-GPS Loosely Coupled System with Unnown Fault Bias 67 he modified ias free filter of the EKF has u Q. For convenience the equation of (8e (8f are rewritten as (23a (23. In (23a u is related to the incomplete incomplete A Q. Q. In (23 ( ( ( + + ( ( U U I Q P ( A u = U U + + A + Q is also related to the ( + + + ( = + = U Q P ( A + + ( + + + (23a Q = Q + U Q U (23 For a compensation of the effects of incomplete information in the modified ias free filter of the EKF the each innovation covariance is defined y (24 (24c. Definition 4.6: Several equations related to the innovation are arranged as follows. η = z H E (24a ( ( ( ( C = H P H + R C ( ( ( ( ( ( ( M = ηη i i i= M+ (24 (24c he α is equal to the forgetting factor λ where C = α C. By the forgetting factor calculated from (24 (24c the state error covariance equation (6 is changed into F ( ( + ( + P ( + P ( = λ (25 F ( ( ( Q + + + he proposed AEKF is applied to the INS-GPS loosely coupled system with unnown fault ias in net section. = [ δ L δl δh δv δv δv φ φ φ ] (27 N E D N E D w = [ w w w w w w ] (28 LC ax ay az gx gy gz PINS PGPS zt ( = H t ( + v ( t = LC LC V V INS GPS 3 3 3 H LC = 3 3 3 (29 (3 where the error state variale ( t represents the position velocity attitude errors of vehicle the ias term t ( represents the inertial sensor ias errors which consist of the accelerometer ias error the gyro ias error w ~ N( Q LC LC is white noise of the inertial sensor v ~ N( R LC LC is white noise of the GPS. (Hong 24; itterton 997 Remar 5.: he accelerometer error consists of white Gaussian noise the accelerometer ias error which is assumed as a rom constant. he gyroscope error consists of white Gaussian noise the gyroscope ias error which is assumed as a rom constant. o analyze the performance of the INS-GPS loosely coupled system eperiments were carried on the campus of the Seoul National University. he Differential GPS (DGPS trajectory is used as a reference trajectory. he IMU (LP-8 the GPS receiver (CMC Allstar were mounted on the test vehicle. LP-8 provides raw IMU measurements at Hz while CMC Allstar provides GPS data at Hz. he specifications for LP-8 CMC Allstar are shown respectively in ale 5. ale 5.2. he initial alignment time is 3 sec the total navigation time is 49 sec. 5 Application to the INS-GPS Loosely Coupled System with Fault Bias 5. INS-GPS Loosely Coupled System he dynamic model of the INS-GPS loosely coupled system can e formed from the differential equations of the navigation error. he time varying stochastic system model is & ( t = F ( t ( t + G ( t ( t + G ( t w (t (26 INS LC LC LC 5.2 Fault Bias Estimation using the AEKF o verify the performance of the AEKF for the INS- GPS loosely coupled system with unnown fault ias we consider an accelerometer fault. We insert a fault ias into data otained from eperiment of section 5..
68 Journal of Gloal Positioning Systems ale 5. he specifications of LP-8 Sensor Accel. ( σ Gyro. ( σ Bias repeataility 5 µ g 3 deg/ h Scale Factor 5 ppm 5 ppm Misalignment 6 arcsec 6 arcsec Noise.2 ft/sec 5 arcsec Correlated Noise 4 µ g. deg/ h ale 5.2 he specifications of CMC Allstar Parameter Allatar ( σ Receive frequency L : 575.42 MHz racing code C/A code (SPS Position accuracy 6 m CEP Velocity accuracy.2 ms ale 5.3 he fault of the Accelerometer Fault position Magnitude of fault ias ime Accel. X-ais mg 3 mg 5 mg mg 8 sec As shown in ale 5.3 we insert fault iases of mg 3 mg 5 mg mg into X-ais of an accelerometer. he fault occurrence time is 8 sec. When the accelerometer sensor has a fault the position error of the AEKF is smaller than that of the EKF. Fig 5. shows the ias estimation results of the EKF the AEKF for several accelerometer fault iases. he tracing performance of the AEKF is etter than that of the EKF. he AEKF quicly tracs unnown fault ias. he estimate of fault ias is used for compensation of the sensor fault so the navigation error in the AEKF is more decreased than the EKF. Finally the result of the AEKF for jumping fault ias is shown in Fig 5.2. otally the AEKF can effectively trac unnown fault ias. 2 4 8 6 4 2 mg AEKF : ACC Bias Estimate Fault Signal 5mg 3mg mg -2 5 5 ime(sec ( Bias estimation (AEKF Fig 5. he ias estimation results of the EKF the AEKF for several accelerometer fault iases 6 4 5 4 3 2 - AEKF : ACC Bias Estimate Fault Signal mg 5mg -2 5 5 ime(sec Fig 5.2 he results of ias estimation in the AEKF for jumping fault ias 2 4 8 6 4 2 mg EKF : ACC Bias Estimate Fault Signal 5mg 3mg mg -2 5 5 ime(sec (a Bias estimation (EKF 6 Conclusion he prolem of unnown rom ias frequently happens in a numer of practical situations. he twostage Kalman filtering technique considers this prolem. he information of unnown rom ias is unnown in general. However until now a designer assumed that this information is nown when two-stage etended Kalman filter is designed. So to solve this prolem this paper proposes the adaptive two-stage etended Kalman filter for nonlinear stochastic system with unnown rom ias. o design the adaptive two-stage etended Kalman filter the adaptive fading etended Kalman filter is firstly designed. his AFEKF can e used for nonlinear stochastic system when the system information is partially nown. he AFEKF can e applied to nonlinear stochastic system with unnown rom ias. But the AFEKF can only estimate true state so this cannot eliminate unnown rom ias. On the other h the
Kim et al.: Adaptive wo-stage EKF for INS-GPS Loosely Coupled System with Unnown Fault Bias 69 AEKF can estimate true state unnown rom ias together so this can eliminate unnown rom ias. he proposed AEKF can e applied to the prolem related a fault compensation. he reliaility of vehicle or aircraft depends on integrated navigation system lie the INS-GPS loosely coupled system. hus the fault tolerance is very important in integrated navigation system. he EKF is widely used for integrated navigation system ut the EKF cannot effectively trac time varying parameters or unnown parameter. he EKF has also this prolem. his paper applies the AEKF to the INS- GPS loosely coupled system with unnown fault ias. Unnown fault iases in accelerometer can e effectively eliminated y the AEKF. As a result this paper shows that the performance of the AEKF is etter than that of the EKF in case of unnown fault ias. References Alouani A.. Xia P. Rice.R. Blair W.D. (993 On the Optimality of wo-stage State Estimation In the Presence of Rom Bias. IEEE ransactions Automatic Control AC-38 pp. 279 282. Caglayan A.K. Lancraft R.E. (983 A Separated Bias Identification State Estimation Algorithm for Nonlinear Systems. Automatica 9(5 pp. 56 57. Friedl B. (969 reatment of ias in recursive filtering. IEEE ransactions Automatic Control AC-4 pp. 359 367. Hong H.S. Lee J.G. Par C.G. (24 Performance improvement of in-flight alignment for autonomous vehicle under large initial heading error. IEE Proceedings Radar Sonar Navigation 5( pp. 57 62. Hsieh C.S. Chen F.C. (999 Optimal Solution of he wo-stage Kalman Estimator. IEEE ransactions Automatic Control AC-44 pp. 94 99. Ignagni M.N. (99 Separate-Bias Kalman Estimator with Bias State Noise. IEEE ransactions Automatic Control AC-35 pp. 338 34. Ignagni M.N. (2 Optimal Suoptimal Separate-Bias Kalman Estimator for a Stochastic Bias. IEEE ransactions Automatic Control AC-45 pp. 547 55. Keller J.Y. Darouach. M. (997 Optimal wo-stage Kalman Filter in the Presence of Rom Bias. Automatica 33(9 pp. 745 748. Ljung L. (979 Asymptotic Behavior of the Etended Kalman Filter as a Parameter Estimator for Linear Systems. IEEE ransactions Automatic Control AC-24 pp. 36 5. Mendel J.M. (976 Etension of Friedl's ias filtering technique to a class of nonlinear systems. IEEE ransactions Automatic Control AC-2 pp. 296 298 Shreve R.L. Hedric W.R. (974 Separating ias state estimates in recursive second-order filter. IEEE ransactions Automatic Control AC-9 pp. 585 586. itterton D.H. Weston J.L. (997 Strapdown inertial navigation technology. Peter Peregrinus United Kingdom.