# A Low-Cost Positioning System for Parallel Tracking Applications of Agricultural Vehicles by Using Kalman Filter

Fangming Zhang, Ximing Feng, Yuan Li, Xiuqin Rao, Di Cui

► To cite this version:

Fangming Zhang, Ximing Feng, Yuan Li, Xiuqin Rao, Di Cui. A Low-Cost Positioning System for Parallel Tracking Applications of Agricultural Vehicles by Using Kalman Filter. 5th Computer and Computing Technologies in Agriculture (CCTA), Oct 2011, Beijing, China. pp.461-470, 10.1007/978-3-642-27281-3_52. hal-01351848

HAL Id: hal-01351848

https://inria.hal.science/hal-01351848

Submitted on 4 Aug 2016

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# A Low-cost Positioning System for Parallel Tracking Applications of Agricultural Vehicles by Using Kalman Filter

Fangming Zhang¹,², Ximing Feng², Yuan Li², Xiuqin Rao³, Di Cui²

¹ Ningbo Institute of Technology, Zhejiang University
1 Xuefu Road, School of Mechanical and Energy Engineering, Ningbo, Zhejiang, China
fangmingzhang@126.com

² Ningbo Yinzhou MicroAgriculture Technology Ltd., Ningbo, Zhejiang, China
feng_63589506@163.com,330204651@qq.com,

³ Zhejiang University, Hangzhou, Zhejiang, China
xqrao@zju.edu.cn

**Abstract.** A position-velocity (PV) model and a multi-sensor system, consisted of a consumer application GPS, a MEMS gyro, two encoders, and a turning angle sensor, was constructed for the positioning system. The two encoders augmented the positioning accuracy greatly that the fluctuation of vehicle position was greatly smoothed comparing with a GPS-only system. The minimal fluctuation was falling from 2.21 m to 0.52 m (east direction), from 0.68 m to 0.23 m (north direction). The maximum XTE was reduced from 2.5 m to 0.77 m, and the RMS value was improved to 0.22m. The GPS bias error was the major difficulty to produce better performance.

**Keywords:** Positioning system, GPS, Kalman filter, parallel tracking, low-cost

## 1 Introduction

Parallel tracking is the main operation method of agricultural vehicles. Global Positioning System (GPS) acts as an important role in navigating agricultural vehicles with parallel tracking. Some researches [1-3] have been reported to use high-accuracy GPS receivers, Real Time Kinematic Global Positioning System (RTK-GPS) or Carrier-Phase Differential GPS (CPD-GPS), to develop automated agricultural vehicles. However, both RTK-GPS and CPD-GPS are too expensive for its actually application in agriculture. Low-cost consumer application GPSs are now widely used in automobile industry, i.e., car navigator, path tracking, but their position accuracy, 2-3 meters, could not be satisfied with requirement of agriculture application.¹

Kalman filter has been extensively used to smooth raw DGPS signals [4-5], which improved the positioning accuracy, and more importantly, it provided reliable positioning information during a short period of time when the GPS signal is lost. For

¹ This paper is supported from the Science Foundation of NingBo (Projects No. 2010A610140)
example, Will [1] constructed a position-attitude (PA) model-based extended Kalman filter. Han et al. [4] set up a position-velocity (PV) model for the Kalman filter. Guo et al.[5] integrated IMU and DGPS data and formed a position-velocity-attitude (PVA) model for fusion algorithms. Fiengo et al.[6] developed a model for vehicle by combining of a GPS, speed sensor, and a gyro sensor. Guo [7] developed a GPS/IMU/magnetometer integrated system with Kalman filtering for vehicles. The author [8] had integrated a vision sensor and two encoders to construct an extended Kalman filter. Those results showed that fusion system could decrease the cost of sensors while kept the necessary accuracy for agricultural applications.

