sensors

Article
Heading Estimation of Robot Combine Harvesters
during Turning Maneuveres
Md Mostafizar Rahman 1,2 and Kazunobu Ishii 3, *
1 Graduate School of Agriculture, Hokkaido University, Kita-9, Nishi-9, Kita-ku, Sapporo 060-8589, Japan;
mmr.fet@gmail.com
2 Department of Food Engineering and Tea Technology, Shahjalal University of Science and Technology,
Sylhet 3114, Bangladesh
3 Research Faculty of Agriculture, Hokkaido University, Kita-9, Nishi-9, Kita-ku, Sapporo 060-8589, Japan
* Correspondence: ici@bpe.agr.hokudai.ac.jp; Tel.: +81-011-706-3847




Received: 9 March 2018; Accepted: 17 April 2018; Published: 1 May 2018


Abstract: Absolute heading is an important parameter for a robot combine harvester or a robot
tracked combine harvester, especially while it is turning, but due to the rapid turning of robot combine
harvesters, its inertial measurement unit gives a gyro measurement bias that causes heading drift.
Our research goal is to estimate the absolute heading of robot combine harvesters by compensating
this gyro measurement bias during non-linear turning maneuvers. A sensor fusion method like the
extended Kalman filter combined with the tracked combine harvester dynamic model and sensor
measurements was used to estimate the absolute heading of a robot combine harvester. Circular,
sinusoidal and concave shapes were used to evaluate the estimated heading produced by the sensor
fusion method. The results indicate that the estimated heading is better than measured heading
which was calculated from the integration of yaw rate gyro measurements, and the root mean square
errors (RMSEs) for estimated headings are smaller than the measured headings. In practics, the target
of this paper is thus the estimation of a heading or absolute heading that is bias compensated,
and can be further used to calculate the exact crop periphery for automatic path planning of robot
combine harvesters.

Keywords: tracked combine harvester; dynamic model; extended Kalman filter; global positioning
system; inertial measurement unit; crop periphery

1. Introduction
Unlike wheeled vehicles, tracked vehicles are widely popular due to their non-linear contact
characteristics between the tracks and the ground, which allows them to be operated under adverse
field conditions for agricultural production, and allows turning at high speed with a small turning
radius or higher steering command. Under autonomous conditions, the tracked vehicle has a global
positioning system and inertial sensor for providing the vehicle state and direction, but when this
tracked vehicle is turning, the inertial sensor reading has measurement uncertainties. Since the vehicle
direction is more important for autonomous guidance and other navigation purposes, it is necessary to
compensate this uncertainty of the inertial sensor measurements.
The main controlling feature of auto guidance is to steer the vehicle to follow a desired path
automatically, which requires a proper guidance system able to detect the vehicle’s position and
direction, create proper steering signals, and steer the vehicle according to the signals [1].There are
different guidance sensing systems, including global positioning system, inertial sensors, geomagnetic
direction sensors, machine vison and laser scanners which are used to find the control parameters
of the autonomous vehicles such as heading and offset [2–5]. The functional characteristics of each
sensor provides the desired information, which contains erroneous measurement readings due to

Sensors 2018, 18, 1390; doi:10.3390/s18051390 www.mdpi.com/journal/sensors

Sensors 2018, 18, x FOR PEER REVIEW 2 of 12

Sensors 2018, 18, 1390 2 of 12

characteristics of each sensor provides the desired information, which contains erroneous
measurement readings due to noise, measurement errors, and time delays. In general, a single sensor
noise, measurement
is not able to provideerrors,
enough and time delays.
information, In general,
whereas a single sensor
multi-sensor integrationis notcanableprovide
to providemoreenough
useful
information, which whereas multi-sensor
is more helpful and integration
informativecan provide
than what morecanuseful information,
be observed using awhich
singleissensor.
more
helpful and informative
This information needs to than what in
be fused can be observed
a way that reducesusingsensor
a single sensor. This
uncertainties and information
the additional needs to
task
be
of fused in a way must
interpretation that reduces
be performedsensor uncertainties
[6]. There areand the additional
different approaches taskused
of interpretation
for sensor fusion must be to
performed [6]. There are different approaches used for sensor fusion to obtain
obtain the position and direction of vehicles, such as complementary filters [7] or Kalman filters with the position and direction
of vehicles,
various such as complementary
architectures [8–13], particle filters filters [7]
or or Kalman Monte
sequential filters with
Carlovarious
methods architectures
[14–18]. Since [8–13],
the
particle
dynamicfilters
motion or sequential
of a vehicleMonte Carlo methods
is non-linear, [14–18].
a non-linear Since the
dynamic model dynamic motion of
and extended a vehicle
Kalman is
filter
non-linear,
are commonly a non-linear
used fordynamic
navigation model and extended
purposes. Kalman filter
For instances, Rohac are[19]
commonly
addressed useda for navigation
cost-effective
solution by
purposes. Forusing a non-linear
instances, Rohac [19] observer and extended
addressed Kalman
a cost-effective filter with
solution commercial
by using gradeobserver
a non-linear inertial
sensors,
and and studied
extended Kalmanthe performances
filter with commercial of different approaches
grade inertial sensors,to obtain navigation
and studied solutions with
the performances of
robustness
different to GNSS outages.
approaches to obtain Noguchi
navigation [20] solutions
developedwith a guidance
robustnesssystem basedoutages.
to GNSS on the real time global
Noguchi [20]
positioninga system
developed guidance (RTK-GPS),
system based geomagnetic
on the realsensor (GDS)
time global and machine
positioning systemvision by using geomagnetic
(RTK-GPS), an extended
Kalman
sensor filterand
(GDS) method
machinewhich provided
vision by using thean
most appropriate
extended Kalman vehicle headingwhich
filter method in realprovided
time. Alatise and
the most
Hancke [21]vehicle
appropriate used anheading
extended Kalman
in real time.filter withand
Alatise inertial
Hancke sensors and camera
[21] used an extended to estimate
Kalman the position
filter with
and direction
inertial sensorsofand a mobile
camerarobot. With athe
to estimate non-linear
position and extended Kalman
direction filter, arobot.
of a mobile trackedWith vehicle model
a non-linear
and sensorKalman
extended measurements are used
filter, a tracked to estimate
vehicle modelthe andtrajectory, orientation and
sensor measurements areother
used soil parameters
to estimate the
for small-scale
trajectory, trackedand
orientation vehicles
other [22].
soil parameters for small-scale tracked vehicles [22].
An autonomous
autonomous tracked trackedcombinecombineharvester
harvesterdeveloped
developed byby Zhang
Zhang [23][23]
waswas used to cut
used to wheat
cut wheat and
paddy
and rice inrice
paddy realintime
realby using
time byan appropriate
using harvesting
an appropriate map which
harvesting map is created
which is based
createdon abased
real time on
aglobal positioning
real time global system
positioning(RTK-GPS)systemand inertial measurement
(RTK-GPS) units (IMUs); before
and inertial measurement harvesting,
units (IMUs); the
before
outside cropthe
harvesting, near to headland
outside crop near is cut twice or thrice
to headland is cutby the tracked
twice or thricecombine harvester,
by the tracked and during
combine harvester,this
timeduring
and the sensor measurements
this time are logged for are
the sensor measurements making
logged a for
navigation
making amap. Generally,
navigation map.the tracked
Generally,
combine
the trackedharvester
combine makes turns makes
harvester at moderateturns toat high speedtowith
moderate higha small
speedturning
with a smallradius, which radius,
turning is very
popular
which is with
very farmers,
popular and withthis turning
farmers, andposition is represented
this turning positionby is arepresented
circle marked by in Figuremarked
a circle 1. During in
this turn,
Figure the measured
1. During this turn, heading of a tracked
the measured heading combine harvester
of a tracked combinecontains drift contains
harvester errors, which result
drift errors,
from the
which IMUfrom
result gyrothemeasurement bias. To get anbias.
IMU gyro measurement absolute heading,
To get this bias
an absolute needs to
heading, be bias
this compensated
needs to
by compensated
be using a tracked bycombine harvestercombine
using a tracked model and sensormodel
harvester fusionand method.sensor In fusion
this research,
method.theInmain this
contributions
research, the mainare the estimated absolute
contributions heading of
are the estimated trackedheading
absolute combineofharvester during non-linear
tracked combine harvester
conditions
during (especially
non-linear high (especially
conditions speed turns with
high speedsmall
turns turning radii)
with small by using
turning radii)a by tracked
using acombine
tracked
harvesterharvester
combine dynamicdynamic model which modelwas developed
which by us based
was developed by usonbased
a realon time global
a real timepositioning system
global positioning
(RTK-GPS)
system and inertial
(RTK-GPS) measurement
and inertial measurement units (IMU). For more
units (IMU). details
For more readers
details readersshould see see
should [24].[24].
In
practice,
In practice,this estimated
this estimated heading
headingcan canfurther
furtherbebeusedusedtotoobtain
obtainthetheexact
exactcropcropperiphery
periphery for calculating
the harvesting map of aa robot robot combine
combine harvester
harvester (for(for more
more details
details see see [25]).
[25]).

