Estimation of Quaternion Motion for GPS-Based Attitude Determination Using the Extended Kalman Filter

Dah-Jing Jwo

Department of Communications,Navigation and Control Engineering,National Taiwan Ocean University,Keelung,202-24,Taiwan

Abstract:In this paper,the Global Positioning System (GPS) interferometer provides the preliminarily computed quaternions, which are then employed as the measurement of the extended Kalman filter (EKF) for the attitude determination system.The estimated quaternion elements from the EKF output with noticeably improved precision can be converted to the Euler angles for navigation applications.The aim of the study is twofold.Firstly, the GPS-based computed quaternion vector is utilized to avoid the singularity problem.Secondly, the quaternion estimator based on the EKF is adopted to improve the estimation accuracy.Determination of the unknown baseline vector between the antennas sits at the heart of GPS-based attitude determination.Although utilization of the carrier phase observables enables the relative positioning to achieve centimeter level accuracy, however, the quaternion elements derived from the GPS interferometer are inherently noisy.This is due to the fact that the baseline vectors estimated by the least-squares method are based on the raw double-differenced measurements.Construction of the transformation matrix is accessible according to the estimate of baseline vectors and thereafter the computed quaternion elements can be derived.Using the Euler angle method, the process becomes meaningless when the angles are at 90° where the singularity problem occurs.A good alternative is the quaternion approach,which possesses advantages over the equivalent Euler angle based transformation since they apply to all attitudes.Simulation results on the attitude estimation performance based on the proposed method will be presented and compared to the conventional method.The results presented in this paper elucidate the superiority of proposed algorithm.

Keywords: Global positioning system(GPS);quaternion;extended Kalman filter;attitude determination

1 Introduction

The Global Positioning System(GPS)[1-3]is a satellite-based navigation system that provides a user with the proper equipment access to useful and accurate positioning information anywhere on the globe.In addition to the code observable commonly used for position and velocity determination,the carrier phase is the other type of observable that can be extracted from the GPS signals.The carrier phase observables contain noise much smaller than that of code observables.Due to its higher accuracy and precision compared to code observations,carrier phase observations can be used for relative positioning in centimeter level and has been widely applied to surveying,attitude determination [4-14], precision approach and automatic landing.

Traditionally used in precise static relative positioning,and thereafter in kinematic positioning,the GPS offers the interferometer [5] for attitude determination by processing the carrier phase observables, which enables the relative positioning in centimeter level.The relative positioning techniques using the carrier phase differential GPS (DGPS) based on interferometer principles can be adopted to solve for the baseline vectors, defined as the vectors between the antenna, designated master and one of the slave antennas, shown as in Fig.1.The attitude of a vehicle can be precisely determined using the GPS carrier phase observables received at two or more nearby antennas attached to the vehicle.In the beginning of 1990s, Van Grass and Braasch [4] conducted research on the GPS to the field of aircraft attitude determination using carrier phase.In their work, the receiver-satellite double differenced observable was employed.The solution of the baseline vector is the approximate interferometer coordinates, which directly influence the performance of the GPS-based attitude determination.Real-time integer ambiguity resolution techniques [5,8,9,11] and attitude determination are two main issues to be resolved for determining the vehicle attitude when applying GPS double-differenced carrier phase.Very accurate relative position estimate will be available once the integer cycle ambiguities are properly resolved.Attitude determination using GPS does not have the error accumulation, which usually happens in the inertial navigation system (INS) [2].

Figure 1:Interferometric configuration considering the plane wave approximation of a GPS interferometer[5]

The rotation angles that relate a coordinate system fixed in the body(body frame)to a coordinate system fixed in space are referred to as the attitudes.The space coordinate system is typically defined to be a local level NED (north-east-down) frame, also referred to as the navigation frame.The purpose of attitude determination essentially involves calculation of the three Euler angles, namely roll, pitch, and yaw.The quaternion method[15-19] uses four parameters instead of nine as in the Euler angle method, by defining the generalized complex number.The quaternion method possesses advantages over the equivalent Euler angle based transformation since they apply to all attitudes with the error equations bounded by the constraint equation.Furthermore, the numerical value of each parameter always lies within the range of-1 to 1, so that the scaling problems in the computing mechanization can be easily handled.

