PATH DUPLICATION USING GPS CARRIER BASED RELATIVE POSITION FOR
AUTOMATED GROUND VEHICLE CONVOYS
by
William E. Travis III
A dissertation submitted to the Graduate Faculty of
Auburn University
in partial fulfillment of the
requirements for a Degree of
Doctor of Philosophy
Auburn, Alabama
May 14, 2010
Keywords: leaderfollower, path following, relative position,
robotic convoy, RTK, unmanned system
Copyright 2010 by William E. Travis III
Approved by
David M. Bevly, Chair, Associate Professor of Mechanical Engineering
George T. Flowers, Professor of Mechanical Engineering
Andrew J. Sinclair, Assistant Professor of Aerospace Engineering
ii
Abstract
A GPS based automated convoy strategy to duplicate the path of a lead vehicle is
presented in this dissertation. Laser scanners and cameras are not used; all information
available comes from GPS or inertial systems. An algorithm is detailed that uses GPS
carrier phase measurements to determine relative position between two moving ground
vehicles. Error analysis shows the accuracy is centimeter level. It is shown that the time
to the first solution fix is dependent upon initial relative position accuracy, and that near
instantaneous fixes can be realized if that accuracy is less than 20 centimeters. The
relative positioning algorithm is then augmented with inertial measurement units to dead
reckon through brief outages. Performance analysis of automotive and tactical grade units
shows the twenty centimeter threshold can be maintained for only a few seconds with the
automotive grade unit and for 14 seconds with the tactical unit.
Next, techniques to determine odometry information in vector form are discussed.
Three methods are outlined: dead reckoning of inertial sensors, time differencing GPS
carrier measurements to determine change in platform position, and aiding the time
differenced carrier measurements with inertial measurements. Partial integration of a
tactical grade inertial measurement unit provided the lowest error drift for the scenarios
investigated, but the time differenced carrier phase approach provided the most cost
feasible approach with similar accuracy.
iii
Finally, the relative position and odometry algorithms are used to generate a
reference by which an automated following vehicle can replicate a lead vehicle's path of
travel. The first method presented uses only the relative position information to
determine a relative angle to the leader. Using the relative angle as a heading reference
for a steering control causes the follower to drive at the lead vehicle, thereby creating a
towing effect on the follower when both vehicles are in motion. Effective use of this
method is limited to short following distances, or line of sight operation, similar to vision
based following approaches. The following vehicle turns about a smaller radius than the
lead vehicle, and this effect intensifies as following distance increases.
The second path duplication method allows for non line of sight operation by
combining the vector odometry with the relative position to create a virtual leader to
follow. The actual difference between the vehicles could be in excess of 100 meters, but
the perceived distance is reduced to a predetermined value based on vehicle speed by re
generating the lead vehicle's position at a previous instance in time with the relative
position and odometry information. Performance curves of path duplication accuracy
versus following distance using different odometry techniques show that the partially
integrated tactical unit provides the best performance, but the time differenced carrier
approach offered very similar performance for a lower total system cost.
Both following methods were implemented on an unmanned ground vehicle.
Tests showed following accuracy for the line of sight method was within 50 centimeters
on straight sections, though the reference accuracy was centimeter level. The non line of
sight method predicted the virtual leader position to within 5 centimeters for following
distances ranging from 10 to 120 meters.
iv
Acknowledgements
9Two are better than one because they have a good return for their labor. 10For if either of them
falls, the one will lift up his companion. But woe to the one who falls when there is not another to
lift him up. 11Furthermore, if two lie down together they keep warm, but how can one be warm
alone? 12And if one can overpower him who is alone, two can resist him. A cord of three strands
is not quickly torn apart.
Ecclesiastes 4:912 (NASB)
It is abundantly clear the Lord intends for us to rely on the assistance and
guidance of others to overcome the challenges presented to us. This consistent theme is
demonstrated across the Bible by partnerships such as Moses and Aaron, Elijah and
Elisha, and Paul and Barnabas. While a graduate program is incomparable to freeing a
group of people from oppression, confronting a popular but false religion, or
withstanding persecution to dutifully deliver the Gospel to other nations, it is nonetheless
an undertaking in which self sufficiency would lead to failure.
The Lord has blessed me with a tremendous support structure, without which I
would have neither the opportunity nor the ability to complete a graduate program. First
and foremost, He provided a loving, caring, patient, understanding, and supportive wife.
Kimberly was and is a consistent source of encouragement and companionship. He also
provided devoted parents willing to make sacrifices to further my goals and interests, and
whose guidance and care emphasized a strong work ethic while instilling a good value
system.
The Lord placed me in a working environment under the advisement of Dave
Bevly in which I was presented with technical challenges and unique, innovative projects.
v
Dr. Bevly provided instruction, incredible opportunities through his hard work, and
imparted indelible experience. Colleagues in the lab provided continual knowledge and
assistance. Specifically, Rob Daily, Matt Lashley, David Hodo, Ben Clark, and Scott
Martin assisted with their individual expertise and help, and were always willing to stay
late to help meet impending deadlines.
Finally, I would like to acknowledge SAIC, GDRS, and TARDEC for providing
the original problem addressed in this dissertation and the necessary funding at various
stages to complete the work.
vi
Table of Contents
Abstract ................................................................................................................... ii
Acknowledgements ............................................................................................................ iv
List of Tables .................................................................................................................. ix
List of Figures ................................................................................................................... x
Nomenclature ................................................................................................................. xv
Chapter 1 Introduction ............................................................................................... 1
1.1 Global Positioning System ........................................................................ 3
1.2 Automated Vehicle Following .................................................................. 9
1.3 Prior Work ............................................................................................... 11
1.4 Contributions ........................................................................................... 15
Chapter 2 DRTK ...................................................................................................... 18
2.1 Introduction ............................................................................................. 18
2.2 Observables ............................................................................................. 21
2.3 Cycle Slip Detection................................................................................ 31
2.4 Single Differenced Integer Estimation .................................................... 33
2.5 Floating Precision Baseline Estimation .................................................. 39
2.6 Ambiguity Integerization ........................................................................ 40
2.7 High Precision RPV Estimation .............................................................. 42
2.8 Experimental Results............................................................................... 43
vii
2.9 Conclusion ............................................................................................... 49
Chapter 3 DRTK/INS .............................................................................................. 51
3.1 Introduction ............................................................................................. 51
3.2 INS Integration Options .......................................................................... 53
3.3 GPS/INS Integration ............................................................................... 56
3.4 DRTK/INS Integration ............................................................................ 73
3.5 Experimental Results............................................................................... 80
3.6 Conclusion ............................................................................................... 84
Chapter 4 Odometry ................................................................................................. 86
4.1 Introduction ............................................................................................. 86
4.2 INS .......................................................................................................... 87
4.3 TDCP ....................................................................................................... 89
4.4 TDCP/INS ............................................................................................... 92
4.5 Error Analysis ......................................................................................... 95
4.6 Conclusion ............................................................................................. 100
Chapter 5 Path Duplication .................................................................................... 102
5.1 Introduction ........................................................................................... 102
5.2 Line of Sight Following ........................................................................ 105
5.3 Non Line Of Sight Following................................................................ 107
5.4 Alternate Operational Modes ................................................................ 116
5.5 Experimental Results............................................................................. 119
5.6 Conclusion ............................................................................................. 124
viii
Chapter 6 Conclusion ............................................................................................. 126
6.1 Conclusion ............................................................................................. 126
6.2 Future Work .......................................................................................... 129
References ............................................................................................................... 132
Appendix A Relative Acceleration ............................................................................ 138
Appendix B Robotic Convoy Experimental Setup .................................................... 142
B.1 Hardware ............................................................................................... 142
B.2 Operation ............................................................................................... 143
B.3 UGV Navigation System ....................................................................... 145
Appendix C Towed Implement Positioning .............................................................. 147
C.1 Introduction ........................................................................................... 147
C.2 Encoder Positioning .............................................................................. 149
C.3 DRTK Positioning ................................................................................. 151
C.4 DRTK Versus Encoder Method ............................................................ 152
C.5 Trailer Length Effects ........................................................................... 156
C.6 Conclusion ............................................................................................. 157
ix
List of Tables
1.1 The standard GPS error budget for individual range errors and the user equivalent
range error ............................................................................................................... 7
2.1 GPS range and phase measurement variable definitions ............................................ 21
2.2 Tracking loop parameters ........................................................................................... 23
2.3 Error between RTK and DRTK solutions as a function of baseline ........................... 45
2.4 Error between RTK and DRTK solutions as a function of observations .................... 45
2.5: RTK and DRTK error versus baseline. ...................................................................... 45
2.6 DRTK error statistics for the dynamic test ................................................................. 47
3.1 Stochastic parameters for inertial sensor error............................................................ 61
3.2 Power spectral density coefficients for various clocks ............................................... 69
4.1: Statistical quantification of the TDCP algorithm output ........................................... 98
5.1: Steady state error during constant 150 m radius turns ............................................. 122
C.1: Accuracy as trailer length varies using the encoder and DRTK methods. .............. 157
x
List of Figures
1.1: The broadcast BPSK GPS signal contains several signals modulated onto the carrier
signal. ...................................................................................................................... 4
1.2: The GPS antenna on the truck receives a shadowed and multipathed signal. ............. 6
1.3: A convoy of UGVs could be controlled by a single human operator. ......................... 9
2.1: Error correlation in the calculated position from two static receivers 500 meters apart
causes the individual positions to drift. The separation distance was removed for
clarity. ................................................................................................................... 19
2.2: An RTK base station broadcasts its observables to a remote GPS receiver to obtain a
centimeter accurate absolute position. .................................................................. 20
2.3: Pseudorange and carrier phase measurement accuracy can be expressed as a function
of the C/N0. ........................................................................................................... 22
2.4: A single differenced observable for a static (top) and dynamic (bottom) platform are
shown. ................................................................................................................... 26
2.5: Double differenced observables for a static (top) and dynamic (bottom) platform are
shown. ................................................................................................................... 29
2.6: The widelane observable has a much longer wavelength than the L1 and L2
observables, which benefits ambiguity resolution. However, the narrowlane
observable has a much shorter wavelength and increases measurement precision.
............................................................................................................................... 30
2.7: The ambiguity observable must be filtered with a sliding window or moving average
filter to remove the obstructing pseudorange noise. ............................................. 31
2.8: A cycle slip can be seen in the single differenced phase observable at the
discontinuity around 1406 seconds. ...................................................................... 32
2.9: Most cycle slips can be clearly seen by monitoring the change in the measured phase
ambiguity. If a cycle slip is detected, the change in ambiguity exceeds a
predefined threshold and the measurement is thrown out. A cycle slip is detected
around 1406 seconds and is well beyond the threshold of one. ............................ 33
xi
2.10: The LAMBDA method takes the estimated covariance (left) and decorrelates it
(right) to create a more favorable and efficient ambiguity search space. ............. 41
2.11: Baseline error from static tests was consistent across a range of separation
distances. ............................................................................................................... 44
2.12: The baseline error remained consistent as the number of observed satellites
changed. ................................................................................................................ 44
2.13: The vehicle paths from the dynamic test. ................................................................ 45
2.14: The baseline between the vehicles varied from 5 to over 500 meters as the vehicles
traveled in opposite directions along a path. ......................................................... 46
2.15: The DRTK low and high precision outputs in the ECEF X direction are shown.
Black dots represent the RPV obtained by differencing the RTK solutions......... 46
2.16: Low and high precision RPV are decimeter and centimeter level accurate,
respectively. .......................................................................................................... 47
2.17: North and east components of the low and high precision solution error are shown.
............................................................................................................................... 48
2.18: Analysis of the north and east error components shows the difference between the
DRTK and RTK solutions is millimeter level. ..................................................... 49
3.1: The number of signal observations and signal quality can fluctuate significantly
when driving in a typical suburban environment. ................................................. 52
3.2: An uncoupled system blends inertial measurements with the navigation output
computed by the GPS receiver. ............................................................................. 54
3.3: Close and tight integrations blend inertial measurements with the range
measurements from a GPS receiver. ..................................................................... 55
3.4: A complementary filter estimates position, velocity, and attitude error to correct the
INS. ....................................................................................................................... 56
3.5: The closely coupled position solution filters out erroneous GPS measurements in a
suburban environment. .......................................................................................... 72
3.6: The closely coupled integration using a full IMU produces 3D velocity estimates. . 73
3.7: Euler angle estimates from the closely coupled system are used to translate motion
from a platform to navigation frame. .................................................................... 73
3.8: The DRTK/INS architecture processes range and inertial data on individual vehicles
to align the inertial measurements. ....................................................................... 73
xii
3.9: The time to first fix increases with the uncertainty of the initial baseline. ................ 77
3.10: The number of ambiguity fixes used to determine the mean TTFF for each initial
error. ...................................................................................................................... 78
3.11: The TTFF is near instant if the RPV error is within a GPS carrier wavelength or
less......................................................................................................................... 78
3.12: The relative inertial error growth using tactical and automotive grade INS for two
ground vehicles traveling straight at 10 meters per second. ................................. 79
3.13: The relative error reaches 20 centimeters in 0.45 seconds with the automotive INS.
............................................................................................................................... 79
3.14: The relative error reaches 20 centimeters in 14.5 seconds with the automotive INS.
............................................................................................................................... 79
3.15: Auburn University's NCAT 1.7 mile oval test track. ............................................... 80
3.16: Processing the inertial measurements gives the DRTK/INS solution a higher output
rate over processing only GPS measurements. ..................................................... 81
3.17: Vehicle separation distance fluctuated throughout the test. .................................... 82
3.18: The DRTK/INS floating point solution tracks the RTK solution with the same
accuracy as the DRTK solution. ........................................................................... 83
3.19: The DRTK/INS solution drifts during a simulated complete outage starting at 328
seconds; accuracy suddenly increases when GPS is available. ............................ 83
3.20: The time for the error to drift 20 cm was recorded during one thousand simulated
full GPS outages. .................................................................................................. 83
4.1: The change in receiver and satellite position is captured in the difference in range
measurements from the user to satellite. ............................................................... 89
4.2: Simulated inertial error growth is reduced when using a partial IMU, provided
assumptions are not violated. ................................................................................ 96
4.3: Two hundred five minute intervals of the 5 Hz and 10 Hz change in position
estimates are overlaid. The 5 Hz estimates have a slightly higher variance. ........ 97
4.4: The accumulation of the 5 Hz and 10 Hz change in position estimates over five
minute intervals show that error is dominated by residual biases. ....................... 97
4.5: The total error accumulation of the 5 Hz and 10 Hz change in position estimates over
five minute intervals. ............................................................................................ 97
xiii
4.6: North and east position (on left, with magnified portion on right) calculated with the
TDCP/INS algorithm agrees with the RTK and SPS solutions. ........................... 99
4.7: The error in position difference from the TDCP/INS algorithm is comparable to the
TDCP algorithm once the filter settles out. ........................................................ 100
5.1: Standard position and RTK position solutions are compared to demonstrate
inconsistencies in the standard solution that would prohibit accurate path
following by dropping waypoint breadcrumbs. .................................................. 103
5.2: The error in change in position demonstrates the frequency and magnitude of
inconsistencies in the reported standard position................................................ 104
5.3: A relative angle between two vehicles can be determined given a relative position
vector. The angle can then be used as a control reference to autonomously follow
a lead vehicle....................................................................................................... 105
5.4: Relative angle accuracy is a function of relative position accuracy and antenna
separation distance. ............................................................................................. 106
5.5: The steady state error is a function of the lead vehicle turning radius and the baseline.
............................................................................................................................. 107
5.6: A virtual lead vehicle is created by adding the change in follower position to a
relative position measurement at a previous instance in time. ............................ 108
5.7: A look ahead distance is computed, and the follower chooses to follow the virtual
lead vehicle just outside of the radius. ................................................................ 110
5.8: Lateral error as a function of following distance and speed with a full automotive
grade INS. ........................................................................................................... 112
5.9: Lateral error as a function of following distance and speed with a full tactical grade
INS. ..................................................................................................................... 112
5.10: Lateral error as a function of following distance and speed with a partial automotive
grade INS. ........................................................................................................... 113
5.11: Lateral error as a function of following distance and speed with a partial tactical
grade INS. ........................................................................................................... 113
5.12: Lateral error as a function of following distance and speed with the TDCP method.
............................................................................................................................. 114
5.13: Lateral error as a function of following distance at five meters per second for all
odometry methods. .............................................................................................. 115
xiv
5.14: Lateral error as a function of following distance at twenty meters per second for all
odometry methods. .............................................................................................. 115
5.15: The lead vehicle's path can be maintained relative to the follower as long as the
follower remains stationary. ................................................................................ 117
5.16: Shown are the positions of the lead and following vehicle (in gray and black,
respectively) after multiple passes on the south portion of the track. ................. 120
5.17: Typical lateral path error while driving straight. ................................................... 121
5.18: The following distance corresponding to the lateral error in Figure 5.17. ............ 121
5.19: Trajectories of the lead and automated following vehicle as the lead vehicle
maneuvered around a parking lot. ....................................................................... 123
5.20: Following distance along the path for the experiment shown in Figure 5.19. ....... 123
5.21: Actual path deviation, shown in gray, was consistently below twenty centimeters,
but the reference generated with the NLOS following method was under five
centimeters. ......................................................................................................... 124
A.1: Point A is expressed in both the stationary i frame and the moving e frame. ........ 138
A.2: The ECI and ECEF frames share an origin and Z axis and the two points, A and B,
are objects moving above the Earth. ................................................................... 140
B.1: The robotic convoy consisted of a human driven Hyundai Santa Fe and an ATV
Corp. Prowler that was converted to a UGV. ..................................................... 142
B.2: The NCAT test track as seen in Google Earth. ....................................................... 143
C.1: A Segway RMP towing a trailer containing geophysical equipment. .................... 147
C.2: A schematic model of a towedtrailer system. ........................................................ 150
C.3: The test path driven by the robot, which was pulling a trailer. ............................... 153
C.4: Zooming in reveals the precision of both methods, but the DRTK position is more
precise. ................................................................................................................ 153
C.5: Total error in trailer position using the DRTK and encoder methods. .................... 154
C.6: Lateral and longitudinal trailer position error. ........................................................ 155
C.7: A scatter plot reveals the accuracy of the methods. ................................................ 156
C.8: A parameter error caused a reduction in accuracy using the encoder method. ....... 156
xv
Nomenclature
Variable nomenclature uses lower case characters for scalars, bold lower case characters
for vectors, and bold upper case characters for matrices. The exception is the variable ?,
which is reserved for the carrier phase ambiguity. Acronyms used in the work are listed
below:
AAR Automated Airborne
Refueling
ANS Autonomous Navigation
System
AGV Autonomous Ground Vehicle BCT Brigade Combat Team
BPSK Binary Phase Shift Keying C/A Clear Acquisition
CAST Convoy Active Safety
Technologies
C/N0 Carrier to Noise ratio
DARPA Defense Advanced Research
Projects Agency
DLL Delay Lock Loop
DoD Department of Defense DOP Dilution of Precision
DRTK Dynamic base Real Time
Kinematic
ECEF Earth Centered, Earth Fixed
ENU East, North, Up FAA Federal Aviation
Administration
FCS Future Combat Systems GBAS Ground Based Augmentation
System
GDRS General Dynamics Robotic
Systems
GPS Global Positioning System
IMU Inertial Measurement Unit INS Inertial Navigation System
xvi
JPALS Joint Precision Approach and
Landing
LAAS Local Area Augmentation
System
LAMBDA Least Squares AMBiguity
Decorrelation Adjustment
NCAT National Center for Asphalt
Technology
P Protected (military GPS
code)
PLL Phase Lock Loop
PNT Position, Navigation, and
Time
PRN PseudoRandom Noise
P(Y) Encrypted protected (military
GPS code)
RCX Robotic Convoy eXperiment
RPV Relative Position Vector RTK Real Time Kinematic
SBAS Satellite Based Augmentation
System
SRGPS Shipboard Relative GPS
SV Space Vehicle TARDEC Tank Automotive Research
Development and Engineering
Center
TDCP Time Differenced Carrier
Phase
TOA Time Of Arrival
UERE User Equivalent Range Error UGV Unmanned Ground Vehicle
US United States (of America) USD US Dollars
UXO UneXploded Ordinance WAAS Wide Area Augmentation
System
WAGE Wide Area GPS
Enhancement
1
Chapter 1
Introduction
The pinnacle for automated systems in the transportation field is a fully autonomous
ground vehicle (AGV), also known as an unmanned ground vehicle (UGV). The UGV
concept is fairly simple. Upon receiving a desired location from either human or
machine, the UGV self sufficiently travels to that location. The potential increase in
efficiency, productivity, performance, and safety to the transportation system is almost
difficult to realize. Imagine a highway system with no traffic lights or stop signs.
Imagine working, watching a video, or even sleeping while in the driver's seat. Imagine
traffic accidents being an anomaly. Imagine never even learning how to drive!
Humans have devised means to automate many tasks for centuries, but a reliable
and trustworthy UGV has proven to be elusive. Leonardo da Vinci is credited with
designing one of the first UGVs around 1495. His open loop design was clocklike in
that a system of gears and springs were used to propel and steer the vehicle. General
Motors and RCA teamed in the 1950s and 1960s to develop an electronic highway
system. Steering automation was made possible by a guidance cable on the road. Speed
control was managed by a vehicle detection system embedded in the road that broadcast
speed commands to passing vehicles. In recent times, the Global Positioning System
(GPS) has enabled UGVs to operate independently of installed infrastructure. Research in
2
the UGV arena exploded in the 1990s due to GPS and the availability of computer chips
that were exponentially decreasing in size and increasing in computational ability. The
Defense Advanced Research Projects Agency (DARPA), in response to a Congressional
mandate stating one third of the operational combat vehicles be automated by 2015,
hosted three events in 2004, 2005, and 2007 to spur the growth of UGV technology. The
technological growth was obvious over the course of the three events. No teams finished
the 2004 Grand Challenge, which took place in a relatively benign desert environment.
In fact, the furthest any team traveled was 7 out of 150 miles. However, five teams
finished in the 2005 Grand Challenge, which took place in a similar, but slightly more
challenging environment. The 2007 Urban Challenge, hosted at a public airport
(formerly George Air Force Base), presented the robotic contestants with traffic scenarios
encountered in everyday driving. All UGVs were on the course simultaneously with a
number of manned automobiles. Despite the significant increase in the presented
challenges, six teams finished.
Further refinement needs to occur before UGVs operate interactively with
humans on a regular basis to minimize the risk to human safety. The military provides
practical scenarios to test components of UGV technology, often while keeping them
under some form of human control or supervision. UGVs have been successfully used to
locate and destroy mines and bombs, perform surveillance missions, or guard a perimeter.
UGVs have even been weaponized, though the use of force remains under human control
and is teleoperated.
Automated vehicle following systems are expected to help refine UGV
technology while interacting with humans on a basic level. The concept is to get a UGV
3
to duplicate the path of a lead vehicle. Both the military and civilian market have current
interest in the technology for automated ground vehicle convoys for the transportation of
equipment, supplies, and even people. However, determining the path of travel of a lead
vehicle is not a simple matter. This dissertation focuses on the use of GPS based relative
position measurements as a reference to enable ground vehicles to duplicate the path
driven by a lead vehicle.
1.1 GLOBAL POSITIONING SYSTEM
GPS is a satellite based radio navigation system that provides worldwide position,
navigation, and time (PNT) information to military and civilian users. The GPS satellite
constellation consists of a nominal 24 SVs, or space vehicles. Each SV transmits ranging
and navigation data on at least two L band carrier frequencies. The L1 carrier is centered
at 1575.42 MHz and the L2 carrier is centered at 1227.60 MHz [43]. A third frequency
named L5 and centered at 1176.45 MHz is being introduced to the constellation, with the
first SV being launched on March 24, 2009 [18]. The transmitted GPS signal is
generated using binary phase shift keying (BPSK) and contains several codes modulated
onto the carrier signal. An example of the signal structure is shown in Figure 1.1.
Periodic ranging codes are modulated onto each carrier and are specific to the
broadcasting SV. These codes, or binary sequences, have the autocorrelation and cross
correlation properties of white noise but remain deterministic. These sequences are
referred to as pseudorandom noise (PRN) sequences. Both a civilian code, known as the
C/Acode (Clear or Coarse Acquisition) code, and a military code, known as the P(Y)
code (an encrypted version of the Protected code, or Pcode) are modulated onto the L1
carrier frequency with designated PRN numbers [37]. PRN numbers 132 are assigned to
4
GPS SV's according to GPSICD200c [38]. Each C/Acode sequence consists of 1023
bits, or chips, and is repeated in one millisecond intervals. The transmission frequency,
also known as the chipping rate, is 1.023 MHz or megachips per second. The Pcode is
extremely long relative to the C/Acode at 6.1871x1012 chips. The Pcode is modulated
with an encryption sequence known as the Wcode to form the encrypted P(Y)code,
which is broadcast ten times faster than the C/Acode at 10.23 MHz. Most satellites
broadcast only the encrypted P(Y)code on the L2 frequency, but modernization efforts
are incorporating a civilian L2Ccode to aid in atmospheric error reduction and enhance
accuracy [27]. Receivers cannot directly track the P(Y)code without an encryption key,
but semicodeless techniques have been developed that allow civilian receivers to track
the P(Y) code without the key.
A navigation message is also embedded into the carrier signal broadcast by each
SV. This message contains ephemeris data to determine the position and velocity of the
SV, clock parameters to remove range error due to the SV clock drift, SV health
information, and almanac data to compute rough positions for all SVs in the GPS
Figure 1.1: The broadcast BPSK GPS signal contains several signals modulated onto the
carrier signal.
B r o a d c a s t S i g n a l
N a v i g a t i o n D a t a
P R N C o d e
C a r r i e r
5
constellation. The data is contained in five 300 bit subframe messages and transmitted at
50 bits per second. The ephemeris data is repeated in 30 second intervals, and the
almanac data is repeated in 12.5 minute intervals [27,37].
The ranging codes are used to determine a user to satellite range using a time of
arrival (TOA) technique. Each range and knowledge of the SV position provides a
directional constraint on the user position. Theoretically, three ranges from three different
SVs should be sufficient to determine a three dimensional position solution. However, a
minimum of four satellites is typically necessary to determine a three dimensional
position solution and a bias in the receiver clock.
The GPS signal is corrupted by various error sources, both when it is generated
and as it propagates through the Earth's atmosphere. Additional error is induced by the
environment surrounding the receiver when the signal is being tracked. These error
sources distort the true range between the user and SV, and the resulting measured range
is referred to as a pseudorange.
The bias and noise in the clock onboard the SV creates the initial error in the
transmitted signal. An erroneous time of transmission will lengthen the pseudorange if
the clock is retarded, or it will shorten the pseudorange if it is advanced. However, bias
effects can be reduced by using the clock correction terms contained in the navigation
data.
The receiver clock errors corrupt the receiver in a similar manner. The difference
is the receiver clock errors are common across all tracking channels (i.e. the same error is
added to all pseudorange measurements). The clock bias and bias drift are estimated in
parallel with the user's position.
6
The atmosphere is a dominant error source because it alters the signal propagation
speed. These errors are dependent on SV elevation relative to the user as the signal
travels greater distances through the atmosphere when the SV is near the horizon. The
ionosphere causes a delay in the data modulated onto the carrier while simultaneously
causing an advance of equal proportion on the phase of the carrier. This delay lengthens
the perceived range measured by the receiver. The magnitude of the ionospheric error
also depends on the user's latitude and the level of ionospheric activity. Ionospheric
activity is cyclical, where cycles correlate with a 24 hour period, seasons, and the solar
cycle. Water content in the troposphere also contributes to a delay in the signal (both in
the modulated signal and the carrier phase), which lengthens the pseudorange. Models
are available to reduce the pseudorange error caused by ionospheric and tropospheric
interference. If a dual frequency receiver is available, the ionospheric delay can be
determined by differencing the arrival times of the L1 and L2 signals [27].
Environmental errors are multipath
and shadowing. Multipath is due to the
reflection of the signal before it reaches the
user's antenna. Therefore, the signal
propagates over an unnecessarily long
distance, and the pseudorange is increased.
Multipath error can vary from centimeters
to tens of meters. Shadowing occurs when
the signal travels through an obstructing medium, such as foliage. The signal can be
significantly attenuated, potentially causing the receiver to track a stronger multipathed
Figure 1.2: The GPS antenna on the truck receives a
shadowed and multipathed signal.
7
signal. These effects are depicted in Figure 1.2 where the attenuated shadowed signal is
shown by a dashed line and the multipath signal is being reflected off a nearby building.
Ephemeris errors, which translate to an error in the projected SV position, are the
last major error source. The SV paths are continuously monitored by stations located
around the world. Ephemeris parameters are generated at these ground stations and
uploaded to the SVs to enable the end user to predict the SV position at a future time.
The parameters essentially provide a curve fit to the orbiting path of the SV. Errors
between the predicted and true path increase as the ephemeris parameters age. The SVs
broadcast their ephemeris parameters every 30 seconds, but the parameter set is only
updated by the ground stations every few hours.
A standard error budget is given in [43] and listed in Table 1.1 containing the user
equivalent range error (UERE) due to atmospheric, clock, and environmental effects on
the GPS signal. Typically, the UERE for both the code and carrier is 1% to 2% of the
signal's wavelength [37]. A C/Acode based range measurement is expected to be slightly
less accurate than a P(Y)code based range because the ionosphere has a larger influence
on the C/A code. A user's expected positional accuracy can be obtained by multiplying
the UERE by the dilution of precision (DOP), which is a unitless numerical parameter
describing the satellite geometry relative to the user.
Table 1.1
The standard GPS error budget for individual range errors and the user
equivalent range error
Error Source C/A Range Error, 1? (m) P(Y) Range Error, 1? (m)
Satellite Clock 2.1 2.1
Receiver Clock 0.5 0.5
Ionosphere 4.0 1.2
Troposphere 0.7 0.7
Multipath 1.4 1.4
Ephemeris 2.1 2.1
UERE 5.25 3.6
8
Differential GPS (DGPS) techniques were developed in the 1980s and 1990s to
take advantage of the spatial error correlation in the broadcast GPS signal. Two receivers
in close proximity will be hampered by very similar atmospheric errors, while the SV
clock and ephemeris errors will be identical. However, the multipath and shadowing
effects are usually different at two locations. Using two receivers, a user can effectively
difference out a portion of the error contained in the signal to improve position accuracy.
A satellite based augmentation system (SBAS) is one system used to determine a
DGPS solution. The GPS signal is recorded from the SVs at many well surveyed
locations, and vector clock, ephemeris, and ionosphere corrections corresponding to each
SV are computed. These corrections are uploaded to geostationary satellites, which then
transmit the data to the end user [44]. The Wide Area Augmentation System (WAAS) is
an SBAS system supported by the Federal Aviation Administration (FAA) and is publicly
available. Most receivers are capable of using the WAAS system to improve the
positioning accuracy down to 3 to 10 meters. The Department of Defense (DoD)
supports Wide Area GPS Enhancement (WAGE) SBAS system for the military. Some
private companies operate their own infrastructure for SBAS that boast better accuracies
over WAAS. For example, John Deere operates the StarFire system, and Fugro operates
the OmniSTAR system.
A ground based augmentation system (GBAS) can be set up to achieve one to
three meter positioning accuracy. A radio or cellular modem is used to broadcast scalar
corrections from a well surveyed location to the end user. The error correlation is
typically higher in a local area, therefore better accuracies can usually be achieved over a
system like WAAS [44]. A differential carrier phase based GBAS technique known as
9
Real Time Kinematic (RTK) positioning, originally developed for surveying purposes,
can achieve centimeter level accuracy [34]. These accuracies are achievable because a
carrier phase UERE is significantly smaller than a code UERE. However, complication
is increased because ambiguities in the carrier phase measurements must be precisely
determined before an RTK solution can be produced.
1.2 AUTOMATED VEHICLE FOLLOWING
Automated vehicle following is a phrase describing a system where an automated vehicle
replicates the trajectory of a designated lead vehicle. It is alternatively known as robotic
following, automated convoying, or leaderfollower. The designated lead vehicle is
typically referred to as the leader, and the vehicles duplicating the leader's path are called
followers. The leader can be a human operated vehicle, but some situations might require
that a follower track the path driven by another automated vehicle.
The ability to precisely follow another vehicle with large separation distances
would have an immediate impact on ground vehicle systems operated by the military and
future automated civilian vehicle systems. Replacing the human operator of the
following vehicle with a control system can increase following accuracy, operation time,
and the overall safety to humans and the surrounding environment. A convoy of UGVs
could be controlled by a single driver, as shown
in Figure 1.3, and operational efficiency would
be improved by freeing more personnel to
handle other tasks. In high risk scenarios, such
as traveling through a mine field, safety could
Figure 1.3: A convoy of UGVs could be
controlled by a single human operator.
H u m a n
O p e r a t e d
V e h i c l e
U G V
10
be improved by having a fleet of vehicles replicate the path driven by a mine clearing
vehicle.
The United States (US) Army has put forth a concerted effort to develop and
operate UGV convoys under the Future Combat Systems (FCS) initiative. FCS was an
extensive program started in 2003 to modernize the Army. The Obama administration
ended the FCS ground vehicle modernization effort and canceled FCS in 2009, but the
leaderfollower program survived with other former FCS programs under the Brigade
Combat Team (BCT) Modernization. The leaderfollower concept has been integrated
into two large scale projects: the Robotic Convoy Experiment (RCX) and the Convoy
Active Safety Technology (CAST) program. The RCX is a series of tactical
demonstrations to test the capabilities of the BCT Autonomous Navigation System
(ANS) and leaderfollower capabilities. The effort is led by General Dynamics Robotic
Systems (GDRS) and strives to develop an integrated approach to the leaderfollower
problem [49]. The CAST program is headed by the US Army Tank Automotive
Research Development and Engineering Center (TARDEC), and a low cost, vehicle
independent kit is being developed to enable driver assisted leaderfollower capability
[53].
Multiple goals exist from the military's effort, some of which will be beneficial in
the civilian sector as well. A robotic convoy can automatically determine intervehicle
spacing to maximize efficiency or safety while traveling. A high speed convoy would
save fuel by maintaining short following distances to reduce air drag, while a convoy
traveling through a dangerous area with potential for attack could reduce risk to multiple
vehicles by increasing the following distances. A single person would have the capability
11
to move multiple vehicles in a leaderfollower system which would provide more time
for persons in the following vehicles to perform other tasks. For example, persons in the
following vehicles could rest, they could focus a greater portion of their attention on the
surrounding environment rather than driving to detect hazards, or they could plan a
mission. Fewer people are necessary to operate continuously as only a single driver
needs to be rotated. Finally, safety can be increased in situations demanding accurate
path duplication, such as driving through a mine field or over a makeshift bridge, because
the robotic follower can outperform human following ability.
1.3 PRIOR WORK
The leaderfollower method presented in this dissertation uses GPS based relative
position measurements to provide a means for the robotic following vehicle to replicate
the path of a human driven lead vehicle. Therefore, previous works from the relative
positioning field and the automated vehicle following field are independently listed in the
following subsections.
1.3.1 GPS Relative Positioning
GPS can be used to obtain relative position information with exceptional accuracy by
utilizing the carrier phase measurements. Carrier differential techniques were first
developed for static surveying purposes in the late 1970s [7,36]. The techniques were
expanded a few years later to include kinematic, or nonstatic surveys with centimeter
level accuracy [51]. This work made Real Time Kinematic (RTK) surveying possible. In
the 1990s, researchers at Delft University of Technology made significant strides in
solving the carrier integer ambiguity problem by decorrelating the cycle ambiguities to
determine a more feasible search space. Their algorithm is the Least Squares Ambiguity
12
Decorrelation Adjustment, which is more commonly known as the LAMBDA method
[10]. A detailed summary of the LAMBDA method and other related ambiguity
resolution algorithms is given in [58].
The success achieved by the FAA and Stanford University with the Local Area
Augmentation System (LAAS) demonstrated the potential for using carrier differential
techniques. LAAS is a high integrity automated aircraft landing system that utilizes data
from a static base station at an airport to mitigate measurement errors in the aircraft's
GPS receiver [13,34,45]. Continuation of the work lead to the Joint Precision Approach
and Landing System (JPALS), which is split into two categories for land and sea based
applications. Shipboard Relative GPS (SRGPS) is a landing system used for landing on
naval vessels, and Local area DGPS (LDGPS) is a landing system that can be temporarily
and quickly setup in terrestrial environments [12,16,46]. Difficulties addressed by the
JPALS system is the lack of a surveyed base station, a nonstatic base station, and the
threat of jamming.
Researchers have also applied carrier based relative positioning to formation
flight and automated aircraft refueling (AAR). Flying in formation can lower operating
costs by saving fuel due to decreased air drag, and AAR improves safety by removing
human error. A relative positioning algorithm for formation flight is described in [6].
The experimental testing was conducted using golf carts, and static accuracies for wide
lane, L1 only, and narrow lane solutions were 4.26, 3.65, and 3.71 centimeters,
respectively. An algorithm for AAR was developed with focus on solution integrity in
[30]. An ambiguity observable was created by subtracting a pseudorange in units of
13
carrier cycles from the measured carrier phase, and then passing the difference through a
moving average filter. The algorithm was validated in real time flight tests.
1.3.2 Inertial Relative Positioning
Many navigation systems couple inertial measurements with GPS measurements to
improve output rate and performance. Inertial sensors can be used to filter noise in the
GPS data and provide a solution that degrades gracefully when GPS becomes
unavailable. Benefits of inertial measurements to RTK techniques have been
demonstrated in the past. A tactical grade inertial measurement unit (IMU) was
integrated into an RTK system with a static base station to improve carrier phase
ambiguity resolution after GPS outages in [48] and [52]. Both works showed a decrease
in the average time to fix the ambiguities to their integer values when using an IMU.
Relative inertial navigation requires the IMUs on each platform to be aligned with
respect to one another before relative information can be determined. Relative inertial
measurements were combined with nondifferential GPS pseudorange measurements for
a formation flight application in [15]. The IMUs were considered to be aligned because
the vehicles were operating in formation. Researchers at QinetiQ offer a high level
discussion on coupling GPS and IMU information to produce relative information for
shipboard landing with focus on solution availability in the presence of GPS outages or
jamming and solution integrity [23,41]. Relative inertial measurements were combined
with vision based relative range measurements for aerial vehicles in [17]. A platform
relative approach was taken, where the relative information is expressed in the body
frame of one vehicle. However, the angular acceleration of one body about the other is
required for this approach. Researchers from the Illinois Institute of Technology removed
14
the need for angular acceleration measurements in [3] and [33]. Both still employ a
platform relative approach, and they use carrier differential GPS to determine high
accuracy relative position between a ship and approaching aircraft.
1.3.3 Automated Vehicle Following
The concept of an automated vehicle convoy is not new; it can be traced back to the 1939
World?s Fair [57]. Original designs were not specifically intended for automated
following as they utilized a rail or wire attached to the road to guide a vehicle and keep it
in a lane. Much work was done in the vehicle platooning area in the 1990s and early
2000s by the California PATH (Partners for Advanced Transit and Highways) program,
which has conducted successful tests on public highways. Magnets were embedded into
public roadways, and magnetometers on each vehicle provided the vehicle with
knowledge of its position in the lane [42,57]. Recently, German engineers have
developed and implemented an advanced driver assistance system to automate heavy
trucks using a similar approach. Vision systems were used to track lane markings and
keep the trucks in the lane. Successful tests were recently performed on German
highways [50].
Most current solutions for true vehicle following require vehicles to maintain line
ofsight (LOS) between one another so perception sensors can "see" the lead vehicle
[8,40,55]. Recognizable objects are on the back of the lead vehicle are tracked, and a
laser scanner [4,32,40], camera [28], or combination [8] can be used to determine relative
position and orientation from the following to lead vehicle. Control references can be
generated by treating the following vehicle as a trailer and steering the following vehicle
towards the leader [40]. The obvious limitation of perception based approaches is non
15
lineofsight (NLOS) following cannot occur as the lead vehicle's position and orientation
cannot be determined.
GPS is an additional tool that can be used for robotic following. However,
standalone positioning accuracy is not sufficient for many applications. Research by
General Motors has shown that simple position differencing cannot provide high
accuracy range and bearing information using low cost receivers. Even expensive, high
end receivers were only capable of providing one meter accurate information [29].
Therefore, GPS is typically used as a secondary sensor in robotic following algorithms.
However, carrier phase DGPS can be used to provide a reference in which an automated
vehicle can trail a lead vehicle. Feasibility of using carrier DGPS for maintaining
longitudinal spacing was shown in [61], and proof of concept for platoon control was
shown in [2].
1.4 CONTRIBUTIONS
The goal of the research described in this dissertation was to develop and implement a
path duplication routine to enable an autonomous vehicle to track the path of a man
driven lead vehicle using GPS based relative position information. Several contributions
were made during the course of the research. These contributions are subsequently
summarized.
A real time, Dynamic base Real Time Kinematic (DRTK) algorithm was
developed and implemented. The algorithm uses carrier phase DGPS techniques to
accurately and precisely estimate the relative position between two moving platforms.
Centimeter level accuracy is available when the phase biases are resolved. Versions of
16
the algorithm are being integrated into the FCS ANS software for use in the RCX and in
the CAST system. The algorithm is described in Chapter 2.
An architecture was developed to combine inertial data with the DRTK
algorithm. Two IMUs on separate vehicles were aligned in a common navigation frame
to form relative inertial measurements. These measurements were integrated with the
DRTK algorithm to aid ambiguity estimation though filtering by providing higher order
information about the relative motion. Also, a dead reckoning with the measurements
provides graceful solution degradation when GPS signals partially or completely lost. It
is shown that the dead reckoned solution can aid relative solution reacquisition for a short
period of time. The aided DRTK algorithm is described in Chapter 3.
The use of inertial and time differenced GPS carrier measurements as a
means to generate vector odometry information was assessed. Extraction of change
in position using inertial measurements, time differenced carrier measurements, and time
differenced carrier measurements aided with inertial measurements is discussed in
Chapter 4. The accuracy of the change in position in a two dimensional NorthEast plane
yielded by each measurement was determined. The time differenced carrier method was
implemented on a real time platform.
A path duplication strategy was developed and implemented to develop both
LOS and NLOS following. The technique revealed in Chapter 5 uses both spatial and
temporal relative position measurements, specifically those mentioned in Chapter 2 and
Chapter 4, to determine the position of a virtual lead vehicle. The NLOS ability allows
the lead vehicle to travel more complicated paths and at greater distances ahead of the
following vehicles. The number of scenarios in which the technology can be used is
17
expanded by removing the need for a additional infrastructure, such as an RTK base
station. An autonomous vehicle was programmed to use the approach to follow the path
of a man driven lead vehicle. Analysis reveals path following error of the LOS and
NLOS methods as a function of following distance and vehicle speed when using the
DRTK and odometry measurements.
18
Chapter 2
DRTK
2.1 INTRODUCTION
GPS can be used to obtain relative position information with exceptional accuracy by
utilizing the carrier phase measurements. Most GPS receivers use the code modulated
onto the carrier to determine the pseudorange between the receiver antenna and satellite.
However, phase measurement accuracy is significantly better than the pseudorange
accuracy. Centimeter level positioning accuracy can be achieved when using carrier
phase based DGPS techniques because the phase can be measured to within 1% to 2% of
the carrier wavelength. The L1 carrier wavelength is 19 centimeters, therefore the
resulting range measurement is accurate to the millimeter level [37]. The phase
measurement is also more robust to multipath error [25].
Using the phase measurements for positioning is difficult because carrier phase
measurements contain an unknown bias corresponding to the number of carrier cycles
between the SV and user's antenna when the receiver begins tracking. In order to use the
phase measurements to obtain higher accuracy position estimates, this integer number of
cycles between the user and satellite must be resolved. This value is commonly known as
the integer ambiguity, or N. The integer ambiguities cannot be determined with a single
19
receiver without processing many epochs of static data that can be hours long. The error
sources present in the signal due to the atmosphere, clocks, ephemeris parameters, and
multipath, prevent the bias from being easily determined.
The errors present in signals received by multiple receivers at nearby locations are
highly correlated. Figure 2.1 shows the computed position output from two static
receivers. Clearly, an underlying trend exists that is common to both solutions. This
trend is due to the high error correlation in the received signals. The error correlation is
the premise upon which SBAS and GBAS are based. DGPS solutions are formed by
broadcasting error corrections from known locations to remote users, or by broadcasting
the raw observables to the remote users.
An RTK system is a form of DGPS that utilizes the phase accuracy by
differencing out common mode errors between GPS receivers in close proximity (<20
km) to determine the relative phase ambiguities, and therefore obtain a high accuracy
position solution. Typically, a static receiver is placed at a known location with some
type of transmitter. This setup is often referred to as a base station. Observables
Figure 2.1: Error correlation in the calculated position from two static receivers 500 meters apart causes the
individual positions to drift. The separation distance was removed for clarity.
0 50 100 150 200 250 300
5
0
5
10
x 1 0
3
T i m e ( s )
E
C
E
F
X
(
m
)
R C V R 1 R C V R 2
20
measured at the base station are broadcast to a
remote GPS receiver, as shown in the schematic in
Figure 2.2. The remote receiver, which can be
dynamic, uses the base station observables to
difference out correlated error in its observables and
determine a high accuracy absolute position.
A Dynamic base RTK (DRTK) system
removes the requirement of installing a base station.
An accurate relative position vector (RPV) can be determined between multiple receivers,
but accurate global position information can no longer be computed. The DRTK
algorithm operates in principle like a typical RTK algorithm; errors correlated in time and
across space are differenced out using multiple receivers to determine the relative
position between those receivers.
The DRTK algorithm is a multistage process that can be briefly described by its
three fundamental stages. First, a combination of the pseudorange and carrier phase
measurements from multiple locations are used in a discrete, linear Kalman filter [20] to
estimate the relative ambiguities between receivers. An attempt is then made to fix the
ambiguities to integer values. Once the ambiguities are fixed, the now unambiguous
phase measurements are used in a least squares routine to estimate the baseline vector
between receivers.
Figure 2.2: An RTK base station
broadcasts its observables to a remote
GPS receiver to obtain a centimeter
accurate absolute position.
21
2.2 OBSERVABLES
2.2.1 Undifferenced Signal Models
To understand the subsequent derivation and appreciate the accuracies achieved by RTK
and DRTK algorithms, mathematical models of the code and carrier based range
measurements, as defined by [37], are given:
??? = ??? + ? ??? ???? + ? ?? + ?? + ?? + ??? (2.1)
??? = ??? + ? ??? ???? + ? ?? ??? + ?? + ?? + ??? (2.2)
The variables are defined in Table 2.1.
Table 2.1
GPS range and phase measurement variable definitions
Variable Description Unit
??? Measured range (pseudorange) from receiver A to satellite j m
??? Measured carrier signal phase from receiver A to satellite j m
? Speed of light through a vacuum: 299,792,458 m/s
??? True range from receiver A to satellite j m
? Wavelength of the carrier (L1: 0.1902; L2:
0.2442)
m
? Ionospheric delay/advancement cycles
? Tropospheric delay cycles
??? Receiver clock errors s
??? Satellite clock errors s
??? Integer number of cycles from receiver A to satellite j cycles
? Multipath effects m
? Measurement noise m
Technically, the range expressed in Equation (2.2) is the difference between the
receiver generated phase at time of reception and the transmitted phase at the time of
transmission plus the cycle ambiguity, as shown in Equation (2.3).
22
??? = ? ?? ? ??? ? ?? + ?? (2.3)
However, this notation is dropped in the following sections for simplicity.
The nondeterministic component contained in the signals is the system noise. All
other nonparametric components can be sufficiently modeled or estimated to reduce the
measurement error when necessary. The noise can be statistically quantified by
determining the accuracy of the delay lock loop (DLL) and phase lock loops (PLL) in the
receiver as a function of the carrier to noise ratio, C/N0. Both pseudorange and carrier
phase measurement accuracy degrade as the C/N0 decreases, as shown in Figure 2.3.
Standard formulas for the DLL and PLL accuracy are given in the following equations
[27,37].
??? = ?? 4?
2???
?/?0 2 1 ?? +
4?
???/?0 (2.4)
??? = ?2? ????/?
0
1 + 1?
??/?0
(2.5)
Figure 2.3: Pseudorange and carrier phase measurement accuracy can be expressed as a function of the C/N0.
25 30 35 40 45
0
20
40
P
s
e
u
d
o
r
a
n
g
e
n
o
is
e
(
m
)
C / N
0
( d B  H z )
0
0 . 0 0 5
0 . 0 1
C
a
r
r
ie
r
p
h
a
s
e
n
o
is
e
(
m
)?
?
?
?
23
Tracking loop parameters are receiver dependent, but approximations were used in this
work based on those given in [27]. Values are listed in Table 2.2.
Table 2.2
Tracking loop parameters
Parameter Description Value Unit
??? Code loop noise
bandwidth
2 Hz
??? Carrier loop noise
bandwidth
18 Hz
?/?? Carrier to noise ratio variable Hz
? Correlator spacing 0.5 chips
? Carrier wavelength L1: 0.1902
L2: 0.2442
m
?? Code chip width C/A:
293.05
m
?? Predetection integration
time
0.005 S
2.2.2 Single Differenced Signal Models
A single differenced observable is formed by subtracting time synchronized
measurements from two receivers observing the same satellites. If the receivers are in
close proximity, an assumption can be made that the atmospheric errors observed at the
two stations are the same. It is important to note that some situations, such as high
ionospheric activity or severe weather, can potentially falsify this assumption [35].
Single differences for the range measurements can be calculated using the signal models
given in Equations (2.1) and (2.2), where ? denotes a single differenced value (note
multipath effects are assumed to be negligible in the following equations).
????? = ??? ? ??? = ??? ? ??? + ????? + ???? (2.6)
????? = ??? ? ??? = ??? ? ??? + ????? + ?????? + ???? (2.7)
24
The resulting range information is the difference in ranges from each location to the
satellite. However, the relative distance between locations can be extracted with
knowledge of the system geometry.
????? = ??? ? ??? (2.8)
The matrix ? is known as the geometry matrix and contains the three dimensional unit
vectors from location A to each satellite. A row of ? corresponding to satellite ? is
represented as follows:
?? = ???
?
???
????
???
????
???
(2.9)
Note that ??? ? ??? because the receivers are very close relative to their respective
distances to the SV.
Since the single difference operation is linear, a transformation matrix, ???? , can
also be constructed to transform a vector of ? undifferenced range or phase
measurements to its single differenced counterpart. Equation (2.10) shows this
relationship, where ? denotes a pseudorange or carrier phase measurement.
? ? ???
?
? ? ???
??1
= ????? ????
? ?
?
?
? ??
? ??
?
? ??
2??1
? ? ?? = ???? ? ? ?
?
(2.10)
25
Notice the atmospheric terms have been removed, but the noise has been
increased. Momentarily assume the variances on the undifferenced measurements are
equal, and a single measurement variance is denoted by a generic variable, ?2.
?? = cov ?? ? = ???? cov ? ? ????? = ?2???? ????? = 2?2???? (2.11)
Therefore, the noise does remain uncorrelated after the single difference operation is
performed.
The noise on the single differenced observables can be statistically quantified by
determining the accuracy of the delay and phase lock loops in the receiver. Under the
assumption that the ionospheric and satellite clock errors were removed, the single
differenced measurement error variance can be calculated by summing the tracking loop
error variances from each receiver. These values can be adjusted for situational specific
operation (i.e. operating in areas known for high multipath).
Figure 2.4 shows an example of single differenced observables for a static and
dynamic platform. These observables contain baseline information, satellite motion,
receiver clock error, and noise. The pseudorange single difference observable is
absolute, and the effect of the integer ambiguity is seen as a bias on the phase observable.
Note that the difference between observables changes in the first plot. This is due to
noise and multipath effects present on the pseudorange measurements, and also
demonstrates the difficulty in determining the carrier phase bias.
26
2.2.3 Double Differenced Signal Models
The result of a single difference is a range measurement between receivers associated
with one satellite. The remaining error terms are receiver clock biases, an increased noise,
and the ambiguity on the phase measurement. Note the new ambiguity is a function of the
two original receiver to satellite ambiguities.
One single difference measurement, preferably of high quality and likely to
remain in sight, is selected as a base measurement (denoted by ?). Often this will be
Figure 2.4: A single differenced observable for a static (top) and dynamic (bottom) platform are shown.
0 100 200 300 400 500 600
 4 9
 4 8
 4 7
 4 6
 4 5
 4 4
 4 3