Figure1.1.AApolygonal
Figure polygonalfield
fieldwith
withaacircle
circle representing
representing the turning
turning area
areaof
ofthe
thetracked
trackedcombine
combineharvester.
harvester.
Sensors 2018, 18, 1390 3 of 12
Sensors 2018, 18, x FOR PEER REVIEW 3 of 12

2. Materials
2. Materials and
and Methods
Methods

2.1. System Platform and Sensors

modelAG1100
A model AG1100trackedtracked combine
combine harvester
harvester (Yanmar
(Yanmar Co., Japan)
Co., Ltd, Ltd, Japan)
was usedwasasused
a primeas research
a prime
research platform,
platform, which was which was equipped
equipped with a Real withTimea Real Time Kinematic
Kinematic Global Positioning
Global Positioning System (RTK-
System (RTK-GPS) and
GPS) and
Inertial Inertial Measurement
Measurement Unit (IMU)Unit (IMU) in
as shown asFigure
shown2.inThis Figure 2. This
tracked trackedharvester
combine combineisharvester
converted is
converted
into a robotinto a robot
combine combinewhich
harvester harvester which
is fully is fully by
controlled controlled
a CAN bus, by aandCAN thebus, andofthe
speed thespeed
trackedof
the tracked
combine combine
harvester harvester
ranges −2.0~2.0
fromranges fromm/s,−2.0~2.0 m/s, approximately.
approximately. This harvester This can
harvester
be used canto be used
harvest
to harvest
cereal cropscereal crops like
like paddy rice paddy rice and
and wheat. Thewheat.
VN-100The IMU VN-100 IMU (VectorNav
(VectorNav Technologies, Technologies,
Dallas, Texas, Dallas,
USA)
Texas,
is USA) ishigh
a miniature, a miniature,
performance high performance
Inertial Measurement InertialUnit
Measurement
and AttitudeUnit and Attitude
Heading ReferenceHeading
System
Reference
(AHRS) System
which (AHRS)
is used which isthe
to measure used to measure
heading the heading
and angular rate ofand
the angular
tracked rate of the
combine tracked
harvester.
combine
Its dynamic harvester.
accuracy Itsfor
dynamic
the yaw accuracy
rate is 1.0 ◦
for theRMS,yaw rate is 1.0°
heading’s RMS, heading’s
dynamic accuracy:dynamic ◦
2.0 RMSaccuracy:
and the
◦ , noise error: ◦ /s/ √ ◦ /h.
2.0° RMS
gyro has an and the gyroerror:
alignment has an±0.05
alignment density: ±0.05°, noise
<0.0035 density: <0.0035°/s/√Hz,
Hz, in-run in-run
bias stability: <10bias
stability:
This IMU,<10°/h.
consistingThisofIMU,
threeconsisting
accelerometers,of three accelerometers,
three magnetometers three
andmagnetometers
three rate gyros,and three nine
provides rate
gyros,which
axes, provides nine axes,
are sampled at which
a rate ofare200sampled
Hz through at a rate of 200
a USB serialHzport.
through
On the a USB
otherserial
hand,port. On the
the update
otherofhand,
rate the update
the RTK-GPS ratetoof20the
is up Hz,RTK-GPS
althoughisfor uplogging
to 20 Hz, of although
the RTK-GPS for logging
output, of thethe RTK-GPS
update rate
output, the update rate is maintained at 5 Hz or 10 Hz and the Baud
is maintained at 5 Hz or 10 Hz and the Baud rate was 115,200, where the IMU also matches that rate was 115,200, where the IMU
also matches
update rate. ThethatRTKupdate rate. The
correction RTK
signal correction
was calculated signal
fromwas calculated
a Virtual from Station
Reference a Virtual Reference
(VRS) via an
Station (VRS)
Internet via antoInternet
connected connected
the on-board computerto thethaton-board
logs the computer
data from that
thelogs the data
RTK-GPS from the
receiver RTK-
through
GPS
an receiver
RS232C through
serial port.anThese
RS232C serialmeasurements
sensor port. These sensor weremeasurements
logged into the were logged
control PCinto thetracked
of the control
PC of theharvester
combine tracked combine
as a GGAharvester
header forasRTK-GPS
a GGA header and VNIMU for RTK-GPS
header for and VNIMU
IMU. The GGAheader for IMU.
header from
TheRTK-GPS
the GGA header sensor from
givesthetheRTK-GPS
position of sensor gives the
the tracked position
combine of the whereas
harvester, tracked combine
the VNIMU harvester,
header
whereas the
provides VNIMU header provides
bias-uncompensated bias-uncompensated
IMU measurements IMU measurements
such as magnetic, accelerationsuch and as magnetic,
angular rate
acceleration andtemperature
measurements, angular rate andmeasurements,
pressure. In this temperature
research, only andposition
pressure. andInbias
thisuncompensated
research, only
positionrate
angular and data
bias uncompensated
were considered angular to estimate ratethe
data were considered
absolute heading oftorobot
estimate the absolute
combine harvester heading
under
of robot combine
non-linear harvester under non-linear conditions.
conditions.