The purpose of the Kalman filter(KF)[2,20,21]is to provide an optimal(minimum mean-square error)estimate of the system state vector.As the nonlinear version of KF,the extended Kalman filter(EKF)deals with the case governed by the nonlinear stochastic differential equations.The EKF linearizes about a trajectory that is continually updated with the state estimates.Both the KF and EKF have been widely applied to the field of navigation, such as GPS receiver position and velocity determination, attitude determination, integrated navigation system design, and the carrier-smoothed-code (CSC) processing.Utilization of the EKF as the estimator of attitude related parameters enhances the accuracy and reliability of the attitude solution.

The remainder of this paper is organized as follows.In Section 2,preliminary background on the GPS carrier phase observation model is reviewed; the computed quaternion vectors based on the GPS interferometer is introduced.Section 3 presents the modelling of quaternion dynamics for the extended Kalman filter.In Section 4, simulation experiments are carried out to evaluate the performance on estimation accuracy using the proposed method as compared to the conventional one.Two numerical examples are presented for illustration.Conclusions are given in Section 5.

2 GPS-Based Computed Quaternion Vectors Based on the GPS Interferometer

In a GPS interferometer, the receiver-satellite double-differenced carrier phase observable has been commonly utilized to solve for the antenna baseline vector.The GPS carrier phase observables representing sum of range,an unknown integer ambiguity and some ranging errors-can be represented by:

where the parameters involved in Eq.(1) are defined as follows.r:True range between a satellite and receiver;c:Speed of light;dt:offset of the satellite clock from GPS time;dT:Offset of the receiver clock from GPS time;dion:Ionospheric error;dtrop:Tropospheric error; λ:Carrier phase wavelength;N:Carrier phase integer ambiguity;vρ,vφ:Measurement noises of code and carrier phases,respectively.

2.1 Formulation of the Transformation Using the Baseline Vectors

The receiver-satellite double differencing operator is defined as∇Δ(·)=Δ∇(·)=∇(·)-Δ(·), where Δ(·)=(·)1-(·)2denotes the between receiver single differencing operator for receivers 1 and 2, and∇(·)=(·)i-(·)jdenotes the between satellites single differencing operator for satellitesiandj.Referring to the configuration as in Fig.2 [4,14], when using the carrier phase signal from satellitei, the between-receiver single-differenced observable is a linear combination of two phase observables received by two antennas

Figure 2:GPS interferometer for determining the baseline vector using the receiver-satellite doubledifferenced carrier phase observables[4,14]

where the effects of errors associated with the satellites are greatly reduced.Similarly,for satellitej,we have

where b is the baseline vector formed by two antennas, and e represents the line-of-sight unit vector from antennas to a satellite.Taking two independent single-differenced observables leads to the receiversatellite double-differenced observable:

which eliminates or greatly reduces the satellite and receiver timing errors.

The signals received fromnsatellites by one GPS interferometer providen-1 independent double differences.When the integer ambiguity parameteris resolved, the range-based equivalent of Eq.(4) is depicted as follows:

which can be expressed in matrix form∇Δr=Gb.The baseline vector can be obtained by the least-squares approachb=(GTG)-1GT∇Δr.The solution of the baseline vector b=[bx by bz]Tis the approximate interferometer coordinates, which directly influences the performance of the GPS-based attitude determination.

Referring to Fig.3, there are two body-frame-mounted non-collinear baseline vectors formed:b1=SA-M and b2=SB-M, where the master antenna (M) position is located at [0 0 0]Tin the body frame, while the other two slave antennas SAand SBare at [1 0 0]Tℓ and [cos γ sin γ 0]Tℓ,respectively.The parameter ℓ is the baseline length parameter used to adjust the length, and γ is the angles between two baseline vectors which are adjustable for design flexibility.The accuracy of the attitude measurement depends on the baseline to noise ratio, and is also a function of antenna placement and GPS satellite geometry.The coordinate transformation matrix from body to local framecan be formed once the baseline vector is determined through the following calculation.

2.2 Transformation Matrix Involving the Euler Angles

In certain applications,the body angular velocities:p,q,r(which can be measured,for example,by body mounted rate gyros) information is required.They can be determined from the Euler rates:˙ϕ, ˙θ, ˙ψ.The relationship between the body angular velocities and the Euler rates can be written as:

Figure 3:Antenna differential position vector geometry

Once the Euler angle rates are available with sufficiently good accuracy,the body angular velocity can be obtained.Taking the inverse transformation for Eq.(7), we have the Euler rates in terms of the body angular velocities:

The direction cosine matrix (DCM) Rb2nis employed as a transition matrix to describe the transformation of a vector quantity defined in the body frame (denoted by ‘b’) to the geodetic frame(denoted by ‘n’), fn=Rb2nfb.There are two common types of transformation approaches available for solving vehicle attitudes, typically including Euler angle method and quaternion method.The transformation matrix relatedbframe relative tonframe can be constructed in terms of the Euler’s angles or the quaternion parameters.

where the subscriptsnandbrepresent the local and body frames,respectively.Since Rb2nis an orthonormal matrix, its inverse can obtained through its transpose:

In Eq.(9),the notationsS(·)≡sin(·)andC(·)≡cos(·)are defined.Since the vehicle attitude is defined by the angles between the NED frame and body frame, therefore, the rotation transformation matrix that relates the body and NED frames provides the information for finding the vehicle attitude [2,4].The vehicle attitude can be obtained through the calculation:

Using the Euler angle method, the singularity problem occurs when the angles are at 90°, the process becomes meaningless.

2.3 Transformation Matrix Involving the Quaternion Elements

A quaternion is a four-dimensional extension to complex numbers,containing four real parameters.The first is considered a scalar and the other three vector components in three-dimensional space:

where a constant equation exists of the formq21+q22+q23+q24=1.The transformation matrix from body frame to navigation frame axes in terms of the unit quaternion parameters is given by

whereq1～q4are the quaternion vector components.The relations between the quaternion q and the two vectors fband fnare nonlinear.

Using the transformation matrix to obtain quaternion parameters can be done through

By comparing Eq.(13) with Eq.(9), conversion of the quaternion parameters to Euler angles can be implemented through the following relationships:

See [2] for more details on the quaternion method.The proposed algorithm for implementing the computation of the quaternion vector derived from the baseline vectors based on the GPS interferometer to be employed as the measurement of the EKF is provided in Fig.4.

The interferometer offers the preliminarily computed quaternion vector using the GPS doubledifferenced carrier phase observables.The implementation procedure is highlighted as the following steps:(1) Determining the baseline vector using the receiver-satellite double-differenced carrier phase observables:andfrom the GPS interferometer; (2) Construct the transformation matrix according to Eq.(6);(3)Compute the quaternion elements.

3 Modelling of the Quaternion Dynamics for the Extended Kalman Filter

Consider a dynamical system whose state is described by a nonlinear,vector differential equation.The process model and measurement model are represented as

Figure 4:Flow chart for computation of the quaternion elements using the GPS double-differenced carrier phase observables

It is assumed that f and h are known functions, u(t) and v(t) are two white-noise processes mutually independent with zero means and:

where δ(t-τ) is the Dirac delta function,E[·] represents expectation, and superscript “T” denotes matrix transpose.The nonlinear filtering deals with the case governed by the nonlinear stochastic difference equations.Assuming the process to be estimated and the associated measurement relationship may be written in the form:

where the state vector xk∈Rn, process noise vector wk∈Rn, measurement vector zk∈Rm, and measurement noise vector vk∈Rm.The vectors wkand vkare zero mean Gaussian white sequences having zero cross-correlation with each other:

where Qkis the process noise covariance matrix,Rkis the measurement noise covariance matrix.The symbol δikstands for the Kronecker delta function:

3.1 The Extended Kalman Filter

The nonlinear process model can be linearized along the currently estimated trajectory where the influence of the perturbations on the trajectory can be represented by a Taylor series expansion about the nominal trajectory.

The actual trajectory is the sum of the estimated trajectory and the small increment:x =^x+δx.The corresponding difference equation by converting the continuous-time model into a discrete-time model is given by

When working with incremental state variables, the linearized measurement equation presented to the EKF is δzk=zk-h(,k)=Hkδxk+vkrather than the total measurement (nonlinear) zk.Consider the incremental estimate update equation at stepk

in which the measurement residual is given by:zk-h(,k)-Hkδ,and the predictive measurement is the sum of h(,k) and Hkδ^x-k.The residual involves the noisy measurement minus the predictive measurement based on the corrected trajectory rather than the nominal one.

which shows that in an EKF it is accessible to keep track of the total estimates rather than the incremental ones.Onceis determined,the predictive measurementcan be formed as h(,k),and the measurement residual atk+1 is formed as the difference (zk-).Projection oftocan be done through the nonlinear dynamics=f(,k).Without the external aiding such as an inertial sensor to provide a reference trajectory, the process dynamics represent the total observer dynamics, as shown in Fig.5.Implementation algorithm for the discrete EKF equations is provided in Tab.1.

Figure 5:Configuration of the EKF

