Direct Kalman Filtering of GPS/INS for Aerospace Applications

J. Wendel, C. Schlaile, Gert F. Trommer

University of Karlsruhe, Germany [email protected]

BIOGRAPHY This contribution investigates the influence of these two different approaches of Kalman filtering on the overall Jan Wendel received the Dipl.-Ing. (M.S.E.E.) from the system performance of a loosely coupled GPS/INS sys- Technical University of Karlsruhe in 1998. He is cur- tem for aerospace applications. Both filter formulations rently a Ph.D. candidate at the University of Karlsruhe, were implemented and compared via Monte Carlo sim- where his research interests are focused on data fusion ulation runs with focus on the accuracy of the estimated algorithms in integrated navigation systems. inertial sensor biases and on GPS drop out situations.

Christian Schlaile received the Dipl.-Ing. (M.S.E.E.) We found a comparable performance of both navigation from the Technical University of Karlsruhe in 2000. algorithms concerning attitude and position errors as He is currently a Ph.D. candidate at the University of well as inertial sensor bias estimation as long as GPS Karlsruhe, where his research interests are focused on aiding was available. However, the simulation results data fusion algorithms in automotive driver assistant indicate a superior performance of the direct Kalman systems. filter formulation in GPS drop out situations, which can by explained by the way the inertial measurements are Gert F. Trommer received the Dipl.-Ing. (M.S.E.E.) and processed. the Dr.-Ing. (Ph.D.) degrees from the TU Munich, Ger- many in 1978 and 1982, respectively. He joined EADS/LFK, formerly DaimlerChrysler Aerospace, where he was project manager for the IMU 1 INTRODUCTION development in the UK ASRAAM program. He held the directorate of ’Flight Control Systems’ with An integrated navigation system exploits the complemen- the responsibility for the Navigation, Guidance and Con- tary characteristics of different navigation sensors to in- trol Subsystem, the IR Seeker and the Fin Actuators in the crease the precision of the navigation solution. In addi- German/Swedish Taurus KEPD 350 program. tion, the resulting redundancy allows the detection of bad Since 1999 he is professor and director of the ’Institute sensor data, which then can be rejected. Such an integrity of Systems Optimization’ at the University of Karlsruhe monitoring increases the reliability of the navigation so- with research focus on automotive driver assistant lution significantly. The navigation technique common systems and INS/GPS/TRN-aided integrated navigation to almost all integrated navigation systems is the iner- systems. tial navigation. In a strapdown inertial navigation system (INS), an inertial measurement unit (IMU) mounted to the vehicle senses accelerations and angular rates for all six degrees of freedom of the vehicle. From these data, a strapdown algorithm (SDA) can compute a navigation so- ABSTRACT lution presumed that initial position, velocity and attitude are known. This type of navigation is autonomous, there- In integrated navigation systems Kalman filters are fore intentional or unintentional external jamming is im- widely used to increase the accuracy and reliability of the possible. Unfortunately, only short-term accuracy of the navigation solution. navigation solution is assured as the measurement errors of the inertial sensors sum up. Therefore, further navi- Usually, an indirect Kalman filter formulation is applied gation sensors such as global positioning system (GPS), to estimate the errors of an INS strapdown algorithm terrain reference navigation (TRN) by baro-altimeter and (SDA), which are used to correct the SDA. In contrast radar-altimeter, and image based navigation (IBN) by IR to that, in the direct Kalman filter formulation total seekers are used to aid the INS and to assure the long- quantities like position, velocity and attitude are among term accuracy of the navigation solution. The data fusion the state variables of the filter, which allows them to be of these different sensors is commonly accomplished by estimated directly. a Kalman filter [1],[2]. The performance of the Kalman filter is crucial to overall system performance, especially when low-cost sensors are integrated. This paper focusses equations[6]: on filtering GPS and IMU data. In the classical way of aiding the INS, which is considered here for reference purposes, an indirect Kalman filter formulation is chosen. The Kalman filter estimates the ∂~q 1 b b n n = ~q 0, ~ωib Cn [~ωie + ~ωen] (1) errors of the SDA state vector, which can be corrected ∂t 2 ∗ − n subsequently. The development of this design took place ∂~ve n ~ b n n n n = Cb fib (2~ωie + ~ωen) ~ve + ~gl (2) when due to limited computational ressources, the com- ∂t − × ∂L vn putational efficiency of the algorithm was of superior im- = e,north (3) portance [3]. In a direct Kalman filter formulation, total ∂t Rn + h quantities like position and velocity are estimated. As it ∂λ vn = e,east (4) is considered to be computationally more demanding, this ∂t (Re + h) cos(L) design was commonly used only for alignment and cali- ∂h n bration tasks and applications in which only slow dynam- = ve,down (5) ∂t − ics were involved [4]. However, as CPU clock speeds are increasing, the computational cost of the algorithms used in an integrated navigation system is becoming less important. Herein denotes This paper describes in detail both approaches to design Kalman filters for the data fusion task in integrated navi- ~q attitude quaternion gation systems. In the next section, the navigation equa- quaternion multiplication ∗ n tions are introduced. In Section three, indirect and direct Cb Direction cosine matrix that transforms a Kalman filter formulations are described. In Section four, vector from its b-frame component form an alternative direct formulation with a computational ef- to its n-frame component form, computed ficiency comparable to the indirect formulation is derived. from ~q. ~ b In Section five, simulation results are given. Finally, con- fib specific force acceleration b clusions are drawn. ~ωib angular rate of the b-frame with respect to a nonrotating inertial frame (i-frame), given in b-frame component form n ~ωie angular rate of a coordinate frame with one 2 THE NAVIGATION EQUATIONS axis parallel to the Earth’s polar axis and the other axes fixed to the Earth (e-frame) The navigation equations are important for both the SDA with respect to the i-frame, given in n- and the Kalman filter. In the SDA, these differential equa- frame component form n tions are integrated to keep track of position, velocity and ~ωen angular rate of the n-frame with respect to attitude of the vehicle. For the indirect Kalman filter for- the e-frame, given in n-frame component mulation, the navigation equations are used to derive the form n error propagation equations by means of a Taylor series ~ve velocity in north, east, and down direction expansion which is truncated after the linear part. The with respect to the Earth, given in n-frame error propagation equations represent the system model component form according to which the indirect Kalman filter is designed. L, λ, h lattitude, longitude, height For the direct Kalman filter, the navigation equations rep- Rn,Re Earth’s meridian, and transverse radius of resent the nonlinear system the filter has to observe. The curvature. n investigations presented in this paper were carried out us- ~gl local gravity vector in n-frame component ing the navigation equations in a navigation frame (n- form frame) mechanisation. The axes of the n-frame are given by the directions north, east and down. The down axis is The IMU and the SDA which integrates (1)-(5) using the parallel to the local gravity vector, which is the sum of the currently available angular rate and specific force mea- gravitational acceleration and the centripetal acceleration surements form the INS. Without any further aiding, the caused by the rotation of the earth. The n-frame as well error in the computed position provided by the INS grows as the body-fixed coordinate frame (b-frame) have their with second or even third order of time, respectively. In origins at the location of the navigation system. The axes this contribution only loosly coupled systems are consid- of the b-frame are aligned with the roll, pitch and yaw ered: it is assumed that the long-term accuracy of the axes of the vehicle. In the SDA, the attitude information navigation solution is assured by aiding the INS with the was represented using a quaternion [5] in order to avoid position information provided by a GPS receiver. The singularities that can occur when Euler angles are used. data fusion of INS and GPS can be accomplished using The continuous form strapdown inertial navigation equa- different Kalman filter formulations, which are described tions are given by following set of nonlinear differential in the next section. 3 INDIRECT AND DIRECT KALMAN FILTER A11 I 0 0 0  n  FORMULATION A21 A22 A23 Cb 0 n A =  A31 A32 A33 0 C  (13)  b  3.1 Indirect Formulation  0 0 0 0 0     0 0 0 0 0  For the indirect Kalman filter formulation in which the errors of the SDA are estimated, the error propagation Herein I denotes the 3 3 unity matrix and 0 denotes equations are needed. The equations for the position and a 3 3 matrix containing× only zeros. Finally, the mea- velocity errors follow from (2)-(5). These nonlinear equa- surement× model which is needed to process the position tions can be written in the following form: information provided by a GPS receiver is given by ∂~x = f~(~x) (6) ∂t ∆~zk = H∆~xk + ~νk (14) Expanding Eq. (6) into a Taylor series and neglecting where higher order terms leads to 1 0 0 ... 0 ~   ∂~x ~ ∂f(~x) H = 0 1 0 ... 0 (15) f(~x) ~x=~xSDA + ~x=~xSDA (~x ~xSDA) (7) ∂t ≈ | ∂~x | · −  0 0 1 ... 0  where ~xSDA denotes the state of the SDA. Rearranging Eq. (7) gives the error propagation equations and ~νk is a zero-mean, white gaussian noise sequence. When GPS data is available at timestep k, the errors of ∂∆~x ∂f~(~x) the SDA can be estimated by applying = ∆~x (8) ∂t ∂~x ~x=~xSDA | + ∆~x = ∆~x − Kk(H∆~x − ∆~zk) (16) where k k − k − where ∆~x = ~x ~xSDA. (9) − The attitude error propagation equations cannot be de- (LSDA LGPS)(Rn + hSDA)  −  rived from (1) directly. As the attitude errors are con- ∆~zk = (λSDA λGPS)(Re + hSDA) cos(LSDA) . − sidered to be small, they are commonly described using  hSDA hGPS  − Euler angles. It is shown in [7] that the attitude errors (17) propagate according to Kk denotes the Kalman gain matrix at timestep k, the su- ∂Ψ n n b n perscripts and + distinguish quantities before and af- = ~ωin Ψ Cb δ~ωib + δ~ωin (10) ∂t − × − ter the measurement− is processed, respectively. The sub- where Ψ is a vector containing the attitude errors in form scripts SDA and GPS distinguish quantities provided by n of Euler angles, ~ωin is the angular rate of the n-frame with the SDA and the GPS receiver, respectively. With the es- respect to an inertial frame given in n-frame component timated errors ∆~x, the state of the SDA is corrected. In b n the indirect Kalman filter formulation described here, the form, δ~ωib are the angular rate sensor biases and δ~ωin are the errors in the n-frame rate estimates. time-consuming Kalman filter estimation step is only ap- The linear system model, of which the discrete equiva- plied when GPS information is available. Accelerometer lent was used in the indirect Kalman filter formulation, is and angular rate sensor data enter the SDA directly after given by adding a zero-mean white gaussian noise pro- a correction by means of the estimated sensor biases. The cess ~w multiplied with an appropriate input matrix G to noise of these sensors is therefore treated incorrectly as the position, velocity and attitude error propagation equa- system noise. A simplified block diagram of the indirect tions. This model was augmented by six additional states Kalman filter formulation is shown in Fig. 1. to allow the estimation of time-constant or slowly varying angular rate sensor and accelerometer biases. The struc- KALMAN GAIN GPS ture of the resulting fifteen-state linear system model can be seen from Eqs. (11) - (13). ∆a ∆v ∆x ∂∆~x RR = A∆~x + G~w (11) ∂t a + ∆a aˆ vˆ xˆ ACC pos. RR