View of the robot combine harvester equipped with RTK-GPS and IMU
Figure 2. View IMU sensors.
sensors.

2.2. Tracked Combine

Combine Harvester
Harvester Model
Model

Figure 3 shows the free body diagram of dynamic model for the tracked combine harvester
moving on a general plane, turning to the left or counter-clockwise [24,26,27]. Its acceleration is in the
counter-clockwise [24,26,27].
positive 𝑥𝑐 , 𝑦𝑐 and 𝜑 directions. The external thrusts and resistive forces acting on the tracked
combine harvester are 𝐹𝑅 , 𝐹𝐿 and 𝑅𝑅 , 𝑅𝐿 , respectively. The value 𝑓𝑦 indicates the lateral friction
Sensors 2018, 18, 1390 4 of 12

positive xc ,18,ycxand
Sensors 2018, FOR ϕ directions.
PEER REVIEW The external thrusts and resistive forces acting on the tracked combine 4 of 12
harvester are FR , FL and R R , R L , respectively. The value f y indicates the lateral friction force due to the
force due
effect to thesoil
of lateral effect of lateral
shear. Figuresoil
3 isshear. Figure 3inisthe
represented represented in the global
global reference reference
frame XYZ, which frame XYZ,
indicates
which
the indicates
tracked combine the harvester
tracked combine harvester
turns around turns aroundcenter
an instantaneous an instantaneous center
of rotation (ICR). Theofangle
rotation
β is
(ICR). sideslip
called The angle angle𝛽and is iscalled sideslipfrom
determined angle and is determined
the velocity axis xc of 𝑉the
from the velocity
Vc and the longitudinal 𝑐 and the
tracked
longitudinal
combine axis 𝑥It𝑐 isofassumed
harvester. the tracked combine
that the normalharvester. It is assumed
pressure distribution along that
thethe normal
track pressure
is non-uniform,
distribution
and along the
the coefficient track isresistance
of lateral non-uniform, andconstant.
µ is not the coefficient center𝜇ofisrotation
of lateral resistance
The instantaneous not constant.
must
The instantaneous center of rotation must shift forwards of the tracked combine
shift forwards of the tracked combine harvester centroid by the amount of D, as shown in Figure 3. harvester centroid
by the
This amount ofshifting
longitudinal D, as shown D dependsin Figure
on the3.tracked
This longitudinal shiftinglateral
combine harvester D depends on the[27].
acceleration tracked
D is
combine harvester
required to develop lateral
a netacceleration
lateral force[27].
thatDaccelerates
is requiredtheto develop
trackedacombine
net lateral force thattowards
harvester acceleratesthe
the tracked combine harvester towards the instantaneous center of rotation,
instantaneous center of rotation, and also minimizes the resistive yawing moment [28]. The centrifugaland also minimizes the
resistive
force yawing
Fc acting onmoment
the tracked [28].combine
The centrifugal
harvesterforce 𝐹𝑐 acting
is given on the
in Figure tracked
3. We combine this
also described harvester
tracked is
given inharvester
combine Figure 3.model We also baseddescribed
on sensor this tracked combine
measurements so that it harvester model
can be further usedbased on sensor
for autonomous
measurements
navigation so that[24].
purposes it can be further used for autonomous navigation purposes [24].

Figure 3. Free body diagram of the tracked combine harvester dynamic model.