T i m e ( s )
S
in
g
le
D
if
f
e
r
e
n
c
e
(
m
)
C a r r i e r P s e u d o r a n g e
0 100 200 300 400 500 600 700 800
 6 0 0
 4 0 0
 2 0 0
0
200
400
600
T i m e ( s )
S
in
g
le
D
if
f
e
r
e
n
c
e
(
m
)
C a r r i e r P s e u d o r a n g e
27
referenced to the SV nearest to the zenith. From it, each of the other time synchronized
single differenced measurements are subtracted to form a double differenced
measurement, denoted by ??.
??????? = ????? ?????? = ?? ? ?? ??? + ?????? (2.12)
??????? = ????? ?????? = ?? ? ?? ??? + ???????? + ?????? (2.13)
This action removes the receiver clock bias error because it is common among all
received signals on a receiver. The double difference operation is also linear, and a linear
transformation matrix, ????, can be constructed to perform the operation on a vector of
single differenced observations. Assuming ? is the base satellite results in
?? ? ????
?
?? ? ????
??1?1
=
?1 1 0 0 0
0 1 ?1 0 0
? ? ?
0 1 0 0 ?1
??1??
? ? ??
?
? ? ???
?
? ? ???
??1
?? ? ?? = ????
? ? ??
?
? ? ???
?
? ? ???
= ? ? ?? (2.14)
An analysis similar to Equation (2.11) can be performed to determine the impact
of the double difference operation on the noise.
??? = cov ??? ? = ????cov ?? ? ????? = ????2?2?????????
= 2?2?????????
(2.15)
The result of Equation (2.15) is the undifferenced variance, ?2, multiplied by a matrix
with four in the diagonal elements and two in the remaining elements. Therefore, the
noise on each double difference observable is correlated with the noise on all double
28
difference observables formed. The correlation is due to the difference of the common
base satellite from all other single difference observables.
The covariance matrix associated with the double differenced observables can be
constructed using the thermal DLL and PLL variances from Equations (2.4) and (2.5).
The single differenced covariance matrix is constructed by summing the variances from
the receivers with respect to each SV, and placing the sums in the diagonal elements of a
matrix. The double differenced covariance matrix can be determined using ????.
??? = ??????????? (2.16)
Double differenced pseudorange and carrier phase observables are shown in
Figure 2.5. The integer ambiguity is clearly present in the dynamic example. The
ambiguity was removed in the static example to highlight the superior measurement
accuracy of the phase observable over the pseudorange observable. Also, notice the
effects due to clock error are gone. The remaining information contained in the signal is
the baseline, satellite motion, and noise. The slope in the plot of static double differenced
observables is due to SV motion.
29
2.2.4 Other Notable Observables
Linear combinations of L1 and L2 observables can be created to alter the observable
wavelength and accuracy. A longer wavelength reduces the search space for the
ambiguity resolution routines as a lower number of carrier cycles is needed to define a
distance. Alternatively, a shorter wavelength provides an increase in the phase
measurement precision. An observable with a longer wavelength, or widelane observable,
is created by subtracting the L2 observable from the L1 observable.
Figure 2.5: Double differenced observables for a static (top) and dynamic (bottom) platform are shown.
0 100 200 300 400 500 600
59
60
61
62
63
64
T i m e ( s )
D
o
u
b
le
D
if
f
e
r
e
n
c
e
(
m
)
C a r r i e r P s e u d o r a n g e
0 100 200 300 400 500 600 700 800
 1 0 0 0
 5 0 0