∆~xned 0 0 velo.    n  ∆~vned Cb 0 accl. n ~wAcc ∆~x =  Ψ  , G~w =  0 Cb   ~ b    ~wGyro  δfib   0 0  Figure 1: Simplified block diagram of the indirect  b     δ~ωib   0 0  Kalman filter formulation (12) 3.2 Direct Formulation indirect Kalman filter formulation described previously, this computation is required only when a GPS measure- In the direct Kalman filter formulation, total quantities ment is available. Here the Kalman gain matrix has to like position, velocity and attitude are estimated directly, be computed additionally when accelerometer and angu- as these quantities are among the state variables of the lar rate sensor measurements are available, which occur filter. The nonlinear system model, according to which at a high rate. A simplified block diagram of the direct the filter is designed, is given by the navigation Eqs. (1)- Kalman filter formulation is shown in Fig. 2. (5). Furthermore, six states were added for the accelera- ~ b b tions fib and the angular rates ~ωib which were modeled as random constants. The model is completed by six states, GPS ACC which are needed to estimate the inertial sensor biases, and an appropriate input matrix G multiplied with a zero- KALMAN GAIN mean, white gaussian noise process ~w to take into account model inadequacies, see Eq. (18) aˆ vˆ xˆ pos. ∂~x RR = f~(~x) + G~w (18) ∂t velo. The required linear system model is obtained by lineariz- accl. ing about the estimated state vector ~xˆ: Figure 2: Simplified block diagram of the direct Kalman ∂f~(~x) filter formulation F = ˆ (19) ∂~x |~x=~x The structure of the resulting twenty-two-state linear sys- 4 AN ALTERNATIVE DIRECT KALMAN FILTER tem model can be seen from Eqs. (20)-(22). FORMULATION ∂~x = F~x + G~w (20) In order to avoid the increased computational cost caused ∂t by the processing of the inertial sensor data in the Kalman filter estimation step as in the algorithm described above, L, λ, h 0 0 an alternative direct Kalman filter formulation was de-     ~vned 0 0 rived. The idea is to discard the state variables reserved ~ b b ~ b  fib   I 0  for the angular rates ~ωib and accelerations fib. Instead,  b    ~w1 ~x =  ωib  , G~w =  0 I  (21) the inertial sensor measurements are treated as known in-     ~w2  ~q   0 0  put vector ~u. This leads to the sixteen-state system model  ~ b   0 0  given by Eqs. (23)-(26).  δfib     b   0 0   δ~ωib    ∂~x = F~x + G~u + G~w (23) ∂t F11 F12 0 0 0 0 0  n  F21 F22 Cb 0 F25 0 0  0 0 0 0 0 0 0  L, λ, h 0 0   F =  0 0 0 0 0 0 0  (22)  ~v   Cn 0    ned b  F51 F52 0 F54 F55 0 0  ~x =  ~q  ,G =  0 F54  (24)        0 0 0 0 0 0 0  δf~ b 0 0    ib    0 0 0 0 0 0 0  b       δ~ωib   0 0  Again, I denotes the 3 3 unity matrix and 0 denotes × 3 3 , 4 3, and 3 4 matrices containing only zeros. ~ b × × × fib,measured ! ~wAcc The measurement models which are needed to process the ~u = b , ~w = (25) ~ω ~wGyro GPS receiver, angular rate sensor, and accelerometer data ib,measured are straight forward, although an appropriate scaling of the GPS measurement matrix is necessary to assure the F11 F12 0 0 0 numerical stability of the algorithm. In opposite to the  n  F21 F22 F25 Cb 0 indirect Kalman filter formulation, all sensor data is pro- − F =  F51 F52 F55 0 F54  (26)  −  cessed in the estimation step of the filter. Therefore, all  0 0 0 0 0    sensor noise is modeled correctly as measurement noise.  0 0 0 0 0  The inherent disadvantage of this filter algorithm is its increased computational cost. This is due to the fact that the The indexing of the submatrices in Eq. (26) is taken Kalman gain matrix has to be computed more frequently, from Eq. (22), so that identical submatrixes receive the which involves a time consuming matrix inversion. In the same index in both equations. The measurement model 25 Table 1: Characteristics of the sensors used for simulation Indirect Formulation Direct Formulation 20 source cycle time noise Alt. Direct Formulation

