A Kalman Filter Implementation for Precision Improvement in Low-Cost GPS Positioning of Tractors

1.1. Error Considerations for GPS Receivers

Two kinds of errors can be defined for GPS receivers and GPS guidance systems [9–11]: (i) precision, relative accuracy, reproducibility, repeatability, or pass-to-pass accuracy, which refer to the degree to which the measurements reported by a GPS receiver in a fixed placement provide close positions regardless of the real position; and (ii) accuracy or absolute accuracy, which refer to the degree of closeness of the measured positions to their real position. Figure 1 illustrates the difference between precision and accuracy.

In guidance system applications, where the time between each pass is relatively short and the trajectories are not saved from year to year, precision can be considered the most important variable. In this way, low-cost GPS receivers with 10 m accuracy but sub-meter precision will be alternatives in such agricultural task.

1.2. Quantization Effects in Low-Cost GPS Receivers

The most common chipsets that low-cost GPS receivers and embedded mobile computing devices integrate are the Sirf [12], the U-blox [13], and the MTK [14]. Receivers with either of these chipsets transmit positioning information by means of the National Marine American Association (NMEA) 0183 protocol, and provide latitude and longitude geodetic coordinates with usually only eight digits for latitude and nine for longitude (Figure 2).

Geodetic coordinates are not appropriate for agricultural data processing and are usually converted to Cartesian coordinates. When positions in geodetic coordinates with a quantization of only 8 digits for latitude and 9 for longitude are converted to Cartesian coordinates such as Universal Transverse Mercator (UTM) [15–17] or East, North, Up (ENU) [18], they appear on a rectangular grid of some decimeters in size. Specifically, at the place where the real tests were conducted, with a latitude of 41.32° N and a longitude of 4.84° W, the quantization grid is 14 cm and 18 cm on the X and Y axes, respectively, in UTM coordinates. Position, speed, and heading errors appear in the trajectories on this grid, which provoke oscillations in the trajectory of the tractor. These oscillations are especially noticeable when they are acquired at low speeds and with headings close to the direction of a coordinate axis (Figure 3).

On the basis of their professional experience with Agroguia^® [7] and Tractordrive^® [8], the authors state that these oscillations negatively affect the use of low-cost GPS receivers in GPS assisted-guidance systems for tractors.

1.3. Kinematic Model of a Tractor

A classic tractor has two front wheels that steer as well as two rear wheels that are straight-driven. The behavior of this kind of tractor vehicle is typically modeled following the tricycle vehicle model [19]. In this model, the system inputs are the vehicle speed modulus, u, and the front-wheel steering angle, δ. The tractor behavior can be described with a vector state, q, defined by the expression:

1.4. The Kalman Filter

The Kalman filter is an efficient, recursive, mathematical algorithm that processes, at each step, inaccurate observation input data and generates a statistically optimal estimate of the subjacent real system state, by employing a prediction model and an observation model [20].

The basic functioning of the filter is conceptualized into two stages. The first stage is called the prediction stage, as it produces an a priori system state estimate from the previous state, by using a system evolution prediction model. The second stage, known as the update stage, takes into account measurements in the system to produce an a posteriori state estimate, by correcting the previous a priori estimate. This two-stage process starts with an initial estimated state, ${\widehat{\mathit{x}}}_0^{-}$, and is repeated in a loop recursively until filtering ends (Figure 5).

Figure 5 summarizes the steps in each stage of the Kalman filtering process and it presents the matrices that are involved and the steps followed to implement the Kalman filter [20,21]. F_k is the state transition model matrix, which performs the prediction model. H_k is the observation model matrix, which maps the state vector space into the measurements vector space. ${\widehat{\mathit{x}}}_k^{-}$ is the a priori state estimate vector, resulting from the prediction stage. ${\widehat{\mathit{x}}}_k^{+}$ is the a posteriori state estimate vector, derived from the measurements update stage. z_k is the measurements vector obtained from the system sensors. K_k is the optimal Kalman gain matrix, which weights the importance of the innovation that introduces the measurements vector z_k in the update stage. ${\mathit{P}}_k^{-}$ is the a priori state covariance matrix, which provides the a priori estimation error covariance after the prediction stage. ${\mathit{P}}_k^{+}$ is the a posteriori state covariance matrix, containing the a posteriori estimation error covariance, given after the update stage. Q is the process noise covariance matrix of the prediction stage noise, which somehow ponders the weight of the process estimates. R is the observation noise covariance matrix of the update stage noise, which in a way ponders the degree of confidence in each one of the measurements. The relative weights become greater as the covariance matrix elements become smaller, meaning that the quantities involved are increasingly reliable.