0
500
1000
T i m e ( s )
D
o
u
b
le
D
if
f
e
r
e
n
c
e
(
m
)
C a r r i e r P s e u d o r a n g e
30
? ?? = ? ?1?
?1
? ? ?2?
?2
??? (2.17)
The widelane wavelength is 86.19 centimeters and significantly longer than the L1 (19.03
centimeters) and L2 (24.42 centimeters) wavelengths. A narrowlane observable with a
shorter wavelength is created by adding the L1 and L2 observables.
? ?? = ? ?1?
?1
+ ? ?2?
?2
??? (2.18)
The narrowlane observable has a shorter wavelength at 10.7 centimeters. Noise is
increased with both new observables, but the benefit to ambiguity resolution in the case
of widelaning and the increase in accuracy in the case of narrowlaning is greater than the
drawback of noise amplification. Figure 2.6 shows the relative difference in wavelength
among the L1, L2, widelane, and narrowlane observables.
Figure 2.6: The widelane observable has a much longer wavelength than the L1 and L2 observables, which
benefits ambiguity resolution. However, the narrowlane observable has a much shorter wavelength and
increases measurement precision.
0 2 4 6 8 10
1
0
1
L1
0 2 4 6 8 10
1
0
1
L2
0 2 4 6 8 10
1
0
1
WL
0 2 4 6 8 10
1
0
1
T i m e ( n s )
NL
31
Though noisy, a widelaned ambiguity observable can also be formed by
combining a widelaned carrier observable with a narrowlaned pseudorange observable
[30]. Single and double differenced versions of the ambiguity observable can also be
created.
? ??? = 1?
??
???? ????? (2.19)
The observable must be passed through a moving average or sliding window filter before
being used because the pseudorange noise obscures the true ambiguity. Figure 2.7 shows
a single differenced ambiguity observable and its filtered counterpart.
2.3 CYCLE SLIP DETECTION
A cycle slip is a sudden change in the cycle ambiguity caused when the PLL momentarily
loses lock on the carrier signal. Loss of signal, severe multipath, or low C/N0 values can
induce cycle slips. Figure 2.8 shows an instantaneous change in the carrier phase, or a
cycle slip. Cycle slips must be detected for carrier based navigation techniques to be
reliable. Otherwise, the solution is subject to often large deviations from its true value.
Figure 2.7: The ambiguity observable must be filtered with a sliding window or moving average filter to remove
the obstructing pseudorange noise.
0 5 10 15 20
 3 0
 2 0
 1 0
0
10
T i m e ( s )
A
m
b
ig
u
it
y
O
b
se
rv
a
b
le
? N F i l t e r e d ? N
32
The approach used in this work was to monitor the change in an ambiguity from
epoch to epoch. This method is relatively simple and does not require much
computational effort. A time difference of the ambiguity is formed by subtracting a time
differenced carrier measurement from a time differenced range measurement and
dividing the total by the carrier wavelength.
??? ? = 1? ???? ? ????1? ? ???? ? ????1? (2.20)
The absolute value of ??? is monitored, and the measurement is discarded for the
current epoch when the value is beyond a threshold. Figure 2.9 shows a cycle slip
detected by this approach. The threshold has units of cycles, and the value chosen for
this work was one. Note that noise on the pseudorange measurement can cause
significant fluctuations in this observable creating a non zero probability of missed
detection and probability of false alarm. Note that a more rigorous test might be
required for some applications, but this method was sufficient for this work.
Figure 2.8: A cycle slip can be seen in the single differenced phase observable at the discontinuity around 1406
seconds.
1370 1380 1390 1400 1410 1420 1430
 1 0
0
10
T i m e ( s )
?
?
(
m
)
33
It is possible to detect and repair cycle slips. A repair involves estimating the
change in phase ambiguity due to the loss of lock and applying the estimate to the
measurement. Double differenced observables are usually necessary to better estimate
the amount of slip because the remaining receiver clock bias in the single differenced
observables can cause inaccuracies in the slip estimate. Cycle slip repair techniques
increase the number of computations in the algorithm and therefore were not developed
for this work.
2.4 SINGLE DIFFERENCED INTEGER ESTIMATION
A Kalman filter can be used to determine floating point estimates of the integer
ambiguities. This subsection describes an estimation algorithm that utilizes the single
differenced observables shown in Equations (2.6) and (2.7). Estimates of the
ambiguities can be used to determine the RPV between two dynamic receivers.
However, the full accuracy of the carrier phase measurements will not be utilized as the
Figure 2.9: Most cycle slips can be clearly seen by monitoring the change in the measured phase ambiguity. If a
cycle slip is detected, the change in ambiguity exceeds a predefined threshold and the measurement is thrown
out. A cycle slip is detected around 1406 seconds and is well beyond the threshold of one.
1370 1380 1390 1400 1410 1420 1430
0
10
20
30
40
T i m e ( s )
T
im
e
D
if
f
e
r
e
n
c
e
d
A
m
b
ig
u
it
y
(
c
y
c
)
? N
k
 ? N
k 1
T h r e s h o l d
34
estimates are not fixed to their true, integer values. Therefore, this algorithm only
produces low precision estimates of the RPV.
2.4.1 Estimator Overview
The single differenced observables can be arranged in a 2? ? 1 vector, where ? is the
number of SVs being tracked on each channel (e.g. ? = 8 when eight L1 signals are
tracked, and ? = 13 when eight L1 signals and five L2 signals are tracked). The
observable models in Equations (2.6) and (2.7) can then be rewritten in a vector/matrix
format, where ? ?? is the RPV.
????
2??1
= ? ???? ? ?????
??
4?1
+ ??????
???
????? (2.21)
where
? = ? ?? ? (2.22)
The goal of this estimator is to determine single differenced ambiguity estimates.
Therefore, the RPV, receiver clock biases, and geometry are unnecessary at this stage.
The left null space of ? can be used to uncouple the unneeded information from the
model. This technique has been previously used in [30,34]. The left null space is defined
as follows:
?? = ? (2.23)
Equation (2.21) can be rewritten with the unnecessary terms removed,
? ???? = ? ??? ?? (2.24)
which fits the ? = ?? form required by the Kalman filter. The measurement covariance
is also updated using the left null matrix.
35
? = ????? (2.25)
Equations (2.24) and (2.25) are used in the measurement update of the Kalman filter,
which is given in Equations (2.26) through (2.28).
? = ????? ?????? + ? ?1 (2.26)
? ?+ = ? ?? + ? ???? ?? (2.27)
??+ = ???? ??? (2.28)
The estimated state vector ? consists of estimates of the single differenced L1 or L1 and
L2 integer ambiguities.
? = ???? or ? = ????? ????? ? (2.29)
The time update is relatively simple; no dynamics are associated with the
ambiguity states, and the process noise is uncorrelated among the ambiguities.
Therefore, the state transition matrix is an ? ? ? identity matrix.
? = ???? (2.30)
The process noise contains small values along the diagonal to prevent the filter gain from
settling to zero. A value of 10?6?? was used for this work, where ?? is the sample rate
of the range measurements.
? = 10?6?????? (2.31)
The time update step in the Kalman filter propagates the state vector and estimated
covariance forward in time using knowledge of the system's dynamics and uncertainty
associated with the dynamics.
? ?? = ??,???? ???+ (2.32)
??? = ??,???????+ ??,???? + ? (2.33)
36
Computational savings can be realized because the state transition matrix is an identity
matrix. Equation (2.32) can be skipped, and the propagation of ????+ can be simplified as
follows:
??? = ????+ + ? (2.34)
Numerical limitations can cause the estimated covariance matrix to become non
symmetric. Problems can occur in the integerization step if ? is nonsymmetric. A
simple operation can be performed periodically to maintain the symmetrical properties of
the matrix.
? = 12 ? + ?? (2.35)
2.4.2 Initialization
The state vector can be initialized two ways. The single differenced ambiguities can be
approximated with the difference between the range and phase measurements.
? ? = ?? = 1? ????? (2.36)
Alternatively, a priori knowledge of the baseline vector between the two GPS antennas
can also be used to approximate the initial integer ambiguities.
? ? = ?? = 1? ????? ?? (2.37)
Such information could be obtained from a laser scanner, vision system, inertial
navigation system (INS), or other radio ranging system. Also, the two platforms could
start at known points. Note that a RPV must be used to determine the initial ambiguities.
Knowledge of the baseline magnitude, with no orientation, does not provide the
necessary constraints to predict the single differenced ranges. However, such
information, perhaps coming from ultrawideband ranging, sonar, or odometry, could be
37
included in the measurement update of the estimator, or used as a metric for ambiguity
validation.
The covariance is initialized with the approximate variance of the initial
ambiguity estimates. Correlation among the ambiguities is ignored initially. For this
work it was determined that a standard deviation of half a cycle (? = 0.5) produced the
best filter performance. The thermal variance from the DLL could also be used to
initialize the covariance matrix, however undesirable behavior was observed using the
thermal variance when a satellite was repeatedly coming in and out of view of the
receiver.
2.4.3 Constellation Changes
The estimated state vector and estimated covariance must be altered before the next
iteration of the Kalman filter equations in the event of a constellation change (i.e. when
the receiver begins tracking a new SV or loses lock on an existing SV). The single
differenced ambiguity estimator is naturally set up such that little complexity is needed to
handle any changes. Having a base SV, as in a double differenced ambiguity estimator,
increases the complexity as changes of the base SV requires a transformation of the
ambiguities.
Loss of an SV requires a relatively straight forward modification to the state
vector and covariance. The element in the state vector, and the row and column of the
covariance matrix, associated with the lost satellite are simply removed. This operation
is best illustrated with an example. If L1 PRNs 4, 16, and 28 are tracked at time ???1, the
estimated state and estimated covariance would consist of the following data:
38
? ??? = ?? 4 ?? 16 ?? 28 ? (2.38)
???? =
??? 42 ??4,?16??? 4??? 16 ??4,?28??? 4??? 28
??4,?16??? 4??? 16 ??? 16 2 ??16 ,?28??? 16??? 28
??4,?28??? 4??? 28 ??16 ,?28??? 16??? 28 ??? 282
(2.39)
Now suppose that L1 PRN 16 is obstructed by a building at ??. The second element of
the state vector needs to be removed, along with the second row and column of the
covariance matrix.
? ??? = ?? 4 ?? 28 ? (2.40)
???? = ??? 4
2 ?
?4,?28??? 4??? 28
??4,?28??? 4??? 28 ??? 282 (2.41)
The addition of a new SV requires the stretching of the state vector and
covariance matrix to accommodate the new data. A matrix can be created to perform the
task of adding the elements. The matrix starts out as an identity matrix with ? ?? 2
elements, where ? is the number of observations at ?? and ? ?? is the number of
observations at ???1, before data from ? new SVs was available. Rows containing zeros
in all elements are added where they correspond to the location of the new data.
Continuing with the example above, L1 PRN 16 is tracked again at the next epoch. The
? ? ? ?? transformation matrix, ????? , is constructed as follows:
????? =
1 0
0 0
0 1
(2.42)
The estimated state vector and estimated covariance matrix are multiplied by the
transformation matrix to make the appropriate number of elements to contain the new
estimates.
? ??? = ????? ? ??? = ?? 4 0 ?? 28 ? (2.43)
39
???? = ????? ????????? ? =
??? 42 0 ??4,?28??? 4??? 28
0 0 0
??4,?28??? 4??? 28 0 ??? 282
(2.44)
The new elements are then initialized using the techniques described in Section 2.4.2.
2.5 FLOATING PRECISION BASELINE ESTIMATION
The floating point ambiguity estimates can be used to determine the RPV between
antennas. Although the RPV estimate will not be as precise as one calculated using
integer ambiguities, it might be useful for some applications or to provide a solution
when the integers have not been fixed. A simple weighted least squares estimator is used
to determine the RPV with the floating point ambiguity estimates.
Double differenced ambiguity estimates must be formed from the single
differenced ambiguity estimates to determine an exact integer value [34]. This double
differenced vector is formed by choosing a "base" ambiguity and subtracting all others
from it, as shown in Equation (2.13) or using the technique described in Equation (2.14).
The measurement covariance is be formed by calculating single differenced PLL
variances for each channel and using Equation (2.16) to transform them to double
differenced variances. Weighted least squares is then used to estimated a three
dimensional RPV using the double differenced phase measurements and double
differenced floating ambiguity, as shown below:
??? = ?????????? ??? ???????????? ????? ?????? (2.45)
where
??? = ????? (2.46)
40
2.6 AMBIGUITY INTEGERIZATION
The integer value of each cycle ambiguity is required to achieve the highest
possible precision. However, simple rounding of the ambiguity estimates is not sufficient
because the high correlation among the estimates will lead to one or multiple ambiguities
being fixed to incorrect values. The LAMBDA method has been proven to provide the
highest probability of acquiring the correct set of integer ambiguities among many integer
ambiguity acquisition algorithms [10]. The floating double differenced ambiguities,
??? , and their associated covariance from the Kalman filter, ???? , are necessary to use
the LAMBDA algorithm. Residual errors from the receivers' clocks are consistent in all
ambiguities, and therefore can lead to incorrect fixes, but they are removed with the
double difference operation.
The LAMBDA method decorrelates the ambiguity estimates to produce a
minimized search space and outputs the candidate integerized solution sets, ??? ,
contained within that space [26]. A volume preserving, integer transformation matrix, ?,
is created to decorrelate the estimates [56].
? ? = ???? (2.47)
?? = ????? ?? (2.48)
The high correlation in the estimated ambiguities is indicated by large off
diagonal terms in the estimated covariance matrix. Once decorrelated, the diagonal of ??
is larger relative to the off diagonal terms. Figure 2.10 is a visual representation of the
decorrelation. Warm colors represent large relative differences between the values in the
cell and surrounding cells, and cool colors represent similar values among neighboring
41
cells. The muddled plot on the left shows the highly correlated ???? , and the plot on the
right with the distinguishable diagonal elements shows the decorrelated ??.
The decorrelated ambiguity estimates and covariance are then used in a sequential
least squares routine to determine candidate sets of integer ambiguities, denoted by ? ?.
A common method to determine the correct ambiguity set is a ratio test between the
square norm of the first two candidate sets of integer ambiguities, where the best set is
deemed correct if the ratio is above some value, ? [6]. In the equation below, a ratio of
the best set to the second best set is calculated.
??? ? ???? ???? ?? ??? ? ???? ?
??? ? ???? ???? ?? ??? ? ???? ?
? ? (2.49)
Note that the candidate sets were transformed back to the original domain using the
integer transformation matrix.
??? = ???? ? (2.50)
Figure 2.10: The LAMBDA method takes the estimated covariance (left) and decorrelates it (right) to create a
more favorable and efficient ambiguity search space.
C o l u m n o f P
? ?
R
o
w
o
f
P
?
?
2 4 6 8 10 12
2
4
6
8
10
12
C o l u m n o f P
Z
R
o
w
o
f
P
Z
2 4 6 8 10 12
2
4
6
8
10
12
42
The value for ? for this work was three, and it was chosen by assessing the time to fix
and number of incorrect fixes with data from a variety of operating environments.
Candidates below the threshold were thrown out, while candidates above the threshold
were retained and used to determine a high precision RPV. New candidate sets can be
created periodically, where the rate is selected by the user. Higher rates require more
computation time, but this was not an issue for this work. A candidate set was generated
every epoch.
2.7 HIGH PRECISION RPV ESTIMATION
A least squares procedure is used to determine a precise RPV once the integer
ambiguities have been correctly determined. The procedure is the same as the one listed
in Section 2.5 with the exception that the integer ambiguities are used in lieu of the
floating point ambiguity estimates. The measurement vector consists of only the double
differenced carrier measurements with the integer ambiguity removed.
??? = ?????????? ??? ???????????? ????? ?????? (2.51)
Care must be taken to ensure the correct integer ambiguities are paired with the
double differenced phase measurements. Mismatches occur when the integer set is not
updated (due to failing a validation test or running the integerization routine at a lower
rate) and the base satellite changes. However, the integer set can easily be transformed to
reflect the base satellite change by subtracting the integer associated with the new base
satellite from the integer ambiguity vector. For instance, if ? is the new base satellite, the
following operation would transform the ambiguities:
??? = ??? ? ??? ? (2.52)
43
In scalar form, where ? denotes the old base satellite and ? is an arbitrary, nonbase
satellite, the above equation can be written as follows:
????? = ????? ? ????? (2.53)
This operation comes from the linear combination of single and double differenced
measurements. The relationship can be clearly seen by isolating the double differenced
operation to just the ambiguities.
????? = ??? ? ???
= ??? ???? + ??? ????
= ??? ???? + ??? ????
????? = ????? + ????? (2.54)
Noting that
????? = ?????? (2.55)
Equation (2.53) can be realized.
2.8 EXPERIMENTAL RESULTS
Two NovAtel PropakV3 dual frequency receivers were mounted in two vehicles. A
Septentrio PolaRx2e dual frequency receiver and radio modem installed at the base
station provided corrections to the NovAtel receivers. The receivers calculated an RTK
position, which was used only for ground truth. The RANGECMP and GPSEPHEM
messages from the ProPakV3 were logged at 5 Hz and processed in the DRTK algorithm
to determine the RPV between vehicles. The DRTK RPV was compared to the
difference in RTK positions of the two vehicles in the Earth Centered, Earth Fixed
(ECEF) frame. Note that the error between the DRTK RPV and the computed RTK RPV
44
will be small when the ambiguities are fixed to integer values because residual error in
the separate RPV solutions will be correlated. Therefore, the following error analysis
validates the DRTK algorithm by exposing the similarity in the solutions.
Static tests were performed to assess accuracy as a function of baseline and
number of observations. Figure 2.11 shows the total magnitude of the error as the
separation distance between receivers increases from 8 meters to 604 meters. The red
dots mark the one sigma bounds centered about the mean. Similarly, Figure 2.12 shows
the baseline error as the number of satellites used to compute a solution ranges from five
to ten. Error for the two tests consistently remained at a subcentimeter level. Specific
values of the mean error and standard deviation pertaining to the difference between RTK
and DRTK solutions for the respective tests are given in Table 2.3 and Table 2.4. Table
2.5 contains the error on the RTK and DRTK solutions after their respective means were
removed.
Figure 2.11: Baseline error from static tests was
consistent across a range of separation distances.
Figure 2.12: The baseline error remained consistent as
the number of observed satellites changed.
45
Table 2.5:
RTK and DRTK error versus baseline.
Baseline (m) Type ?? (cm) ??(cm) ??(cm)
8.85 RTK 0.29 0.48 0.42 DRTK 0.25 0.39 0.44
111.16 RTK 0.18 0.36 0.40 DRTK 0.17 0.35 0.42
363.99 RTK 0.21 0.37 0.36 DRTK 0.23 0.30 0.34
604.05 RTK 0.30 0.67 0.36 DRTK 0.25 0.53 0.35
Dynamic tests were also performed
to assess performance of the DRTK
algorithm. The following plots show the
results from a single dynamic test where two
vehicles traveled along a path in opposite
directions for approximately fifteen minutes.
The path is shown in Figure 2.13. Individual
vehicle speeds ranged from 5 to 25 meters
per second (approximately 10 to 50 mph).
Separation distance ranged from 5 to over
500 meters, as shown in Figure 2.14.
Table 2.3
Error between RTK and DRTK solutions as a
function of baseline
Baseline (m) ? (cm) ?? (cm)
8.85 0.11 0.1
111.16 0.12 0.07
363.99 0.2 0.13
604.05 0.50 0.22
Table 2.4
Error between RTK and DRTK solutions as a
function of observations
Obs ? (cm) ?? (cm)
5 0.63 1.03
6 0.24 0.46
7 0.42 0.17
8 0.33 0.25
9 0.14 0.30
10 0.10 0.22
Figure 2.13: The vehicle paths from the dynamic
test.
 2 0 0 0 200
 4 0 0
 3 0 0
 2 0 0
 1 0 0
0
E a s t ( m )
N
o
r
th
(
m
)
C a r 1
C a r 2
46
Figure 2.15 compares the low and high precision DRTK solutions to the
differenced RTK solution using the fixed base station. The low precision solution is
shown in green, the high precision solution is shown in blue, and the black dots are the
"true" RPV computed with the RTK solutions. The low precision solution does not use
the integerized ambiguities, and the highest precision and accuracy is not realized.
However, the high precision solution does use the integer ambiguities, and it is in
agreement with the true RPV.
Figure 2.14: The baseline between the vehicles varied from 5 to over 500 meters as the vehicles traveled in
opposite directions along a path.
Figure 2.15: The DRTK low and high precision outputs in the ECEF X direction are shown. Black dots
represent the RPV obtained by differencing the RTK solutions.
0 100 200 300 400 500 600 700 800
100
200
300
400
500
T i m e ( s )
B
as
el
in
e
(m
)
0 200 400 600 800
 2 0 0
 1 0 0
0
100
200
300
T i m e ( s )
R
P
V
X
(
m
)
134 135 136
192
1 9 2 . 5
193
1 9 3 . 5
194
T i m e ( s )
R
P
V
X
(
m
)
LP
HP
T r u t h
47
Figure 2.16 is a more revealing description of the RPV error. The ECEF X, Y,
and Z components of the error are shown for both the low and high precision solution.
Again, the low precision solution is shown in green, and the high precision solution is
shown in blue. Error statistics of the data in the plot are given in Table 2.6, where the
similarity between the high precision DRTK solution and the RTK based RPV is seen in
the millimeter level standard deviation.
Table 2.6
DRTK error statistics for the dynamic test
LP HP
? (cm) ?? (cm) ? (cm) ?? (cm)
X 21.49 9.22 0.09 0.31
Y 28.84 21.21 0.07 0.92
Z 6.88 6.98 0.18 0.47
Figure 2.16: Low and high precision RPV are decimeter and centimeter level accurate, respectively.
0 100 200 300 400 500 600 700 800
 0 . 6
 0 . 4
 0 . 2
0
T i m e ( s )
R
P
V
X
E
r
r
o
r
(
m
)
0 100 200 300 400 500 600 700 800
1
0
1
T i m e ( s )
R
P
V
Y
E
r
r
o
r
(
m
)
LP HP
0 100 200 300 400 500 600 700 800
 0 . 5