As sensor technology is developing rapidly, low-cost positioning system shows
possible and attractive for agricultural vehicles. Rong Zhu et al.[9] developed an
extended Kalman-based fusion algorithm for attitude estimation by using inexpensive
micromachined gyroscopes, accelerometers and magnetometers. Akira Mizushima et
al.[10] used low-cost sensors, three vibratory gyroscopes and two inclinometers, to
estimate tilt angles (roll and pitch) by least-squares method. The drift error of the
gyroscopes was estimated using the inclinometers. Ndjeng et al.[11] solved the
problem of outdoor vehicle localization with Interacting Multiple Model (IMM) and
Extended Kalman Filter (EKF) approaches, which allows the method to be optimized
for highly dynamic vehicles with low-cost IMU-odometer-GPS composition. Zhi
Shen et al.[12] integrated low-cost sensors, a MEMS-grade gyroscope, a vehicle built-
in odometer, and a GPS to provide 2D navigation for land vehicles. Fast Orthogonal
Search is suggested for modeling the higher order of reduced inertial sensor system
RISS errors.

The objective of this research was developing a positioning system with low-cost
guidance sensors for agricultural parallel tracking application.

## 2. Materials and Methods

A multi-sensors system was constructed on a rice transplanter (ZP60, ISEKI, Japan) as shown in figure 1, which consists of a consumer application GPS receiver (U-blox LEA-5S, Zoglab Inc., China), a MEMS gyro as heading angle sensor (GX1, Xunjie Inc., China), two encoders as speed sensor (E6B2-CWZ6C, Omron Inc., Japan), and a precision potentiometer as turning angle sensor (Copal N35, Japan). The antenna of the U-blox GPS was mounted on a rigid frame in the front the vehicle 2.5 m above ground level. The receiver transmitted data at 1 Hz with Baud rate of 9600 bps. A RTK-GPS (S82E, South surveying & mapping instrument Inc, china) was used to record track of the vehicle with its antenna mounted beside the U-blox one. The gyro, measured angular velocity for yaw direction at maximum ability of ±70°/s, was installed on the body of vehicle. Two encoders, outputted 360 pulses per round, were driven by the left and right rear wheel through a pair of gear transmission with rate of 1:1. The potentiometer, whose resistance is 5KΩ within rotary angle of 345°, was installed under the turning axis. A computer system consists of one embedded central computer (ECC) (ARM S3C2440 Developing board, Tianxiang Inc., China) and five electric circuit units (ECU) (PIC 16F873A Developing board, Microagriculture Inc., China), which were connected by a RS-485 net. The 1st ECU acted as transferring
data from the GPS receiver to the ECC. The 2nd and 3rd ECUs acted as sampling tracking speed from the left encoder and right encoder at the left and right rear wheel correspondingly. The 4th ECU acted as sampling turning angle from the potentiometer and sampling heading angle from the gyro sensor, and the last one acted as controlling a step motor to steering the rice transplanter. The outputs of these sensors were acquired through a 10-bit analog/digital converter. The total cost of this attitude sensor was approximately $250, only 1/5 of a DGPS.

<figure><img src="image_2.png" /><figcaption>Fig. 1. (a) configuration of the multi-sensor system; (b) the test platform on the ISEKI ZP60</figcaption></figure>

The GPS signal triggered all ECUs to begin sampling signal from corresponding sensors at the same time. When the ECU1 received the '$GPGGA' frame, it extracted latitude and longitude and then sent them into the RS-485 net, which could be received by the ECC and all other ECUs, while the latter would sampled signal from corresponding sensors. The ECC processed the latitude and longitude after a button in its monitor, 'START', was pressed, and it got all sensor data by serial communication between one ECU and itself. The ECC transformed those data into significative decimal value, such as turning angle, left wheel speed. All data were then sent to the Kalman Filter for further processing.

A local coordinate system was set up that the 1st point in every test was thought as the origin point, and the x coordinate pointed to east direction and y coordinate pointed to north direction. Coordinate x and y of any position were transformed from the latitude and longitude of GPS according to Chang's method [13]. Furthermore, the rice transplanter was modeled as a three-tyre vehicle as shown in figure 2. Relying on kinematics analysis, a position-velocity (PV) model was set up for the Kalman filter. It shows the angle between the road and y-axis is fixed value, α, while the heading angle, Ψ, offset, e, and speed, v₁ and v₂, changed when driving the vehicle.
<figure><img src="image_3.png" /><figcaption>Fig. 2. The vehicle model, which is transformed into three-tyre vehicle, locates on the local coordinates. Point ‘C’ represents the estimated position point.</figcaption></figure>

The discrete Kalman filter recursive equations are [4]:

$$X_{k+1} = \Phi_k X_k + w_k \quad (1)$$