1.5. The Tuning of the Kalman Filter

Kalman filter tuning consists of setting the relevant parameter values for the related noise covariance matrices Q, R, and ${\mathit{P}}_0^{+}$ [22]. Matrices Q, R, ${\mathit{P}}_k^{-}$, and ${\mathit{P}}_k^{+}$ reflect, respectively, the certainty or accuracy of the prediction model, the measurement model, the a priori prediction, and the a posteriori correction. The Kalman filter uses these matrices to weight the relevance and degree of confidence in predictions and measurements. The Kalman filter assumes that the involved noise characteristics have a zero-mean multivariate Gaussian distribution with covariance matrices Q and R for the process and measurements noises, respectively. Process noise is the random vector affecting the state x_k, meanwhile measurement noise is the random vector affecting the measurements vector x_k. Typically Q and R values must be estimated, in order to achieve statistically optimal filtering results. A covariance matrix contains the variance and cross-covariance information between each pair of elements of a random vector. In a general case, given a random vector, Y=[Y₁Y₂…Y_n]^T, its covariance matrix is expressed as:

There are two main approaches to address the filter tuning: static and dynamic. Static tuning approaches only tune the filter just before its use. Static procedures are based on techniques such as the Autocovariance Least Squares (ALS) method [24], performance-convergence cost functions [25], and general numerical optimization methods [26]. On the other hand, dynamic or adaptive tuning approaches tune the filter while it is operating, thus providing a self-tuning capability. Dynamic tuning methods are based on techniques such as Fuzzy Logic (FL) [27], Artificial Neural Networks (ANN) [28], Reinforcement Learning (RL) [29], the Dynamic Error System Analysis (DESA) method [30], and Genetic Algorithms (GA) [31,32]. Adaptive methods tend to yield a more robust behavior in cases such as those where noise characteristics change in time.

2.1. Kalman Filter Particularization in Tractor Guidance

The system presented in this study uses a particularization of the Kalman filter applied to GPS receiver data, in order to achieve path smoothing and partial restoration of the lost resolution in positioning data. The prediction model of this system is based on the tricycle kinematic model, seen in Equation (2), assuming that the vehicle speed and heading angle will change slowly. Additionally, the system also makes use of the measurements provided by a GPS receiver to update the predictions previously made.

The system takes the array of GPS measurements z_k(x_GPS, Y_GPS, θ_GPS,u_GPS)^T, as input variables, where x_GPS is the position in the X-axis, y_GPS is the position in the Y-axis, θ_GPS is the measured heading angle formed by the tractor heading and the positive X-semiaxis, and u_GPS is the speed modulus of the tractor. Given these inputs, z_k, to the system, a related system state-vector is defined as x_k = (x_k,y_k, θ_k,u_k)^T, which will be estimated by the Kalman filter. Figure 6 shows a black box diagram of this particular Kalman filter implementation.

Based on the system state, x_k, along with Equation (2), a prediction model is applied, which supposes that vehicle heading angle (θ) and speed (u) will change slowly. The a priori state estimate is obtained from the previous a posteriori state estimate as:

Labeling the a priori state estimate vector as ${\widehat{\mathit{x}}}_k^{-}={(x_k^{-},y_k^{-},{\theta }_k^{-},u_k^{-})}^T$ and the a posteriori state estimate vector as ${\widehat{\mathit{x}}}_k^{+}={(x_k^{+},y_k^{+},{\theta }_k^{+},u_k^{+})}^T$, Equations (4)–(7) are rewritten into the matrix form of the Kalman filter as:

It may be seen from Equation (9) that the prediction matrix has to be updated at every step, since the variable ${\theta }_k^{+}$ can change within each iteration, and the prediction matrix F_k depends on it in a non-linear way, which is not accounted for in the F_k matrix.

It is necessary to define the observation model matrix, H_k, in Equation (10), for the complete characterization of the proposed Kalman filter instance:

As the system state vector and measured magnitudes perfectly match, the particular observation model matrix H_k is chosen as the identity matrix in Equation (11):

2.2. Procedures

The Kalman filter was particularized for tractor guidance as detailed in Section 2.1, and the F and H system model matrices were obtained. Assuming statistical independence between all state variables, and assuming that the covariance matrices are time-invariant, the Q and R covariance matrices can be represented as Equation (12):

The Kalman filter was tuned using Monte Carlo Sampling techniques [33], by repeating the following two steps two million times. Initially, the elements of covariance matrices Q and R were randomly chosen between 0 and 6. Secondly, the particularized Kalman filter, using these covariance matrices, was applied over the 18 straight lines in Figure 7a, and then the Root Mean Square Error (RMSE) was computed, following Equation (13), over all straight lines, in which N is the number of samples, x̂_i the i-th estimate of the X axis position, ŷ_i the i-th estimate of the Y axis position, and x and y are the ideal position reference coordinates. The couple of matrices with the lowest Root Mean Square Error were taken as covariance matrices Q and R. Figure 8 and Equation (13) detail the Root Mean Square Error (RMSE) computing procedure.

The proposed Kalman filter performance was evaluated with artificial data, through the following steps (Figure 7b): (i) a set of 18 ideal straight trajectories (Figure 7a) were sampled at a 5 Hz update rate, following vehicle kinematics constraints, at a constant speed of 5 km/h, which is a typical speed for agricultural tasks; (ii) trajectories were quantized to a 14 × 18 cm grid, because in Valladolid, Spain, when low-cost GPS receivers that provide eight digits for latitude and nine for longitude in NMEA are employed, positions appears over this grid; (iii) the proposed Kalman filter model was applied to the quantized paths; and (iv) the performance improvements were evaluated in terms of distance with respect the ideal path using all the paths shown in Figure 7a, and in terms of course angle distribution by using a 60° heading angle straight path.

Finally, the proposed Kalman filter performance was evaluated with real GPS data by following the next steps (Figure 7b): (i) GPS receiver data were acquired at a 5 Hz update rate from a GPS placed on a tractor that traveled along straight path with a 60° heading angle, and the GPS positions were converted to UTM coordinates; (ii) the proposed Kalman filter was applied to the acquired data; and (iii) the performance achievements in course angle distribution were evaluated. All the required simulations and data processing were carried out in MATLAB^® programming environment.

2.3. Experimental System

The materials employed in the experimental tests of this article were: a low-cost GPS receiver, a precise GPS receiver, a laptop computer, and an agricultural tractor. The low-cost GPS receiver was a Navilock NL-402U with an U-blox LEA-5H chipset (Figure 9b), and it was employed to acquire the GPS trajectories to be processed in this article at a 5 Hz rate. A precise Trimble R4 GPS receiver, configured to use RTK corrections, was employed to position the stakes that were used to mark the paths in the plot (Figure 9a). The laptop was a Lenovo N3000 (Figure 9c) and it was used to acquire and store the trajectories from the low-cost GPS receiver. The agricultural tractor was a Kubota M6950DT (Figure 9a), and it was employed to perform the trajectories with the low-cost GPS receiver over its cab (Figure 9b).

Each one of the straight trajectories of the experimental tests was marked with three stakes and were joined by a cord. The stakes were driven into the center of the trajectory and at each extreme. Figure 9a shows one of the trajectories and one stake.

GPS receiver data were read and processed with an application running on the laptop. The application, developed using Labwindows CVI, read the NMEA sentences from the GPS receiver and transformed the geodetic data of the NMEA sentences to UTM Cartesian coordinates for analysis.