0
0 . 5
T i m e ( s )
R
P
V
Z
E
r
r
o
r
(
m
)
48
The DRTK output was translated to the East, North, Up (ENU) frame. A relative
orientation with respect to the ground plane can be determined with the ENU RPV,
therefore it is useful to note the error in this frame. Figure 2.17 shows the dynamic error
of the low and high precision solutions expressed in the EastNorth plane. The low
precision solution is shown in green and is within half a meter in both directions. Note
that the low precision solution can have meter level error due to excessive noise or
multipath on the pseudorange observables. The high precision solution is shown in blue,
with a close up view displayed on the right. Figure 2.18 shows the density of the high
precision solution error. The error is normally distributed with a standard deviation of
3.5 and 3.2 millimeters in the East and North directions, respectively.
Figure 2.17: North and east components of the low and high precision solution error are shown.
49
2.9 CONCLUSION
An algorithm to determine the relative position between two moving receivers has been
described in detail. Formulation of different observables and their incorporation into a
Kalman filter was explained. Also, methods were described to detect cycle slips and
handle constellation changes. This algorithm was implemented in real time in C++ using
the L1 and L2 observables, and versions of it are currently being integrated into the FCS
ANS software for the RCX and TARDEC's CAST system.
Error analysis showed the algorithm's high precision output is in agreement with
the differenced RTK solution. Therefore, it can be extrapolated that the DRTK accuracy
Figure 2.18: Analysis of the north and east error components shows the difference between the DRTK and RTK
solutions is millimeter level.
50
is of the same order and level of magnitude as RTK accuracy when ambiguities are fixed
to integer values, which is typically two centimeters for baselines less than 20 kilometers.
This result was anticipated as the carrier measurements are processed in a similar manner
in the DRTK and RTK algorithms when the ambiguities are fixed.
51
Chapter 3
DRTK/INS
3.1 INTRODUCTION
The prevalence and persistence of challenges presented by the ground vehicle operational
environment create difficulties when determining an accurate and consistent GPS based
navigation solution. The challenges are enhanced when attempting to simultaneously
process measurements from multiple noncolocated receivers as the local environment
around each receiver is unique. Effects such as shadowing and multipath obfuscate the
independently received signals and the resulting accuracy of the measurements, reducing
the ability to estimate the ambiguities with sufficient accuracy for integer fixing. The low
elevation of the antenna increases the likelihood and frequency of objects obscuring the
lines of sight between it and satellites. Therefore, partial outages can be common. Also,
consistency of observations is not guaranteed. A receiver might track four satellites, but it
might not track the same four from epoch to epoch. The probability of two receivers
observing the same satellites, which is necessary to obtain a DRTK solution, can be low
in such situations.
Frequent outages also reduce the feasibility of some techniques used to enhance
the accuracy of the ambiguity estimate. For instance, the JPALS algorithm as described
52
in [30] computes a geometryfree widelane observable, which is also shown in Equation
(2.19). This observable is inherently noisy, but it is very stable. It is passed through a
moving average filter before insertion into the estimation algorithm to remove the noise.
This computation results in a very stable, multipath free ambiguity measurement.
However, this is not feasible on a ground vehicle because many operational environments
create frequent outages that would reduce the effectiveness of the moving average filter.
Figure 3.1 displays the L1 observations and associated C/N0 of a single NovAtel
PropakV3 receiver on a passenger vehicle in a typical suburban environment. The
vehicle was static for the first 100 seconds, where the receiver consistently tracked nine
satellites and the C/N0 was reasonably high (between 4548 dBHz). Once the vehicle
started traveling, it traveled under overhanging trees, signs, and by buildings of up to four
Figure 3.1: The number of signal observations and signal quality can fluctuate significantly when driving in a
typical suburban environment.
0 50 100 150 200 250 300 350
0
5
10
O
b
s
e
r
v
a
t
i
o
n
s
0 50 100 150 200 250 300 350
20
30
40
50
T i m e ( s )
C
/
N
0
(
d
B

H
z
)
53
stories. The vehicle came to rest between several two story buildings. The number of
observations fluctuated between three and ten satellites as the vehicle traveled the route.
Also, the reported signal quality for many of the satellites was quite poor, with
only three observations having a C/N0 above 40 dBHz at the end of the run. Large
pseudorange errors can arise in such situations. A result is a reduction in the accuracy of
ambiguity estimates which decreases the probability of correctly fixing the integer phase
ambiguities.
The following sections of this chapter discuss an integration architecture that
combines inertial measurements from two platforms with a DRTK algorithm to overcome
some of the availability and signal quality problems, especially those observed on ground
vehicles. GPS/INS integration with a complementary filter is outlined, then the
DRTK/INS algorithm is presented with discussion on reinitialization benefits provided
by and the dead reckoning performance of relative inertial measurements.
3.2 INS INTEGRATION OPTIONS
A natural addition to aid the fluctuations in the GPS range measurements are inertial
measurements. GPS and inertial systems are often fused together to produce a high rate,
smooth navigation solution capable of dead reckoning through short GPS outages. The
same idea can be employed to combat the environmental impact on relative GPS
measurements; relative inertial measurements can dead reckon through intermittent
outages and smooth jumps in the RPV solution created by frequently changing
observations. The GPS and INS data can be fused using several methods, so
consideration to multiple methods was given to determine the most efficient integration
routine.
54
Combining the raw inertial data with the GPS relative range data poses a problem
because inertial data from each vehicle will be noncoincident. The data must be aligned
before it can be differenced to produce relative inertial data. Furthermore, the relative
inertial data must be expressed in the navigation frame before it can be integrated to
produce relative velocity and position information. Preprocessing the inertial data on
each vehicle can calibrate and rotate the inertial data into a navigation frame common
among all vehicles.
Three integration strategies are
defined in [44]: uncoupled, loosely coupled,
and tightly coupled integration. Uncoupled
and loosely coupled schemes fuse inertial
data with position and velocity data
computed by the receiver to form a
navigation solution. A block diagram of an
uncoupled system is shown in Figure 3.2
These two techniques are relatively simple to implement. The difference between the two
methods is loose coupling provides feedback into the INS to correct errors. Loose
integration can also feedback the solution into the receiver to aid its tracking loops. The
inherent drawback to these methods is the rejection of information when the GPS
receiver is incapable of producing a solution. Often, the receiver is still tracking some
satellites, but range information is not utilized because the estimator uses only the
calculated position solution from the receiver.
Figure 3.2: An uncoupled system blends inertial
measurements with the navigation output
computed by the GPS receiver.
G P S
R e c e i v e r
R a n g i n g
P r o c e s s o r
I M U
M e c h a n i z a t i o n
G P S / I N S
N a v i g a t i o n
F i l t e r
G P S
N a v i g a t i o n
F i l t e r
f
i b
b
, ?
i b
b
f
i b
n
, ?
i b
n
C o r r e c t i o n s
X
G P S
n
, V
G P S
n
X
K F
n
, V
K F
n
? , ? , f
D
55
A tightly coupled system processes the inertial data with range measurements to
each satellite and aids the receiver tracking loops with the position solution. A derivative
of this fusion method known as a closely coupled system was implemented. A closely
coupled system does not aid the tracking loops but still processes the inertial and range
data to produce a navigation solution.
Figure 3.3 is a block diagram
depicting a closely/tightly coupled system.
The IMU is mechanized and produces
measurements in a navigation frame before
being combined with GPS data. The dashed
line represents feedback to the receiver
tracking loops, which was not implemented
in this work. The closely coupled routine was
chosen to preprocess the inertial data on each vehicle prior to forming any relative
measurements. The measurements are placed in a common navigation frame, which was
chosen to be the Earth centered, Earth fixed (ECEF) frame, in the mechanization process.
An added benefit is the availability of a high rate, robust position solution to each vehicle
and its subsystems.
Once the relative inertial data is obtained, one must decide how to best couple it
with the relative range measurements to aid the relative position solution. The same two
basic options are presented: loose or close coupling. A loose coupling algorithm would
combine the output of the DRTK algorithm with the relative inertial data to provide some
filtering and a high rate RPV. However, this approach does not contribute to the integrity
Figure 3.3: Close and tight integrations blend
inertial measurements with the range
measurements from a GPS receiver.
G P S
R e c e i v e r
R a n g i n g
P r o c e s s o r
I M U
M e c h a n i z a t i o n
G P S / I N S
N a v i g a t i o n
F i l t e r
G P S
N a v i g a t i o n
F i l t e r
f
i b
b
, ?
i b
b
f
i b
n
, ?
i b
n
C o r r e c t i o n s
X
G P S
n
, V
G P S
n
X
K F
n
, V
K F
n
? , ? , f
D
56
of the ambiguity estimates as only the corrupted range measurements are available in the
DRTK algorithm. A close coupling approach provides more information about the
motion of the two vehicles, so emphasis on potentially degraded relative range
measurements can be lowered. This has a direct impact on the ambiguity estimates. Also,
the close coupling approach can offer a graceful solution degradation in the event of a
severe or full outage.
3.3 GPS/INS INTEGRATION
The algorithm in this section will utilize all outputs from an IMU (three accelerations and
three rotational rates) and combine them with GPS measurements. The complementary
filtering approach shown in Figure 3.4 will be used. First, IMU measurements will be
mechanized to form an INS position, velocity, and attitude solution. The INS solution
will be used to form predicted GPS measurements. The predicted measurements will be
compared with GPS measurements, and the result will be used in an error state Kalman
filter. The Kalman filter estimates INS error and IMU biases, and also GPS error states
in the closely coupled implementation.
Figure 3.4: A complementary filter estimates position, velocity, and attitude error to correct the INS.
M e a s u r e m e n t
P r e d i c t i on
M e c ha ni z a t i on
E r r or S t a t e
K a l m a n F i l t e r
G P S
I N S
I M U
?
? b
bf
f
?
?
?bb f ,
?,f
+

z
z?
z?
+
PVAPVA ??
PVA?
PVA
+

57
3.3.1 IMU Mechanization
A strap down IMU must be mechanized before integration with the GPS measurements.
The IMU's acceleration and turning rate measurements will be in the IMU frame, which
will be assumed to be perfectly aligned with the platform body frame. The navigation
frame and inertial/body frame are usually noncoincident, therefore the inertial
measurements must be transformed from the frame of measurement to the navigation
frame. An Euler angle based approach is presented, and while fairly intuitive, caution
must be taken because a singularity exists when the pitch angle is 90 degrees. If this
presents a problem, an alternative method, such as a quaternion based approach, should
be sought.
The algorithm presented in this section uses the ECEF frame as the navigation
frame. Therefore, the Euler angles (?, ?,?) correspond to the three rotations necessary
to rotate inertial measurements from the IMU frame to the ECEF frame. The inertial
frame is assumed to be coincident with the body frame, so the body (?) to ECEF (?)
frame angles can be expressed as ???, ???, and ???. The notational style to express data
with respect to different frames follows that defined in [22]. A generic variable can be
expressed as
???? (3.1)
where the vector ? is the kinematic relationship from the ?frame to the ?frame
expressed in the ?frame.
A coordinate transformation matrix is constructed in order to express the IMU
accelerometer measurements in the ECEF frame. It consists of operations on the sine and
58
cosine of the angles. The notation ?(?) and ?(?) denote the sine and cosine of an angle,
respectively.
??? =
?? ?? ??? ?? + ?? ?? ?? ?? ?? + ?? ?? ??
?? ?? ?? ?? + ?? ?? ?? ??? ?? + ?? ?? ??
??? ?? ?? ?? ??
(3.2)
Knowledge of the vehicle?s original position and orientation is necessary to initialize this
matrix.
The accelerometer output is denoted by the vector ?, and it contains the three
measurements about the local X, Y, and Z axes.
???? = ????? ????? ????? ? (3.3)
This variable is chosen to reflect the fact that the accelerometer is technically outputting a
measurement of specific force. Forces that move the accelerometer bend or move a proof
mass, and the output is a ratio of the measured force to the proof mass. A superscript
denotes the frame in which the measurement is expressed. For instance, ??denotes the
specific force measurement expressed in the ECEF frame, and ?? denotes the specific
force measurement in the body frame. The information can also be expressed in skew
symmetric form.
???? =
0 ?????? ?????
???
?
? 0 ??
???
?
????
?
? ?
???
? 0
(3.4)
Rotational rate vectors are expressed using the vector ?. The IMU measures both
the body and Earth rotation, but these two can be separated. The body frame rotation
vector about the inertial (?) frame expressed in the body frame is defined as follows:
59
???? = ? ? ? ? (3.5)
where p, q, and r are the body roll, pitch and yaw rates. Alternatively, the rotational
information can be contained in skew symmetric form.
???? =
0 ?? ?
? 0 ??
?? ? 0
(3.6)
The Earth rotation vector about the inertial frame expressed in the ECEF frame is defined
as follows:
???? = 0 0 ?? ? (3.7)
where ?e is 0.00007292115 rad/s. A skew symmetric form of the Earth rotation can be
also be written.
???? ? = ???? =
0 ??? 0
?? 0 0
0 0 0
(3.8)
The specific force expressed in the ECEF frame is integrated to produce the
ECEF velocity and ECEF position. The measurement is corrected for gravitational,
centripetal, and Coriolis effects so only the specific force corresponding to the
translational motion of the receiver is integrated.
????? = ???????? + ?? ??????? ? 2???? ??????? (3.9)
The middle term represents the gravity model. This accounts for the gravitational
and centripetal effects. Various gravity models based on the user?s location and altitude
can be implemented. A simple model using the standard Earth gravitational parameter,
GM, where GM = 398,600,441,800,000 m3/s2, is shown below.
?? ? ??? = ? ??
???? 3
???? ????? ???? ???? (3.10)
60
Once the specific force at time k is determined in the navigation frame, the
position, velocity, and coordinate transformation matrix can be discretely propagated
using the sample rate, ??.
????? = ??????? +??????? ?? + ????? ??
2
2
(3.11)
????? = ??????? + ????? ?? (3.12)
???? = ?????? ???? + ???? ?? ? ???? ?????? ?? (3.13)
3.3.2 Inertial Error Propagation
The IMU mechanization with resulting INS output given by Equations (3.11) through
(3.13) is an open loop process. Any initial error or error within the inertial measurements
will propagate through the INS and corrupt its output. However, these errors can be
estimated and used to augment the INS to enhance performance.
Inertial sensors produce erroneous measurements due to noise, biases, scale
factors, misalignment, and nonorthogonality. Misalignment and nonorthogonal effects
are constant in most strap down INS systems, and therefore can be calibrated offline.
Scaling factors will be treated with the same manner in this work, although in reality they
can vary slightly during operation. A reduced inertial sensor error model can be
constructed where the error is driven by white noise and a drifting bias. An
accelerometer is used in the following description, but the same model is applicable to
rate gyroscopes. Error can be expressed in the body frame as the difference between the
true and measured acceleration.
???? = ? ??? + ????? ? ????? = ???? ?? ??? (3.14)
61
Sensor error is a combination of normally distributed zero mean white noise,
???~? 0, 2??? 2??2 , and a drifting bias, ???, where both are in the body frame.
????? = ??? + ??? (3.15)
The bias drift is often modeled as a first order GaussMarkov process, where the drift has
some time constant, ??, and is driven by zero mean white noise, ??? ~? 0, 2 ???
?
???2 .
???? = ???? ?? ?????? + ????? (3.16)
Values for ???, ???? , and ?? vary with sensor quality. Noise values decrease and
time constants increase as sensor quality improves. Parameters characterizing sensor
error can be identified through autocorrelation and Allan variance analysis [59].
Parameters for automotive and tactical grade IMUs were determined in [19] and are listed
in Table 3.1.
It is necessary to estimate the INS errors and the biases to enhance the INS
solution. The error propagation effect on position, velocity, and attitude output from an
INS as a function of time is derived in [22]. The result, expressed in continuous time and
where ? denotes an error quantity, is shown in the following equations:
?? ??? = ????? (3.17)
?? ??? = ????? ????? ? 2???? ????? + ?? ????? + ?????? (3.18)
Table 3.1
Stochastic parameters for inertial sensor error
Quality Accelerometer Rate Gyroscope
?? ? ?
?
??
?? (?) ??? ??? ?? ??? ?
?
?? ?? (?) ???
???
??
Automotive 1.0e2 60 1.2e2 2.4e7 100 8.7e4
Tactical 5.0e3 100 5.0e4 8.2e9 300 1.7e6
62
?? ??? = ????? ????? + ????? (3.19)
The variable ???? is the skew symmetric form of the ECEF specific force measurements
( ???? ?) contained in ???? from Equation (3.9). The variable ?? is a tensor of gravity
gradients [47].
?? = ??
???? 3
?????
2
???? 2
? 1 ????
? ???
?
?
???? 2
????? ?????
???? 2
????? ?????
???? 2
????? 2
???? 2
? 1 ????
? ???
?
?
???? 2
????? ?????
???? 2
????? ?????
???? 2
????? 2
???? 2
? 1
+ ????? 2
(3.20)
Finally, the variable ???? is a vector of body to navigation frame Euler angles, and the
error in ???? , or ????? , is the angular misalignment.
???? = ??? ??? ??? ? (3.21)
The error model must be expressed in a state space representation for use in a
Kalman filter. The errors and biases are collected in a fifteen state vector with three
dimensional position error, velocity error, and attitude error. Note the navigation states
are in the navigation frame, but the bias states are in the local body frame.
? = ????? ????? ????? ??? ??? ? (3.22)
The dynamic equations listed in Equations (3.17) through (3.19), along with continuous
time expressions of Equation (3.16) for the accelerometer and rate gyroscope biases, are
differentiated with respect to the state vector to form the dynamics matrix, ?.
63
? =
? ? ? ? ?? ?2?
??
? ??
??
? ?
?
? ?
? ? ????? ? ???
? ? ? 1?
?
? ?
? ? ? ? 1?
?
?
(3.23)
Next, the driving noise sources are collected in a single vector.
? = ??? ??? ???? ???? ? (3.24)
The process noise covariance matrix is determined by calculating ? ??? . The noise
sources are assumed to be uncorrleated, such that ? ???? = 0 for ? ? ?. Therefore, the
process noise matrix is a diagonal matrix with noise variances along the diagonal. Again,
these values vary with sensor quality. The process gain matrix is computed by
differentiating the dynamic equations with respect to the noise vector.
? =
? ? ? ??
?
? ? ? ?
? ??? ? ?
? ? ? ?
? ? ? ?
(3.25)
Finally, the continuous state space model can be formed.
? = ?? + ?? (3.26)
A discrete model was utilized for implementation. The discrete state transition
matrix, ?, can be approximated with a first order Taylor series expansion, assuming the
sample rate is sufficiently small (?? < 1 second).
? = ? + ??? (3.27)
The discrete process covariance matrix can also be approximated with the small sample
rate assumption [14].
64
? = ?? ??? ???? (3.28)
The discrete state transition matrix and process covariance are used to propagate
the estimated error state vector and the associated covariance estimate in a Kalman filter.
? ?? = ??,???? ???+ (3.29)
??? = ??,???????+ ??,???? + ? (3.30)
3.3.3 GPS Measurement Update
Available measurements are modeled as a function of the state vector plus additive white
noise, ?~?(0,??2).
?? = ? ?? + ? (3.31)
The INS system is used to form a predicted measurement, ? . This prediction is compared
to a GPS based measurement, and the difference or residual,??, is multiplied by the
Kalman gain to determine the values by which each estimate is appropriately adjusted.
??? = ?? ?? ? (3.32)
The measurement covariance is determined by taking the expectation of the square of the
measurement noise.
? = ? ??? (3.33)
This information is used in the measurement update of a Kalman filter, which updates the
estimated state vector and covariance with information contained in the GPS
measurements.
? = ????? ?????? + ? ?1 (3.34)
? ?+ = ? ?? + ??? (3.35)
??+ = ???? ??? (3.36)
65
Once the measurement update is carried out, the INS solution is updated with the
estimated error vector, and the error terms are reset to zero [22].
???? = ???? + ????? (3.37)
???? = ???? + ????? (3.38)
??? = ? + ????? ? ??? (3.39)
A loose coupling compares the INS predicted measurements to the computed
navigation solution from the GPS navigation filter. A close coupling compares INS
predicted measurements to GPS measurements of pseduorange and pseudorange rates.
Both implementations are briefly described.
Loose Integration
A loosely coupled approach is relatively straightforward, especially if the GPS receiver
outputs its measurements in the desired navigation frame. A measurement error vector is
determined by computing the difference between the GPS measurements and INS
predicted measurements. Typically, a GPS receiver is capable of providing position and
velocity measurements. Therefore, a direct relationship exists between the measurements
and INS states, so the measurement matrix is easily formed. Assuming the receiver
outputs position and velocity in the ECEF frame, the measurement matrix is written as
follows:
? = ? ? ? ? ?? ? ? ? ? (3.40)
Measurement variance is based on the quality of the receiver's output. This information
is provided by the receiver manufacturer or output with the measurements. The
measurement covariance assumes no correlation between any measurements provided by
66
the GPS receiver, which is technically not true. However, the correlation information is
not available without greater access to the internal navigation filter in a GPS receiver, so
a simplified covariance matrix was used.
? =
??
???
2 ? ?
? ??
???
2 ?
(3.41)
Close Integration
A close integration increases the complexity of the system by adding two additional
states to be estimated and requiring larger matrices in the measurement update. The
benefits to the approach are the ability to individually weight measurements from each
satellite and the availability of some information to reduce inertial error growth when less
than four GPS signals are tracked.
Range and rangerate information are used to aid the INS in a close integration
architecture. The GPS pseudorange model was given in Equation (2.1), and it contained
a multitude of error sources. However, atmospheric error models can be used to decrease
atmospheric error, and a clock model is used with parameters broadcast in the navigation
message to reduce the SV clock error. A reduced pseudorange model from receiver ? to
satellite ? containing only the true range, receiver clock error, and noise can be
constructed.
??? = ??? + ???? + ??? (3.42)
A pseudorange rate measurement is formed by using the Doppler measurement, ??, to
determine the user to satellite relative velocity. However, the receiver clock drift term is
present in the measurement.
67
? ?? = ?????? = ? ?? + ??? ? + ??? (3.43)
The estimated state vector of the closely coupled filter must be appended to
remove effects from the receiver clock error and error drift. The oscillator driving the
receiver clock will have a phase and frequency error, which produces a bias and drift in
the clock output. Two states are added to the vector to contain these states, bringing the
total number of elements to seventeen. It is numerically advantageous to estimate the
impact of the clock effects on the range distance rather than the actual clock errors,
therefore the speed of light constant, ?, is present in the estimates.
? = ????? ????? ????? ??? ??? ??? ??? ? (3.44)
The measurement model for pseudoranges is nonlinear because the measurements
contain the vector magnitude of distance and speed while the estimated states are the
vector components. However, a linearized model is commonly used for pseudorange
rates. Both use the GPS clock error estimates appended to the state vector. The two
models for ranges to satellite ? are contained in the following vector field.
? = ? ? = ????
? ????
?
? 2 + ?
???
? ????
?
? 2 + ?
???
? ????
?
? 2 + ???
?? ????? ?????? ????? ?????? ????? ?????? ? + ???
(3.45)
Note the slight change in nomenclature from Chapter 2. The satellite is treated as an
additional body ? and appropriately referenced in the subscript. The measurement matrix is
as follows:
? = ? ? ? ? ? ? ?? ? ? ? ? ? ? (3.46)
The variable ? contains the unit vectors to each satellite as indicated by Equation Error!
eference source not found.Error! Reference source not found.. Technically, coupling
68
between the position and velocity states appears in the first column of the second row of ?,
but the terms are small and can be safely neglected.
Measurement accuracy can be determined by computing the thermal variance in
the tracking loops. The DLL variance is computed using Equation (2.4). The frequency
lock loop (FLL) tracks the Doppler shift, and its variance is representative of the noise in
the pseudorange rate signal.
??? = ?2??
?
4????/?
0
1 + 1?
??/?0
+ ??3
(3.47)
Values for the variables in the above equation are listed in Table 2.2. The variable ?? is
the dynamic stress error; a value of three meters per second was used in this work. The
measurement covariance matrix is a diagonal matrix containing the pseudorange and
pseudorange rate variances.
? = ??
2? ?
? ?? 2?
(3.48)
Finally, the state transition and process covariance matrices must be modified to
accommodate the additional states. The INS error states and SV clock error states are
independent in the time propagation step, therefore they can be partitioned.
? = ???? ?? ?
???
? = ???? ?? ?
???
(3.49)
Equation (3.27) defines ???? and Equation (3.28) defines ????. The clock noise model
was derived in[1,43]. It assumes both the bias and drift exhibit a random walk over time,
and that the drift drives the bias state. The state transition matrix using the derived
model is defined as follows:
69
???? = 1 ??0 1 (3.50)
Knowledge of the clock quality is required to determine the spectral variance of the bias
and drift.
?? = 2?2??2?2 (3.51)
?? = ?02 ?2
(3.52)
Values for ?0 and ??2 can be obtained from an Allan variance plot of the clock output.
Values listed in [1] for several clocks of varying quality are listed in Table 3.2. This
work assumed a compensated crystal clock type. The discrete process noise covariance
matrix is constructed once clock model parameters have been chosen.
???? =
???? + ?? ??
3
3 ??
??2
2
?? ??
2
2 ????
(3.53)
It is worth noting that fault detection techniques, such as those described in [5],
can be used to improve the close integration solution. Also, the techniques could be used
to identify erroneous range measurements and exclude them from the DRTK algorithm.
3.3.4 Initialization
The filter must be initialized with knowledge of the platform position, velocity, and the
Euler angles going from the platform body frame to the ECEF frame. Initial knowledge
Table 3.2
Power spectral density coefficients for various clocks
Clock Type ?? ???
Compensated Crystal 2 x 1019 2 x 1020
Ovenized Crystal 8 x 1020 4 x 1023
Rubidium 2 x 1020 4 x 1029
70
can be obtained by starting at a known point facing a known direction, or by processing
GPS data to determine position and velocity and infer orientation. Alternatively, an IMU
can be used to establish initial orientation by executing a static alignment procedure,
where the gravity vector and Earth's rotation are measured over a period of time.
However, this procedure requires a tactical grade or higher IMU ($20,000+ USD), and
therefore is not feasible for many applications.
Initial orientation data is usually observed in the body frame. These angles, ???? ,
???? , and ???? , respectively, define the body roll and pitch with respect to flat ground and
yaw (heading) with respect to north and follow SAE J670e to define positive rotation.
This convention defines ? forward, ? to the right, and ? down for ground vehicles [21].
Body roll and pitch angles can typically be assumed to be zero for initialization purposes
on ground vehicle platforms. The body to ENU frame rotation matrix is a simple series
of rotations to align the body and ENU frames. First, a series of functions defining
rotation matrices about arbitrary ?, ?, and ? axes are described to simplify the notation,
where the input argument (?) is a rotation angle.
??(?) =
0 0 0
0 ?(?) ?(?)
0 ??(?) ?(?)
(3.54)
??(?) =
?(?) 0 ??(?)
0 0 0
?(?) 0 ?(?)
(3.55)
??(?) =
?(?) ??(?) 0
?(?) ?(?) 0
0 0 0
(3.56)
The body to ENU transformation is defined as follows:
??? = ?? ???? ?? ???2 ?? ? (3.57)
71
The ECEF to ENU coordinate transformation is defined with knowledge of the vehicle's
latitude (????) and longitude (????).
??? = ?? 90 ????? ? 180? ?? 90 + ???? ? 180? (3.58)
Noting that ??? = ????, the rotation matrix can be initialized.
??? = ?????? (3.59)
3.3.5 Close Integration Solution
Data was collected on a vehicle traveling through a typical suburban environment using
an automotive grade IMU and a GPS receiver. GPS data was logged at 5 Hz, and IMU
data was logged at 50 Hz. Figure 3.5 shows the vehicle position plotted in an ENU frame
of reference. The 50 Hz position estimate from the closely coupled system is superior to
the 5 Hz GPS only solution as jumps in the position are eliminated as the vehicle travels
between three story buildings.
72
A high rate, three dimensional velocity estimate is also provided by the closely
coupled system. Figure 3.6 depicts the vehicle velocity in the ECEF frame. Figure 3.7
shows the Euler angles from the body frame to the ECEF frame as estimated by the
closely coupled algorithm.
Figure 3.5: The closely coupled position solution filters out erroneous GPS measurements in a suburban
environment.
1100 1150 1200 1250 1300 1350 1400 1450 1500 1550
50
100
150
200
250
300
E a s t ( m )
N
o
r
t
h
(
m
)
G P S / I N S
G P S
73
3.4 DRTK/INS INTEGRATION
A block diagram of the DRTK/INS
architecture is shown in Figure 3.8. Each
vehicle produces its own navigation solution
with the GPS and inertial measurements. The
lead vehicle then transmits its specific force
measurement with any available range
measurements to other vehicles, where the
RPV is then computed. A double differenced
DRTK algorithm is used to remove effects
from receiver clock errors in the relative
range measurements. Also, the relative state
estimates are left in the estimated state
Figure 3.6: The closely coupled integration using a full
IMU produces 3D velocity estimates.
Figure 3.7: Euler angle estimates from the closely
coupled system are used to translate motion from a
platform to navigation frame.
Figure 3.8: The DRTK/INS architecture processes
range and inertial data on individual vehicles to
align the inertial measurements.
300 320 340
 1 0
