ON-LINE ESTIMATION OF IMPLEMENT DYNAMICS FOR ADAPTIVE
STEERING CONTROL OF FARM TRACTORS
Except where reference is made to work of others, the work described in this thesis is my
own or was done in collaboration with my advisory committee. This thesis does not
include proprietary or classified information
Evan R. Gartley
Certificate of Approval:
George T. Flowers
Professor
Mechanical Engineering
David M. Bevly, Chair
Assistant Professor
Mechanical Engineering
John Y. Hung
Associate Professor
Electrical and Computer Engineering
Stephen L. McFarland
Dean
Graduate School
ON-LINE ESTIMATION OF IMPLEMENT DYNAMICS FOR ADAPTIVE
STEERING CONTROL OF FARM TRACTORS
Evan Robert Gartley
A Thesis
Submitted to
the Graduate Faculty of
Auburn University
in Partial Fulfillment of the
Requirements for the
Degree of
Masters of Science
Auburn, Alabama
December 16, 2005
iii
ON-LINE ESTIMATION OF IMPLEMENT DYNAMICS FOR ADAPTIVE
STEERING CONTROL OF FARM TRACTORS
Evan Robert Gartley
Permission is granted to Auburn University to make copies of this thesis at
its discretion, upon the request of individuals or institutions and
at their expense. The author reserves all publication rights.
____________________________
Signature of the Author
____________________________
Date of Graduation
iv
THESIS ABSTRACT
ON-LINE ESTIMATION OF IMPLEMENT DYNAMICS FOR ADAPTIVE
STEERING CONTROL OF FARM TRACTORS
Evan Robert Gartley
Master of Science, December 16, 2005
(B.M.E., Auburn University, 2003)
135 Typed Pages
Directed by David M. Bevly
An adaptive control technique for the control of a farm tractor during low levels
of excitation and at low velocities is presented. Results of a set of system identification
experiments are compared to previous tractor models. A cascaded controller is then
designed for the feedback of steer angle, yaw rate, and lateral position baed on the new
tractor model. An on-line analysis of the data is used to determine if enough excitation is
available for adaptation. A cascaded Kalman Filter is presented to estimate the slope of
the DC gain of the steer angle to yaw rate transfer function, MDC, with respect to velocity.
An estimator also provides faster updates of position. From the on-line estimate of MDC,
the controller gains are scheduled based on a lookup table of predetermined values that
were calculated from system identification tests.
v
The sensitivity of the controller to model simplifications, incorrect velocities, and
MDC estimate errors are investigated. The accuracy of the estimated MDC due to
neglected dynamics and the rate of convergence is shown. A simulation is used to show
the errors that can be induced in the position estimator by the GPS delay. The yaw rate
estimator is designed for fixed point math using a square root covariance filter.
Experimental and simulation results are provided which show the validity of the MDC
estimate. Finally, experimental results which show that the accuracy changes little as a
result of hitch loading and velocity are presented and discussed.
vi
ACKNOWLEDGEMENTS
The author would like to thank the John Deere Corporation for providing a tractor
for this research and Andy Rekow for providing assistance with the tractor. Thanks are
expressed to the AU Agriculture Department for supplying land and an implement for
testing. Thanks are also due to the entire GPS and Vehicle Dynamics Laboratory for
their help.
vii
Style of Journal Used:
ASME Journal of Dynamic Systems, Measurement, and Control
Computer Software Used:
Microsoft Word 2003
viii
TABLE OF CONTENTS
LIST OF FIGURES................................................................................................ xi
LIST OF TABLES.................................................................................................. xv
1. INTRODUCTION
1.1 Motivation............................................................................................. 1
1.2 Background........................................................................................... 2
1.3 Outline of Thesis................................................................................... 3
1.3 Contributions......................................................................................... 5
2. MODELING AND SYSTEM INENTIFICATION
2.1 Introduction............................................................................................ 6
2.2 Steering Servo Dynamics....................................................................... 6
2.3 Previous Tractor Yaw Dynamics Models.............................................. 9
2.4 8420 Tractor Yaw Dynamics................................................................. 13
2.5 Summary and Conclusion...................................................................... 21
ix
3. CASCADED CONTROLLER DESIGN AND ANALYSIS
3.1 Introduction............................................................................................ 23
3.2 Steering Servo Controller...................................................................... 24
3.2 Yaw Rate Controller.............................................................................. 26
3.3 Closed Loop Yaw Rate Sensitivity........................................................ 31
3.4 Lateral Position Controller..................................................................... 38
3.5 Closed Loop Lateral Position Sensitivity.............................................. 43
3.6 Summary and Conclusion...................................................................... 46
4. STATE AND PARAMETER ESTIMATION
4.1 Introduction........................................................................................... 48
4.2 Extended Kalman Filter......................................................................... 48
4.3 Design of Yaw Rate Dual Estimator..................................................... 53
4.4 Sensitivity of Estimation to Neglected Dynamics................................. 59
4.5 Rate of Convergence............................................................................. 70
4.6 Design of Position Estimator................................................................. 73
4.7 Effects of GPS Delay............................................................................. 79
4.8 Summary and Conclusion...................................................................... 85
5. EXPERIMENTAL IMPLEMENTATION
5.1 Introduction............................................................................................ 86
5.2 Trajectory Design.................................................................................. 86
x
5.3 Results.................................................................................................... 89
5.4 Summary and Conclusion...................................................................... 94
6. FIXED POINT IMPLEMENTATION OF YAW RATE ESTIMATOR
6.1 Introduction............................................................................................ 95
6.2 Square Root Covariance Filter............................................................... 95
6.3 Fixed Point Math................................................................................... 97
6.4 Estimator Model.................................................................................... 98
6.5 Comparison Between Fixed and Floating Point Implementation.......... 100
6.6 Summary and Conclusion...................................................................... 108
8. CONCLUSIONS
8.1 Summary................................................................................................ 110
8.2 Recommendation for Future Work........................................................ 112
REFERENCES....................................................................................................... 114
APPENDIX
A Experimental Setup................................................................................ 118
xi
LIST OF FIGURES
2.1 Steady state steering rate versus command input....................................... 7
2.2 ETFE of steering servo dynamics from input to slew rate......................... 9
2.3 Bicycle model augmented with a hitch force and moment........................ 10
2.4 Comparison of ETFE and Box Jenkins Fits............................................... 14
2.5 Experimental fits of DC gain...................................................................... 15
2.6 Experimental fits of the dominant poles..................................................... 17
2.7 Experimental fits of the secondary poles.................................................... 18
2.8 Experimental fits of the imaginary zeros.................................................... 19
2.9 Experimental fits of the right half plane zero time constant....................... 20
3.1 Block diagram of the controller system...................................................... 23
3.2 Block diagram of steering servo system..................................................... 24
3.3 1?s versus velocity for each yaw model...................................................... 29
3.4 1?r versus velocity for each model............................................................... 30
3.5 Sensitivity of yaw rate closed loop for no implement................................ 32
3.6 Root locus of closed loop yaw rate............................................................. 33
3.7 Sensitivity of yaw rate closed loop with an implement.............................. 34
3.8 Sensitivity with no implement to too low MDC........................................... 36
xii
3.9 Sensitivity with no implement with 2 mph control gains........................... 37
3.10 Sensitivity with no implement with 5 mph control gains........................... 38
3.11 Controller values for various velocities and hitch loadings....................... 41
3.12 Controller values for various velocities and hitch loadings....................... 42
3.13 Sensitivity of lateral error closed loop for no implement........................... 44
3.14 Sensitivity of lateral error closed loop with an implement......................... 45
4.1 Windowed auto-correlation of steer angle.................................................. 56
4.2 Estimate of MDC and yaw rate.................................................................... 58
4.3 Mean MDC versus input frequency using the input of ( )wtAsin=? ......... 61
4.4 Magnitude and phase difference between yaw rate and steer angle times
DC gain for ( )t3sin5=? ............................................................................
62
4.5 Estimate of MDC with an input of ( )t3sin5=? .......................................... 63
4.6 Standard deviation of estimated MDC versus input frequency.................... 64
4.7 Bode magnitudes divided by velocity for no implement............................ 65
4.8 Bode magnitudes divided by velocity for various hitch loadings at 3
mph.............................................................................................................
66
4.9 Mean MDC versus input frequency for second order model....................... 68
4.10 Frequency response of the actual system and second order estimator
model..........................................................................................................
69
4.11 Standard deviation of MDC for improved estimator.................................... 70
4.12 Convergence from different offsets............................................................ 71
4.13 Time to converge within 99.9 % of the correct value................................ 72
xiii
4.14 Estimated standard deviation for MDC........................................................ 73
4.15 Delay in GPS messages.............................................................................. 76
4.16 Comparison of measured and estimated course from experimental test.... 78
4.17 North error.................................................................................................. 80
4.18 East error..................................................................................................... 82
4.19 Heading Error............................................................................................. 83
4.20 Comparison of covariances for an estimator with no delay and estimator
that is compensated for the delay...............................................................
84
5.1 Tracking a straight line............................................................................... 87
5.2 Curved trajectory to the straight line.......................................................... 88
5.3 Estimation of MDC during experimental tests............................................. 90
5.4 Lateral error versus velocity....................................................................... 91
5.5 Lateral error without an implement............................................................ 92
5.6 Lateral error with a four shank ripper at a twelve inch depth..................... 93
6.1 Windowed analysis of experimental data to determine when to adapt...... 101
6.2 Comparison of the estimation of the slope of the dc gain equation with
respect to velocity for fixed and floating point using experimental data...
102
6.3 Comparison of the estimation of the yaw rate bias in fixed and floating
point using experimental data.....................................................................
103
6.4 Comparison of the estimation of yaw rate in fixed and floating point
using experimental data..............................................................................
104
xiv
6.5 Comparison of the estimation of yaw rate in fixed and floating point for
simulated data.............................................................................................
105
6.6 Comparison of the estimation of the yaw rate bias in fixed and floating
point using simulated data..........................................................................
106
6.7 Comparison of the estimation of the yaw rate dc gain slope with respect
to velocity in fixed and floating point for simulated data...........................
107
6.8 Windowed analysis of experimental data to determine when to adapt...... 108
A.1 John Deere 8420 Tractor............................................................................ 118
A.2 Versalogic PC/104 CPU [Versalogic, 2005a]............................................ 119
A.3 Versalogic PC/104 data acquisition board [Versalogic, 2005b]................. 119
A.4 Versalogic VL-ENCL-4 Ruggedized Enclosure [Versalogic, 2005c]........ 119
A.5 Bosch IMU................................................................................................. 120
A.6 Steer angle sensor....................................................................................... 120
A.7 Starfire GPS reciever.................................................................................. 120
xv
LIST OF TABLES
2.1 Steering servo input transformation............................................................. 8
2.2 DC Gain Linear Fits..................................................................................... 16
2.3 Tractor Model Parameters........................................................................... 21
3.1 Inverse mapping of steady state steering rate versus command value........ 25
3.2 Yaw rate controller values........................................................................... 30
3.3 Gain margin for yaw rate............................................................................. 35
3.4 Yaw rate controller values........................................................................... 42
3.5 Gain margin for lateral position................................................................... 46
3.6 Phase margin for lateral position................................................................. 46
4.1 Discrete noise values used for the yaw rate dual estimator......................... 59
4.2 Square root of covariance of MDC provided by the estimator...................... 65
4.3 Discrete noise values used for the position estimator.................................. 79
5.1 Statistics of lateral error............................................................................... 94
1
CHAPTER 1
INTRODUCTION
1.1 Motivation
Farm tractor models vary widely due to the many configurations possible and the
different ground conditions encountered. The controller must give an adequate response
to a system in which towed or hitched implements of varying sizes can be used at a wide
range of depths. By automating tracking operations, costs such as row overlaps can be
reduced. Also, less trained operators can be used without a reduction in accuracy and
tractor operations can be performed during low visibility.
Due to the various implement loadings that are possible, a controller designed for a
time-invariant vehicle model has the potential to either have a poor response or go
unstable. Therefore, it is desired to have a controller that can compensate for the change
in dynamics. It is also desired to not add more sensors to the tractor and as such only
sensors that can be found on tractors that currently have an option for a controlled
response were used. Complicating the problem, desired trajectories consist of straight line
segments, which lead to low levels of excitation. Also, the implement depth is not set
until the straight line trajectory begins. Therefore, fits from when the tractor is lining up
for the controlled run are not applicable for the controller design.
2
The algorithms designed must also be able to operate on a low cost microcontroller.
The microcontroller that is considered for this research is only capable of fixed point
math.
1.2 Background
A number of companies currently produce tractor control systems for tracking
straight lines, such as John Deere and Beeline Technologies, which use GPS for
navigation. Work at Stanford developed simple tractor models and control algorithms
that produced sub inch accuracy of a tractors position using carrier-phase differential
GPS [O?Conner, 1996]. Further research allowed for the tracking of additional
trajectories [Bell, 1999]. Additionally, combines have been controlled with GPS
[Cordesses, 1999].
Before the use of GPS as a navigation system, computer vision was used for the
control of tractors [Reid, 1987]. Research into blending the navigation of GPS and vision
has shown improvements in position accuracy [Zhang, 1999]. A comparison of the
blending of GPS with a magnetometer against using only GPS has also been shown
[Benson, 1998].
System identifications of tractor dynamics between steer angle and yaw rate have
been conducted for input frequencies up to one hertz, for controller design [Bevly, 2001;
Stombaugh, 1998]. Additionally a model of a tractor tire has been developed which
accounts for faulty circularity through the modification of a Pacejka Similarity Method
tire model and has been verified with measured tire data [Bohler, 1999].
3
Numerous tire-soil interaction models exist [Saarilahti, 2002]. However, they are
currently not used for the design of controllers for the lateral dynamics of tractors.
Tractor models used for the design of tractor controllers have included kinematic models
[O?Conner, 1997]. Kinematic models have also been able to model the dynamics
between an object being towed and a vehicle [Svestka, 1995; Hingwe, 2000]. These
kinematic models for the angle between the implement and tractor have been used to
control the position of the implement [Bevly, 2001]. Controllers have also been designed
for tractors modeled using a first order lag and a bicycle model [O?Conner, 1997]. A
bicycle model with tire relaxations has been used to control a tractor at high velocities
[Bevly, 2001].
Online adaptation of a tractor model has been investigated using an Extended
Kalman Filter / LMS algorithm [Rekow, 2001]. That work identified the parameters
defined in [O?Conner, 1997]. An adaptive controller has also been designed for a
tractor?s steering servo to account for a changing dead band and dynamics [Wu, 2001].
Additionally, the slope of the DC gain with respect to velocity of the steer angle to yaw
rate transfer function has been previously estimated along with a steering bias [Bell,
1999].
1.3 Outline of Thesis
In Chapter 2, system identification of the steering servo is performed and a model
is presented which accounts for its nonlinearities. A review of previous yaw dynamic
models for tractors is also covered. The results of open loop system identification
experiments of a tractor with and without an implement are given. The similarities and
4
differences between the previously used models and the open loop system identification
fits are discussed. It is justified that the slope of the DC gain with respect to velocity
(MDC) of the steer angle to yaw rate transfer function can be estimated rather than the DC
gain. Finally, the model of the tractor used for the design of the controllers in this thesis
is given.
In Chapter 3, the design of the controllers for the steering servo, yaw rate, and
lateral position are detailed. The method of determining the controller gains using the
estimation of the parameter MDC is discussed. The sensitivity of the controllers to the
model simplifications in Chapter 2, incorrect velocities, and an incorrect MDC estimation
are shown.
Chapter 4 develops a method for estimating the yaw rate and MDC for the
adaptation of the controller. A method of determining when there exists sufficient
excitation to estimate MDC is given. The rate of convergence upon initial determination
of when to adapt is shown. The effect of the neglected dynamics on the estimation is
displayed and a method of correcting for some of these errors is discussed. The rate of
the convergence of the estimated parameter due to the forgetting factor in the estimator is
shown. The design of the position estimator for providing faster updates of position is
also detailed. The output equations for the estimator which account for the GPS delay are
justified through the use of a simulation. Also, approximations are given for the errors
that are induced from not including the GPS delay in the estimator, which are validated in
simulation.
In Chapter 5, the design of the trajectories that were used for the tractor controller
in this thesis are discussed. Experimental results showing the accuracy of the estimation
5
algorithms is given. It is shown that the tracking response using the estimation and
adaptive control methods developed in this thesis do not vary significantly.
The estimator for the yaw rate and MDC estimation is designed using fixed point
calculations in Chapter 6. A comparison of estimators using fixed and floating point
math are displayed for both experimental and simulation data. Finally, in Chapter 7, the
conclusions and future work are given.
1.4 Contributions
A summary of the contributions provided in this thesis are listed below.
? A model for a John Deere 8420 tractor was developed using system
identifications experiments
? A controller was designed which compensated for changes in velocity and hitch
loading. Analysis showed that not accounting for changes in the velocity and
hitch loading could cause the tractor controller to be unstable.
? An estimator was designed to estimate the effect of the hitch loading on the
tractors dynamics. The accuracy and the amount of time for convergence was
provided.
? A position estimator was designed which accounted for the delay in the receipt of
the GPS message.
? A fixed point estimator was developed for estimating hitch loading through the
parameter MDC, in order to allow implementation of the algorithm on low cost
microcontrollers.
6
CHAPTER 2
MODELING AND SYSTEM IDENTIFICATION
2.1 Introduction
In this chapter the modeling of the hydraulic steering servo and the tractors yaw
rate dynamics will be discussed. While a number of different models have been
presented for the yaw rate dynamics of a tractor, few have provided justifications of their
designs through the use of system identification techniques, instead relying on controlled
responses to determine which model gives the best results. A review of past models as
well as their similarities to system identifications of a John Deere 8420 tractor is
presented. While the past models do not adequately describe the yaw rate dynamics that
of the 8420 tractor, it is shown that one of the models shows promise in describing the
DC gain of the steer angle to yaw rate transfer function. The DC gain equation of this
model will be used as part of the justification for only estimating the slope of the DC gain
(MDC). This will also be justified with the use of system identification results. Finally,
the model that is used in the design of the controllers, which is based on the system
identification results, is presented.
2.2 Steering Servo Dynamics
Based on earlier research [Bell, 1999], the steering servo dynamics were assumed
to be a nonlinear system described using an input transformation and a transfer function
7
which included an integrator. An integrator is necessary since the steering servo is
hydraulic with an input of flow rate and an output of steer angle. To identify the input
transformation, constant inputs were commanded to the tractor resulting in the steer angle
moving at a constant rate as shown below in Figure 2.1.
Figure 2.1: Steady state steering rate versus command input.
The steer angle measurement was differentiated with a center difference equation
to obtain the steer angle rate, slew rate, for a wide range of inputs in order to produce the
data in Figure 2.1. The input transformation was then modeled with a dead-band,
saturations, and nonlinear equations with the values given in Table 2.1, where u is the
8
value provided to the tractor. The fitted model is also compared to the identified values
in Figure 2.1.
Table 2.1: Steering servo input transformation
Steady State Slew Rate ( DC?& ) Input Command (u)
36.0?=DC?& 598__= -0.36 & u? < 0
u = -887.9 2?u +1045u? +1059 u? >= 0 & u? < 0.36
u = 1325 u? >= 0.36
Since the input frequency to the tractor is at 50 hertz and the identified transfer
function for the steering servo given previously in Equation (2.1) is at 100 hertz, a
discrete conversion [Franklin, 1998] was done leading to Equation (3.1).
( )
( ) 32213 32 azazaz
bzb
zu
z
+++
+=? (3.1)
The discrete conversion adds a zero to the transfer function that is located close to minus
one inside of the discrete unit circle.
A constant proportional control law was used for the steering servo control loop.
This results in a closed loop with three poles. The decision on modeling the controller as
a proportional gain was made because the steering dynamics already was an integrator in
the plant and it reduced the number of poles in the design of the yaw rate and position
controllers. Equation (3.2) gives the polynomials of the closed loop denominator used in
the controller design.
( ) ( )( )2121 fzfzezzB +++= (3.2)
26
The first order polynomial contains the pole that can be placed by the controller. The
second order polynomial contains the poles that can not be placed because the controller
only allows the placement of one variable.
The system of equations that must be solved to place the desired pole is shown in
Equation (3.3), where s0 is the proportional controller gain.
?
?
?
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
3
2
11
2
1
0
13
12
0
1
010
a
a
ae
f
f
s
eb
eb (3.3)
Also, solving for this equation allows for the set of poles that have not been placed to be
checked to make sure that they don?t interfere with the desired closed loop dominant
pole. The closed loop system in Equation (3.4) then can then be used in to the design of
the yaw rate and position controllers.
( )
( )
( )
( )32032213
320
bzbsazazaz
bzbs
z
z
des +++++
+=
?
? (3.4)
3.3 Yaw Rate Controller
A set of yaw rate controllers were designed for each of the yaw rate models given
previously in Table 2.3, as a function of velocity. Fourth order curve fits of each set were
then found so that upon the estimation of MDC it could be determined what controller
values to use through interpolation. In order to design the individual points for the fit, the
damping and natural frequency of the poles and zeros were calculated from Table 2.3
based on the velocity for each value of MDC. This led to a transfer function that when
converted to 50 Hz led to Equation (3.5) which could be used to calculate the controller
values for that velocity and MDC.
27
( )
( ) 4322314
43
2
2
3
1
azazazaz
bzbzbzb
z
zr
++++
+++=
? (3.5)
The transfer function from steer angle to yaw rate (Equation (3.5)) was then
multiplied by the closed loop steering servo (Equation (3.4)) producing the open loop
yaw rate system, shown below in Equation (3.6).
( )
( )
( )
( )
76
2
5
3
4
4
3
5
2
6
1
7
76
2
5
3
4
4
3
43
2
2
3
1
4
43
2
2
3
1
32032
2
1
3
320
???????
?????
azazazazazazaz
bzbzbzbzb
azazazaz
bzbzbzb
bzbsazazaz
bzbs
z
zr
des
+++++++
++++=
++++
+++?
+++++
+=
?
(3.6)
The yaw rate controller was chosen to be a lag controller [Ogata, 1998] consisting
of a pole and no zero as shown below.
1
1
?
?
rz
s
er
des
+=
? (3.7)
This was primarily done to mitigate the effect of the second resonant peak that was
identified in Figure 2.3. It was also done in order to prevent a non-minimum phase zero
being placed during the design process, which tended to occur when designing for larger
values of MDC. This would have caused complications in the design of the position
controller.
Since the controller has two design variables, the location of two closed-loop
poles can be selected. Equation (3.8) shows the closed loop denominator, which contains
the second order polynomial of the poles that can be selected and the sixth order
polynomial of the remaining closed loop poles.
( ) ( )( )65423342516212 ??????? fzffzzfzfzfzezezzB ++++++++= (3.8)
28
By calculating the denominator from Equations (3.6) and (3.7) and setting that
equal to Equation (3.8), leads to the set of equations in Equation (3.9) to be solved for to
obtain the controller values.
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
???
???
???
???
??
?
0
?
?
?
?
?
??
??
?
?
?
?
?
?
?
?
?00000??
??0000??
1??000??
01??00??
001??0??
0001??0?
00001?0?
00000101
7
6
5
4
3
22
11
6
5
4
3
2
1
1
1
277
1266
1255
1244
1233
122
11
a
a
a
a
a
ae
ae
f
f
f
f
f
f
s
r
eba
eeba
eeba
eeba
eeba
eea
ea
(3.9)
Solving for Equation (3.9) can also determine if any of the remaining poles would cause
the system to be unstable.
The controller values were calculated for various velocities for each of the yaw
rate models in Table 2.3. The trends in the controller values could then be seen as a
function of hitch loading. Figures 3.3 and 3.4 show how the controller values varied for
the various yaw rate models.
29
Figure 3.3: 1?s versus velocity for each yaw model
As seen in Figure 3.3 the variation due to the implement simply shifts the curve
down. However, in Figure 3.4 there is a more significant difference in the trend of the
curves. Also, there is a less of a percentage difference between the curves in Figure 3.4
than there is in Figure 3.3.
As stated previously, fourth order fits were used for of the curves. Since each of
these curves has a known MDC value related to it, the controller gains could then be
calculated through interpolation using an estimated value of MDC. In Table 3.2, the curve
fits used for the interpolations are listed according to the yaw rate controller variable.
30
Figure 3.4: 1?r versus velocity for each model
Table 3.2: Yaw rate controller values (variable 43223140 gVgVgVgVg ++++= )
Values for 0?s
MDC 0g 1g 2g 3g 4g
0.355 0.016544 -0.12928 0.39308 -0.59242 0.45384
0.285 0.020274 -0.15792 0.47963 -0.7158 0.53252
0.258 0.02307 -0.17972 0.54481 -0.81169 0.60811
0.227 0.02681 -0.20882 0.63224 -0.94108 0.70793
31
Values for 0?r
MDC 0g 1g 2g 3g 4g
0.355 0.00069355 -0.0054523 0.017887 -0.021717 -0.90949
0.285 0.0010538 -0.0066872 0.021571 -0.019729 -0.9006
0.258 0.00093468 -0.006382 0.020994 -0.020493 -0.89685
0.227 0.00088336 -0.0063049 0.020818 -0.021387 -0.89455
3.4 Closed Loop Yaw Rate Sensitivity
To examine the how the accuracy of the MDC estimate (developed in Chapter 4)
will effect the phase and gain margins of the yaw rate controller, a frequency domain
analysis using the system identification models and the previously described controller
adaptation was conducted. The values of MDC were assumed to be known exactly and
within ten percent of the actual value.
Figure 3.5 shows the results of the bodes that were obtained with no implement.
In the figure, the closed loop poles are at 4 rad/s which is before the first resonant peak
that can be seen in the magnitude graph. Recall that if this resonant peak crossed 0 dB
then the system is unstable. Fortunately, it decreases as velocity increases, ensuring the
system is stable with increasing velocity.
32
10?1 100 101 102
?100
?50
0
Magnitude (dB)
10?1 100 101 102
?800
?600
?400
?200
0
Phase (deg)
Frequency (rad/sec)
Figure 3.5: Sensitivity of yaw rate closed loop for no implement
The cause of the resonant peak can be seen more clearly in the root locus of the
closed loop system. Figure 3.6 demonstrates that after the dominant poles there are a set
of poles which have little damping. Recall that the yaw controller selected could only
specify the location of two poles. Therefore these additional poles are not constrained to
a specified location.
Velocity
Increasing
33
?1 ?0.5 0 0.5 1 1.5?1
?0.8
?0.6
?0.4
?0.2
0
0.2
0.4
0.6
0.8
1
Pole?Zero Map
Real Axis
Imaginary Axis
Figure 3.6: Root locus of closed loop yaw rate.
Figure 3.7 shows the closed loop bode for the tractor with an implement at all of
the depths. Note that the resonant peak with the implement is not nearly as pronounced
as in Figure 3.5. It looks like it could exist on a couple of lines, but these are believed to
be bad fits, since that Box-Jenkins fit did not match the ETFE well. The DC portion of
the bode also looks thicker then that of the frequency response without an implement.
This is most likely due to the fact that the fits are better for the system identification tests
without an implement.
34
10?1 100 101 102
?100
?50
0
Magnitude (dB)
10?1 100 101 102
?800
?600
?400
?200
0
Phase (deg)
Frequency (rad/sec)
Figure 3.7: Sensitivity of yaw rate closed loop with an implement
For all of the cases tested varying MDC the controller was found to be stable.
Table 3.3 shows the maximum, minimum, mean, and standard deviation of gain margin
for the various hitch loadings. As seen in the table, the mean gain margin appears
increase as the implement hitch loading increases. This is most likely caused by the
unconstrained set of closed loop poles near the first resonant peak increasing in damping
ratio as the hitch loading is increased. Note that all the cases investigated had infinite
phase margin.
35
Table 3.3: Gain margin (dB) for yaw rate
Maximum Minimum Mean ?
No Implement 8.65 3.77 6.67 1.52
Ripper 4? Depth 14.09 8.93 11.63 1.40
Ripper 8? Depth 15.60 7.80 11.18 1.90
Ripper 12? Depth 15.00 8.89 12.44 1.51
Figure 3.8 shows that for a low value of MDC used in the design of the control law
design would cause the closed loop yaw rate to be unstable. Additionally, values of MDC
significantly greater than the actual value in the controller design can lead to degraded
performance.
36
10?1 100 101 102
?100
?50
0
Magnitude (dB)
10?1 100 101 102
?800
?600
?400
?200
0
Phase (deg)
Frequency (rad/sec)
Figure 3.8: Sensitivity with no implement to too low MDC.
Additionally, if the controller does not account for the changes in velocity it can
cause the yaw rate controller to be unstable. Figure 3.9 shows the results of the controller
being designed for 2 mph for all of the system identification models without an
implement. Note that the magnitude crosses 0 dB after there has been -180 degrees of
phase change for a number of models, indicating that the closed loop system is unstable.
This instability was also seen using the system identification models with an implement.
37
10?1 100 101 102
?100
?50
0
Magnitude (dB)
10?1 100 101 102
?800
?600
?400
?200
0
Phase (deg)
Frequency (rad/sec)
Figure 3.9: Sensitivity with no implement with 2 mph control gains.
When the controller was designed for a constant velocity of 5 mph the closed loop
systems were stable for all hitch loadings identified as seen in Figure 3.10. However the
performance of the controller was highly degraded since the system was at a lower
bandwidth.
38
10?1 100 101 102
?100
?50
0
Magnitude (dB)
10?1 100 101 102
?800
?600
?400
?200
0
Phase (deg)
Frequency (rad/sec)
Figure 3.10: Sensitivity with no implement with 5 mph control gains.
3.5 Lateral Position Controller
The transfer function for lateral position used in the controller design was
obtained from Equations (3.10) and (3.11).
( )?? sinVy =& (3.10)
?? && += r (3.11)
In Equation (3.10), the lateral velocity is defined as the total velocity times the sine of the
course angle (which is taken with respect to the desired longitudinal direction). Equation
(3.11) shows that the course angle is the integrated yaw rate plus side slip.
39
Neglecting the vehicle sideslip and linearizing about a small course angle, the
Laplace transform of Equations (3.10) and (3.11) can be combined to produce the
approximation shown below for lateral position.
rsVy 2?? (3.12)
By discretizing the above equation, the following discrete transfer function between yaw
rate and lateral position is obtained.
( )
( ) zzz
bzb
zr
zy
+?
+=
22
21
((
(3.13)
The above transfer function can then be combined with the closed loop yaw rate transfer
function, forming the open loop transfer function for lateral position shown below.
( )
( ) 109283746556473829110
109
2
8
3
7
4
6
5
5 ~~~~~~~~~~
~~~~~~
azazazazazazazazazaz
bzbzbzbzbzb
zr
zy
des ++++++++++
+++++= (3.14)
A simple lead control law with on zero and one pole was used for the lateral error
controller as seen below.
( )
( ) 1
10 ~
~~
rz
szs
ze
zr
y
des
+
+= (3.15)
Since there are three parameters in the controller, three poles of the closed loop
system could be selected. Equation (3.16) shows the polynomials of the selected poles
and the polynomial for the remaining poles of the closed loop transfer function
denominator.
( ) ( )( )87263544536271832213 ~~~~~~~~~~~~ fzfzfzfzfzfzfzfzezezezzB +++++++++++= (3.16)
The controller values can then solved for using Equation (3.17), which uses the values
from Equations (3.14-3.16).
40
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
???
????
????
????
????
????
???
??
?
0
~
~
~
~
~
~
~
~~
~~
~~
~
~
~
~
~
~
~
~
~
~
~
~0000000~0~
~~000000~~~
~~~00000~~~
1~~~0000~~~
01~~~000~~~
001~~~00~~~
0001~~~00~~
00001~~~00~
000001~~00~
0000001~00~
00000001001
10
9
8
7
6
5
4
33
22
11
8
7
6
5
4
3
2
1
1
0
1
31010
239109
123898
123787
123676
123565
12354
1233
122
11
a
a
a
a
a
a
a
ae
ae
ae
f
f
f
f
f
f
f
f
s
s
r
eba
eebba
eeebba
eeebba
eeebba
eeebba
eeeba
eeea
eea
ea
(3.17)
Once the controller gains were determined for various velocities for each of the
yaw rate models, the trends in the controller values as a function of hitch loading could
be studied. Figures 3.11 and 3.12 show how the controller values varied gains vary with
velocity and implement loading.
41
Figure 3.11: Controller values for various velocities and hitch loadings.
Like the yaw rate controller, fourth order fits were made for the controller gain
curves shown in Figure 3.11 and 3.12. Each of these curves has a known MDC value
related to it, the controller gains could again be calculated through interpolation using an
estimated value of MDC. In Table 3.4, the curve fits used for the interpolations are listed
according to the position controller variable.
42
Figure 3.12: Controller values for various velocities and hitch loadings.
Table 3.4: Yaw rate controller values (variable 43223140 gVgVgVgVg ++++= )
Values for 0~s
MDC 0g 1g 2g 3g 4g
0.355 8.0185 -58.726 161.18 -200.3 106.83
0.285 12.479 -89.378 242.36 -295.36 156.02
0.258 12.108 -88.024 240.09 -294.62 155.17
0.227 11.843 -86.358 235.71 -289.88 152.3
43
Values for 1~s
MDC 0g 1g 2g 3g 4g
0.355 -7.9577 58.281 -159.95 198.77 -106
0.285 -12.382 88.687 -240.48 293.07 -154.8
0.258 -12.014 87.34 -238.22 292.32 -153.94
0.227 -11.75 85.684 -233.87 287.6 -151.09
Values for 1~r
MDC 0g 1g 2g 3g 4g
0.355 0.011359 -0.080389 0.22571 -0.27599 -0.7263
0.285 0.028812 -0.17191 0.4395 -0.48022 -0.61013
0.258 0.019994 -0.13449 0.3703 -0.43411 -0.62486
0.227 0.017956 -0.12553 0.34986 -0.41878 -0.63368
3.6 Closed Loop Lateral Position Sensitivity
To examine the how the accuracy of the MDC estimate effects the phase and gain
margins of the position loop, a bode analysis using the system identification models and
the previously described controller adaptation was again conducted. The values of MDC
were again assumed to be known exactly and within ten percent of the actual value.
Figure 3.13 shows the results of the bode plots that were obtained with. Note that
in the figure the closed loop poles are at 1 rad/s.
44
10?1 100 101 102
?200
?100
0
Magnitude (dB)
10?1 100 101 102
?1000
?500
0
Phase (deg)
Frequency (rad/sec)
Figure 3.13: Sensitivity of lateral position closed loop for no implement
Figure 3.14 shows the results of the bode analysis with the varying MDC when an
implement is used. Unlike the results from the yaw rate analysis, the change in MDC has
very little change on the closed loop frequency response (with or without an implement).
45
10?1 100 101 102
?200
?100
0
Magnitude (dB)
10?1 100 101 102
?1000
?500
0
Phase (deg)
Frequency (rad/sec)
Figure 3.14: Sensitivity of lateral position closed loop with an implement
The gain margins for Figure 3.13 and 3.14 are shown in Table 3.5. Like the yaw
rate controller, the gain margin for the position controller increases with hitch loading
(which improves the stability of the controller).
46
Table 3.5: Gain margin (dB) for lateral position
Maximum Minimum Mean ?
No Implement 3.504 2.383 2.989 0.283
Ripper 4? Depth 3.654 1.434 2.469 0.638
Ripper 8? Depth 5.382 0.673 3.019 1.109
Ripper 12? Depth 5.158 2.563 3.745 0.726
The phase margins for Figure 3.13 and 3.14 are shown in Table 3.6, which also
shows that the phase margin increases with hitch loading.
Table 3.6: Phase margin (deg) for lateral position
Maximum Minimum Mean ?
No Implement 30.963 20.332 26.221 3.436
Ripper 4? Depth 30.053 12.926 21.423 5.235
Ripper 8? Depth 38.202 7.421 26.594 7.693
Ripper 12? Depth 42.268 28.401 34.874 4.710
3.7 Summary and Conclusion
The controller for the steering servo using a transformation for the nonlinearities
was designed. The yaw rate and position controllers were also designed using the system
identification models from Chapter 2. Information was also provided on how the
controller gains would be adapted through the estimation of MDC. The method involves
interpolating the control gain curve fits for various values of MDC. The sensitivity of the
47
closed loop system was investigated for the values of MDC within plus and minus ten
percent. It was also shown that the controllers could become unstable if designed with a
velocity that was less than the actual value or for a value of MDC that was less than the
actual value of MDC. Additionally, when the controllers were designed with too high of a
velocity or too large of a value of MDC, they had a degraded performance.
48
CHAPTER 4
STATE AND PARAMETER ESTIMATION
4.1 Introduction
This chapter describes the design of the estimator used to identify the slope of the
DC gain with respect to longitudinal velocity (MDC) of the steer angle to yaw rate transfer
function. The errors associated with neglecting dynamics in the yaw rate transfer
function will be examined and an improvement in the estimator design is given. The rate
of convergence of the estimator due to the forgetting factor on MDC is also investigated.
A description of the position estimator is detailed and a justification of its output
equations is produced.
4.2 Extended Kalman Filter
The Extended Kalman Filter [Stengal, 1994; Bryson, 1975; Maybeck, 1979b;
Gelb, 1992] is a first order Taylor series expansion of the Kalman Filter for nonlinear
systems. The state and measurement equations are shown in Equations (4.1) and (4.2).
( )twuxfx ,,,=& (4.1)
( )tvuxhy ,,,= (4.2)
The values passed to the state function are the previous estimate of the states,
inputs, disturbances, and time. The values passed to the measurement function include
the current estimate of the states, inputs, measurement noise, and time. The
49
nonlinearities for the yaw rate estimator occur from a state being multiplied by the input.
In the linearization, the second order partial derivatives in the Taylor series expansion
and higher are zero. Hence the estimator is not sub-optimal because an Extended Kalman
Filter is being used. The estimator is sub-optimal because an approximated system is
being used. The position estimator contains states times the sine and cosine of another
state. This results in higher order partial derivatives in its Taylor series expansion which
are non-zero. Therefore, a sub-optimal estimation is caused by the use of an Extended
Kalman Filter. However, a state times the sine or cosine of another state is not a large
nonlinearity for an estimator and an Extended Kalman Filter will provide a decent
estimate, especially since it is updated at 100 hertz. An iterated Extended Kalman Filter
[Gelb, 1992] could also provide the same improvement that a faster update gives. The
position estimator is sub-optimal do to the approximation necessary for the use of an
Extended Kalman Filter and an approximated system being used. Neglecting the higher
order terms of a system that is modeled perfectly causes the estimate to be biased and the
estimate of the covariance to be smaller then the correct value. The covariance being less
than the actual value can lead to filter divergence. However, this can be corrected by
designing the estimator with extra process noise.
Second order Taylor expansion Kalman Filters [Maybeck, 1979b] such as the
Truncated Second Order Filter and the Gaussian Second Order Filter use Hessian
matrices for bias correction and improvement of the covariance propagations. However,
these estimators are much more computationally intensive then the Extended Kalman
Filter. Most of the added computation is in the covariance propagations, and not in the
50
bias correction terms. The bias correction terms from these estimators can easily be used
in an Extended Kalman Filter [Maybeck, 1979b].
Other Kalman Filters that would provide a better approximation include
statistically linearized filters such as the Unscented Kalman Filter [Wan, 2000; Haykin,
2001]. The Unscented Kalman Filter provides a third order approximation for Gaussian
noise systems and second order approximations for all other noise systems. It takes
fewer computations than the second order Taylor series filters. This estimator does not
require that the states or measurements have a derivative. Additionally, the Unscented
Kalman Filter can be extremely helpful in debugging other estimators because it doesn?t
require derivative information.
The Extended Kalman Filter was chosen since the majority of the errors in the
estimator are caused by model approximation and not by the format of the estimator.
Also, the Extended Kalman Filter was chosen so that the yaw rate estimator could be
implemented in fixed point as discussed in Chapter 6, for which a simple estimator was
desired.
In the Extended Kalman Filter, the Jacobian matrices defined in Equations (4.3-
4.5) are linearized with the state estimates from the time update.
( )
x
twuxfH
?
?= ,,, (4.3)
( )
w
twuxfG
?
?= ,,, (4.4)
( )
v
tvuxhU
?
?= ,,, (4.5)
51
The covariance matrices for the Extended Kalman Filter are given in Equations
(4.6) and (4.7), where Q is semi-definite and R is positive definite.
[ ]wwEQ ,= (4.6)
[ ]vvER ,= (4.7)
The Kalman Filter gain (K) is computed with Equation (4.8), where U is typically an
identity matrix.
( ) 1?+= TTT URUHPHPHK (4.8)
Sequential processing is frequently used so that only a scalar value is inverted. The
Kalman Filter gain is then used for updating the state estimates with the current
measurements as shown below.
( )( )tuxhyKxx kkkkkkk ,0,,1|1| ?? ?+= (4.9)
As a result of the measurement update the covariance decreases by Equation (4.10),
where I is the identity matrix.
( ) 1| ??= kkk PKHIP (4.10)
Equations (4.8-4.10) are typically referred to as the measurement update since
they only need to occur if a measurement is received. Equations (4.11) and (4.12) are
typically referred to as the time update since they propagate the estimates to the next time
step.
( )tuxfx kkkk ,0,, 111| ??? =& (4.11)
TT
kkkk GQGFPFPP ++= ??? 111|& (4.12)
The time update for an Extended Kalman Filter can either be in discrete time or in
continuous time. Equations (4.11) and (4.12) show the continuous state and covariance
52
propagations, which is how they are used for the position estimator. Note that the time
update can be performed before a measurement is received.
The covariance propagation in the time update (4.12) increases the uncertainty in
the states. The Jacobian matrices in Equation (4.12) are linearizied with the state
propagations from Equation (4.11) and the two equations are ideally integrated together
so that the most accurate Jacobian matrices available are used.
The yaw rate estimator used in Section 4.3 has a discrete time update, and
Equation (4.12) is replaced with Equation (4.13).
d
T
dkdkk QFPFP += ?? 11| (4.13)
Additionally, a discrete state equation replaces Equation (4.11).
The Extended Kalman Filter reduces to the Kalman Filter if the state and
measurement equations are linear. Furthermore, the Extended Kalman Filter reduces to a
Recursive Least Squares estimator [Stengel, 1994] if the system is linear, discrete and
Equations (4.14) and (4.15) are used for the time update.
01| =?kkx& (4.14)
11| ?? = kkk PP (4.15)
The Extended Kalman Filter can reduces to a Recursive Least Squares [Astrom, 1989]
with a forgetting factor by using Equations (4.14) and (4.13) using the following
equation, and setting Fd to an identity matrix.
11
1
???
??
?
? ?=
kd PQ ? (4.16)
53
4.3 Design of Yaw Rate Dual Estimator
A dual estimator simultaneously estimates clean states and model parameters. A
clean estimate of yaw rate is needed for the position estimator. An estimate of MDC is
needed to adapt the steering controller. The state equations used for the design of the
Extended Kalman Filter are shown in Equation (4.17).
( )( )
?
?
?
?
?
?
?
?
?
?
+
+
++
=
?
?
?
?
?
?
?
?
?
?
=
DC
bias
MDC
rbias
VxDC
DC
bias
wM
wr
wwVM
M
r
r
x
??
(4.17)
The states in the discrete estimator are yaw rate, yaw rate bias, and MDC. Note
that the yaw rate depends on the current measurement of steer angle. Therefore, the time
update can not be calculated before a measurement is received. The velocity used in the
estimator is from GPS which is available at 5 Hz. Although GPS provides velocity in the
direction of course, a small angle approximation allows GPS velocity to be used for the
longitudinal velocity.
The Jacobian of the state equations with respect to the states (4.18) shows that the
rate at which MDC converges will depend on the velocity and the steer angle.
?
?
?
?
?
?
?
?
?
?
=
100
010
00 ?x
d
V
F (4.18)
The measurement in the Kalman filter is the yaw rate gyro which includes the yaw rate
plus its bias, which is reflected in the Jacobian of the measurement equation with respect
to the states shown below.
[ ]011=H (4.19)
54
However, the yaw rate gyro bias will only be estimated when MDC can be
estimated, otherwise the bias would be corrupted with scale factor errors. Therefore,
Equation (4.20) will be used when there is too little excitation.
[ ]001=H (4.20)
The bias could be estimated under low excitation by augmenting the estimator with a
heading state and using the approximation that course angle equals heading angle at low
levels of excitation [Bevly, 2001]. However, in Chapter 6 the estimator is designed in
fixed point where only a limited number of states can be estimated.
Since a discrete covariance time update is used, the discrete process noise matrix
was approximated using Equation (4.21).
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
==
100
010
00
00
000
000
000
000
1000
0100
00
2
2
2
2
1
?
?
?
?
?? ?
DC
xDC
M
r
V
DCxDC
T
ddd
M
VMMVM
GQGQ
DC
bias
(4.21)
In Equation (4.21), the process noise on the yaw rate state was computed to be a
function of the DC gain times the yaw rate disturbance covariance ( ?? ) plus the noise
covariance for the velocity ( V? ) times MDC and the steer angle measurement. The sensor
noise on the steer angle measurement was considered negligible. The yaw rate bias and
MDC were both modeled as random walks with covariances
biasr
? and
DCM
? , which will
determine how fast the estimates will react to changes in the states after the estimator has
settled. The measurement noise for the estimator was simply found with Equation (4.22).
[ ]2rR ?= (4.22)
55
Even though there is a clear relationship between the yaw rate and steer angle, it
can not be estimated continuously, because too little excitation will cause the estimate to
fit disturbances. A method of determining how much excitation exists in the system is
therefore needed. Auto-correlation matrices of varying sizes can be used to determine
how many parameters can be estimated [Astrom, 1995]. The largest auto-correlation
matrix that is of full rank corresponds to the order of excitation there is in the system.
The order of excitation then corresponds to how many parameters can be estimated.
Since only one parameter is desired to be estimated in this thesis, only a one by one auto-
correlation matrix would have to be of full rank, which is always of full rank if it is non
zero. It is possible to determine when there is enough excitation by evaluating larger
auto-correlation matrices. However, sensor noise may make it appear as if there is more
excitation than actually exists. Therefore, the magnitude of a windowed auto-correlation
was used to determine the degree of excitation in the system. This indicates that there is
a current trend in the steer angle, as opposed to a single large measurement, which makes
it likely that MDC can be estimated for some period of time. With this method there is a
possibility that the window analysis may determine there is enough excitation for
adaptation for the period defined by the time in the window, even though current
excitation is quite low. However, the rate of convergence decreases, as seen in Equation
(4.18), as the magnitude of the steer angle decreases. Therefore, little convergence would
occur towards an incorrect value in this case. The main goal of the determination of
when to adapt is to prevent a prolonged estimation during low levels of excitation.
Figure 4.1 shows the value of the auto-correlation of the steer angle over a one
second window for an experimental test. When this value is large enough that a fixed
56
relationship can be seen between the steer angle and yaw rate, it is determined that it is
safe to adapt the system. A threshold value is used for this determination which should
depend on the maximum expected disturbance in the yaw rate and the amount of sensor
noise on the steer angle. As can be seen from Figure 4.1, a reliable time to adapt usually
occurs at the beginning of the controlled run when the controller makes a step input to the
tracked line. The analysis also allows for an improved estimate if the tracking degrades
as is also shown Figure 4.1 (at time = 60 seconds).
20 40 60 80 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
Time (s)
Windowed Auto?Correlation of Steer Angle
Auto?Correlation
Threshold
Figure 4.1: Windowed auto-correlation of steer angle.
When there is adequate excitation to accurately estimate MDC, the covariance
associated with MDC is reset 0.0025 in order to inject uncertainty into the system and
57
allow the estimate to converge quickly. When there is too little excitation in the system,
the Kalman gain associated with MDC is over-ridden with zero and the noise covariance
for MDC and the yaw rate bias is set to zero. This is numerically equivalent to changing
(4.18) to (4.23) and changing (4.21) to (4.24), where
DCM
P is the estimated covariance of
MDC.
?
?
?
?
?
?
?
?
?
?
=
100
010
000
dF (4.23)
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
00
100
010
00
00
0000
00000
00000
0000
0000
01000
00100
00 2
2
?
??
?
??
?
x
DC
xDC
M
VxDCxDC
d
V
M
VM
P
VMVM
Q
DC
(4.24)
Figure 4.2 shows the results of MDC being estimated in a simulation with a steady-
state input. The estimator was told to start estimating MDC at time = 200 seconds and
stop at time = 400 seconds. The actual value of MDC was also decreased by ten percent at
time = 300 seconds without the covariance being reset. As seen, the estimate of MDC
almost immediately reaches the correct value due to the covariance being reinitialized at
time = 200 seconds. The estimate takes longer to converge to the new value at time =
300 seconds because only the forgetting factor is causing the estimate to move. This rate
of convergence is investigated more in Section 4.5. System identification tests found
that the disturbances were best described as second or third order transfer functions.
However, the process disturbances are modeled as white noise in the estimator. In order
for all of the process disturbances to propagate through the estimator, the value of the
58
disturbance covariance ( ?? ) would have to be large. Unfortunately, this would result in
the MDC estimate taking a long time to converge. Therefore, the disturbance covariance
is kept small to allow for faster convergence at the cost of filtering out some of the
process disturbances. Because the disturbances were filtered, adaptively changing the
noise covariance for MDC was not investigated.
200 250 300 350 400
0.02
0.025
0.03
Yaw Rate (rad/s)
200 250 300 350 4000.2
0.25
0.3
0.35
0.4
Time (s)
M D
C
Estimate
? Bounds
Estimate
? Bounds
Figure 4.2: Estimate of MDC and yaw rate.
The nominal noise values used in the design of the Extended Kalman Filter are
listed in Table 4.1.
59
Table 4.1 Discrete noise values used for the yaw rate dual estimator
Parameter Value Units Frequency
V? 0.05 meters/second 5 Hz
?? 8.73x10
-4 radians 100 Hz
biasr
? 1x10-7 radians/second 100 Hz
DCM
? 1x10-4 1/meters 100 Hz
r? 5.23x10
-3 radians/second 100 Hz
4.4 Sensitivity of Estimation to Neglected Dynamics
To examine how the accuracy of the estimate is affected by neglecting the poles
and zeros in the yaw rate transfer function, a simulation was designed to see what kind of
errors this would cause. The transfer function used to simulate the disturbance free plant
and the disturbances (shown in Equations (4.25) and (4.26), respectively) were taken
from a Box Jenkins fit of experimental data at 2 mph with no implement.
0.8994213.522835z-5.343205z3.718368z-z
0.2393940.765003z-0.814621zz 0.288557-
234
23
++
++=
?
r (4.25)
These transfer functions represented a worse case scenario, because they
contained the largest magnitude and phase change over the low frequencies found from
system identification tests.
0.9444991.880024z-z
1.0021912.002189zz
2
2
+
++=
e
rdist (4.26)
The standard deviation of the noise generating the disturbances was also obtained
from the Box Jenkins fit, and is displayed below with the units of radians.
60
13869622000.0=e? (4.27)
Sine waves at a number of different frequencies and amplitudes were tested as inputs to
determine the resulting mean and standard deviations of the estimates after the estimator
had settled. The simulation assumed that there was always enough excitation to estimate
MDC and was run long enough to determine a clear trend in the data.
Figure 4.3 shows the actual value of MDC with the average value of the MDC
versus frequency with input amplitudes of 5, 10, and 15 degrees. Also shown in the
figure is the magnitude of the gain in Equation (4.25) divided by velocity, which is
labeled as (Bode Magnitude)/V. Recall that MDC is the magnitude of the DC gain divided
by velocity and is therefore represented as a constant value. The magnitude of gain in
Equation (4.25) divided by velocity corresponds to the value of MDC that should be
estimated if there is no phase change. As can be seen, the estimated values are close to
the bode curve at less than 1 rad/sec but estimates lower than it at higher frequencies.
The estimates became worse at higher frequencies. It should be noted that the tractor
dynamics rarely operate in these high frequencies. As will be shown later in this section,
these errors can be reduced using a more accurate model in the estimator.
61
0.5 1 1.5 2 2.5 30.36
0.37
0.38
0.39
0.4
0.41
0.42
0.43
0.44
0.45
Frequency (rad/s)
M D
C
Amplitude = 5 deg.
Amplitude = 10 deg.
Amplitude = 15 deg.
Actual MDC
(Bode Magnitude)/V
Figure 4.3: Mean MDC versus input frequency using the input of ( )wtAsin=? .
Figure 4.4 contains the clean yaw rate, the steer angle times the DC gain of
Equation (4.25), and the estimated yaw rate for a 3 rad/s simulation with a steer angle
input of 5 degrees. As seen in the figure, there also exists a phase difference in addition
to the magnitude difference. However the magnitude error is the main source of error
shown in Figure 4.3. This is because the difference in the phase between the actual
system and estimator model is minimal (compared to the differences in magnitude) as is
shown later in Figure 4.10.
62
50 52 54 56 58 60
?0.03
?0.02
?0.01
0
0.01
0.02
0.03
0.04
0.05
0.06
Time (s)
Yaw Rate (rad/s)
Actual
Steer Angle * DC gain
Estimate
Figure 4.4: Magnitude and phase difference between yaw rate and steer angle times DC
gain for ( )t3sin5=? (degrees).
Figure 4.5 shows the estimated MDC for the simulated run shown in Figure 4.4.
As seen in the figure, the estimate contains some of the 3 rad/s sine wave due to the
forgetting factor placed on the estimate.
63
50 52 54 56 58 600.425
0.43
0.435
0.44
0.445
0.45
0.455
Time (s)
Estimated M
DC
Figure 4.5: Estimate of MDC with an input of ( )t3sin5=? (degrees).
In Figure 4.6 shows the standard deviation of the estimated MDC for each velocity
and input amplitude, which should contain the same standard deviation regardless of
frequency. The Jacobian of the state equations with respect to the states (4.18) shows
that the estimate will be less uncertain when the input is larger. However, this is not
reflected in Figure 4.6. This may because due to the fact that at higher amplitudes more
of the sine wave is incorporated into its estimate.
64
0 0.5 1 1.5 2 2.5 33.6
3.8
4
4.2
4.4
4.6
4.8
5 x 10
?3
?
Frequency (rad/s)
5 degrees (?est = 0.0033)
10 degrees (?est = 0.0026)
15 degrees (?est = 0.0022)
Figure 4.6: Standard deviation of estimated MDC versus input frequency.
The Kalman Filter provides an estimate of the covariances of the states. The
square root of the covariance once the estimator has converged for the MDC state is
contained in Table 4.2 for an input amplitude of 1 rad/s. Note that the values in Figure
4.6 are greater than the values in Table 4.2, because as mentioned previously the
forgetting factor causes some of the sine wave to be incorporated into the estimate.
65
Table 4.2: Square root of covariance of MDC provided by the estimator
Sine Wave Amplitude Standard Deviation
5 degrees 3.11x10-3
10 degrees 2.18x10-3
15 degrees 1.77x10-3
Using the bode magnitudes from the system identification tests divided by
velocity for the upper bounds on the MDC estimate, Figure 4.7 shows that with no
implement the errors decrease as velocity increases. Since the value should be the same
as at zero frequency. Similarly Figure 4.8 shows that the error decreases with hitch
loading.
0
1
2
3
0
1
2
3
0.3
0.35
0.4
0.45
Frequency (rad/s)Velocity (m/s)
M D
C Bounds
Figure 4.7: Bode magnitudes divided by velocity for no implement.
66
0.5 1 1.5 2 2.5 3
0.24
0.26
0.28
0.3
0.32
0.34
0.36
0.38
0.4
Frequency (rad/s)
M D
C Bounds
No Implement
Ripper at 4 inches
Ripper at 8 inches
Ripper at 12 inches
Figure 4.8: Bode magnitudes divided by velocity for various hitch loadings at 3 mph.
The estimation of MDC can be improved through the use of the dominant poles of
the system. Equation (4.28) shows the transfer function that was used in an improved
estimator.
( )
0.9706041.962266z
0.9706041.9622661
2 +?
+?=
z
VMr xDC
? (4.28)
This requires that an additional intermediate state (?) be added to the estimator, which is
shown in Equation (4.29).
67
( ) ( )( )
?
?
?
?
?
?
?
?
?
?
?
?
+
+
+++?+
+
=
?
?
?
?
?
?
?
?
?
?
?
?
=
DCMDC
biasrbias
VxDC
DC
bias
wM
wr
wwVM
M
r
r
x ??
?
? 970604.096226.110.970604r-
1.962266r
(4.29)
The Jacobian of the measurement equations with respect to the states then changes to
Equation (4.30) when the bias can be estimated.
[ ]0101=H (4.30)
The computation of the discrete covariance matrix is performed with Equation (4.31)
using the same approximation as in Equation (4.21) and using the same value of Q1.
( ) ( )
?
?
?
?
?
?
?
?
?
?
?
? +?+?
=
1000
0100
0000
00970604.096226.11970604.096226.11 ?DCxDC
d
MVM
G (4.31)
The simulation was then run again with the new estimator. As seen in Figure 4.9
the difference between the estimated gain and the Bode magnitude at 3 rad/sec is now
half that of what was obtained using the previous estimator. The (Bode Magnitude)/V
value shown in the figure was obtained through Equation (4.32).
???
?
???
? ??=?
DC
ModelOrderSecondModelFullestsys M
V
Mag
V
Mag
V
Mag (4.32)
This corrects the Bode magnitude from Equation (4.25) for the magnitude improvement
obtained from using the second order model in the estimator. As can be seen in Figure
4.9, the estimates are now closer to the Bode magnitude, which implies that the phase
errors are having less of an effect then what was shown in Figure 4.3.
68
0.5 1 1.5 2 2.5 30.36
0.365
0.37
0.375
0.38
0.385
0.39
0.395
0.4
0.405
0.41
Frequency (rad/s)
M D
C
Amplitude = 5 deg.
Amplitude = 10 deg.
Amplitude = 15 deg.
Actual MDC
(Bode Magnitude)/V
Figure 4.9: Mean MDC versus input frequency for second order model.
Figure 4.10 shows the frequency response of the simulated system and the second
order system in the estimator for the frequencies used in the simulation. As seen in the
figure, the dominant poles do not account for all of the magnitude and phase change in
the simulated system. This is most likely caused by the set of zeros after the dominant
poles having little damping themselves. Therefore, it may be possible to design an
improved estimator by adding more dynamics or by possibly by modifying the identified
dominant poles to have less damping such that the magnitudes of the actual model and
estimator model match more closely.
69
10?1 100 101
?10
?5
0
5
10
Magnitude (dB)
10?1 100 101
?150
?100
?50
0
Phase (deg)
Frequency (rad/sec)
Actual Model
Second Order Model
Figure 4.10: Frequency response of the actual system and second order estimator
model.
Figure 4.11 shows the standard deviation of the simulations with the improved
estimator. Note that the estimates now have the same standard deviations regardless of
the frequency and that the effects of the estimate incorporating the sine wave have been
reduced.
70
0.5 1 1.5 2 2.5 32.8
3
3.2
3.4
3.6
3.8
4
4.2
4.4
4.6
4.8
x 10?3
Frequency (rad/s)
?
5 degrees (?est = 0.014)
10 degrees (?est = 0.0095)
15 degrees (?est = 0.0078)
Figure 4.11: Standard deviation of MDC for improved estimator.
4.5 Rate of Convergence
A simulation was conducted using steady state inputs to examine how long it
takes for the estimator to converge on the correct value without using the covariance
resetting, such that only the forgetting factor caused the convergence. No noise was
added to the simulated measurements passed to the estimator, but the assumed that the
nominal noise characteristics. This produces the same results as a monte carlo simulation
since the measurement noise is zero mean. Figure 4.12 shows the results from three
different offsets, for which it can be seen that they all converge at the same amount of
time. This is understandable because the Extended Kalman Filter effectively acts as a
71
closed loop controller of the estimates and the value of MDC is effectively making a step
input to the correct value.
0 10 20 30 40 500.2
0.22
0.24
0.26
0.28
0.3
0.32
0.34
0.36
0.38
Time (s)
M D
C
Actual Value
Initially 60% of Actual Value
Initially 80% of Actual Value
Initially 90% of Actual Value
Figure 4.12: Convergence from different offsets.
While the amount of time to converge is independent of the offset, it is dependant
on Vx? according to Equation (4.18). Therefore, the simulation was conducted again for
various Vx? with different noise values used for MDC (
DCM
? ). The time to converge to
within 99.9 % of the correct value was recorded. As shown in Figure 4.13, MDC
converges faster for larger values of
DCM
? and when Vx? is larger.
72
0.1
0.2
0.3
0.4
1
1.5
2
2.5
3x 10
?3
0
20
40
60
80
Vx? (m*rad/s)?
MDC
Time (s)
Figure 4.13: Time to converge within 99.9 % of the correct value.
The square root of the estimated covariances for the simulations in Figure 4.13
was also recorded and is shown in Figure 4.14. It can be seen that the estimates get
nosier as the noise value on MDC is increased. It can also be seen in Figure 4.14 that the
estimator noise placed on the MDC state can be increased, allowing for faster convergence
of the estimate, when ?xV increases without sacrificing accuracy of the MDC estimate.
However, recall that in Section 4.3 it was found that the actual accuracy of the estimate is
likely much larger than the predicted accuracy shown in Figure 4.14.
73
0.1
0.2
0.3
0.4
1
1.5
2
2.5
3x 10
?3
0
1
2
3
4
5
x 10?3
Vx? (m*rad/s)?
MDC
? E
sti
ma
te
Figure 4.14: Estimated standard deviation for MDC.
4.5 Design of Position Estimator
The states in the position estimator are north, east, and course. The input to the
position estimator is the filtered yaw rate from the yaw rate estimator. Unlike the yaw
rate estimator which uses GPS velocity to approximate longitudinal velocity, the position
estimator needs the velocity in the direction of the course angle. Therefore, GPS
provides the correct measurement of velocity for this estimator. The calculation of north
and east involves a coordinate transformation from the body fixed frame to the global
frame, as can shown in Equation (4.31), which contains the continuous state equations for
the estimator.
74
( ) ( )
( ) ( )
?
?
?
?
?
?
?
?
?
?
+
+
+
=
?
?
?
?
?
?
?
?
?
?
=
r
V
V
wr
wV
wV
E
N
x ?
?
?
?
?
sin
cos
&
&
&
& (4.31)
Since there is no velocity perpendicular to the course direction, there is only one
term in the north and east derivatives. Note that yaw rate plus the derivative of side slip
is the derivative of course ( ?? && += r ). However, it will be shown that the course
measurement provided by GPS is clean enough to compensate for neglecting the
derivative of sideslip in the estimator.
The Jacobian of the state equations with respect to the states is shown in Equation
(4.32).
( )
( )
?
?
?
?
?
?
?
?
?
? ?
=
000
cos00
sin00
?
?
?
?
V
V
F (4.32)
Since the errors in the state equations include sensor noise on the velocity and uncertainty
in the yaw rate estimate passed to the position estimator, the Jacobian of the state
equations with respect to the disturbance contains two columns as shown in Equation
(4.33).
( )
( )
?
?
?
?
?
?
?
?
?
?
=
10
0sin
0cos
?
?
G (4.33)
The first column is associated with the velocity sensor noise and the second column is
associated with the uncertainty in the yaw rate estimate.
75
The continuous process noise covariance matrix shown in Equation (4.34)
contains the covariance of the sensor noise for the velocity measurement ( V? ) and the
covariance of the yaw rate that has been passed from the yaw rate estimator (
DCM
P ).
???
?
???
?=
DCM
V
PQ 0
02? (4.34)
It should be noted that the yaw rate estimator provides a discrete covariance for its
filtered yaw rate which must be converted to continuous before being used in Equation
(4.34). However, no coordinate transformation of the yaw rate covariance is necessary
since roll and pitch angles have been assumed to be negligible.
The GPS messages have significantly more computation time than the inertial
measurements. However, if this computational delay is known, it can be accounted for in
the output equation of the estimator. In order to obtain the amount of delay in the GPS
output, an RTD GPS receiver with a pulse per second (PPS) output, a one hundred
kilohertz clock (synchronized to the GPS PPS), and two counters were used. Knowing
that the pulse per second of the RTD GPS receiver and the Starfire measurement are
syncronized, the one hundred kilohertz clock and the two counters were used to
determine the lag between when the GPS measurement occurred and when the serial
message arrived at the computer. Figure 4.15 shows the results of one experimental test.
The mean value of the delay in the figure is 0.0787 seconds. It should be noted
that some of the noise is most likely to be from the message passing between the serial
driver and the logging program, which can generally be slow. A more accurate way of
getting this lag would be to monitor the serial port?s interrupt line.
76
0 20 40 60 80 1000.076
0.078
0.08
0.082
0.084
0.086
0.088
0.09
0.092
0.094
Time (s)
Delay (s)
Figure 4.15: Delay in GPS messages
The lag in the receipt of the GPS messages can then be accounted for in the
measurement update of the estimator. Equation (4.35) shows that a simple Eulers
approximation can be used along with the estimates of the states to propagate the current
estimates back to when the measurement should have been received.
( )
( )
?
?
?
?
?
?
?
?
?
?
+?
+?
+?
=
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
? vrT
vTVE
vTVN
E
N
y
lag
Elag
Nlag
meas
meas
meas
sin
cos
(4.35)
Section 4.6 will provide further evidence that this compensation is needed. In Equation
(4.35) Tlag is the time that has elapsed between when the measurements should have been
77
received by the estimator and when they are used in the estimator. While the time update
of the position estimator is conducted at 100 hertz, the measurement update only occurs
when a GPS measurement is available, which is at 5 hertz.
In the Jacobian of the measurement equations with respect to the states, Equation
(4.36), it can be seen that a correction of the course angle is provided directly from the
position measurements because of the delay.
( )
( )
?
?
?
?
?
?
?
?
?
?
?=
100
cos10
sin01
lag
lag
TV
TV
H ?
?
?
?
(4.36)
The Jacobian of the measurement equations with respect to the measurement
noises shown in Equation (4.37) is no longer an identity matrix, since there is now a
direct contribution from the input of the estimator to the output.
?
?
?
?
?
?
?
?
?
?
?
=
lagT
U
100
0010
0001
(4.37)
The discrete covariance of yaw rate from the yaw rate estimator is then needed to
be included in the measurement noise covariance matrix shown in Equation (4.38).
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
DCM
E
N
P
R
000
000
000
000
2
2
2
??
?
?
(4.38)
In the equation N? and E? are the noises on the position states, whose errors are
dominated by its bias. However, since the estimator doesn?t have a way to remove this
bias, the estimator uses a noise value in order to filter some of the bias. The value ?? is
78
the standard deviation of the course angle measurement which is a function of velocity
and
DCM
P is the covariance passed from the yaw rate estimator.
In Figure 4.16 a comparison of the measured and estimated course shows that
despite the derivative of sideslip ( ?& ) being neglected in the position estimator, the GPS
course measurement is accurate enough to provide corrections.
10 20 30 40 50 60 70 80 90 1003.85
3.9
3.95
4
4.05
4.1
4.15
4.2
Time (s)
Course (rad)
Measured
Estimated
Figure 4.16: Comparison of measured and estimated course from experimental test.
The discrete noise values used in the Extended Kalman Filter for the position
estimator are listed in Table 4.3. These values and their continuous counterparts were
used in Equations (4.34) and (4.38).
79
Table 4.3: Discrete noise values used for the position estimator
Parameter Value Units Frequency
V? 0.05 meters/second 5 Hz
N? 0.02 meters 5 Hz
E? 0.02 meters 5 Hz
??
V
05.0 radians 5 Hz
4.6 Effects of GPS Delay
To test the effects of the GPS delay on the position estimator, the simulation in
Section 4.4 was again used. The input used was a sinusoidal steer angle with a 5 degree
amplitude at 2 mph, driving in open loop, in the direction of north. Three simulations
were run to compare the effect of the GPS delay. The first simulation had no GPS delay,
the second simulation had a 0.08 second delay with no compensation in the estimator,
and the third simulation had a 0.08 second delay which was compensated for in the
estimator. The estimators used the nominal sensor and input noise specifications, but
noiseless values were provided to the estimators so that a clearer picture of the errors
could be obtained. Additionally, no side slip was included in the simulation and hence
course reduced to heading.
Figure 4.17 shows the result for the error in the north position estimate for the
three simulations. Without compensating for the delay the vehicle estimate is behind the
true location. However for this work, longitudinal accuracy is not a degrading factor.
Some of the cosine wave can also be seen in the uncompensated system, which is due to
80
the lateral effects from the tractor not always heading north. The estimates for the
compensated system are slightly ahead of the true position, likely due to integration
errors.
0 50 100 150 200?0.08
?0.07
?0.06
?0.05
?0.04
?0.03
?0.02
?0.01
0
0.01
Time (s)
Notrh Error (m)
No Delay
Compensating For Delay
Not Compensating For Delay
Figure 4.17: North error
From Equation (4.35) it can be seen that the longitudinal error of the vehicle can
be approximated with Equation (4.39).
lagxallongitudin TVe ?= (4.39)
For the simulation, the value computed from (4.39) was 0704.0? meters, which
corresponds well with what is seen in Figure 4.17. Equation (4.40) approximates the
longitudinal error with respect to the desired path, where err? is the difference between
81
the heading of the desired path and the tractor heading during the GPS delay, and err? is
the angle between the desired path and the course angle.
( ) ( )[ ] ( ) lagerrlagerryerrxpathtoallongitudin TVTVVe ??? ? cossincos ?=??= (4.40)
Note that the above equation is an approximation since the sensor noise covariances in
the estimator will provide some filtering and there is an interaction of errors.
Figure 4.18 shows that the error in the east position from the uncompensated
system can produce errors that would affect the tractors lateral error tracking
performance. If the measurement delay is not compensated, the GPS measurement of the
tractor could be to the left of the desired path when the tractor is actually to the right of
the desired path. This would cause the controller to move the tractor in the wrong
direction leading to instability. The error shown in Figure 4.18 is due to the effect of the
longitudinal error entering into the East error at small heading angles which affects the
lateral error of the tractor since it is not always heading parallel to the line it is tracking.
Equation (4.41) provides an estimate of the lateral error in the estimator due to the GPS
measurement delay.
( ) ( )[ ] ( ) lagerrlagerryerrxpathtolateral TVTVVe ??? ? sincossin ?=+?= (4.41)
For the simulation in Figure 4.18, the maximum value of err? was 0.115 radians which
resulted in a maximum lateral error of 0.008 meters. This corresponds very well to the
predicted lateral error of 0.00808 using Equation (4.41).
The lateral errors induced from this measurement latency can also be important if
the tractor is oscillating on the verge of instability, since it could cause the tractor to go
unstable. Recall that measurement latency can be modeled as a loss of phase in the
82
system which reduces the phase margin [Franklin, 2002]. The compensated system still
contains some of the cosine wave but its errors are much more manageable.
0 50 100 150 200?0.01
?0.005
0
0.005
0.01
0.015
Time (s)
East Error (m)
No Delay
Compensating For Delay
Not Compensating For Delay
Figure 4.18: East error
Figure 4.19 shows that the heading error produces errors that appear similar to
that of the error in the lateral direction from the vehicle. Equation (4.42) approximates
the heading estimation error due to the measurement latency.
lagcourse rTe ?= (4.42)
For the simulation in Figure 4.19 the maximum heading error using Equation (4.42) is
0.00225 radians which corresponded well with the results in Figure 4.19.
83
0 50 100 150 200
?2
?1
0
1
2
3
4 x 10
?3
Time (s)
Heading Error (rad)
No Delay
Compensating For Delay
Not Compensating For Delay
Figure 4.19: Heading Error
The covariance estimates of the estimators with no GPS measurement delay and
with the delay compensation are shown in Figure 4.20. As can be seen, the uncertainty in
the compensated system is only slightly higher than without the delay. Additionally, no
lag can be seen in the covariance estimates between the two simulations. The north
estimate is also cleaner then east estimate. This is because the uncertainty in course does
not enter into the position state in the direction of course (as seen in Equation (4.32)),
which is mostly in the direction of north in this simulation.
84
50 60 70 80 90 100
2
2.5
3
3.5
x 10?6
North Covariance
50 60 70 80 90 100
6
8
10
x 10?5
East Covariance
50 60 70 80 90 100
2
4
6
8
10
x 10?5
Heading Covariance
Time (s)
Compensating For Delay
No Delay
Figure 4.20: Comparison of covariances for an estimator with no GPS measurement
delay and an estimator that compensates for the delay.
85
4.7 Summary and Conclusions
Details on the design of an estimator to estimate the yaw rate, yaw rate bias, and
MDC were given in this chapter. A method was given that determined when to estimate
the yaw rate bias and MDC so that disturbances did not dominant the estimation of MDC.
It was shown that with the covariance resetting, the estimate of MDC would converge
quickly. A solution to estimating the bias during low levels of excitation was also
explained and the effect of the neglected dynamics on the estimation was displayed.
It was shown that most of the errors occurred because of magnitude errors
between the estimator model and actual system and not phase errors. It was also shown
that the covariance estimate for the estimated MDC was lower than the actually accuracy
of the estimated MDC. The errors in the estimate of MDC caused by the magnitude errors
were found to decrease as the velocity and hitch loading were increased. A method of
compensating the adaptation for these errors was found by approximating the pole
locations. The rate of convergence of MDC due to the forgetting factor was found to be
independent of its offset and was instead simply a function of the longitudinal velocity
times the steer angle. It was shown that MDC would converge faster with higher
amplitudes of excitation and with a larger forgetting factor on the estimated state.
However, increasing the forgetting factor resulted in a noisier estimate of MDC.
The design of the position estimator for providing faster updates of position was
also detailed. This estimator was designed to compensate for the delay in the receipt of a
GPS message since it was shown that the effect of this lag could cause large position
errors. Equations were provided that approximated the errors induced by the GPS lag.
86
CHAPTER 5
EXPERIMENTAL IMPLEMENTATION
5.1 Introduction
The results of experimental tests using the controllers and estimators discussed in
previous chapters are given in this chapter. A John Deere 8420 tractor was used for the
experimental tests. A Bosch gyroscope was used to measure yaw rate and a linear
potentiometer was used to measure the steer angle. Both measurements were
synchronized to GPS measurements using a GPS pulse per second (PPS). The GPS
measurements were provided by a Starfire GPS receiver. The delay in the GPS messages
was obtained using a ten kilohertz clock synchronized to the one hertz clock (PPS) as
discussed in Chapter 4.
5.2 Trajectory Design
In this thesis all tractor trajectories consist of straight line segments. Figure 5.1
displays a desired path in a North-East coordinate system. The desired path is defined by
a reference point (Nref,Eref) and a reference heading (?ref). The point (N,E) is the location
of the tractor. (Ndes,Edes) is the desired point on the path which is defined by a line
perpendicular to the path that goes to the point (N,E). The tractor position can then be
described as a longitudinal distance from the reference point and a lateral distance from
the path.
87
Figure 5.1: Tracking a straight line.
Using the coordinate transformation given in Equation (5.1) the longitudinal and
lateral distances can be found from the North-East coordinate system.
( ) ( )
( ) ( ) ??
?
??
?
?
?
??
?
??
?
?=??
?
??
?
ref
ref
refref
refref
EE
NN
y
x
??
??
cossin
sincos
(5.1)
It was shown in Chapter 4 that higher frequencies can cause the estimator to
estimate the incorrect value of MDC. Thus, it can be an advantageous to prevent the
tractor from executing a step input by redrawing the path when the lateral distance is
large and conducting a steady-state turn to the path. Additionally, if the tractor is at too
large of an offset it will approach the desired path perpendicular to it, creating an
unsatisfactory response. Therefore, the desired path is redefined using a circular path,
which is tangential to the desired straight line trajectory, when the tractor is a large
y
x
(Ndes,Edes)
(N,E)
(Nref,Eref)
?ref
North
East
Desired
Path
88
distance away from the desired line. A description of the circular path is shown in Figure
5.2.
Figure 5.2: Curved trajectory to the straight line.
The value z is the distance between the circular path and the straight line path.
When it is determined that the offset from the desired straight line path is too large, the
circular path is formed by setting the current lateral offset as shown in the equation
below.
yxinit = (5.2)
The distance d that is desired for when the straight line path and the curved path
converge is then computed as a function of the initial offset using Equation (5.3), where l
is a value larger or equal to one.
z
d
|R|
(Ndes,Edes)
x
(Nref,Eref)
(N,E)
y
89
initinit lxd = (5.3)
As will be seen in the next few equations l sets the rate to which the tractor will (limited
by the tractor dynamics) approach the line trajectory. In this work l was chosen to be 4.
The radius can then be calculated by solving for the hypotenuse of a triangle that
takes into account the initial offsets as shown below.
( ) ( )222 initinit xradiusdR ?+= (5.4)
Substituting Equation (5.3) into Equation (5.4) results in Equation (5.5), where the sign
of R contains the information corresponding to which side of the straight line the circular
path is on.
initx
lradius
2
12 += (5.5)
Solving for Equation (5.4) with the current offsets instead of the initial offsets, the
distance between the circular and straight path while the tractor is moving can be solved
as shown below.
( ) 22 dradiusradiussignradiusx ??= (5.6)
The lateral distance to the circular path can then be found using Equation (5.6) and the
distance y.
5.3 Results
To first validate the ability of the algorithms to provide correct estimates of MDC.
Experimental test were conducted. For these experiments, the tractor was commanded to
track a straight line from an initial offset. The estimator used an initial value of MDC set
to 0.25. Figure 5.3 shows the results of two tests. The on-line experimentally estimated
90
MDC values have less than a five percent error from the true value identified in Chapter 2
(MDC = 0.334).
6 6.5 7 7.5 8 8.5 9 9.5 10
0.22
0.24
0.26
0.28
0.3
0.32
Time (s)
M D
C
Figure 5.3: Estimation of MDC during experimental tests.
Tests were also conducted to determine how the lateral error was affected by
changes in velocity. Figure 5.4 shows that the lateral error increases only slightly with
increased velocity.
91
0.5 1 1.5 2 2.5?0.01
0
0.01
0.02
0.03
mean (m)
0.5 1 1.5 2 2.50.005
0.01
0.015
0.02
0.025
? (m)
Velocity (m/s)
Figure 5.4: Lateral error versus velocity
The tracking performance of the controller without an implement is displayed in
Figure 5.5, where five experimental results are shown. As seen in the figure, the tracking
performance appears to remain fairly constant for each of the tests.
92
20 30 40 50 60 70 80 90 100?0.08
?0.06
?0.04
?0.02
0
0.02
0.04
0.06
0.08
Time (s)
Lateral Position (m)
Figure 5.5: Lateral error without an implement.
In Figure 5.6 the tracking performance of the controller with a four shank ripper
at a twelve inch depth is shown for five experimental tests. As shown in the above
figure, the lateral tracking accuracy degrades slightly with the implement.
93
20 30 40 50 60 70 80 90 100?0.08
?0.06
?0.04
?0.02
0
0.02
0.04
0.06
0.08
Time (s)
Lateral Position (m)
Figure 5.6: Lateral error with a four shank ripper at a twelve inch depth.
The corresponding mean and standard deviation of the experimental tests with
and without an implement are shown in Table 5.1. As can be seen in the table, the
standard deviations of the controlled runs with an implement are larger than without an
implement. This is most likely due to the fact that the implement induces larger
disturbances into the system. However, the fact that the experimental control accuracy
with and without an implement is fairly consistent shows that the adaptive control
algorithm, based on on-line estimation of MDC, provides an effective means to handling
the tractor dynamic variations.
94
Table 5.1: Statistics of lateral error at a constant velocity
No Implement Ripper at 12? depth
Run Mean (m) ? (m) Mean (m) ? (m)
1 0.00491 0.0191 0.00496 0.0187
2 0.00639 0.0120 -0.0105 0.0167
3 0.0109 0.0156 -0.00747 0.0221
4 0.00454 0.0167 -0.00646 0.0228
5 0.00453 0.0205 -0.0118 0.0210
Average 0.00625 0.0168 -0.0252 0.0202
5.4 Summary and Conclusions
This chapter has detailed the experimental implementation of the estimation and
control algorithms developed in this thesis. The trajectories used for the tractor controller
were discussed. Experimental results showing the accuracy of the estimation algorithms
were then provided. Experimental tests demonstrated that the tracking response changed
little with velocity. The average standard deviations while varying velocities was found
to be 0.016 m. It was also shown that the standard deviation of the lateral position
changed little with hitch loading with a standard deviation of 0.0168 m without an
implement and 0.0202 m with a 4 shank ripper at a 12? depth while driving at 2 mph.
95
CHAPTER 6
FIXED POINT IMPLEMENTATION OF YAW RATE ESTIMATOR
6.1 Introduction
Implementation of a Kalman filter is difficult in a fixed point microprocessor.
The propagation of the covariance presents the largest challenge, since it can be poorly
conditioned if small machine precision is used. However, filters that propagate the
square root of the covariance are more easily manageable, because they propagate a
matrix that has the square root of the condition number of what the Kalman filter
requires. Examples of square root Kalman filters include the square root covariance filter
and square root information filter [Anderson, 1979; Bierman, 1977; Maybeck 1979a].
In this chapter, a square root covariance filter (SRCF) will be used to show that
the Kalman filter from Chapter 4 can be implemented using fixed point math. The
estimator provides an estimate of the slope of the DC gain with respect to velocity of the
transfer function between steer angle and yaw rate (MDC).
6.2 Square Root Covariance Filter
The square root covariance filter is derived from the Kalman filter equations. It
uses orthogonal matrices, T1 and T2, shown in Equations (6.2) and (6.3) to solve for upper
triangular matrices.
( )111| , ??? = kkkk uxfx (6.1)
96
??
?
??
?=
??
?
??
? ??
TT
TT
k
T
kk
LQ
FSTS
2/1
1
1
1|
0 (6.2)
The process of splitting the matrix into an orthogonal and upper triangular matrix is
known as QR decomposition and the most popular methods used for square root Kalman
filters are the householder [Golub, 1989] and modified Gram-Schmidt methods. The
householder method is used in this work. Equations (6.1) and (6.2) consist of the time
update of the estimator. Note that the state time update is the same as in the Kalman
filter.
Equations (6.3-6.5) show the measurement update equations for the estimator.
( )
???
?
???
?=
???
?
???
? +
??
T
kk
TT
kk
T
T
k
TTT
SHS
RT
S
KPHHR
1|1|
2/1
2
2/1 0
0
~
(6.3)
( ) 2/1~ ?+= PHHRKK T (6.4)
( )1|1| ?? ?+= kkkkk HxzKxx (6.5)
The QR factorization displayed in Equation (6.3) returns terms that are used for both the
covariance square root and the Kalman gain. The state measurement update shown in
(6.5) is the same as the Kalman filter state measurement update. In Equation (6.4) it is
evident that a matrix inverse is needed if there is more then one measurement. However,
sequential processing is used in square root filters to eliminate the need for a matrix
inversion.
When compared with the Kalman filter the square root covariance filter takes
more computation time [Bierman, 1977; Kaminski, 1971]. This is due to the fact that the
SRCF needs more multiplications, additions, divisions, as well as square root
calculations.
97
6.3 Fixed Point Math
For the fixed point implementation, additions and subtractions were performed as
normal. Multiplications were performed using Equation (6.6), where >> is the right shift
operator and N is the number of bits to shift.
Nab >> (6.6)
Divisions were carried out using Equation (6.7), where << is the left shift operator.
( ) bNa /<< (6.7)
An overflow would result if the answer to the equations was larger then 215, since
variables were defined as 16 bit signed integers.
The householder method was used on Equation (6.8) as an example of the
accuracy of the fixed point implementation of the algorithm.
?
?
?
?
?
?
?
?
?
?
?
?
?
?=
412756119
1129645210
89623261435
15543156345
A (6.8)
The householder method reduces Equation (6.8) into A=QR, by doing a column wise
reduction.
Equation (6.9) shows the result of the decomposition when floating point is used.
?
?
?
?
?
?
?
?
?
?
?
?
?
???
=
6349.184000
4878.3523396.12100
2676.176926.8164332.2990
0321.324095.1254271.962026.607
R (6.9)
Performing the decomposition in fixed point results in errors build upon one another as
each column is reduced as shown in Equation (6.10).
98
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
???
=
214002
33313034
18280129512
3012692605
R (6.10)
This implies that the first state will have the most accurate covariance estimate.
6.4 Estimator Model
Since the first state will have the most accurate covariance estimate, the slope of
the DC gain (MDC) was made the first state. The yaw rate state in the estimator was set to
be scaled by 9=N bits from the units of degrees/second. Meaning that the yaw rate state
would be yaw rate in degrees/second times 29. The measured yaw rate, velocity, and
steer angle were also scaled by 9 bits. MDC was scaled by N bits plus 4=DCN bits or
( )DCNN +2 . The bias was scaled by N bits plus 4=
biasN bits or
( )biasNN +2 . MDC and the bias
were scaled by extra bits in order to reduce the error in those states caused by the fact that
MDC and the bias were smaller in magnitude then the yaw rate state.
Equation (6.11) shows the state time update equations, where the states are MDC,
yaw rate, and the yaw rate bias.
( )[ ]
1
1
1
?
?
?
=
>>>>>>=
=
kbiaskbias
kkDCkDCk
kDCkDC
rr
NNVNMr
MM
? (6.11)
The MDC and rbias values are simply the previously computed values. The MDC state has
to be shifted down in the yaw rate computation since it has a larger scaling in the
estimator. The order of the shifts can be changed, as long as it does not increase the risk
of overflow.
99
Equations (6.12-6.14) display the Jacobian matrices used in the square root
covariance filter.
( )
?
?
?
?
?
?
?
?
?
?
<<
+>>
<<
=
N
NNV
N
F DC
100
00
001
? (6.12)
( )[ ]biasNNNH ?<<<<= 110 (6.13)
?
?
?
?
?
?
?
?
?
?
<<
<<
<<
=
N
N
N
L
100
010
001
(6.14)
Note that in F, the term related to the partial of the yaw rate with respect to MDC is shifted
down due to the extra scaling on the parameter. Additionally in H, the partial derivative
with respect to the bias is shifted up less due to the extra scaling on the bias.
Equations (6.15) and (6.16) contain the square root of the noise covariances,
which are displayed as a function of the extra scaling on the states.
?
?
?
?
?
?
?
?
?
?
<<
<<
=
bias
DC
N
N
Q
600
01540
004
2/1 (6.15)
[ ]682/1 =R (6.16)
The noise values had to be larger than the nominal values given in Chapter 4. This was
done do to the fact that even though the covariance may not converge to zero, the Kalman
gain can become zero if the covariance is too small.
100
The initial covariance square root is shown in Equation (6.17).
?
?
?
?
?
?
?
?
?
?
<<
=
biasN
S
46000
000
000
(6.17)
The initial value for the yaw rate covariance is set to zero, since the estimator will be
initialized with the most recent yaw rate measurement. The MDC covariance square root
initial value is also set to zero because adaptation will not be performed until there is
adequate excitation.
To determine when to estimate the parameter, Equation (6.18) was calculated
recursively, where 6=wN was chosen to prevent an overflow.
( )? = ? >>= 19 0 2m wmk Nstatus ? (6.18)
The equation is simply an estimate of the covariance of the steer angle over a short period
of time. This period was chosen to be one second at a twenty hertz resolution.
The value of status was arbitrarily chosen to be 1920, therefore, if 1920__