angular rate sensors 5 ms 0.1 ◦/√h 15 accelerometers 5 ms 0.05 mg/√Hz 10 GPS receiver 1 s 10 m Position Error (m) 5 required to process the GPS receiver data is straightfor- 0 ward and nearly identical to the direct Kalman filter for- 0 200 400 600 800 1000 1200 Time (s) mulation described before. This algorithm offers a computational cost comparable to the indirect Kalman filter formulation. Unfortunately, the inertial sensor noise is Figure 4: Position errors obtained with different Kalman treated incorrectly as system noise, too. A simplified filter formulations. (The red graph is covered almost com- block diagram of this alternative direct Kalman filter for- pletely by the blue one.) mulation is shown in Fig. 3.

2.5 GPS Indirect Formulation 2 Direct Formulation Alt. Direct Formulation KALMAN GAIN 1.5

aˆ vˆ xˆ ACC pos. 1 RR

velo. Velocity Error (m/s) 0.5 accl. 0 0 200 400 600 800 1000 1200 Figure 3: Simpliﬁed block diagram of the alternative di- Time (s) rect Kalman ﬁlter formulation

Figure 5: Velocity errors obtained with different Kalman filter formulations. (The red graph is covered almost com- 5 SIMULATION RESULTS pletely by the blue one.) The aim of the numerical simulations was to compare the performance of the different Kalman filter formulations described in the previous sections during a representative Figures 4 to 6 show the averaged position, velocity and twenty-minute mission flight. Hypothetical inertial sensor attitude errors of the different filters obtained by Monte and GPS receiver data was generated by corrupting the Carlo simulation runs. The GPS outage is marked yel- ideal values with white noise according to Table 1. In low. Although there are significant structural differences addition, constant accelerometer and angular rate sensor between the alternative direct and the indirect Kalman fil- biases were assumed. At the 900 second point into the ter formulations shown in blue and red, respectively, the mission, a GPS outage lasting one minute was assumed. plots indicate an identical performance of these two fil- As the initial values of the covariance matrix of the ters. During the time when GPS-aiding is available, the Kalman filter state have a significant influence on filter direct formulation shows a performance comparable to performance especially in the first few minutes after start the indirect and alternative direct formulation. However, of the mission, all corresponding values were initialized in the case of GPS loss, the errors in position, velocity and identically: The variances of the initial attitude errors of attitude grow slower for the direct formulation. This ad- the indirect Kalman filter formulation given in terms of vantage results from the processing of the inertial sensor Euler angles were used to compute the initial variances data in the estimation step of the Kalman filter. of the quaternion coefficients of the two direct Kalman Based on appropriate system models, the Kalman filters filter formulations. In the same way, the variances of lat- are able to estimate the biases of the inertial sensors. Fig- titude and longitude were initialized from the variances ures 7 and 8 show the biases estimated by the different of the errors in north and east directions. With this pro- filter formulations. The true biases that were added to the cedure, the initial values of the state covariance matrix of noisy inertial meassurements are shown in black. Again, the sixteen-state direct formulation were specified com- the performance of the indirect and the alternative di- pletely. For the twenty-two-state direct formulation, some rect formulation is identical and comparable to the per- degrees of freedom persisted. formance of the direct formulation. 6 CONCLUSION 3.5 In this paper, the design of different Kalman filters for 3 the datafusion in integrated navigation systems was 2.5 described. Simulation results indicate an identical per-