Table 1:Implementation algorithm for the discrete EKF equations

3.2 The Extended Kalman Filter for Quaternion Estimation

It can be shown that the quaternion elements are propagated according to the differential equation:

The differential equations for describing the propagation of quaternion elements are given by

where the product terms in the parentheses are introduced by quaternion product between the angular rate and the quaternion,which can be written in matrix form

Therefore, the quaternion vector is propagated according to the differential equation

where q=[q1q2q3q4]Tdenotes the quaternion vector,and=[p q r]Tdescribing the vector of body angular velocities.The symbol Ω()=(×) represents the skew symmetric matrix with components of ωnbin the body frame:

The differential equations for describing the quaternion vector can be represented by

where

and

Eq.(26) may also be written as

Augmented by the propagation of the body angular rates, described by the random walk models, i.e.,the differential equation has the form:The resulting state vector consists of seven states, in which the first four components are the quaternion elements, and the other three components are the body angular velocities.

The corresponding Jacobian matrix can be shown to be

The corresponding discrete state transition matrix is given by Φk=£-1[(sI-F)-1]=exp(FΔt)≅I+FΔt,where Δtis the step size.

Improved accuracy is accessible for the attitude solutions based on the EKF using the preliminarily computed quaternion vector from the GPS interferometer as the measurement.The measurement equations in this case are linear and are much simpler,and they are the four quaternion components

i.e.,zi=xi+vi,i=1...4, whereviis the white noise measurement.The measurement model written in matrix form is given by zk=Hkxk+vk, where vk=[v1v2v3v4]T.With the GPS-based computed quaternions available as the measurement,the measurement matrix Hkand noise vkcan be expressed as

and the covariance matrix for the measurement noise is given by

respectively.The measurement model Hkis a 4×7 matrix.Although the measurement equations are linear,an EKF is still required since the process model is nonlinear.The attitude estimation is implemented based on the block diagram shown as in Fig.6.Alternative models for body angular velocities in the process model.

Figure 6:Attitude estimation using the GPS-based computed quaternion

or the first-order Gauss-Markov process

4 Illustrative Examples and Performance Evaluation

Simulation study has been carried out to evaluate the performance of the proposed approach in comparison with the conventional method for GPS-based attitude determination.Two illustrative examples were implemented through numerical experiments.Computer codes were developed using the Matlab® software.The commercial software Satellite Navigation (SatNav) Toolbox by GPSoft LLC(2003) [22] was employed for generation of the GPS satellite orbits/positions and thereafter, the satellite pseudorange, and carrier phase observables, required for simulation.Furthermore, the Inertial Navigation System Toolbox (2007) [23] was employed for generation of the Euler attitude angles of the threedimensional flight.

The error sources corrupting the GPS carrier phase observables include ionospheric error,tropospheric delay,receiver thermal noise and multipath errors.The variances of the receiver noise are assumed to be 1 m2.Most of the receiver-independent common errors can be corrected through the differential correction while the multipath and receiver thermal noise cannot be eliminated.The antenna geometry is set up as in Fig.2,with the baseline length variable ℓ equal to 1 meter and γ 90 degrees.

(a)Illustrative Example 1-the three-dimensional test on a rotating platform with constant position.The time-varying body angular velocities for this simulation example are given as follows:

with initial Euler attitude angles ϕ0=θ0=ψ0=0.The Euler angles can be derived from the body angular velocities by processing integration for Eq.(8)with the initial conditions as time progresses.The update rate of the measurements is 1 Hz.The time intervaldt=0.1 sec, initial state vector x=[1 0 0 0 0 0 0]T.The noise covariance matrix for this example is set as

wherem=4 is the number of measurements and equals the number of quaternion elements.After resolving the integer ambiguity, the least-squares approach is utilized for constructing the baseline vector.Fig.7 presents the body angular ratesp,qandrfor Example 1.There were 8 visible satellites available in the clear open sky during the time period of simulation.Fig.8 provides the skyplot at the final time.

Figure 7:Body angular rates p,q and r for Example 1

The proposed method in which Kalman filtering is used provides estimate of the quaternion vector with noticeable accuracy improvement.It should be noticed that it is risky to set the process noise variance zero to avoid filter divergence.As a result, when the system reaches steady state, the steady-state gain will not approach zero and, subsequently, the filter is able to catch the time-varying attitude dynamics.One still needs to find the suitable values to meet the specific design/mission requirement.The estimation accuracy of the quaternion vectors are essential and directly influence the resulting accuracy of Euler attitude angles.To confirm the correctness of the solutions, the estimated quaternion vectors were examined, as shown in Fig.9.Comparison of the attitude solutions is shown in Fig.10; Tab.2 summarizes the error statistic of attitude solutions for both the conventional and proposed methods.It can be seen that estimation accuracy by the proposed method has been remarkably improved.