For aa tracked
For tracked combine
combine harvester
harvesterof mass 𝑚
ofmass and aa moment
m and moment ofof inertia
inertia about
about the
the center
center of mass 𝐼,
of mass I,
the equations of motion can be written in the body reference frame by using Equations
the equations of motion can be written in the body reference frame by using Equations (1)–(3), (1)–(3),
respectively:
respectively:
..
m𝑚𝑥̈
x c 𝑐==FR𝐹𝑅++F𝐹
L 𝐿−−R𝑅R𝑅−
−R𝑅L𝐿 − F
𝐹𝑐c sin𝛽
sin β (1)
(1)
..
myc = Fc cos β − µmg (2)
𝑚𝑦̈𝑐 = 𝐹𝑐 cos𝛽 − 𝜇𝑚𝑔 (2)
.. [( FR − R R )𝑅− ( F𝐿L−𝑅−𝐿 )]𝐵
R L )] B
I ϕ =𝐼𝜑̈ = [(𝐹𝑅 −𝑅 )−(𝐹
−𝑀− 𝑟
Mr (3)
(3)
22
Using
Using aa kinematic
kinematic model of the
model of the tracked combine harvester,
tracked combine harvester, the velocity V
the velocity 𝑉𝑐c is
is calculated
calculated based
based
on
on each
each track’s
track’s velocity
velocity including the slip
including the slip of
of tracks
tracks from
from the
the tracked
tracked combine
combine harvester
harvester expressed
expressed by
by
Equation (4). The RTK-GPS positions used to calculate the actual turning radius 𝑅 by using the least
Equation (4). The RTK-GPS positions used to calculate the actual turning radius R by using the least
square
square method
method [24]
[24] is given by
is given by Equation
Equation (6),
(6), which
which was
was derived
derived from
from the
the general
general circle
circle Equation
Equation (5),
(5),
where (𝑎, b)𝑏)and
where (a, andC𝐶are the
are center
the circle
center and
circle andcoefficient:
coefficient:
𝑉𝑅 (1−𝑆𝑟 )+𝑉𝐿 (1−𝑆𝑙 )
𝑉𝑐 = (4)
2

(𝑎 − 𝑥)2 + (𝑦 − 𝑏)2 = 𝑅2 (5)

𝑅 = √(𝑎2 + 𝑏 2 − 𝐶) (6)
Sensors 2018, 18, 1390 5 of 12

VR (1 − Sr ) + VL (1 − Sl )
Vc = (4)
2
( a − x )2 + ( y − b )2 = R2 (5)
q
R = ( a2 + b2 − C ) (6)

Now, Equations (1) and (2) are integrated to transfer the longitudinal and lateral direction
velocities of the tracked combine harvester to the local coordinates. In order to run this tracked
combine harvester in real time, the velocities in the longitudinal and lateral direction in the harvester
coordinates are expressed in a global reference frame by the Equation (7):
" . # " #" . #
X cos ϕ − sin ϕ xc
. = . (7)
Y sin ϕ cos ϕ yc

By using the Equation (7), the tracked combine harvester model equations can be rewritten as
Equation (8):
   
. 1
( FR + FL − R R − R L − Fc sin β) cos ϕ − 1
m ( Fc cos β − µmg ) sin ϕ ∆t
 
Xc m
 .  
  
 Y c  = 1
m ( FR + FL − R R − R L − Fc sin β) sin ϕ + 1
m ( Fc cos β − µmg ) cos ϕ ∆t (8)


.   
ϕ [ ( FR − R R )−( FL − R L )] B
2 − Mr ) ∆tI

The discrete-time process model is developed from the continuous-time process model, where
the state space equation is obtained by integrating the continuous-time equations over the interval
from tk to tk+1 . The discrete-time model is an approximation of the continuous-time model. Now,
the discretization of the Equation (8) can be written by the following Equation (9):
 .   
 .  X k + m1 ( FR + FL − R R − R L − Fc sin β) cos ϕ − m1 ( Fc cos β − µmg) sin ϕ ∆t
X k +1  .   2 
  Xk + X k ∆t + m1 ( FR + FL − R R − R L − Fc sin β) cos ϕ − m1 ( Fc cos β − µmg) sin ϕ ∆t2
 
 Xk +1  
.

.
   
Y k + m1 ( FR + FL − R R − R L − Fc sin β) sin ϕ + m1 ( Fc cos β − µmg) cos ϕ ∆t
   
 Y k +1   
 (9)
=
  
.  
  Yk + Y k ∆t + 1 ( FR + FL − R R − R L − Fc sin β) sin ϕ + 1 ( Fc cos β − µmg) cos ϕ ∆t2

 Yk+1 m m 2


.
  
.
  
[( FR − R R )−( FL − R L )] B ∆t
  
 ϕ k +1   ϕk + − Mr

2 I 
.
  
[( FR − R R )−( FL − R L )] B
− Mr ∆t
ϕ k +1 2
ϕk + ϕk ∆t + 2 2I

2.3. RTK-GPS and IMU Fusion Algorithm

During the moderate to high speed turns of the tracked combine harvester, the yaw rate
measurement from the IMU sensor will give a bias error that creates a heading drift error, which
requires compensation for estimating the absolute heading. For this reason, the model given in
Equation (9) was used to estimate the heading of the tracked combine harvester by using a non-linear
extended Kalman filter. According to the given Equation (9), the state vector of tracked combine
harvester was defined as the Equation (10):
h . . .
iT
Xk+1 = Xk+1 , Yk+1 , X k+1 , Y k+1 , ϕk+1 , ϕk+1 , ϕ gk+1 , bk+1 (10)

where:

Xk+1 = Tracked combine harvester position in the east direction at time tk+1 ;
Yk+1 = Tracked combine harvester position in the north direction at time tk+1 ;
.
X k+1 = Tracked combine harvester velocity in the X-direction at time tk+1 ;
.
Y k+1 = Tracked combine harvester velocity in the Y-direction at time tk+1 ;
Sensors 2018, 18, 1390 6 of 12

.
ϕk+1 = Yaw rate at time tk+1 ;
ϕk+1 = Heading of tracked combine harvester at time tk+1 ;
ϕ gk+1 = Heading from RTK-GPS at time tk+1 ; and
bk+1 = Yaw rate gyro measurement bias at time tk+1 .

The system measurement vector is defined by the following Equation (11):

 . T
Zk+1 = Xgps , Ygps , ϕimu , ϕ gps (11)

where:

Xgps = Measurement position in the east direction from RTK-GPS;

Ygps = Measurement position in the north direction from RTK-GPS;
.
ϕimu = Yaw rate gyro measurement from the IMU; and
ϕ gps = Measurement heading from the RTK-GPS.

Now, the extended Kalman filter equations in the following Equations (12) and (13)
are considered for the prediction and correction steps; which is maintained in a cyclic
manner.
h i matrix Qk =
The initial state covariance matrix Pk , process noise covariance
diag. 0.002909839 0.002909839 0 0 0.000781853 0.001350055 0.00508 0 and measurement
h i
noise covariance matrix Rk = diag. 0.00000297878 0.00000297878 0.000000438461 0.010348 are
used for this extended Kalman filter, which were obtained by the static and dynamic test of RTK-GPS
and IMU sensors:
Prediction: 
Predicted state, Xk+1 = f ( Xk , uk ) 

Predicted covariance, Pk+1 = Ak Pk AkT + Qk (12)

Predicted measurement, Zk+1 = h( Xk+1 ) 
Correction: )
Estimate state, X̂k+1 = Xk+1 + Kvk
(13)
Estimate covariance, P̂k+1 = Pk+1 ( I − KHk )