$$Z_k = H_k X_k + v_k \quad (2)$$

where

$X_k$ is the (n x 1) process state vector at time $t_k$

$\Phi_k$ is the (n x n) state transition matrix

$w_k$ is the (n x 1) process noise vector with a known covariance $Q_k$

$Z_k$ is the (m x 1) measurement vector at time $t_k$

$H_k$ is the (m x n) measurement connection matrix

$v_k$ is the (m x 1) measurement noise vector with a known covariance $R_k$.

Equation 1 is the process model, and equation 2 is the measurement model. Since the objective of this study was to improve the 2-D positioning accuracy, four state variables were set as following:

$$X_k = [x_k, y_k, \psi_k, v_k]^T \quad (3)$$

where

$x_k$ and $y_k$ are local coordinates to be estimated.

$\psi_k$ is heading angle to be estimated.

$v_k$ is velocity to be estimated.

The state transition matrix was:
$$ \Phi_k = \begin{bmatrix} 1 & 0 & 0 & T \sin(\psi_k + c \cdot \Delta\delta) \\ 0 & 1 & 0 & T \cos(\psi_k + c \cdot \Delta\delta) \\ 0 & 0 & 1 & T \tan(\Delta\delta)/W \\ 0 & 0 & 0 & 1 \end{bmatrix} \quad (4) $$

The raw receiver outputs, $x_{GPS}$ and $y_{GPS}$ transformed from latitude and longitude by GPS, heading angle from gyro, speed from encoders, and turning angle from potentiometer, are the measurement variables. The measurement vector and the measurement connection matrix are:

$$ Z_k = [x_{GPS}, y_{GPS}, \psi_{GYRO}, v_{GPS}, \delta_{Turntyre}]_k^T \quad (5) $$

$$ H_k = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{bmatrix} \quad (6) $$

The numerical solution to the discrete Kalman filter model is a step-wise procedure [4]:

Step 1: Compute the Kalman gain, $K_k$:

$$ K(k) = P(k|k-1)H^T[R + H \cdot P(k|k-1)H^T]^{-1} \quad (7) $$

where $K_k$ is the Kalman gain, $P_{k,k-1}$ is the initial error covariance matrix, and $R$ is the covariance matrix for the measurement noise vector.

Step 2: Update the estimate, $\hat{X}_{k,k}$, with the measurement, $Z_k$:

$$ \hat{X}(k|k) = \hat{X}(k|k-1) + K(k)[Y(k) - H\hat{X}(k|k-1)] \quad (8) $$

where $\hat{X}_{k,k-1}$ is the updated estimate, and is a priori estimate.

Step 3: Compute the error covariance, $P_{k,k}$, for the updated estimate:

$$ P(k|k) = [I - K(k) \cdot H]P(k|k-1) \quad (9) $$

where $P_{k,k-1}$ is a priori error covariance matrix.

Step 4: Project ahead:

$$ \hat{X}(k+1|k) = \Phi(k)\hat{X}(k|k) \quad (10) $$

$$ P(k+1|k) = \Phi(k)P(k|k)\Phi(k)^T + Q_k \quad (11) $$

where $\hat{X}_{k+1,k}$ and $P_{k+1,k}$ are the projected estimate and projected error covariance matrix that the next iteration requires.

In the application of the above procedure, three matrices, the process noise covariance matrix $Q_k$, the measurement noise covariance matrix $R_k$, and the initial
error covariance matrix $P_{k,k-1}$, need to be defined prior to the start of the iteration. We derived these matrices by trial:

$$Q_k = \begin{bmatrix} 0.033 & 0 & 0 & 0 \\ 0 & 0.033 & 0 & 0 \\ 0 & 0 & 0.003 & 0 \\ 0 & 0 & 0 & 0.0025 \end{bmatrix} \qquad (12)$$

$$R_k = \begin{bmatrix} 6.25 & 0 & 0 & 0 & 0 \\ 0 & 6.25 & 0 & 0 & 0 \\ 0 & 0 & 0.25 & 0 & 0 \\ 0 & 0 & 0 & 0.025 & 0 \\ 0 & 0 & 0 & 0 & 0.025 \end{bmatrix} \qquad (13)$$