5
0
5
10
15
T i m e ( s )
S
p
e
e
d
(
m
/s
)
X Y Z
300 320 340
 2 0 0
 1 0 0
0
100
200
300
T i m e ( s )
E
u
le
r
A
n
g
le
s
(
d
e
g
)
? ? ?
G P S I M U
C l o s e l y C o u p l e d
I n t e g r a t i o n F i l t e r
D R T K
N a v i g a t i o n
S o l u t i o n
?
?
f
D
f
i b
b
, ?
i b
b
f
i b
e
X
e
, V
e
S D
I n e r t i a l s
D D
R a n g e s
G P S I M U
C l o s e l y C o u p l e d
I n t e g r a t i o n F i l t e r
N a v i g a t i o n
S o l u t i o n
?
?
f
D
f
i b
b
, ?
i b
b
f
i b
e
X
e
, V
e
? X
e
? V
e
L e a d
V e h i c l e
F o l l o w i n g
V e h i c l e
74
vector, therefore the left null space of the geometry matrix is not necessary.
3.4.1 Estimator Overview
The closely coupled system on board each vehicle processes pseudorange and pseudo
range rate data with inertial data to produce a position solution. The specific force
expressed in the navigation frame is a byproduct of the coupling routine, and it can be
combined with specific force measurements from other locations if they are in the same
frame. The result is the relative acceleration between multiple points in the navigation
frame, or the second derivative of the RPV with respect to time (derived in Appendix A).
? ???? = ???? ? ???? (3.60)
The estimated state vector contains the relative states and double differenced
phase ambiguity estimates.
? = ????? ????? ??? ? (3.61)
The equations of motion for the relative states are the integration of the relative
acceleration.
????? ? = ????? ??? + ?????? ?? + ????? ? ??
2
2
(3.62)
????? ? = ????? ??? + ????? ??? (3.63)
The state transition matrix and input matrix are constructed to discretely propagate the
states.
? =
? ??? ?
? ? ?
? ? ?
(3.64)
? = ??
2
2 ? ??? ?
?
(3.65)
75
? ? = ?? ??? + ??????? (3.66)
The process noise covariance matrix was hand tuned. Values corresponding to the
ambiguity states were those used in Equation (2.31). Estimated covariance of the states
was propagated as follows:
??? = ?????+ ?? + ? (3.67)
An alternative implementation could utilize the difference between ECEF
velocities from the individual navigation systems (i.e. ?????? = ????? ?????? ). This
would remove the relative velocity state from the estimated state vector.
? = ????? ??? ? (3.68)
The equations of motion would be first order, integrating only the relative velocity.
????? ? = ????? ??? + ?????? ?? (3.69)
The state transition matrix would be an identity matrix, and the input matrix would lose
the first row. Process noise on the relative position states would be determined by
summing the individual estimated covariances associated with the estimated velocities.
The measurement update occurs at a reduced interval when GPS range data is
available. Double differenced pseudorange and carrier phase combinations are used in
order to remove receiver clock effects.
? = ??? ??? ? (3.70)
The measurement matrix contains the differenced geometry matrix from Equation
(2.46) in columns corresponding to the relative position states and the carrier
wavelengths in the elements relating the phase ambiguity to their phase measurement.
? = ??? ? ??
?? ? ??
(3.71)
76
A single differenced measurement covariance matrix is constructed using the DLL and
PLL thermal noise variance, and then it is transformed to a double differenced covariance
matrix with ????, as in Equation (2.16). The Kalman filter measurement update sequence
is executed once ?, ?, and ? are computed.
? = ????? ?????? + ? ?1 (3.72)
? ?+ = ? ?? + ? ???? ?? (3.73)
??+ = ???? ??? (3.74)
The estimated ambiguities are fixed to integer values at this point in the algorithm
using the same approach described in Section 2.6. However, the focus of this chapter
remains on the improvements to the floating solution by providing the ability to
accurately reinitialize after an outage. Future work should assess the contribution, if any,
to ambiguity integerization.
3.4.2 Initialization
The DRTK/INS estimator is initialized in the same manner as presented in Section 2.4.2.
However, reinitialization can be handled differently because the inertial data provides a
means of dead reckoning through partial or full outages. The time necessary to determine
a high precision solution can be reduced if the RPV is reinitialized with sufficient
accuracy.
Figure 3.9 shows the mean time to first fix (TTFF) of the DRTK algorithm for a
range of initialization error across four different baselines. The TTFF is defined as the
time necessary to fix the ambiguities to integer values. Although variability exists due to
the dependence on other factors such as signal quality and satellite geometry, the general
trend is a growing mean TTFF as the error on the initial RPV increases.
77
Figure 3.10 shows the number of fixes used to calculate the mean TTFF for each
error level. The decreasing trend is due to fewer fixes because of the higher TTFF. The
bump in the curves is caused by running the Monte Carlo simulation more times with the
higher error standard deviation levels.
Figure 3.11 is a close up of Figure 3.9 for small initialization errors. The TTFF is
near instant (<2 seconds) over all baselines tested for initialization errors equal to or less
than the GPS carrier wavelength. Therefore, one should expect fast initialization times if
the RPV can be sufficiently defined after a partial or total outage.
Figure 3.9: The time to first fix increases with the uncertainty of the initial baseline.
0 0 . 5 1 1 . 5 2 2 . 5 3
0
20
40
60
80
100
3 D I n i t i a l i z a t i o n E r r o r S T D ( m )
L
1
/L
2
M
e
a
n
T
T
F
F
(
s
)
b = 8 . 2 5 m
b = 1 1 1 m
b = 3 6 4 m
b = 6 0 3 m
78
A Monte Carlo simulation was run to determine relative inertial error growth
when using both automotive grade and tactical grade IMUs. The total error magnitude is
plotted for a five minute simulation in Figure 3.12. Each INS was simulated using the
stochastic inertial error parameters listed in Table 3.1 with the nonlinear propagation
given by Equation (3.9) through Equation (3.13). The two platforms were moving in a
straight line traveling 10 meters per second.
Figure 3.13 and Figure 3.14 show the automotive and tactical error growth for the
period of time it takes each INS to reach twenty centimeters. The tactical units vastly
outperforms the automotive units, but this is expected given the cost difference between
the two types of units. The relative error reaches twenty centimeters in 0.45 and 14.5
seconds for the automotive and tactical INS, respectively. Although this error plot is for
one specific motion profile, the quick error growth exhibited by the automotive units
indicates that units of similar quality will only provide a minimal increase in system
robustness during outages.
Figure 3.10: The number of ambiguity fixes used to
determine the mean TTFF for each initial error.
Figure 3.11: The TTFF is near instant if the RPV
error is within a GPS carrier wavelength or less.
0 1 2 3
10
2
10
3
10
4
10
5
10
6
3 D I n i t i a l i z a t i o n E r r o r S T D ( m )
N
u
m
b
e
r
o
f
L
1
/L
2
F
ix
e
s
0 0 . 1 0 . 2
0
0 . 5
1
1 . 5
2
3 D I i t i a l i z a t i o n E r r o r S T D ( m )
L
1
/L
2
M
e
a
n
T
T
F
F
(
s
)
79
3.4.3 Constellation Changes
A preliminary step that checks and accounts for a change in the base satellite is necessary
before techniques described in Section 2.4.3 can be used. A transformation must be
applied to the estimated state vector and covariance matrix when the base satellite
Figure 3.12: The relative inertial error growth using tactical and automotive grade INS for two ground vehicles
traveling straight at 10 meters per second.
Figure 3.13: The relative error reaches 20 centimeters
in 0.45 seconds with the automotive INS.
Figure 3.14: The relative error reaches 20 centimeters
in 14.5 seconds with the automotive INS.
0 50 100 150 200 250
10
5
10
0
10
5
T i m e ( s )
3
D
R
e
la
ti
v
e
I
n
e
r
ti
a
l
E
r
r
o
r
G
r
o
w
th
S
T
D
(
m
)
A u t o m o t i v e G r a d e I M U
T a c t i c a l G r a d e I M U
0 . 1 0 . 2 0 . 3 0 . 4
0
0 . 0 5
0 . 1
0 . 1 5
0 . 2
T i m e ( s )
3
D
R
e
la
ti
v
e
I
n
e
r
ti
a
l
E
r
r
o
r
G
r
o
w
th
1
?
(
m
)
A u t o m o t i v e
T a c t i c a l
2 4 6 8 10 12 14
0
0 . 0 5
0 . 1
0 . 1 5
0 . 2
T i m e ( s )
3
D
R
e
la
ti
v
e
I
n
e
r
ti
a
l
E
r
r
o
r
G
r
o
w
th
1
?
(
m
)
A u t o m o t i v e
T a c t i c a l
80
changes. A transformation matrix is defined in the same vein as Equation (2.52). An
identity matrix is used in the upper right subset to maintain the relative state estimates.
The subset pertaining to the ambiguity states contains ones along the diagonal with
negative ones inserted into the column corresponding to the old base satellite.
????? =
? ? ? ?
? ? ?? ?
? ? ? ?
? ? ?? ?
(3.75)
The state vector and its covariance are transformed as follows:
? = ?????? (3.76)
? = ???????????? (3.77)
Note that the transformation matrix should be square, and the transformation should be
completed before any elements are added or removed to accommodate new or lost
satellites. Once the transformation is applied, the procedure described in Section 2.4.3 is
followed.
3.5 EXPERIMENTAL RESULTS
Two NovAtel PropakV3?s and Crossbow
IMU440?s were mounted separately in two
vehicles. A Septentrio PolaRx2e dual frequency
receiver and radio modem installed at the base
station provided corrections to the NovAtel
receivers. The range and inertial data were recorded for post processing, and the RTK
position of each vehicle was logged at 1 Hz to serve as a truth measurement. The vehicles
were driven in a convoy formation around Auburn University?s National Center for
Asphalt Technology (NCAT) 1.7 mile test track. Speeds started at 20 mph and increased
Figure 3.15: Auburn University's NCAT 1.7
mile oval test track.
 80 0  60 0  40 0  20 0 0 200
 30 0
 20 0
 10 0
0
N or t h ( m )
Ea
st
(m
)
81
in 10 mph increments to 50 mph after the completion of each lap around the course.
Figure 3.15 is a plot of the test path.
The data was processed in the DRTK/INS algorithm to produce a 50 Hz RPV.
Figure 3.16 displays the estimated and measured RPV between the vehicles as they
traveled around the track. The ECEF frame served as the navigation frame; blue is the
relative distance in X, green is the relative distance in Y, and red is the relative distance
in Z. The baseline between the vehicles is shown in Figure 3.17. The RTK positions of
the vehicles were differenced to determine a ?true? RPV between the vehicles.
Figure 3.16: Processing the inertial measurements gives the DRTK/INS solution a higher output rate over
processing only GPS measurements.
82
Figure 3.17: Vehicle separation distance fluctuated throughout the test.
Detailed versions of Figure 3.16 and Figure 3.17 are given in Figure 3.18 and
Figure 3.19. It can be seen that the RPV estimate using the floating point ambiguities
tracks the relative motion of the vehicles, indicated by the differenced RTK positions,
when GPS is available. Figure 3.19 shows a 20 second simulated GPS outage that started
at 328 seconds, which illustrates that a total denial of GPS signals leads to a graceful
degradation of the solution.
0 100 200 300 400 500 600 700 800 900 1000
6
8
10
12
14
16
18
20
T i m e ( s )
B
a
s
e
li
ne
(
m
)
R T K
D R T K / I N S
83
Figure 3.18: The DRTK/INS floating point solution
tracks the RTK solution with the same accuracy as the
DRTK solution.
Figure 3.19: The DRTK/INS solution drifts during a
simulated complete outage starting at 328 seconds;
accuracy suddenly increases when GPS is available.
Figure 3.20: The time for the error to drift 20 cm was recorded during one thousand simulated full GPS outages.
Full GPS outages were simulated at one thousand arbitrarily chosen times during
separate trials using the same data as the above plots. The time taken for the solution to
drift beyond twenty centimeters was recorded. As shown in Figure 3.20, the drift time
was inconsistent between outages, which was to be expected due to the nonlinearities in
Equations (3.9) through (3.13). The clump of data with a relatively higher drift time
330 340 350 360
 1 2
 1 0
8
6
4
2
T i m e ( s )
E
C
E
F
R
P
V
(
m
)
R T K
D R T K / I N S
330 340 350 360
1 1 . 5
12
1 2 . 5
13
1 3 . 5
14
1 4 . 5
15
T i m e ( s )
B
a
s
e
li
n
e
(
m
)
D R T K / I N S
R T K
0 200 400 600 800 1000
0
5
10
15
20
25
O u t a g e N u m b e r
T
im
e
t
o
2
0
c
m
3
D
E
r
r
o
r
84
corresponding to the first one hundred outages was measured when both vehicles were
static. The shortest time recorded was 0.64 seconds, and the longest time was 25.24
seconds. The mean time to drift twenty centimeters was 5.51 seconds, which given the
fact the Crossbow IMU440 is an automotive grade IMU, and measurements from two
IMUs were combined, is in line with the results presented in [47], where a single tactical
grade IMU was integrated with a traditional RTK system and the mean time to twenty
centimeter error drift was approximately 14 seconds. The results are better than those
predicted in Figure 3.13 because the Crossbow IMU440 is a higher quality automotive
IMU that employs DSP technology and temperature compensation to improve its output.
3.6 CONCLUSION
An architecture to combine inertial measurements from two noncoincident platforms
with a DRTK algorithm was described. Individual IMUs were aligned by a
mechanization process that placed them in a common navigation frame. Relative inertial
measurements were formed and used to propagate the relative position and velocity states
in the DRTK algorithm. A form of the DRTK algorithm using double differenced
measurements was described, including a method to alter the estimated state and
covariance when the base satellite changes.
It was shown that the DRTK algorithm's TTFF of integer ambiguities increased
with uncertainty in the initial position. INS error drift between two tactical and two
automotive grade IMUs demonstrated the reinitialization capability when using inertial
measurements to dead reckon through brief GPS outages. Experimental results using
automotive grade IMUs showed better performance than predicted in Figure 3.13. This
improvement was due to two factors: the quality of the IMU, although still automotive
85
grade, was better than the one used in the simulation, and the motion profile during the
experiments varied whereas the predicted error growth charts depict only one motion
profile.
86
Chapter 4
Odometry
4.1 INTRODUCTION
Odometry is the measure of change in platform position, or perhaps better stated as the
distance traveled from some nominal starting point. Odometers are used to measure the
distance traveled, and the most common form of odometers provide the measurement by
accumulating a number of counts which correspond to a distance interval. Encoders are
commonly used on ground vehicles to measure full or partial revolutions of a wheel or
track. Knowledge of the wheel's radius or track length is used to transform the number of
revolutions measured into a distance traveled. Similarly, pedometers are used on
ambulated platforms to measure the number of steps taken. Knowledge of the gait
distance is used to convert the counts to distance.
The two common odometers mentioned provide a measurement of distance
magnitude. A use for distance traveled in vector form has been identified for robotic
following strategies. Inertial sensors and GPS can both be used to provide this
information. An INS inherently provides vector odometry information by integrating the
accelerations and turning rates measured by an IMU. GPS also provides distance
traveled information by simply differencing current position from a nominal starting
87
position. However, processing range measurements, specifically the GPS carrier signal,
for odometry information can offer a significant error reduction.
The following chapter discusses the use of an IMU and GPS as an odometer that
provides a vector change in platform position. Three techniques will be demonstrated to
determine the information: a full and partial INS system, a time differenced carrier phase
(TDCP) routine, and an integrated solution using the INS and TDCP measurements. An
error analysis will determine and compare the systems' performance. The odometry
information will be combined with the DRTK RPV in Chapter 5 in an ENU frame, so
only two dimensional error in the EastNorth plane is of interest.
4.2 INS
The full IMU mechanization to produce an INS was presented in Section 3.3.1. The
procedure is briefly repeated here, starting with the rotation of the measured specific
force into a navigation frame, denoted by ?, and the subsequent removal of gravity,
Coriolis, and centripetal effects.
????? = ???????? + ?? ??????? ? 2???? ??????? (4.1)
?? ? ??? = ? ??
???? 3
???? ????? ???? ???? (4.2)
Next, the specific force expressed in the navigation frame is integrated to determine
position and velocity, and the measured body frame rotational rates are used to propagate
the rotation matrix.
????? = ??????? +??????? ?? + ????? ??
2
2
(4.3)
???? = ??????? + ????? ?? (4.4)
88
???? = ?????? ???? + ???? ?? ? ???? ?????? ?? (4.5)
A full INS uses all available inertial information to determine three dimensional
platform position, velocity, and orientation. Altitude information is useful on an aerial
platform. The orientation information can be useful on some ground vehicles employing
rollover prevention control or perception stabilization algorithms. However, the ground
vehicle will be constrained to the ground and body roll and pitch will typically be under
five degrees under all but the most extreme scenarios.
Altitude, roll, and pitch information can be disregarded in an attempt to improve
the INS accuracy. Rather than integrating six measurements with their individually
combined bias and noise, a subset of inertial measurements can be used to indicate basic
vehicle velocity, position, and orientation. The reduction in error sources directly leads
to reduced error at the expense of having less information about all vehicle states.
Specifically, vehicle state information is limited to speed over ground, direction of travel,
and two dimensional ground position. However, this information is sufficient for most
ground platforms intended for automation.
The navigation frame of the partial INS system will be a local tangent ENU frame
for the following derivation and denoted by ?. Longitudinal acceleration and yaw rate are
the only body frame inertial measurements necessary. With a nominal starting point,
integration reveals the platform's speed and direction of travel, or heading.
????? = ?????1? + ????
?
? ?? (4.6)
???? = ????1? + ????? ?? (4.7)
Position is determined by integrating north and east velocities, which are created using
the velocity and heading from Equations (4.6) and (4.7).
89
?????? = ?????? + ????? sin ???? ?? (4.8)
?????? = ?????? + ????? cos ???? ?? (4.9)
Note that the system can be expressed in the form of Equations (4.1) through (4.5) with
minor alteration if desired.
Two assumptions are made in the above equations. First, the body frame and
ENU frame ? axis are aligned such that
???? = ???? (4.10)
This assumption asserts that the vehicle is traveling on flat ground. Second, platform
dynamics are assumed to be benign enough that lateral movement can be ignored. All
motion is along the body frame ? axis, so any sideslip is disregarded. Any violation of
these assumptions will degrade the system's performance.
4.3 TDCP
The previous discussion in Chapter 2 on the error mitigation achieved by differencing
carrier measurements across receivers and across satellites can now be amended to
present another technique to reduce errors inherent in the GPS signal. Time differenced
carrier phase (TDCP) measurements can provide an
accurate measure of change in position. Differencing
successive phase measurements yields the total
change in phase between epochs, created by both user
and satellite motion, as seen in Figure 4.1. Similar to
differencing across receivers or satellites, differencing
across time can reduce the atmospheric and satellite
Figure 4.1: The change in receiver and
satellite position is captured in the
difference in range measurements from
the user to satellite.
90
clock errors to negligible values and remove the integer ambiguity. The underlying
stipulation is the time difference is small enough to assume all the errors are correlated.
The carrier phase measurement model in Equation (2.2) is used as a basis for the
algorithm derivation. A time differenced measurement is constructed with ? representing
the number of epochs between measurements.
???? ??????? = ??? ? + ? ???? ????? + ? ??? ???? + ?? + ???? (4.11)
? ??? ??? ?? ?????? ??????? ?? ????? ?????? + ?? ???????
The TDCP measurement, assuming the time difference is sufficiently small, is
negligibly affected by atmospheric effects, satellite clock bias, and the integer ambiguity.
Variation in the atmosphere and clock parameters usually occurs with a time constant on
the order of hours, therefore these terms are removed. Even most multipath effects can
be removed because their effect is slow compared to the measurement sample rate. The
remaining terms are a change in range, the receiver clock drift over the sampling interval,
and noise.
???,???? = ??? ? ? ??? ??? + ? ????,??? + ???,???? (4.12)
Noting that
??? ? = ??? ???? (4.13)
and
?? ? ???? (4.14)
Equation (4.12) can be rewritten as follows:
???,???? = ??? ???? ??????? + ? ????,??? + ???,???? (4.15)
91
= ??? ??? ?????? ???? ??? ?????? + ? ????,??? + ???,???? (4.16)
Finally, the change in range due to satellite motion is subtracted from the time
differenced carrier measurement.
?? = ???,???? ???? ???,???? = ??? ? ???,???? ??
??,???
+ ???,????
(4.17)
A weighted least squares routine can be used to estimate the change in position
and the clock drift. A diagonal weighting matrix can be constructed using the thermal
noise model of the PLL described in Equation (2.5). This model incorporates the carrier
to noise ratio, C/N0, so the degraded signals can be deweighted. Also, erroneous
observations due to cycle slips must be detected and removed from the measurement
update. Cycle slip detection was previously discussed in Section 2.3.
The user position can be determined relative to some reference value by
accumulating the estimates.
???,0 = ??0 + ???,???
?
?=1
(4.18)
Position error will drift over time due to the summation of the inherent noise and small
residual biases present in the estimates. The drift, which is quantified in a later section, is
small for short periods of time. The advantage to tracking position using this technique is
a more consistent and potentially more accurate position solution that is less susceptible
to jumps due to constellation changes and multipath when compared to a standalone
solution. The improved accuracy comes from the carrier phase measurement accuracy
combined with the mitigation of error sources present in the transmitted signal.
92
4.4 TDCP/INS
The TDCP observations and the inertial measurements were combined in a navigation
algorithm to determine the change in platform position. The combination of
measurements can provide a bridge between intermittent GPS outages, but the TDCP
observable poses a problem because it contains states at successive instances in time.
Successful implementations of TDCP observations with inertial aiding have been
previously demonstrated for positioning of aerial vehicles [11,54,60].
A delayed state Kalman filter was implemented to handle the fact that state
information from a previous instance in time is contained within the TDCP observable.
This is different than the previous approaches; a Taylor series expansion was used to
bypass the problem in [11,54], and an alternate measurement matrix is derived to handle
the problem in [60]. An error state approach like the one described in Section 3.3 was
used, so the INS mechanization and error state propagation remain unchanged and will
not be repeated here. The TDCP observable differenced across time ? and ? ? ? is listed
below.
???,???? = ??? ? + ????? ? ??? ??? + ??????? + ???,???? (4.19)
The TDCP observable contains range information to a GPS SV at time ? and at the
previous time ? ??. The measurement model reflects the relationship between the
TDCP observable and position state.
?? = ? ??? ?? ??????? + ?? (4.20)
The variable ? is used to denote the standard measurement matrix containing the
geometry matrix and a column of ones corresponding to the clock error. Note the
measurement matrices and state vectors used correspond to the current time, ?, and the
93
time of the last measurement update, ? ??, and not simply the previous time instance of
? ? 1.
The delayed state Kalman filter measurement update using the above model was
developed by following the procedure in [1]. First, a few properties of the state transition
matrix are noted. The state can be mapped forwards in time from time ? ? ? to ? by
successively multiplying the state transition matrices from the intermediate instances in
time.
??,??? = ??,???????,??? ?????+?,??? (4.21)
Also, the state can be mapped backwards in time by multiplying by the inverse of the
state transition matrix.
????,? = ??,????? (4.22)
Noting that the forward propagation of the state over a single time instance is denoted by
?? = ??,??????? + ???? (4.23)
the backwards propagation of the current state to time ? ?? is expressed as follows:
???? = ??,????? ?? ???,????? ???? (4.24)
The measurement model can be updated with the above result to contain only the
current state vector.
?? = ? ? ?? ?????,????? ?? + ? ?????,????? ???? + ?? (4.25)
Now the measurement matrix contains both the new and old measurement matrices and
the state transition matrix.
?? = ? ? ?? ?????,????? (4.26)
Also the measurement noise model has changed by the inclusion of process noise from a
previous instance in time. The original measurement covariance, ? , pertaining to the
94
carrier measurement is determined by computing the thermal variance in the PLL. The
original covariance is incorporated into the new measurement covariance, as shown in the
following equation.
?? = ? ? ?????,????? ???? + ?? ? ?????,????? ???? + ?? ?
= ? ?????,????? ??????,?????? ? ???? + ? (4.27)
Since the process noise appears in the measurement model, correlation exists between the
measurement noise and process noise. This correlation is expressed as follows:
?? = ? ???? ? ?????,????? ???? + ?? ? (4.28)
= ???????,?????? ? ???? (4.29)
The Kalman filter is time update iterated at a higher rate than the measurement
update. The state and its covariance are propagated through time at the measurement rate
of the IMU.
??? = ??,???????+ (4.30)
??? = ??,???????+ ??,???? + ???? (4.31)
The measurement update is iterated at the rate of received GPS measurements. The
Kalman filter gain and covariance update account for the new measurement covariance
and process and measurement noise correlation created by the delayed state term in the
observable.
?? = ?????? + ?? ???????? + ?? + ???? + ?????? ?1 (4.32)
??+ = ??? + ????? (4.33)
??+ = ?????? ??? + ????? (4.34)
95
Note that ?? is the difference between the measured observation and the predicted
observation using the INS position states.
4.5 ERROR ANALYSIS
4.5.1 INS
A tactical and automotive grade IMU were simulated using the stochastic inertial error
parameters listed in Table 3.1. Both the full INS solution given by Equation (4.1)
through Equation (4.5) and the partial INS solution given by Equations (4.6) through
(4.9) were computed. The platform was moving in a straight line traveling 10 meters per
second in the simulation, as in Section 3.4.2. Note that simulations must emulate
anticipated maneuvers to predict performance due to the nonlinearities. Figure 4.2 shows
the standard deviation of position error for the full and partial tactical and automotive
grade INS systems for the five minute simulation. Removing unnecessary inertial
measurements from the INS can drastically reduce the position error growth as seen by
the partial automotive INS outperforming the full tactical INS. However, error in the
partial INS solution may be worse than the full INS solution if the unmodeled states of
roll, pitch, lateral motion, and vertical motion are present.
96
4.5.2 TDCP
Two hundred sets of GPS range data were collected at 1 Hz, 5 Hz, and 10 Hz on a static
NovAtel PropakV3, each in five minute intervals. Data was processed in real time in
C++ with the TDCP algorithm. Figure 4.3 shows the 5 Hz and 10 Hz estimates of change
in position. Notice the 5 Hz estimates have a slightly larger variance than the 10 Hz
estimates. This is expected as the errors are time correlated.
The accumulated error of the estimates is of primary interest since the path
following algorithm sums the change in position to determine a baseline between the
following vehicle at the current time and lead vehicle at a past time. Figure 4.4 and
Figure 4.5 show the accumulated estimates versus time and interval, respectively. Since
truth is zero, the accumulated estimates correspond to the accumulated error. Although
the data in Figure 4.3 is zero mean, the error growth is dominated by biases present in the
short time intervals instead of white noise. The performance improvement when using
higher rate measurements is more evident in the accumulated measurements.
Figure 4.2: Simulated inertial error growth is reduced when using a partial IMU, provided assumptions are not
violated.
0 50 100 150 200 250
10
5
10
0
10
5
T i m e ( s )
I
n
e
r
ti
a
l
E
r
r
o
r
G
r
o
w
th
S
T
D
(
m
)
A ut om ot i ve G r a de F ul l I M U
T a c t i c a l G r a de F ul l I M U
A ut om ot i ve G r a de P a r t i a l I M U
T a c t i c a l G r a de P a r t i a l I M U
97
Quantification of the noise and residual bias effects present in the estimate are
necessary to predict error growth and determine the impact on the path following
algorithm. The onesigma bounds on the noise and bias for the change in position
estimates are given in Table 4.1, where ???? is the white noise standard deviation and
???? is the standard deviation of the residual biases. The error growth in the accumulated
Figure 4.3: Two hundred five minute intervals of the 5 Hz and 10 Hz change in position estimates are overlaid.
The 5 Hz estimates have a slightly higher variance.
Figure 4.4: The accumulation of the 5 Hz and 10 Hz
change in position estimates over five minute intervals
show that error is dominated by residual biases.
Figure 4.5: The total error accumulation of the 5 Hz
and 10 Hz change in position estimates over five
minute intervals.
5 0 5
0
50
100
150
200
? ? r
A
( m )
T
r
ia
l
5H z
10H z
98
position can be expressed as a function of the statistical quantities and the number of
accumulations, n.
????? = ?????2 + ????? 2
(4.35)
A trade off exists between measurement rate and accuracy. Measurement errors
are more correlated with higher sampling rates, therefore more of the errors will be
removed by the time difference. Slower sampling rates will yield less accurate results
because of the lower error correlation between observations. However, computational
limitations might require a slower measurement rate.
4.5.3 TDCP/INS
The TDCP/INS integration was tested using dynamic data and comparing the solution to
that of an RTK solution. The TDCP/INS algorithm output position estimates at the rate
of the IMU, which was 20 Hz, therefore the estimate after a measurement update was
compared to the RTK solution. Data from a NovAtel PropakV3 was logged at 5 Hz, and
inertial data from a Crossbow IMU440 was logged at 20 Hz. The data was processed
offline to determine a solution.
Figure 4.6 shows the computed position estimate of the TDCP/INS algorithm in
black, the RTK position solution in blue, and the standard positioning service (SPS)
solution in cyan. The green and red dots mark the beginning and end points of the test.
The vehicle drove counterclockwise through a parking lot two times before exiting and
coming to a stop.
Table 4.1: Statistical quantification of the TDCP algorithm output
Parameter 1 Hz 5 Hz 10 Hz
???? 8.378 mm 1.038 mm 0.922 mm
???? 8.006 mm 0.468 mm 0.181 mm
99
Figure 4.7 displays the odometry error of the TDCP/INS algorithm for the same
dynamic test. The top plot is the overall error, and the bottom plot is a close up to
demonstrate the magnitude of the error after the filter transients settle out. The position
estimate after each measurement update was differenced with the estimate after the
previous measurement update to determine the change in position between measurement
updates. This difference was then compared to the position difference of the RTK
solution to determine the error. The differenced RTK solution was assumed to be truth.
After the filter settles out, the output resembles that of the TDCP algorithm with no
inertial aiding, which is to be expected. Statistically, there was no discernable difference
between the outputs of the two methods after the transients had settled.
Figure 4.6: North and east position (on left, with magnified portion on right) calculated with the TDCP/INS
algorithm agrees with the RTK and SPS solutions.
 4 0  2 0 0
 4 0
 2 0