where:

Innovation, vk = Zk+1 − h( Xk+1 )

Kalman gain, K = Pk+1 HkT S−1
Innovation covariance, S = Hk Pk+1 HkT + Rk

The Jacobian matrices of Ak and Hk in the prediction step are the following, in which Ak was
obtained from the partial derivatives of each state vector by using the Equation (9), and Hk was
obtained from the partial derivatives of each measurement vector.
 ∂ f1 ∂ f1   ∂h1

∂x1 ··· ∂xm ∂x1 ··· ∂h
∂xm
∂f  .. .. ..  and, Hk = ∂h = 
  .. .. .. 
Ak = = . . . . . .

∂x   ∂x  
∂ fn ∂ fn ∂hn ∂hn
∂x1 ··· ∂xm ∂x1 ··· ∂xm

3. Results and Discussion

3.1. Trajectory of Tracked Combine Harvester

For the evaluation of the estimated heading for the tracked combine harvester by compensating
the yaw rate gyro measurement bias under non-linear conditions, the tracked combine harvester was
Sensors 2018, 18, 1390 7 of 12

run in circular and sinusoidal shapes, and during this time, the sensor measurements were logged,
and the running trajectory of the tracked combine harvester can be obtained by using RTK-GPS
position. Figure 4 shows the circular trajectories of the tracked combine harvester from the different
input steering commands that were at 10◦ and 15◦ , respectively. On the other hand, the sinusoidal
trajectories of the tracked combine harvester were made by a series of input steering commands such
Sensors
as ◦ and
±202018, 18,±x40 ◦ , respectively,
FOR PEER REVIEW as shown in Figure 5. Based on these circular and sinusoidal trajectory
7 of 12
sensor measurements,
Sensors 2018, 18, x FOR PEER the absolute heading of the tracked combine harvester was estimated by7 the
REVIEW of 12
extended Kalman filter combined with the tracked combine harvester model.

Figure 4. Circular trajectories of the tracked combine harvester at 10° and 15° input steering angles.
Figure 4. Circular trajectories of the tracked combine harvester at 10◦ and 15◦ input steering angles.
Figure 4. Circular trajectories of the tracked combine harvester at 10° and 15° input steering angles.

Sinusoidal trajectories ◦ and ±40◦ input

±20input
Figure5. 5.
Figure Sinusoidal trajectories of theof the tracked
tracked combinecombine
harvesterharvester at ±40°
at ±20° and steering angles.
steering angles.
Figure 5. Sinusoidal trajectories of the tracked combine harvester at ±20° and ±40° input steering angles.
3.2. Estimated Heading of Circular Trajectory
3.2. Estimated Heading of Circular Trajectory
3.2. Estimated Heading ofthe
Figure 6 indicates Circular
measured Trajectory
and estimated heading of circular trajectories (left figures) as
well asFigure
the 6 indicates
difference the
of measured
these headings andfrom estimated headingGPS
the reference of circular
headingtrajectories
(right figures)(left which
figures)was as
Figure 6 indicates the measured and estimated heading of circular trajectories (left figures) as
well
obtainedas theby difference
the of these
linear headings
regression of from
the the reference
noisy trend GPS heading.
GPS heading (right The figures) which
heading from was
GPS
well as the difference of these headings from the reference GPS heading (right figures) which was
obtained
measurements by the linear regression
is indicated by aof the line,
blue noisywhere
trend GPSthetrendheading. The
measured heading from GPS measurements
obtained by the linear regression of the noisy GPS and estimated
heading. Theheadings
headingare frommarked
GPS
is
by indicated
the black by
and a blue
red line,
lines. where
In general, the measured
a full circle and estimated
rotation is headings
counted to are
360°, marked
and when bythethetracked
black
measurements is indicated by a blue line, where the measured and estimated headings
◦ , and when the tracked combine are marked
and
combinered lines.
harvester In general,
was runaIn infull
the circle
field rotation is counted to 360
by the black and red lines. general, awith
full acircle
constant steering
rotation command,
is counted therefore,
to 360°, and whena fullthe trajectory
tracked
harvester
of circle wasberun
will in the
360°. That field
why, withthe aleft
constant
Figure steering
6a,b command,
indicate that therefore,
the output a full trajectory
headings were of circle
bounded
combine harvester was run in the field with a constant steering command,
◦ . That why, the left Figure 6a,b indicate that the output headings were bounded by 360◦ . therefore, a full trajectory
will
by be 360According
360°.
of circle will be 360°.toThat these why, figures,
the leftthe red line
Figure 6a,b tried
indicateto follow
that thethe blueheadings
output one, which were means
boundedthe
According
estimated to these figures,
heading based the red
on the line tried
extended to follow
Kalman the blue
filter to
wasone, which
consistent means the
withone, estimated
the which
GPS heading. heading In
by 360°. According to these figures, the red line tried follow the blue means the
based
addition, on the
the extended
heading Kalman filter
difference was is
analysis consistent
shown in with
right the GPS6a,b,
Figure heading.
which Inindicates
addition,the thedifference
heading
estimated heading based on the extended Kalman filter was consistent with the GPS heading. In
difference
between analysis is shown andinestimated
right Figure 6a,b, which indicates the difference betweenSince,the measured
addition,the the measured
heading difference analysisheadings
is shownfrom in righttheFigure
reference
6a,b,GPS which heading.
indicates the GPS
the difference
and
headingestimated
provides headings
the from
exact the
directionreference
of the GPS
vehicleheading.
that Since,
is computedthe GPS heading
from the provides
exact position the ofexact
the
between the measured and estimated headings from the reference GPS heading. Since, the GPS
direction
tracked of
combinethe vehicle
harvester,that is
the computed
linear from
regression the ofexact
the position
GPS of
heading the tracked
was used combine
as a harvester,
reference GPS
heading provides the exact direction of the vehicle that is computed from the exact position of the
the linear regression
heading of theofGPS heading was used asresult
a reference GPS heading the for the evaluation
tracked for combinethe evaluation
harvester, estimated
the headings.
linear regression The
of the GPS indicated
headingout was thatused estimated
as a reference heading
GPS
(red line) matches the reference GPS heading for all circular trajectories
heading for the evaluation of estimated headings. The result indicated out that the estimated heading as shown in Figure 6a,b (right
figures)
(red line) rather
matches thanthe thereference
measured GPS heading.
heading for all circular trajectories as shown in Figure 6a,b (right
From the error analysis
figures) rather than the measured heading. of these headings, the RMS errors between the measured or estimated
heading and the reference GPS
From the error analysis of these headings, heading were calculated.
the RMS The errorsRMS errorsthe
between for measured
the measured heading
or estimated
are 6.74 for 10° steering angle and 4.18 for 15° steering angle, whereas the RMS errors for estimated
Sensors 2018, 18, 1390 8 of 12