$$P(k | k-1) = \begin{bmatrix} 2 & 0 & 0 & 0 \\ 0 & 2 & 0 & 0 \\ 0 & 0 & 0.1 & 0 \\ 0 & 0 & 0 & 0.01 \end{bmatrix} \qquad (14)$$

The updated estimate, equation 8, is the best estimate of the current position.

The first experiment was done on a road (50 meter long) located in campus of Ningbo Institute of Technology, Zhejiang University on Jan. 29, 2011, where tracks of right wheel were drew by Chalks, and another one was done on a road (90 meter long) located in Jiangshan, NingBo on July 1, 2011, where tracks of the center of the vehicle were recorded by the RTK-GPS. The vehicle was driven along two parallel rows, transecting approximately 2.2 m in the former experiment, and 4.0 m in the later one. Data were recorded with a 1-s interval in all experiments. Every experiment was repeated 2 times with 3 travel speeds, 0.25m/s, 0.73 m/s, and 1.1 m/s.

One program, including serial communication and Kalman filter module, was written, compiled and run real time in the ECC, and another Matlab program was written for data analysis. The record of chalk or RTK-GPS was used as a baseline (reference) to evaluate the performance of the filters. A cross-track error (XTE) [4] is defined as the distance between the currently measured GPS position and the desired track. Minimal fluctuation was defined as coordinates jumping in east direction or north direction to evaluate performance of the kalman filter when RTK-GPS was not available.

### 3. Results and Discussion

Figure 3 shows one results by drawing line on ground. It shows that the Kalman filter improves the positioning system. The state variables, coordinates x, y, and heading angle $\Psi$ were smoothed. In these experiments, the minimal fluctuation was falling from 2.21m to 0.52 m (east direction), from 0.68 m to 0.23 m (north direction). However, the absolute bias still could not be evaluated by drawing line on ground.
<figure><img src="image_4.png" /><figcaption>**Fig. 3.** Experiment data by drawing line on ground, (a)original GPS position, estimated position and track of right wheel; (b) original gyro data and estimated heading angle; (c) left encoder and right encoder data; (d) turning angle sensor data</figcaption></figure>

The truth was exposed when receiving position data both from the U-blox GPS and the RTK-GPS at the same time. Figure 4 shows one result of this kind of experiments. The first point got by the RTK-GPS was also the original point of the U-blox coordinates system, where the first point got by U-blox deviated to coordinates (2.610747, 2.771199). Though positioning coordinates were smoothed by the Kalman filter, they deviated in most time that the mean bias was 2.32m, and the RMS is 0.72m, and the difference between the maximum and minimum XTE is 5.30m. The two lines formed by the U-blox GPS are not parallel. Compared with the former experiments, bad performance might be caused by close distance of the two antennas.
<figure><img src="image_5.png" /><figcaption>**Fig. 4.** experiment data by using the two GPSs together. The track go and back are nearly parallel, but whether the U-blox GPS or the estimated coordinates are not parallel.</figcaption></figure>

Results shows high accuracy sensors improve performance of positioning system. A close look at the GPS data we found the least GPS data changing is 1.5 m in x axis, and 0.45 m in y axis, the original GPS output could hardly be used to navigate an agriculture vehicle. After Kalman filter, the average data changing decreases to 0.45 m in x axis, and 0.20 m in y axis. This improvement owes to two encoders as they provide high accuracy tracking distance. The turning angle sensor is another high precision sensor; however, its output was not used as state variable yet, so it hardly contributed to improve positioning system. If it will be added as a state variable in future work, it will then bring more positive effect.

Another way to improve accuracy is speeding up the update of sensors. The maximum ability of this GPS receiver is 4Hz, while the working frequency was 1Hz only in this experiment due to stability of serial communication. Moreover, the gyro could update its output in 10 Hz. So software should be improved in future work to obtain more accuracy positioning data.

This experiment also certified that combine of some low cost guidance sensors could produce high accuracy position. The most expensive sensor in this system is the gyro, $100, and the total sensor cost is less than $250. It is important to use one or two low cost, high precision sensor, such as encoder, to improve the performance.

The experiments showed that Kalman filter does not work when the vehicle is turning around which Han [4] mentioned. In our experiments, estimated position might move to wrong direction when starting turning, and heading angle will lag behind the gyro data greatly. Solution to this difficulty is closing the Kalman filter when turning, and initializing it when it goes into a new row. If using a local coordinate system, we could set the first GPS data in the first row as the origin, and set the initial state variable with changed x and y, as well as adjust the state transition matrix, whose sign of encoder should be reversed.
## 4. Conclusions

