1

Application of Adaptive Kalman Filtering Algorithms for Smoothing Attitude Solutions with GPS Source Variability

Mahmoud Efatmaneshnik School of Surveying & SIS, UNSW, Sydney 2052

Tel: 02 9385 4190, Email: mahmoud.e@unsw.edu.au Yong Li

School of Surveying & SIS, UNSW, Sydney 2052 Tel: 02 9385 4173, Email: yong.li@unsw.edu.au

Andrew G Dempster School of Surveying & SIS, UNSW, Sydney 2052

Ph: 0 2 9385 6890, a.dempster@unsw.edu.au

ABSTRACT

GNSS/INS (Global Navigation Satellite System/Inertial Navigation System) systems have found widespread usage in industry, especially in automated agriculture. Automated agriculture requires high frequency, precise, steady and smooth attitude solutions. This necessitates smooth error patterns in navigation solutions. Given that navigation solutions can be derived from different information sources, combining several information sources can result in smoother attitude solutions relative to when a single information source is utilized. A Multiple Model Kalman Filter (MMKF) is used for this purpose. The results show that the fine attributes of both information sources are adequately captured.

Introduction

Machine automation forms the foundation of precision agriculture and relies heavily on precise positioning and attitude determination technologies. Guidance and steering control systems are now in widespread use by farmers for ploughing and cultivating their lands with unprecedented accuracy. The guidance and steering control system requires centimetre-level position as well as attitude information accurate to a few degrees. The guidance and steering control system particularly relies on heading and roll measurements to steer the tractor on parallel straight lines or curves known as plough lines. The attitude accuracy requirements for automated agriculture differ by application type. Spraying, for example, can be continued under low accuracy whereas irrigation ditching requires continuous high attitude accuracy. For these kind of applications, if the error jumps more than a certain amount the automatic guidance system must disengage. Large-scale crop (broad acre) planting can tolerate degrees of attitude errors provided that the error jumps are not too sharp in the form of rapid shifts in attitude. This necessitates smooth error patterns in navigation solutions.

Cost affordability and precision are not important issues for localization because with employing a local reference station the current Real-Time Kinematic (RTK), GPS can achieve centimetre level accuracy. Low-cost Micro Electronic Mechanical Systems (MEMS) inertial gyros are typically used in such systems. They are much less accurate than their optical counterparts due to their larger biases, axis misalignment errors, scale factor biases and greater susceptibility to temperature drift. A solution derived from low-cost MEMS inertial sensors alone does not provide the required accuracies, even for short periods. Therefore

2

designing well-calibrated and smart filtering capabilities that achieve a low-cost, precise and high update rate attitude determination system has been on the agenda in recent years (Li et al., 2006). Leica’s mojoRTK system, as an example, makes use of automotive-grade MEMS inertial sensors and a dual frequency survey grade GPS/GLONASS receiver to provide position and attitude information for agricultural guidance and machine control applications.

A Kalman filter (KF) is the most common estimation engine for integrated navigation systems. At UNSW, an Attitude and Heading Reference System known as AhrsKf has been developed for a new generation of mojoRTK system. The AhrsKf system is based on a closed loop KF. It uses strapdown Inertial Navigation System (INS) computation equations in the navigation coordinate system (n-frame). In AhrsKf, the GNSS solutions of position and velocity are fed into the Kalman Filter for state error estimation of INS measurements. Despite the low quality of the INS sensors used in the mojoRTK device, the AhrsKf algorithm delivers stable results and its performance is better than the mojoRTK algorithm as reported earlier in Li, et al. (2009).

In this paper, first an extension to the basic AhrsKf is considered. This extension is a position-domain capability that enables the filter to produce smooth positions despite RTK dropout or sudden unavailability. RTK dropout can happen when the tractor is, for example, passing by a tree that blocks the line of sight of the RTK reference station. The current mojoRTK system in this situation switches to less accurate position inputs. It is shown that for a specific position input source, the attitude solutions can be more accurate. Based on this a Multiple Model Kalman Filter (MMKF) that enables the AhrsKf to accommodate multiple GPS input measurement sources is introduced and the results are presented.

The sequel is an introduction to AhrsKf. Position domain smooth transition is discussed next and then demonstrated with an example. The better attitude solutions with alternative position solution are discussed after that. Finally several adaptive KF algorithms including MHF are explained and MHF results are presented in the end.