of estimated headings. The result indicated out that the estimated heading (red line) matches the
reference GPS heading for all circular trajectories as shown in Figure 6a,b (right figures) rather than
the measured heading.
From the error analysis of these headings, the RMS errors between the measured or estimated
heading and the reference GPS heading were calculated. The RMS errors for the measured heading
are 6.74 for 10◦ steering angle and 4.18 for 15◦ steering angle, whereas the RMS errors for estimated
heading are 1.93 for 10◦ steering angle and 1.58 for 15◦ steering angle, respectively. The RMS errors for
the estimated
Sensors 2018, 18, xheading
FOR PEERare lower than the measured heading for all circular trajectories, indicating8 of
REVIEW that
12
the heading drift errors caused from the yaw rate gyro measurement bias are compensated.

Figure 6. Measured and estimated headings (left figures) for circular trajectories as well as heading
Figure 6. Measured
difference and estimated
(right figures) headings
(where, GPSH = GPS(left figures)MH
Heading, for circular trajectories
= Measured Heading,as well
EH =asEstimated
heading
difference
Heading and(right
GPSHfigures)
(Reg)(where, GPSH
= Linear = GPS of
Regression Heading, MH = Measured Heading, EH = Estimated
GPS Heading).
Heading and GPSH (Reg) = Linear Regression of GPS Heading).
3.3. Estimated Heading of Sinusoidal Trajectory
3.3. Estimated Heading of Sinusoidal Trajectory
Figure 7 shows the measured and estimated heading (left figures) and heading difference between
Figure 7 or
the measured shows the measured
estimated heading and andtheestimated
non-linear heading (left of
regression figures)
the GPSand heading
heading difference
(right figures).
between the measured or estimated heading and the non-linear regression of the
The purple color line in the left figures indicates the non-linear regression of the GPS heading called GPS heading (right
the
figures). The purple color line in the left figures indicates the non-linear regression
reference GPS heading which was obtained by using the Gauss-Newton algorithm. The Gauss-Newton of the GPS heading
called the reference
algorithm is a simple GPS heading
method for which
solvingwasanyobtained
non-linearby regression
using the Gauss-Newton algorithm. The
 problem, and the equation for
Gauss-Newton algorithm is a simple method for solving any non-linear regression problem, and the

non-linear regression analysis was considered as Yi = Asin XTi + θ + aX 𝑋𝑖 i
+ b, where A, T, θ, a and
equation for non-linear regression analysis was considered as 𝑌𝑖 = 𝐴𝑠𝑖𝑛 ( + 𝜃) + 𝑎𝑋𝑖 + 𝑏, where 𝐴,
b are the amplitude, period, phase shift angle, phase shift (horizontally) and 𝑇 phase shift (vertically),
𝑇, 𝜃, 𝑎 and 𝑏On
respectively. arethe
theother
amplitude,
hand, theperiod, phaseand
measured shift angle, phase
estimated shift (horizontally)
headings were represented andby phase shift
the black
(vertically),
and red color respectively. On the other
lines. According hand,7a,b
to Figure the (left
measured
side), and
the estimated
estimated headings
heading were represented
is better than the
by the black and red color lines. According to Figure 7a,b (left side), the
measured heading and it tried to match with the reference GPS heading. The heading differenceestimated heading is better
for
than the measured heading and it tried to match with the reference GPS
the estimated heading and measured heading was obtained from the reference GPS heading as shown heading. The heading
difference
in the rightfor the estimated
Figure 7a,b. From heading
the errorandanalysis
measured heading was
of measured and obtained
estimatedfrom the reference
heading regardingGPS the
heading
reference GPS heading, the RMS errors for the measured heading are 2.39 for ±20 steering estimated
as shown in the right Figure 7a,b. From the error analysis of measured◦ and angle and
heading
3.96 for ± regarding
40◦ steeringtheangle,
reference GPS heading,
whereas the RMS
the RMS errors for errors
estimatedfor the measured
heading heading
are 2.29 for ±20are 2.39 for
◦ steering
±20° steering angle and 3.96 for ±40° steering angle, whereas the RMS errors for estimated heading
are 2.29 for ±20° steering angle and 3.48 for ±40° steering angle, respectively. The RMS errors for the
measured and estimated heading do not have a big difference because the reference GPS heading is
sometimes matched with the noisy trend GPS heading, and sometimes not matched. If the reference
GPS heading matches with the exact GPS heading, the RMS error of measured and estimated heading
Sensors 2018, 18, 1390 9 of 12

angle and 3.48 for ±40◦ steering angle, respectively. The RMS errors for the measured and estimated
heading do not have a big difference because the reference GPS heading is sometimes matched with
the noisy trend GPS heading, and sometimes not matched. If the reference GPS heading matches with
the exact
Sensors GPS
2018, 18, xheading,
FOR PEERthe RMS error of measured and estimated heading will change, and the RMS
REVIEW 9 of 12
error of the estimated heading will be lower than that of the measured heading.

Figure 7. Measured and estimated headings (left figures) for sinusoidal trajectories as well as heading
difference (right figures) (where, GPSH = GPS Heading, MH = Measured Heading, EH = Estimated
Heading
Figure 7.and GPSH (Reg)
Measured = Non-linear
and estimated Regression
headings of GPSfor
(left figures) Heading).
sinusoidal trajectories as well as heading
difference (right figures) (where, GPSH = GPS Heading, MH = Measured Heading, EH = Estimated
3.4. Estimated Heading
Heading and GPSH of(Reg)
Concave Polygon Field
= Non-linear Regression of GPS Heading).