0
20
E a s t ( m )
N
o
r
th
(
m
)
R T K
SPS
T D C P / I N S
 1 0 5 0
 2 0
 1 8
 1 6
 1 4
 1 2
 1 0
8
E a s t ( m )
N
o
r
th
(
m
)
100
4.6 CONCLUSION
Techniques to determine vector components of the change in position of a platform were
explained in this chapter. This information is necessary in the approach outlined in the
next chapter to enable a UGV to replicate a path traveled by a preceding vehicle. The
total change in position is realized by accumulating the odometry information, and
therefore information about the error growth using each technique is necessary.
First, inertial measurements were used to dead reckon from an initial point. Both
a full, six degree of freedom system (three dimensional position and three dimensional
orientation) and a constrained, three degree of freedom system (two dimensional position
and orientation in one dimension) were analyzed. It is difficult to fully characterize
inertial error growth due to nonlinear coupling of the measurements. However, the error
can easily be simulated if information about the platform's motion profile is known. A
Monte Carlo simulation was used to demonstrate performance of a tactical and
automotive grade IMU with the full and partial system for a simple motion profile. The
Figure 4.7: The error in position difference from the TDCP/INS algorithm is comparable to the TDCP
algorithm once the filter settles out.
10 20 30 40 50 60 70
1
 0 . 5
0
T i m e ( s )
E
r
r
o
r
(
m
)
?
? E
?
? N
10 20 30 40 50 60 70
 0 . 0 5
0
0 . 0 5
T i m e ( s )
P
o
s
it
io
n
D
if
f
e
r
e
n
c
e
101
partial INS systems showed the best performance provided the assumptions of no roll,
pitch, lateral, and vertical motion are not violated.
Second, a time difference of the GPS carrier measurement was used to estimate a
three dimensional change in position of a platform. The accuracy of the GPS carrier
measurement translated to a millimeter level odometry estimate. It was shown that
higher sampling rates increasingly exploit the time correlated errors to improve estimate
accuracy. The algorithm was developed originally in Matlab and ported into C++ for real
time operation. All data shown was from the real time C++ version of the algorithm.
Finally, the TDCP observables were aided with a full INS. The INS and error
state approach from Section 3.3 was recycled, but a delayed state Kalman filter was used
because the measurement model did not fit the standard form of the Kalman filter. Data
from a dynamic test was pushed through the algorithm, and the output was compared to
an RTK solution. Odometry accuracies were consistent with the output of the unaided
TDCP algorithm.
102
Chapter 5
Path Duplication
5.1 INTRODUCTION
In order to duplicate a vehicle's path of travel, reference points for the following vehicles
must be generated on the fly. Perhaps the easiest method of generating reference points
is to broadcast the lead vehicle's current global position to the following vehicles. The
following vehicles' control systems would compare their respective positions to the path
traveled by the lead vehicle and issue commands to move the vehicles to the path. Such a
method is often referred to as "dropping breadcrumbs". This method, however, is not
sufficient when high accuracy path duplication is required as limitations arise due to the
accuracy of standard positioning solutions.
The one sigma accuracy of a standalone receiver is, at best, three meters. Position
error generally drifts with a bound over time when using a high quality GPS receiver.
Vehicles following in close formation should experience similar error magnitudes from
the various error sources, with the exception of multipath and shadowing, so accurate
path following could conceivably occur provided the vehicles remain close to one another
in an area with no environmental obstructions. Accurate path following would not be
possible when the vehicles are separated by long periods of time due to the error drift.
103
For example, waypoints marked by a lead vehicle driving down the middle of a lane
could be perceived by the following vehicle some period of time later as shifted in the
lane or even off the road.
Consistency issues also exist in a standalone solution as multipath, shadowing, or
constellation changes can cause discontinuities. Figure 5.1 compares standalone and
RTK solutions of a vehicle on a test track, where the GPS environment is relatively
benign. The offset from the RTK solution is due to error drift, and could be tolerable
under some operational scenarios as previously mentioned. The jumps in the standard
solution are intolerable when high accuracies are required. Aside from the obvious
problem of the vehicle driving off the intended path, the jump produces a significant step
in the reference which could lead to unstable behavior or heavy oscillation depending on
the type of following vehicle and controller tuning. Figure 5.2 shows the regularity of
such jumps even in a benign environment. Shown is the error in the change in position,
which is determined by differencing the change in position from the standard solution
with the change in position from an RTK solution. This difference demonstrates the
number and magnitude of position inconsistencies in the standalone solution.
Figure 5.1: Standard position and RTK position solutions are compared to demonstrate inconsistencies in the
standard solution that would prohibit accurate path following by dropping waypoint breadcrumbs.
 2 1 0  2 0 0  1 9 0  1 8 0  1 7 0  1 6 0  1 5 0
4
2
0
E a s t ( m )
N
o
rt
h
(
m
)
S P S R T K
104
If an RTK system is available, the breadcrumb dropping method would actually
work well as the positional accuracy would be reduced by an order of magnitude, and
most consistency issues would be removed (remaining issues would be nondetrimental
to desirable operation). However, the requirement of a fixed base station could reduce
the feasibility of autonomous trajectory duplication in many environments. Therefore,
methods that do not rely simply on broadcasting and maintaining a log of lead vehicle
position need to be developed to broaden the possible applications of automated ground
vehicle convoys.
Two following methods are presented in the subsequent subsections. A LOS
method uses the DRTK RPV to generate a reference for the following vehicle to track the
lead vehicle's path for short following distances. A NLOS method uses the DRTK RPV
in conjunction with vector odometry measurements to generate an RPV to a virtual
leader, which is the lead vehicle's position at a previous instance in time. The newly
perceived RPV is then used to generate a reference to track the leader's path, even when
Figure 5.2: The error in change in position demonstrates the frequency and magnitude of inconsistencies in the
reported standard position.
100 200 300 400 500
2
0
2
SPS
?
E
E
r
r
o
r
(
m
)
100 200 300 400 500
2
0
2
T i m e ( s )
SPS
?
N
E
r
r
o
r
(
m
)
105
the leader is out of sight of the follower. Also, it is shown how the NLOS method can be
used to follow a lead vehicle's path of travel when a significant delay exists between
leader and follower operation. A simple proportional derivative (PD) control originally
designed for waypoint following was used on a UGV for real time testing of the two
methods. Details of the experimental setup are given in Appendix B.
5.2 LINE OF SIGHT FOLLOWING
Given a relative position vector, the angle between a leader and follower vehicle can be
easily calculated in a local ENU navigation frame using the relative north and east
position (???? and ???? , respectively). The relative angle is denoted by ?? and is positive
clockwise from North.
?? = atan ?????
???
(5.1)
Commanding the following vehicle?s heading
angle to the relative angle makes the follower
point towards the lead vehicle. This creates a
towing effect without a physical link between the
vehicles. Figure 5.3 is a schematic demonstrating
the concept.
Relative angle accuracy can be expressed
as a function of the relative position accuracy and
the antenna separation distance.
??? = asin ???? ?
??
(5.2)
Figure 5.3: A relative angle between two
vehicles can be determined given a relative
position vector. The angle can then be used as
a control reference to autonomously follow a
lead vehicle.
r
L F
?
L
?
R
?
F
N
E
106
A highly accurate relative position is required for short distance following. Figure 5.4
displays relative angle accuracy versus following distance for three relative positioning
accuracies. The DRTK algorithm can provide a suitable reference angle, but a standalone
solution will not have sufficient accuracy for some applications using this method
because of aberrations in the estimated positions as shown in Figure 5.1 and Figure 5.2
(note that accuracy requirements are application specific, where some such as following a
mine clearing vehicle require centimeter level accuracy, while others like desert driving
would only require meter level accuracy).
Two deficiencies exist using this short distance following method. The following
vehicle will not be able to track the lead vehicle path if the lead vehicle performs
maneuvers with a turning radius shorter than the baseline length. The constraint of short
distance following reduces the possibility of this occurring in many operational
environments. The second shortcoming is the inability to replicate the lead vehicle?s
Figure 5.4: Relative angle accuracy is a function of relative position accuracy and antenna separation distance.
0 10 20 30 40 50
10
2
10
1
10
0
10
1
10
2
R
e
la
ti
v
e
A
n
g
le
A
c
c
u
r
a
c
y
(
d
e
g
)
A n t e n n a S e p e r a t i o n ( m )
? = 0 . 0 2 m
? = 0 . 1 m
? = 1 m
107
turning radius, which leads to a lateral path error. The steady state error (??) is a
function of the lead vehicle turning radius (??) and the baseline length.
?? = ?? 1 ? sin acos ??? ?
?
(5.3)
Figure 5.5 is a plot of the anticipated error as the turning radius and baseline varies and
demonstrates the operational region when using this concept. A short following distance
yields low lateral error for most turning radii, and a longer following distance requires
larger turning radii to keep the error reasonable.
5.3 NON LINE OF SIGHT FOLLOWING
A following vehicle can accurately repeat the leader's path for short following distances
using the method mentioned in the previous subsection. Longer following distances
increase the likelihood of the following vehicle deviating from a driven path. The
Figure 5.5: The steady state error is a function of the lead vehicle turning radius and the baseline.
108
inability to travel about the same turning radius causes the vehicle to cut corners, which
effectively filters the path. The effect increases with following distance. This effect also
renders the method useless when intricate paths are driven by the leader. Therefore, the
separation distance perceived by the following vehicle must be reduced to improve
accuracy.
The approach in this subsection creates a virtual lead vehicle that is sufficiently
close to the following vehicle to inherit the benefits of the short distance following
method. All information gathered and retained is relative to the following vehicle, so no
global position information is necessary. The approach hinges on the availability of
accurate RPV measurements between vehicles and accurate measurements of the change
in following vehicle position. GPS based methods for determing the RPV were discussed
in Chapter 2 and Chapter 3, and odometry techniques were discussed in Chapter 4. If the
change in position of the following vehicle (???,???) can be accumulated, it can be
subtracted from a previous RPV (??????) to create an RPV to the virtual leader (??????,?).
This reduces the perceived baseline from the follower to the leader, and the RPV is now
the distance between the following vehicle at the current time (??) and the lead vehicle at
Figure 5.6: A virtual lead vehicle is created by adding the change in follower position to a relative position
measurement at a previous instance in time.
t
k
t
k
t
k  1
t
k  1
t
k  n
t
k  n
r
L F k  n
? r
k , k  1
? r
k , k  n
r
L F k  n , k
L e a d V e h i c l e
F o l l o w i n g V e h i c l e
C u r r e n t P o s i t i o n
P r e v i o u s P o s i t i o n
109
a previous time (????). This notion is graphically explained in Figure 5.6, where the gray
lines represent measurements of the RPV and change in position at previous times and
the black line represents the shortened RPV between the leader at a previous time and
follower at the current time. The solid color vehicles indicate their location at the current
time step, and the transparent vehicles are past locations.
Sufficient knowledge of past changes in the follower's position can shrink the
RPV to minimize trajectory duplication error using Equation (5.1) to obtain a heading
reference. Creation of the RPV to the virtual leader is mathematically expressed as
follows:
????,??? = ?????? + ???
?
?=???
(5.4)
The vector components of change in position, ??, are summed from index kn to the
current index k. These values are then subtracted from the corresponding vector
components of the RPV at index kn to determine the RPV between the follower at time k
and leader at time kn. The change in position measurements must be accumulated at a
rate greater than or equal to the RPV measurement rate. Higher rates result in a higher
fidelity RPV between the following vehicle and virtual lead vehicle.
A desired virtual following distance or "look ahead distance" must be determined
to pick an appropriate ???? and create a virtual leader position to track. The desired
value for this distance is modeled after human action. Human drivers alter how far down
their intended path of travel they look based on the speed they are traveling. In a parking
lot environment, focus is placed in an area close to the vehicle. However in a highway
environment, the area of focus could be a hundred meters from the vehicle. The virtual
110
following distance can be a chosen as some minimum following distance plus the scaled
speed.
?? = ???? + ?????? ?? (5.5)
For this work, the minimum distance is chosen to be one meter and the scaling value is
one. It can be seen in Equation (5.5) that a shorter following distance decreases path
error, but it also increases the sensitivity of the reference angle to changes in vehicle
position. Changing the scale affects the damping in the system response; higher scaling
values can cause the following vehicle to effectively filter out high frequency content in
the path, while lower values increase the influence of noise on the reference value.
The virtual following distance creates a
circular field around the following vehicle, as
shown in Figure 5.7. The field is refined to a
semicircle including the region in front of the
vehicle by incorporating the vehicle's heading
measurement. This field is used to select a
virtual leader position that is immediately
beyond the desired following distance. A
search is performed using Equation (5.4). The
index n starts at zero and increments until the
virtual leader location is within the region or
behind the follower. It is then incremented by one to place the virtual leader position at
the first location outside of the region. Scenarios where the follower is close to the (real)
lead vehicle reveal no suitable candidates to the search routine, and therefore the RPV at
Figure 5.7: A look ahead distance is computed,
and the follower chooses to follow the virtual
lead vehicle just outside of the radius.
d
v
r
L F k ,k  n
F o l l o w e r
V i r t u a l L e a d e r
F o llo w e r?s ? V is io n ?
L e a d V e h i c l e
P a t h o f T r a v e l
111
the current time is used to generate the heading reference. In other words, the method
defaults to the short distance LOS following method described in the previous subsection.
The vector addition of the DRTK baseline estimate and the odometer estimate
increases the noise on the perceived RPV. The independent variances of the two
estimates can be added.
???,??? = ?????2 + ????,???2 (5.6)
Error growth usually is dominated by the change in position error, as the RPV error is on
the order of two centimeters and the odometry error quickly crosses the two centimeter
threshold. Using Equation (5.6), the error in the heading angle can be computed. The
angular error is a function of the lateral error and baseline. This represents the difference
in the directed angle of travel and the real angle of travel.
??? = asin
???,???
????,???
(5.7)
This noise increase is proportional to the number of accumulated position
changes. Equation (5.8) expresses the maximum attainable lateral path error as a
function of DRTK and TDCP accuracy and the number of accumulations.
???,??? = ?????2 + ?????2 + ????? 2
(5.8)
A closed form analytical solution is not available for the lateral path error with the INS
based odometry information, therefore simulations must be used to predict the INS error
based on anticipated motion profiles [59]. Given an average speed, the number of
accumulations of position change estimates corresponds to a following distance. Figure
5.8 through Figure 5.12 show lateral error growth as a function of following distance
112
using the different odometry methods for average speeds of 2.5, 5, 10, and 20 meters per
second.
Figure 5.8: Lateral error as a function of following distance and speed with a full automotive grade INS.
Figure 5.9: Lateral error as a function of following distance and speed with a full tactical grade INS.
As expected, the tactical grade INS offers better performance than the automotive grade
INS. The performance of the full automotive grade INS is unacceptable for any
0 100 200 300 400 500
0
20
40
60
80
100
F o l l o w i n g D i s t a n c e ( m )
1
?
L
a
te
r
a
l
E
r
r
o
r
W
it
h
F
u
ll
A
u
to
m
o
ti
v
e
I
N
S
(
m
)
2 . 5 m / s
5 m / s
1 0 m / s
2 0 m / s
0 100 200 300 400 500
0
0 . 1
0 . 2
0 . 3
0 . 4
0 . 5
F o l l o w i n g D i s t a n c e ( m )
1
?
L
a
te
r
a
l
E
r
r
o
r
W
it
h
F
u
ll
T
a
c
ti
c
a
l
I
N
S
(
m
)
2 . 5 m / s
5 m / s
1 0 m / s
2 0 m / s
113
meaningful following distances as the error is beyond one meter at following distances of
4.5, 9, 11, and 26 meters for each of the respective velocities.
Figure 5.10: Lateral error as a function of following distance and speed with a partial automotive grade INS.
Figure 5.11: Lateral error as a function of following distance and speed with a partial tactical grade INS.
Assuming platform motion constraints in the INS model as discussed in Section 4.2
offers a performance increase in path following capability. The automotive grade IMU
0 100 200 300 400 500
0
0 . 1
0 . 2
0 . 3
0 . 4
0 . 5
F o l l o w i n g D i s t a n c e ( m )
1
?
L
a
te
r
a
l
E
r
r
o
r
W
it
h
P
a
r
ti
a
l
A
u
to
m
o
ti
v
e
I
N
S
(
m
)
2 . 5 m / s
5 m / s
1 0 m / s
2 0 m / s
0 100 200 300 400 500
0
0 . 1
0 . 2
0 . 3
0 . 4
0 . 5
F o l l o w i n g D i s t a n c e ( m )
1
?
L
a
te
r
a
l
E
r
r
o
r
W
it
h
P
a
r
ti
a
l
T
a
c
ti
c
a
l
I
N
S
(
m
)
2 . 5 m / s
5 m / s
1 0 m / s
2 0 m / s
114
becomes a useful tool that can provide the means to follow a path at short distances, and
the tactical grade IMU further extends the usable range of the method.
Figure 5.12: Lateral error as a function of following distance and speed with the TDCP method.
The TDCP odometry measurement offers a different error profile than the inertial based
odometry methods. This is due to integration of a single biased measurement, where the
inertial systems couple a single integration of gyroscope biases with two integrations of
accelerometer biases, resulting in a quadratic curve. The TDCP curves are representative
for both the unaided and inertial aided approaches as the accuracies were similar after the
aided filter transients were gone, as shown in Section 4.5.3.
For comparison purposes, the lateral error using all odometry methods is shown
for a robotic follower traveling at five meters per second in Figure 5.13 and at twenty
meters per second in Figure 5.14. The clear winner in terms of providing the lowest
accuracy is the partial tactical grade INS based odometry measurement. However, it
comes with the stipulations of negligible roll and pitch and no sideslip. The partial
automotive grade INS based odometry measurement provides acceptable accuracies at
0 100 200 300 400 500
0
0 . 1
0 . 2
0 . 3
0 . 4
0 . 5
F o l l o w i n g D i s t a n c e ( m )
1
?
L
a
te
r
a
l
E
r
r
o
r
W
it
h
T
D
C
P
(
m
)
2 . 5 m / s
5 m / s
1 0 m / s
2 0 m / s
115
higher speeds, but for practical purposes it likely cannot be used as the platform
obviously must operate at lower speeds before attaining the higher speeds which provide
the better accuracy. The TDCP methods provide similar accuracies to the partial tactical
grade INS, and they are not susceptible to errors from unmodeled platform dynamics.
However, the unaided method will not function in the event of a GPS outage, while the
aided method can provide a level of immunity and function during intermittent outages.
Figure 5.13: Lateral error as a function of following distance at five meters per second for all odometry methods.
Figure 5.14: Lateral error as a function of following distance at twenty meters per second for all odometry
methods.
0 100 200 300 400 500
0
0 . 0 5
0 . 1
0 . 1 5
0 . 2
0 . 2 5
F o l l o w i n g D i s t a n c e ( m )
1
?
L
a
te
r
a
l
E
r
r
o
r
T
r
a
v
e
li
n
g
5
m
/s
(
m
)
F u l l A u t o I N S
F u l l T a c t i c a l I N S
P a r t i a l A u t o I N S
P a r t i a l T a c t i c a l I N S
T D C P
0 100 200 300 400 500
0
0 . 0 5
0 . 1
0 . 1 5
0 . 2
0 . 2 5
F o l l o w i n g D i s t a n c e ( m )
1
?
L
a
te
r
a
l
E
r
r
o
r
T
r
a
v
e
li
n
g
2
0
m
/s
(
m
)
F u l l A u t o I N S
F u l l T a c t i c a l I N S
P a r t i a l A u t o I N S
P a r t i a l T a c t i c a l I N S
T D C P
116
From a cost perspective, the best accuracy is obtained using the unaided TDCP method as
carrier measurements are readily available on lower cost GPS receivers (<$500 USD),
while a tactical grade IMU can cost upwards of $20,000 USD.
Again, these charts represent the minimum attainable error as the errors
correspond to the error in virtual leader position, or the difference between the computed
leader position and where it actually was at a previous instance in time. Beyond this step
it is up to the vehicle controller to accurately follow the reference to maintain acceptable
path following accuracies. However, the method of reference point generation offers
significant improvement over simple waypoint following, and the accuracy over
lengthened following distances can allow for NLOS operation.
5.4 ALTERNATE OPERATIONAL MODES
5.4.1 Delayed Following
An extension of the NLOS method provides the ability to redrive a vehicle's path of
travel at some later time. For example, a vehicle departs the base carrying a unit of
soldiers to some tactical environment. A day later, a UGV carrying supplies departs from
the base, travels the same path as the vehicle did the day before, and delivers supplies to
the soldiers in the tactical environment. Again, the simplest way to accomplish this task
is to broadcast GPS waypoints to remote vehicles, but this does not provide the necessary
precision to drive through perilous terrain or consistently remain in a designated lane of
travel. The NLOS method can be used without modification to offer this operational
capability.
The DRTK RPV is used to track the position of the mobile leader relative to the
current position of the following vehicle. If the follower remains stationary, the leader's
117
path is maintained relative to the following vehicle, as shown in Figure 5.15. Therefore,
the leader can perform its necessary maneuvers, and a precision path is known for a
theoretically infinite period of time as long as the following vehicle does not move. Once
the follower is given the appropriate commencement signal, it uses the odometry
measurements to shrink the RPV and follow each successive closest virtual leader.
An alternative description of the process would consider the static following
vehicle a temporary RTK base station with no surveyed location. All position
measurements are measured with RTK accuracy with respect to the following vehicle.
The following vehicle then uses the odometry information to dead reckon through the
relative path measured with the DRTK algorithm. Use of the carrier measurements
allows for the creation of a consistent and accurate reference path, as well as consistency
in the dead reckoned solution if the TDCP method is used.
Figure 5.15: The lead vehicle's path can be maintained relative to the follower as long as the follower remains
stationary.
r
L F k
r
L F k  1
r
L F k  2
r
L F k  n
S t a t i o n a r y F o l l o w i n g V e h i c l e
L e a d V e h i c l e ? s
P a t h o f T r a v e l
D R T K
M e a s u r e m e n t
118
Practical limitations reduce the time period between vehicle operations, but
periods of minutes, hours, days, or even weeks are feasible given the proper equipment
and power sources. In situations where it is not feasible to maintain a stationary
following vehicle, the DRTK RPV can be measured to a reference point using additional
hardware. The following vehicle would then need to be precisely aligned with the
reference point before starting its mission.
5.4.2 Vehicles as Temporary Base Stations
The vehicles can serve as temporary static base stations in some operational scenarios,
and the DRTK RPV accuracy can be still be used for path following. Revisit the example
from the previous subsection where a human driven vehicle carries a unit of soldiers from
a base to some destination. A remote, stationary robotic follower records the vehicle's
position relative to itself with the DRTK algorithm. After the soldiers deploy, the
original vehicle remains in a static state. The follower then departs the base and traces
the original vehicle's path of travel. It is able to do so because it maintains
communication with the vehicle to compute the DRTK RPV.
The path following problem reduces to the simple GPS breadcrumb following
approach for this scenario because the leader's path is known relative to the follower's
initial position. Reference path and vehicle position consistency issues are resolved by
using the DRTK algorithm, provided sufficient GPS coverage is available. The leader
departs, and its position is recorded relative to the follower's static position using DRTK.
When the robotic follower starts moving, its initial position is retained as the origin of the
local map. The follower can accurately determine where it is within the local map by
using the DRTK RPV from the stationary lead vehicle. In principle, the method of
119
operation is similar as that discussed in the previous subsection. The distinction lies in
the ability to use the DRTK algorithm to keep track of the follower's position as its
traveling because the leader is serving as a static base station. Therefore, dead reckoning
with odometry measurements is unnecessary, and path following error is limited to
DRTK accuracy, which is irrespective of distance traveled.
5.5 EXPERIMENTAL RESULTS
Experimental tests were conducted using the system described in Appendix B. An ATV
Corp. Prowler was converted to a UGV and served as the robotic follower, and a Hyundai
Santa Fe was the man driven lead vehicle. Both LOS and NLOS techniques were tested
successfully. Truth was recorded using a traditional RTK system.
5.5.1 LOS Following
Experimental tests demonstrated the effectiveness of the short distance following method
with the DRTK RPV used as a reference. The automated Prowler successfully trailed the
human driven Santa Fe with following distances ranging from 5 to 50 meters with speeds
ranging from two to nine meters per second. Behavior was as anticipated; the Prowler
positioned itself as if it were intow as it continually oriented its heading to point at the
lead vehicle. Results are presented for straight driving maneuvers and constant radius
turning maneuvers to emphasis both the strengths and weaknesses of the LOS following
method.
Experimental Results for Straight Driving
Deviation from the lead vehicle?s path of travel was small, although it was nearly always
present. Figure 5.16 displays the positions of the two vehicles as they travelled along the
120
south portion of the track. Note the scale of the figure due to the slight counter clockwise
rotation of the test track.
Figure 5.16: Shown are the positions of the lead and following vehicle (in gray and black, respectively) after
multiple passes on the south portion of the track.
Figure 5.17 shows a single pass down the straight section of the track and
demonstrates the typical lateral path error. Figure 5.18 contains the following distance
during that pass.
 1 0 0 0  8 0 0  6 0 0  4 0 0  2 0 0 0
 4 0 0
 3 0 0
 2 0 0
 1 0 0
