Relative Positioning of Unmanned Ground Vehicles Using Ultrasonic
Sensors
Except where reference is made to the 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.
Harold Paulk Henderson, Jr.
Certificate of Approval:
Dan Marghitu
Professor
Mechanical Engineering
David Bevly, Chair
Associate Professor
Mechanical Engineering
Thaddeus Roppel
Associate Professor
Electrical and Computer Engineering
Joe F. Pittman
Interim Dean
Graduate School
Relative Positioning of Unmanned Ground Vehicles Using Ultrasonic
Sensors
Harold Paulk Henderson, Jr.
A Thesis
Submitted to
the Graduate Faculty of
Auburn University
in Partial Fulfillment of the
Requirements for the
Degree of
Master of Science
Auburn, Alabama
May 10, 2008
Relative Positioning of Unmanned Ground Vehicles Using Ultrasonic
Sensors
Harold Paulk Henderson, Jr.
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 Author
Date of Graduation
iii
Vita
Harold Paulk Henderson, Jr. was born in Tifton, GA on May 7, 1976. The family
moved to Hattiesburg, MS while his father pursued a Ph.D at the University of Southern
Mississippi. He graduated from Tift County High School in 1994 and accepted an appoint-
ment to the United States Military Academy at West Point, NY. He graduated in 1998
with a B.S. in Mechanical Engineering and was commissioned as a Second Lieutenant in
the U.S. Army. He served as a Platoon Leader, Troop Executive Officer, Squadron Logistics
Officer, and Troop Commander. In 2000 he married Carrie Hain Benefield who is an Envi-
ronmental Scientist with URS. He has two sons, Hain and Ben. In 2006 he was selected to
return to West Point to serve as an Instructor in the Department of Civil and Mechanical
Engineering. During his studies at Auburn University he received a promotion to Major.
iv
Thesis Abstract
Relative Positioning of Unmanned Ground Vehicles Using Ultrasonic
Sensors
Harold Paulk Henderson, Jr.
Master of Science, May 10, 2008
(B.S., United States Military Academy, 1998)
113 Typed Pages
Directed by David Bevly
In this thesis, a Global Positioning System/Inertial Navigation System (GPS/INS) is
developed and applied to a tracked Unmanned Ground Vehicle (UGV) to provide estimates
of position, heading, and velocity. The navigation estimator is then augmented with ul-
trasonic sensors to accurately determine the relative position of a pair of tracked UGVs.
The estimator provides estimates on critical system parameters to allow for automation
or collaboration. The capabilities of UGVs can be greatly increased through automation
and collaboration of multiple UGVs. Automation reduces the operator workload and al-
lows the UGV to continue operations during a loss of communication. Collaboration allows
multiple small UGVs to accomplish tasks previously requiring a single large UGV. A mi-
crocontroller uses the ultra-precise Pulse Per Second (PPS) from the GPS to trigger the
ultrasonic sensors. This configuration minimizes the problems traditionally encountered
with use of ultrasonic sensors in an outdoor environment while providing additional infor-
mation to the navigation estimator. Small, low cost sensors using MEMS technology are
employed to minimize size and cost, providing a solution that can be implemented on current
v
UGVs. Experimental results are presented to compare the performance using GPS alone,
GPS/INS, and GPS/INS/Ultrasonic sensors for estimating relative positions and headings
of multiple UGVs.
vi
Acknowledgments
I would like to thank my advisor, Dr. David Bevly, for his support and direction
during the entire process. He provided the resources and the expertise necessary to make
the research a reality. I would also like to thank Dr. Dan Marghitu for his instruction in
class on kinematics and Dr. Thaddeus Roppel for access to the ultrasonic sensors that were
critical to the execution of the research.
My success in both this research and in class is due to the assistance, generosity and
patience of the members of the GAVLAB. They provided invaluable assistance and guidance
throughout the process. Without their experience and previous work, this research would
not have been possible.
I would like to thank my family for their constant emphasis on the importance of
education and the vital role of educators. Their support and toleration of an incessant
curiosity have allowed me to achieve the level of success I currently enjoy and have a lot of
fun along the way.
I would like to thank my sons, Hain and Ben, for constantly providing direction as
to what is truly important in life. Regardless of how poorly the day had gone, they were
always excited to see me and genuinely interested in ?Dad?s robot?.
I must thank my wife for her continued support in yet another adventure. Her unwa-
vering support during deployments, field problems, research, and the joys of parenting have
kept me going and constantly remind me of how blessed I am.
vii
Style manual or journal used Journal of Approximation Theory (together with the style
known as ?aums?). Bibliography follows van Leunen?s A Handbook for Scholars.
Computer software used The document preparation package TEX (specifically LATEX)
together with the departmental style-file aums.sty.
viii
Table of Contents
List of Figures xi
List of Tables xiv
1 Introduction 1
1.1 Congressional Mandate . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Prior Art . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.3 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.4 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2 Overview of Base Matilda Hardware, Instrumentation Design and Soft-
ware Architecture 7
2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.2 MATILDA . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.2.1 Mobile Base Unit (MBU) . . . . . . . . . . . . . . . . . . . . . . . . 8
2.2.2 Operator Control Unit (OCU) . . . . . . . . . . . . . . . . . . . . . 10
2.2.3 Payloads . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.3 Instrumentation Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.4 Sensor Suite . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.4.1 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.4.2 Inertial Measurement Unit (IMU) . . . . . . . . . . . . . . . . . . . 16
2.4.3 Encoder . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.4.4 Ultrasonic Sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.5 Processor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.6 Rabbit Core Module 4100 (RCM4100) . . . . . . . . . . . . . . . . . . . . . 23
2.7 Completed Test Rig . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
2.8 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
2.8.1 Dynamic C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
2.8.2 C++ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
2.8.3 MATLAB . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
2.9 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3 UGV Navigation Estimator 30
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.2 Vehicle Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.2.1 Kinematic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3.3 Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
ix
3.3.1 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . 42
3.3.2 Measurement Update . . . . . . . . . . . . . . . . . . . . . . . . . . 43
3.3.3 Time Update . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
3.3.4 Hybrid Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
3.3.5 Heading Wrap . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
3.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4 Relative Position Model 48
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
4.2 State Transition Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
4.3 Inputs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
4.4 Input Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
4.5 Measurements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
4.6 Observation Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
4.7 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
4.8 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5 Experimental Work 57
5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
5.2 Sensor Data . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
5.2.1 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.2.2 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
5.2.3 Ultrasonic Sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
5.3 Navigation Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
5.3.1 Simulated Static Data . . . . . . . . . . . . . . . . . . . . . . . . . . 62
5.3.2 Simulated Dynamic Data . . . . . . . . . . . . . . . . . . . . . . . . 65
5.3.3 Experimental Performance . . . . . . . . . . . . . . . . . . . . . . . 68
5.4 Relative Position Estimator . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
5.4.1 Simulated Static Lead with Static Trail . . . . . . . . . . . . . . . . 73
5.4.2 Simulated Dynamic Lead with Static Trail . . . . . . . . . . . . . . . 77
5.4.3 Simulated Dynamic Lead with Dynamic Trail . . . . . . . . . . . . . 80
5.4.4 Experimental Performance . . . . . . . . . . . . . . . . . . . . . . . 82
5.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
6 Conclusion 90
6.1 Summary of Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
6.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
Bibliography 94
A Nomenclature 97
x
List of Figures
2.1 Mobile Base Unit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2 Operator Control Unit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.3 Manipulator Arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.4 Arm Axes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.5 Printed Circuit Board . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.6 uBlox RCB-4H . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.7 IMU 605 Top and Side . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.8 Optical Encoder . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
2.9 Parallax Ping Ultrasonic Sensor . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.10 Ultrasonic Sensor Theory of Operation . . . . . . . . . . . . . . . . . . . . . 20
2.11 Scenarios for Ultrasonic Measurements . . . . . . . . . . . . . . . . . . . . . 21
2.12 Rabbit Core Module 4100 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.13 Lead UGV Test Rig . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
2.14 Test Rig used to replicate Trail UGV . . . . . . . . . . . . . . . . . . . . . . 25
2.15 Test Configuration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.1 GPS Position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.2 Integration of Bias and Noise . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.3 Instantaneous Center of Rotation Location Comparison . . . . . . . . . . . 33
3.4 Lateral track velocity of UGV during pure rotation . . . . . . . . . . . . . . 34
3.5 Tracked UGV ICR Locations . . . . . . . . . . . . . . . . . . . . . . . . . . 35
xi
3.6 Schematic of Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3.7 Relation of Course (?), Heading (?), and Sideslip (?) . . . . . . . . . . . . 41
4.1 Description of Range and Bearing Measurement . . . . . . . . . . . . . . . . 53
4.2 Derivation of Bearing Measurement . . . . . . . . . . . . . . . . . . . . . . . 54
5.1 Schematic Illustrating the Range and Bearing Calculation . . . . . . . . . . 60
5.2 Experimental Ultrasonic Range Measurements . . . . . . . . . . . . . . . . 61
5.3 Experimental Ultrasonic Bearing Measurements . . . . . . . . . . . . . . . . 61
5.4 Simulated Static Position Estimate . . . . . . . . . . . . . . . . . . . . . . . 63
5.5 Simulated Static Heading Estimate . . . . . . . . . . . . . . . . . . . . . . . 63
5.6 Simulated Static Position Error . . . . . . . . . . . . . . . . . . . . . . . . . 64
5.7 Simulated Static Heading Error . . . . . . . . . . . . . . . . . . . . . . . . . 64
5.8 Simulated Linear Position Estimate . . . . . . . . . . . . . . . . . . . . . . . 65
5.9 Simulated Linear Heading Estimate . . . . . . . . . . . . . . . . . . . . . . 65
5.10 Simulated Linear Position Error . . . . . . . . . . . . . . . . . . . . . . . . . 66
5.11 Simulated Linear Heading Error . . . . . . . . . . . . . . . . . . . . . . . . 66
5.12 Simulated Square Position Estimate . . . . . . . . . . . . . . . . . . . . . . 67
5.13 Simulated Square Heading Estimate . . . . . . . . . . . . . . . . . . . . . . 67
5.14 Simulated Chicane Position Estimate . . . . . . . . . . . . . . . . . . . . . . 68
5.15 Simulated Chicane Heading Estimate . . . . . . . . . . . . . . . . . . . . . . 68
5.16 Experimental Lead Unit Static Estimate . . . . . . . . . . . . . . . . . . . . 69
5.17 Experimental Trail Unit Static Estimate . . . . . . . . . . . . . . . . . . . . 69
5.18 Line Lead Position Estimate . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
5.19 Line Trail Position Estimate . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
xii
5.20 Line Lead Heading Estimate . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
5.21 Line Trail Heading Estimate . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
5.22 Elliptical Lead Position Estimate . . . . . . . . . . . . . . . . . . . . . . . . 72
5.23 Elliptical Trail Position Estimate . . . . . . . . . . . . . . . . . . . . . . . . 72
5.24 Lead Heading Estimate . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
5.25 Trail Heading Estimate . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
5.26 Simulated Static Position Estimate . . . . . . . . . . . . . . . . . . . . . . . 74
5.27 Simulated Static Position Error . . . . . . . . . . . . . . . . . . . . . . . . . 74
5.28 Simulated Static Heading Estimate . . . . . . . . . . . . . . . . . . . . . . . 76
5.29 Simulated StaticHeading Error . . . . . . . . . . . . . . . . . . . . . . . . . 76
5.30 Simulated Dynamic/Static Position Estimate . . . . . . . . . . . . . . . . . 78
5.31 Simulated Dynamic/Static Position Error . . . . . . . . . . . . . . . . . . . 78
5.32 Simulated Dynamic/Static Heading Estimate . . . . . . . . . . . . . . . . . 79
5.33 Simulated Dynamic/Static Heading Error . . . . . . . . . . . . . . . . . . . 79
5.34 Simulated Dynamic/Dynamic Position Estimate . . . . . . . . . . . . . . . 81
5.35 Simulated Dynamic/Dynamic Position Error . . . . . . . . . . . . . . . . . 81
5.36 Simulated Dynamic/Dynamic Heading Estimate . . . . . . . . . . . . . . . 81
5.37 Simulated Dynamic/Dynamic Heading Error . . . . . . . . . . . . . . . . . 81
5.38 Experimental Static/Static Position Estimate . . . . . . . . . . . . . . . . . 83
5.39 Experimental Static/Static Heading Estimate . . . . . . . . . . . . . . . . . 84
5.40 Experimental Dynamic/Dynamic Position Estimate . . . . . . . . . . . . . . 85
5.41 Experimental Dynamic/Dynamic Heading Estimate . . . . . . . . . . . . . 86
5.42 Position Estimates using Navigation Estimators . . . . . . . . . . . . . . . . 87
5.43 Position Estimates using Relative Position Estimator . . . . . . . . . . . . . 87
5.44 Relative Position Error on Elliptical Path . . . . . . . . . . . . . . . . . . . 88
xiii
List of Tables
2.1 MATILDA Specifications . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.2 Receiver Specifications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.3 Inertial Measurement Unit Specifications . . . . . . . . . . . . . . . . . . . . 17
2.4 Effect of Temperature on Measurement . . . . . . . . . . . . . . . . . . . . . 20
3.1 Process Noise Variances . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
3.2 Sensor Measurement Noise Variances . . . . . . . . . . . . . . . . . . . . . . 42
5.1 Lead Accelerometer Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.2 Trail Accelerometer Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.3 Lead Gyro Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
5.4 Trail Gyro Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
5.5 Lead GPS Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
5.6 Trail GPS Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
5.7 Ultrasonic Sensor Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
5.8 Simulated and Experimental Position Standard Deviation . . . . . . . . . . 70
5.9 Simulated Static Position Estimate Error and Standard Deviation . . . . . 75
5.10 Process Noise Covariance . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
5.11 Measurement Noise Covariance . . . . . . . . . . . . . . . . . . . . . . . . . 77
5.12 Simulated Linear Estimate Error and Standard Deviation . . . . . . . . . . 80
5.13 Noise Characteristics Comparison of Estimator Position Standard Deviation 83
xiv
Chapter 1
Introduction
Unmanned Ground Vehicles (UGV) are robotic platforms that provide an extension
of human capability. They are designed to carry some type of payload, but not a human
occupant. Hazardous operating environments necessitate the development of the majority
of UGVs. The environments include Explosive Ordinance Disposal (EOD), search and
rescue in collapsed buildings, hazardous material (HAZMAT) detection, and manipulation
of nuclear fuel. The inherent risk of these environments limits the incorporation of human
occupants due to the cumbersome nature of the safeguards required.
UGVs can be classified by the manner in which they are controlled [11]. Tele-operated
UGVs receive guidance instructions via a communications link from an operator. The link
may be a physical connection such as a wire or fiber optic tether; wireless connection; or a
combination of the two. The simplest example would be a toy radio controlled car which is
entirely dependent on instructions from the controller.
Autonomous UGVs use onboard sensors to gather information about their surroundings
and then determine the appropriate action to take. These sensors include accelerometers,
gyroscopes, range finders (laser, ultrasonic, radar, camera), odometers, and GPS receivers.
There are multiple examples of this type of UGV including the entries for the Defense
Advanced Research Projects Agency (DARPA) Urban Challenge that navigate through an
urban environment without operator input [10]. This classification should not be confused
with automated UGVs whose path is predetermined via a mechanical rail or fixed trail of
magnetic sensors [11].
1
Supervisory Control is a combination of the two previous classifications [11]. Inputs
from the operator and the onboard sensors are used by the UGV to determine the appro-
priate output. This approach is used by the Jet Propulsion Laboratory (JPL) to control
the Mars Exploration Rovers [5]. Complete control is prevented by the latency due to the
vast operating distances and the limited communication bandwidth. Instruction sets are
transmitted that request information or changes in position. The rover then uses onboard
sensors with onboard algorithms to determine the best course of action to accomplish the
instruction set.
1.1 Congressional Mandate
The importance of UGVs has received additional emphasis in the form of a Con-
gressional mandate in the National Defense Authorization Act for Fiscal Year 2001. The
mandate established a goal that unmanned, remotely controlled technology be fielded so
that one-third of the operational ground combat vehicles are unmanned by 2015. DARPA
staged three separate races to further the technologies needed to achieve this goal. The
first DAPRA Grand Challenge was held in 2004 over a 142 mile desert course. No vehicle
successfully completed the entire course. The second DARPA Grand Challenge was held in
2005 where four vehicles successfully completed a 132 mile course in the required 10 hours.
The DARPA Urban Challenge was held in 2007 and featured a mock urban environment
requiring vehicles to interact with other vehicles while obeying traffic control devices.
The technologies developed for primarily military applications are trickling into com-
mercial areas and even into homes. iRobot has successfully marketed UGVs for use by
consumers in their homes. They can vacuum, mop or even clean gutters. These robots are
2
very simple, but learn and perform an assigned task with little or no human interaction
[13]. Low cost sensors coupled with improving battery technology are allowing UGVs to
accomplish an ever increasing list of tasks.
1.2 Prior Art
The integration of GPS and IMU measurements for land navigation has been well
studied and implemented in a variety of forms [1]. The bias free measurements from the
GPS compliment the higher rate measurements from the IMU. Improving the reliability
and the accuracy of the estimators has taken several different paths. An obvious approach
is to use more accurate GPS receivers, Differential GPS (DGPS), and higher grade IMUs,
tactical and navigation grade [12]. A second approach improved performance of the esti-
mator by augmenting the GPS/IMU measurements with map data and additional sensor
measurements [18]. These approaches focused on an estimator for a single system. An
alternate approach was presented and evaluated for localization of a group of robots us-
ing a distributed Kalman Filter [21]. This approach used varying sensor configurations on
each robot in the group to minimize hardware requirements without sacrificing performance,
while spreading the calculations for the filter over several processors. These approaches have
provided performance gains, but have significantly increased the cost or the complexity of
system required to implement the approach.
Modeling of small UGVs has focused primarily on wheeled differential drive vehicles in
an indoor environment [6][11][27]. The complexity of modeling the dynamics of a tracked
UGV can be attributed to the highly nonlinear interaction between track and terrain. One
approach for use on small UGVs focused on a dynamic model based on the Instantaneous
3
Center of Rotation [16]. This research used experimental identification to calculate param-
eters based on a given UGV configuration and operating surface. This provided reasonable
results as long as the operating conditions for the UGV remained constant. A second
approach used finite element analysis to simulate the interaction between the track and
terrain of a M113 Armored Personnel Carrier [22]. This approach incurred a tremendous
computational burden and is not conducive to a real time application on a small UGV.
Since GPS became operational, its use has continually expanded from just providing
position measurements. Ultra-precise timing is necessary for the system to calculate the
user?s position. GPS receivers provide time measurements via the output messages and
through the Pulse Per Second signal. The correct GPS receiver can provide time measure-
ments accurate to 10 ns world wide. A typical OEM receiver with an antennae uncertainty
of 10 m will provide a measurement accurate to 100 ns [15]. The GPS receiver provides
an ultra precise time measurement that is virtually free of drift and with a bias sufficiently
small to be neglected for most applications.
Ultrasonic sensors provide a simple method to measure distance without physical con-
tact. Ultrasonic sensors have successfully been used to detect and avoid obstacles for the
last 20 years [7]. The simplicity of the sensor allows for ease of implementation but limits
the accuracy and reliability in certain situations [14]. The use of various filters including the
Kalman Filter has improved the accuracy of the measured time of flight, and ultimately the
distance measured [2]. Research has also extended the use of the ultrasonic sensor outside
by modulating the output noise [26]. The approach uses a Code Division Multiple Access
approach to allow multiple sensors to operate at the same time in close proximity without
sacrificing reliability.
4
1.3 Contributions
This thesis developed and tested a concept that used ultrasonic sensors triggered by
GPS receivers to provide relative measurements on a pair of UGVs. The proposed con-
figuration was shown to reliably provide these measurements. The resulting measurement
were then used to estimate position and heading. A summary of the specific contributions
provided in this thesis are listed below.
? Designed and implemented IMU/GPS sensor suite for tracked UGV
? Developed tracked UGV navigation estimator
? Developed method to trigger ultrasonic sensors with GPS PPS to reliably measure
relative position and orientation of UGV
? Examined the accuracy of position and heading estimates of navigation and relative
position estimators using simulated and experimental data
? Compared performance gains achieved in relative position and heading estimates using
relative measurements
1.4 Outline
Chapter 2 provides an overview of the hardware and software used to implement the
relative position estimator. Each sensor is presented and the manufacturer?s published per-
formance parameters are presented for comparison. Images of the components are provided
to convey the size of the sensors and the completed sensor suite. The separate software
packages and their implementation are covered in this chapter as well. The designation
assigned to each program and the function it serves is also explained.
5
Chapter 3 gives an overview of Kalman and Extended Kalman Filters in addition to
the navigation estimator developed for a single UGV. The kinematic model used for the
EKF is presented with justification for its selection over a dynamic model. The equations
used for the model are provided, and the process used to create the matrices necessary to
implement the EKF for a navigation estimator are shown.
Chapter 4 expands the navigation estimator from a single UGV to include both UGVs
as well as additional measurements. The implementation of the ultrasonic sensors to mea-
sure range and bearing is shown along with the equations defining the measurements. The
matrices necessary to implement the EKF are derived and presented.
Chapter 5 covers the testing with simulated and experimental data used to evaluate
the performance of the EKF. Each sensor is evaluated to measure the unique noise charac-
teristics for inclusion in the EFK. Simulated results are presented to demonstrate the ideal
performance of both the navigation and relative position estimator. Experimental results
using measurements from the sensor suites are presented to demonstrate the improvement
through the use of the ultrasonic sensors.
Chapter 6 presents the conclusions from the research. The goals for the thesis are
evaluated and areas of future work are identified and discussed. The areas of future work
include alternative methods to implement the EKF, implementation in realtime, additional
sensors, and increasing relative measurement rates.
6
Chapter 2
Overview of Base Matilda Hardware, Instrumentation Design and Software
Architecture
2.1 Introduction
This Chapter presents the UGV hardware, instrumentation and software architecture
used for the design and implementation of the navigation and relative position estimator.
The goal of the research is to augment the sensors necessary for a navigation estimator
with ultrasonic sensors to provide relative measurements between a pair of UGVs. The
relative measurements are used to refine the estimated states of both UGVs, allowing the
UGVs to maintain a desired formation or orientation. The research focuses on improving
the performance of the navigation estimator without the reliance of more expensive sensors
or a significant increase in computational burden.
2.2 MATILDA
The UGV used for testing purposes was produced by Mesa Robotics for the Unmanned
Ground Vehicle/Systems Joint Program Office (UGVS/JPO) at Redstone Arsenal, AL.
Mesa Associates? Tactical Integrated Light-Force Deployment Assembly (MATILDA) was
developed as a man-portable teleoperated UGV for a variety of missions. These missions
included reconnaissance, surveillance, remote breach, improvised explosive device (IED)
disposal, casualty recovery, and Nuclear Biological Chemical (NBC) detection [17]. Table
2.1 provides the dimensions and capabilities of the system.
7
Table 2.1: MATILDA Specifications
Size 50.8 cm x 30.48 cm x 66.04 cm
Platform Weight 20 kg w/o Batteries
25.4 kg with Batteries
Batteries 4x12 Vdc
Speed 0.9144 m/s
Operating Time 4 - 6 hrs
Payload Bay Dimensions 34.29 cm x 41.91 cm
Payload Capacity 56.7 kg
Towing Capacity 215 kg
Cameras Drive Dual Camera Assembly Color plus Black & White
Cameras Rear Fixed Black & White
Fording Depth 15.24 cm
Control Range LOS 1.0 km
Tunnel Crawl-In 50 m
Tunnel Walk-In 100 m
Tunnel Drive-In 600 m
2.2.1 Mobile Base Unit (MBU)
The MBU shown in Figure 2.1 is a tracked robotic vehicle that serves as the base for
the additional implements such as a manipulator arm, Nuclear-Biological-Chemical (NBC)
detector, or recoilless disrupter. The MBU can be used in a standalone configuration or with
the additional implements. The MBU is operated via a radio frequency (RF) link or a fiber
8
optic (FO) link with a 200 m cable. The data links passes instructions from the Operator
Control Unit (OCU) to the MBU and returns audio and video feeds from the MBU for the
operator. The MBU has three on board cameras for the operator. The operator can pan
and tilt the forward looking black/white and color cameras. A fixed camera provides a rear
view to aid in maneuvering the vehicle and maintaining situational awareness.
Figure 2.1: Mobile Base Unit
The suspension has two fixed road wheels in front and back with two intermediate
road wheels per side that are mounted on spring loaded arms. The majority of the weight is
born by the front and rear road wheels. This fixed suspension is adequate for the relatively
small size and a maximum speed of 0.9 m/s. Over uneven terrain, the MBU may exhibit
erratic steering since the track cannot conform to the ground profile. Separate direct current
motors power each track through a reduction gearbox attached to a drive sprocket. The
MBU uses pulse width modulation (PWM) to control the motor speed but has no feedback
loop to control track speed. The inherent friction imparted by the gear box and track make
slow speed maneuvering difficult. The user must apply sufficient throttle input to ?break
loose? the MBU then reduce it to prevent unwanted acceleration.
9
2.2.2 Operator Control Unit (OCU)
The OCU show in Figure 2.2 serves as the interface for the user. It presents the audio
and video links to the operator and accepts the inputs from the user. A single joystick is
used to control the direction and the speed of the MBU. A separate joystick is used to pan
and tilt the forward looking camera with rocker switches used to control camera settings
such as zoom and iris. A rotary switch selects the camera view displayed on the monitor.
Two additional joysticks are used to control the manipulator arm. The arm contains five
degrees of freedom allowing a full range of motion. A key is used to control the firing circuit
that can be used to initiate breaching charges or the recoilless disruptor.
Figure 2.2: Operator Control Unit
The controls are logically arranged but require a great deal of practice to develop profi-
ciency in operating the MBU with only the provided video and audio cues. The workload of
the operator can be quite high and could be significantly reduced by automating processes,
such as way point navigation or control of the manipulator arm.
10
2.2.3 Payloads
The main payload of the MBU is the manipulator arm shown in Figure 2.3. The arm
provides the operator with the capability to manipulate objects within one meter of the
MBU. The arm?s maximum lift is 6.80 kg at full extension and increases to 15.87 kg as
the arm is retracted toward the MBU. The arm incorporates two additional cameras and
a search light. One camera is located directly above the gripper for use in aligning the
gripper. A second camera incorporates a color camera with an 18x zoom lens. The search
light provides both white and infrared (IR) light for use in a variety of environments.
Figure 2.3: Manipulator Arm
The five degrees of freedom of the manipulator arm are shown in Figure 2.4. The
joints are best described by the analogous human joints: waist, shoulder, elbow, and wrist.
Smooth operation of the arm to manipulate objects requires a great deal of practice to de-
velop proficiency. The lack of depth perception when viewing objects through the on board
11
cameras further complicates the process. Multiple cameras located at different points pro-
vide the capability to judge distance; however, the operator must manually switch between
the cameras to accomplish this task.
Figure 2.4: Arm Axes
2.3 Instrumentation Design
The MATILDA provided the test bed for this research but could not provide any mea-
surements of platform dynamics for implementation of the navigation and relative position
estimators. The MATILDA was retrofitted with sensors to provide these measurements.
The UGVs were on loan and installation could not to damage the UGV or degrade per-
formance. Only commercially available sensors were considered to reduce cost and devel-
opment time. An interface was developed to integrate and record the measurements for
use by the estimator. These additions provide an instrumented UGV capable of providing
the measurements and inputs necessary to implement a navigation estimator. The instru-
mentation package was duplicated to replicate a second UGV, thus providing the relative
measurements required for the research.
12
2.4 Sensor Suite
The size of the robot requires a small dedicated sensor suite that incorporates the
necessary sensors, communication hardware, and computing power capable of being carried
by the MBU without degrading capability. A microcontroller and MEMS based sensors
were selected to meet both size and budgetary constraints. A printed circuit board (PCB)
shown in Figure 2.5 was designed and developed that provides a common interface for all
sensors. The PCB receives the necessary measurements and the onboard microcontroller
performs the necessary operations for use by the estimator.
Figure 2.5: Printed Circuit Board
2.4.1 GPS
The uBlox RCB-4H GPS receiver shown in Figure 2.6 is a small yet relatively accurate
receiver that can easily be incorporated into the sensor suite. The actual receiver is the size
of a postage stamp and is a surface mount device. The RCB-4H consists of the receiver
mounted on a printed circuit board that incorporates an antennae receptacle and a standard
13
20 pin through-hole header. The board is roughly 4 times larger than the actual receiver,
but does not require specialized equipment to mount on the printed circuit board of the
sensor suite.
Figure 2.6: uBlox RCB-4H
Table 2.2 provides the performance specifications for the receiver. The position ac-
curacy for this receiver is typical for an OEM receiver that costs $100. Satellite Based
Augmentation System (SBAS) corrections are available in the form of the Wide Area Aug-
mentation System (WAAS), the anticipated accuracy is 2.0 m circular error probable (CEP).
CEP represents the radius of a circle required to capture 50 percent of the data points. An-
other common measure of accuracy is Root Mean Square (RMS), the equivalent RMS value
for 2.0 m CEP is 2.4 m. This level of accuracy is sufficient for most consumer applications.
14
Table 2.2: Receiver Specifications
Parameter Value
Channels 16
Position 2.5 m CEP
Position w/SBAS 2.0 m CEP
Update Rate 4 Hz
Cold Start 34 s
Reacquisition 1 s
Timing Accuracy rms 50 ns
99% < 100ns
The GPS receiver also provides a Pulse Per Second (PPS) which is a digital signal that
is synchronized with GPS time. The PPS provides a convenient method to synchronize mul-
tiple devices. GPS provides the capability of time comparison with an accuracy measured
in nanoseconds. The accuracy of the time comparison is a function of the accuracy of the
position where the comparison is made. With antennae coordinates having an uncertainty
of 10 m, the accuracy is 100 ns [15]. The PPS output is most often used to synchronize IMU
measurements with the corresponding position measurements. This research will also use it
to synchronize the ultrasonic sensors. In comparison, the accuracy of the real time clock on
the microcontroller is 150 ns [20]. The additional benefits of using the GPS measurement,
are the corrections GPS uses to minimize drift and bias in the receiver clock and synchro-
nization of the receiver clock to a common time. An elaborate scheme would be necessary
to synchronize the real time clocks of multiple microcontrollers without the use of the GPS
15
measurements. Drift between the microcontroller?s clocks would introduce a bias into the
ultrasonic measurements.
2.4.2 Inertial Measurement Unit (IMU)
A Sentera Technology IMU was selected to provide the required measurements of plat-
form acceleration and rotation. The IMU 605 shown in Figure 2.7 is a six degree of freedom
IMU based on Analog Devices MEMS components. The IMU incorporates the accelerome-
ters and gyroscopes into a single package that significantly simplifies implementation. The
IMU also compensates for temperature effects on the MEMS sensors and outputs the data
via a standard serial port. Also, the IMU synchronizes its measurements using the Pulse
Per Second (PPS) provided by the GPS receiver, which simplifies data alignment. The
IMU has the capability to measure acceleration in three separate axes and roll rates about
the same axes. Only one accelerometer and one rate gyro measurement are used since the
robot is constrained to a level plane for this thesis. Based on the constraint imposed by
the level plane the UGV is not allowed to tilt or roll due to its fixed suspension. A more
robust model and avenue for future research, would require the inclusion of the additional
measurements to allow the model to compensate for roll, pitch, and yaw.
Figure 2.7: IMU 605 Top and Side
16
The MEMs sensors are inexpensive, small, and require very little power to operate;
however, the devices do not have the precision necessary for standalone navigation. The
IMU performance specifications are shown in Table 2.3. Integration of the rate measure-
ments would lead to a navigation solution that would grow without bound. The high update
rate of the IMU compliments the unbiased measurements of lower update rate GPS. The
IMU measurements provide measurements of system dynamics when GPS measurements
are not available.
Table 2.3: Inertial Measurement Unit Specifications
Parameter Value
Digital Output Rate 60 Hz
Angular Rate
Range ?150 deg/s
Bias ? 1 deg/s
Resolution <0.025 deg/s
Acceleration
Range ?19.62 m/s2
Bias ? 0.1472 m/s2
Resolution <0.002943 m/s2
2.4.3 Encoder
The MATILDA originally had no means to measure track speed or position. Optical
encoders were retrofitted to the MATILDA at the drive sprocket in order to measure track
17
position for this research. The velocity can then be calculated by averaging the track po-
sitions. The resulting measurements include a small amount of error due to quantization.
The variance of the quantization error is approximated as ?2 = q212 where q is the quanti-
zation step size [24]. The calculated variance for the velocity measurements is 0.0008 m/s.
This value is an order of magnitude smaller than the variance of the velocity measurement
from GPS receiver.
Figure 2.8: Optical Encoder
The US Digital E6S-64I optical encoder is appropriate for this application due to its
small size and ease of operation. The encoder has a resolution of 64 cycles per revolu-
tion(CPR). This resolution coupled with the use of the quadrature decoder on the RCM4100
allowed the position of the track to be measured to ? 1.4 degrees. This angular measure-
ment corresponds to ? 9.36 cm based on the diameter of the drive sprocket. An additional
advantage of using the 64 CPR encoder is that a single byte counter in the RCM4100 can be
used to measure a complete revolution. The limit of the counter?s size for the RCM4100?s
18
quadrature decoder prevented the use of higher resolution encoders at the MBU?s operating
speed.
2.4.4 Ultrasonic Sensor
In order to measure the relative distance between UGVs and their orientation an ul-
trasonic sensor was needed. The Parallax Ping ultrasonic sensor shown in Figure 2.9 in-
corporates an ultrasonic transducer, receiver, and timer into a single module allowing for
non-contact measurement with a range of two centimeters up to two meters. The module
is ideal based on reliability, cost, and ease of use. A three pin connector is used to provide
power, ground, and input/output(I/O).
Figure 2.9: Parallax Ping Ultrasonic Sensor
The theory of operation is depicted in Figure 2.10. The RCM4100 generates a pulse
on the I/O pin for 2 to 5 ?s. The Ping sensor waits 750 ?s them emits an ultrasonic burst
and generates a pulse on the I/O pin. The Ping holds the I/O pin high until the echo of
the burst is detected. The length of the pulse is proportional to the distance traveled by
the sound wave. The maximum wait time is 18 ms to prevent the sensor from hanging up
19
should no return be received. This time equates to a round trip distance of approximately
6 meters.
Figure 2.10: Ultrasonic Sensor Theory of Operation
The speed of sound can approximated as cideal = ?kRT. The ideal speed of sound
at 70 degrees F is 343.85 m/s. Testing was conducted from 50 degrees F to 90 degrees
F. Temperature variation over this window represents a change of only ?2 percent and
therefore was neglected. Table 2.4 shows the effect of varying temperatures on measured
distances.
Table 2.4: Effect of Temperature on Measurement
Temperature Actual Distance Mean Measured Distance Difference
51 F 1.334 m 1.307 m -2%
70 F 1.334 m 1.332 m -0.1%
89 F 1.334 m 1.357 m +1.7%
The Ping sensor is designed for use on small robots for proximity detection. Graphical
depictions of the manner of operation of the sensor are shown in Figure 2.11. Ideally the
object that reflects the burst is flat and perpendicular to the path of propagation. When
20
the angle between the path of propagation and smooth surface are less than 45 degrees no
energy is reflected for the sensor to detect. An object that is too small may reflect a portion
of the energy, but this reflected energy is below the threshold set for detection.
Figure 2.11: Scenarios for Ultrasonic Measurements
The frequency of the signal is the controlling factor in the size of object that can be
readily detected. The wavelength of the sound wave should be comparable to the irregular-
ities on the reflecting surface [14]. The required wavelength of the ultrasonic burst based
on a specified feature size is presented in Equation (2.1).
? = cf (2.1)
? = 343.85m/s40000Hz
? = 8.5mm
21
The sensor can be expected to detect objects larger than 8.8mm given the 40kHz
operating frequency. Therefore the sensor should reliably detect the sister UGV based on
the size of the MATILDA.
The simplicity of the sensor is offset by limitations inherent to most ultrasonic sensors.
Echoes from other sensors may provide false readings as the sensor has no way to determine
the origination of the echo. The use of the PPS to trigger both sensors will exploit this
limitation. 99% of the PPS signals are accurate to within 100 ns. This corresponds to a
distance error of ?3.45cm.
Research has shown that the burst for the sensor may be modulated with a digital
signal [26]. The use of convolution allows a comparison of the transmitted signal with the
echo to minimize faulty readings. This approach improves the reliability from 11% to 92%
in a noisy environment with the frequency of the noise matching the transmitted signal. In
addition to improving reliability, it also allows multiple sensors to operate simultaneously
in close proximity [26]. The study used a dedicated transducer emitting white noise at the
ultrasonic frequency to simulate a noisy environment.
The constraints previously illustrated in Figure 2.11 are overcome by using the GPS
PPS as a precision timing device. The common time removes the need to differentiate
between multiple echoes and does not use reflected signals. The use of a common time to
trigger the sensors, in this case GPS time, allows the sensor to measure the time it takes
the other UGV?s burst to reach its position instead of waiting for the echo. The signal from
the sister UGV has a much higher intensity and is not susceptible to multi path assuming
a clear line of sight is available between the UGVs.
22
2.5 Processor
The Rabbit Core Module 4100 shown in Figure 2.12 was selected for the processor to
use on the PCB. The RCM4100 incorporates a 58 MHz microcontroller with 512 KB of
memory and several hardware capabilities that simplify the incorporation of the sensors.
The RCM4100 has dedicated serial ports that allow the IMU and GPS to directly provide
digital measurements. The serial ports also allow the RCM4100 to output data via a radio
modem and serial connector. The input capture function allows the RCM4100 to precisely
measure the time of flight for the ultrasonic ping. The input capture counter is configured
for 1 ns per count. The integral quadrature decoder allows the RCM4100 to calculate the
track speed and direction using standard optical encoders.
2.6 Rabbit Core Module 4100 (RCM4100)
Figure 2.12: Rabbit Core Module 4100
23
Rabbit Semiconductor developed a proprietary programming language, Dynamic C, to
control the RCM4100. Dynamic C combines the functionality of C with built in mnemonics
to control the hardware functions unique to the RCM4100. A relatively simple program
was developed to receive the measurements from each sensor, parse the messages to extract
the necessary values and then output the information needed to implement the estimators.
2.7 Completed Test Rig
A suitable platform to mount the sensors was required for testing. A rigid aluminum
frame shown in Figure 2.13 was constructed to mount the sensors and allow for installation
in the payload bay of the MATILDA. The frame provided the necessary offset to prevent
interference between the body of the MATILDA and the ultrasonic sensors. The IMU was
located at the center of gravity in the East North plane to minimize any lever effects the
IMU might experience during rotation. The height of the frame offset the IMU in the Up
direction but was neglected due the constraint imposed by operation on flat level surfaces.
Figure 2.13: Lead UGV Test Rig
24
A leader-follower scenario was used for experimental testing to simplify implementation
and ease comparison of results. A second platform was constructed to replicate the trail
UGV rather than using two separate UGVs for testing. The location of the IMU and
GPS antennae on the trail unit match the dimensions present on the lead platform. The
platform shown in Figure 2.14 was attached to the lead UGV via a hitch. This configuration
allowed for the orientation and distance between the UGVs to be controlled and accurately
measured. The sensor suites from both UGVs would undergo similar dynamics during
testing, but would be recorded by their respective sensors.
Figure 2.14: Test Rig used to replicate Trail UGV
The sensor platforms are used in conjunction for static and dynamic testing. The
tongue of the trailer is marked with the effective distance between the center of gravity of
both platforms. A protractor provides a convenient method to verify the measured bearing
between the lead and trail platform. The aluminum frame was selected due to its light
weight, exceptional rigidity and low cost. Figure 2.15 shows the lead MATILDA with trail
sensor following at 2.431 m.
25
Figure 2.15: Test Configuration
2.8 Software
The software developed for this research is divided into three separate categories. The
programs are delineated by language used and also location employed. The sensor suite
on the UGV uses Dynamic C, a C++ routine runs on the PC to parse and log data, and
MATLAB is used for data analysis.
2.8.1 Dynamic C
A simple program was implemented on the RCM4100 that incorporated several pars-
ing subroutines. The IMU outputs data at 60 Hz as a constant stream. The RCM4100
determines the start of each message and stores the measurement. The outputs from the
IMU are scaled and must be multiplied by the scale factor to provide measurements in the
appropriate units. The RCM4100 outputs all sensor measurements to the serial port with
the receipt of each new IMU message. The IMU message serves as the trigger since it has
26
the highest update rate. This approach yields duplicate GPS and Ultrasonic Measurements
that are discarded during post processing.
The GPS receiver outputs data at 4 Hz in the National Marine Electronics Association
(NMEA) Recommended Minimum (RMC) format. The message is parsed for Coordinated
Universal Time (UTC), Latitude, Longitude, velocity, and heading. The parser also verifies
the measurements received are valid via the flag field in the RMC format.
An Interrupt Service Routine (ISR) is implemented with the input capture subroutine
on the RCM4100. This routine is time critical and must be executed as soon as the PPS is
received. The use of the ISR allows the RCM4100 to execute the routine within 24 ticks of
the clock on the RCM4100. The maximum delay is 0.4 ?s based on the 58 MHz processor.
The ISR was written to minimize the time required to run, thus preventing conflicts with
the other routines.
The quadrature decoder counter is read and reset with each IMU measurement. This
output rate ensures that the counter does not overrun, leading to possible loss of data. Left
and right track measurements are recorded to allow for determination of both velocity and
yaw rate. The resolution of the encoder and counter do introduce quantization error, but
these values are extremely small and accounted for in the estimator.
Dynamic C does not have the capability to perform the matrix operations required
directly. The operations can be decomposed into the individual algebraic operations; how-
ever, this approach would require significant software development. Therefore a two way
radio modem is employed to allow the PC to execute programs developed in C++. The
application used in this thesis is run post-process. An avenue of future research would be
implementation in real-time.
27
2.8.2 C++
A stand alone application was created for use on a laptop to receive the measurements
output from the RCM4100. The application sets up and opens the serial ports to allow the
measurements to be received and saved to a standard text file. The application displays
the measurements or writes them to a file for later analysis. The application also provides
a command link to the RCM4100 and ultimately the MBU.
2.8.3 MATLAB
All analysis was conducted post-process in MATLAB to simplify the development pro-
cess. Real-Time Workshop is available to execute MATLAB code in real-time. Converting
the algorithm into a real-time application exceeds the scope of this research. Once the
algorithms have been validated and verified, future research would incorporate them into a
real-time application.
2.9 Summary
This chapter presents the hardware, instrumentation and software necessary to im-
plement a navigation and relative position estimator. The MATILDA represents UGVs
currently in use with the typical capabilities and limitations. The sensors traditionally used
to implement a navigation estimations are presented in addition to the ultrasonic sensors
that will provide the relative measurements. A simple yet effective estimator can be con-
structed with commercial off the shelf (COTS) components with relative ease. The only
custom parts required were the encoder mount and the printed circuit board and both were
28
acquired commercially. The incorporation of these components will allow the UGV to esti-
mate its position using the algorithms developed in the following chapter. The prototype
device does not appreciably increase the size or cost of the UGV and does not degrade
payload while providing a new capability.
29
Chapter 3
UGV Navigation Estimator
3.1 Introduction
The first problem that must be solved is the determination of the robot?s location with
respect to some reference frame. This reference frame may be defined many different ways.
Ideally a common reference frame is chosen to allow for ease of comparison to positions of
interest, i.e. the desired end point, obstacle, or partner robot. The reference frame selected
is determined by the area of operation as well as the dynamics of interest. Aircraft operate
over large distances and rely on Latitude, Longitude, and Altitude. UGVs traditionally
operate over relatively small areas measured in meters to kilometers, and thus an East,
North, Up reference frame is more useful. Regardless of the reference frame chosen, the
sensor measurements must be evaluated to determine the robot?s absolute position. A single
sensor may be used to determine the position; however, the resulting solution will be limited
by the quality of the sensor used.
Although GPS provides a bias free measurement, the position measurements will fall
within a 10 meter radius using a single receiver [19]. The higher the quality of the receiver,
the lower the standard deviation of those measurements. For example, the uBlox error has
outliers out to 10 meters, but half are clustered within 2 meters of the actual point. A plot
of calculated positions is shown in Figure 3.1 to illustrate the point. The plot shows data
taken over a 15 minute period.
30
?2 ?1 0 1 2 3 4 5 6 70
1
2
3
4
5
6
7
8
East (m)
North (m)
 
 
Figure 3.1: GPS Position
This level of accuracy is sufficient for navigation on the macro scale, but is lacking when
attempting to determine multiple UGV locations in close proximity. Additionally the use
of only the MEMS IMU is effective over very short periods of time. Integration of the noise
on the measurements causes the error in the navigation solution to grow without bound [4].
Figure 3.2 illustrates the effect of integrating a sensor bias, zero mean white noise, and the
combination of the two over a two second period.
0 0.5 1 1.5 2?0.5
0
0.5
1
1.5
2
Time (s)
Heading (deg)
 
 
Noise Bias+Noise Bias
Figure 3.2: Integration of Bias and Noise
31
The combination of multiple sensors allows for a complementary solution to be de-
veloped for the entire system. The manner in which they are integrated has been widely
studied and each configuration provides a slightly different capability. One approach fo-
cused on using inertial measurements to update position estimates between low rate (1-5
Hz) GPS measurements [9]. The IMU provides measurements of system dynamics between
the GPS measurements. Another focused on using the inertial measurements during GPS
outages [3]. This approach relied heavily on the GPS and used the IMU only during GPS
outages. A third approach combined the two approaches and augmented GPS with inertial
measurements [1].
The system design and manner in which the estimator uses the measurements varies
greatly. Cost and size are a controlling factor so a relatively simple algorithm requiring
only moderate computational loads was selected. Therefore, the scheme selected for this
thesis uses an Extended Kalman Filter (EKF) to evaluate the measurements provided and
generate a reasonably accurate estimate.
3.2 Vehicle Model
The Kalman Filter for this research employs a kinematic model for its estimation. The
kinematicmodelisbasedonthe relationshipbetween the variousstates andtheirderivatives.
The relation between the states and their respective derivatives are known and can be
calculated without extensive a priori knowledge. The MATILDA is a tracked UGV which
would require a complex dynamic model [16] [22]. Extensive model identification would
be necessary to employ such a model. The interaction between the track and the terrain
32
introduce numerous complexities which invalidate most assumptions used for differential
drive vehicles.
The instantaneous center of rotation (ICR) for each track does not lie within the
contact patch which is the case for small differential drive vehicles on smooth surfaces [23].
A comparison of ICR locations for a tracked and wheeled differential drive UGV is made
in Figure 3.3. The diagram is not to scale. The contact patch for the tracked UGV is
significantly larger than a wheeled UGV of comparable size and weight. The ICRs for the
wheeled UGV lie within the contact patch for the respective wheel. The small area of the
contact patch, and the location of the ICR ensures that all points within the contact patch
lie in close proximity. This proximity allows for the assumption that no slip occurs between
the tire and terrain.
Figure 3.3: Instantaneous Center of Rotation Location Comparison
The ICRs for tracked vehicle lies outside the contact patch of the track during turns and
the location is a function of the yaw rate, CG location, and coefficient of friction between
the track and terrain. The larger contact area coupled with the offset of the ICR invalidates
the no-slip assumption used in the wheeled model. Figure 3.4 graphically illustrates the
lateral velocities along the length of the contact patch during a counter-clockwise turn. The
lateral velocity increases significantly the farther the point is displaced from the lateral axis.
33
The lateral axis is shown as a dotted line in the figure. The no-slip assumption is only valid
for tracked vehicles under purely longitudinal translation or extremely small rotation rates.
Figure 3.4: Lateral track velocity of UGV during pure rotation
One approach for modeling a small UGV used experimental identification to determine
the location of the instantaneous centers of rotation given a specific UGV configuration and
type of terrain. This approach imposed only a small computational burden, but required
extensive experimentation [16]. A generic schematic depicting the relation of ICR for the
UGV is shown in Figure 3.5. Each track rotates about an ICR identified as ICRR and
ICRL. The ICR for the UGV, ICR, lies along the lateral axis that passes through the
track ICRs and the center of gravity. The offset from the CG for the ICR is the sum of
the individual track ICRs. A second dynamic model was based on finite element analysis
of the track pad interaction of a M113 Armored Personnel Carrier [22]. This approach only
required soil parameters for evaluation, but incurred a significant computational burden.
Both of these models provide valuable insight into the dynamics of the UGV but are too
complex for implementation on a small inexpensive UGV and beyond the scope of this
thesis.
34
Figure 3.5: Tracked UGV ICR Locations
3.2.1 Kinematic Model
Figure 3.6: Schematic of Robot
The position of the UGV in general ENU coordinates is shown in Figure 3.6. The
velocity of the UGV in terms of the longitudinal velocity, Vx, and heading, ? is defined in
35
EQ (3.1).
?? = r (3.1)
?E = Vxsin(?)
?N = Vxcos(?)
Vy is the lateral velocity at the center of gravity (CG) in a body fixed frame and is
neglected for the purposes of this research due to the UGV?s relatively slow speed. The
maximumspeedoftheMATILDAis0.9144m/sonsmoothterrainwithafullbatterycharge.
The maximum speed observed during testing never exceeded 0.9 m/s. The magnitude of any
potential lateral velocity would be below the noise floor of the sensors used for calculations.
The additional complexity of the computations required to account for lateral velocity would
not yield an appreciable improvement in the fidelity of the model.
The model dynamics in Equation (3.1) are of the form:
?x = f(x)+Bu (3.2)
where the states selected for this model are:
x = [Vx ? N E]T (3.3)
36
3.3 Estimator
Equations(3.1) must be placed in the following form to allow the use of linear state
space estimation:
?X = AX +Bu (3.4)
Y = CX (3.5)
Because both sensor inputs are biased, the fourth order model described defined by Eq (3.1)
must be augmented with two additional states resulting in a sixth order estimator. The two
additional states are the accelerometer bias ab and the rate gyro bias gb. The six resulting
estimated states are:
?X = [ ?Vx ?ab ?? ?gb ?N ?E]T (3.6)
The dynamics in Equation (3.1) must be linearized about a given point due to their
nonlinear nature in order to employ the planned state space estimation. The state equations
are linearized through the use of the Jacobian (J) at each time step. The linearized state
equations are of the form:
?X = JX +Bu (3.7)
37
The Jacobian is defined as:
J =
?
??
??
?
?f1
?x1 ???
?f1
?xn
... ... ...
?fn
?x1 ???
?fn
?xn
?
??
??
?
(3.8)
The Jacobian based on the proposed model is listed below:
J =
?
??
??
??
??
??
??
??
??
?
0 ?1 0 0 0 0
0 0 0 0 0 0
0 0 0 ?1 0 0
0 0 0 ?1 0 0
cos(?) 0 ?VLsin(?) 0 0 0
sin(?) 0 VLcos(?) 0 0 0
?
??
??
??
??
??
??
??
??
?
(3.9)
The input vector(u) is defined by:
u =
?
?
along
rz
?
? (3.10)
38
The corresponding input matrix(B) is defined for the model by:
B =
?
??
??
??
??
??
??
??
??
?
1 0
0 0
0 1
0 0
0 0
0 0
?
??
??
??
??
??
??
??
??
?
(3.11)
The relationship between the measurement vector(Ymeas) and the observation ma-
trix(C) is defined by:
Ymeas = CX (3.12)
where:
Ymeas = [VGPSL VWSL ?GPSL NGPSL EGPSL ]T (3.13)
The observation matrix for this model is defined by:
39
C =
?
??
??
??
??
??
??
?
1 0 0 0 0 0
1 0 0 0 0 0
0 0 1 0 0 0
0 0 0 0 1 0
0 0 0 0 0 1
?
??
??
??
??
??
??
?
(3.14)
The GPS receiver does not directly provide a heading measurement. The measurement
from the receiver is course, ?, that is defined as the direction of the velocity vector. Figure
3.7 shows the relation between course, heading, and side slip angle ?. Lateral velocity,
Vy, is neglected for this thesis, hence ? is zero. The course and heading are equal for
dynamic scenarios based on this assumption. The course angle is undefined when the UGV
is stationary, hence no information on the heading is available. The implications of this
assumption are discussed in greater detail in Section 5.3.
40
Figure 3.7: Relation of Course (?), Heading (?), and Sideslip (?)
The process noise variances were also assumed to be uncorrelated such that the covari-
ance matrix (Q) is diagonal. The variance of each state is provided in Table 3.1.
Table 3.1: Process Noise Variances
State Variance(?2)
VL 0.00000431 m/s
ab 10?4 m/s2
? 10?6 deg
gb 10?3 deg/s
N 10?6 m
E 10?6 m
41
Additionally, the sensor noises were assumed to be uncorrelated with respect to the
other measurements such that the covariance matrix (R) is diagonal. The variance of each
measurement is provided in Table 3.2
Table 3.2: Sensor Measurement Noise Variances
Measured State Sensor Variance(?2) Update Rate
Vx GPS 0.0025 m/s 4 Hz
Vx Encoder 0.00000072 m/s 60 Hz
? GPS 0.000484 deg 4 Hz
N GPS 1.832 m 4 Hz
E GPS 1.551 m Hz
3.3.1 Extended Kalman Filter
The Extended Kalman Filter(EKF) shares many similarities with the standard Kalman
Filter. The EKF is used due to the nonlinear dynamics of the model. Unlike the KF which
produces an optimal solution, the EKF does not achieve a precisely ?optimum? solution
due to the linearization process. The solution is ?optimal? in the sense that they tend to
the optimum [25]. The state equations must be linearized at each time step to ensure that
assumptions necessary to employ the EKF are met. The EKF consists of two updates:
a time update, and a measurement update. Both updates are implemented at each time
step(k). The EKF must also use numerical integration of nonlinear differential equations in
the time update.
42
3.3.2 Measurement Update
The measurement update incorporates the new measurements into the estimator. Ide-
ally only new measurements at that time step are included to prevent corrupting the filter.
The measurement update is defined by:
Lk = P?k CT
parenleftBig
CPkCT +R
parenrightBig?1
(3.15)
?X+k = ?X?k +LkparenleftBigymeas ?C ?X?k parenrightBig (3.16)
P+k = (I ?LkC)P?k (3.17)
The ? notation of the matrices is used to indicate the value with respect to the updates
performed at each time step. The notation is used to clarify operations with regards to
the measurement and time updates. They are included only for clarification and do not
represent an additional operation.
The observation matrix is used to control the measurements that are included in each
measurement update. The sensors used by the sensor suite output measurements at 60 Hz
and 4 Hz. Table 3.2 from the previous sections shows the update rates for each measurement.
Several approaches have been proposed to effectively turn on and off the measurements. One
approach is the use of look up tables to select the appropriate C matrix given. This approach
requires a separate C matrix to be maintained for each possible combination. An alternate
approach was selected in this thesis to use logical operators to switch each entry in the C
matrix based on the measurements used for that time step.
43
3.3.3 Time Update
The time updates propagates the dynamics of the states over each time step(k). The
simplest manner is through the use of Euler Integration. A time update using this method
is:
?X?k+1 = ?X+k +?t? (3.18)
P?k = ?P+k ?T +Qw (3.19)
The Euler Integration provides a very simple operation at the expense of introduction of
error. The error stems from the inherent limitation of Euler Integration of functions as time
steps are increased. At higher update rates, this limitation is negligible; however, the given
60 Hz measurement update requires a better integration technique.
Runge-Kutta integration can be implemented with a slight increase in computational
burden. The time update when implemented with Runge-Kutta integration is:
K1 = ?t ?X
K2 = ?t ?X + K12
K3 = ?t ?X + K22
K4 = ?t ?X + K32
?X?k+1 = ?X+k + K1 +2K2 +2K3 +K4
6 (3.20)
M1 = ?t
parenleftBig
AP +PAT +BwQBTw
parenrightBig
44
M2 = ?t
parenleftbigg
A
parenleftbigg
P + M12
parenrightbigg
+
parenleftbigg
P + M12
parenrightbigg
AT +BwQBTw
parenrightbigg
M3 = ?t
parenleftbigg
A
parenleftbigg
P + M22
parenrightbigg
+
parenleftbigg
P + M22
parenrightbigg
AT +BwQBTw
parenrightbigg
M4 = ?t
parenleftbigg
A
parenleftbigg
P + M32
parenrightbigg
+
parenleftbigg
P + M32
parenrightbigg
AT +BwQBTw
parenrightbigg
P?k = P+k + M1 +2M2 +2M3 +M46 (3.21)
3.3.4 Hybrid Filter
A further improvement on the EKF is a hybrid approach that attempts to overcome
some of the difficulties introduced when estimating a continuous process using discrete
calculations. The proposed hybrid filter is most appropriate if sampled-data of a continuous
process is available. The hybrid filter combines continuous and discrete models in the
estimation process. The dynamics, observation and covariance propagation use a continuous
model. Discrete models are employed to propagate the measurement update and gain
computation [25].
3.3.5 Heading Wrap
An potential issue for implementing the EKF extends from the unique nature of the
heading measurement. The heading measurement is usually measured as an angle ranging
from 0 to 360 degrees or 0 to 2pi radians. The issue arises after one complete revolution
of the UGV. Assuming the UGV rotates clockwise, the heading measurement will increase
until it reaches 359 degrees. After an additional degree of rotation, the GPS receiver
will measure a heading of zero degrees. The estimator will estimate a heading of 360
degrees. The measurements are physically equivalent, but represents an error of 360 degrees.
45
The problem is further magnified as the UGV completes the second complete rotation.
To minimize the effect of this wrap, a subroutine is implemented to unwrap the heading
estimate measurement. The routine will add or subtract 360 degrees from the heading to
ensure the measurement falls within the appropriate range.
An alternate range for heading was used for this research. The heading is defined
over the range of -180 deg to 180 deg or ?pi to pi. This alternate definition ensured the
estimator encountered both positive and negative residuals. The heading measurements
were unwrapped in three separate places within the algorithm. The heading measurements
from the GPS receiver were unwrapped to ensure consistency. The heading estimate was
also unwrapped for calculation of the measurement update and following the time update.
The effect of this operation can be seen in plots of heading as the estimate approaches
?180 deg. The apparent discontinuity corresponds to a complete revolution of the UGV.
The heading measurement can be ?rewrapped? if a smooth function is need for analytical
analysis.
3.4 Conclusion
An Extended Kalman Filter is employed to receive noisy, incomplete measurements
and estimate a systems response. The solution is not by definition optimal; however, the
error can be evaluated to ensure proper performance. The EKF provides a relatively simple,
yet robust algorithm to estimate the states of the UGV, specifically position, velocity, and
heading. The performance of the filter is constrained by the quantity and the quality of the
measurements fed into it. When properly implemented, the EKF provides an estimate of
46
the states shown in Equation (3.6) that is of higher quality than an estimate based on an
individual measurement or input.
47
Chapter 4
Relative Position Model
4.1 Introduction
This chapter develops a relative position estimator, which is an extension of the pre-
viously derived navigation estimator. The relative position estimator combines the two
navigation estimators with additional measurements utilizing the ultrasonic sensor. The
first additional measurement is the range (r) between the center of gravity of two UGVs.
The second additional measurement is the bearing (?) between the centerline of the lead
UGV and the CG of the trail UGV. The additional measurements provide information on
the relative position of both UGVs and the heading of the lead UGV.
4.2 State Transition Matrix
The state transition matrix is comprised of two six state estimators combined into a
single twelve state estimator. The same states are maintained, but a designation for lead
(l) versus trail (t) UGVs is introduced. The states of the estimator are shown in Equation
(4.1).
?X = [ ?Vlx ?alb ??l ?glb ?Nl ?El ?Vtx ?atb ??t ?gtb ?Nt ?Et]T (4.1)
The equations governing the states remain the same but are used for each UGV indepen-
dently. The inclusion of the two relative measurements requires the combination of states
48
into a single estimator instead of a cascaded approach. This approach incurs a higher
computational burden due to inversion of matrices that are twice as large. A cascaded
approach based on the use of two navigation estimators feeding a third estimator to include
the relative measurements would minimize the effect of the relative measurements while
complicating the implementation. Use of the estimates from the navigation estimators as
measurements for the third estimator could introduce correlated noise, thus corrupting the
output. The application could provide comparable or improved results; however, this ap-
proach would require significantly more analysis to evaluate the process noise of all three
estimators. The nonlinear state equations must be evaluated and linearized, as before, at
each time step. The Jacobian of the state matrix is shown in Equation (4.2).
JA =
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
0 ?1 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 ?1 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
cos(?) 0 ?VLsin(?) 0 0 0 0 0 0 0 0 0
sin(?) 0 VLcos(?) 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 ?1 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 ?1 0 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 cos(?) 0 ?VLsin(?) 0 0 0
0 0 0 0 0 0 sin(?) 0 VLcos(?) 0 0 0
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
(4.2)
49
4.3 Inputs
The inputs for the system are the accelerometer and rate gyro measurements provided
by the sensor suites on each UGV. The noise characteristics for the lead and trail IMU
are unique and are accounted for in the Q matrix. The inputs from each UGV will be
comparable given identical system dynamics. The inputs to the estimator are defined in
Equation (4.3).
u = [allong rlz atlong rtz ]T (4.3)
4.4 Input Matrix
The dimensions of the input matrix are increased in the same manner as the dimensions
of the state matrix. Two sets of acceleration and rotation measurements comprise the inputs
to the system. The acceleration and rotation measurements are fed directly into the velocity
50
and heading states, respectively. The resulting matrix is
B =
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
?
1 0 0 0
0 0 0 0
0 1 0 0
0 0 0 0
0 0 0 0
0 0 0 0
0 0 1 0
0 0 0 0
0 0 0 1
0 0 0 0
0 0 0 0
0 0 0 0
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
?
(4.4)
4.5 Measurements
The relative position estimator uses the same measurements as the navigation estimator
plus measurements from the ultrasonic sensor. The measurements used by the estimator
are shown in Equation (4.5).
The notation for Equation (4.5) separates the global measurements for each UGV and
the relative measurements between them. The first ten measurements use the global ENU
reference frame. The last two measurements are relative measurements in the lead UGV
body fixed frame. A subscript is added to the relative measurements to clarify the reference
51
used to provide the measurement.
Yl = [VGPSl VWSl ?GPSl NGPSl EGPSl ]
Yt = [VGPSt VWSt ?GPSt NGPSt EGPSt ]
Yrel = [rlt ?lt ]
Ymeas = [Yl Yt Yrel ]T (4.5)
Figure 4.1 shows the relation of the bearing and range to the positions of the UGVs.
The measurements provided by the ultrasonic sensor must be transformed into the global
coordinate frame for use by the EKF. These measurements provide additional information
about the relative position of both UGVs and the orientation of the lead UGV. A third
bearing measurement in the trail UGV body fixed frame would berequired to gain additional
information about the orientation of that UGV.
52
Figure 4.1: Description of Range and Bearing Measurement
The range measurement used by the estimator is defined by Equation (4.6).
r =
radicalBig
(Etrail ?Elead)2 +(Ntrail ?Nlead)2 (4.6)
The bearing measurement requires a transformation from the body fixed frame of the
lead UGV to the global reference frame. Figure 4.2 illustrates the relationship between the
relative bearing measurement and the global heading of the UGV.
53
Figure 4.2: Derivation of Bearing Measurement
The equation that defines the bearing measurement is:
? = atan?E?N ??l (4.7)
4.6 Observation Matrix
The estimator uses a 12 x 12 observation matrix to relate the measurements to the
states. The first 10 rows are created by using the observation matrix from the navigation
estimator used in Chapter 3. The last two rows are derived from Equations (4.6) and (4.7).
The matrix must be linearized and evaluated at each time step along the estimators nominal
trajectory using the Jacobian. The Jacobian of the observation matrix is defined by
54
JC =
?
??
??
?
?f1
?x1 ???
?f1
?xn
... ... ...
?fn
?x1 ???
?fn
?xn
?
??
??
?
(4.8)
This results in the following Jacobian observation matrix:
JC =
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
1 0 0 0 0 0 0 0 0 0 0 0
1 0 0 0 0 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0 0 0
0 0 0 0 1 0 0 0 0 0 0 0
0 0 0 0 0 1 0 0 0 0 0 0
0 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 0 0 0 1
0 0 0 0 ??N??N2+?E2 ??E??N2+?E2 0 0 0 0 ?N??N2+?E2 ?E??N2+?E2
0 0 ?1 0 ?E?N2+?E2 ??N?N2+?E2 0 0 0 0 ??E?N2+?E2 ?N?N2+?E2
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
(4.9)
4.7 Extended Kalman Filter
The relative position estimator uses the same measurement and time updates as de-
scribed in Section 3.3.1. The dimensions of the matrices that are evaluated for the updates
are twice as large, but handled in the same manner. The inclusion of the relative measure-
ments requires evaluation of the Jacobian of the observation matrix (C), shown in Equation
55
(4.9), at each time step. The process noise covariance matrix (Q) and the measurement
noise covariance matrix (R) are populated with the empirically determined values for the
corresponding measurement or input.
4.8 Conclusion
This Chapter has provided the state matrices used to design the relative navigation
estimator used in this thesis. The use of a common time allows ultrasonic sensors to
reliably measure relative range and bearing between the two UGVs. These two additional
measurements provide valuable information that allows the EKF to refine the position and
heading estimates. This approach builds on the navigation estimator derived in Chapter
3. An increased computational burden is incurred to implement the estimator due to the
increased number of states. The ultrasonic sensors are the only additional hardware required
to implement this estimator. The marked improvement in the state estimates provided by
the inclusion of the relative measurements offset the additional computational costs. The
additional hardware does not significantly affect system cost or degrade payload capability.
Chapter 5 will provide the design of the relative navigation estimator as well as experimental
validation of the algorithm.
56
Chapter 5
Experimental Work
5.1 Introduction
This Chapter presents the experimental results of tests performed on the individual
sensors and the system as a whole. The measured noise characteristics for each sensor used
to instrument the UGV are presented. Simulated data is used to evaluate and compare
the navigation estimator and the relative position estimator. These results were used to
verify proper operation and provide insight into the level of performance that could be
expected based on the empirically determined noise characteristics. Experimental results are
presented to demonstrate the EKF?s performance with measurements taken under normal
operating conditions. The experimental results are also used to validate the simulated
results.
5.2 Sensor Data
The EKF uses the process noise covariance matrix (Q) to weight each measurements
contribution. The sensor noises are assumed to be additive white noise with Gaussian
distribution. The variance is the metric used to quantify the noise by the EKF. Each sensor
is commercially available with statistical values provided in the specification sheets. These
values are important since the quality of the estimate provided by the EFK is a function of
the quality of the measurements and how well the noise can be modeled.
57
5.2.1 IMU
The Sentera IMU was tested over three separate ten minute periods to determine the
mean and standard deviation for each accelerometer and gyro. This approach provides a
sample size that is comparable to the measurements taken in an experimental run. The
separate data sets also provide insight into the health of the sensors and the presence of
other error sources such as drift or bias walk. A significant increase in noise or a drifting
mean value is an indicator of MEMS failure.
Accelerometer
The parameters determined from averaging the three runs closely match the perfor-
mance specs provided by Sentera shown in Table 2.3. Tables 5.1 and 5.2 provide the
measured standard deviations and means for the accelerometers on both IMUs. The EKF
should calculate a bias that is comparable to the mean value for each sensor. Platform
dynamics will cause the EKF estimate to vary; however, the values should be very close
under steady state conditions.
Table 5.1: Lead Accelerometer Noise
Parameter Value (m/s2)
?x 0.0042
biasx -0.01443
?y 0.0002893
biasy -0.02221
?z 0.0002954
biasz -0.0003018
Table 5.2: Trail Accelerometer Noise
Parameter Value (m/s2)
?x 0.01116
biasx -0.01380
?y 0.01189
biasy -0.03640
?z 0.01802
biasz 0.003535
58
Rate Gyroscope
The parameters calculated for the rate gyroscopes are shown in Tables 5.3 and 5.4.
These values are higher than the specifications shown in Table 2.3 but fall within the
expected range for a MEMS based gyro. The slightly higher values of the trail IMU are due
to the use of earlier model Analog Digital rate gyros. These values can provide reasonable
performance for dead reckoning for several seconds. The EKF receives GPS updates at 4
Hz, which bounds the heading estimate error to acceptable levels.
Table 5.3: Lead Gyro Noise
Parameter Value (deg/s)
?x 0.3916
biasx -0.1877
?y 0.4310
biasy -0.3883
?z 0.4161
biasz -0.0983
Table 5.4: Trail Gyro Noise
Parameter Value (deg/s)
?x 1.4090
biasx 0.04732
?y 1.4962
biasy -0.1014
?z 1.8112
biasz -0.1854
5.2.2 GPS
The noise measurements for the GPS receivers are presented in Table 5.5 and 5.6.
These parameters provide an indication of the quality of the measurement calculated by
the receiver, but they do not completely describe the noise present. The error present
in the measurements is ?colored? in that it does not exhibit precisely zero mean white
noise. The position measurements exhibit ?walk? over time. The measured position follows
59
a meandering path that is centered about a central point. Figure 3.1 shown previously
illustrates this walk that is common to all GPS receivers regardless of cost.
Table 5.5: Lead GPS Noise
Parameter Value (m)
?N 1.8320
?E 1.5551
Table 5.6: Trail GPS Noise
Parameter Value (m)
?N 1.6651
?E 1.9698
5.2.3 Ultrasonic Sensor
The Ping sensors are capable of providing reliable measurements under static and dy-
namic testing. Static test sessions were implemented to calculate the parameters necessary
to accurately describe the range and bearing noise. The range and bearing measurements
were produced by the returns of the sensors located on the lead UGV. Figure 5.1 illustrates
the relationship between the raw range measurements, rl and rr, and the measurements
used for estimation, r and ?.
Figure 5.1: Schematic Illustrating the Range and Bearing Calculation
60
The range and bearing measurements are defined by Equations (5.1-5.3).
? = acos
parenleftBigg
r2l +w2 ?r2r
2rlw
parenrightBigg
(5.1)
r =
radicalBigg
r2l +w2
4 ?rlwcos(?) (5.2)
? = acos
parenleftBiggr2 + w2
4 ?r
2l
rw
parenrightBigg
(5.3)
The measurements from the Ping sensor are quite precise when using the GPS PPS to
provide a common time for the sensor suites. Errors introduced are based on variances of the
PPS signal and delays due to computations of the RCM4100. Variations in temperature will
induce a bias since the measurements are based off of time of flight. A ten minute sample of
static range and bearing measurements are shown in Figure 5.2 and Figure 5.3 respectively.
The actual range measured for the test was 2.431 m. The actual bearing between units for
the test was 90 deg.
0 100 200 300 400 500 600 7002.38
2.385
2.39
2.395
2.4
2.405
2.41
Time (s)
Range (m)
Figure 5.2: Experimental Ultrasonic Range
Measurements
0 100 200 300 400 500 600 70088.5
89
89.5
90
90.5
91
91.5
92
Time (s)
Bearing (deg)
Figure 5.3: Experimental Ultrasonic Bear-
ing Measurements
61
The statistics characterizing the error present in the measurements are listed in Table
5.7. The precision of the ultrasonic measurements are offset by their 1Hz rate. An avenue
for additional work would focus on increasing the frequency of the measurements.
Table 5.7: Ultrasonic Sensor Noise
Parameter Value
meanrange 0.038 m
?range 0.004163 m
meanbearing 0.036 deg
?bearing 0.5064 deg
5.3 Navigation Estimator
The navigation estimator employs a relatively simple routine to estimate the six states
identified in Chapter 3. The estimator provides reliable results given a valid model and
accurate noise statistics. However, an error in either the model or the noise statistics can
yield unreliable estimates or an unstable estimator. The observability of the system was
verified to ensure that all states could be estimated from the measurements. The stability
of the estimator was also verified to ensure proper operation.
5.3.1 Simulated Static Data
A set of simulated measurements was produced to replicate measurements from a sta-
tionary UGV. The simulated measurements include a zero mean normal distribution scaled
to approximate the standard deviation of the respective sensor noise. Figure 5.4 compares
the GPS position measurements to the EKF?s position estimates. The initial measurement
62
is used to initialize the EKF, which is the position of the initial outlier. The filter refines
its estimate as additional measurements are available.
?6 ?4 ?2 0 2 4 6 8 10
?4
?2
0
2
4
6
East (m)
North (m)
Actual GPS EKF
Figure 5.4: Simulated Static Position Esti-
mate
0 5 10 15 20 25 30?100
?50
0
50
100
150
Time (s)
Heading (deg)
 
 
GPS EKF Actual
Figure 5.5: Simulated Static Heading Esti-
mate
Figure 5.5 illustrates the inability of the EKF to estimate heading when stationary.
The heading obtained from GPS is discarded to prevent corruption of the estimate with the
introduction of false readings. The GPS receiver is unable to measure a course as discussed
previously in Section 3.2.1. GPS from a single antennae cannot provide a deterministic
measure of heading when the vehicle is stationary. The noise present in the heading mea-
surement is a function of the measured velocity. Equation (5.4) is used to model the inverse
relationship between the noise and velocity [8].
?GPS? = ?
GPSV
VGPS (5.4)
63
In this research, the heading measurements provided by the GPS receiver were dis-
carded when the UGV?s velocity fell below 0.1 m/s. Below this threshold the noise rep-
resents the largest component of the measurement. When the heading measurement is
discarded the EKF integrates only the rate gyro input with each time update. The heading
estimate is allowed to ?coast? based on the IMU inputs until the next heading measurement
is available.
The performance of the filter when estimating a static position is shown in Figure
5.6. The position error drops to within 30 cm during the 30 second run. The mean of
the error after the filter settled is 0.5787 m with a standard deviation of 0.2083 m. The
first two seconds worth of estimates are discarded when calculating statistics due to the
presence of transient error. This mean value and standard deviation provides a significant
improvement over a GPS only estimate. Figure 5.7 shows the EKF?s heading estimate over
the same period. The heading error is a function of the values used to initialize the filter
and the noise present in the rate gyro inputs. The estimate does not provide any useful
information with regards to the heading of the UGV.
0 5 10 15 20 25 300
0.5
1
1.5
2
2.5
3
3.5
Time (s)
Error (m)
Figure 5.6: Simulated Static Position Error
0 5 10 15 20 25 30?30
?20
?10
0
10
20
30
Time (s)
Error (deg)
Figure 5.7: Simulated Static Heading Error
64
5.3.2 Simulated Dynamic Data
A second set of test data is used to simulate the UGV accelerating from zero velocity to
a constant velocity. The heading is held constant to evaluate the performance of the filter in
estimating a heading with available GPS measurements during non stationary tests. Noise
was incorporated representative of the noise present in the actual measurements. Figure 5.8
demonstrates the EKF?s ability to accurately estimate the position. Figure 5.9 shows that
the inclusion of heading measurements does allow the filter to provide an accurate estimate
of heading.
?10 0 10 20 30 40
0
5
10
15
20
25
30
35
40
East (m)
North (m)
GPS EKF
Figure 5.8: Simulated Linear Position Esti-
mate
0 5 10 15 20 25 300
10
20
30
40
50
60
Time (s)
Heading (deg)
Actual GPS EKF
Figure 5.9: Simulated Linear Heading Esti-
mate
The EKF provides a reasonably accurate position estimate with an average error of
0.25 m and standard deviation of 0.1412 m. The heading estimate has an average error of
-0.214 degrees and a standard deviation of 1.6818 degrees. These statistics were calculated
after the filter settled at three seconds.
65
5 10 15 20 25 300
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
Time (s)
Error (m)
Figure 5.10: Simulated Linear Position Er-
ror
5 10 15 20 25 30
?20
?10
0
10
20
30
Time (s)
Error (deg)
Figure 5.11: Simulated Linear Heading Er-
ror
A second set of simulated dynamic inputs and measurements were generated to validate
the EFK at a constant speed with varying yaw rates. A square path with rounded corners
simulated the path of the UGV while it maintained a constant velocity. This path closely
matches conditions the UGV would likely see during operation. Figure 5.12 shows the
simulated path with the GPS coordinates overlaid for reference and the estimate from the
EKF. The filter is initialized with the first position measurement, and as a result requires
time to provide an accurate estimate. The EKF was able to estimate the UGVs position
to within 20 cm by the end of the 30 second run. The mean position error was 0.3409
m with a standard deviation of 0.1411 m. The mean error is slightly higher than the
error present in the straight line simulation; however, the standard deviation is comparable.
Figure 5.13 compares the heading estimate to the actual heading over the entire course.
The EKF successfully estimates heading when receiving GPS heading measurements. This
bounds the error from the integration of the inputs due to noise and drift in the gyro. The
mean heading error for the run is -1.085 deg with a standard deviation of 8.49 degrees.
These values are significantly higher than those present in the straight line simulation. The
66
increase can be attributed to the inclusion of the four turns coupled with the noisy rate
gyro and heading measurements.
?5 0 5 10 15 20
?6
?4
?2
0
2
4
6
8
10
12
East (m)
North (m)
GPS EKF Actual Start/Finish Initial EKF Value
Figure 5.12: Simulated Square Position Es-
timate
0 5 10 15 20 25 30?200
?150
?100
?50
0
50
100
150
200
Time (s)
Heading (deg)
Actual GPS EKF
Figure 5.13: Simulated Square Heading Es-
timate
The final set of simulated inputs changed yaw rate and acceleration sinusoidally. The
scenario simulates the UGV following a series of tight serpentine turns or chicane while
accelerating. This scenario was evaluated to ensure the filter was robust and to prevent
problems later when evaluating experimental data. Figure 5.14 compares the EKF esti-
mate to the simulated path. The velocity in this simulation is significantly greater than
the maximum speed of the MATILDA, but the scenario does provide confidence in the
capabilities of the filter. Figure 5.15 compares the heading estimate to the actual heading.
The filter is able to provide a reliable estimate even with change in speed and heading. The
EKF?s performance using simulated data provides confidence in the ability to estimate the
states using actual measurements.
67
?50 0 50 100 150
0
20
40
60
80
100
120
140
160
180
East (m)
North (m)
GPS EKF Actual
Figure 5.14: Simulated Chicane Position Es-
timate
0 5 10 15 20 25 30?20
?10
0
10
20
30
40
50
60
70
80
Time (s)
Heading (deg) 
 
 
GPS EKF
Figure 5.15: Simulated Chicane Heading
Estimate
5.3.3 Experimental Performance
The EKF was then evaluated using actual measurements received from both sensor
suites. Data from the same time period was used for both sensor suites to save time test-
ing and also to ensure the use of the same GPS satellites. Ideally, the receivers should
provide measurements with similar dilutions of precision (DOP) when using the same satel-
lite signals. The separate runs provided an opportunity to identify any anomalies in the
measurements.
Static Experimental Data
In this experiment, the test rigs were spaced 2.413 m apart while mounted on the UGV
to provide as realistic performance as possible. The two units were arrayed along the same
Easting with the lead unit being the southern unit. The orientation was selected to ease
comparison of position and heading data. The measurements from these runs were used to
68
calculate the noise of each sensor. The position and heading estimates for the lead unit are
shown in Figure 5.16. The estimates for the trail unit are provided in Figure 5.17.
2 4 6 8 10 12
1
2
3
4
5
6
7
East (m)
North (m)
 
 
GPS EKF
0 100 200 300 400 500 600 700?200
?150
?100
?50
0
50
100
150
200
Time (s)
Heading (deg) 
 
 
GPS EKF
Figure 5.16: Experimental Lead Unit Static Estimate
2 3 4 5 6 7 8 9 10 11
1
2
3
4
5
6
7
8
East (m)
North (m)
 
 
GPS EKF
0 100 200 300 400 500 600 700?200
?150
?100
?50
0
50
100
150
200
Time (s)
Heading (deg)
 
 
GPS EKF
Figure 5.17: Experimental Trail Unit Static Estimate
The results using the experimental data are comparable to the simulated runs. The
heading estimate suffers due to lack of heading measurements as was seen in the simulated
data experiments. The integration of the input from the rate gyro yields an estimate;
however, the effect of noise and drift is seen over time. A comparison of the standard
deviation of the simulated and experimental position estimates are provided in Table 5.8.
69
The values closely match between the simulated and experimental data. The simulated
data used additive white noise to simulate the noise present in the measurements. The
noise present in the experimental measurements include limited frequency content. The
slight variation in the values for the experimental data is due to the ?color? of the noise
present in the actual measurements.
Table 5.8: Simulated and Experimental Position Standard Deviation
?N ?E
Simulated 0.4197 m 0.3507 m
Experimental Lead 0.434 m 0.2659 m
Experimental Trail 0.6249 m 0.2069 m
Dynamic Experimental Data
The first dynamic scenario involved driving the UGV in a straight line at a constant
speed from north to south. A MATILDA carried a sensor suite and towed a second sensor
suite 2.431 meters behind along the same path. The same speed was maintained with a brief
pause in the middle of the run. The estimate initially diverged due to faulty heading readings
from the GPS receivers. The receiver heading measurements were correct after several
seconds and the estimates began to converge. No information was exchanged between the
sensor suites for these estimates. Figure 5.18 shows the performance of the estimator using
data from the lead UGV. The measurements and position estimate for the EFK using data
from the trail UGV is shown in Figure 5.19. The scale of the East axis is exaggerated to
show detail. If presented with axis of equal scale the position estimate appears as a straight
line.
70
1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5?60
?50
?40
?30
?20
?10
0
10
20
30
East (m)
North (m)
 
 
GPS EKF
Figure 5.18: Line Lead Position Estimate
?3 ?2 ?1 0 1 2 3 4 5 6?60
?50
?40
?30
?20
?10
0
10
20
East (m)
North (m)
GPS EKF
Figure 5.19: Line Trail Position Estimate
The heading estimate provided by the EKF is significantly improved when heading
measurements are available. The heading estimate for the lead UGV is shown in Figure
5.20. The MATILDA paused in the middle of the run causing faulty GPS readings for the
trail unit. The heading estimate suffers as a result as seen in Figure 5.21 at 60 seconds.
The position estimate for the trail unit at this point also suffers due to the heading error
as the position estimate laterally diverges.
0 20 40 60 80 100 120?200
?150
?100
?50
0
50
100
150
200
Time (s)
Heading (deg)
GPS EKF
Figure 5.20: Line Lead Heading Estimate
0 20 40 60 80 100 120?200
?150
?100
?50
0
50
100
150
200
Time (s)
Heading (deg)
 
 
GPS EKF
Figure 5.21: Line Trail Heading Estimate
71
The second dynamic scenario was performed by driving the UGV in an elliptical path.
Fine steering adjustments of the MATILDA are difficult if not impossible in its current
form. Therefore a smooth trajectory was selected due to its ease of replication and ability
to achieve the desired path. The smooth trajectory also ensures the sensor suites followed
the same path. The separation between the units was reduced to 1.34 m due to the size
of the test area. Figure 5.22 shows the position estimate using data from the lead UGV.
A similar result shown in Figure 5.23 shows how varying measurements yield different
estimates given identical paths. The estimator was able to approximate the path driven
despite GPS measurements that were skewed due to multipath.
?10 ?5 0 5 10
?8
?6
?4
?2
0
2
4
6
8
East (m)
North (m)
GPS EKF
Figure 5.22: Elliptical Lead Position Esti-
mate
?10 ?5 0 5 10
?8
?6
?4
?2
0
2
4
6
8
East (m)
North (m)
 
 
GPS EKF
Figure 5.23: Elliptical Trail Position Esti-
mate
Figure 5.24 presents the heading estimate from the EKF using date from the lead UGV.
As with the straight line experiments, the EKF is able to accurately estimate heading when
GPS measurements are included to bound the error introduced by the integration of IMU
input noise. The heading estimate for the trail UGV is shown in Figure 5.25. The EKF
provides reasonable estimates from the data provided by both sensor suites. The estimates
72
vary slightly due to the noise inherent to the process and measurements from each set of
sensors.
0 10 20 30 40 50 60 70?200
?150
?100
?50
0
50
100
150
200
Time (s)
Heading (deg)
GPS EKF
Figure 5.24: Lead Heading Estimate
0 10 20 30 40 50 60 70?200
?150
?100
?50
0
50
100
150
200
Time (s)
Heading (deg)
 
 
GPS EKF
Figure 5.25: Trail Heading Estimate
5.4 Relative Position Estimator
The relative position estimator was evaluated in the same manner as the navigation
estimator to evaluate the limit of its performance. The same simulated data is used from
the previous section for the lead UGV. A second set simulated data is used for the trail
UGV to ensure no correlation in the noise of the measurements.
5.4.1 Simulated Static Lead with Static Trail
The first scenario evaluated the relative position estimator using stationary UGVs. The
lead UGV was placed at three meters north and four meters east with an initial heading of
45 degrees. The trail UGV was placed at three meters north and twelve meters north with
an initial heading of 45 degrees. This results in a bearing from the lead UGV to the trail
73
UGV of 45 degrees. The EKF is fed 30 seconds of simulated measurement to evaluate its
performance.
This estimator improves the trail position estimate when compared to the results from
the navigation estimator developed in Chapter 3. The relative position estimator uses
global information on both UGVs and relative information on the position and orientation
to generate the estimates. The trail position improvement is due to the inclusion of the
range measurement. Two coordinates are necessary to define a point in a plane. In this
case, the range confines the trail position to an arc about the lead position with a radius
equal to the range measurement. The position estimate for the lead UGV is significantly
improved due the combination of the range and heading measurement. These relative polar
coordinates define the lead position given a trail position. Figure 5.26 demonstrates the
effect of the additional measurement. Figure 5.27 compares the position error of the lead and
trail UGV. The lead position is visibly improved over the trail position without degrading
the trail position.
0 5 10 15 20
?4
?2
0
2
4
6
8
10
12
East (m)
North (m)
 
 
GPSl EKFl GPSt EKFt
Figure 5.26: Simulated Static Position Esti-
mate
0 5 10 15 20 25 300
0.5
1
1.5
2
2.5
3
3.5
Time (s)
Error (m)
EKFl EKFt
Figure 5.27: Simulated Static Position Error
74
The average errors and the standard deviation of the error is presented in Table 5.9.
The error characteristics from the navigation estimates are included for comparison. The
estimators used identical inputs and measurements for the navigation estimator and the lead
unit for the relative position estimator. A comparison of these values further demonstrates
gains realized through the inclusion of the relative measurements.
Table 5.9: Simulated Static Position Estimate Error and Standard Deviation
mean ?
Navigation Estimator 0.5787 m 0.2083 m
Relative Estimator Lead 0.183 m 0.108 m
Relative Estimator Trail 0.1482 m 0.0575 m
The heading estimate for the lead UGV quickly converges to zero as seen in Figure
5.28. This appears to be the result of an error within the algorithm, but upon further
investigation represents the appropriate solution. When the UGV is stationary no heading
measurements are available. The GPS heading measurements are presented in the plots
for reference only. As a result, the EKF has no measurements to bound the error incurred
by the noise introduced by the rate gyro. The heading estimate for the trail UGV shows
how the estimate drifts over time due to this error. The bearing measurement requires
transformation from the body fixed frame to the global ENU frame, thus the heading state
for the lead UGV is related to the relative bearing measurement. The EKF minimizes the
error of each state with the corresponding measurements. The EKF minimizes the error
associated with the bearing measurement by driving the lead heading estimate to zero. With
no heading measurements, the EKF has no basis for comparison to the actual heading of
the UGV. Figure 5.29 compares the estimates to the true heading, further demonstrating
75
the point. The lead UGV estimate is not accurate, but does provide information on the
relative orientation of the UGVs.
0 5 10 15 20 25 30?200
?150
?100
?50
0
50
100
150
200
Time (s)
Heading (deg)
 
 
GPSl EKFl GPSt EKFt
Figure 5.28: Simulated Static Heading Esti-
mate
0 5 10 15 20 25 30?200
?150
?100
?50
0
50
100
150
200
Time (s)
Error (deg)
 
 
EKFl EKFt
Figure 5.29: Simulated StaticHeading Error
This simulation was also used to tune the filter and ensure the additional measurements
will yield improved estimates. The initial values used for the process noise covariance matrix
actually produced estimates with more error than the navigation estimator. Inspection of
the Kalman Gain (L) showed the bearing measurement was improperly weighted. The faulty
weight corrupted the lead heading measurement and subsequently the position estimate for
the lead UGV. More appropriate values for the process noise covariance matrix were selected
and the additional simulated runs generated the expected improvements. The final process
noise and measurement noise covariance matrices are provided in Table 5.10 and Table 5.11
respectively.
76
Table 5.10: Process Noise Covariance
Position Parameter Value
Q1,1 ?2ax ??T 4.3121e-6 m/s
Q2,2 axwalk 0.01 m/s2
Q3,3 ?2rz ??T 1.432e-6 deg
Q4,4 rzwalk 0.001 deg/s
Q5,5 GPSN 1e-6 m
Q6,6 GPSE 1e-6 m
Q7,7 ?2ax ??T 4.3121e-6 m/s
Q8,8 axwalk 0.01 m/s2
Q9,9 ?2rz ??T 1.432e-6 deg
Q10,10 rzwalk 0.001 deg/s
Q11,11 GPSN 1e-6 m
Q12,12 GPSE 1e-6 m
Table 5.11: Measurement Noise Covariance
Position Parameter Value
R1,1 ?2VGPS 0.0025 m/s
R2,2 ?2VWS 7.2082e-7 m/s
R3,3 ?2? 1e4 deg
R4,4 ?2GPSN 3.356 m
R5,5 ?2GPSE 2.4183 m
R6,6 ?2VGPS 0.0025 m/s
R7,7 ?2VWS 7.2082e-7 m/s
R8,8 ?2? 1e4 deg
R9,9 ?2GPSN 2.7726 m
R10,10 ?2GPSE 3.8801 m
R11,11 ?2r 1.2000e-5 m
R12,12 ?2? 1 deg
The variance of the heading measurement used inR3,3 andR8,8 is a function of velocity
and is defined by Equation 5.4. A logic switch is employed to prevent a division by zero
error when the velocity drops below 0.001 m/s and 1e4 is used.
5.4.2 Simulated Dynamic Lead with Static Trail
A second scenario evaluates the ability of the relative position estimator to estimate
the positions and headings of one moving and one stationary UGV. The lead UGV started
at the origin and accelerated to 7.5 m/s along a heading of 45 degrees while the trail
UGV remained stationary. The simulated top speed exceeds the normal operating speed of
77
the MATILDA but was selected to ensure the estimator could function past the operating
range of the UGV. The estimator successfully estimated the position and heading of the
lead UGV. Figure 5.30 compares the GPS position measurements to the estimated positions
for both UGVs. The position error is shown in Figure 5.31 for comparison to the error of
the navigation estimator. The position error of the lead UGV is less than the error present
in the trail UGV estimate up to 15 seconds. This trend matches expectations with the
inclusion of the additional relative measurements. After 15 seconds, the error in the lead
estimate becomes more erratic and exceeds the error of the trail estimate.
EKFlt
0 5 10 15 20 25 30 35 400
5
10
15
20
25
30
35
40
East (m)
North (m)
Figure 5.30: Simulated Dynamic/Static Po-
sition Estimate
0 5 10 15 20 25 300
0.5
1
1.5
2
2.5
3
3.5
Time (s)
Error (m)
EKFl EKFt
Figure 5.31: Simulated Dynamic/Static Po-
sition Error
The increase in error can be attributed to the distance separating the UGVs at this
point. At 15 seconds, the relative range between the UGVs is 12.5 m and the range linearly
increases up to 50 m by the end of the run. The noise characteristics of the range and
bearing are modeled as additive, white and Gaussian with fixed variations. As the range
increases, the error introduced by the bearing measurement represents a larger and larger
portion of the position estimate. The relative range and hence position are a function
78
of the range and bearing measurements. The increase in noise in the measurements yields
additional error in the corresponding estimate. The ultrasonic sensors are unable to operate
at ranges past 6 meters, and the error introduced at greater ranges is thus neglected. The
focus of the thesis has been to determine relative position of the UGVs in close proximity.
The dynamic platform ensured GPS position measurements were available to bound
the heading error for the lead UGV. A comparison of the heading estimates for both UGVs
are shown in Figure 5.32. The additional measurements provide only marginal gains in
performance for the heading of the lead UGV. The precision of the ultrasonic sensors are
offset by the low update rate. The heading error shown in Figure 5.33 illustrates this point.
The main gains achieved by the additional measurements occur in the position refinement
when the lead UGV is moving.
0 5 10 15 20 25 30?200
?150
?100
?50
0
50
100
150
200
Time (s)
Heading (deg)
 
 
EKFl EKFt GPSl GPSt
Figure 5.32: Simulated Dynamic/Static
Heading Estimate
0 5 10 15 20 25 30?200
?150
?100
?50
0
50
100
150
200
Time (s)
Error (deg)
EKFl EKFt
Figure 5.33: Simulated Dynamic/Static
Heading Error
A comparison of the mean error and standard deviation for simulated straight line data
is provided in Table 5.12. A significant decrease in the position error is seen. The heading
79
error is comparable for the navigation and the relative position estimator. The values are
calculated after the filter settles to accurately reflect the steady state performance.
Table 5.12: Simulated Linear Estimate Error and Standard Deviation
meanposition ?position meanheading ?heading
Navigation Estimator 0.25 m 0.14112 m -0.214 deg 1.6818 deg
Relative Estimator 0.1364 m 0.1225 m -0.379 deg 1.719 deg
5.4.3 Simulated Dynamic Lead with Dynamic Trail
A final simulation evaluated the estimator?s performance while both UGVs are moving.
The UGVs maintained the same speed and heading to mimic traveling in formation. The
UGVs traveled along parallel paths with the UGVs separated by 4 meters both laterally
and longitudinally. Figure 5.34 shows the position estimates during the simulation. The
estimates are initially erratic due to the exclusion of faulty GPS heading measurements.
The EKF was able to estimate the position of both UGVs to within half a meter after
several seconds of measurements. The error of the position estimates is shown in Figure
5.35. The minimum error achieved is 8 cm; however, the mean lead error is 14.2 cm and the
mean trail error is 21.1 cm. The standard deviation for the lead and trail error is 11.7 cm
and 5.85 cm respectively. The mean value of the error is a function of the noise introduced
with each measurement. Reduction of the error requires less noise to be introduced into the
system via higher quality measurements or increase the number of measurements available.
80
0 5 10 15 20 25 30 35 40 45
5
10
15
20
25
30
35
East (m)
North (m)
 
 
GPSl EKFl GPSt EKFt
Figure 5.34: Simulated Dynamic/Dynamic
Position Estimate
0 5 10 15 20 25 300
0.5
1
1.5
2
2.5
3
3.5
Time (s)
Error (m)
 
 
EKFl EKFt
Figure 5.35: Simulated Dynamic/Dynamic
Position Error
The inclusion of the relative measurements visibly alters the response of the estimator.
Figure 5.36 compares the estimated headings of the trail and lead UGV. The lead esti-
mate converges smoothly from the initial value to the actual heading. The process takes
approximately five seconds, which represents five sets of relative measurements. The trail
estimate overshoots the actual heading twice before settling to the actual value. Figure 5.37
compares the error present in both estimates over time.
5 10 15 20 25 300
10
20
30
40
50
60
Time (s)
Heading (deg)
 
 
GPSl EKFl GPSt EKFt
Figure 5.36: Simulated Dynamic/Dynamic
Heading Estimate
0 5 10 15 20 25 30?150
?100
?50
0
50
100
150
200
Time (s)
Error (deg)
 
 
EKFl EKFt
Figure 5.37: Simulated Dynamic/Dynamic
Heading Error
81
5.4.4 Experimental Performance
Once the EKF was successfully evaluated with simulated data, actual data received
from both sensor suites was evaluated. The experimental data was collected simultaneously
from both sensor suites and the UTC field was used to line up measurements. The estimates
for each UGV vary due to the noise unique to each sensor?s measurements.
Static/Static Experimental Tests
The same test procedure from Section 5.3.3 was used to evaluate the static performance
using the experimental system. The trail position estimate for the UGVs showed improve-
ment over the estimate from the navigation estimator. The results matched the simulated
results for both UGVs. The estimator is able to effectively use the additional measurements
to refine the position estimate.
The position estimates of the lead and trail UGV are shown in Figure 5.38. The
estimates converge to values along the same east value and are properly spaced based on the
actual spacing of the robots. The estimate provided using the actual data from the sensors
illustrates how the noise present in the measurements affects the estimate. The simulation
used a Gaussian white distribution to replicate noise on the GPS measurements. The noise
in the actual GPS measurements is slightly colored and the error present in the estimate
reflects this characteristic. The estimate does converge, but meanders as it approaches the
steady state value in the same manner as the GPS measurements. The presence of colored
noise violates one of the assumptions of the EKF and the resulting estimate includes slightly
higher error. The resulting estimate is not optimal but does represent a much better result
than could be achieved with a single measurement or input.
82
4 5 6 7 8 9
2
2.5
3
3.5
4
4.5
5
5.5
6
East (m)
North (m)
EKFl EKFt
Figure 5.38: Experimental Static/Static Position Estimate
The standard deviation of the position estimates are provided in Table 5.13. The
values previously presented in Table 5.8 are included for comparison. The most significant
improvement can be see when comparing the standard deviations for the trail unit.
Table 5.13: Noise Characteristics Comparison of Estimator Position Standard Deviation
Navigation Estimator Relative Estimator
?N ?E ?N ?E
Simulated 0.4197 m 0.3507 m 0.5326 m 0.4521 m
Experimental Lead 0.434 m 0.2659 m 0.4848 m 0.2495 m
Experimental Trail 0.6249 m 0.2069 m 0.4890 m 0.1793 m
83
A comparison of the heading estimates from the same run are shown in Figure 5.39.
The heading estimate of the lead UGV converges to zero and dithers about the value for
the entire ten minute run. The variation of the experimental heading for the lead unit is
slightly higher than the variation observed in the simulated runs, but the estimates are
comparable. The heading estimate of the trail UGV varies over the entire run as expected
due to the lack of trail heading measurements.
0 100 200 300 400 500 600 700?200
?150
?100
?50
0
50
100
150
200
Time (s)
Heading (deg)
 
 
EKFl EKFt
Figure 5.39: Experimental Static/Static Heading Estimate
Dynamic/Dynamic Experimental Tests
The experimental procedures outlined in Section 5.3.3 are replicated to evaluate the
relative position estimator with dynamic inputs. The UGVs travel in a clockwise direction
with the trail sensor suit being towed 1.34 m behind the lead UGV. The elliptical path
84
allows for comparison of the lead and trail estimates. Figure 5.40 compares the position
estimates for both UGVs. The estimates are quite promising throughout the entire run.
The trail estimate diverges at the turn in the lower left corner of the figure. Inspection
of the GPS measurements provided to the respective units in Figure 5.18 and Figure 5.19
shows significantly more noise in the trail unit?s measurements at that period. Comparing
the estimates from both EKFs shows the increased accuracy of the estimate provided by
the relative position estimator.
?10 ?5 0 5 10
?8
?6
?4
?2
0
2
4
6
8
East (m)
North (m)
 
 
EKFl EKFt
Figure 5.40: Experimental Dynamic/Dynamic Position Estimate
Figure 5.41 compares the heading estimates for both UGVs during the same period.
The spikes in the heading estimate for the lead UGV are not removed but the magnitude of
the spike is attenuated when compared to Figure 5.24 and Figure 5.25 from the navigation
estimator estimates. The lag between the lead and trail estimate is due to the separation
85
maintained between the sensors. The MATILDA is traveling at 0.9 m/s. The separation of
1.34 m between units represents a time delay of 1.48 s between the lead UGV?s measurements
and the trail UGV?s measurements from the same point on the path.
0 10 20 30 40 50 60 70?200
?150
?100
?50
0
50
100
150
200
Time (s)
Heading (deg)
 
 
EKFl EKFt
Figure 5.41: Experimental Dynamic/Dynamic Heading Estimate
A visual comparison of position estimates from the navigation estimator are shown
in Figure 5.42. The position estimates are generated by separate estimators with no in-
formation included from the other UGV. The position estimates match the traveled path;
however, the relative position and orientation vary over the length of the run. The position
estimates from the relative position estimator are shown in Figure 5.43 for comparison.
86
?10 ?5 0 5 10
?8
?6
?4
?2
0
2
4
6
8
East (m)
North (m)
 
 
EKFt EKFl
Figure 5.42: Position Estimates using Nav-
igation Estimators
?10 ?5 0 5 10
?8
?6
?4
?2
0
2
4
6
8
East (m)
North (m)
 
 
EKFl EKFt
Figure 5.43: Position Estimates using Rela-
tive Position Estimator
The relative position error between the lead and trail UGV are shown in Figure 5.44.
Once the relative position estimator reaches steady state after 3 seconds, the relative po-
sition error remains less than 20 cm. The mean error for the relative position estimator
is 6.595 cm compared to 71.86 cm for the navigation estimator. This value represents a
significant improvement for estimating relative position when compared to the values from
using a pair of navigation estimators or GPS alone.
87
0 10 20 30 40 50 60 70?0.2
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
Time (s)
Position Error (m)
 
 
Relative Estimator Navigation Estimator
Figure 5.44: Relative Position Error on Elliptical Path
5.5 Conclusion
The Extended Kalman Filter processes noisy measurements in order provide an op-
timum estimate of the states. The noise characteristics for each sensor were empirically
determined and used to populate the sensor noise and process noise covariance matrices
for the EKF. The performance of the navigation estimator EKF was evaluated using both
simulated and experimental data with the individual estimator of Chapter 3 and the relative
estimator of Chapter 4. The simulations provided insight into the estimate error under a
variety of static and dynamic scenarios. The navigation EKF evaluated actual data from
the lead and trail sensor packages to ensure proper operation of the estimator and the sensor
88
packages. The estimates from the position navigator served as base lines for comparison of
the relative position estimator.
The relative position estimator was evaluated using the same simulated and experimen-
tal data. The simulations provided insight into the performance of the EKF and the lower
limit for error that could be expected in the provided estimates. A visible improvement in
the heading and position estimates can be seen when compared to the navigation estimates.
The inherent limitation of estimating heading while stationary was not overcome; however,
the relative measurements do provide additional information about the orientation and po-
sition of the UGVs when static. The inclusion of the additional shared measurements was
shown to improve the fidelity of the position estimates. The relative position error was
reduced an order of magnitude by using the relative position estimator.
89
Chapter 6
Conclusion
6.1 Summary of Contributions
This thesis has shown how relative measurements can be used to improve position and
heading estimates for control of multiple UGVs. Ultrasonic sensors were employed in a
novel manner to provide relative range and bearing measurements between a pair of UGVs
in an outdoor environment. A low cost GPS receiver provided an ultraprecise Pulse Per
Second (PPS) signal to synchronize the ultrasonic sensors on each UGV. The common time
provided by the PPS allowed the sensors to reliably provide measurements in a noisy outside
environment.
A navigation estimator based on an Extended Kalman Filter was also developed to
provide position and heading estimates. The filter used inputs and measurements from a
MEMS based IMU, track sensors, and GPS receiver. The navigation estimator outputs
provided the baseline for comparison of the relative position estimator. Simulated static
and dynamic data was used to test the performance of the algorithm to ensure proper
operation and baseline values for later comparison. The navigation estimator provided the
base algorithm for the relative position estimator.
The PPS triggered ultrasonic sensor concept was implemented and validated to ensure
the selected components would reliably provide range and bearing measurements. Experi-
mental testing demonstrated that the concept reliably provided measurements with preci-
sion that was comparable to the other sensors. The primary concern associated with the
measurements was the slower output rate of 1 Hz.
90
The relative position estimator developed was an extension of the navigation estimator
that uses the global measurements from both UGVs and the relative measurements of their
position and orientation. The use of the shared measurements yielded a higher fidelity
position and heading estimate under many conditions. The position estimates improved
under both static and dynamic conditions when compared to the navigation estimator.
The heading estimates improved when the GPS provided heading measurements. The
inability of the GPS to provide course measurements when stationary prevented the EKF
from providing a meaningful heading estimate for static data. This limitation is present in
both estimators due to lack of observability of that state.
The research presented the performance gains that can be achieved with relative mea-
surements and a concept to provide them. The noise present in the measurements limit
the precision of the estimates provided by the EKF. Additional work is required to refine
the concept and the implementation of the relative position estimator. Additional work is
required to fully explore the gains in performance that can be achieved using ultrasonic
sensors to provide relative measurements of UGV position and orientation.
6.2 Future Work
The first avenue of future work is to implement the relative position estimator in real
time to allow for use in a controller for the UGV. This nontrivial task requires the algorithm
to be transferred from MATLAB to C++. The current algorithm is used in post process
and does not have the same time constraints that will be imposed on the real time version.
The mathematical operations required and the memory necessary are within the capabilities
91
of current computer technology, but significant development will be required to ensure the
algorithm runs properly.
Another avenue of future work is to investigate the problems and performance gains
achieved by implementing the relative position estimator with a cascaded filter approach.
The use of several smaller filters would reduce the computational burden. The correlation
of noise in the estimates would require further investigation as discussed in Section 4.2.
Reducing the computational cost of the algorithm will also aid in real time implementation.
Further work is also needed to overcome the estimator?s inability to measure heading.
This limitation stems from the inability of a single antennae GPS receiver to measure course
when the UGV is not moving. The use of a dual antennae GPS system could provide
this measurement. The dual antennae system could also be used to measure heading in
addition to course. The inclusion of heading measurements when stationary would allow
for estimation of initial heading. This approach would require additional hardware and
additional operations within the algorithm.
Another avenue for further work is investigating the affect of increasing the relative
measurement rate. The RCM4100 clock synchronized with the PPS could be used to gen-
erate a higher frequency signal for the ultrasonic sensors. The drift of the clock on the
RCM4100 would introduce noise into the relative measurements; however, the magnitude
may be small enough to warrant the use of the higher measurement rate. uBlox recently
introduced a receiver that provides a 10 PPS signal that would provide the higher rate
without the noise introduced by the RCM4100 clock. The effect on the IMU would have
to be evaluated since its design is based on a 1 Hz PPS signal. The higher rate may cause
conflicts with the embedded IMU processor. Both solutions provide a higher measurement
92
rate; however, evaluation of the performance based on the rate is also necessary to ensure
the selected rate complements the other measurements and system dynamics.
The inclusion of a bearing measurement from the trail to the lead UGV warrants
investigation as well. The inclusion of range and bearing measurements from the trail unit
should yield performance similar to the position and heading estimates of the lead unit.
The increase in performance must also be evaluated based on the increased computational
burden necessary to implement the larger estimator.
93
Bibliography
[1] E. Abbott and D. Powell, Land-vehicle navigation using gps, Proceedings of the IEEE,
vol. 87, IEEE, January 1999, pp. 145?162.
[2] L. Angrisani, A. Baccigalupi, and R. Schiano Lo Moriello, A measurement method
based on kalman filtering for ultrasonic time-of-flight estimation, Instrumentation and
Measurement, IEEE Transactions on 55 (April 2006), no. 2, 442?448.
[3] Z. Berman and J.D. Powell, The role of dead-reckoning and inertial sensors in fu-
ture general aviation navigation, Proceedings of the 1998 IEEE Position Location and
Navigation Symposium, 1998, pp. 510?517.
[4] D. M. Bevly, Global positioning system (gps): A low-cost velocity sensor for correcting
inertial sensor errors on ground vehicles, Journal of Dynamic Systems, Measurement,
and Control 126 (2004), 255?264.
[5] J. J. Biesiadecki, P. C. Leger, and M. W. Maimone, Tradeoffs between directed and au-
tonomous driving on the mars exploration rovers, The International Journal of Robotics
Research 26 (2007), 91?104.
[6] J. Borenstein and L. Feng, Measurement and correction of systematic odometry errors
in mobile robots, IEEE Transactions on Robotics and Automation 12 (1996), no. 6.
[7] J. Borenstein and Y. Koren, Obstacle avoidance with ultrasonic sensors, Robotics and
Automation, IEEE Journal of [see also IEEE Transactions on Robotics and Automa-
tion] 4 (Apr 1988), no. 2, 213?218.
[8] R. Daily and D.M. Bevly, The use of gps for vehicle stability control systems, IEEE
Transactions on Industrial Electronics 51 (2004), no. 2, 270?277.
[9] G. Dedes, R. Da and K. Shubert, Design and analysis of a high-accuracy airborne
gps/ins system, Proceedings of the 9th International Technical Meeting of the Satellite
Division of The Institute of Navigation, September 1996, pp. 955?964.
[10] Defense Advanced Research Projects Agency, Arlington, VA, Urban challenge rules,
October 27 2007.
[11] D. W. Gage, Ugv history 101: A brief history of ummaned ground vehicle (ugv) devel-
opment efforts, Ummaned Systems Magazine 13 (1995).
94
[12] S. Godha and M.E. Cannon, Integration of dgps with a low cost mems-based inertial
measurement unit (imu) for land vehicle navigation applications, Procceedings of the
ION GNSS (Long Beach, CA), ION, September 2005.
[13] J.L. Jones, Robots at the tipping point: the road to irobot roomba, Robotics & Automa-
tion Magazine, IEEE 13 (2006), no. 1, 76?78.
[14] H. Kuttruff, Room acoustics, 2nd ed., Applied Science Publishers, 1979.
[15] W. Lewandowski and C. Thomas, Gps time transfer, Proceedings of the IEEE 79 (Jul
1991), no. 7, 991?1000.
[16] J.L. Martinez, A. Mandow, J. Morales, A. Garcia-Cerezo, and S. Pedraza, Kinematic
modelling of tracked vehicles by experimental identification, Proc. IEEE/RSJ Interna-
tional Conference on Intelligent Robots and Systems (IROS 2004), vol. 2, 28 Sept.?2
Oct. 2004, pp. 1487?1492.
[17] Mesa Associates, Inc, Madison, AL, Operator?s and maintenance manual, April 2004.
[18] S. Panzieri, F. Pascucci, and G. Ulivi, An outdoor navigation system using gps and
inertial platform, IEEE/ASME Transactions on Mechatronics 7 (2002), no. 2.
[19] B.W. Parkinson and J.J. Spilker (eds.), Global postioning system: Theory and applci-
ations, vol. I, AIAA, 1996.
[20] Rabbit Semiconductor, Rcm4100 rabbitcore user?s manual, 2007.
[21] S.I. Roumeliotis and G.A. Bekey, Collective localization: a distributed kalman filter
approach to localization of groups of mobile robots, Proc. IEEE International Conference
on Robotics and Automation ICRA ?00, vol. 3, 24?28 April 2000, pp. 2958?2965.
[22] D. Rubinstein and R. Hitron, A detailed multi-body model for dynamic simulation of
off-road tracked vehicles, Journal of Terramechanics 41 (2004), 163?173.
[23] J.E. Shigley and J.J. Uicker, Theory of machines and mechanism, 2nd ed., McGraw-Hill
seried in mechanical engineering, McGraw-Hill, 1995.
[24] B. Skylar, Digital communications, 2nd ed., Prentice Hall PTR, 2004.
[25] R.F. Stengel, Optimal control and estimation, Dover books on advanced mathematics,
Dover Publications, New York, 1994.
[26] T. Tanzawa, N. Kiyohiro, S. Kotani, and H. Mori, The ultrasonic range finder for
outdoor mobile robots, Intelligent Robots and Systems 95. ?Human Robot Interaction
and Cooperative Robots?, Proceedings. 1995 IEEE/RSJ International Conference on 3
(5-9 Aug 1995), 368?373 vol.3.
95
[27] J. Yang and J. Kim, Sliding mode control for trajectory tracking of nonholonomic
wheeled mobile robots, IEEE Transactions on Robotics and Automation 15 (1999),
no. 3, 578?587.
96
Appendix A
Nomenclature
97
Nomenclature
?E East component of velocity vector in global ENU frame
?N North component of velocity vector in global ENU frame
?X First derivative of state
? Wavelength
? Heading
ab Accelerometer Bias
B Input Matrix
C Observation Matrix
c Speed of sound
E Position of robot in North direction
f Frequency
gb Rate Gyro Bias
J Jacobian
Lk Filter gain
N Position of robot in North direction
P Covariance Matrix
98
u Inputs to system
Vx Longitudinal velocity of robot
Vx X component of velocity vector in body fixed frame
x State matrix
Y Measurements of system
99