The heading estimation method based on the tracked combine harvester dynamic model and the
3.4. Estimated Heading of Concave Polygon Field
extended Kalman filter can also be evaluated for any polygon-shaped field, because this polygon field
The heading
with crops estimationbymethod
will be harvested the robot based on the
combine tracked which
harvester, combine harvester
is our dynamictarget.
main research modelHere,and
the extended
only a concaveKalman
polygonfilter
fieldcan also be in
as shown evaluated
Figure 8forwas any
usedpolygon-shaped field, The
for this estimation. because
circlethis polygon
marked on
field with crops will be harvested by the robot combine harvester, which is our
the concave polygon field (top figure) indicates the turning position of the tracked combine harvester. main research target.
Here,
In theseonly a concave
circle positions, polygon field as
the tracked shownharvester
combine in Figurecuts 8 wastheused for this estimation.
surrounding The circle
crop to calculate the
marked
exact croponperiphery
the concave polygon the
for obtaining field (top figure)
working path ofindicates
the robotthe turning
combine positionbyofforward
harvester the tracked
and
combine harvester.
backward movement, In these circle positions,
and sometimes turnsthe tracked
at high combine
speed harvester
with low turningcutsradius
the surrounding
are considered,crop
to calculate
which createsthe exactrate
a yaw crop periphery
gyro for obtaining
measurement bias. Thisthe bias
working
createspath
theofmeasured
the robotheading
combinedriftharvester
error
by forward
that and backward
is important to compensate. movement,
Figure and sometimes
8 (bottom turnsshows
figure) at high speed with
a heading of alow turning
concave radius
polygon
are considered,
field which
where the black and creates a yaw
red lines rate gyro
are marked formeasurement
the measuredbias. This bias creates
and estimated headings,thewhich
measured
was
heading drift
analyzed witherror
respectthattoisthe
important
reference toGPS
compensate.
heading Figure 8 (bottom
(blue color line). figure) shows aGPS
This reference heading
headingof a
concave polygon field where the black and red lines are marked for
was obtained from the moving average of the noisy trend GPS heading. The estimated heading is the measured and estimated
headings,
better which
matched was
with theanalyzed
reference with respect than
GPS heading to the thereference
measuredGPS heading
heading (blue8color
in Figure (bottomline). This
figure).
reference
In addition,GPStheheading
headingwas obtainedbetween
difference from thethe moving
measuredaverage of theor
heading noisy trend GPS
estimated heading.
heading and The
the
estimatedGPS
reference heading
headingis better matched with
was calculated, the reference
as shown in Figure GPS heading
9. The RMS than
errorthe
wasmeasured
calculated heading
from the in
Figure 8 (bottom
measured figure).
or estimated In addition,
heading the heading
and the reference GPSdifference
heading, and between
from thetheerror
measured
analysis,heading
the RMS or
estimated
errors heading and
for measured and estimating
the reference GPS were
heading heading14.93 ◦ and
was 5.17◦ , respectively.
calculated, as shownSince,in Figure 9. The
the RMS RMS
error of
error washeading
estimated calculated from than
is lower the measured
the measured or estimated
heading, the heading and heading
estimated the reference
showsGPS moreheading,
consistencyand
from the error analysis, the RMS errors for measured and estimating heading were 14.93° and 5.17°,
respectively. Since, the RMS error of estimated heading is lower than the measured heading, the
estimated heading shows more consistency with the reference GPS heading. The results also describe
that the measured heading drift error was compensated by the extended Kalman filter and the
tracked combine harvester dynamic model.
Sensors 2018, 18, 1390 10 of 12
Sensors 2018, 18, x FOR PEER REVIEW 10 of 12

Sensors
with 2018,
the 18, x FORGPS
reference PEERheading.
REVIEW The results also describe that the measured heading drift error
10was
of 12

compensated by the extended Kalman filter and the tracked combine harvester dynamic model.

Figure 8. Outlook of a concave polygon field (top figure) and, measured and estimated heading
(bottom8.figure)
Figure for of
Outlook theaconcave
concavepolygon
polygonfield.
field (top figure) and, measured and estimated heading
Figure 8.
(bottom Outlook
figure) of concave
for the a concave polygon
polygon field (top figure) and, measured and estimated heading
field.
(bottom figure) for the concave polygon field.

Figure 9. Heading difference based on the reference heading for a concave polygon field.
Figure 9. Heading difference based on the reference heading for a concave polygon field.

4. Conclusions
Figure 9. Heading difference based on the reference heading for a concave polygon field.

4. Conclusions
Sensors 2018, 18, 1390 11 of 12

4. Conclusions
This research deals with the tracked combine harvester dynamic model, sensor measurements
and the extended Kalman filter, which are used to estimate the absolute heading of a tracked combine
harvester under non-linear condition by compensating the IMU yaw rate gyro measurement bias
which influences the heading drift errors. Different sets of experiments such as circular, sinusoidal
and concave polygon trajectories were executed to evaluate this estimation method. The results for
different trajectories revealed that the IMU yaw rate gyro measurement bias was compensated based
on the extended Kalman filter and the tracked combine harvester dynamic model, and the absolute
heading was simultaneously determined, which is better than the measured heading. The RMS errors
of the estimated heading are lower than those of the measured heading for all trajectories. Therefore,
this estimation method can be used to estimate the absolute heading when the tracked combine
harvester will makes turn at high speed and with high order steering commands in order to cut
wheat and paddy rice near headlands to calculate the exact crop periphery for the development of a
harvesting map, for more details see [25].

Author Contributions: For research articles with several authors, a short paragraph specifying their individual
contributions must be provided.
Conflicts of Interest: The authors declare no conflict of interest.