E a s t ( m )
N
o
r
t
h
(
m
)
 1 0 0 0  9 0 0  8 0 0  7 0 0  6 0 0  5 0 0  4 0 0  3 0 0  2 0 0  1 0 0
 3 5 5
 3 5 0
 3 4 5
 3 4 0
E a s t ( m )
N
o
r
t
h
(
m
)
L e a d e r
F o l l o w e r
121
Figure 5.17: Typical lateral path error while driving straight.
Figure 5.18: The following distance corresponding to the lateral error in Figure 5.17.
Oscillation is clearly present in the plot and consistent over multiple passes. The
period of oscillation varied from 5 to 20 seconds while travelling down the straight
sections of the test track. This oscillation is due to a combination of things: mechanical
slop in the steering system, a small turn on steering bias, and perturbation of the system
from switching reference modes (if DRTK is unavailable, the reference generation
algorithm defaults to using a position difference from position solutions computed by
each receiver to determine relative position).
The maximum lateral error experienced was 58 centimeters when the DRTK
relative position was used. The total maximum error was 1.5 meters and can be seen in
Figure 5.16. This error occurred when the DRTK solution was momentarily lost while
travelling through the canyon at the southwest corner of the track, and the relative
position was determined by differencing the nonRTK position solutions computed by
0 20 40 60 80 100 120
 0 . 5