mojoRTK and AhrsKF

The mojoRTK sensors consist of three rate gyros, a three axis accelerometer, and a 2-axis magnetometer (Leica Geosystems, 2009). In addition to these sensors, the system includes two GPS receivers – a dual frequency L1/L2 GPS/GLONASS receiver and a low-cost L1-only single frequency GPS receiver, that produce a two axis attitude solution which is used for estimating the biases of the inertial sensors. The inertial and GPS data are then processed by an embedded 400MHz PowerPC processor, as illustrated in Figure 1. The specifications of the MEMS gyros in mojoRTK are listed in Table 1.

Figure 1. The configuration of the mojoRTK (left), and mojoRTK console (right) (Kellar et al. 2008)

3

Table 1. Technical specifications of MEMS gyros (Geng et al., 2007). Characteristics Specifications

Gyro Sensitivity Error ±6%

Gyro Linearity ±0.3%

Gyro Bias Variation at Constant Temperature ±0.4 °/S

Gyro Bias Stability over one hour ±0.4 °/S

Automated guidance systems for agricultural environments, in order to achieve better performance than human operators in rendering parallel plough lines and curvatures onto the farm land, need to deliver 55mm accuracy in cross track error; this is defined as “the deviation of the centre of the rear of the tractor from the desired path” (Geng et al., 2007). Additionally, position and attitude update rate of no less than 20Hz is required for successful guidance in agricultural environments. The current mojoRTK system is capable of producing position and attitude solutions at a rate of 10Hz.

AhrsKf software operates on the mojoRTK system. The temperature-compensated inertial data are fed into the integration Kalman filter. In AhrsKf the GPS position and velocity are used to derive the position and velocity errors of the INS in a closed loop. This means that the error corrected INS position and velocity solutions are integrated with the next epoch GPS solution. The AhrsKf state vector is:

TzyxzyxUNEUNE hLVVV ],,,,,,,,,,,,,,[ !!!= """##$####%%%X (1)

The first three states are attitude errors, then velocity errors, and position errors. The final 6 states are sensor error terms (respectively related to three gyros and three accelerometers), which are modeled as a random walk in the AhrsKf. The measurement equations for velocity and position are

)()()( ttvvvvvv

t vv

GUIU

GNIN

GEIE

v VXHZ +=!!!

"

#

$$$

%

&

'

'

'

=

)()()( tthh

LLt pp

GUIU

GNIN

GEIE

p VXHZ +=!!!

"

#

$$$

%

&

'

'

'

= ((

(2)

When the speed of the vehicle is high enough, e.g. 3m/s, the GPS velocity can be used to derive the heading of the movement at an accuracy of about 0.57deg (assuming the GPS velocity error of 3cm/s). The heading derived from the GPS velocity can then be used to correct the INS heading in the integration Kalman filter:

MuGINS VtZ +=!= "### )( (3)

So the equation of measurement is written in a matrix form as below

4

VX0000000I000000I00

ZZ

Z +!!!

"

#

$$$

%

&

=!!!

"

#

$$$

%

&

=

'''''

''''''

''''''

3131313121

333333331323

333333331323

1(Zp

v

(4)

The system model in Kalman filter is:

GwFxtx +=)(! (5)

where F is the system model matrix described in (Li et al., 2009) and G is the system noise distribution matrix and w is the system noise vector. Matrix w contains the variances of accelerometers and gyros. F and G×w need to be discretized for discrete systems. Matrices Φ and Q are the result of discretization process and are used in the Kalman Filter State vector estimation equations through time propagation:

+!!

! = 11 kkk xx !"

1111 !!+!!

! += kTkkkk QPP ""

1)( !!! += kTkkk

Tkkk RHPHHPK

)( !!+ !+= kkkkkk xHzKxx !!!

(6)

The expected value of corrections are zero in close loop filters, thus 0=!kx! .

Position Domain Smooth Transition

In farms there are times that the L1/L2 receiver antenna cannot receive the RTK correction signals due to the blockage for example by a tree. In this case mojoRTK switches to GL1DE that is a relative Pseudorange/Delta-Phase (PDP) filter by NovAtel and provides an enhanced solution of 1 meter accuracy over 20 minutes (NovAtel, 2008). The PDP filter provides a filtered position and velocity solution based on assumed vehicle dynamics (NovAtel, 2008). By doing that in conditions where the GPS signal tracking is hampered by obstructions such as trees or buildings, the PDP filter will bridge through brief partial or even complete GPS outages while providing a continuous position/velocity solution (NovAtel, 2008). Thus it is a perfect substitute when there is an RTK dropout. The positioning requirement for a clean farming plough line or curvature is only 5-10 cm. Thus an RTK dropout and transition to GL1DE can effectively introduce unacceptable error in the plough line. As such a smooth transition from RTK to GL1DE and back is required. Smooth transition is a position domain correction. A new capability has been added to AhrsKf to accommodate this requirement. By calculating the bias of GL1DE position from RTK position at each epoch, when and if there is an RTK dropout the bias related to the latest epoch is used to keep the tractor on a smooth curvature. If the RTK solution becomes available again, the bias of the carrier phase solution is calculated relative to the biased GL1DE solution of the last epoch, thus a smooth curvature is maintained. It should be noted that smooth transition is an optional setting and can be switched on or off by the user.

A test was conducted at Boonah on 14 July 2010 with an intermittent RTK signal. In this test the RTK reference station was turned on and off. Figure 2 (a) shows the recorded track and a small region at which the RTK reference station was turned off is marked. Figure 2 (b) shows the close up of the normal transition to GPS (L1) solution (blue) along side with the raw GL1DE solution (green). Figure 2 (c) shows the AhrsKf smooth transition from RTK and

5

back to RTK. It can be seen that the smooth solution produces a smooth curvature where as the normally it would be tilted or biased relative to a neat curvature.

Attitude Solution with GL1DE

An interesting outcome of the addition of the above train smoothing mechanism is the effect of the GL1DE solutions on AhrsKf attitude. Surprisingly, the AhrsKf attitude solutions are more stable with GL1DE input rather than RTK input as measurement to KF. This fact is tested by using the Normalized Innovation Squared statistic λk that reflects the stability of KF (Bar-Shalom, et al., 2001):

( ) +!!+ += kkTkkk

Tkk zRHPHz ""#

1

(7) Where m is the size of measurement vector and +

kz! is the innovation:

(a)

(c) (b)

Figure 2. Shows the non-smooth and smooth transition from carrier phase solution in AhrsKf.

6

++ != kkkk xHzz" (8)

If all other measurements in the KF are Gaussian then λ should follow a chi-squared distribution (Bar-Shalom, et al., 2001). Figure 3 shows the λ stream of AhrsKf for input with RTK dropouts. The RTK dropout is indicated with a green line that has a value of 2 for when GL1DE is used and a value of 3 for when RTK is used. Close examination of the figure reveals that λ is less jittery or ragged when GL1DE is used. This means GL1DE leads to less significant white noise in λ, which can be interpreted as high frequency stability in the AhrsKf attitude solutions. Figure 4 shows that λ for the AhrKf solution produced by using GL1DE is almost always less noisy than that produced by using RTK/GPS information. Note that in the RTK/GPS information at the times that the RTK is not available single frequency L1 GPS solution is used (Figure 4). In the following the examination of the attitude errors clarifies this issue further.

Figure 3. λ statistic for post processed AhrKf solution with switching between RTK (a) and GL1DE information sources (blue). In (a) green line shows the information source. The comparison between λ statistic for post processed AhrKf solution with GPS/RTK (blue)

and GL1DE (green) information sources (b).

(a)

(b)

7

The attitude error was compared to the SPAN-CPT solution as the reference system. Although the RTK module of the SPAN-CPT was disabled it was understood that the system’s attitude solution would still be a reliable reference, since the SPAN-CPT is originally designed to accommodate restricted GNSS reception (NovAtel, 2009). The specifications of SPAN-CPT are summarized in Table 2. The attitude data acquisition for the SPAN-CPT was set at 50 Hz in the test. The installation of the SPAN-CPT in the tractor is shown in Figure 4.

Table 2. The Specifications of the SPAN�CPT (NovAtel 2009) Specifications Gyros Accelerometers

Range ±375deg/s ±10g

Bias ±20deg/h ±50mg

Bias stability ±1deg/h ±0.75mg

Scale factor 1500ppm 4000ppm

Random walk 0.0667deg/sqrt�h -

Figure 4. Installation of the SPAN-CPT in the tractor.

Figure 5. and Table 3 show that AhrsKf can produce less jittery (noisy) and smoother attitude solutions when it is supplied by GL1DE position, velocity and heading solutions relative to when it is operating with RTK solutions. The table shows the static results of the attitude error. Generally speaking the STD of attitude error for both roll and yaw are lower for GL1DE driven solution. Note that in this figure the sudden hikes in the errors are related to when the tractor turns.

8

Table 3. Static results for the comparison between post processed AhrsKf solutions with different information sources and SPAN attitude solutions

With GPS and RTK With GL1DE

Roll Yaw Roll Yaw

Mean -1.5021 -1.5853 -1.1335

-2.3596

STD 4.4021 4.4032 3.9263 3.8395

The reason for the better performance of AhrsKf with GL1DE relative to RTK rests in the mechanism that GL1DE solutions are derived. The PDP solution optimizes the absolute positioning accuracy of the GPS code observation and leverages the relative stability of the GPS carrier phase and Doppler observations (NovAtel, 2008). The advantage of this approach is a smoother solution output (both velocity and position) and greater solution availability. The GL1DE filter produces a very smooth solution with consistent rather than absolute position accuracy. There is typically less than 1 cm difference from epoch to epoch

Figure 5. The comparison between yaw (a) and roll (b) errors for post processed AhrKf solution with GPS/RTK and GL1DE information sources relative to SPAN-CPT solution.

(a)

(b)

9

(NovAtel, 2008). Figure 6 shows the velocity trajectory of RTK and GPS solutions (Figure 6 (a)) and GL1DE solution (Figure 6 (b)). It is clearly observable that the velocity trajectory of GL1DE is smoother and had less jitter or noise. This fact reflects excellently on the performance of AhrsKf because in AhrsKf the northing and easting velocities are used to derive the heading (or yaw) as a measurement input to the KF (Equation 3.) at each epoch.

The problem now remains to be a way of integrating the GL1DE solution into the AhrsKf at all epochs and when there RTK is still available. A straight forward idea is to feed both GL1DE and RTK/GPS solutions at the same time into AhrsKf. This means that if at epoch k KF is fed by GL1DE solution then the next epoch it is fed by RTK/GPS solution. Note that both frequencies of the RTK/GPS and the GL1DE were set at 10 Hz. The static result of this approach is shown in Table 4 below. It can be seen that this method does not lead to better results, which was predictable since the noisier velocity measurements of RTK/GPS solutions will wholly reflect on the output. A Multiple Hypothesis algorithm might be useful in this situation and is discussed next.

Table 4. Static results for the comparison between the post processed AhrsKf solutions simultaneously fed with both GPS/RTK and GL1DE with SPAN attitude solution.

With GPS/RTK and GL1DE Roll Yaw Mean -1.7950 -1.9354 STD 4.2989 4.3804

Figure 6. The velocity trajectory for RTK/GPS solution (a) and GL1DE solution (b). The GL1DE trajectory is visibly smoother.

(a) (b)

10

Multiple Model Adaptive Kalman Filter

Several adaptive KF were proposed with different utilities. These fit in three general categories:

1. Adaptive estimation of system noise and/or measurement covariance matrices

2. Adaptive utilization and (in some cases estimation) of a fading or fudge factor for the scaling of either system noise or predicted covariance matrix

3. Multiple model and multiple hypothesis adaptive filtering

Adaptive estimation of noise and/or measurement covariance matrices can be performed in two distinct ways by using a process residual sequence over a sliding window or process innovation sequence. These filters are only useful if the error characteristics of the measurement model or system model are not known a priori. In the case of AhrsKf this adaptive filter is not useful because in AhrsKf Q and R matrices are very carefully defined.

The second type of adaptive filters use an adaptively estimated fading factor fudge factor. The Kalman filtering estimation at epoch k can be considered as ‘weighted’ adjustment between the new measurements (observation model) and the predicted state vector based on the dynamic model and all previous measurements. If too much weight were given to the dynamic model, the estimation would ignore the information from measurements and causes the divergence of the filtering process. Fagin (1964) initiated a method to limit the memory of the KF by using an exponential fading of past data via forgetting factor sk . The scaling factor scales up the predicted state covariance. This can be done in two ways by applying the factor to state covariance of last epoch (Geng, 2008) or to process noise covariance (Bar-Shalom, et al., 2001).

Multiple Model Adaptive Kalman Filter (MMAKF) is a way to combine several parallel KF solutions together and to from a filter bank. Each KF denoted by an index i, in this case is based on independent information from other filters, e.g. different measurement sources, different R or Q matrices or even different initial P matrices. The simplest ways to perform this are one of the following:

1. Weighted fix: weighted averaging of the solutions and their process covariance noises

2. Best fix: accepts the solution with the highest probability and rejects others

The weighted fix process can be either based on predetermined fixed weights or adaptive weights calculated as follows (Groves, 2008):

!=

"= l

ijk

ikik

p

pp

1,

,,

( ) !"

#$%

&+'

+=( ''''

'

'ikik

Tikikik

Tik

ikTikikik

m

ikik zRHPHz

RHPH

pp ,

1,,,,,

,,,,

,1, 2

1exp

)2())

*

(9)

where m is the number of components of the measurement vector and l is the number of filter models. Note that the matrix gain is already performed as part of the KF calculations. The filter model or hypothesis with the smallest normalized measurement innovations is

11

most consistent with the measurement stream, so is allocated the largest probability (Groves, 2008). Over time the probability of the best filter hypothesis will approach unity, while others approach zero. If that’s the case the filter cannot go back to the adaptive mode. To avoid this, the weights must be reset to an initial value. The overall state estimate and error covariance are obtained as follows (Groves, 2008):

( )1,,,,,,,!!! += ik

Tikikik

Tikikik RHPHHPK

!!+ += ikikkik zKxx ,,, "!!

!=

++ =l

iikikk xpx

1,,!!

( )( )[ ]!=

++++++ ""+=l

i

Tkikkikikikk xxxxPpP

1,,,,!!!!

(10)

The best fix filter chooses the model’s solution with the highest weight.

Results

For the purpose of making simultaneous use of RTK/GPS and GL1DE solutions in AhrsKf three algorithms were used. First a Multiple Model KF was utilized that is similar to MHAKF with the difference that the weights for each model are fixed rather than being chosen adaptively. In this model the two weights were chosen as p1 = 0.5 and p2 = 0.5. SPAN-CPT solution was used as the reference and the results are shown in Table 5 as well as Figure 7. For the data set collected in this test, the fixed weight two-model AhrsKf showed an important improvement for the roll error STD and yaw error mean. The fixed weight solution didn’t improve the mean in roll and STD of yaw error relative to the Ahrskf solution with GL1DE. Note that a negative improvement in the all the following tables is an indicator of a decline.

Table 5. Static results for the comparison between the fixed weights MMKF AhrsKf solutions simultaneously fed with both GPS/RTK and GL1DE with SPAN attitude solution.

Improvement relative to GPS and RTK

Improvement relative to GL1DE MMKF Fixed

weights Roll error (Degrees)

Yaw error (Degrees)

Roll Yaw Roll Yaw

Mean -1.8688 0.2677 0.3667 1.3176 -0.7353 2.0919

STD 2.8750 4.2693 1.5271 0.1339 1.0513 -0.3743

12

The adaptive weighted fix MMAKF on the other hand showed improved mean and STD relative to AhrsKF solution with either GL1DE and GPS/RTK (Table 6 and Figure 8). Although the improvement in adjusting yaw error mean and STD was much better with this filter than the fixed weight MMKF, the roll error STD improvement was not.

Table 6. Static results for the comparison between the weighted fix MMAKF AhrsKf solutions simultaneously fed with both GPS/RTK and GL1DE with SPAN attitude solution.

Improvement relative to GPS/RTK

Improvement relative to GL1DE

MHAKF adaptive

weighted fix

Roll error (Degrees)

Yaw error (Degrees)

Roll error Yaw error Roll error Yaw error

Mean -1.0901 -0.3948 0.4120 1.2853 0.0434 1.9648

STD 3.9282 3.6406 0.4739 0.7626 0.4739 0.1989

Figure 7. The comparison between yaw (a) and roll (b) errors for fixed weights MMKF AhrKf solution with simultaneous GPS/RTK and GL1DE information sources relative to

SPAN-CPT solution.

(a)

(b)

13

Table 7 and Figure 9 show the result for best fix MMAKF, in which the solution of filter (or model) with the highest weight is chosen as the global KF state solution. The improvement in roll STD, again, was less significant than the improvement of the fixed weight filter and yaw error improvement was similar to adaptive weight MMAKF.

Table 7. Static results for the comparison between the best fix MMAKF AhrsKf solutions

simultaneously fed with both GPS/RTK and GL1DE with SPAN attitude solution. Roll Yaw Roll Yaw

MHKF best fix

Roll error (Degrees)

Yaw error (Degrees) Improvement relative to

GPS and RTK Improvement relative

to GL1DE

Mean -1.5616 -0.0210 -0.0595 1.5643 -0.4281 2.3386

STD 3.9939 3.6797 0.4082 0.7235 -0.0676 0.1598

Figure 8. The comparison between yaw (a) and roll (b) errors for weighted average fix MMAKF AhrKf solution with simultaneous GPS/RTK and GL1DE information sources

relative to SPAN-CPT solution.

(a)

(b)

14

Conclusion

The AhrsKf attitude solutions tend to jump in terms of attitude solution error STD when the GPS measurement source is switched from RTK to GL1DE. This happens for example when due to densely wooded areas in farms the RTK phase corrections are unavailable, prompting measurement inputs from alternative GPS sources such as GL1DE. The shift in solution error STD is due to the fact that GL1DE measurements derived by incorporating Doppler shift frequencies are more consistent especially in terms of the velocity measurements. The attitude solutions with GL1DE can be on the other hand more erroneous in terms of mean error because of the bias that exist in the velocity as well as position solutions of GL1DE. Thus combining the two information sources should principally deliver smoother solutions. Several MMAKF were used to combine two information sources

Figure 9. The comparison between yaw (a) and roll (b) errors for best fix MMAKF AhrKf solution with simultaneous GPS/RTK and GL1DE information sources relative to SPAN-

CPT solution.

(a)

(b)

15

as input to the KF. The algorithms prove useful in extracting the useful information of each of the sources. The SPAN-CPT was used as the reference system for this test.

ACKNOWLEDGEMENTS

This work was funded by the Australian Research Council Linkage project LP0667730. Leica Geosystems is the industrial partner. The authors acknowledge GPSat Systems Ltd for providing the SPAN-CPT used in the test. , Damien Dusha and William Kellar from Leica Geosystems are appreciated for their collaboration in the research.

References Bar-Shalom, Y., Li, X-R. , Kirubarajan, T. (2001) Estimation with Applications to Tracking and Navigation, John Wiley and Sons, Inc. Geng, Y., Cole A., Dempster, A. G., Rizos, C., and Wang, J. (2007) Developing a low-cost MEMS IMU/DGPS integrated system for robust machine automation, Proceedings of ION-GNSS 2007, Fort Worth, Texas, 1618-1624. Geng, Y., Wang J. (2008) Adaptive estimation of multiple fading factors in Kalman filter for navigation applications, GPS solutions, 12(4): 273-27. Groves P.D. (2008) Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, Artech House, London. Kellar, W., Roberts, P., Zelzer, O. (2008) A Self Calibrating Attitude Determination System for Precision Farming using Multiple Low-Cost Complementary Sensors, 1st International Conference on Machine Control & Guidance. Li, Y., Dempster. A. G., Li, B., Wang, J., and Rizos, C. (2006) A Low-cost Attitude Heading Reference System by Combination of GPS and Magnetometers and MEMS inertial sensors for mobile applications. Journal of Global Positioning Systems 5(1-2): 88-95. Li, Y., Dusha, D., Kellar, W., and Dempster, A. G. (2009) Calibrated MEMS Inertial Sensors with GPS for a Precise Attitude Heading Reference System on Autonomous Farming Tractors, Paper presented at ION-GNSS2009, Savannah, GA, 22-25 September 2009. NovAtel (2008) Application Note on Pseudorange/Delta-Phase (PDP) and GL1DE Filters http://www.Novatel.com, accessed in 2010. NovAtel (2009) SPAN-CPT specifications, novatel.com, accessed in 2009.