References
1. Zhang, Q.; Reid John, F.; Noguchi, N. Agricultural vehicle navigation using multiple guidance sensors.
In Proceedings of the International Conference on Field and Service Robotics, Pittsburgh, PA, USA, 29–31
August 1999.
2. Benson, E.R.; Reid, J.F.; Zhang, Q. Machine vision based steering system for agricultural combines.
In Proceedings of the ASAE Annual Meeting, Sacramento, CA, USA, 31 July–2 August 2001.
3. Choi, J. Development of Guidance System Using Local Sensors for Agricultural Vehicles. Ph.D. Thesis,
Graduate School of Agriculture, Hokkaido University, Sapporo, Japan, 25 March 2014. Available online:
http://hdl.handle.net/2115/56098 (accessed on 25 March 2014).
4. Ishii, K.; Terao, H.; Noguchi, N. Studies on Self-learning Autonomous Vehicles (Part-1). J. Jpn. Soc. Agric.
Mach. 1994, 56, 53–60.
5. Kise, M.; Noguchi, N.; Ishii, K.; Terao, H. Field mobile robot navigated by RTK-GPS and FOG (Part-2). J. Jpn.
Soc. Agric. Mach. 2001, 63, 80–85.
6. Adla, R.; Yousef, B.; Al-Holou, N. Multi sensor Data Fusion, Methods and Problems. In Proceedings of the
International Conference on Parallel and Distributed Processing Techniques and Applications, Las Vegas,
NV, USA, 22–23 July 2013; Volume II.
7. Reinstein, M.; Kubelka, V. Complementary filtering approach to orientation estimation using inertial sensors
only. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), St. Paul,
MN, USA, 14–18 May 2012.
8. Alam, M.; Moreno, G.; Sipos, M.; Rohac, J. INS/GNSS localization using 15 state extended Kalman filter.
In Proceedings of the International Conference in Aerospace for Young Scientists, Beijing, China, 12–13
November 2016; pp. 425–435.
9. Azam, S.E.; Chatzi, E.; Papadimitriou1, C.; Smyth, A. Experimental validation of the Kalman-type filters for
online and real-time state and input estimation. J. Vib. Control 2017, 23, 2494–2519. [CrossRef]
10. Rezaeian, A.; Zarringhalam, R.; Fallah, S.; Melek, W.; Khajepour, A.; Chen, S.-K.; Litkouhi, B. Cascaded Dual
Extended Kalman Filter for Combined Vehicle State Estimation and Parameter Identification. In Proceedings
of the SAE World Congress and Exhibition, Detroit, MI, USA, 16–18 April 2013.
11. Simanek, J.; Reinstein, M.; Kubelka, V. Evaluation of the EKF-based estimation architectures for data fusion
in mobile robots. IEEE ASME Trans. Mechatron. 2015, 20, 985–990. [CrossRef]
12. Simon, D. Kalman filtering with state constraints: A survey of linear and nonlinear algorithms. IET Control
Theory Appl. 2010, 4, 1303–1318. [CrossRef]
Sensors 2018, 18, 1390 12 of 12

13. Zihajehzadeh, S.; Loh, D.; Lee, T.J.; Hoskinson, R.; Park, E.J. A cascaded Kalman filter-based
GPS/MEMS-IMU integration for sports applications. Measurement 2015, 73, 200–210. [CrossRef]
14. Gustafsson, F.; Gunnarsson, F.; Bergman, N.; Forssell, U.; Jansson, J.; Karlsson, R.; Nordlund, P.-J. Particle
filters for positioning, navigation and tracking. IEEE Trans. Signal Process. 2002, 50, 425–437. [CrossRef]
15. Martino, L.; Read, J.; Elvira, V.; Louzada, F. Cooperative parallel particle filters for online model selection
and applications to urban mobility. Digit. Signal Process. 2017, 60, 172–185. [CrossRef]
16. Míguez, J. Analysis of selection methods for cost-reference particle filtering with applications to maneuvering
target tracking and dynamic optimization. Digit. Signal Process. 2007, 17, 787–807. [CrossRef]
17. Sotak, M.; Sopata, M.; Kmec, F. Navigation systems using Monte Carlo method. In Proceedings of the 6th
International ESA Conference on Guidance, Navigation and Control Systems, European Space Agency,
Loutraki, Greece, 17–20 October 2005.
18. Urteaga, I.; Bugallo, M.F.; Djuri’c, P.M. Sequential Monte Carlo Methods under model uncertainty.
In Proceedings of the IEEE Statistical Signal Processing Workshop (SSP), Palma de Mallorca, Spain, 26–29
June 2016.
19. Rohac, J.; Hansen, J.M.; Alam, M.; Sipos, M.; Johansen, T.A.; Thor, I.; Fossen, T.I. Validation of nonlinear
integrated navigation solutions. Annu. Rev. Control 2017, 43, 91–106. [CrossRef]
20. Noguchi, N.; Reid, J.F.; Benson, E.; Will, J.; Stombaugh, T. Vehicle Automation System Based on Multi-Sensor
Integration; Paper 983111; ASAE (American Society of Agricultural Engineers): St. Joseph, MI, USA, 1998.
21. Alatise, M.B.; Hancke, G.P. Pose estimation of a mobile robot based on fusion of IMU data and vision data
using an extended Kalman filter. Sensors 2017, 17, 2164. [CrossRef] [PubMed]
22. Dar, T.M.; Longoria, R.G. Slip estimation for small scale robotic tracked vehicles. In Proceedings of the
American Control Conferencem, Marriott Waterfront, Baltimore, MD, USA, 30 June–2 July 2010.
23. Zhang, Z. Development of a Robot Combine Harvester Based on GNSS. Ph.D. Thesis, Graduate School of
Agriculture, Hokkaido University, Sapporo, Japan, 25 March 2014. Available online: http://hdl.handle.net/
2115/56100 (accessed on 25 March 2014).
24. Rahman, M.M.; Ishii, K.; Noguchi, N. Study on Tracked Combine Harvester Dynamic Model for Automated
Navigation Purposes. Adv. Robot. Autom. 2017, 6, 178. [CrossRef]
25. Rahman, M.M.; Ishii, K.; Noguchi, N. Automatic Path Planning of Robot Combine Harvester based on
Optimum Harvesting Area of Convex and Concave Polygon Field. IFAC J. Syst. Control. under review.
26. Kitano, M.; Jyozaki, H.A. Theoretical analysis of steerability of tracked vehicles. J. Terramech. 1976, 13,
241–258. [CrossRef]
27. Wong, J.Y. Theory of Ground Vehicles; A Wiley-Interscience Publication: New York, NY, USA, 2008.
28. Le, A.T.; Rye David, C.; Durrnat-Whyte Hugh, F. Estimation of track-soil interactions for autonomous tracked
vehicles. In Proceedings of the IEEE International Conference on Robotics and Automation, Albuquerque,
New Mexico, 20–25 April 1997; pp. 1388–1393.

© 2018 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access
article distributed under the terms and conditions of the Creative Commons Attribution
(CC BY) license (http://creativecommons.org/licenses/by/4.0/).