0
0 . 5
T i m e ( s )
P
a
th
E
rr
o
r
(m
)
0 20 40 60 80 100 120
10
20
30
T i m e ( s )
B
a
se
li
n
e
(
m
)
122
each receiver. The lateral error, when accounting for all tests on straight sections, had a
mean of 5 centimeters and a standard deviation of 24 centimeters. Speed did not have a
noticeable impact on the system's performance.
Experimental Results for Constant Radius Turning
The vehicle acted as expected during the constant radius turns by turning about a smaller
radius than the lead vehicle. Oscillations became less persistent in the corners. Steady
state path error measured in the turns is listed in Table 5.1. Interestingly, the path error
was smaller than values predicted using Equation (5.3). This is likely due to an inability
to hold the following distance constant. The theoretical path error was derived under the
assumption that the centers of rotation for each vehicle were concentric, which is true
when the baseline between vehicles is constant. A time rate of change of the baseline
invalidates the assumption, and the vehicles rotate about nonconcentric points.
Table 5.1: Steady state error during constant 150 m radius turns
Baseline (m) Actual Error Theoretical Error
13 0.52 0.56
13.5 0.39 0.61
16 0.72 0.86
17 0.57 0.96
5.5.2 NLOS Following
The effectiveness of the NLOS method was also demonstrated by experimental tests.
Figure 5.19 shows a test in the NCAT parking lot. The Santa Fe was driven through and
around the parking lot, and the automated Prowler followed behind at varying following
distances (which are shown in Figure 5.20). The RTK positions are plotted, where the
circle is the leader's position and the dot is the follower's position. The Prowler clearly
replicated the path driven by the Santa Fe. Some path deviation is present during a few
123
of the turns. This is due to carrying too high of a speed into the corner and overdriving
the desired path.
Figure 5.21 displays the reference accuracy and actual duplication accuracy as a
function of distance. Reference accuracy is the accuracy in which the NLOS method
places the virtual lead vehicle compared when compared to the actual position of the lead
vehicle. Multiple laps were driven around the test track to collect the data. Error in path
following was consistently at or below 20 centimeters, although it did rise occasionally to
80 centimeters. This error is primarily due to vehicle control (the controller used was a
Figure 5.19: Trajectories of the lead and automated following vehicle as the lead vehicle maneuvered around a
parking lot.
Figure 5.20: Following distance along the path for the experiment shown in Figure 5.19.
 3 0  2 0  1 0 0
0
10
20
30
40
E a s t ( m )
N
o
r
t
h
(
m
)
 2 0  1 5  1 0 5 0
30
35
40
45
50
55
E a s t ( m )
N
o
r
t
h
(
m
)
L e a d e r
F o l l o w e r
S t a r t
E n d
0 10 20 30 40 50 60 70 80
0
20
40
T i m e ( s )
F
o
ll
o
w
in
g
D
is
ta
n
c
e
(
m
)
124
gain scheduled, proportional derivative controller). The reference accuracy remains
below 5 centimeters, which agrees with the predicted values shown in Figure 5.12.
5.6 CONCLUSION
Two GPS based methods enabling an automated following vehicle to replicate the path
driven by a lead vehicle were presented. The first method allows for short distance
following where the follower maintains LOS to the lead vehicle. The second approach
removes the LOS constraint, and the follower can accurately replicate the leader's path
when the leader is hundreds of meters ahead of the follower. The first method used only
the DRTK RPV estimate, while the second method incorporated vector odometry
information with the DRTK RPV.
Error analyses of the two methods predicted the operational capability and
exposed maneuvers and following distances where performance degrades. The short
distance method breaks down as following distances increase and turning radii decrease.
Figure 5.21: Actual path deviation, shown in gray, was consistently below twenty centimeters, but the reference
generated with the NLOS following method was under five centimeters.
125
The long distance following method relies on precision odometry information to shrink
the perceived RPV. Therefore, its capability is directly linked to odometer quality.
Both methods were demonstrated on a physical robotic convoy. Both unaided
DRTK and TDCP algorithms were implemented in C++ for real time operation. The
UGV successfully replicated the path of the human driven lead vehicle using both
methods, demonstrating their feasibility and capability. Accuracies recorded were in line
with the predicted values for both methods.
126
Chapter 6
Conclusion
6.1 CONCLUSION
The research described in this dissertation developed GPS based path duplication
strategies for automated ground vehicle convoys. Specifically, two strategies were
described in detail: a short distance, LOS following method, and a long distance, NLOS
virtual leader following method. The GPS carrier measurement was used to determine a
high accuracy relative position vector between two dynamic vehicles. Odometry
information was developed from inertial sensors and from the GPS carrier measurement
for coupling with the relative measurements to determine virtual leader positions. Both
the relative positioning algorithm and the GPS carrier based odometry were aided with
inertial measurements to improve their robustness in the presence of short partial or total
GPS outages, and to minimize the phase ambiguity reacquisition time in the case of the
relative positioning algorithm.
The relative positioning algorithm was akin to traditional RTK techniques. The
distance between two points can be measured with both accuracy and precision, but
global position information is lost due to the removal of the RTK base station. The lead
vehicle serves as a dynamic base station and broadcasts its observables to following
127
vehicles. Because of this method of operation, the algorithm was given the moniker of
Dynamic base Real Time Kinematic, or DRTK for short. The following vehicles use the
leader's observables to difference out common mode errors in the GPS pseudorange and
carrier phase measurements. The observables are processed to estimate carrier phase
biases known as ambiguities, which are then fixed to integer values. The integer phase
ambiguities are removed from the phase measurements, and the unbiased phase
measurements are processed to determine a centimeter level accurate relative position
between the vehicles.
Next, the DRTK algorithm was aided with inertial measurements. IMUs on two
independent platforms were aligned and used to form relative inertial measurements. The
relative inertial measurements were input into the DRTK algorithm to provide dynamic
information about the relative platform motion. Dead reckoning of the inertial
measurements provide a graceful solution degradation in the event of a partial or total
GPS outage. It was shown that ambiguity reacquisition times were directly related to
initial relative position accuracy, and that the acquisition was near instantaneous (< ~2
seconds) for initial position accuracies under 20 centimeters. Three dimensional error
growth profiles of relative automotive and tactical inertial measurements were developed
to show the IMU quality needed for outages of a given duration, and experimental results
using the DRTK/INS algorithm with automotive grade IMUs were provided.
The virtual leader following method utilized a vector of information describing
the change in platform position from a nominal point to translate the DRTK relative
position from two points at some previous time to two points across time and space.
Therefore, methods were developed to provide this odometry information to the
128
following method. An inertial sensor was first dead reckoned to provide the change in
position information. Both a full and partial IMU were used, and the performance
improvement of the partial IMU mechanization was noted, provided assumptions about
motion constraints were not violated. Second, the GPS carrier measurements were
differenced across time to reduce time correlated errors present in the signal and the
phase ambiguity. The time differenced carrier observable was processed to form
millimeter accurate, three dimensional odometry information. Third, the inertial
measurements were used to aid the time differenced carrier observables to improve
robustness against short GPS outages. It was shown that accuracy was the same as using
the unaided observables after the filter settled out.
The first path following method used only the relative position between two
points to determine the relative orientation between two vehicles. The relative heading
was used as a control reference, essentially commanding the following vehicle to drive at
the lead vehicle. This method emulated many vision based approaches that use cameras,
laser scanners, or a combination of both to determine range and bearing information from
the following vehicle to the lead vehicle. It was shown that the path following error
increased with both an increase in following distance and a decrease in turning radius.
Therefore, the LOS constraint necessary for the vision based approaches must be
maintained for any practical use of the following method.
The virtual leader following method removed the main constraint of common
vision based following techniques that require the following vehicle to maintain LOS to
the leader, thus providing the user a unique capability to duplicate the trajectory of a lead
vehicle that can be utilized in two specific ways. First, an automated vehicle can directly
129
follow a human driven vehicle while replicating its traveled path. Following distances
are lengthened, and complex maneuvers can be performed by the lead vehicle. Second,
an automated vehicle can drive the same path as a human driven vehicle minutes, hours,
or even days later, provided the automated vehicle remains stationary during that period
of time. This ability comes from maintaining all information relative to the follower.
Path duplication accuracy versus following distance was demonstrated using the three
odometry techniques developed. It was shown that a partial tactical grade IMU could
offer the best performance up to a 500 meter following distance, but the time differenced
carrier method offered the most cost feasible option by offering a similar performance.
However, some aspects might deem the virtual leader method infeasible for some
applications. The method is more complicated since a database of previous measurements
is required. A medium must be in place for intervehicle communication to occur. Any
error in the change in position measurement will accumulate, so the effectiveness of this
method degrades with following distance. Accuracy requirements of this measurement
might be too stringent for some applications as the path duplication accuracy degrades
with following distance.
6.2 FUTURE WORK
Several avenues of future work exist involving the research presented in this dissertation.
A reliability metric for the DRTK algorithm is necessary to relay confidence information
to any subsystems using its output. Currently, the ratio test is used to validate the integer
phase ambiguities. However, this is simply a passfail test, and there is a small likelihood
that an incorrect integer vector or a subset within the vector are incorrect despite passing
the test. Methods exist that can compute the probability of correctly fixing the integer
130
ambiguities, but they cannot be used with the current version of the DRTK algorithm.
The bootstrapping methods used to estimate the probability of correct fix fail by
providing overly optimistic values when time correlated errors are present within
observations. Unfortunately that is the case when processing the pseudorange and carrier
phase measurements at higher rates. Alternatives are to process the measurements at
slower rates to remove effects from multipath (usually sampling rates with units of
minutes are necessary), or to form ambiguity observables as mentioned in Section 2.2.4.
These are not favorable alternatives on ground vehicle platforms as individual satellite
outages can be frequent, and cycle slips resulting from the outages would effectively
require the DRTK algorithm to reinitialize ambiguity states too often for effective use of
the alternatives mentioned.
A networked DRTK solution could be sought to provide additional constraints on
the relative position solutions, and therefore the ambiguity combinations, when multiple
vehicles are in a convoy. This would be a computationally intensive effort requiring the
processing of observables from all or a subset of vehicles in the convoy, but such a
technique should improve the assurance in the DRTK solution and methods to detect
faults in measurements from any vehicle. The additional constraints provided by a
vehicle convoy application might be questionable, as the system geometry is more or less
a straight line under many operational scenarios, but the application of the networked
approach could extend to purposes other than path following, specifically those that
would have diverse geometry. Potential examples are soldiers and robots distributed in a
battlefield, or a construction site with equipment spaced around and within the perimeter
of a site.
131
Incorporation of perception sensors would improve all elements of the research
discussed in this dissertation. Range and bearing information could be used to both
initialize the DRTK algorithm and validate its output when the robotic follower is within
LOS of the leader. Environmental features could be detected and used to bound inertial
error growth. This would aid in ambiguity reacquisition when using the DRTK/INS
algorithm, and improve inertial based odometry information. Finally, map matching
techniques could be used where environmental landmarks detected by the lead vehicle are
broadcast to the following vehicles. The following vehicles would use the information to
determine their position and pose relative to the leader's perspective of the surrounding
environment. A control strategy could align the following vehicle in the environment
based on the correlation of environmental features between the vehicles. This subsystem
could be combined with the DRTK to improve the accuracy of the environmental
landmark locations, and it could also be combined with the path following methods
presented to add robustness in the presence of GPS outages.
132
References
[1] R.G Brown and P.Y.C. Hwang, Introduction to Random Signals and Applied
Kalman Filtering. New York, New York, USA: John Wiley and Sons, 1997.
[2] M. Cannon, C. Basnayake, S. Crawford, S. Syed, and G. Lachappelle, "Precise GPS
Sensor Subsystem for Vehicle Platoon Control," in Proceedings of the 16th
International Technical Meeting of the Satellite Division of the Institute of
Navigation (ION GPS 2003), Portland, Oregon, USA, 2003, pp. 213224.
[3] F.C. Chan, "A State Dynamics Method for Integrated GPS/INS Navigation and Its
Application to Aircraft Precision Approach," Illinois Institute of Technology,
Chicago, Illinois, USA, Ph.D. Dissertation 2008.
[4] C. Chen et al., "Target Tracking and Path Planning for Vehicle Following in Jungle
Environment," in Proceedings of the IEEE 8th International Conference on Control,
Automation, Robotics, and Vision, Kunming, China, 2004, pp. 455460.
[5] B.J. Clark and D.M. Bevly, "FDE Implementations for a Low Cost GPS/INS
Module," in Proceedings of the 22nd International Technical Meeting of the Satellite
Division of the Institute of Navigation (ION GNSS 2009), Savanna, Georgia, USA,
2009, pp. 970977.
[6] S.J. Comstock, "Development of a Low Latency, High Data Rate, Differential GPS
Relative Positioning System for UAV Formation Flight Control," Air Force Institute
of Technology, Ph.D. Dissertation 2006.
[7] C.C. Counselman and I.I. Shapiro, "Miniatgure Interferometer Terminals for Earth
Surveying," Journal of Geodesy, vol. 53, no. 2, pp. 139163, June 1979.
[8] S. Crawford, M. Cannon, D. Ltourneau, P. Lepage, and F. Michaud, "Performance
Evaluation of Sensor Combinations on Mobile Robots for Automated Platoon
Control," in Proceedings of the 17th International Technical Meeting of the Satellite
Division of the Institute of Navigation (ION GNSS 2004), Long Beach, CA, 2004,
pp. 706717.
[9] R. Daily and D.M. Bevly, "The Use of GPS for Stability Control Systems," IEEE
Transactions on Industrial Electronics, vol. 51, no. 2, pp. 270277, 2004.
133
[10] P. de Jong and C. Tiberius, "The LAMBDA Method for Interger Ambiguity
Estimation: Implementation Aspects," Delft Geodetic Computing Center, Delft,
Netherlands, LGR Series 12, 1996.
[11] W. Ding, "Integration of MEMS INS with GPS Carrier Phase Derived Velocity: A
New Approach," in Proceedings of the 20th International Technical Meeting of the
Satellite Division of the Institute of Navigation (ION GNSS 2007), Fort Worth,
Texas, USA, 2007, pp. 20852093.
[12] S. Dogra, J. Wright, and J. Hansen, "Sea Based JPALS Relative Navigation
Algorithm Development," in Proceedings of the 18th International Technical
Meeting of the Satellite Division of the Institute of Navigation (ION GNSS 2005),
Long Beach, California, USA, 2005, pp. 28712881.
[13] P Enge, "Local Area Augmentation of GPS for the Precision Approach of Aircraft,"
Proceedings of the IEEE, vol. 87, no. 1, pp. 111132, January 1999.
[14] J.A. Farrell, Aided Navigation. New York, New York, USA: McGraw Hill, 2008.
[15] S. Felter and N. Wu, "A Relative Navigation System for Formation Flight," IEEE
Transactions on Aerospace and Electronic Systems, vol. 33, no. 3, pp. 958967, July
1997.
[16] T. Ford, M. Hardesty, and M. Bobye, "Helicopter Ship Board Landing System," in
Proceedings of the 18th International Technical Meeting of the Satellite Division of
the Institute of Navigation (ION GNSS 2005), Long Beach, California, USA, 2005,
pp. 979988.
[17] A.M. Fosbury and J.L. Crassidis, "Relative Navigation of Air Vehicles," Journal of
Guidance, Control, and Dynamics, vol. 31, no. 4, pp. 824834, August 2008.
[18] G.X. Gao et al., "Moderinizing Milestones: Observing the First GPS Satellite with
an L5 Payload," Inside GNSS, vol. 4, no. 3, pp. 3036, May/June 2009.
[19] D. GebreEgziabher, "Design and Performance Analysis of a LowCost Aided Dead
Reckoning Navigator," Stanford University, Stanford, California, USA, Ph.D.
Dissertation 2004.
[20] A. Gelb, Applied Optimal Estimation.: The MIT Press, 1974.
[21] T. Gillespie, Fundamentals of Vehicle Dynamics. Warren, Pennsylvania, USA:
Society of Automotive Engineers, 1992.
134
[22] P.D. Groves, Principles of GNSS, Inertial, and Multisensor Integrated Navigation
Systems. Boson, Massachusettes, USA: Artech House, 2008.
[23] P.D. Groves et al., "Optimising the Algorithm Design for High Integrity Relative
Navigation Using Carrier Phase Relative GPS Integrated with INS," in Proceedings
of the 21st International Technical Meeting of the Satellite Division of the Institute
of Navigation (ION GNSS), Savannah, Georgia, USA, 2008, pp. 13231334.
[24] D.W. Hodo, "Development of an Autonomous Mobile RobotTrailer System for
UXO Detection," Auburn University, Auburn, Alabama, USA, M.S. Thesis 2007.
[25] B. HofmannWellenhof, H. Lichtenegger, and J. Collins, GPS: Theory and Practice,
Fourth Edition ed.: SpringerVerlag, 2001.
[26] P. Joosten and C. Tiberius, "LAMBDA: FAQs," GPS Solutions, vol. 6, pp. 109114,
November 2002.
[27] E.D. Kaplan and C.J. Hegarty, Understanding GPS: Principles and Applications,
2nd ed.: Artech House, 2006.
[28] G.C. Kebler, J.P. Maschuw, and D. Abel, "Lane Change of HeavyDuty Vehicle
Platoons Without Lateral Position Information," in Proceedings of the Fifth IFAC
Symposium on Advances in Automotive Control, Monterey, California, USA, 2007.
[29] C.C. Kellum, "Basic Feasibility of GPS Positioning Without Carrier Phase
Measurements as a Relative Position Sensor Between Two Vehicles," in
Proceedings of the 18th International Technical Meeting of the Satellite Division of
the Institute of Navigation (ION GNSS 2005), Long Beach, California, USA, 2005,
pp. 903910.
[30] S. Khanafseh, "GPS Navigation Algorithms for Autonomous Airborne Refueling of
Unmanned Air Vehicles," Illinois Institute of Technology, Chicago, Illinois, USA,
Ph.D. Dissertation 2008.
[31] S. Khanafseh, B. Kempny, and B. Pervan, "New Applications of Measurement
Redundancy in High Performance Relative Navigation Systems for Aviation," in
Proceedings of the 19th International Technical Meeting of the Satellite Division of
the Institute of Navigation (ION GNSS 2006), Fort Worth, Texas, USA, 2006, pp.
30243034.
[32] G. Klancar, D. Matko, and S. Blazic, "Wheeled Mobile Robots Control in a Linear
Platoon," Journal of Intelligent and Robotic Systems, vol. 54, no. 5, pp. 709731,
2009.
135
[33] S.E. Langel, S.M. Khanafseh, F.C. Chan, and B.S. Pervan, "Cycle Ambiguity
Reaquisition in UAV Applications Using a Novel GPS/INS Integration Algorithm,"
in Proceedings of the 2009 International Technical Meeting of the Satellite Division
of the Institute of Navigation (ION ITM), Anaheim, California, USA, 2009, pp.
10261037.
[34] D. Lawrence, "Aircraft Landing Using GPS: Development and Evaluation of a Real
Time System for Kinematic Positioning Using the Global Positioning System,"
Stanford University, Stanford, California, USA, Ph.D. Dissertation 1996.
[35] D. Lawrence, R.B. Langley, D. Kim, F.C. Chan, and B. Pervan, "Decorrelation of
Troposphere Across Short Baselines," in Proceedings of the IEEE/ION Position,
Location, and Navigation Symposium, San Diego, California, USA, 2006, pp. 94
102.
[36] P.F. MacDoran, "Satellite Emission Radio Interferometric Earth Surveying
(SERIES)  GPS Geodetic System," Journal of Geodesy, vol. 53, no. 2, pp. 117138,
June 1979.
[37] P. Misra and P. Enge, Global Positioning System: Signals, Measurements, and
Performance, 2nd ed. Lincoln, Massachusettes, USA: GangaJamuna Press, 2006.
[38] "Navstar GPS Space Segment / Navigation User Interfaces," ARINC Research
Corporation, El Segundo, CA, ICDGPS200c, 2000.
[39] A. Nevin and D.M. Bevly, "Landmark Extraction Using Corner Detection and K
Means Clustering for Autonomous LeaderFollower Caravan," in Proceedings of the
Twelfth IASTED International Conference, Cambridge, Massachusettes, USA, 2009,
pp. 8892.
[40] T.C. Ng, J.I. Guzman, and M.D. Adams, "Autonomous VehicleFollowing Systems:
A Virtual Trailer Link Model," in IEEE International Conference on Intelligent
Robots and Systems, Edmonton, Alberta, Canada, 2005, pp. 3057 3062.
[41] C.R. Offer, P.D. Groves, A.A. Macaulay, D.L.J. Nash, and C.J. Mather, "Use of
Inertial Integration to Enhance Availability for Shipboard Relative GPS (SRGPS),"
in Proceedings of the 19th International Technical Meeting of the Satellite Division
(ION GNSS), Fort Worth, Texas, USA, 2006, pp. 726737.
[42] I. Papadimitriou and M. Tomizuka, "Lateral Control of Platoons of Vehicles on
Highways: The Autonomous Following Based Approach," International Journal of
Vehicle Design, vol. 36, no. 1, pp. 2437, 2004.
136
[43] B.W Parkinson and J.J. Spilker, Global Positioning System: Theory and Practice
Volume I. Washington, DC, USA: American Institute of Aeronautics and
Astronautics, 1996.
[44] B.W. Parkinson and J.J. Spilker, Global Positioning System: Theory and Practice
Volume II. Washington, DC, USA: American Institute of Aeronautics and
Astronautics, 1996.
[45] B. Pervan, "Navigation Integrity for Aircraft Precision Landing Using the Global
Positioning System," Stanford University, Stanford, California, USA, Ph.D.
Dissertation 1996.
[46] B. Pervan et al., "Performance Analysis of Carrier Phase DGPS Navigation for
Shipboard Landing Aircraft," NAVIGATION: Journal of the Institute of Navigation,
vol. 50, no. 3, pp. 181191, 2003.
[47] M.G. Petovello, "RealTime Integration of a TacticalGrade IMU and GPS for High
Accuracy Positioning and Navigation," University of Calgary, Calgary, Alberta,
Canada, Ph.D. Dissertation 2003.
[48] M. Petovello, G. Lachapelle, and M. Cannon, "Using GPS and GPS/INS Systems to
Assess Relative Antenna Motion Onboard an Aircraft Carrier for Shipboard Relative
GPS," in Proceedings of the 18th International Technical Meeting of the Satellite
Division of the Institute of Navigation (ION GNSS 2005), Long Beach, California,
USA, 2005, pp. 219229.
[49] M.W. Price and S. Munkeby, "Future Combat Systems (FCS) Autonomous
Navigation System (ANS) Technology Will Revolutionize Warfare," Army AL&T,
pp. 2426, AprilJune 2008.
[50] R. Ramakers, K. Henning, S. Gies, D. Abel, and M. Haberstroh, "Electronically
Coupled Truck Platoons on German Highways," in Proceedings of the 2009 IEEE
Conference on Systems, Man, and Cybernetics, San Antonoio, Texas, USA, 2009,
pp. 24832488.
[51] B. Remondi, "Performing Centimeter Level Surveys in Seconds with GPS Carrier
Phase: Initial Results," NAVIGATION, Journal of the Institute of Navigation, vol. 32,
no. 4, pp. 386400, 1985.
[52] B.M. Scherzinger, "Precise Robust Positioning with Inertial/GPS RTK," in
Proceedings of the 13th International Technical Meeting fo the Satellite Division of
the Institute of Navigation (ION GPS), Salt Lake City, Utah, USA, 2000, pp. 115
162.
137
[53] E. Schoenherr, "Moving Future Convoy Operations with Convoy Active Safety
Technologies (CAST)," Accelerate, pp. 7478, JulySeptember 2009.
[54] B.K. Soon, S. Scheding, Hy.K. Lee, Hu.K. Lee, and H. DurrantWhyte, "An
Approach to Aid INS Using Time Differenced GPS Carrier Phase Measurements,"
GPS Solutions, vol. 12, no. 4, September 2008.
[55] Q. Tang, W. Wang, Z. Gan, R. Zhang, and J. Moh, "Automatic Vehicle Following
System," 5,572,499, November 5, 1996.
[56] P.J.G. Teunissen, "The LeastSquares Ambiguity Decorrelation Adjustment: A
Method for Fast GPS Integer Ambiguity Estimation," Journal of Geodesy, vol. 70,
pp. 6582, 1995.
[57] M. Tomizuka, "Advanced Vehicle Control Systems (SVCS) Research for
Automated Highway Systems in California PATH," in Vehicle Navigation and
Information Systems Conference, Yokohama, Japan, 2002, pp. PLEN41PLEN45.
[58] S. Verhagen, "The GNSS Integer Ambiguities: Estimation and Validation," TU
Delft, Delft, The Netherlands, Ph.D. Dissertation ISBN: 9080414743, 2005.
[59] J.H. Wall, "A Study on the Effects of Stochastic Inertial Sensor Errors in Dead
Reckoning Navigation," Auburn University, Auburn, Alabama, USA, M.S. Thesis
2007.
[60] J. Wendel, O. Meister, R. Monikes, and G. Trommer, "Time Differenced Carrier
Phase Measurements for Tightly Coupled GPS/INS Integration," in Proceedings of
the IEEE/ION Position, Location, and Navigation Symposium, San Diego,
California, USA, 2006, pp. 5460.
[61] M.J. Woo, J.W. Choi, and H.S. Han, "Carrier Phase GPS/Millimeter Wave Radar for
Vehicle Platooning," in IEEE International Symposium on Industrial Electronics,
vol. 3, Pusan, South Korea, 2001, pp. 15481552.
138
Appendix A
Relative Acceleration
Figure A.1: Point A is expressed in both the stationary i frame and the moving e frame.
Consider the stationary inertial frame of reference, or i frame, centered at point I and the
non stationary frame of reference, or e frame, centered at point E shown in Figure A.1.
Point A is an arbitrary point that can be referenced with respect to both frames. It is
desired to know the acceleration of point A with respect to the e frame.
Point A can be expressed in the inertial frame as the vector sum of the vector from
point I to E in the i frame and the vector from point E to A in the i frame.
???? = ???? + ???? (A.1)
Z
i
Z
e
X
i
Y
i
Y
e
X
e
E
I
r
I E
r
I A
r
E A
A
139
Alternatively, the vector from E to A can be expressed in the e frame, but must be
multiplied by an e frame to i frame coordinate transformation matrix.
???? = ???? + ??????? (A.2)
The time derivative reveals the velocity of point A in the i frame.
? ??? = ? ??? + ???? ??? + ? ?????? (A.3)
Noting that
? ?? = ??????? (A.4)
Equation (A.2) can be rewritten, where ???? is a skew symmetric matrix containing the
rotational rates of the e frame about the i frame.
? ??? = ? ??? + ???? ??? + ??????? ???? (A.5)
The derivative of the velocity of point A with respect to time is the point's acceleration.
? ??? = ? ??? + ???? ??? + ??????? ? ??? + ??????? ? ??? + ???? ??? ???? + ??????? ???? ???? (A.6)
? ??? = ? ??? + ??? ? ??? + 2???? ? ??? + ? ??? ???? + ???? ???? ???? (A.7)
Finally, rearranging reveals the acceleration of point A with respect to the e frame.
? ??? = ? ??? ?? ??? ? 2???? ? ??? ?? ??? ???? ????? ???? ???? (A.8)
Now consider the case where the i frame is the Earth Centered Inertial (ECI)
frame, the e frame is the Earth Centered, Earth Fixed (ECEF) frame, and point A is an
object moving within Earth's atmosphere. The origins of the ECI and ECEF frame are
coincident, the Z axis is shared between the two, and their respective XY planes are
coplanar as shown in Figure A.2. The Earth's rotation rate is constant, therefore ? ??? = 0.
Also, ? ??? = 0 point E has no translational motion. The previous equation can be further
simplified.
140
? ??? = ? ??? ? 2???? ? ??? ????? ???? ???? (A.9)
The specific force measured at point A contains the acceleration due to motion
and the effects of gravity.
???? = ? ??? + ???? (A.10)
Rearranging and substituting the value in for ? ??? yields the same expression used in
Equation (3.9).
? ??? = ???? ???? ? 2???? ? ??? ????? ???? ???? (A.11)
The focus now shifts to determining ? ??? , the relative acceleration between points
A and B expressed in the e frame. The relative distance is expressed as the difference
between absolute positions.
???? = ???? ????? (A.12)
The second time derivative yields the expression for relative acceleration.
Figure A.2: The ECI and ECEF frames share an origin and Z axis and the two points, A and B, are objects
moving above the Earth.
Z
i
, Z
e
?
i e
X
i
Y
i
Y
e
X
e
A
I , E
r
E A
r
E B
r
A B
B
141
? ??? = ? ??? ?? ??? (A.13)
Applying Equation (A.11) results in the following:
? ??? = ??????? ???? ? 2???? ? ??? ????? ???? ????
? ??????? ???? ? 2???? ? ??? ????? ???? ????
(A.14)
? ??? = ??????? ???????? + ??? ???? + ?2???? ? ??? ????? ???? ???? (A.15)
Note the body frame specific forces are expressed with their appropriate
transformation matrix. Gravity effects will be essentially identical for two platforms
operating in the same region at similar altitudes (??? ? ??? ). Therefore, gravity effects are
differenced out. Additionally, the square of the Earth's rotation rate is on the order of
10?9, so the centripetal effects are negligible. The resulting expression for relative
acceleration is as follows:
? ??? = ??????? ???????? ? 2???? ? ??? (A.16)
The expression can be further simplified when relative velocities are small or when the
last term is negated by system noise, as in the case when all but the highest of quality
IMUs are used. The result is the difference in the body frame accelerations after they
have been rotated into a common frame.
? ??? = ??????? ???????? (A.17)
142
Appendix B
Robotic Convoy Experimental Setup
B.1 HARDWARE
Figure B.1: The robotic convoy consisted of a human driven Hyundai Santa Fe and an ATV Corp. Prowler that
was converted to a UGV.
Figure B.1 shows the vehicles used in the convoy experiment. A Hyundai Santa Fe was
the man driven lead vehicle, and an automated ATV Corp Prowler was the following
vehicle. Tests were conducted at Auburn University's National Center for Asphalt
143
Technology's (NCAT) test track, which is a closed 1.7 mile oval test course with 300 m
diameter, 8 degree banked turns. A picture of the test track is shown in Figure B.2.
Figure B.2: The NCAT test track as seen in Google Earth.
Each vehicle had a NovAtel PropakV3 GPS receiver to record both raw
measurements and an RTK position. A Crossbow IMU440, outputting inertial
measurements at 20 Hz, was mounted on the Prowler and used in its onboard navigation
system, which was also implemented in C++. Two 900MHz Digi radio modems were
used in each vehicle; one received the RTK corrections from a Septentrio PolaRx2e at the
base station, and the other transmitted the necessary information required by the DRTK
algorithm between vehicles. Each modem pair operated on different channels, and the
RTK solution was only used as a measurement of truth for post processed data analysis.
The DRTK and TDCP algorithms were both run at 5 Hz.
B.2 OPERATION
A proportionalderivative (PD) controller was used to steer the Prowler, with commands
being issued at 20 Hz. The PD gains were scheduled based on vehicle speed. Heading
144
error was computed by differencing the reference heading from an estimate of the
vehicle's heading. The time derivative of the heading error is analytically expressed as
follows:
? ? = ???????? ??????????
???2 + ????2
(B.1)
From a practical standpoint, it is easier to compute the time derivative of the heading
error by differencing the heading error across successive iterations and dividing by the
time interval. The heading error derivative contained large deviations corresponding to
changes in the selected reference point. The error derivative was filtered with a median
filter to remove the inconsistencies and provide a smoother, more realistic representation
of heading error rate to the controller.
Throttle was operated manually during the tests as the purpose was to determine
the path following capability of the trailing vehicle using the GPS based relative
measurements as a reference. Following distances were not maintained to specific values
because emphasis was placed on path duplication ability. Many scenarios require the
following distance to fluctuate as the vehicles can be performing different maneuvers,
especially as the separation distance increases. For instance, the lead vehicle might be
traveling on a straight road at a relatively high speed, while the following vehicle is still
negotiating tight turns before entering the straight road. It would be unreasonable to
maintain a specific following distance in that scenario. Also, capabilities of vehicles in
the convoy will likely differ, and therefore speeds will differ causing separation distance
to vary as each type of vehicle negotiates terrain or performs maneuvers.
145
B.3 UGV NAVIGATION SYSTEM
A simple kinematic navigation system was written to provide the controller with a 20 Hz
heading estimate, which is necessary to operate autonomously. Other systems could be
implemented to describe other vehicle states, as long as they provide information to the
controller at a sufficient rate beyond that of the closed loop poles.
Estimated states were speed, longitudinal accelerometer bias, heading, yaw rate
gyroscope bias, and North and East position.
? = ?? ??? ?? ?? ? ??? ??? ? (B.2)
Inputs to the system were longitudinal acceleration and yaw rate.
? = ?? ? ? ? (B.3)
The nonlinear kinematic relationships in the navigation model, with white noise driving
the bias states, were as follows:
??? = ????1 + ?? ??? ??????1 (B.4)
???? = ?????1 (B.5)
??? = ????1 + ?? ? ?? ??? ??1 (B.6)
?? ? = ?? ??1 (B.7)
???
?
= ???
??1
+ ????1 cos ????1 (B.8)
???
?
= ???
??1
+ ????1 sin ????1 (B.9)
GPS measurements were translated to the local ENU frame and provided the estimator
with speed, course over ground, and North and East position information.
? = ? ? ?? ?? ? (B.10)
Course over ground was used as the heading measurement, which is a valid assumption
when the vehicle experiences little to no sideslip under normal operation. Note that
146
course measurement accuracy is inversely related to vehicle speed due to the method by
which it is obtained [9].
An extended Kalman filter was used to accommodate the nonlinear relationships
existing in the model. Execution of the time updated was based on the 20 Hz output rate
of the IMU, and the measurement update was performed at 5 Hz when GPS
measurements were available. The estimator output was a 20 Hz filtered solution of the
minimum states required for autonomous operation. However, only the heading estimate
was used for this work.
147
Appendix C
Towed Implement Positioning
C.1 INTRODUCTION
A robotic tractortrailer system is currently being developed at Auburn University in
conjunction with the U.S. Army Corps of Engineers and the Environmental Security
Technology Certification Program (ESTCP) for the purpose of locating and mapping
unexploded ordnance (UXO). It consists of a fourwheel differential drive robot (Segway
RMP) towing a trailer, which carries various geophysical equipment such as
magnetometers and electromagnetic sensors. The robot autonomously tows the trailer
along a predefined path to map a given area. A picture of the robot towing a Geonics
EM61MK2 electromagnetic sensor is shown in Figure C.1. A detailed description of the
system can be found in [24].
Figure C.1: A Segway RMP towing a trailer containing geophysical equipment.
148
Precise control of the position of the trailer is required in order to create an
accurate geophysical map. A control algorithm has been designed for the robot to guide
the trailer along a path consisting of lines and circular arcs. Precise knowledge of the
trailer's current position are required for this algorithm to perform properly. Currently,
the position of the trailer is calculated from the position and orientation of the robot and
the angle between the robot and trailer using an encoder.
Another option for determining the trailer's position is to measure it directly using
an RTK GPS system. Traditionally, an RTK system involves a minimum of two GPS
receivers; one is statically stationed at a known location, and the other is allowed to roam.
The relative position vector is then combined with the known location of the base to
provide a global position that is accurate to within two centimeters. An RTK system is
relatively expensive, however, and requires that a base station be placed on a known
location. The DRTK algorithm described in Chapter 2 removes the constraint of the
static base while retaining the high accuracy relative position estimate. When applied to
the system detailed above, the trailer's position relative to the robot can be estimated
using the DRTK method.
In this appendix, methods are described that allow the position of the trailer to be
determined using both an encoder and the DRTK algorithm. Experiments are carried out
using both methods and the accuracies are compared. Results show a marginal accuracy
improvement over a properly calibrated encoder based method, but a significant
improvement is seen when calibration errors are present.
149
C.2 ENCODER POSITIONING
Currently, the position of the robot is determined using a GPS/INS positioning system.
The positioning system consists of a NovAtel DL4Plus GPS receiver blended with a
HG1700 IMU using the NovAtel SPAN system. The GPS receiver operates in RTK
mode, receiving corrections from a static base station at a known location. The relative
angle between the robot and the trailer (hitch angle) is measured using an optical encoder.
The geometry of the system is known, so the position of the trailer can then be calculated
from the position of the robot and the hitch angle using the following equations:
?? = ?? ??? sin ?? ??? sin(??) (C.1)
?? = ?? ??? cos ?? ??? sin(??) (C.2)
?? = ?? + ? (C.3)
where (??, ??) and (??, ??) are the positions of the robot and trailer, respectively, in UTM
coordinates, ?? and ?? are the orientations of the robot and trailer respectively measured
clockwise with respect to north, and ? is the hitch angle. A schematic representation of
the system is given in Figure C.2.
150
Although the position and orientation of the robot are currently determined using
a GPS/INS system with RTK corrections, that is not the focus of this work; only the
accuracies in the relative measurement between the robot and trailer are examined. An
accurate measurement of the robot's global position and heading are assumed but could
come from GPS or other methods such as odometry, vision systems, etc.
The use of an encoder to determine the position of a trailer towed behind a robot
is a commonly used technique, but issues can arise that introduce errors potentially
causing significant problems if the precise location of the trailer is desired. It was
assumed earlier that the geometry of the system is known. This requires exact
measurements of the hitch and trailer lengths (?? and ??). Errors in measuring these
lengths result in an inaccurate estimate of the trailer's position. An accurate robot heading
and hitch angle measurement are also required for this method. One common source of
error is a bias in the encoder measurement, which can result from improper alignment,
joint backlashes, etc. The effects of these errors are studied extensively in [24]. Other
Figure C.2: A schematic model of a towedtrailer system.
?
r
?
r
v
r
v
t
?
t
?
l
r
l
t
151
more fundamental problems with this method can also affect the accuracy of the trailer
position. The hitch connecting the robot and trailer has three degrees of freedom (roll,
pitch, and yaw), but only one angle (yaw) is measured. When the trailer's position is
calculated, it is assumed that the robot and trailer are on smooth ground and therefore in
the same plane. The majority of experimental results given in the literature focus on
vehicles that operate on streets or in indoor environments so the assumption is generally
valid. On more varied terrain, which would be expected when performing outdoor
surveys, the assumption does not always hold. The hitch could be modified to allow all
three degrees of freedom to be measured but would require a more complex and
expensive hitch and sensing system.
C.3 DRTK POSITIONING
The hitch angle (?) and robot heading (??) measurements are not needed if the DRTK
method is used to estimate the trailer position. Instead, a second GPS antenna is placed
on the trailer. A baseline is estimated in the ECEF frame from the robot antenna to the
trailer antenna using the DRTK algorithm. The trailer's position, ??, is then determined by
simply adding the RPV, ???, to the robot's ECEF position, ??.
?? = ?? + ??? (C.4)
Once the trailer's ECEF position is known, it can be translated into different coordinate
frames that are more suitable for ground vehicle control. For this work, the resulting
trailer positions were translated into the UTM frame for use by the controller.
The DRTK method requires a sufficient number of GPS satellites that are visible
to both receivers. Any significant obstruction or interference can hinder the DRTK
algorithm if enough satellites cannot be seen by the receivers. In some environments,
152
such as forests, cities, or canyons, this could be an issue as GPS availability is limited.
Integration of the algorithm with other sensors could improve its robustness by providing
a lower precision baseline estimate during the outage, thus reducing the integer ambiguity
acquisition time when the GPS signal is visible again.
C.4 DRTK VERSUS ENCODER METHOD
The accuracy of the trailer position estimate using the encoder based method and the
DRTK based method were compared experimentally. A path consisting of several
straight line segments connected by circular arcs in an Stype pattern was driven
autonomously for the experiments. Two NovAtel Propak V3 receivers were used to log
the range measurements, ephemerides, and user position, which are needed by the DRTK
algorithm. One was placed on the robot, and the other was placed on the trailer. The
signal from the antenna on the robot was split using a GPS Networking Inc. splitter, so
the DL4Plus and PropakV3 received the same signal. A Navcom 2050M was set up as a
static RTK base station, and the corrections were broadcast via 900 MHz radio modems
to the robot mounted DL4 and trailer mounted PropakV3. The RTK position of the
trailer was used as truth for analysis of the experimental data. The position of the trailer
was estimated using both the encoder and DRTK algorithms for each experiment. The
trailer's position was calculated in realtime using the encoder and the position
measurement from the DL4Plus mounted on the robot. This measurement was used by
the controller to guide the robot.
The result of one experiment (with parameters ??=0.83m and ??=2.73 m) is
presented in Figure C.3 and Figure C.4. The desired path, the estimated trailer position
using both methods, and the RTK truth measurement are shown in Figure C.3 and a
153
zoomed in portion is shown in Figure C.4. It can be observed that the DRTK estimate is
very close to the true position, while a small bias exists in the encoder estimate.
Figure C.3: The test path driven by the robot, which
was pulling a trailer.
Figure C.4: Zooming in reveals the precision of both
methods, but the DRTK position is more precise.
The difference between in the trailer's estimated position (determined with both
methods) and the truth data (from the second RTK receiver) is shown in Figure C.5.
Approximately 10 minutes of static data was taken at the beginning of each run to allow
sufficient time for the DRTK algorithm to converge since the algorithm was being run
postprocess and therefore the exact convergence time needed was unknown. The same
bias that was seen in Figure C.4 can also be seen in the plot of total error (Figure C.5,
particularly during the static data, where it appears that the bias is approximately 67cm.
This bias is most likely caused by a bias in the encoder measurement. This error could
possibly be reduced by more carefully aligning the encoder, but alignment can be a
difficult and time consuming process. While both methods perform well, the DRTK
method has a noticeably smaller error than the encoder method and does not require exact
knowledge of the robot geometry or careful encoder calibration.
0 10 20 30
0
10
20
30
E a s t ( m )
N
o
rt
h
(
m
)
 0 . 5 0 0 . 5
1 7 . 2
1 7 . 4
1 7 . 6
1 7 . 8
18
1 8 . 2
E a s t ( m )
N
o
rt
h
(
m
)
P a t h
T r u t h
D R T K
E n c o d e r
154
The total estimation error was also broken into lateral and longitudinal
components in the vehicle frame. The lateral error is of more importance than the
longitudinal error for the application to UXO detection. Longitudinal error will shift the
entire survey area in one direction, but will not affect the area covered within that area.
Errors in the lateral position, however, can cause gaps in coverage within the survey area.
The lateral and longitudinal error components as functions of time are shown in Figure
C.6, and a scatter plot of the lateral and longitudinal error is shown in Figure C.7. From
Figure C.7 it can be seen that he DRTK method is very accurate with the majority of the
points having a total error of less than 1 cm, while the errors from the encoder method are
on average 56 cm and can be as large as 25 cm.
Figure C.5: Total error in trailer position using the DRTK and encoder methods.
0 200 400 600 800 1000 1200 1400
 0 . 1
0
0 . 1
0 . 2
P
o
s
it
io
n
E
r
r
o
r
(
m
)
T i m e ( s )
D R T K
E n c o d e r
155
For the UXO system described previously, trailer lengths are varied often based
on the type and configuration of sensor being towed. Because of this, the trailer length
parameter (??) changes often. Note that the tongue length parameter (??) is determined by
the portion of the tongue that is a part of the robot and does not change. When a new
trailer configuration is used, the length is typically measured in the field using a tape
measure and entered into the control software. This process is prone to errors which,
when they occur, can significantly affect the performance of the system. An error was
made while measuring the trailer length during one of the experimental runs performed
for this study. The length entered was off by approximately 10 cm. The resulting lateral
and longitudinal error are shown in Figure C.8. Since the trailer length parameter is used
to calculate the trailer's position using Equations (C.1) and (C.2), the error in the
Figure C.6: Lateral and longitudinal trailer position error.
0 0 . 2 0 . 4 0 . 6 0 . 8 1 1 . 2 1 . 4
 0 . 2
0
0 . 2
L
a
t
E
r
r
o
r
(
m
)
0 0 . 2 0 . 4 0 . 6 0 . 8 1 1 . 2 1 . 4
 0 . 2
0
0 . 2
L
o
n
g
E
r
r
o
r
(
m
)
T i m e ( s )
D R T K E n c o d e r
156
parameter also appears in the trailer position estimate. While the error is easy to detect
when truth data is available, this information is not normally available and the error
would result in inaccurate path following and as a result an inaccurate map of the area
being surveyed. The DRTK method, however, does not require the trailer length
parameter and still provides an accurate measurement of the trailer position.
Figure C.7: A scatter plot reveals the accuracy of the
methods.
Figure C.8: A parameter error caused a reduction in
accuracy using the encoder method.
C.5 TRAILER LENGTH EFFECTS
The effect of changes in the trailer length was examined for both estimation methods.
The error in the DRTK measurement is expected to be constant for reasonable trailer
lengths. The error from the encoder method, however, should increase with trailer length.
The encoder method is sensitive to errors in both the hitch angle and the robot's
orientation. Both have the effect of biasing the trailer estimate to one side or the other of
the actual position. The sensitivity to these errors is proportional to the hitch and trailer
lengths; error increases with the length of the trailer.
157
The experiment described in Section C.4 was repeated for two additional trailer
lengths (??=0.91m and ??=1.83m). The total, lateral, and longitudinal errors were
calculated for both the DRTK and encoder methods for each trailer length. The mean, ? ,
and standard deviation, ??, were calculated for each error and the results are given in
Table 1 for the DRTK method and in Table 2 for the encoder method. As expected, the
errors using the DRTK method stayed essentially constant over the runs, while the errors
from the encoder method increased with trailer length.
Table C.1: Accuracy as trailer length varies
using the encoder and DRTK methods.
Trailer
Length
?? Error (cm)
Encoder DRTK
0.91 2.49 0.81
1.83 4.0 0.80
2.73 6.62 0.75
C.6 CONCLUSION
An application of the DRTK algorithm to a robot trailer system was described. It was
shown that the DRTK algorithm could be used to accurately determine the trailer's
position relative to the robot. This method of trailer position determination was
experimentally compared to a method that uses an optical encoder to measure the angle
between the robot and trailer.
The experimental results showed that the DRTK algorithm could provide a more
accurate measurement of the trailer's position than the method using an encoder in areas
where GPS coverage is sufficient. The degree of improvement over the encoder method
is directly related to the length of the trailer. The DRTK method also eliminates the need
for exact knowledge of the system's geometry, removing a possible source of human
error from the system.