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: leader-follower, 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:9-12 (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 narrow-lane 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 towed-trailer 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 Pseudo-Random 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 clock-like 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 tele-operated. 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 pseudo-random noise (PRN) sequences. Both a civilian code, known as the C/A-code (Clear or Coarse Acquisition) code, and a military code, known as the P(Y)- code (an encrypted version of the Protected code, or P-code) are modulated onto the L1 carrier frequency with designated PRN numbers [37]. PRN numbers 1-32 are assigned to 4 GPS SV's according to GPS-ICD200c [38]. Each C/A-code 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 P-code is extremely long relative to the C/A-code at 6.1871x1012 chips. The P-code is modulated with an encryption sequence known as the W-code to form the encrypted P(Y)-code, which is broadcast ten times faster than the C/A-code at 10.23 MHz. Most satellites broadcast only the encrypted P(Y)-code on the L2 frequency, but modernization efforts are incorporating a civilian L2C-code to aid in atmospheric error reduction and enhance accuracy [27]. Receivers cannot directly track the P(Y)-code without an encryption key, but semi-codeless 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/A-code 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 leader-follower. 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 leader-follower program survived with other former FCS programs under the Brigade Combat Team (BCT) Modernization. The leader-follower 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 leader-follower capabilities. The effort is led by General Dynamics Robotic Systems (GDRS) and strives to develop an integrated approach to the leader-follower 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 leader-follower 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 inter-vehicle 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 leader-follower 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 leader-follower 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 non-static 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 non-static 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 non-differential 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- of-sight (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 line-of-sight (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 North-East 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 multi-stage 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 narrow-lane observable with a shorter wavelength is created by adding the L1 and L2 observables. ? ?? = ? ?1? ?1 + ? ?2? ?2 ??? (2.18) The narrow-lane 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 narrow-laning is greater than the drawback of noise amplification. Figure 2.6 shows the relative difference in wavelength among the L1, L2, widelane, and narrow-lane observables. Figure 2.6: The widelane observable has a much longer wavelength than the L1 and L2 observables, which benefits ambiguity resolution. However, the narrow-lane 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 narrow-laned 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 non-symmetric. 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 ultra-wideband 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, non-base 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 PropakV-3 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 ProPakV-3 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 sub-centimeter 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 East-North 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 non-colocated 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 geometry-free wide-lane 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 PropakV-3 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 45-48 dB-Hz). 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 dB-Hz 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 re-initialization 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 non-coincident. 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. Pre-processing 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 pre-process 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 non-coincident, 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 non-orthogonality. Misalignment and non-orthogonal 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 Gauss-Markov 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.0e-2 60 1.2e-2 2.4e-7 100 8.7e-4 Tactical 5.0e-3 100 5.0e-4 8.2e-9 300 1.7e-6 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 range-rate 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 10-19 2 x 10-20 Ovenized Crystal 8 x 10-20 4 x 10-23 Rubidium 2 x 10-20 4 x 10-29 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 pseudo-range and pseudo- range rate data with inertial data to produce a position solution. The specific force expressed in the navigation frame is a by-product 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, re-initialization 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 re-initialized 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 PropakV-3?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 non-coincident 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 re-initialization 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 East-North 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 de-weighted. 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 un-modeled 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 Propak-V3, 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 one-sigma 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 PropakV-3 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 non-detrimental 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 k-n to the current index k. These values are then subtracted from the corresponding vector components of the RPV at index k-n to determine the RPV between the follower at time k and leader at time k-n. 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 un-modeled 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 re-drive 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 in-tow 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 non-RTK 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 non-concentric 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 inter-vehicle 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 pass-fail 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. 213-224. [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. 455-460. [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. 970-977. [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. 139-163, 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. 706-717. [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. 270-277, 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. 2085-2093. [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. 2871-2881. [13] P Enge, "Local Area Augmentation of GPS for the Precision Approach of Aircraft," Proceedings of the IEEE, vol. 87, no. 1, pp. 111-132, 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. 958-967, 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. 979-988. [17] A.M. Fosbury and J.L. Crassidis, "Relative Navigation of Air Vehicles," Journal of Guidance, Control, and Dynamics, vol. 31, no. 4, pp. 824-834, 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. 30-36, May/June 2009. [19] D. Gebre-Egziabher, "Design and Performance Analysis of a Low-Cost 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. 1323-1334. [24] D.W. Hodo, "Development of an Autonomous Mobile Robot-Trailer System for UXO Detection," Auburn University, Auburn, Alabama, USA, M.S. Thesis 2007. [25] B. Hofmann-Wellenhof, H. Lichtenegger, and J. Collins, GPS: Theory and Practice, Fourth Edition ed.: Springer-Verlag, 2001. [26] P. Joosten and C. Tiberius, "LAMBDA: FAQs," GPS Solutions, vol. 6, pp. 109-114, 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 Heavy-Duty 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. 903-910. [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. 3024-3034. [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. 709-731, 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. 1026-1037. [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. 117-138, June 1979. [37] P. Misra and P. Enge, Global Positioning System: Signals, Measurements, and Performance, 2nd ed. Lincoln, Massachusettes, USA: Ganga-Jamuna Press, 2006. [38] "Navstar GPS Space Segment / Navigation User Interfaces," ARINC Research Corporation, El Segundo, CA, ICD-GPS-200c, 2000. [39] A. Nevin and D.M. Bevly, "Landmark Extraction Using Corner Detection and K- Means Clustering for Autonomous Leader-Follower Caravan," in Proceedings of the Twelfth IASTED International Conference, Cambridge, Massachusettes, USA, 2009, pp. 88-92. [40] T.C. Ng, J.I. Guzman, and M.D. Adams, "Autonomous Vehicle-Following 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. 726-737. [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. 24-37, 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. 181-191, 2003. [47] M.G. Petovello, "Real-Time Integration of a Tactical-Grade 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. 219-229. [49] M.W. Price and S. Munkeby, "Future Combat Systems (FCS) Autonomous Navigation System (ANS) Technology Will Revolutionize Warfare," Army AL&T, pp. 24-26, April-June 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. 2483-2488. [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. 386-400, 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. 74-78, July-September 2009. [54] B.K. Soon, S. Scheding, Hy.K. Lee, Hu.K. Lee, and H. Durrant-Whyte, "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 Least-Squares Ambiguity Decorrelation Adjustment: A Method for Fast GPS Integer Ambiguity Estimation," Journal of Geodesy, vol. 70, pp. 65-82, 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. PLEN41-PLEN45. [58] S. Verhagen, "The GNSS Integer Ambiguities: Estimation and Validation," TU Delft, Delft, The Netherlands, Ph.D. Dissertation ISBN: 90-804147-4-3, 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. 54-60. [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. 1548-1552. 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 X-Y 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 PropakV-3 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 proportional-derivative (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 tractor-trailer 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 four-wheel 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 EM61-MK2 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 DL4-Plus 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 towed-trailer 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 S-type pattern was driven autonomously for the experiments. Two NovAtel Propak V-3 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 DL4-Plus and PropakV-3 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 DL-4 and trailer mounted PropakV-3. 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 real-time using the encoder and the position measurement from the DL4-Plus 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 post-process 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 6-7cm. 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 5-6 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.