A Direct Kalman Filtering Approach for GPWINS Integration Honghui Qi, John B. Moore, IEEE Fellow Honghui Qi is CRC for Robust and Adaptive Systems, Research School of Information Science and Engineering, The Australian National University, Canberra, ACT 0200, Australia. Tel: (61 2) 62798611,Fax:(612)

62798615. Email: honghui.qi @syseng.anu.edu.au

John Moore is with Department of System Engineering, Research School of Information Science and Engineering, The Australian National University, Canberra, ACT 0200, Australia. Tel: (612) 62798687, Fax:(612) 62798688. Email: john.moore@syseng.

anu.edu.au

Abstract: In this paper we present a direct Kalman filtering approach for GPS/INS integration. In the approach, GPS and INS non–linearities are preprocessed prior to a Kalman filter. The GPS preprocessed data are taken as measurement input, while the INS preprocessed data are taken as an additional information to the Kalman filter’s state prediction. The advantage of this approach is that a simple and linear Kalman filter can be implemented to achieve significant computation saving with little sacrifice in performance.

1 Introduction The global positioning systern(GPS) is a worldwide radio navigation system and has applications in aviation, aircraft automatic approach and landing, land vehicle navigation and tracking, marine applications, and surveying, etc. [ 1]–[2]. A GPS receiver is a low frequency response navigation sensor and can provide an instantaneous position accuracy in the order of 15 to 100 metres normally at 1Hz rate. Inertial navigation systems (INS) are one of the most widel y used dead reckoning systems. They can provide continuous position, velocity and also orientation estimates which are accurate for a short term, but are subject to drift due to sensor drifts. The integration of GPS and INS can limit shortcomings of the individual systems namely, the typically low rate of GPS measurements as well as the long term drift characteristics of INS. Integration can also combine the advantages of the two systems, such as, the uniform high accuracy trajectory information of GPS and the short term stability of INS. There are several methods to integrate INS and GPS, such as, loosely coupled or tightly coupled integration, closed–loop

or open–loop integration, separated INS and GPS unit or embedded

GPS with INS hardware, etc. [6]–[9]. In all these designs, GPS or the GPS/INS integration filter is typically some form of a Kalman filter. Usually an indirect (extended) Kalman filter with inertial errors as its state is used. A high order filter is required to achieve near optimal performance. The computation load associated is very heavy, since on–line Kalman gains have to be calculated. A fixed Kalman gain is often used to decrease the computational

load but with sacrifice of per-

formance. In this paper, we present a direct Kalman filter integration approach in order to eliminate the drawbacks as discuss above. Here, the so called direct Kalman filter is a filter with the vehicle’s position, velocity among its states. We aim to design a low order, linear Kalman filter to achieve simple computations but with little sacrifice in performance. The optimal GPS filtering methodology in [3] is applied here to preprocess the non–linearity prior to the Kalman filter. The same methodology is also applied here to the nonlinear INS system, but this is not so straightforward since the non–linearity

for INS is dynamic rather than simple static as for GPS.

2

The paper is organised as follows. A direct Kalman filter integration approach is given in Section 2. Simulation results are shown in Section 3. Finally, a conclusion is made in Section 4.

2 A Direct Kalman Filtering Approach for GPWINS Integration The diagram of the proposed GPWINS integration is shown in Figure 2.1. The functions of the blocks are as follows. Dynamical INS equations: The navigation equations convert the IMU measured accelerations and angular velocity, along with feedback position and velocity estimates, to INS estimated accelerations in an earth centre earth fixed (ECEF) co–ordinate frame and to attitude estimates. The IMU and navigation equations together are referred to as the INS unit. Algebraic GPS equations: As in [10], the algebraic GPS equations give position, velocity and clock error estimates by solving the nonlinear GPS pseudorange and delta range equations. The GPS front end and algebraic GPS equations are referred to as the GPS unit. Datafusiorz Kalman filter(direct): This filter obtains estimates of position, velocity, GPS receiver clock errors, as well as bias and drift in the INS estimated accelerations, based on the information data from both the INS and GPS units. Its estimates are fed back to the INS and GPS units as required. The essential feature of the proposed integration is to implement all the various nonlinear operations prior to linear Kalman filtering and to put as much of the necessary dynamics as possible into the Kalman filter. Recall that the Kalman filter is the optimal minimum variance filter for a known linear stochastic model with zero mean Gaussian noise of known covariance. Should the model be linear but the noise be non Gaussian; then the Kalman filter is the best linear minimum variance filter. Of course, the more sophisticated extended Kalman filtering or optimal nonlinear filtering will improve performance, although perhaps not always significantly, but will cost an order of magnitude or more in computational The GPS non–linearities