A low-cost positioning system, consisted of a consumer application GPS receiver, a MEMS gyro, two encoders and one turning angle sensor, was developed to improve positioning accuracy of the GPS by using Kalman filter. A computer system embedded on vehicles was constructed, which composed of an ECC (ARM embed computer), 5 ECUs (PIC16 microcomputer), and a RS-485 net. Local coordinates, heading angle, and vehicle speed were set as state variables in the Kalman filter. Experiment results show positioning coordinates got by the GPS were improved after filter processing that they were smoothed, but bias of the GPS made the estimated coordinates uncertainty. The minimal fluctuation was falling from 2.21 m to 0.52 m (east direction), from 0.68 m to 0.23 m (north direction). The maximum XTE was reduced from 2.5 m to 0.77 m, and the RMS value was improved to 0.22m. However, the Kalman filter could not remove bias of GPS. In addition, the proposed Kalman filter makes inaccurate position estimates when turning around. Further work should be on reducing the GPS bias error for parallel tracking applications.

**Acknowledgments.** The authors wish to express their sincere thanks for the financial support from the Science Foundation of NingBo (Projects 2010A610140), Agriculture R&D project of Yingzhou (3rd batch, 2010), Machine R&D project of Ningbo Agriculture Machinery Bureau (2010-2011), and Initial Research Foundation of NIT.

## References

1. Will, J.D.: Sensor fusion for field robot localization. Ph.D thesis, University of Illinois at Urbana-Champaign, (2001)

2. Noguchi, N., Reid, J.F., Zhang, Q., Will, J.D., Ishii, K.: Development of Robot Tractor Based on RTK-GPS and Gyroscope. 2001 ASAE Annual International Meeting, Paper Number: 01-1195, July 30-August 1, Sacramento, California, USA, (2001)

3. Bell, T.: Automatic tractor guidance using carrier-phase differential GPS. Computers and Electronics in Agriculture, 25,53-66(2000)

4. Han, S.f, Zhang, Q., Noh, H. K.: Kalman filtering of GPS positions for a parallel tracking application. Transactions of the ASAE, Vol. 45(3): 553-559, (2002)

5. Guo, L., He, Y., Zhang, Q., Han, S.: Real-time tractor position estimation system using a kalman filter. Journal of CSAE, Vol.18(5):96-101, (2002)

6. Fiengo, G., Domenico, D.D., Glielmo, L.: A hybrid procedure strategy for vehicle localization system: Design and prototyping. Control Engineering Practice. Vol.17: 14-25, (2009)

7. Guo, H., Yu, M., Zou, C., Huang, W.: Kalman filtering for GPS/magnetometer integrated navigation system. Advances in Space Research, Vol.45:1350-1357, (2010)

8. Zhang, F., Ying, Y., Zhang, Q., Shin, B.: Vision-based Position System for Agricultural Vehicle Using Extended Kalman Filter. ASAE Annual International Meeting, Paper No. 061157, 9 - 12 July, Portland, Oregon, (2006)

9. Zhu R., Sun, D., Zhou, Z., Wang, D.: A linear fusion algorithm for attitude determination using low cost MEMS-based sensors. J. Measurement. 40,322-328(2007)
10 Mizushima, A., Ishii, K., Noguchi, N., Matsuo, Y., Lu, R.: Development of a low-cost attitude sensor for agricultural vehicles. J. Computers and Electronics in Agriculture. 76, 198-204(2011)

11 Ndjeng, A., Gruyer, D., Glaser, S., Lambert, A.: Low cost IMU-Odometer-GPS ego localization for unusual maneuvers. J. Information Fusion 12, 264-274(2011)

12 Shen, Z, Georgy, J., Korenberg, M.J., Noureldin, A.: Low cost two dimension navigation using an augmented Kalman filter/Fast Orthogonal Search module for the integration of reduced inertial sensor system and Global Positioning System. Transportation Research Part C. Article in press (2011)

13 Chang, C., Chen, Y., Xiao, T.: Research of GPS used in Farm machines guidance system (in Chinese). Annual meeting of CSAE, 952-957 (2006)