2 formance of the widely used indirect formulation where the errors of the SDA are estimated, and the alternative 1.5 direct formulation where total quantities like position, 1 velocity and attitude are estimated directly. The direct

Attitude Error (mrad) Indirect Formulation formulation, which processes the inertial sensor data in 0.5 Direct Formulation Alt. Direct Formulation the estimation step of the ﬁlter, offers better results in 0 GPS drop out situations at the expense of an increased 0 200 400 600 800 1000 1200 Time (s) computational load.

Further work will be carried out to assess the robustness Figure 6: Attitude errors obtained with different Kalman of these algorithms concerning unknown correlations and ﬁlter formulations. (The red graph is covered almost com- variances of the inertial sensor noise in the case of strong pletely by the blue one.) mechanical missile vibrations [8] as well as to extend the present systems to tightly coupled GPS/INS systems with additional aiding sensors.

1.5 REFERENCES

1 [1] Hutchinson, C. E.: The Kalman Filter Applied to Aerospace and Electronic Systems. IEEE Transac- 0.5 tions on Aerospace and Electronic Systems, Vol.20, No.4, July 1984, pp.500-504. 0 Indirect Formulation Direct Formulation [2] Gelb, A.(Editor): Applied Optimal Estimation. The Alt. Direct Formulation -0.5 M.I.T. Press - Fourteenth Printing, Cambridge, Mas- Accelerometer Biases (mg) sachusetts, and London, England 1996. -1 0 200 400 600 800 1000 1200 Time (s) [3] Wagner, J., and Kasties, G.: Aspects of Combining Satellite Navigation and Low-Cost Inertial Sensors Symposium Gyro Technology, Stuttgart,Germany , Figure 7: Estimation of accelerometer biases obtained 1996. with different Kalman ﬁlter formulations. (The red graphs [4] Maybeck, P. S.: Stochastic Models, Estimation and are covered almost completely by the blue ones.) Control, Volume 1. Academic Press, New York, 1979. [5] Gupta, S.: Linear Quaternion Equations with Ap- 4 plication to Spacecraft Attitude Propagation IEEE h)

/ 3 Aerospace Conference, Vol.1, 1998, pp.69-76. ◦ 2 [6] Titterton, D.H., and Weston, J.L.: Strapdown Iner- 1 tial Navigation Technology. Peter Peregrinus Ltd., 0 Indirect Formulation on behalf of the Institution of Electrical Engineers, Direct Formulation London, England 1997. -1 Alt. Direct Formulation -2 [7] Britting, K.R.: Inertial Navigation System Analysis -3 New York: Wiley-Intersciences, 1971. Angular Rate Sensor Biases ( -4 0 200 400 600 800 1000 1200 [8] Wendel, J., and Trommer, G.J.: Impact of Mechan- Time (s) ical Vibrations on the Performance of Integrated Navigation Systems and on Optimal IMU Speciﬁca- tion. accepted for publishing at The ION 57th An- Figure 8: Estimation of angular rate sensor biases ob- nual Meeting & CIGTF’s Guidance Test Sympo- tained with different Kalman ﬁlter formulations. (The red sium June 11-13, Albuquerque, New Mexico, USA, graphs are covered almost completely by the blue ones.) 2001.