are pre–processed

effort.

in the block denoted ‘Algebraic GPS equations’ at

the GPS sample rate. The non–linearities of the INS are pre–processed in the block denoted ‘Dy -

3

namical INS equations’. These involve an integration and so are dynamical and not algebraic; they calculate an orthogonal co-ordinate transformation matrix. The outputs from the INS block are estimates of the vehicular attitude, and estimates of accelerations in the ECEF frame, denoted by superscript e here. The INS sampling rate is typically an order of magnitude or so faster than the GPS rate. The acceleration estimates from the INS unit are fed forward to the data fusion Kalman filter, along with the GPS estimates.

position and velocity feedback ,---------------------1 , accelerations 1 > , 1 IMU I 1 I I # t INS unit b------------------------

attitude * : accelerations in e frame I t 1 1

-------

----- ----- ----- -------, ,----- ----, Algebraic : GPS rx pseudorange GPS ; front-end ‘ equations 1 delta range I I A GPS unit ,---- ------------------------!

I I

I : D ~ PVT ~with noise I

Data fusion Kalman filter(direct~

estimation of position, velocity * GPS receiver clock errors

velocity feedback FIGURE 2.1 –Diagram of GPS/lNS integration via direct Kalman filter approach

2.1 Dynamical INS Equations The computations performed by a strapdown navigator may be regarded as comprising two major parts: propagation of the attitude reference, and solution of the navigation equations. The former uses the gyro outputs to calculate the attitude of the body coordinate frame with respect to a reference coordinate frame. The latter uses this relationship to transform the coordinates of vectors (which may include accelerometer outputs, velocity, gravity effects, earth rotation) between the frames and hence to calculate acceleration of the body. In our proposed GPS/INS integration method, consequent velocity and position calculations frequently carried out in an INS unit are carried out in the data fusion Kalman filter. The coordinate frame in which computation

is performed, is usually chosen such that it agrees

4

with the coordinate frame for the output. Here we choose the ECEF frame, so as to obtain directly the geocentric Cartesian coordinates which in turn are convenient for integration with GPS information. The continuous–time

nonlinear dynamical equations in the ECEF frame are of the form[6]

(2.1)

where re, Ve are the position and velocity vectors in the ECEF frame (e–frame), @ the specific force vector in the body frame (b-frame), ge the gravity vector in the e–frame and is rc dependent, R: is the transformation matrix from the b–frame to the e–frame, so that ~ = R~fb is the specific force vector in the e–frame, Q:’ is the skew–symmetric

matrix of the angular velocityvector U~b

of the b–frame with respect to the e–frame coordinated in the b–frame, and fi?~eis the skew–symmetric matrix of the earth’s rotation rate o;, which is known precisely. The skew–-symmetric matrices are of the form

The various vectors are dependent on time t and the super dot denotes derivatives with respect

tot. The angular velocity vector co~~can be obtained by ~b

= eb

~b – ]b

R:we ,e,

(2.3)

with R: = R~T

where u: is the gyro measured angular velocities with respective to the inertial frame coordinated in the body frame. After solving (2.1), a rotation matrix, R:, from the body frame to the navigation frame (n–frame) can then be obtained (2.4)

R; = R~R:

where R; is a rotation matrix from the e–frame to the n–frame, which is position rc dependent. The rotation matrix R: is composed of three successive rotations with angles called yaw, pitch and roll, which are the attitude of a vehicle. Therefore the attitude of the vehicle can be obtained through the rotation matrix R; [9].

5

2.1.1 Discrete Time Navigation Equations The INS unit seeks to implement (2.1) in discrete time at the INS sampling rate with period 8T. This rate is chosen so that the discrete–time equations follow closely the continuous–time

equa-

tions (2.1). To this end, let us first construct a sampled version of (2.1) with state matrix R:(l) and output Ve(l), driven by S2~~(l), C?;e(l),@(l), Ve(l), ge(re(l)): R:(1 + 1) = R~(l)exp(Q~b(l)bT)

(2.5)

v’(l) = R:(l)~(/)

(2.6)

– 2f2;e(l)ve(l) + ge(rc(l))

Since (2.5) holds only when ~~~(1) is constant during the period of 6T from 1to 1+1, the accuracy of R: is dependent on the sample period 8T. The smaller the 8T, the more accurate the R: calculation. Now exp(Cl~~(l)8T) can be approximated using a (1, 1) Pad& approximation,

denoted with

a superbar as

exp(Q~$08T)

= (2I + f2~~(l)?iT)(21 – Q~~(l)5T) -1

which, as is readily verified, preserves orthogonality.

Higher accuracy approximations

