Kinematic Attitude Determination from GPS Derived Accelerations and a Tri-Axial Accelerometer Cameron Ellum and Naser El-Sheimy Department of Geomatics Engineering The University of Calgary, Canada
[email protected]
BIOGRAPHIES
1
Cameron Ellum is currently completing an M.Sc. in Geomatics Engineering at the University of Calgary. He received his B.Sc. from the same department. His research interests are close-range photogrammetry, multi-sensor integration, and programming for geomatics applications.
For many applications the navigation solution has three components: position, velocity, and orientation. Since the advent of GPS, the former two components are available both accurately and inexpensively. Unfortunately, the cost of technologies for orientation determination remains relatively high. Typical technologies include multi-antenna GPS systems and gyroscopes. The former technology requires specialised and expensive receivers. Furthermore, if it is to be accurate then it also requires large inter-antenna distances that limit its application areas. The latter technology – gyroscopes – suffer from undesirable long-term error characteristics. Only expensive gyroscopes offer the long-term accuracy necessary for most mobile-mapping applications. If a substitute can be found for these or similarly expensive technologies, then the cost of orientation determination can be correspondingly reduced.
Dr. Naser El-Sheimy is an Assistant Professor at the Department of Geomatics Engineering of the University of Calgary. His area of expertise is the integration of GPS/INS/Imaging sensors for mapping and GIS applications with special emphasis on the use of multisensors in Mobile Mapping Systems. He is now the chairman of the special study group for Mobile MultiSensor Systems of the International Association of Geodesy (IAG) and the chairman of The International Federation of Surveyors (FIG) working group C5.3 on “Kinematic and Integrated Positioning Systems”. ABSTRACT This paper describes an inexpensive kinematic attitude determination system that uses GPS and a triaxial accelerometer. By removing the GPS-derived accelerations from the specific forces sensed by the accelerometers, the gravity vector in body frame of the vehicle can be determined. From this, roll and pitch can be calculated. Azimuth is provided by the GPSmeasured trajectory. Details are given on the system components and configuration, and on the kinematic attitude determination algorithm. A preliminary test of the system shows that the RMS error is within 1◦ . A technique to derive Taylor series finite-difference differentiators of any order is also given, and such differentiators are examined in the frequency domain. From this analysis, it is shown that direct differentiators are preferable to cascaded differentiators unless the suppression of high frequencies is desired.
INTRODUCTION
This paper will present a system for determining orientation using only GPS and a triad of micro-electromechanical (MEMs) based accelerometers. The accelerometer triad is much cheaper than any competing technology, and therefore its use significantly reduces the cost of attitude determination. The goal of the system is to provide attitude accuracies in kinematic mode that are comparable to those obtained from the tri-axial accelerometer in static mode – namely, less than one degree in roll and pitch. These accuracies are sufficient for many close-range mobilemapping applications. The system also has the potential to determine orientation in real-time. Briefly, the concept of the system is as follows: Differentiation of the GPS positions provides both vehicle velocities and accelerations. The accelerations derived from the GPS can be subtracted from the specific forces measured by the accelerometers, and the result is the gravity vector. From this, roll and pitch can be
180
determined. Azimuth is provided by the GPS-derived velocities. This paper is divided into two parts. The first part, which includes sections 2 and 3, gives a derivation and analysis of Taylor series differentiators. Specifically, section 2 provides a derivation – based upon Jacobson (1998) – of a method to easily determine differentiators of any degree, while section 3 analyses the performance of such differentiators in the frequency domain. The second part of the paper deals with the implementation of the system. In particular, section 4 details the system configuration and attitude determination algorithm and section 5 contains results from some preliminary testing. 2
q−1
dm f (t) X ≈ hk f t + (r + k)∆t m dt
r ≤ 0.
(1)
k=0
In Equation (1), k are the q consecutive epochs relative to t starting at r. The nth order Taylor series expansion of the same function at any epoch k can be expressed as n X 1 i f t + (r + k)∆t ≈ [(r + k)∆t] f (i) (t). (2) i! i=0
Substituting Equation (2) into Equation (1) yields
k=0
( n ) X1 i (i) [(r + k)∆t] f (t) . (3) i! i=0
If we set Bi =
q−1 X k=0
1 i hk [(r + k)∆t] , i!
i = 0, · · · , n ,
(4)
then Equation (3) can be written as n
dm f (t) X ≈ Bi f (i) (t). dtm i=0
hk f (t + (r + k)∆t) =
n X
Bi f (i) (t).
(6)
i=0
k=0
Equation (4) represents a system of equations that – after multiplying the left hand side by the factorial in right-hand side – can be expressed in matrix form as y = Ax.
(7)
where y = 0!B0
···
1!B1 h1
n!Bn T hq−1 ,
···
T
,
and
The general equation for a finite difference approximation of a function’s derivative at time t can be expressed as
q−1
q−1 X
x = h0
FINITE-DIFFERENCE DIFFERENTIATORS
dm f (t) X ≈ hk dtm
Therefore,
(5)
A=
(r+q−1)∆t
.. .
... ... .. .
(r+1)n ∆t
...
(r+q−1)n ∆tn
1
1
r∆t
(r+1)∆t
.. . r n ∆tn
1
.. .
.
From the above, it is evident that for a system that uses a Taylor series expansion of order n to be uniquely determined, q − 1 = n or q = n + 1. From Equation (5) it can also be seen that for the mth derivative Bm = 1 and Bi = 0, i 6= m. Thus, the vector y has the value (0 1 0 · · · 0)T and (0 0 2 0 · · · 0)T for the first and second derivatives respectively. Making the appropriate substitutions for q, r, and Bi into Equation 7 allows for the unknown coefficients hk of the finite difference approximation to be solved for. For low-order systems, the system of linear equations in Equation 7 can be solved using a standard technique such as LU decomposition by Crout’s method or Gauss-Jordan elimination. However, A is very poorly conditioned, and thus coefficients determined in this manner will be inaccurate for higher-order systems. Fortunately, A is a Vandermonde matrix, and there are special techniques to help solve systems of equations with matrices of this form – see Press et al. (1992) for an example. Finite-difference derivatives can be classified depending on which epoch the expansion of the Taylor series is made at. If the expansion is made in the centre of the q epochs used in the approximation,
181
the differentiator is called a central-difference differentiator. If the expansion is made at the first or last epoch, the differentiator is called a forward-difference or backward-difference differentiator respectively. For example, the second-order forward, central, and backward difference approximations to a first derivative are as follows: df (t) −3f (t) + 4f (t + ∆t) − f (t + 2∆t) ≈ (8a) dt 2∆t df (t) f (t + ∆t) − f (t − ∆t) ≈ (8b) dt 2∆t f (t − 2∆t) − 4f (t + ∆t) + 3f (t) df (t) ≈ . (8c) dt 2∆t An additional example is given in the appendix. Central and forward difference differentiators require data from epochs in the future and are therefore non-causal. They can, however, be made causal simply by shifting the series so that the epoch at which the derivative is calculated lies sometime in the past. For example, the central difference approximation of Equation (8b) would become df (t − ∆t) f (t) − f (t − 2∆t) ≈ (9) dt 2∆t Obviously, this shift means that there is a time delay between the measurement and the calculation of the derivative. However, as will be shown below, a time delay in real-time is unavoidable, even for backward difference differentiators. Central-difference approximations of derivatives have been widely used in the GPS community to both derive a doppler measurement from the carrier phase observable and to determine velocities or accelerations from positions – examples of their use include Cannon et al. (1997) and Bruton et al. (1999). The order of these approximations are sometimes referred to by the number of samples they use on one side of the point of expansion. For example, the second-order approximation of Equation (8b) may be called a “first-order” approximation. However, the order of these approximations are more correctly determined by the order of the Taylor series expansion and hence the terminology adopted herein. 3
FREQUENCY DOMAIN CHARACTERISTICS
A finite difference differentiator applied to GPS positions is equivalent to a finite impulse response (FIR)
filter, and just like any filter it has two effects on a signal. The first is a scaling of the signal’s amplitude, and the second is a shift of the signal’s phase. The former effect is termed the magnitude response of the system, and the latter is termed the phase response. Together, both describe the system’s frequency response. By examining the frequency response of such systems – as was done in Bruton et al. (1999) – the performance of different finite-difference differentiators can be compared. To determine the magnitude and phase responses of any discrete-time system, it is first necessary to determine the system’s transfer function. The transfer function, indicated by H(z), is the Z-transform of the system’s impulse response h(t). For finite-difference differentiators, it can be easily shown that the elements of the impulse response vector are equivalent to the coefficients of the differentiator. Once the transfer function has been determined, the magnitude response of the system is given by M (ω) = H ejω∆t . (10) and the phase response by Θ(ω) = 6 H ejω∆t ,
(11) √
In Equations (10) and (11), j = −1, ω is the angular frequency of the sinusoid (= 2πf ), and ∆t is the sampling interval. A more intuitive description of system’s phase response is its group delay, which can be interpreted as the frequency-dependent time delay. Mathematically, the group delay is defined as τg (ω) = −
d Θ(ω). dω
(12)
From Equation (12) it is obvious that a system with a linear phase-response will have a constant group delay – i.e., all frequencies will be delayed by the same amount. 3.1 First Derivatives For the first derivatives, the basis for comparison will be an ideal differentiator, whose transfer function is (Antoniou, 1993) H ejω∆t = jω (13) for 0 ≤ ω <
ωs 1 ⇒ 0 ≤ f < , 2 2∆t
(14)
182
where ws is the sampling frequency. The restriction in Equation 14 arises as a consequence of sampling theory, which states that the maximum resolvable frequency in a signal will be half the sampling rate. This is an obvious but important consideration, as it means that one of the factors limiting the accuracy of the kinematic attitude determination system is the data rate of the GPS. The magnitude responses for central-difference first derivative approximations of order 2 through 10 are shown in Figure 1. From this figure, it can be seen that derivatives of higher order more closely approximate the ideal differentiator. It can also be seen that all first derivative approximations greater than second-order amplify the medium-to-high frequencies – at least to a certain point. After this point, the higher frequencies begin to be suppressed. At the Nyquist frequency – i.e., half the sampling interval – there is complete suppression. If the higher frequencies are primarily noise, then by suppressing them a low order approximation may give better performance than a higherorder approximation. However, if, as is likely the case, the higher frequencies contain information, then their suppression is undesirable.
systems are always linear, and the group delays are always constant (Oppenheim et al., 1999). Thus, in the central difference approximations all frequencies pass without delay, and the comparison of such differentiators can be based purely on their magnitude responses. Unfortunately, for the backward and forwarddifference differentiators, only the first-order derivative approximations are asymmetric. Higher order approximations are neither symmetric nor asymmetric. Consequently, as shown in Figure 2 their phase responses are not linear, and equivalently their group delays are not constant. This is clearly an undesirable characteristics, as different frequencies will be affected differently by the differentiator. These nonconstant delays mean that the first-order approximation is the only appropriate backward-difference differentiator. However, even the first order approximation has a delay equivalent to half the sampling interval.
7 n=5 n=4 n=3 n=2 n=1
6
5 n = 10 n=8 n=6 n=4 n=2
|H(ej2π f∆ t)|
2.36
Group Delay (s)
3.14
Ideal Differentiator
4
3
2 1.57 1
0 0
0.79
0.1
0.2
0.3
0.4
0.5
frequency (Hz)
0.00 0
0.1
0.2
0.3
0.4
0.5
Frequency (Hz)
Figure 2: Group Delay of Backwards First Differentiators (∆t = 1)
Figure 1: Magnitude Response of Central-Difference First Derivatives (∆t = 1)
Although it will not be shown here, it can easily be shown that any central-difference derivative approximation centered at t = 0 (i.e., non-causal) will have a constant phase response of π/2 and a corresponding zero group delay. This is a special case of the phase response for any FIR system with a symmetric or asymmetric impulse response – the phase response of such
The magnitude response of the first order backwards (and forwards) difference differentiator is compared with the magnitude responses of the second and tenth order central difference differentiators in Figure 3. It is apparent that the backward-difference differentiator retains much more high frequency information than either central difference approximation, even if it does not follow the ideal differentiator as closely at lower frequencies.
183
9.87
3.14 10th Order Central Difference 2nd Order Central Difference 1st Order Backward Difference
n = 10 n=8 n=6 n=4 n=2
7.40
2.36
Ideal Differentiator
|H(ej2π f∆ t)|
|H(ej2π f∆ t)|
Ideal Differentiator
1.57
4.93
2.47
0.79
0.00
0.00 0
0.1
0.2
0.3
0.4
0
0.5
0.1
Error
0.2
0.3
0.4
0.5
Frequency (Hz)
0.00
Figure 4: Magnitude Response of Direct Second Derivatives (∆t = 1)
−0.39
−0.79 0.1
0.2
0.3
0.4
0.5
Frequency (Hz)
Figure 3: Magnitude Response of Backwards-Difference First Derivative (∆t = 1)
3.2
Second Derivatives
For the second derivatives, the transfer function of the ideal differentiator is H ejω∆t = −ω 2 (15) When the magnitude responses of the centraldifference second derivative approximations, which are shown in Figure 4, are examined, the same observations as made for the first derivatives apply for the second derivatives. Namely, the higher the order of the derivative approximation, the better it approximates the ideal differentiator. However, unlike the first derivatives the second derivatives do not start to suppress the higher frequencies. The second derivatives also differ from the first derivatives in that they do not all have the same magnitude response at the Nyquist frequency. In addition to direct differentiation, it is also possible to determine a second derivative by twice applying a first derivative. A derivative determined in this manner will be referred to as a cascaded derivative because the signal or function is essentially cascaded through two systems. Intuitively, the cascaded differentiators should perform worse than their direct counterparts as each differentiation will attenuate the higher frequencies. This effect is visible in
Figure 5, where it can be seen that in the medium-tohigh frequencies the cascaded differentiators perform significantly worse than the direct differentiators. For this reason, the cascaded differentiators should not be used unless the signal being differentiated has very little high-frequency information, or if suppression of the high frequencies is desired. Cascaded differentiators are additionally disadvantaged in that they have a greater delay than direct differentiators, and require a longer initial period before they can begin to operate. 9.87 4th order th 10 order
7.40
|H(ej2π f∆ t)|
0
Direct Differentiators
4.93
2.47
Cascaded Differentiators 0.00 0
0.1
0.2
0.3
0.4
0.5
Frequency (Hz)
Figure 5: Comparison of Magnitude Responses of Direct and Cascaded Second Derivatives (∆t = 1)
As a final note, all second-derivative backwarddifference differentiators have non-asymmetric impulse responses. Therefore, such systems have undesirable phase responses, and the only appropriate im-
184
plementation of a backward-difference approximation is the application of two first-order differentiators. 4
KINEMATIC ATTITUDE DETERMINATION
In the following, greater detail will be given on the algorithm used to determine orientation and the components used in the system. 4.1
System Configuration
The primary components used in the prototype attitude determination system are a GPS receiver, a triaxial accelerometer, and a logging computer. For the GPS, a Novatel Millennium RT2 receiver was used. The Novatel is advantageous for inclusion in multisensor systems because of its pass-through logging capability. It is also relatively easy to write custom software to control the receiver and log data from it. Leica’s Digital Magnetic Compass (DMC) was used for the accelerometer triad. The DMC combines a triad of microelectromechanical (MEMs) accelerometers with a triad of magnetic sensors – Table 1 highlights some of the specifications of the device. By making measurements of the earth’s magnetic field, the magnetic sensors in the DMC can be used to determine azimuth. However, such measurements are very sensitive to disturbances in the local magnetic field and consequently their installation in vehicles is not straightforward. Because of this, the measurements from the magnetic sensors were not used in this research. Also, the inclusion of magnetic sensors in the system would increase its cost. In addition to the research presented here, the DMC has found use in mobile-mapping/close-range photogrammetry (Ellum and El-Sheimy, 2001) and personal navigation (Ladetto et al., 2001). The system components are connected as shown in Figure 6. In this arrangement, the GPS receiver is the core of the system and is responsible for both making its own measurements, and for time-tagging the DMC’s measurements. It would also be possible to have both the GPS and the DMC connected to a computer; however, this would require either complex (and less-accurate) software timing or an expensive timing board. Passing DMC’s measurement’s through the GPS receiver eliminates these requirements. Pass-through logging also simplifies communication requirements, as the logging computer only requires a single (serial) communication port.
It is worth noting that the accelerometer triad is small enough that it can be mounted essentially coincident with the GPS antenna. Thus the effect of a lever arm between the GPS and the accelerometer triad can be neglected. Table 1: Leica DMC Specifications
Static Angle Accuracies Azimuth 0.5 ◦ (2σ) Pitch 0.15 ◦ (2σ for tilt +/- 30◦ ) Roll 0.15 ◦ (2σ for tilt +/- 30◦ ) Measurement Rate Standard 30 Hz (up to 150Hz in raw data mode) Optional 60 Hz Physical Parameters Weight Less than 28 grams Dimensions 31.0 mm × 33.0 mm × 13.5 mm Other RS232 Serial Interface. Max. Baud. 38,4000 Internal soft-hardmagnetic compensation procedures
4.2
Acceleration Determination from GPS Observables
There are several techniques to determine GPS velocities and accelerations. These techniques can be broadly divided into two categories: techniques in the measurement domain which use the raw GPS observable, and techniques in the navigation domain which use derived navigation quantities. Perhaps the most commonly used technique is the differentiation of positions. In this technique, which occurs purely in the navigation domain, positions are either directly differentiated to determine accelerations, or twice differentiated to determine velocities and then accelerations. Also in the navigation domain it is possible to use the raw observed doppler to determine velocity, and then to differentiate that velocity to determine acceleration. Unfortunately, the raw doppler is typically very noisy, and velocities determined in this manner are correspondingly noisy. Because differentiation increases noise, the accelerations will be even noisier. To reduce the noise, several investigations have been made where a doppler was derived by differentiation of the carrier phase measurement. The velocities are then determined in the same manner as those from the raw doppler. Because the derived Doppler is less noisy then the raw doppler, so too are the corresponding velocities (Cannon et al., 1997; Bruton et al., 1999). This technique lies in between the measurement and navi-
185
Digital Compass / Inclinometer
GPS Antenna
§Measures specific forces
Logging Computer §Records continuous measurements of position and specific forces
GPS Receiver §Measures positions §Time-tags specific forces
Figure 6: System Layout
gation domains, as both the raw observables and the derived navigation quantities are differentiated. Finally, purely in the measurement domain it is possible to determine accelerations directly from GPS carrier phase measurements (Jekeli and Garcia, 1997). However, airborne gravity research at the University of Calgary has shown that this technique is difficult to implement in practice because of problems with receiver clock corrections and cycle slips. As shown in Table 2, all methods of determining acceleration from GPS except for one have a minimum group delay of 1 epoch. The exception is the differentiation of velocities derived from the raw Doppler. In this case, the velocities are available at the same time as the measurements are made, and the accelerations can be calculated using a first-order backwards differentiator which has a group delay of half an epoch. In Bruton et al. (1999), accelerations resulting from the differentiation of positions were compared with accelerations resulting from the differentiation of velocities determined from the derived Doppler. It was concluded that for high dynamics use of the derived doppler was best and for low dynamics differentiation of positions was best. Unfortunately, the differentiator used on the positions was a cascaded differentiator, and it has already been shown in section 3 that such a differentiator performs much worse in the high frequencies than a direct differentiator. A direct differentiator would almost certainly provide better results for high dynamics and consequently the conclusions
Table 2: Methods of Acceleration Determination from GPS measurements Method Direct differentiation of positions Cascaded differentiation of positions Differentiation of velocities derived from raw doppler measurements Differentiation of velocities derived from derived doppler measurements Direct differentiation of carrier phases Cascaded differentiation of carrier phases
Domain
Minimum Delay
navigation navigation
1 epoch 1 epoch
navigation
0.5 epochs
measurement / navigation
1 epoch
measurement
1 epoch
measurement
1 epoch
of that paper may not necessarily hold true. Also, the differentiation of positions is obviously much easier to implement than differentiation to determine a derived Doppler. For these reasons, in the algorithm and tests presented below differentiation was performed in the navigation domain using the positions. Finally, it should also be noted that much research has been performed in the airborne gravity field on the optimal determination of accelerations from GPS – see, for example, Bruton (2000). Unfortunately, in airborne gravimetry the frequencies of interest mostly lie below 0.01 Hz. Hence, tests and conclusions for acceleration determination are normally made using data that has been heavily filtered. In this applica-
186
tion, much useful information exists above these frequencies and consequently conclusions from airborne gravimetry do not necessarily apply.
enables Equation (19) to be written as
4.3
Or, using the rotation matrix between the body and geodetic co-ordinate frames, g(t)b = R(t)be ¨r(t)e + 2 Ωeie r˙ (t)e − f (t)b (22)
Algorithm
In a purely inertial frame, acceleration gravitation, and force can be related by Newton’s second law. For a known proof mass this law can be expressed as (Schwarz and Wei, 2000), ¯ (t)i . f (t)i = ¨r(t)i − g
(16)
¯ are the vector of acceleraIn Equation (16), ¨r and g tion and gravitation respectively. The vector of specific forces – indicated by f – can be measured by an accelerometer triad. However, an accelerometer triad makes its measurements in a co-ordinate system – typically termed the body (b) frame – defined by the axes of the triad itself. These measurements must be rotated into the inertial measurement frame using the rotation matrix Rib that relates the body and inertial frames, i
f (t) =
Rib f (t)b .
g(t)e = ¨r(t)e + 2 Ωeie r˙ (t)e − Reb f (t)b
Once the gravity vector in the body frame has been determined, the roll and pitch angles can then be calculated by gyb gzb
ϕ = arctan
! (23)
and θ = − arctan
gxb gzb
.
(24)
The geometry for this calculation is shown in Figure 7.
(17)
Both the vehicle accelerations and the gravitation can be expressed in an earth-fixed, earth-centered coordinate frame – the e-frame. In this case, the transformation between frames cannot be done by a simple pre-multiplication of a rotation matrix as was done in Equation (17), as the transformation of accelerations from geodetic frame to the inertial frame must account for accelerations due to earth’s angular velocity. If this is done, then the acceleration can can be expressed as ¨r(t)i = Rie ¨r(t)e + 2 Ωeie r˙ (t)e + Ωeie Ωeie r(t)e , (18) where Ωeie is a skew-symmetric matrix of the angular velocity ω eie due to earth’s rotation. The 2 Ωeie r˙ (t)e and Ωeie Ωeie r(t)e terms in Equation (18) are respectively the Coriolis acceleration and centripetal accelerations. An additional term which is due to nonconstant earth-rotation has been neglected. Substituting Equations (17) and (18) into (16) and expressing the gravitation in the geodetic frame results in Rib f (t)b = Rie ¨r(t)e + 2 Ωeie r˙ (t)e
(21)
(19)
yb
zb gyb gxb ϕ θ
xb Front of Vehicle
g
gz b
Figure 7: Calculation of Roll and Pitch from the Gravity Vector
Recognizing that the gravity vector is equivalent to
In Equation (22), the rotation matrix Rbe between the geodetic and body frames is required. It is most easily calculated using
¯ (t)e − Ωeie Ωeie r(t)e g(t)e = g
b R(t)be = R(t)ll e R(t)ll
¯ (t)e . Ωeie Ωeie r(t)e − g
(20)
(25)
187
where Rll e is the rotation matrix between the geodetic and local-level co-ordinate frames and Rbll is the rotation matrix between the local-level and body coordinate frames. If the local level frame is defined as a right-handed system with its origin coincident with the sensor frame and y-axis pointing north, then Rll e can be calculated using Rll e = Rx (π/2 − φ)Rz (π/2 + λ).
(26)
where φ and λ are respectively the latitude and longitude of the vehicle. If the body frame is also righthanded with the z-axis pointing up and the x-axis pointing forward – as depicted in Figure 7 – then Rbll is calculated using Rbll = Rz (π/2 − α)Rx (θ)Ry (ϕ).
(27)
where α is the vehicle’s azimuth. The azimuth of the vehicle’s trajectory can be calculated from the local-level velocities using α = arctan
r˙xll r˙yll
(28)
It is important to note that this is not the vehicle’s azimuth. For a land-vehicle, the vehicle’s azimuth and the azimuth of the trajectory will typically be the same; for an airborne vehicle they will likely not. Furthermore, it is not possible to determine azimuths in this manner if the vehicle is stationary, and at low velocities they will likely be very noisy because of noise in the GPS derived velocities. The problems with the azimuth determination are significant. Obviously, any error in the azimuth angles will affect the calculation of the roll and pitch angles, as incorrect azimuth angles will affect the calculation of the Rbll matrix. This will in turn influence how the GPS accelerations are applied to the measured specific forces, and consequently the accuracy of the roll and pitch angles will also be degraded. One possibility for dealing with the problem of azimuth angles is to use an external azimuth aid – for example, the magnetic sensors in the DMC. Unfortunately, the inclusion of extra components would partially defeat the purpose of the system – attitude determination at a lower cost. A flow chart of the algorithm used to calculate the position, velocity, and orientation is shown in Figure 8. From this figure the simplicity of the algorithm
is evident. The figure also highlights the recursive nature of the algorithm, in that the calculation of the roll and pitch angles for any epoch depends on the roll and pitch angles calculated in the previous epoch. 5
PRELIMINARY TESTING
To examine the accuracy of the system, a kinematic test was performed using a van on which a highaccuracy Inertial Navigation System (INS) was also mounted. The attitude angles from the INS, which were determined using the KINGSPAD GPS/INS processing software (Schwarz and El-Sheimy, 2001), are treated as the true angles in all the comparisons that follow. At approximately 1 minute intervals the van was stopped and a minute was allowed to pass while the van was stationary so that a zero-velocity update (ZUPT) could be performed for the INS. The stopping and starting meant that the system could be examined under reasonably high accelerations (-2.5 m/s2 to 2 m/s2 ) and during low and high velocities (0 km/hr to 100 km/hr). For the test, GPS measurements were made at 1 Hz, and DMC measurements were made at 10 Hz. The GPS accelerations were linearly interpolated to provide accelerations at the same times as the DMC specific force measurements. The primary purpose of the test was to examine the accuracy of the angles calculated using the measurements from the DMC – i.e., the roll and pitch angles. Figure 9 shows these angles during a kinematic period (between ZUPTs) both before and after the correction for vehicle accelerations are applied to the measured specific forces. The large improvement in both roll and pitch angles is immediately apparent. For these figures, the GPS accelerations were determined using a tenth-order direct differentiator on differential carrier-phase positions. The actual roll and pitch angular errors are shown in Figure 10. From this figure, it is apparent that the largest errors in roll and pitch occur at the beginning and end of the test runs as the vehicle is travelling at the lowest speeds. It is during these periods that the azimuth is also the poorest. The root of both problems can be traced to the GPS derived velocities that are used to calculate the azimuth. At low speeds, these velocities are relatively noisy. Consequently, the derived azimuth is also noisy, and the noisy azimuths contaminate the Rbll matrix. This, in turn, affects the calculation of the roll and pitch angles. Also during
188
f
Accel
b
- Σ
g
b compute ϕ, θ
roll, pitch
..r b GPS
..r ll
.e
r
r
ll R e
e
Σ
e
9 ie s
s
.e
R
r
b ll
. ll
r
compute α
azimuth
velocity
..r e
compute f,l,h
position
Figure 8: Algorithm
the beginning and end of the Kinematic periods the van was accelerating while pulling off and onto the shoulder of the road. Thus, the highest frequency accelerations – both longitudinal and transverse – occur during this period, and it is likely that the relatively low GPS data rate meant that these accelerations were not adequately represented.
10
Angle (degrees)
Litton Roll DMC Raw Roll DMC Corrected Roll
5
0
−5
−10 156880
156890
156900
156910
156920
156930
156940
156950
156960
GPS Time (s)
(a) Roll 10 Litton Pitch DMC Raw Pitch DMC Corrected Pitch
Angle (degrees)
5 0 −5 −10 −15 −20 156880
156890
156900
156910
156920
156930
156940
GPS Time (s)
(b) Pitch Figure 9: Roll and Pitch Angles (Detail)
156950
156960
The results for just the kinematic data periods are summarised in Tables 3 and 4. For these tables, only periods where the velocity was above 20 km/hr were considered. This eliminated the periods where the noisy azimuths were harming the roll and pitch angles. Also, the results in these tables are those when the best central-difference differentiator was used. Surprisingly, both the differential carrier phase positions and the differential code positions gave similar results – RMS accuracy in both cases is better than 1 degree (RMS). Somewhat distressingly, however, is the fact that in both cases the maximum error can exceed 4 degrees. When differential carrier-phase positions were used, a tenth-order direct differentiator gave the best results. This was expected, as such a differentiator best ap-
189
15 Roll Pitch
Angular Error
10
Mean (◦ ) RMSE (◦ ) Max (◦ ) Min (◦ ) Epochs
5 0 −5 −10 −15 156880
156890
156900
156910
156920
156930
156940
156950
Roll 0.08 0.90 4.42 -5.34 1209
Pitch 0.05 0.53 3.54 -2.04 1209
Azimuth 0.00 0.36 3.05 -2.79 1209
156960
GPS Time (s)
Table 4: Attitude Error using Differential Code and 2th -Order Cascaded Differentiator – Kinematic Epochs
(a) Before 15 Roll Pitch
Angular Error
10 5 0 −5 −10 −15 156880
156890
156900
156910
156920
156930
156940
156950
156960
GPS Time (s)
(b) After Figure 10: Roll and Pitch Angular Error
proximates an ideal differentiator and best maintains the high frequencies. In contrast, for the differential code positions, a second-order cascaded differentiator gave the best results. This is because such a differentiator best attenuates the high frequency accelerations, which in this case are mostly due to the differential code positions being very noisy. This noise is, in turn, likely the result of multipath, as the test was conducted over a relatively small area where most of the errors due to the atmosphere were eliminated.
Mean (◦ ) RMSE (◦ ) Max (◦ ) Min (◦ ) Epochs
Roll 0.08 0.78 3.45 -4.61 1209
Pitch 0.03 0.36 2.27 -1.73 1209
Azimuth 0.00 0.29 2.93 -2.21 1209
Table 3: Attitude Error using Differential Carrier Phase and 10th -Order Direct Differentiator – Kinematic Epochs
For several reasons, the results in Tables 3 and 4 should not be considered entirely representative. Certainly, the elimination of the periods with low-velocity meant that the largest orientation errors were not included. Obviously, the inclusion of these periods would result in poorer statistics, and consequently the
results may be optimistic. Several problems with the test, however, mean that the results may also be considered somewhat pessimistic. For instance, several weeks after the conclusion of the test it was discovered that the GPS/INS system had timing errors. Thus, the angles being compared were not always occuring at the same time. Additionally, the GPS/INS processing software could only output measurements at 1 Hz, meaning that it was necessary to linearly interpolate between the INS attitudes in order to get times corresponding to times from the GPS/DMC system. The 1 Hz data rate of the GPS used to determine accelerations is also problematic, as it means that only accelerations of 0.5 Hz or less could be determined and that it was necessary to linearly interpolate the GPS accelerations so that they would coincide with the DMC specific forces. In particular, it is felt the linear interpolation of both the GPS/INS results and the GPS accelerations contributes to the large maximum and minimum values. 6
CONCLUSIONS AND FUTURE WORK
Like the body of this paper, the conclusions can be divided into two parts. First, regarding the finitedifference differentiators, the following conclusions can be made: • Direct central-difference approximations to second derivatives have the same phase-response characteristics as cascaded differentiators, but have magnitude responses that more closely approximate an ideal derivative – particularly for higher frequencies. Unless the suppression of higher frequencies is desired, direct differentiation is preferable. • Backward-difference derivative approximations higher than first-order have non-constant and undesirable phase response characteristics. Consequently, the first-order approximation is the
190
only practical backward-difference differentiator. This differentiator has a group delay equivalent to half the sampling interval, but this delay is the smallest of any causal differentiator. Thus, the first-order backwards-difference differentiator is the most appropriate for “real-time” implementation. Because only preliminary testing has been performed using the GPS/Acceleromater attitude determination system, only preliminary conclusions can be made. However, it can be said with certainty that such a system is effective at reducing the cost of orientation determination with accuracies that are sufficient for some mobile-mapping applications. The system is also relatively simple to implement and can potentially be used in real-time. Of concern, however, are the large maximum errors. One investigation notably absent from this paper is an error analysis. For the system itself, the effects of misalignment of the accelerometer triad with the vehicle and errors in the GPS-derived azimuth should both be examined. Also, the accelerometer triad should be calibrated. For the GPS accelerations, the effect of atmospheric, multipath and other errors should be looked at. As noted in section 4.2, such analyses have been conducted, but only for accelerations of a much lower frequency. 7
ACKNOWLEDGEMENTS
Tom Ford and Mike Bobye at Novatel Inc. provided the van and help for the kinematic test. Their assistance is greatly appreciated. Also, Eun-Hwan Shin provided both assistance during the test and valuable comments on the text of this paper. His help is also appreciated. BIBLIOGRAPHY Antoniou, A. 1993. Digital Filters: Analysis, Design, and Applications. McGraw-Hill, Inc., New York, New York. Bruton, A. M. 2000. Improving the Accuracy and Resolution of SINS/DGPS Airborne Gravimetry. PhD thesis, University of Calgary, Calgary, Canada. URL: http://www.geomatics.ucalgary. ca/GradTheses.html.
Bruton, A. M., Glennie, C. L., and Schwarz, K.-P. 1999. “Differentiation for High-Precision GPS Velocity and Acceleration Determination”. GPS Solutions, 2(4):7–21. Cannon, M. E., Lachapelle, G., Szarmes, M. C., Hebert, J. M., Keith, J., and Jokerst, S. 1997. “DGPS Kinematic Carrier Phase Signal Simulation Analysis for Precise Velocity and Position Determination”. Navigation: Journal of the Institute of Navigation (ION), 44(2):231–245. Ellum, C. M. and El-Sheimy, N. 2001. “Portable Mobile Mapping”. In proceedings of FIG Working Week, Seoul, South Korea. The International Federation of Surveyors (FIG). On CD-ROM. Jacobson, M. Z. 1998. “Overhead Slides for Fundamentals of Atmospheric Modeling”. URL: http://efml.stanford.edu/FAMbook/ FAMbook.html. Jekeli, C. and Garcia, R. 1997. “GPS Phase Accelerations for Moving-Base Vector Gravimetry”. Journal of Geodesy, 71(10):630–639. Ladetto, Q., Gabaglio, V., and Merminod, B. 2001. “Two Different Approaches for Augmented GPS Pedestrian”. In proceedings of International Symposium on Location Based Services for Cellular Users (Locellus), Munich, Germany. Oppenheim, A. V., Schafer, R. W., and Buck, J. R. 1999. Discrete-Time Signal Processing. PrenticeHall, Inc., Upper Saddle River, New Jersey, second edition. Press, W. H., Flannery, B. P., Teukolsky, S. A., and Vetterling, W. T. 1992. Numerical Recipes: The Art of Scientific Computing. Cambridge University Press, Cambridge, United Kingdom, 2nd edition. Schwarz, K.-P. and El-Sheimy, N. 2001. KINGSPADTM User’s Manual. Department of Geomatics Engineering, The University of Calgary, Calgary, Canada. Schwarz, K.-P. and Wei, M. 2000. ENGO 623 Lecture Notes – INS/GPS Integration for Geodetic Applications. Department of Geomatics Engineering, The University of Calgary, Calgary, Canada. Lecture Notes.
191
APPENDIX Example of calculation of fourth-order centraldifference differentiator: 1 1 1 1 1 −2∆t −∆t 0 ∆t 2∆t 2 2 2 2 (−2∆t) (−∆t) 0 (∆t) (2∆t) A= (−2∆t)3 (−∆t)3 0 (∆t)3 (2∆t)3 (−2∆t)4 (−∆t)4 0 (∆t)4 (2∆t)4 y = h0
h1
h2
h3
h4
T
First derivative: B1 = 1 x= 0 h0 =
1
0
0
1 = −h4 12∆t
T 0 h1 =
−8 = −h3 12∆t
h2 = 0
df (t) f (t + ∆t) − f (t − ∆t) ≈8 dt 12∆t f (t + 2∆t) − f (t − 2∆t) − . 12∆t Second derivative: B2 = 1 x= 0
0
2
0
−1 = h4 12∆t2 −30 h2 = 12∆t2
h0 =
T 0 h1 =
16 = h3 12∆t2
d2 f (t) f (t) f (t + ∆t) + f (t − ∆t) ≈ − 30 + 16 dt2 12∆t 12∆t f (t + 2∆t) + f (t − 2∆t) − . 12∆t2
192