Figure 8:The skyplot at the final time for Example 1

(b)Illustrative Example 2-the three-dimensionalflight.The scenario for the second example is a threedimensional flight.The 3D flight path is generated with the Inertial Navigation System toolbox,shown as in Fig.11.Tab.3 provides the description of the motion for the flight path.The body angular velocities as time progresses for this example are depicted in Fig.12.In this example, the duration for simulation is 110 seconds with 8 visible satellites.The covariance matrices for the process and noise models,respectively,are given by

Fig.13 presents the estimate of the quaternion components for Example 2.Fig.14 shows the estimate of Euler attitude angles and the corresponding errors;error statistics for the attitude solutions are summarized in Tab.4.In the two illustrative examples,the improvement by the proposed method has been demonstrated.The results for the two examples demonstrate the effectiveness against the noisy measurement errors.Incorporation of the EKF into the attitude determination system for estimating the quaternions elucidates the superiority of the proposed algorithm.

Figure 9:Estimate of the quaternion components for Example 1

The standard EKF is sensitive to the changes of the process and measurement models,thus yielding poor performance.Furthermore, the EKF framework does not possess capability to deal with the non-Gaussian measurement errors/outliers.The EKF associates the contaminated measurements with an increase in the measurement covariance, causing the reformulated error covariance.This modified covariance inflation is known to cause an increase of error in the state estimation.Performance based on the EKF will degrade when the noise strength is varying and/or the actual distribution deviates from the assumed Gaussian model.To further improve the performance of the EKF, an adaptive mechanism or a robust technique can be incorporated for performance improvement.Due to appropriate tuning, the adaptive EKF (AEKF)exhibits robust behavior and therefore outperforms the standard EKF when the time-varying dynamic process and measurement models are involved.

Figure 10:Estimate of attitude angles and the corresponding errors for Example 1

Table 2:Error statistics for the attitude solutions-Example 1

Figure 11:The 3D flight path

Table 3:Description of the motion for the three-dimensional flight path(initial position:[0,0,0 meters]in NED frame)

Figure 12:Body angular velocities p,q and r for Example 2

Table 4:Error statistics for the attitude solutions-Example 2

Figure 13:Estimate of the quaternion components for Example 2

Figure 14:Estimate of attitude angles and the corresponding errors for Example 2

5 Conclusions

A novel GPS-based attitude determination method has been presented.The quaternion vector derived from GPS interferometer has been used as the measurement of the EKF in the attitude determination system.Although utilization of the carrier phase observables enables the relative positioning to achieve centimeter level accuracy, nevertheless, the quaternion elements derived from the GPS interferometer are inherently noisy.This is essentially due to the fact that the baseline vectors estimated by the least-squares method are based on the noisy double-differenced measurements.

The preliminarily computed quaternion elements from the GPS interferometer is employed as the measurement of the EKF based quaternion estimator.The model based approach using the EKF is adopted for estimating the quaternion elements, which can then be converted to the Euler angles.Results show that, by incorporating the EKF into the GPS interferometer, the errors in the solutions of the baseline vectors, and thereafter the quaternion elements have been remarkably mitigated and the estimation accuracy of the attitude solutions has been noticeably improved.The proposed method provides several advantages, such as accuracy improvement, reliability enhancement, and real-time characteristics.

Since the EKF is sensitive to the changes of the process and measurement models,when implementing the EKF approach,poor knowledge of the noise statistics may seriously degrade the estimation performance,and even provoke the filter divergence.For achieving improved estimation accuracy, the designers are required to possess the completea prioriknowledge on both the dynamic process and measurement models.In the two illustrated examples, both the process and measurement noise parameters remained unchanged based on the stationary noise assumption.In the cases of high dynamic or multipath contaminated environments, the noise parameters in the two models need to be -properly tuned.Further investigation can be carried out using the AEKF as the noise-adaptive filter for tuning the noise covariance matrices in the high dynamic or multipath environments.

Funding Statement:This work has been partially supported by the Ministry of Science and Technology of the Republic of China[Grant No.MOST 108-2221-E-019-013].

Conflicts of Interest:The author declares that he has no conflicts of interest to report regarding the present study.