~ ~8T,~2 and applying the (1, 1) Pad& approximations. made by working with ~exp ( ~e~(l)

(2.7) can be

(Actual-

ly, it is straightforward to show that (n,m) Pad& approximations preserve orthogonality with n=m, but otherwise do not.) The INS unit implements an approximation to (2.6) using accelerometer measurements of accelerations P(l) which typically are noisy with bias and drift: likewise, for the gyro rotation rates Wb and thus for w~b. Also in the INS unit, the position and velocity vectors re, Veare estimated lb’ from the data fusion Kalman filter which introduces further errors (noise). Using superhats to denote estimates or measurements, then the navigation equations are implemented in the INS unit as

R;(1+ .e

1) = ~e,

(2.8)

6

Recalling that there are drifts and biases in the measurements from gyros and accelerometers, we expect that these in turn lead to biases and drifts in the estimates of the e–frame acceleration Ve. The details are as follows.

2.2 Bias and Drift in INS The solution matrix R;(t) of (2.1) is an orthogonal matrix, and C?:b is skew–symmetric.

It can

be represented in the form of R;(t) = R~(0)exp(S2~~t)

(2.9)

((2.9) holds only when fl~~ is constant.) Let angular velocity errors on w~bcaused by gyro drifts be Aw~~, leading to drifts AC?~~in C?~~and AR~(t) in R:(t). Then (2.9) can be rewritten as (2.10)

R:(t) + AR~(t) = R~(0)exp((C?~~ + AC?~~)t) = R;(t) + R~(0)Af2~bt + H.O.T where H.O.T denotes higher order terms. Let accelerometer

(R;(t)+ R;(OAQ’&)(fb := F+

bias be Af’. Recalling

+ Afb) = ~ + R;(t)Afb + R;(o)A~~b(fb

that

+ Afb)t (2.11)

A~+A~t

From (2. 11), it can be concluded that gyro drift and accelerometer bias cause bias Afi and drift A% on specific forces in thee frame, where Afi : = R~(t)Afb

(2.12)

AfTj: = R;(0)Af2~b(fb + Afb)

(2.13)

From (2.8), we can see that Afi, A~ are the bias and drift, respectively, of the e–frame acceleration Ve, which are also caused by gyro drift and accelerometer bias. Actually the errors on the e–frame acceleration are also dependent on e–frame velocity errors and position errors which are introduced by the Kalman filter.

2.3 Data Fusion Kalman Filter Design Let the state space model for the design of the data fusion Kalman filter be

~= [

(’%)qT

xTbxTbA~T ( J

7

(2.14)

where x is the GPS receiver’s position coordinates in the ECEF frame, b is the GPS receiver’s clock range bias. Consider a discrete time signal model for data fusion, operating at a fast sample rate with sampling period 6T, as (2.15)

&I+ 1 = A&I + BIU, + Wi

with l=O, 1,2, . . .. We assume first that the fast sample rate is equal to the INS sample rate and is N times the GPS rate, where N is some integer, typically between 10 and 200. Thus the GPS sample period AT is N?iT. Here the model known exogenous inputs u 1are the e–frame acceleration estimates from the INS unit via u, = Ve,and o 4x3 B,

=

13x@T

(2.16)

o

[1 7x3 The unknown inputs WIrepresent the estimation errors in the accelerations (including estimation errors in biases and drifts in accelerations), and other modelling errors. These are assumed to be zero mean and with known or estimated covariance matrix Q for Kalman filter design proposes. The transition matrix, ~ has the form

Al=

All A12 o A

[1

(2.17)

22

with

All =

10008TO O1OOO6TOO OO1OOO8TO OOO1OOO8T 00001000’

O

000000

0

000

0

0

000000 000000 4TOOAT200 O AT2 O OATO O AT; 00 ATO 000 0 0 0

A~2 = I

00000100 00000010 00000001

0

J

(2.18)

(2.19)

A ,2 =

A~2, if 1 = mN o ~X6, otherwise ‘

A 22=

A~2, ifl=mN otherwise

()~ x ~,

(2.20)

{’ for integers m=O, 1,2, ..., where c 1,C2 s 1 are forgetting rates for the biases and drift rate, respectively. Actually the GPS clock bias and drift can be processed at the GPS rate. The measurement equation for the data fusion Kalman filter model can be represented by

8

z, = c~g,+

(2.21)

v,

where for integers m (as above)

[ 18X8

CT = {

08X6

o

1forl=mN

(2.22)

otherwise

Here Z, are the computed position, velocity and clock errors from the algebraic GPS equations, and the covariance matrix IQ of the measurement noise V1is covariance matrix of the solution from the algebraic GPS equation[3][ 10]. The measurement equation is identical to that for the GPS Kalman filter in [3] at times O, N, 2N, . . .. and is zero otherwise. For the data fusion signal model (2. 15) – (2.21), the standard Kalman filter algorithm can be applied. Three cases are now considered. Case ]. Identical GPS and INS sampling rates (N= 1) Even if the GPS and INS sampling rates are identical (N= 1), the data fusion Kalman filter is somewhat more sophisticated than the GPS Kalman filter of [3]. This is by virtue of the external “inputs” U, = Ve from the INS unit and the bias and drift state estimates and associated (Kalman) gain terms. In this case the data fusion filter will, in general, be time–varying.

There is a time–

varying Riccati error covariance and Kalman gain, to take account of initial transients and slow variations in the noise covariance. However, such a filter can be quite accurately approximated in our context (performance wise) by a time–invariant filter using limiting Riccati equation solutions working with typical noise covariance. Of course, the Kalman gain can also be gain–scheduled depending on some of the variables. The crucial design variables for the linear filter are then the forgetting rates c1, C2and the noise variances Q, ~. Selection of these variable will benefit from knowledge of the sensor noise, bias, and drift characteristics.

Initialisation

e,+, =b %//-1

Prediction

=

‘fJ

(2.23)

&/1-l= A/-lL//-l+B/-lU/-l P 1/1– 1 = %-lb/H

AT1–1 + Q1-l

9

(2.24)

Update

(2.25) Case 2. Integer ratios of GPS and INS sampling rates (N=O, 1, 2, . ..) In this case, the Cl matrix is periodic with its period N being the ratios of the sampling rates, assumed here to be an integer (typicall y between 10 and 100). The associated Riccati error covariance matrix has a consequent periodic component, as then has the Kalman gains. However, such a filter be quite accurately approximated (performance wise) by a filter with precisely periodic Kalman gains, perhaps even with the gains to the bias and drift states being constant and the gains to the position and velocity and GPS clock drift states being obtained by Kalman filter algorithm when 1=0,N, 2N, . . . and zero otherwise. In the follow, equations used for Kalman filter with fixed gains to the bias and drift states are shown.

Initialisation

Prediction

‘3)

i,+,

=

P/-l//-l

= Po

(2.26)

ii/l-l=%-il+l +B1-1ul-1

(2.27)

l//-l ATII + q,_l

P[//- ] = AllP/-

–1

~1 = P//[- Icl C?PI/l - 1c+R, 1 [

P[/[ = [1 – wT]P///i,,, =

Update

t,/[_ ] +

L

Z[

1

1

-

cR/1-

[

k, K, =

{[

(2.28)

1

(kfix)b ~ ~

o

1

I

,ifl=rnN

, otherwise

where p, k, c are covariance matrix, Kalman gain, observation matrix, respectively, of the first 8 states, such as, position, velocity and GPS clock error states. IBX8 for 1 = mN

c, = {

o

otherwise

(2.29)

‘

10

q is the covariance matrix of the process noise vector W1associated with states of position, velocity and GPS clock errors and kfix is the Kalman gain matrix of the 6 bias and drift states, it is constant, being the form of

kfix = [L

‘3x3

g113x3

‘3x2

‘3x3

g213x3

‘3x2

1

(2.30)

J

where g], g2 are constant values, typically less than 1.0, which are again the design parameters of the Kalman filter. Case 3. Non synchronised GPS and INS sampling This case can in principle be handled within the context of Kalman filtering theory, see for exanlple [5], but for our purposes since N is typical large, there can be approximate round off to the nearest integer value and the results used as for Case 2 above. We do not explain this further here. Remarks Using the Kalman filter for an assumed linear stochastic model (2. 15) – (2.22), achieves optimality in a minimum error covariance sense when the noise is zero mean, white and Gaussian, and is the best linear filter when the noise is non–Gaussian. Loss of optimality, perhaps to a negligible degree occurs in practice since noise errors maybe correlated, non Gaussian, or the model may also have a degree of error due to finite filter sampling rates and other approximations.

Even so,

our conjecture is that the extra benefit of using extended Kalman filtering or other nonlinear optimal methods may not be worth the extra computational effort which would be an order of magnitude or so increase. The GPS receiver clock error states can be excluded from the Kalman filter to achieve a low dimensional filter with only a marginally degraded performance.

3 Implementation

and Simulation Results

A trajectory used for simulation is assumed as follows. An airplane’s initial position is at latitude of 35 degrees south, longitude of 150 west and height of 1000 metres, the initial velocities are 700 Krn/hr, 200m/hr, – 100m/hr along the three axes of the navigation frame, i.e., north, east and vertical down respectively. It is accelerated along the north, east and down with the acceleration of 6Km/hr2, 6Krn/hr2, –6Km/hr2’ respectively. The airplane’s initial orientation is assumed to

11

be parallel to the navigation frame, i.e. Odegree of yaw, pitch and roll and then rotated with 0.005 degrees per second for yaw, pitch and roll. The simulation period is one hour. In the implementation

of the new proposed 14 state Kalman filter, the algorithm (2.26) - (2.28)

is used, therefore fixed Kalman gains for the bias and drift Afi, A~ are employed. The forgetting rates c 1,C2are set to be 0.99, and the fixed Kalman gains, gl, g2 are set to be 0.008. The predication rate is set to be 32 Hz, the measurement update rate is set to be the GPS sample rate, 1Hz. Therefore 8T=0.03 125 seconds, AT= 1 seconds and N=32. For comparison, two other integration options are presented. The first integration option, called GPS resetting, also the simplest from the implementation

viewpoint, is resetting the INS–derived

position and velocity with GPS–derived position and velocity. Here the GPS receiver employs a Kalman filter as the one discussed in [3]. However, the prediction equations are implemented at a fast sample rate, such as, 32 Hz here, while the update equations are implemented at GPS sample rate, 1 Hz. The second option is the tightly coupled integration as discussed in [9]. Their simulation results are shown in Table 1. Table 1: Mean and variance of estimated position errors for the three design options of GPS/INS.

IMU statistics: 3 deg/hr gyro drift, 0.025m/s2 accelerometer bias

I

GPS measurement noise variance pseudorange: 30.0 m, pseudorange rate: 50crn/s

GPS measurement noise variance pseudorange: 3.0 m, pseudorange rate: 5cm/s

position errors

latitude

longitude

height

latitude

longitude

height

GPS resetting

mean (m)

–5.310

–7.722

0.852

–1.003

–1.388

0.172

variance (m2)

28.175

117.932

31.615

0.715

2.489

0.847

proposed new method

mean (m)

-4.504

-3.373

0.761

-0.936

-1.572

0.137

variance (m2)

27.373

102.576

29.721

0.695

2.530

0.844

Tightly coupled integration

mean (m)

–0.80834

1.317

0.069

1.313

1.361

-0.255

variance (m2)

25.785

102.163

29.906

0.4

1.043

0.6445

From Table 1, it can be seen that the proposed integration method gives better performance than the GPS resetting approach, since the former take INS calculated accelerations as known inputs and acceleration biases and drifts as Kalman filter states. Certainly the proposed method makes

12

use of the information from INS. However, it gives worse performance than the tightly coupled integration method, since the errors on the calculated accelerations are modelled by the bias and drift states Afi, At in an approximated and simple way as discussed in Section 2.2, while a linearized differential equation of the acceleration errors are modelled in the tightly coupled Kalman filter. The advantage of the proposed method as compared with the tightly coupled method is that off–line Kalman gain computation can be implemented in the proposed method so that on– line computation is low.

4 Conclusions In this paper we have presented a direct Kalman filtering approach for GPS/INS integration. In the approach, GPS and INS non–linearities

are preprocessed prior to a Kalman filter. The GPS

preprocessed data are taken as measurement input, the INS preprocessed data are taken as an additional information to the Kalman filter’s state prediction. The advantage of this approach is that a simple and linear Kalman filter can be implemented to achieve significant computation saving with little loss in performance. References [1]

E.D. Kaplan, “Understanding

[2]

B. W. Parkinson, J. J. Spilker Jr., P. Axelrad, P. Enge, “Global Positioning Theory and Applications”

GPS, principles and applications”,

Artech House, 1996. System:

Volume I and H, AIAA, 1996.

[3]

H.Qi, J.B. Moore,’’Towards

optimal GPS Filtering” submitted to publication.

[4]

Anderson B. D. O,, and J.B. Moore, “Optimal Filtering”,

Prentice–Hall,

Englewood

Cliffs, NJ, 1979. , Prentice Hall, Australia, 1991.

[5]

D. Williamson, “Digital Control and Implementation”

[6]

M. Wei, and K.P. Schwarz, “A Discussion of Models for GPS/INS Integration”,

In Y.

Bock and N. Leppard, editors, Global Positioning Systems: An Overview, pp316 – 327, Spring–Verlag,

1989.

13

[7]

K. P.Schwarz, M. Wei, and M.V. Gelderen, “Aided versus embedded: a Comparison of two approaches to GPS/INS Integration”, In Proceedings of IEEE PLANS 94, pp314 -321.

[8]

H.Qi, J.B. Moore, “A Simple and Near Optimal Filtering approach for GPS/INS Integration”,

To be submitted for publication.

[9]

H. Qi, M.E. Evans, “Integration of GPS/INS”, DSTO, J9505/1 4/8, 1997.

[10]

H.Qi, J.B. Moore, “A direct solution to GPS pseudorange IEEE Trans. AES.

14

equations”

submitted to