Direct Kalman Filtering of GPS/INS for Aerospace Applications
Total Page:16
File Type:pdf, Size:1020Kb
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 sys- tem (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 consid- ered here for reference purposes, an indirect Kalman fil- ter 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 im- portant. 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 0 n 1 FORMULATION A21 A22 A23 Cb 0 n A = B A31 A32 A33 0 C C (13) B b C 3.1 Indirect Formulation B 0 0 0 0 0 C B C @ 0 0 0 0 0 A 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.