Analysis of Three Different Kalman Filter Implementations for Agricultural Vehicle Positioning

Conventional positioning techniques based on GPS receivers are not accurate enough to be used with autonomous guidance systems. High accuracy GPS receivers can be employed, but the cost of the system would be very high. The alternative solution presented in this article is to combine the data provided by different positioning sensors using a Kalman filter. The described procedure also uses an odometric estimation of the mobile position, based on the kinematic model of the agricultural vehicle. Three different implementations of the Kalman filter are described, using different sensor combinations but based on the same vehicle model.


INTRODUCTION
Autonomous vehicle guidance includes positioning techniques to determine vehicle position and speed, and control techniques which implement the required actions to move the vehicle through the desired trajectory.This article focuses only on positioning techniques.
Positioning systems may be classified into absolute positioning systems and relative positioning systems.The first group can be used to obtain the mobile position with respect to a global reference; on the other hand, the second group provides the mobile position with respect to its previous position [1].Nowadays, the most widely used positioning system for agricultural vehicles is the GPS system.However, there are many other different positioning techniques.Hague et al. describe several non GPS based positioning systems [2].
Each positioning system has advantages and disadvantages, and is more suitable for certain kind of applications.However, the best option is usually the fusion of data provided by different positioning sensors.This increases the accuracy of the system and it is useful to avoid positioning problems, for example, if the GPS reception fails [3][4][5][6][7].
One of the most widely used methods for this fusion is the Kalman filter [8].The Kalman filter combines measurement data with estimations, minimizing the mean square error of the predictions.Also, this filter can be used only with a GPS receiver, in order to improve accuracy as it is described in [9].configurations of the system.All the configurations use a GPS receiver and the same kinematic model (more complex kinematic models can be found in [10,11]).The first configuration combines the data from the GPS with a measurement of the steering angle.The second configuration uses an electronic compass instead of the steering angle sensor.Finally the last configuration uses an IMU (Inertial Measurement Unit) and a steering angle sensor.Note that the most complex and expensive configuration is the last one, because it uses more sensors and calculation requirements are higher.
Kalman filtering is a powerful tool that has many other applications in agriculture, not only sensor fusion.For example, it can be used in topographic mapping or in obstacle detection algorithms [12,13], but that is out of the scope of this article.

TRICYCLE MODEL
Tricycle model (also known as Ackerman model) can be used to model the kinematic behavior of the most part of agricultural vehicles with three and four wheels.
As it is shown in Fig. (1), it consists of two main wheels which provide the vehicle speed, and a third wheel which controls the vehicle direction.
In Fig. (1) it is very easy to deduce that the turn radius R C is given by the following equation (1): where L is the length of the vehicle and the steering angle.
Taking into account that the linear speed and the angular speed are related by the turn radius, the following expression can be obtained (2): where is the angular speed of the mobile, is the orientation of the vehicle in an external inertial reference system, and v is the longitudinal speed of the mobile.Usually v and are the control variables of the system, L is a known parameter, and the other variables must be estimated or measured.The proposed equations have many simplifications, more complex models can be found in the literature, see [4,5], but the proposed model is enough for the purposes of this study.

KALMAN FILTER IN AN AGRICULTURAL VEHI-CLE
Comparing the GPS with dead reckoning positioning signals, the GPS usually gives low frequency information position, and dead reckoning provides high frequency information position.The GPS positioning signal is often noisy but bounded, while the dead reckoning signal is often smooth but unbounded.All the facts indicate that GPS positioning and dead reckoning positioning are complementary in nature, so that dead reckoning signals can be used to smooth out the short-term GPS error, and GPS signal can be used to recalibrate or align the dead reckoning drifting over a long period of time.
GPS signals arrive at fixed intervals, typically 1 second, and 0.2 seconds in the case of some more expensive models.Steering information from a steering sensor or from an electronic compass is virtually continuous, so an odometry model can be run at a much greater repetition rate.When a GPS reading arrives, it can be compared against the computed odometry estimation to correct drift and slip.This is basically the function of the Kalman filter.
A Kalman filter can be used to integrate the observations from absolute positioning and relative positioning.A very "friendly" introduction to the general idea of the Kalman filter can be found in Chapter 1 of [14], while a more complete introductory discussion can be found in [8], which also contains an interesting historical review.More extensive references include [13,[15][16][17].

Kalman Filter Using Steering Angle Sensor Information
One of the simplest Kalman filter implementation uses the positioning information of a GPS, with information of the mobile steering angle, as it is shown in Fig. (2).This information is provided by a sensor attached to the steering axis.
In this case, the state vector of the system can be defined in the following way: where x k , y k ( ) is the position of the vehicle in the instant t k in the local coordinate system, fixed to the earth.k , v k are respectively the heading (angle between the movement direc- tion of the vehicle and the north), and the vehicle speed at time instant t k .
The system state equation can be expressed in the following way [11]: where the state transition matrix (system matrix) F k is described as: In equation ( 4) k is white noise, and T is the update period of the Kalman filter.This means that the Kalman filter algorithm is executed each T seconds.The parameter corresponds to the direction angle of the wheels.
The observations of the system are defined in the following way: where x GPS , y GPS , GPS and V GPS are the position, heading and speed signals of the GPS respectively.Note that the update rate of these measurements is very small, whereas the update period of the filter is greater.The filter uses the odometric estimations based on the system model to interpolate the intermediate system state values.
The observation equation can be written in the following way [11]: The matrix that defines the observation model is: It is the identity matrix because the measurements are directly related with the system state variables.As in the system state equation, here a white noise component v k is introduced.Note that the system and observation white noise variables v k and k are considered gaussian sequences of zero mean and incorrelated.The initial value X 0 of the system state is a gaussian variable of known mean X 0 , and co- variance matrix P 0 also known.This means that the following conditions are met: where n Q k is the covariance matrix of the system noise, and R k is the covariance matrix of the observation noise.All of them are defined and positive.
The adjustment of Q k and R k is critical in the develop- ment of the Kalman filter [11].In this research, R k is de- fined in the following way: where are the covariances of the GPS observations.Q k usually has the following shape: where q is a parameter which measures the quality of the estimation.A small value means that the filter will give more importance to the estimation and less importance to the measurements.The value of this parameter must be chosen carefully, and there are several methods to determine the most convenient value for a specific application, see for example [8].
The equations for the estate estimation, update and gain matrix for this Kalman filter are listed in the following lines.See [11,15,16] for a more detailed explanation of these equations.
Extrapolation of the estimated estate: Extrapolation of the error covariance: Update of the estimated estate using the observations: Update of the estimation error covariance: Gain matrix of the filter: In the equations presented here, the (-) sign represents the a priori values of the covariance and the system state estimations, and the (+) sign represents the a posteriori values.Fig.
(3) shows the sequence of operations for Kalman filtering, summarizing the previous equations.

Kalman Filter for GPS-Compass Data Fusion
Another possible configuration of a Kalman filter applied to an agricultural vehicle uses an electronic compass instead of a steering angle sensor.The main advantage of this configuration is that the electronic compass does not require any special configuration to be installed in the vehicle.One disadvantage is that it is affected by the presence of external metallic objects, particularly if they are big.The solution is to put the compass far enough of these objects and perform a compensation of their effect.There are several algorithms to do this, see [5], but they are out of scope of this paper.The system configuration in this implementation is shown in Fig. (4).
In this case, the state vector of the system can be defined in the following way: where x k , y k ( ) is the position of the vehicle in the instant t k in the local coordinate system, fixed to the earth.k , v k are respectively the heading (angle between the movement direction of the vehicle and the north), and the vehicle speed in the time instant t k .
The state transition matrix (system matrix) F k is de- scribed as: If the angle of the wheels ( ) is additionally considered, the F k matrix could be described as: This is the same that has been previously obtained in the first method.Note that the measurement of the wheels angle ( ) improves the accuracy of the estimation and therefore the performance of the Kalman filter.However, it requires an additional sensor.
The observations of the system are defined in the following way: where x GPS , y GPS , GPS and V GPS are the position, heading and speed signals of the GPS respectively, and COM is the heading provided by the compass.
The matrix that defines the observation model is: As in the first method, it is necessary to define the covariances of the different observations: and the parameter q that defines the quality of the estimation.The value of these parameters must be chosen carefully.There are several methods to determine the most convenient values for a specific application, see [8] for more information.
The equations for the estate estimation, update and gain matrix for this Kalman filter are the same that for the first filter.See equations ( 15), ( 16), ( 17), ( 18) and (19).

Kalman Filter Using IMU Information
It is very common to use Kalman filters to combine data from a GPS receiver with data from an inertial system (IMU).Fig. (5) shows the block diagram of this system, where information provided by a heading angle sensor is also used.
In this case, the state vector of the system can be defined in the following way: where: Position of the vehicle in the instant t k in the local coordinate system fixed to the earth.
• k : Heading (angle between the movement direction of the vehicle and the north).

•
x k , y k ( ): Linear speed in each axis.

•
x k , y k ( ): Linear accelerations in each axis.
• v k : Speed of the vehicle in the instant t k .
In this implementation the state transition matrix (system matrix) F k is described as: In the previous matrix T is the update period of the Kalman filter and corresponds to the direction angle of the wheels.L is a parameter related to the tricycle model (length of the vehicle).
The observations of the system are defined in the following way: where x GPS , y GPS , GPS and V GPS are the position, heading and speed signals of the GPS respectively.
IMU , IMU , x IMU , y IMU are respectively the heading, angular speed and linear accelerations provided by the IMU.
The last two rows of the previous matrix have implicit the relation between the system accelerations and the accelerations provided by the inertial sensor (Fig. 6).In this case, it is necessary that the X axis of the inertial sensor is aligned with longitudinal axis of the mobile.
As in the previous methods, it is necessary to define the covariances of the different observations: for IMU related observations.It is also necessary to set the value of the parameter q that defines the quality of the estimation.The value of all these parameters must be chosen carefully, there are several methods to determine the most convenient values for a specific application, see [8] for more information.
The equations for the estate estimation, update and gain matrix for this Kalman filter are the same that for the previous filters.See equations ( 15), ( 16), ( 17), ( 18) and (19).

CONCLUSIONS
The aim of this article is to apply the Kalman filter theory to improve the accuracy of positioning techniques used in autonomous guidance systems of agricultural vehicles.Dif-ferent positioning techniques are possible for this kind of applications depending on the required accuracy.Conventional systems use a GPS as a positioning element, but the cost of the system is very high when good accuracy is required.Moreover, there are many other problems related to the use of a GPS as a positioning element in a vehicle, for example the absence of heading information when the vehicle is not moving or the temporary errors due to a bad signal reception [9].These problems can be solved by combining the GPS information with complementary data provided by other kind of sensors like electronic compasses or IMU (Inertial Measurement Units).This study applies the Kalman filter to combine the data of these sensors.The first proposed system uses an external steering sensor, the second an electronic compass and the last one a IMU.
For each sensor combination, the equations of the system have been developed using an Ackermann kinematic model.The article only gives a brief overview of the Kalman filter theory and equations, as they can be consulted in the related literature [11,[15][16][17].
Note also that although GPS noise will have a Gaussian component, there are aspects of the errors that have very different statistical properties, such as multipath and change of constellation.Kalman filtering theory supposes that the noise that affects the system is Gaussian, therefore the application to a GPS has some limitations that must be considered [9] but they are out of scope of this paper.
The practical application of a Kalman filter to a specific problem requires correct configuration of its parameters, specially the determination of the values of the system noise variance and the measurement noise variance.There are several techniques to determine the optimal values of these parameters.Literature about this subject can be found in [8,14].

Fig. ( 6 ).
Fig.(6).Relation between the accelerations in the global reference system and the accelerations in the mobile reference system.