Integration of GPS-aided INS into AVCSS

0 downloads 0 Views 2MB Size Report
Dec 1, 2000 - Automobiles--Automatic control, Global Positioning System, Inertial navigation systems, .... on-vehicle carrier phase GPS-aided INS closed loop control. ..... landing systems (ILS) is that the probability of undetected guidance ...
California Partners for Advanced Transit and Highways (PATH) UC Berkeley

Title: Integration of GPS-aided INS into AVCSS Author: Farrell, Jay Barth, Matthew Publication Date: 12-01-2000 Series: Research Reports Publication Info: Research Reports, California Partners for Advanced Transit and Highways (PATH), Institute of Transportation Studies, UC Berkeley Permalink: http://escholarship.org/uc/item/21c4t39w Keywords: Automobiles--Automatic control, Global Positioning System, Inertial navigation systems, Advanced vehicle control systems Abstract: This report describes the results of an effort to implement and analyze the performance a vehicle control system using control state information obtained from a carrier phase Differential Global Positioning System (DGPS) aided Inertial Navigation System (INS). Keywords: Vehicle Positioning Systems, Global Positioning System, Inertial Navigation, Differential Carrier Phase, Advanced Vehicle Control

eScholarship provides open access, scholarly publishing services to the University of California and delivers a dynamic research platform to scholars worldwide.

This paper uses Postscript Type 3 fonts. Although reading it on the screen is difficult it will print out just fine.

CALIFORNIA PATH PROGRAM INSTITUTE OF TRANSPORTATION STUDIES UNIVERSITY OF CALIFORNIA, BERKELEY

Integration of GPS-aided INS into AVCSS Jay Farrell, Matthew Barth University of California, Riverside

California PATH Research Report

UCB-ITS-PRR-2000-22

This work was performed as part of the California PATH Program of the University of California, in cooperation with the State of California Business, Transportation, and Housing Agency, Department of Transportation; and the United States Department of Transportation, Federal Highway Administration. The contents of this report reflect the views of the authors who are responsible for the facts and the accuracy of the data presented herein. The contents do not necessarily reflect the official views or policies of the State of California. This report does not constitute a standard, specification, or regulation. Report for MOU 374

December 2000 ISSN 1055-1425

CALIFORNIA PARTNERS FOR ADVANCED TRANSIT AND HIGHWAYS

Integration of GPS-aided INS into AVCSS MOU 374{Final Report

Principal Investigator: Jay Farrell Department of Electrical Engineering University of California, Riverside [email protected] Co-Principal Investigator: Matthew Barth Center for Environmental Research and Technology University of California, Riverside [email protected] October 30, 2000

Abstract This report describes the results of an e ort to implement and analyze the performance a vehicle control system using control state information obtained from a carrier phase Di erential Global Positioning System (DGPS) aided Inertial Navigation System (INS). The initial premise of this project was that DGPS/INS technology has the potential to serve as a centimeter-level position reference system as necessary for automated driving functions. As a standalone system, key advantages of this approach include: 1) no changes to the highway infrastructure are required; therefore, the DGPS/INS system should be less expensive to install and maintain than alternative reference systems; 2) knowledge of the DGPS position of a vehicle allows the use of signi cantly more path preview information than alternative position reference systems; therefore, control performance could be improved; 3) the navigation system would output the full vehicle state (position, velocity, and attitude) and inertial measurements at signi cantly higher rates than alternative navigation systems would provide even a subset of the vehicle state. However, as a standalone system, the GPS/INS approach (or any other non-triplicate sensing approach) will not have the integrity required for AVCS applications. The primary objectives of this study (MOU 374) were to integrate the DGPS/INS carrier phase system with the PATH vehicle control system and to demonstrate and evaluate the resulting integrated system control performance. The testing was completed with the magnetometer sensing system running so that it could provide a ground truth reference for comparison with the DGPS/INS. No e ort was made under MOU 374 to fuse the magnetometer and DGPS/INS information. The fusion is being pursued under MOU 391, which began in November 1999. Under MOU 374, performance was successfully demonstrated with lateral position accuracy at the centimeter level and with the full navigation state updated at 150 Hz. Communication with the control system occurred at 30 Hz. An in-depth analysis is presented in Section 5. Keywords: Vehicle Positioning Systems, Global Positioning System, Inertial Navigation, Di erential Carrier Phase, Advanced Vehicle Control

Executive Summary

Various sensors have been considered within the context of Automated Vehicle Control and Safety Systems (AVCSS). Each sensor has its advantages and disadvantages. In addition, no single sensor technology will be able to attain the high level of reliability necessary for automated vehicle deployment. Therefore, some integrated suite of sensors will be required. An integrated sensing system involving GPS-aided INS (Global Positioning System-aided Inertial Navigation System) and additional sensors (e.g., magnetometers) has several distinct advantages: 1. High-Sample Rate - The INS system is capable of providing state variable estimates at greater than 150Hz, signi cantly faster than the magnetometer or GPS systems could alone. The sample rate is also independent of vehicle velocity. Higher sample rates allow for better vehicle control. 2. Triple Redundancy - Safe vehicle operation will require the ability to detect, isolate, and accommodate sensor failures. Reliable sensor fault isolation requires at least triplicate redundancy. No single sensing system will be capable of providing the integrity necessary for reliable vehicle control over a highway system. 3. Preview Information - Although road preview information can be encoded in the magnetometer-based system, a GPS based approach used in conjunction with an on-board roadway database allows preview of curvature, super-elevation, velocity pro les, entrance/exit trajectories, etc. 4. Reduced Infrastructure Cost - Since the GPS-aided INS provides redundant control information, it may be possible to increase the magnet spacing and reduce the number of magnets used per mile, thus reducing overall infrastructure installment and maintenance costs. 5. Richer State Information - The GPS-aided INS not only provides o -track position information, but also provides additional variables for high performance vehicle control (e.g., position, velocity, acceleration, attitude and angular rates). This information not only allows improved control in normal operation, but may be necessary in more demanding emergency situations. 6. Advanced Maneuver Capability - Using the magnetometer-based system alone constrains the closed loop vehicle lateral control to operate within 0.4 m of the path of magnets. By integrating GPS-aided INS, automated closed loop vehicle control will also be possible for advanced maneuvers, such as lane changing, AHS entry and exit, platoon merging, etc. See the test results of Section 5.3.2. This report describes the results of MOU #374 that integrated the carrier-phase di erential GPS-aided INS developed at UCR under MOU #292 into current AVCSS work being performed at PATH. The GPS-aided INS system provides high sample rate (currently fs = 150 Hertz) vehicle state information (i.e., position, velocity, acceleration, attitude, and angular rate). The demonstrated positioning accuracy under PATH MOU #292 was approximately 2.5 cm (lateral error std). MOU #374 capitalized on the previous control work by PATH and GPS-aided INS work at UCR. Speci cally, UCR collaborated with the PATH vehicle control group to integrate the GPS-aided INS into the current vehicle control system and demonstrate the on-vehicle carrier phase GPS-aided INS closed loop control. To decrease expense and allow analysis of the characteristics of the GPS-aided INS control, the project demonstrated vehicle control based solely on GPS-aided INS vehicle state information. We maintained the magnetometer software and hardware on-board, for ground truth veri cation. In future research, a multi-sensor system is of interest that integrates the various forms of sensor information. i

Contents 1 Project Introduction

1

2 Project Scope and Objectives

4

1.1 Global Positioning System and Vehicle Control . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.2 Inertial Navigation and Vehicle Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.3 Redundant Positioning Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1 Scope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 2.2 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 2.3 Project Objective . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3 Methodology

3.1 DGPS Aided INS . . . . . . . . 3.2 Overall System Design . . . . . 3.2.1 Hardware Description . 3.2.2 Software Description . .

.. .. .. ..

. . . .

.. .. .. ..

.. .. .. ..

.. .. .. ..

4 Task Descriptions 5 Performance Analysis

5.1 Stationary Data . . . . . . . . . . . . . . . . . . 5.2 Magnetometer Control Tests . . . . . . . . . . 5.3 GPS/INS Control Tests . . . . . . . . . . . . . 5.3.1 Basic Maneuvering . . . . . . . . . . . . 5.3.2 Advanced Maneuvering { Sinusoids . . . 5.3.3 Advanced Maneuvering { Lane Change

. . . .

. . . . . .

.. .. .. ..

.. .. .. .. .. ..

.. .. .. ..

.. .. .. .. .. ..

.. .. .. ..

.. .. .. .. .. ..

. . . .

. . . . . .

.. .. .. ..

.. .. .. .. .. ..

.. .. .. ..

.. .. .. .. .. ..

.. .. .. ..

.. .. .. .. .. ..

. . . .

. . . . . .

.. .. .. ..

.. .. .. .. .. ..

.. .. .. ..

.. .. .. .. .. ..

.. .. .. ..

.. .. .. .. .. ..

. . . .

. . . . . .

.. .. .. ..

.. .. .. .. .. ..

.. .. .. ..

.. .. .. .. .. ..

6

6 7 7 8

10 12

12 13 14 14 15 15

6 Conclusions and Future Research

31

A Appendix: INS Implementation

36

6.1 Future Research . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31 6.2 Publications Resulting from this Project . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

A.1 Inertial Navigation System (INS) . . . . . . . . . . . A.1.1 Tangent Plane Mechanization Equations . . . A.1.2 Tangent Plane INS Nominal Error Equations A.1.3 Numeric Integration . . . . . . . . . . . . . . ii

.. .. .. ..

.. .. .. ..

. . . .

.. .. .. ..

.. .. .. ..

.. .. .. ..

. . . .

.. .. .. ..

.. .. .. ..

.. .. .. ..

. . . .

.. .. .. ..

.. .. .. ..

36 36 37 39

B GPS Methodology

40

C Control State Calculation

51

B.1 Di erential Global Positioning (DGPS) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40 B.1.1 GPS Observables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40 B.1.2 Di erential GPS Operation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47 C.1 Review of Analytic Geometry . . . C.2 Curve tting data set. . . . . . . . C.3 Curve tting . . . . . . . . . . . . C.3.1 Control State Calculation .

. . . .

.. .. .. ..

.. .. .. ..

.. .. .. ..

D Appendix: Constant and Notation De nition

. . . .

.. .. .. ..

.. .. .. ..

.. .. .. ..

. . . .

.. .. .. ..

.. .. .. ..

.. .. .. ..

. . . .

.. .. .. ..

.. .. .. ..

.. .. .. ..

. . . .

.. .. .. ..

.. .. .. ..

52 53 53 54

59

D.1 Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59 D.2 Constants . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

iii

Chapter 1

Project Introduction Automated vehicle position control systems for an AHS require both a means for determining vehicle position and a means for controlling the vehicle position [41, 18]. The vehicle position may be determined in either relative (e.g., position relative to nearby known point) or absolute (e.g., latitude, longitude, altitude) coordinates. A vehicle control system is then designed to maneuver the vehicle along a speci ed trajectory in the speci ed coordinate system. A variety of reference positioning systems may be considered: embedded wires [18, 9, 19], embedded magnets [41, 46], radar [18, 33], vision [28, 21, 10, 11, 32, 40, 43], INS and DGPS technology. This e ort has focused on research to develop, analyze, and demonstrate a DGPS aided INS with accuracy and sample rates suÆcient for vehicle control.

1.1 Global Positioning System and Vehicle Control The Global Positioning System [1] is a convenient and accurate method for determining vehicle position in a global coordinate system. The system is built around a constellation of 24 satellites already orbiting the earth. The orbits are designed in a manner that allows the signals from at least four satellites to be received simultaneously at any point on the surface of the earth with an unobstructed via of the sky. Each satellite broadcasts data allowing a receiver to calculate the satellite (x,y,z) position and receiver-to-satellite range. A GPS receiver can use four sets of these signals to determine its own (x,y,z) position (see Appendix B). GPS navigation systems are currently used in many applications including aircraft [35] and ships [39]. The standard accuracy of GPS position estimates is on the order of 20 meters. Increased accuracy (at the meter level) can be achieved through the use of Di erential GPS (DGPS) [35, 27, 45, 4, 5, 12, 17]. Furthermore, a DGPS system that uses carrier phase observations can provide accuracies of 1 to 3 cm. Appropriate techniques are discussed in the appendices of this report. For vehicle control, a trajectory corresponding to the desired path (i.e., the automated highway lane trajectory) would be de ned in the global coordinate reference frame and stored on-board the vehicle. Additional information such as the road inclination, turning radius and direction, and super-elevation angle could also be stored on-board (using CD technology) as a function of roadway position to improve control performance. The vehicle control system would then drive the steering and propulsion actuation systems to force the vehicle to follow the desired trajectory. Advantages of working with an absolute or global coordinate system are: 1. both lateral and longitudinal position information relative to the desired trajectory are known at all 1

times (regardless of the distance from the trajectory); 2. the vehicle can always return to the desired path since it always knows its position and the desired position; 3. signi cant amounts of path preview information can be used to facilitate control; 4. the vehicle velocity pro le as a function of global position can be speci ed (or planned on-line); and 5. the global position of the vehicle is available for other purposes (e.g., traveler information, traÆc management). Additional advantages of using the Navstar Global Positioning System include 1. no changes are required to the roadway infrastructure; 2. DGPS can determine 3-dimensional vehicle attitude; and 3. DGPS signals are available in all weather conditions. The main drawbacks of a DGPS system are low update rates, latency, and that the satellite signals can be blocked by obstacles (trees, tunnels, buildings). The rst two drawbacks can be overcome by receiver technology alone. All three drawbacks can be e ectively addressed via inertial navigation systems (see below). See Appendix B for a detailed discussion of GPS methods.

1.2 Inertial Navigation and Vehicle Control For control purposes, it is desirable to know the position and additional vehicle state and inertial information (i.e., position, attitude, velocity, angular rates, linear acceleration) at high ( 100 Hz) sampling rates. Inertial Navigation Systems (INS) [6, 7, 16] have been developed and are used in air, land, and sea applications. Such systems are capable of providing the full vehicle state information at rates suitable for accurate vehicle control, even in applications where some of the sensors are sampled at slower rates. A typical INS system integrates the di erential equation describing the system kinematics for a short period of time using high rate data from a set of inertial instruments. During this integration process, the error variance of the navigation state increases primarily due to sensor noise and errors in sensor calibration and alignment. After a period of integration, aiding sensor measurements (e.g., GPS) can be used to correct the state estimates. The main advantages of INS systems are: 1. full vehicle state estimates are produced without di erentiation (i.e., low sensitivity to high frequency noise); 2. the update rate of the navigation state estimates are not limited by the update rates of the aiding sensors; and 3. the accuracy of the navigation state is not a ected by variables or elds external to the navigation system. The main disadvantage of an unaided INS system is unbounded growth in the position estimation error of the vehicle; however, an INS system used in conjunction with aiding sensors can provide the full state estimates at the desired control frequency more accurately than either technique used individually. In addition, the INS would continue to provide position estimates at times when signals from the aiding sensors were not available. 2

1.3 Redundant Positioning Systems Taken independently, any positioning system of interest has advantages and disadvantages. The strongest criticism of any of the individual techniques is that it is susceptible to a single point failure. However, used jointly (with e ective sensor fusion and fault detection techniques) the overall performance and reliability of the system could be signi cantly improved. No single reference system can supply adequate reliability and availability to ensure safe longitudinal and lateral control. For example, the requirements for instrument landing systems (ILS) is that the probability of undetected guidance error be less than 3:3  10 per landing for the entire system, ground and airborne [38]. While present highway safety could be enhanced with a lower reliability level (Interstate highways have a fatal accident every 2:4  10 hours of driving and an injury accident every 58,000 hours), there are political and legal considerations that will push the guidance portions of AHS towards a comparable level of reliability. As in aviation, there will be more tolerance for accidents resulting from human errors and mechanical problems than for accidents resulting from guidance or computer software errors. Since no single black box is likely to have a reliability/availability of better than 0.9999, we can expect that at least two and likely three reference systems will be required. Thus DGPS needs to be seriously considered along with the other referencing systems if three viable independent systems are to be available for AHS. Consider, for example, a system incorporating an embedded magnet reference system (EMRS), DGPS positioning, and an Inertial Navigation System (INS). A desired path would be speci ed in global coordinates to pass through the locations of the embedded markers that specify an automated lane. The INS would provide estimates of state and position relative to the desired trajectory at a rate high enough to satisfy control system requirements, even though the EMRS and GPS measurements occurred at a slower rate. While near the desired trajectory, the three available redundant estimates of vehicle position relative to the desired path would allow e ective fault detection and isolation. When the vehicle was signi cantly o the desired trajectory (e.g., lane changing, entering/leaving a platoon, initialization, disturbances), where the EMRS losses accuracy, the DGPS and INS systems would still provide the accurate position information necessary to complete the maneuver of interest. In addition, knowledge of global vehicle position would facilitate both the process of negotiating maneuvers with neighboring vehicles and the process of determining relative vehicle position and velocity. 9

6

3

Chapter 2

Project Scope and Objectives 2.1 Scope The research of MOU 374 built upon the DGPS-aided INS system developed under PATH MOU 292. In that project, we developed a di erential GPS carrier phase aided INS for use in AVCSS. In this project (i.e., MOU 374), UCR and the PATH vehicle control group collaborated to integrate and evaluate the di erential GPS carrier phase aided INS in a PATH instrumented vehicle. The integrated system was demonstrated in controlled experiments.

2.2 Motivation The GPS-aided INS system being developed has been demonstrated to provide high sample rate (fs = 150 Hz) vehicle state information (i.e., position, velocity, attitude) at high accuracy. An integrated sensing system involving GPS, INS, and magnetometers has several distinct advantages: 1) High-Sample Rate -The INS system is capable of providing state variable estimates at greater than one hundred Hertz, signi cantly faster than the magnetometer or GPS systems could alone. The sample rate is also independent of vehicle velocity. Higher sample rates allow for better vehicle control. 2) Triple Redundancy - Safe vehicle operation will require the ability to detect, isolate, and accommodate sensor failures. Reliable sensor fault isolation requires triplicate redundancy. No single sensing system will be capable of providing the integrity necessary for reliable vehicle control over a highway system. 3) Preview Information- Although road preview information can be encoded in a magnetometer-based system, a GPS based approach used in conjunction with an on-board roadway database allows preview of curvature, super-elevation, velocity pro les, entrance/exit trajectories, etc. 4) Reduced Infrastructure Cost - Since the GPS-aided INS provides redundant control information, it may be possible to increase the magnet spacing and reduce the number of magnets used per mile, thus reducing overall infrastructure cost. 5) Richer State Information- The GPS-aided INS not only provides o -track position information, but also provides additional variables for high performance vehicle control (e.g., position, velocity, acceleration, attitude and angular rates). This information not only allows improved control in normal operation, but may be necessary in more demanding emergency situations. 6) Advanced Maneuver Capability - Using the magnetometer-based system alone constrains closed loop vehicle lateral control to operate within 40 cm of the path of magnets. By integrating GPS-aided INS, closed loop vehicle control will also be possible regardless of the o -trajectory distance for advance maneuvers, such as lane 4

changing, AHS entry and exit, platoon merging, etc.

2.3 Project Objective The objective of this project was to demonstrate and evaluate GPS-aided INS based vehicle control. The project built upon previous investments by the PATH program in vehicle control and GPS-aided INS technology by: 1) providing the rst demonstration of vehicle control based on a di erential GPS carrier phase aided INS; and 2) evaluating GPS-aided INS based vehicle control relative to the performance and reliability requirements of an automated highway system. As AHS and intelligent vehicle systems mature, informed decisions will have to be made relative to which sensor suites provide an optimal tradeo between system cost, reliability, and performance. At this early point in the de nition of Automated Vehicle Control and Safety Systems (AVCSS), it is crucial to understand the bene ts and drawbacks of the several candidate sensing technologies.

5

Chapter 3

Methodology The navigation system developed for this project incorporates a strapdown inertial navigation system with various modes of DGPS aiding. The following sections provide an overview of the system design. Section 3.1 discusses the use of DGPS as an INS aid. Section 3.2 speci es the remainder of the overall system design. Speci cs of the approach are included in the appendices. Appendix A discusses the INS implementation and error equations. Appendix B discusses various issues related to obtaining high accuracy position information from the GPS system.

3.1 DGPS Aided INS Figure 3.1 shows a block diagram of the DGPS aided INS implemented for this project. This implementation is referred to as a complementary lter. The noisy inertial measurement unit (IMU) outputs are processed by the INS. Since the INS is an integrative process, the output of the navigation system can be accurately modeled as the actual state plus a predominantly low frequency error. The INS output is processed to provide an estimate of the DGPS measurement. The di erence between the estimated DGPS output and the measured DGPS output is a signal that contains two noise components|the predominantly low frequency INS component and the predominantly high frequency DGPS component. The frequency content of each noise component can be accurately modeled as described in the appendices. The objective of the state estimation design is to attenuate the DGPS measurement error and provide an accurate estimate of the INS state error denoted by Æx. Therefore, the state estimator has a predominantly low pass characteristic. Subtracting the (extended Kalman lter) error estimate from the INS output, in a well designed system, produces an accurate estimate of the navigation state. In the complementary lter approach, the INS is the primary navigation system which calculates the navigation state at the high rate at which it is used for control, guidance, and planning functions. The DGPS aiding information is used when it is available and satis es conditions designed to verify proper sensor operation. When such aiding sensor information is not available or judged inaccurate, the INS continues its normal (unaided) operation (i.e., transformation and integration of the IMU outputs). The error state estimator is implemented by a Kalman lter based on the linearized error dynamics presented in Section A.1.2 and the linearized observation matrix of eqn. (B.7). All driving noise spectral density strengths were estimated from experimental data. The main advantages of the complementary lter approach selected for this implementation are that 6

INS

IMU

p,v,q r (p) ~ + r +_ DGPS

d x EKF

Figure 3.1: Complementary Filter 1. the high rate INS navigation outputs are available regardless of the availability of the GPS aiding information; 2. the inputs to the Kalman lter can be accurately and properly modeled as stochastic processes, as appropriate for the technique; and, 3. the computationally intensive Kalman lter covariance propagation equations can be implemented at a low update rate even though the navigation state is calculated at (or above) the desired rate of the control system. The complementary lter is implemented in `feedback' con guration. This means that the INS state is corrected for the Kalman lter error estimates after each DGPS update.

3.2 Overall System Design This section describes the hardware and software speci cations for the implemented navigation system. Algorithms for calculating the trajectory relative control state are discussed in Appendix C. 3.2.1 Hardware Description

The inertial instruments consist of one Crossbow DMU-6X which includes a three-axis 2g solid-state accelerometer (100 Hz bandwidth) and a three-axis 100 degs solid-state gyro (10 Hz bandwidth). The DMU-6X performs anti-alias ltering, analog-to-digital conversion, start-up bias correction, axis-misalignment correction, and outputs the set of six inertial measurements in serial format at approximately 150 Hz. This instrument includes inexpensive solid state devices similar to the type of instruments expected in commercial automotive applications. A 266 MHz Pentium notebook implemented the GPS and INS computations. The GPS hardware consisted of two Novatel RT-2 receivers. The DGPS base station to rover serial port connection was implemented through a 115k baud Freewave radio modem connection. The DGPS base station implementation is discussed in [15]. Magnetometer, speedometer, odometer, and brake pulse information were not used in this implementation to aid the navigation system. The magnetometer system was running during on vehicle testing and that data was saved for comparison (i.e., ground truth) with the DGPS carrier phase aided INS data. MOU #391 is currently working on the problem of integrating magnetometer and brake pulse derived di erential 7

wheel speeds with the DGPS carrier phase aided INS to attain the system integrity desired for commercial applications. 3.2.2 Software Description

The INS implemented a xed tangent plane system at 150 Hz . The origin was xed at the location of the based station antenna phase center. The navigation states included: north, east, and vertical (up { positive) positions in meters; north, east, and down velocity in meters per second; roll, pitch, and yaw angles in radians; receiver clock error and drift rate in meters and meters per second; platform frame gyro drift rates in radians per second; and, platform frame accelerometer bias in meters per second per second. The navigation error states were identical with the exception of the attitude errors. The estimate attitude errors were estimated in navigation frame as the north, east, and down tilt errors. The INS was implemented as an interrupt driven background process to ensure the designed update rate. GPS aiding was implemented at a 1.0 Hz rate with scalar measurement processing. Four primary modes of GPS aiding were implemented: 1. INS only: This is the default mode of operation. Since INS is implemented as a background process, it will continue to run at 150 Hz regardless of the availability of aiding information. When GPS measurements are available, the software automatically switches to Mode 2. 2. Di erential Pseudo-range: This is the default start-up mode. The primary objectives of this mode are to accurately estimate the navigation state errors and to switch to the widelane phase mode. Therefore, in this mode the system begins a search and veri cation process for the widelane integer ambiguities using the algorithm of Section B.1.1 with  = w . In parallel with the integer ambiguity search, the Kalman lter estimates the navigation state on the basis of the di erentially corrected pseudo-range measurements. In this mode, the measurement noise covariance for each measurement is set to R = 2:0 m . When the search process is complete, the software automatically switches to Mode 3. 3. Di erential Widelane Phase: In this mode, the software attempts to estimate and verify the L1 integer ambiguities using the algorithm of Section B.1.1 with  =  . In parallel with this integer search, the Kalman lter estimates the navigation state errors using the di erentially corrected widelane phase measurements. In this mode, the measurement noise covariance for each measurement is set to R = 0:1 m . When this search process is complete, the software automatically switches to Mode 4. If widelock to at least four satellites is lost, the system automatically reverts to Mode 2. 4. Di erential L1 Phase: This is the desired system operating mode. To be in this mode, the system will have estimated and veri ed the L1 integer ambiguities for at least four satellites. While in this mode, the Kalman lter will estimate the navigation error state using the di erentially corrected L1 phase measurements. The measurement noise covariance for each L1 phase measurement is set to R = 0:01 m. The INS is running in the background in all four modes. Also, the GPS measurement matrix in all GPS related modes is speci ed in equation (B.7). The di erentially corrected Doppler signal is also used in each mode of system operation. 1

2

2

1

2

2

2

2

Higher rates could be achieved with the current hardware and software, other than the limitation of the Crossbow serial interface. 1

8

In modes 3 and 4, the system monitors for loss of lock on each of the `locked' satellites. If lock is lost on the satellite, then the Kalman lter will utilize the di erentially corrected pseudo-range for that satellite instead of the corresponding phase measurement. The measurement information is correctly weighted by the Kalman lter by speci cation of the R matrix. As long as the system has lock to at least four satellites, it is usually able to correctly determine the integer ambiguities for `unlocked' satellites by direct estimation instead of search. While in Mode 4, the system will search for L2 integer ambiguities. Once estimated, the L2 phase range is used as a consistency check on the L1 and widelane phase ranges. The consistency check is that the three ranges are statistically equal and that the integers satisfy Nw = N N . The GPS receiver supplies a pulse aligned with the time of applicability of the GPS measurements. Receipt of this pulse by the INS computer causes the INS state to be saved and used for computation of the predicted GPS observables. The GPS measurements are received over the serial connection 0.1-0.3 seconds after the pulse (time of applicability). The delay is dependent on the number of satellites. Base corrections also transmitted serially do not arrive until approximately 0.5 seconds after the time of applicability of the GPS measurement. This left 0.5 seconds to complete the Kalman lter computation of the navigation error state and correct the navigation system prior to beginning the processing for the next measurement epoch. In the approach described above, the navigation error state applicable at the beginning of an epoch is not available until approximately 0.5 seconds after its time of applicability. It is straightforward to propagate the error state from its time of applicability (i.e., t) to its time of availability (i.e., t + 0:5). This is most important at system initialization, since in steady state the errors are all quite small and slowly time varying. This delay compensation is implemented. An additional improvement that will be included in the future is lever arm compensation. The antenna phase center and accelerometer position do not coincide in most applications. For the platform for this project, the separation is approximately 0.2 meters. Therefore, the INS and GPS estimates of position and velocity will di er as the vehicle roll and pitch angles change. Lever arm compensation can be achieved through a more involved algorithm for predicting the GPS outputs as a function of the INS state, and an alternative (fuller) measurement matrix. 1

9

2

Chapter 4

Task Descriptions To successfully complete the speci cation, development, integration, and evaluation of GPS-aided INS technology within AVCSS, UCR worked closely with the PATH vehicle control group. The period of performance of this project was twenty-one months, beginning October 1, 1998 and ending June 30, 2000. The total project consisted of six main tasks as described below:

Task 1: Analysis of GPS-aided INS Control Integration. Task 2: De nition and Development of the GPS/INS to Control Interface. Task 3: De nition and Implementation of the Control Changes. Task 4: De nition and Implementation of the GPS-aided INS Changes. Task 5: Control and GPS-aided INS Integration and Testing. Task 6: GPS-aided INS Based Vehicle Control Demonstration.

The pre-existing PATH control and UCR GPS/INS software run under QNX and DOS, respectively. To minimize the project cost related to software changes, so that technical progress could be maximized, UCR and PATH decided to use a two computer approach. The GPS/INS system was implemented on one computer running DOS with a serial port connection to a second PATH computer running QNX. The PATH computer implemented the control system and magnetometer code. The magnetometer hardware and software were kept onboard to provide a means of ground truth data analysis. The control system code did not change signi cantly. The control system parameters were tuned during the testing phase. The serial port interface between the two computers was designed and implemented between January and April 1999. The main message sends the control state from the navigation computer to the control computer. This message was sent at 30 Hz. The control state message included: a header, a time stamp, the signed o -track distance, the component of the velocity normal to the trajectory, the heading error, the rate of change of the heading error, the component of velocity along the trajectory, the commanded tangent velocity, the trajectory curvature, an integer control status ag, and a checksum (one Byte). Therefore, the nominal message length must be suÆcient to include: a string of ve characters, eight (single precision)

oats, an integer and a CHAR. For the testing performed to date, the commanded tangent velocity, curvature, and control status ag were not used. Depending on the future control implementations, additional state variables may be required. For example, the message may need an extra oat variable to indicate the look 10

ahead distance. Therefore, the message implemented included a few dummy oats to allow expansion in the future with out major software changes. The interface was tested and improved between April and June 1999 so that it was available for the July 1999 San Diego testing. The algorithms for translating the GPS/INS navigation state into the control system state variables are described in Section C. Testing onboard a PATH vehicle was rst performed in July 1999 at the Caltrans-controlled HOV lanes on I-15 in San Diego. The July 1999 testing succeeded in 1) demonstrating the desired level of navigation accuracy, 2) testing and verifying the serial port interface, and 3) identifying a set of issues for UCR to address to achieve satisfactory reliability levels. These issues and their solution were documented in a memo to PATH in September 1999. The GPS/INS was tested onboard the PATH vehicle a second time in March 2000 at the Crow's Landing test site. The result of the Crow's Landing testing are 1) the system accuracy was again demonstrated (and slightly improved), and 2) the system reliability was greatly improved. Two minor problems were identi ed: a periodic disturbance every 40 s. and de ciencies in the algorithm for translating the navigation state into the control state. Both of these problems have now been corrected. The identi cation of these two issues highlights the importance of on-vehicle testing, especially when a second measurement system is available to establish ground truth. Both of these problems had a very small e ect on the estimated variables. Therefore, without an accurate ground truth system for comparison, the e ects cannot be discerned. Also, the 40 s issue was caused in part by an interrupt con ict on the UCR computer during the serial port communication to the PATH computer. This issue did not arise until the GPS/INS system had to communicate with the PATH computer at the test site. The GPS/INS systems was tested onboard the PATH vehicle a third time in September 2000 at the Crows Landing test facility. These tests successfully demonstrated cm accuracy navigation and reliable GPS/INS based vehicle control. The control demonstrations included basic trajectory following as well as more advanced maneuvers such as lane changes. The results of this testing are included in the following section.

11

Chapter 5

Performance Analysis This section presents the results on on-vehicle testing at the Crow's Landing site between March 28-29, 2000. All results were obtained while the GPS/INS system was in Mode 4 (Di erential L1 Phase). This mode of operation was typically attained in less than 1 minute.

5.1 Stationary Data Tables 5.1 and 5.2 and Figures 5.1-5.4 show data acquired while the vehicle was not moving. Although the goal of this report is to show that accurate navigation and vehicle control are achieved while the vehicle is in motion, the stationary results are valuable since they allow certain characteristics to be more clearly observed than when the vehicle is in motion. The positions shown in the graphs correspond to random locations at which the vehicle stopped during other testing. The estimated positions in any two di erent gures need not be the same. An important rst point to note is that with the exception of computational inaccuracy, theoretically, the navigation accuracy is essentially independent of velocity at the speeds of interest. Therefore, the quantitative measures of performance determined in this section are indicative of the performance expected in the later sections. Table 5.1 contains the standard deviation of the linear positions, velocities, and accelerations for four experiments performed in March 2000. Table 5.2 contains the standard deviations of the attitude and angular rates for four experiments. For these experiments, the navigation system was mounted on a vehicle that contained three passengers and had its engine running. Therefore, the results include the e ects of engine File N E D vn ve vd an ae ad 16 0.013 0.010 0.014 0.010 0.008 0.011 0.060 0.136 0.077 19 0.011 0.006 0.028 0.008 0.006 0.006 0.058 0.089 0.061 23 0.011 0.014 0.008 0.009 0.010 0.006 0.054 0.077 0.051 24 0.013 0.013 0.013 0.010 0.010 0.006 0.053 0.076 0.058 Table 5.1: Standard deviations of estimated position, estimated velocity, and measured acceleration for four experiments. The vehicle was nominally still during these four experiments. Position units are meters. Velocity units are ms . Acceleration units are sm . 2

12

File Roll Pitch Heading p; 10 q; 10 r; 10 16 0.028 0.022 3.41 0.88 0.78 0.74 19 0.031 0.024 6.42 0.91 0.82 0.71 23 0.027 0.028 9.18 0.94 0.83 0.67 24 0.025 0.028 15.10 0.93 0.80 0.72 Table 5.2: Standard deviations of estimated attitude and measured angular rates for four experiments. The vehicle was nominally still during these four experiments. Angular units are degrees. Angular rates are in degrees per second. 3

3

3

vibration and passenger movement. Even with these e ects, the position standard deviation demonstrated is well within the centimeter level accuracy that was the goal of the project. Also, note the consistency of the standard deviations between the repetitions of the experiment. Figures 5.1-5.4 show plots of the time series data for the four repetitions of the static experiment. Note the following issues: 1. The gyros are operating at their quantization limit. A 100 degree per second gyro with 16 bits of accuracy has a quantization step-size of  1:5  10 degrees per sec. The quantization steps are clearly evident in this stationary data. 2. Pitch and roll are being accurately (0:03 deg) estimated. 3. When the measured acceleration is constant, not all error states can be estimated. When the acceleration vector is equal to the gravity vector, the vehicle heading is not observable. This can be easily shown theoretically and is evident in the plots of heading versus time in the plots of this section. 4. During all the March 2000 Crow's Landing testing, the navigation system exhibited a timing glitch that occurred with a periodicity of approximately 40 seconds. The March 2000 data is only included in this section of this report. Since stationary data sets were not saved during the September 2000 tests. The 40 s periodicity is most clearly observed in the plot of vertical position d of Figure 5.1 at t =0,40, 80, 120, and 160 s. Looking at the other plots of Figure 5.1, it is clear that the (a) the IMU data is valid at these times, and (b) the accuracy of the estimated data deteriorates at the same times. The cause of this problem has been found and eliminated. The performance of the system is actually better than shown in the tables and gures of this section, once this timing glitch is removed. The fact that the 40 timing glitch has been removed is clearly demonstrated by the fact that the data in the following sections (form the September 2000 test) is free of the 40 s. timing glitch. 5. All aspects of performance are repeatable between the four experiments. 3

5.2 Magnetometer Control Tests Figure 5.5 shows data for a run with the magnetometer controlling the vehicle lateral position. The GPS/INS control state vector (d; d;_ ; _) is shown in the four plots in the left column. The magnetometer measurement of the o -trajectory distance (i.e., (df + dm)=2) is also shown in the upper left plot for comparison. Note that the correspondence between the magnetometer and GPS/INS estimates of d is excellent (i.e., a few cm of di erence). Although the magnetometer data provides a direct ground truth check of the GPS/INS 13

estimate of d, no ground truth estimates are available for the other three states. Subsequent experiments will show that the four states are consistent. The right column includes plots of various items related to the trajectory and the experiment that are useful for interpreting the results in the left column. The top plot shows the vehicle velocity tangent to the trajectory during the time range corresponding to the left column of plots. For the portion of the run shown, the velocity is nearly constant. The second plot from the top shows the trajectory curvature  versus time. The vehicle enters the southern most turn at 80 s. Note that  = 1:251=km which corresponds to an 800 m radius turn. This matches the design speci cations for the magnet trajectory. The third graph displays the vehicle and trajectory heading versus time. The error between the two is shown in the third plot of the left column. The bottom right hand plot shows rc (dotted) and gz (solid), the trajectory and vehicle yaw rates. Note the transients in the control state as the vehicle passes points where the trajectory curvature changes either sign or magnitude. Throughout the transients corresponding to the changes in curvature, the dynamic characteristics of the GPS/INS estimates of the control system state are consistent and the magnetometer and GPS/INS estimates of d match to within a few cm.

5.3 GPS/INS Control Tests This section presents three examples of trials with the vehicle under GPS/INS lateral control. The results are divided into basic and advanced maneuvering. The basic maneuvering trials are similar to the previous experiment using lateral magnetometer control presented in Section 5.2 where the control objective was to follow the trajectory with zero error. In the advanced maneuvering trials, the vehicle was commanded to drive along perturbations to the trajectory. For these initial results, sinusoidal and lane change perturbations were implemented. For these initial tests, the perturbations were injected as disturbances which the control system rejected; therefore, the trajectory and vehicle curvature and heading do not match. Under application conditions, the maneuver trajectories would be de ned to implement, for example, emergency maneuvers or lane changes while maintaining consistency between the various trajectory variables. 5.3.1 Basic Maneuvering

Figures 5.6 and 5.7 display data for the same experiment. Figure 5.6 shows the entire data set. Figure 5.7 expands the time scale for a portion of the run to allow closer analysis of the relation between the variables in the control state vector. During the run, the GPS/INS control state is used to drive the vehicle. Therefore, there are two distinct issues. The control accuracy and the navigation accuracy. Note that although the structure of the magnetometor and GPS/INS control laws are the same, the control parameters for the GPS/INS controller are smaller than those of the magnetometer controller. Therefore, comparison of the size of d or  between Figures 5.5 and 5.6 shows that the larger control parameters of the magnetometer controller give it a higher bandwidth and smaller tracking errors. The main issue for this project is not the size of the control state (i.e., control accuracy), but the accuracy of the estimated control state (i.e., navigation accuracy). Figure 5.7 is included to allow a more detailed analysis of the navigation accuracy. The GPS/INS control state vector (d; d;_ ; _) is shown in the four plots in the left column. The upper left graph compares the GPS/INS estimate of d (solid) with the ground truth measurement of d from the magnetometer (dotted). Note that the two graphs match to cm accuracy. 14

The right column includes plots of various items related to the trajectory and the experiment that are useful for interpreting the results in the left column. The top right plot shows the vehicle velocity tangent to the trajectory during the time range corresponding to the left column of plots. The second plot from the top shows the trajectory curvature  versus time. Note that during the turns,  = 1:251=km which corresponds to an 800 m radius turn. This matches the design speci cations for the magnet trajectory. The third graph displays the vehicle and trajectory heading versus time. The error between the two is shown in the third plot of the left column. The bottom right hand plot shows rc (dotted) and gz (solid), the trajectory and vehicle yaw rates. Throughout, the transients in the control states have consistent dynamic characteristics. Note for example that at any time that d_ = 0, the variable d is at an extreme value. Similarly, at any time that _ = 0, the variable  is at an extreme value. The cm accuracy comparisons between the magnetometer and GPS/INS estimates of d and the consistency of the GPS/INS control state vector demonstrates the accuracy of the GPS/INS system. Figures 5.8 and 5.9 show results from a second experiment using the GPS/INS control state to drive the vehicle along the trajectory. Figure 5.9 again expands the time axis of Figure 5.8 to allow a more detailed analysis of the data characteristics. Note that the previous conclusions of this section apply again. Five experiments (three in addition to those shown herein) were run using the GPS/INS state to drive the vehicle along the trajectory. In all cases, the above conclusions apply. 5.3.2 Advanced Maneuvering { Sinusoids

Figure 5.10 shows the GPS/INS control state and trajectory data for a run where the GPS/INS state is used to force the vehicle to follow a 0.3 m amplitude 200 m wavelength sinusoidal perturbation to the trajectory. Note that the magnetometer and GPS/INS measurements both show d changing in a sinusoidal fashion. The sinusoidal phase (i.e., integral) relationships between heading and heading rate, d and d_ are all clearly visible. The descriptions of each of the plotted variable is the same as for the previous sections. Figure 5.11 shows the GPS/INS control state and trajectory data for a run where the GPS/INS state is used to force the vehicle to follow a 1.0 m amplitude 200 m wavelength sinusoidal perturbation to the trajectory. The descriptions of each of the plotted variable is the same as for the previous sections. Note that while the magnetometer measurement of d saturates at the extent of its range (i.e., about 0.5 m), the GPS/INS measurement shows d changing in a sinusoidal fashion. Within the range of the magnetometer (i.e., d < 0:5m), the magnetometer and GPS/INS estimates of d are comparable to cm accuracy. The sinusoidal phase (i.e., integral) relationships between heading and heading rate, d and d_ are all clearly visible. This demonstrates that the GPS/INS estimate of the control state is accurate regardless of the distance from the trajectory. 5.3.3 Advanced Maneuvering { Lane Change

Figures 5.12, 5.13, and 5.14 show the results of three di erent experiments. Each experiment implemented a lane change maneuver. For Figures 5.12 and 5.13 the maneuver was a 3.6 m lane change that was held for 120 m followed by a return to the nominal lane trajectory. For Figure 5.14 the maneuver was a 3.6 m lane change that was held for 360 m followed by a return to the nominal lane trajectory. The lane change maneuvers occurred at arbitrary locations along the trajectory, so the magnetometer measurement was not available once the vehicle was beyond the magnetometer range (i.e., 0.5 m). Note that for the experiment 15

depicted in Figure 5.14, the lane change maneuver was performed while the vehicle was in the curved section of track.

16

Velocity Tangent Plane Position

Position error

Accelerations, m/s/s

0.04

0.2

0.02

0.1

vn

0.05 n

−1283.6

Acc x

−1283.58

0

0

−0.02

−1283.62 −0.05

0 0

50

100

0 −0.1

50

100

150

0

50

100

150

0

50

100

150

0

50

100 Time, s.

150

150

−1283.64

0.4 0.04

−1283.68

ve

e

n

0.02

Acc y

−1283.66

0.02

0

0 −0.02

0 −0.2

−0.02

−0.04 0

−1283.7

0.2

50

100

0

150

50

100

150

0.04

0 vd

0.02 d

0

−1283.74

−9.6 Acc z

−1283.72

−0.02

−9.8

−0.02 −1283.76 −73.1

−0.04

−0.04 −73.05

−73 e

−72.95

−72.9

0

50 −3

Attitude, deg.

100 Time, s.

−10 0

150

50

100 Time, s.

150

gyros

x 10 4

−1 gyro x

roll

2 −1.05 −1.1

0 −2 −4

−1.15 0

50

100

150

0 −3 x 10

50

100

150

0 −3 x 10

50

100

150

0

50

100 Time, s.

150

8

−0.1 gyro y

pitch

6 −0.15

4 2 0

−0.2 0

50

100

150 0 gyro z

heading

155

150

−2 −4 −6

145

0

50

100 Time, s.

150

Figure 5.1: Position, velocity, angle data for run 16. Upper left: Scatter plot of horizontal position data and time series plots of position estimates. Positions and position errors in m. Upper right: Time series plots of estimated velocity and measured acceleration. Velocity in ms . Lower left: Time series plots of estimated attitude and measured angular rates.

17

Tangent Plane Position

Position error

−371.68

Velocity

Accelerations, m/s/s

0.08 Acc x

0.04

vn

n

−371.7

0.1

0.02

0.06 0.02

0

−0.02

−371.72

0 −0.1

0 −0.02 0

50

100

0

50

100

0

50

100

0

50

100

0

50

0.02 0.01 ve

e

n

−0.01 −0.02

−371.76

0.2

0.01

0

Acc y

−371.74

0

0

−0.01

−0.03

−0.2

−0.02 0

50

100

0

50

100

−371.78 0

−9.5

−0.01

−0.1 −0.15

−371.82 223.52

−0.02

223.54

223.56 e

223.58

223.6

−9.7

−9.9 0

50

100

0

Time, s. −3

Attitude, deg.

−9.6

−9.8

−0.03

−0.2 50

100 Time, s.

100 Time, s.

gyros

x 10 4

−0.35

2 gyro x

roll

Acc z

d

−371.8

vd

−0.05

−0.4 −0.45

0 −2 −4 −6

−0.5 0

50

100

0 −3 x 10

50

100

0 −3 x 10

50

100

0

50

4

−0.1 gyro y

pitch

2 −0.15

0 −2

−0.2 0

50

100

2 gyro z

heading

−50 −55 −60

0 −2

−65 0

50

100 Time, s.

100 Time, s.

Figure 5.2: Position, velocity, angle data for run 19. Upper left: Scatter plot of horizontal position data and time series plots of position estimates. Positions and position errors in m. Upper right: Time series plots of estimated velocity and measured acceleration. Velocity in ms . Lower left: Time series plots of estimated attitude and measured angular rates

18

Tangent Plane Position

Position error

Velocity

Accelerations, m/s/s

−178.92

0.2

0.02

0.02

−0.02

Acc x

−178.94

vn

n

0 0

−0.04 −0.02

−0.06 −178.96

0

150

50

100

150

0.04

0.2

0.02

0.1

Acc y

0.1

0

0

−179

0

50

100

150

0 −3 x 10

0.02

50

100

Acc z

vd

d

−10

−0.02

191

191.05 191.1

0

50 −3

Attitude, deg.

100 Time, s.

150

150

0

50

100

150

0

50

100 Time, s.

150

−9.7 −9.8

−20

e

100

−9.6

0

−179.06 190.85 190.9 190.95

50

0

150

0 −179.04

0

−0.1

−0.02

−0.05 −179.02

−0.1 0

ve

e

100

0

0.05

−178.98 n

50

0.1

0

50

100 Time, s.

150

gyros

x 10 4 gyro x

roll

−0.25 −0.3 −0.35

2 0 −2 −4

50

100

150

0

4

−0.05

2 gyro y

pitch

0

−0.1

0 −3 x 10

50

100

150

0 −3 x 10

50

100

150

0

50

100 Time, s.

150

0 −2

−0.15 0

50

100

150

gyro z

heading

2 −50 −60

0 −2

−70 0

50

100 Time, s.

150

Figure 5.3: Position, velocity, angle data for run 23. Upper left: Scatter plot of horizontal position data and time series plots of position estimates. Positions and position errors in m. Upper right: Time series plots of estimated velocity and measured acceleration. Velocity in ms . Lower left: Time series plots of estimated attitude and measured angular rates

19

Tangent Plane Position

Position error

Velocity

Accelerations, m/s/s

−178.86 0.05

0.1 Acc x

vn

0.02

n

−178.88

0

0 −0.02 −178.9

0

100

200

300

0

100

200

300

0

100

200

300

0

100

200

300

0

100

200

300

0.06 0.1

Acc y

ve

e

−178.94

0.2

0.04

0.05 n

−0.1

−0.05

−178.92

0.02

0

0

−0.05

−0.02 0

100

200

300

0

191 e

191.1

0

−0.02

−0.01 0

100

200

300

−9.6

−3

−9.7 −9.8 −9.9

0

Time, s.

Attitude, deg.

300

0.01

0

191.2

200

Acc z

0.02

vd

d

190.9

100

0.02

0.04 −179

0

0.03

0.06

−178.98

0.1

−0.1

−178.96

−179.02 190.8

0

100

200 Time, s.

300

Time, s.

gyros

x 10 4 2 gyro x

roll

−0.3 −0.35

0 −2 −4 −6

−0.4 0

100

200

300

0 −3 x 10

100

200

300

0 −3 x 10

100

200

300

0

100

200

300

4

0 gyro y

pitch

2 −0.05 −0.1

−4

−0.15 0

100

200

300 4

−80

2

−90

gyro z

heading

0 −2

−100

0 −2 −4

−110

−6

−120 0

100

200 Time, s.

300

Time, s.

Figure 5.4: Position, velocity, angle data for run 24. Upper left: Scatter plot of horizontal position data and time series plots of position estimates. Positions and position errors in m. Upper right: Time series plots of estimated velocity and measured acceleration. Velocity in ms . Lower left: Time series plots of estimated attitude and measured angular rates

20

40 vT, m/s

d, m

0.2

0

−0.2 60

20

70

80

90

0 60

100

0

−0.2

dε/dt, deg/s

80

90

100

70

80

90

70

80

90

100

70

80

90

100

70

80 Time, t, sec

90

100

20 10 0 −10 60

100

2 0 −2 −4 60

100

0

60 30 heading, deg

70

0

−1 60 4

90

−1

Yaw rate, deg/s

ε, deg

60 1

80

1 κ, 1/km

vN, m/s

0.2

70

70

80 Time, t, sec

90

100

5 0 −5 60

Figure 5.5: Control results with the magnetometer data used to control the vehicle. The left column of plots shows the GPS/INS based control state. The right column of plots shows trajectory relative data. Top left: Time series plot of the o -track distance d as determined by the magnetometers (dotted) and GPS/INS (solid). Second left: GPS/INS estimate of d_, the velocity normal to the trajectory. Third left: GPS/INS estimate of  = v c the trajectory relative heading error . Bottom left: Gyro measurement of yaw rate error _ = gz rc. Top right: GPS/INS estimate of vT , the velocity tangent to the trajectory. Second right: The trajectory curvature  in units of (km) . Third right: The trajectory heading c (dotted) and vehicle heading v (solid). Bottom right: The trajectory yaw rate rc (dotted) and vehicle yaw rate gz (solid). 1

21

40 v , m/s

d, m

0.2

20

T

0 −0.2 −0.4

40

60

80

100

120

0

140

40

60

80

100

120

140

40

60

80

100

120

140

40

60

80

100

120

140

40

60

80 100 120 Time, t, sec

140

1 κ, 1/km

vN, m/s

1 0

0 −1

−1

40

60

80

100

120

140 30 heading, deg

ε, deg

4 2 0 −2 −4

40

60

80

100

120

0

Yaw rate, deg/s

dε/dt, deg/s

10

−10

140

4 2 0 −2 −4

20

40

60

80 100 120 Time, t, sec

140

5 0 −5

Figure 5.6: Control results with the GPS/INS data used to control the vehicle lateral position. The left column of plots shows the GPS/INS calculated control state. The right column of plots shows trajectory relative data. Top left: Time series plot of the o -track distance d as determined by the magnetometers (dotted) and GPS/INS (solid). Second left: GPS/INS estimate of d_, the velocity normal to the trajectory. Third left: GPS/INS estimate of  = v c the trajectory relative heading error . Bottom left: Gyro measurement of yaw rate error _ = gz rc . Top right: GPS/INS estimate of vT , the velocity tangent to the trajectory. Second right: The trajectory curvature  in units of (km) . Third right: The trajectory heading c (dotted) and vehicle heading v (solid). Bottom right: The trajectory yaw rate rc (dotted) and vehicle yaw rate gz (solid). 1

22

40 vT, m/s

d, m

0.2 0

20

−0.2 −0.4 70 1

80

90

100

0 70

110

80

90

100

110

80

90

100

110

80

90

100

110

90 100 Time, t, sec

110

κ, 1/km

N

v , m/s

1 0

0 −1

80

90

100

110

70 30 heading, deg

ε, deg

−1 70 4 2 0 −2

80

90

100

0 −10 70

110

2 0 −2 −4 70

10

Yaw rate, deg/s

dε/dt, deg/s

−4 70 4

20

80

90 100 Time, t, sec

110

5 0 −5 70

80

Figure 5.7: Control results with the GPS/INS data used to control the vehicle lateral position. This gure expands a portion of the time axis of Figure 5.6. Top left: Time series plot of the o -track distance d as determined by the magnetometers (dotted) and GPS/INS (solid). Second left: GPS/INS estimate of d_, the velocity normal to the trajectory. Third left: GPS/INS estimate of  = v c the trajectory relative heading error . Bottom left: Gyro measurement of yaw rate error _ = gz rc. Top right: GPS/INS estimate of vT , the velocity tangent to the trajectory. Second right: The trajectory curvature  in units of (km) . Third right: The trajectory heading c (dotted) and vehicle heading v (solid). Bottom right: The trajectory yaw rate rc (dotted) and vehicle yaw rate gz (solid). 1

23

40 vT, m/s

d, m

0.2 0

20

−0.2 −0.4

120

140

160

180

0

200

120

140

160

180

200

120

140

160

180

200

120

140

160

180

200

120

140 160 Time, t, sec

180

200

1 κ, 1/km

N

v , m/s

1 0

0 −1

−1

120

140

160

180

200 30 heading, deg

ε, deg

4 2 0 −2 −4

120

140

160

180

0

Yaw rate, deg/s

dε/dt, deg/s

10

−10

200

4 2 0 −2 −4

20

120

140 160 Time, t, sec

180

200

5 0 −5

Figure 5.8: Control results with the GPS/INS data used to control the vehicle lateral position. The left column of plots shows the GPS/INS calculated control state. The right column of plots shows trajectory relative data. Top left: Time series plot of the o -track distance d as determined by the magnetometers (dotted) and GPS/INS (solid). Second left: GPS/INS estimate of d_, the velocity normal to the trajectory. Third left: GPS/INS estimate of  = v c the trajectory relative heading error . Bottom left: Gyro measurement of yaw rate error _ = gz rc . Top right: GPS/INS estimate of vT , the velocity tangent to the trajectory. Second right: The trajectory curvature  in units of (km) . Third right: The trajectory heading c (dotted) and vehicle heading v (solid). Bottom right: The trajectory yaw rate rc (dotted) and vehicle yaw rate gz (solid). 1

24

40 vT, m/s

d, m

0.2 0

20

−0.2 −0.4 140 1

150

160

170

0 140

180

150

160

170

180

150

160

170

180

150

160

170

180

160 170 Time, t, sec

180

κ, 1/km

N

v , m/s

1 0

0 −1

150

160

170

180

140 30 heading, deg

ε, deg

−1 140 4 2 0 −2

150

160

170

0 −10 140

180

2 0 −2 −4 140

10

Yaw rate, deg/s

dε/dt, deg/s

−4 140 4

20

150

160 170 Time, t, sec

180

5 0 −5 140

150

Figure 5.9: Control results with the GPS/INS data used to control the vehicle lateral position. This gure expands a portion of the time axis of Figure 5.8. Top left: Time series plot of the o -track distance d as determined by the magnetometers (dotted) and GPS/INS (solid). Second left: GPS/INS estimate of d_, the velocity normal to the trajectory. Third left: GPS/INS estimate of  = v c the trajectory relative heading error . Bottom left: Gyro measurement of yaw rate error _ = gz rc. Top right: GPS/INS estimate of vT , the velocity tangent to the trajectory. Second right: The trajectory curvature  in units of (km) . Third right: The trajectory heading c (dotted) and vehicle heading v (solid). Bottom right: The trajectory yaw rate rc (dotted) and vehicle yaw rate gz (solid). 1

25

40 v , m/s

d, m

0.5

20

T

0

−0.5 60 1

80

100

0 60

120

80

100

120

80

100

120

80

100

120

80

100 Time, t, sec

120

κ, 1/km

vN, m/s

1 0

0 −1

80

100

120

60 30 heading, deg

ε, deg

−1 60 4 2 0 −2

80

100

0 −10 60

120

2 0 −2 −4 60

10

Yaw rate, deg/s

dε/dt, deg/s

−4 60 4

20

80

100 Time, t, sec

120

5 0 −5 60

Figure 5.10: Control results with the GPS/INS data used to control the vehicle lateral position to follow a 0.3m amplitude and 200 m length sinsusoidal perturbation to the trajectory. Top left: Time series plot of the o -track distance d as determined by the magnetometers (dotted) and GPS/INS (solid). Second left: GPS/INS estimate of d_, the velocity normal to the trajectory. Third left: GPS/INS estimate of  = v c the trajectory relative heading error . Bottom left: Gyro measurement of yaw rate error _ = gz rc. Top right: GPS/INS estimate of vT , the velocity tangent to the trajectory. Second right: The trajectory curvature  in units of (km) . Third right: The trajectory heading c (dotted) and vehicle heading v (solid). Bottom right: The trajectory yaw rate rc (dotted) and vehicle yaw rate gz (solid). 1

26

40 vT, m/s

d, m

1 0

20

−1 0

20

40

0

60

0

20

40

60

0

20

40

60

0

20

40

60

0

20

1 κ, 1/km

vN, m/s

1 0

0 −1

−1

0

20

40

60 210 heading, deg

ε, deg

4 2 0 −2 −4

0

20

40

180

Yaw rate, deg/s

dε/dt, deg/s

190

170

60

4 2 0 −2 −4

200

0

20

40 Time, t, sec

60

5 0 −5 40 Time, t, sec

60

Figure 5.11: Control results with the GPS/INS data used to control the vehicle lateral position to follow a 1.0 m amplitude and 200 m length sinsusoidal perturbation to the trajectory. Top left: Time series plot of the o -track distance d as determined by the magnetometers (dotted) and GPS/INS (solid). Second left: GPS/INS estimate of d_, the velocity normal to the trajectory. Third left: GPS/INS estimate of  = v c the trajectory relative heading error . Bottom left: Gyro measurement of yaw rate error _ = gz rc. Top right: GPS/INS estimate of vT , the velocity tangent to the trajectory. Second right: The trajectory curvature  in units of (km) . Third right: The trajectory heading c (dotted) and vehicle heading v (solid). Bottom right: The trajectory yaw rate rc (dotted) and vehicle yaw rate gz (solid). 1

27

40 vT, m/s

d, m

4 2

20

0 20

30

40

0

50

20

30

40

50

20

30

40

50

20

30

40

50

20

30 40 Time, t, sec

50

1 κ, 1/km

vN, m/s

1 0

0 −1

−1

20

30

40

50 210 heading, deg

ε, deg

4 2 0 −2 −4

20

30

40

180

Yaw rate, deg/s

dε/dt, deg/s

190

170

50

4 2 0 −2 −4

200

20

30 40 Time, t, sec

50

5 0 −5

Figure 5.12: Control results with the GPS/INS data used to control the vehicle lateral position to perform a 3.6 m lane change maneuver for 120 m. Top left: Time series plot of the o -track distance d as determined by the magnetometers (dotted) and GPS/INS (solid). Second left: GPS/INS estimate of d_, the velocity normal to the trajectory. Third left: GPS/INS estimate of  = v c the trajectory relative heading error . Bottom left: Gyro measurement of yaw rate error _ = gz rc. Top right: GPS/INS estimate of vT , the velocity tangent to the trajectory. Second right: The trajectory curvature  in units of (km) . Third right: The trajectory heading c (dotted) and vehicle heading v (solid). Bottom right: The trajectory yaw rate rc (dotted) and vehicle yaw rate gz (solid). 1

28

40 vT, m/s

d, m

4 2

20

0 10

20

30

0

40

10

20

30

40

10

20

30

40

10

20

30

40

10

20 30 Time, t, sec

40

1 κ, 1/km

vN, m/s

1 0

0 −1

−1

10

20

30

40 210 heading, deg

ε, deg

4 2 0 −2 −4

10

20

30

180

Yaw rate, deg/s

dε/dt, deg/s

190

170

40

4 2 0 −2 −4

200

10

20 30 Time, t, sec

40

5 0 −5

Figure 5.13: Control results with the GPS/INS data used to control the vehicle lateral position to perform a 3.6 m lane change maneuver for 120 m. Top left: Time series plot of the o -track distance d as determined by the magnetometers (dotted) and GPS/INS (solid). Second left: GPS/INS estimate of d_, the velocity normal to the trajectory. Third left: GPS/INS estimate of  = v c the trajectory relative heading error . Bottom left: Gyro measurement of yaw rate error _ = gz rc. Top right: GPS/INS estimate of vT , the velocity tangent to the trajectory. Second right: The trajectory curvature  in units of (km) . Third right: The trajectory heading c (dotted) and vehicle heading v (solid). Bottom right: The trajectory yaw rate rc (dotted) and vehicle yaw rate gz (solid). 1

29

40 v , m/s

2

20

T

d, m

4

0 0

20

40

0

60

0

20

40

60

0

20

40

60

0

20

40

60

0

20

40 Time, t, sec

60

1 κ, 1/km

vN, m/s

1 0

0 −1

−1

0

20

40

60 210 heading, deg

ε, deg

4 2 0 −2 −4

0

20

40

180

Yaw rate, deg/s

dε/dt, deg/s

190

170

60

4 2 0 −2 −4

200

0

20

40 Time, t, sec

60

5 0 −5

Figure 5.14: Control results with the GPS/INS data used to control the vehicle lateral position to perform a 3.6 m lane change maneuver for 360 m. Top left: Time series plot of the o -track distance d as determined by the magnetometers (dotted) and GPS/INS (solid). Second left: GPS/INS estimate of d_, the velocity normal to the trajectory. Third left: GPS/INS estimate of  = v c the trajectory relative heading error . Bottom left: Gyro measurement of yaw rate error _ = gz rc. Top right: GPS/INS estimate of vT , the velocity tangent to the trajectory. Second right: The trajectory curvature  in units of (km) . Third right: The trajectory heading c (dotted) and vehicle heading v (solid). Bottom right: The trajectory yaw rate rc (dotted) and vehicle yaw rate gz (solid). 1

30

Chapter 6

Conclusions and Future Research This project has achieved its objective of integrating, demonstrating, and evaluating the use of DGPS/INS in AVCSS applications. The carrier phase DGPS aided INS system runs at 150 Hz. The demonstrated control accuracy from the September 2000 testing is comparable at the cm level of accuracy to the ground truth measurement supplied by the magnetometer system. The advanced maneuvering experiments onboard the PATH vehicles demonstrate the enhanced capabilities that will be possible with an integrated magnetometer, GPS and INS system. In addition to enhanced maneuvering capabilities, the integrated system will have increased reliability and fault tolerance due to the triplicate sensor redundancy.

6.1 Future Research Several directions are available for future research: 1. MOU 391 is analyzing and implementing an approach that integrates the magnetometer and GPS/INS sensing systems. Although accuracy will not improve signi cantly, the integrated system will be more reliable than either system alone. In addition, the integrated system will be available (i.e., provide the control state vector) further from the trajectory than the magnetometer-only system. 2. Many modern braking systems contain wheel encoders. If properly calibrated these encoders provide free, redundant measurements of vehicle speed and angular rate. MOU 391 is also analyzing the accuracy of properly calibrate wheel speed based measurements to determine the value of integrating these measurements into an integrated system. 3. Vehicle heading is important for vehicle control. At least three possible heading estimation approaches are possible. The existing system uses the time varying vehicle acceleration vector. This approach has diÆculty only when the acceleration vector is approximately constant over a given time interval. An alternative approach is to estimate the vehicle heading through integration with the magnetometer system. Since the magnetometers are o set in the horizontal plane from the INS, the magnetometer readings will allow heading estimation. The accuracy of this approach will be evaluated under MOU 391. Alternatively, a second GPS antenna could be added. By using carrier phase double di erence methods between the two GPS antennae and knowledge of the o set vector in vehicle coordinates, the vehicle heading could again be estimated. 31

4. GPS receivers contain delay lock loops that track the satellite signals as they arrive at the receiver. These loops can lose lock when the GPS signals are temporarily blocked. The time that the receiver is able to maintain lock during such blockages can be increased by integrating the INS information back into the GPS receiver. Such an approach would signi cantly reduce the last remaining technical issue detracting from the use GPS in automotive applications (i.e., loss of lock due to temporary signal blockage).

6.2 Publications Resulting from this Project The research described in the following articles was partially supported by this project. Farrell, J. and T. Givargis. \Di erential GPS Reference Station Algorithm: Design and Analysis" IEEE Transactions on Control Systems Technology. Vol. 8 (3), May 2000, pp. 519-531. Farrell, J., T. Givargis, and M. Barth. \Real-time Di erential Carrier Phase GPS-Aided INS," IEEE Transactions on Control Systems Technology. Submitted November 1998. Accepted January 27, 2000. Final Submission February 2000. Farrell, J., T. Givargis. \Experimental Di erential GPS Reference Station Evaluation", pp. 3645-3649. Proceedings of the 1999 American Control Conference, San Diego, CA, June 1999. Farrell, J., T. Givargis, and M. Barth. \Di erential Carrier Phase GPS Aided INS for Automotive Applications", pp. 3660-3664. Proceedings of the 1999 American Control Conference, San Diego, CA, June 1999. Y. Yang, J. Farrell, and M. Barth, \High-Accuracy, High-Frequency Di erential Carrier Phase GPS Aided Low-Cost INS," IEEE PLANS, San Diego, CA March 13-16, 2000.

32

Bibliography [1] \Navstar GPS Interface Control Document," Report #ICD-GPS-200B-PR, Joint GPS Program OÆce, 1991. [2] Anderson, J. and S. Lazar, \Lm and Lc: How the Military and Civilian Users of GPS can Share our Existing Spectrum," ION GPS 97 Presentation, September 18, 1997, Kansas City, MO. See also `http://www.laafb.af.mil/SMC/CZ/homepage/lm/overview.htm' [3] Bancroft, S., \An Algebraic Solution of the GPS Equations," IEEE Transactions Aerospace and Electronic Systems, Vol. AES-21, No. 7, Jan 1985, pp. 56-59. [4] M. Barth and E. Johnston, \Using GPS Technology to Obtain Accurate Speed, Acceleration, and Grade Information for On-Road Emissions Measurement", in Fifth CRC On-Road Vehicle Emissions Workshop, San Diego, CA, 1995. [5] Blomenhofer, H., G. Hein, E. Blomenhofer, and W. Werner, \Development of a Real-Time DGPS System in the Centimeter Range," IEEE 1994 Position, Location, and Navigation Symposium, Las Vegas, NV, 1994, pp. 532-539. [6] Britting, K. R., Inertial Navigation Systems Analysis, Wiley-Interscience, New York, 1971. [7] Brown, R. G., Y.C. Hwang, Introduction to Random Signals and Applied Kalman Filtering, 2nd ed. New York : J. Wiley, c1992. [8] Cannon, E. \High-Accuracy GPS Semikinematic Positioning: Modeling and Results," Navigation: Journal of the Institute of Navigation, Vol. 37, No. 1, Summer 1990, pp. 53-64. [9] W. Cormier and R. Fenton, \On the Steering of Automated VehiclesA Velocity Adaptive Controller," IEEE Trans. on Vehicular Technology, Vol. 29, no.4, pp. 375-385, 1980. [10] R. T. Dunlay, \Obstacle avoidance perception processing for the autonomous land vehicle," in Proceedings of the IEEE International Conference on Robotics and Automation, pp. 912-917, 1987. [11] E. Dickmanns and B. Mysliwetz, \Recursive 3-D road and relative ego-state recognition," IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol. 14, pp. 199-213, 1992. [12] [23] H. Euler, \Achieving High-Accuracy Relative Position in Real-time: System Design, Performance, and Real-Time Results," in IEEE 1994 Position Location and Navigation Symposium, Las Vegas, NV, USA, pp. 540-546, 1994. 33

[13] Fang, B. T., \Trilateration and Extension fo Global Positioning System Navigation," Journal of Guidance, COntrol, and Dynamics, Vol. 9, No. 6, Nov./Dec. 1986, pp. 715-717. [14] Farrell, J., T. Givargis, and M. Barth. \Real-time Di erential Carrier Phase GPS-Aided INS," IEEE Transactions on Control Systems Technology. Submitted November 1998. Accepted January 27, 2000. Final Submission February 2000. [15] Farrell, J. and T. Givargis. \Di erential GPS Reference Station Algorithm: Design and Analysis" IEEE Transactions on Control Systems Technology. Vol. 8 (3), May 2000, pp. 519-531. [16] Farrell, J. and M. Barth, The Global Positioning System and Inertial Navigation: Theory and Practice, In Preperation, expected publication in 1998. [17] Farrell, J., M. Djodat, M. Barth, and M. Grewal, \Latency Compensation for Di erential GPS," Navigation: Journal of the Institute of Navigation, Vol. 44, No. 1, Spring 1997, 99-107. [18] R. Fenton and R. Mayhan, \Automated Highway Studies at The Ohio State UniversityAn Overview," IEEE Trans. on Vehicular Technology, Vol. 40, no.1, pp. 100-113, 1991. [19] R. Fenton, G. Melocik, and K. Olson, \On the Steering of Automated Vehicles: Theory and Experiment," IEEE Transactions of Automatic Control, Vol. 21, no.3, pp. 306-315, 1976. [20] Frodge, S.,S. Deloach, B. Remondi, D. Lapucha, and R. Barker, \Real-Time on-the-Fly Kinematic GPS System Results," Navigation: Journal of the Institute of Navigation, Vol. 41, No. 2, Summer 1994, pp. 175-186. [21] A. Hanson, E. Riseman, and C. Weems, \Progress in computer vision at the University of Massachusetts," in DARPA Image Understanding Workshop, pp. 39-47, 1993. [22] Grissom, L. \DGPS Correction Communication System Design," EE Senior Design Report, University of California, Riverside, June 1995. [23] Hatch, R., \The Synergism of GPS Code and Carrier Measurements," Proc. 3rd Int. Geodetic Symposium on Sat. Doppler Pos., 1982, Las Cruces, NM. [24] Hatch, R., \Instantaneous Ambiguity Resolution," Symposium No. 107, Kinematic Systems in Geodesy, Surveying and Remote Sensing, Ban , Canada, September 10-13, 1990, pp. 299-308, Springer Verlag. [25] Hatch, R., \The Promise of a Third Frequency," GPS World, May 1996, pp. 55-58. [26] Hwang, P. and R. Brown, \GPS Navigation: Combining Pseudorange with Continuous Carrier Phase using a Kalman Filter," Navigation: Journal of the Institute of Navigation, Vol. 37, No. 2, Summer 1990, pp. 181-196. [27] C. Kee and B. Parkinson, \Wide Area Di erential GPS as a Future Navigation System in the US," in IEEE 1994 Position Location and Navigation Symposium, Las Vegas, NV, USA, pp. 788-795, 1994. [28] Kuan, Phipps, and Hsueh, \Autonomous Robotic Vehicle Road Following," IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol. 10, pp. 648-658, 1988. [29] Lachapelle, G., M. Cannon, and G. Lu, \High-Precision GPS Navigation with Emphasis on CarrierPhase Ambiguity Resolution," Marine Geodesy, 15, pp. 253-269, 1992. 34

[30] Leva, J., \An Alternative Closed Form Solution to the GPS Pseudorange Equations," Institute of Navigation (ION) National Technical Meeting, Anahiem, CA, Jan. 1995. [31] Mader, G., \Dynamic Positioning using GPS Carrier Phase Measurements," Manuscripta Geodetica, Springer-Verlag, Vol. 11, 1986, pp. 272-277. [32] I. Masaki, Vision-Based Vehicle Guidance, New York: Springer-Verlag, 1992. [33] R. Mayhan and R. Bishel, \A Two-Frequency Radar for Vehicle Automatic Lateral Control," IEEE Trans. on Vehicular Technology, Vol. 31, no.1, pp. 32-39, 1982. [34] J. Mendel, Lessons in Estimation Theory for Signal Processing, Communications, and Control, Prentice Hall: Englewood Cli s, 1995. [35] J. Meyer-Hilberg and T. Jacob, \High Accuracy Navigation and Landing System using GPS/IMU System Integration," in IEEE 1994 Position Location and Navigation Symposium, Las Vegas, NV, USA, pp. 298-305, 1994. [36] The Global Positioning System: A Shared National Asset, National Research Council, Washington, D.C.: National Academy Press, 1995. [37] RTCM Recommended Standards for Di erential Navstar GPS Service, Version 2.1, RTCM Special Committee No. 104, January 3, 1994. [38] Rowson, S. V., G. R. Courtneyand, and R. M. Hueschen, \Performance of Category IIIB Automatic Landings Using C/A Code Tracking Di erential GPS," Navigation, Vol. 41, No. 3, pp 127-143, 1994. [39] J. W. Spalding and L. A. Luft, \Di erential GPS Integrity Monitor," in IEEE 1994 Position Location and Navigation Symposium, Las Vegas, NV, USA, pp. 225-232, 1994. [40] H. Schneidermann and M. Nashman, \Visual processing for autonomous driving," in IEEE Workshop on Applications of Computer Vision, Palm Springs, CA, pp. 164-171, 1992. [41] S. Shladover, et al., \Automatic Vehicle Control Developments in the PATH Program," IEEE Trans. on Vehicular Technology, Vol. 40, no.1, pp. 114-130, 1991. [42] Teunissen, P. \A New Method for Fast Carrier Phase Ambiguity Estimation," IEEE 1994 PLANS, pp. 562-573. [43] C. Thorpe, et al, \Toward autonomous driving: the CMU Navlab", Part 1-Perception, IEEE Expert, Vol. 6, pp. 31-42, 1991. [44] Widnall, W. S. and P. A. Grundy, \Inertial Navigations System Error Models," Intermetrics Inc., TR03-73, May 11, 1973. [45] T. Younglove, E. Johnston, and M. Barth, \DGPS Technology for Vehicle Parameter Estimation," in International Workshop on Driving Cycles and Vehicle Data-logging, Ottawa, Canada, 1995. [46] Zhang, et al., \An Intelligent Roadway Reference system for Vehicle Lateral Guidance/Control," in American Control Conference, San Diego, CA, pp. 281-286, 1990. 35

Appendix A

Appendix: INS Implementation A.1 Inertial Navigation System (INS) This section summarizes the mechanization equations for a strapdown tangent plane INS. The derivations using the same notation can be found in [16]. The mechanization equations are the computer solution of the INS di erential equations based on computed and measured variables. These equations are summarized below, in the Britting notation [6], where x^ and x~ denote respectively, the computed and measured values of the variable x. A.1.1 Tangent Plane Mechanization Equations

The (local xed) tangent plane INS mechanization di erential equations are 2 3 2 3 2 3 2 3 n^_ 1 0 0 0 0 6 _ 7 6 7 6 7 6 7 6 e^ 7 6 72 3 6 0 7 6 0 7 0 1 0 6 7 6 7 v^N 6 7 6 7 6 ^_ 7 6 7 6 7 6 7 0 0 1 6 h 7 6 76 7 6 0 7 6 0 7 (A.1) 6 7 = 6 7 4 v^E 5 + 6 7+6 7 6 v^_ N 7 6 7 6 f^N 7 6 0 7 0 2!ie sin(^) 0 6 7 6 7 v^D 6 7 6 7 6 _ 7 6 2! sin( 6 f^ 7 6 0 7 ^) 0 2!ie cos(^) 75 ie 4 v^E 5 4 4 E 5 4 5 0 2!ie cos(^) 0 f^D g^ v^_ D where all variables are de ned in Appendix D. For use in the above equation, the accelerometer measurements are processed as follows: 2 3 02 3 2 ^bu 31 f^N f~u 6 ^ 7 6 7 6 C ^pn B bv 7 4 fE 5 = C @4 f~v 5 4 ^ 5A : ^bw f^D f~w The platform to navigation frame vector transformation matrix is calculated based on the Euler roll, pitch, and yaw angles. The Euler angles are maintained by solving the following di erential equations 2 6 6 4

^_ ^_

_^

3 7 7 5

2

=

6 6 4

1 sin(^) tan(^) cos(^) tan(^) 0 cos(^) sin(^)   0   sin( ^) cos( ^)

36

cos( ^) cos( ^)

32 76 74 5

p^ q^ r^

3 7 5

(A.2)

with the gyro attitude rate measurements as inputs. For use in the above equation, the gyro measurements are processed as follows: 2 3 02 3 2 0 2 31 ^bp 31 p^ p~ cos(^) 6 7 B6 7 6 C 6 ^n pB (A.3) bq 7 0 75C 4 q^ 5 = @4 q~ 5 4 ^ 5A R @!ie 4 A ^ ^ r^ r~ br sin() A quaternion approach would have been preferred, and would be pursued in future e orts, but was not achievable within the given resource constraints. The quaternion approach would have the advantages of faster computation and no singularities. The approach which used here (eqn. (A.2)) achieved the desired update rates and was not a ected by the singularities due to the small tilt angles. The local, xed tangent plane INS was suÆcient for this demonstration due to the limit geographic area expected for testing. In the ultimate application, the geographic INS described in [16] would be required to allow unrestricted (global) travel. Minor changes to the INS are required to transform the currently implemented INS to the geographic (latitude, longitude based) INS. 2

A.1.2 Tangent Plane INS Nominal Error Equations

For error analysis and on-line error estimation via Kalman ltering (see Section 3.1), it is convenient to linearize the equations of the previous section about the vehicle trajectory. The linearized error equations are derived in [6, 16, 44] and summarized in this and the following subsections. The summary uses the following notation !N = !e cos() (A.4) !D = !e sin() (A.5) for the north and vertical components of earth rotation rate in the navigation frame The dynamic equations for the linearized INS error are Æx_ (t) = F(t)Æx(t) + (t) + !(t): (A.6) where 2 3 0 0 0 1 0 0 0 0 0 6 6 0 0 0 0 1 0 0 0 0 777 6 6 0 0 0 0 0 1 0 0 0 77 6 6 ! v n E 6 0 0 0 2!D 0 0 fD fE 777 R 6 F = 666 R (vN !N + vD !D ) 0 0 2!D 0 2!N fD 0 fN 777 : (A.7) vE !D 6 0 R 0 2!N 0 fE fN 0 77 R 6 ! D 6 0 0 0 0 0 0 !D !E 77 6 R 6 0 0 0 0 0 0 !D 0 !N 75 4 !N 0 0 0 0 0 !E !N 0 R ! Due to the small magnitude of the velocity relative to Re , the rst column can be approximated as zero. For notational convenience in the following sections, the linearized error equations can be written as 2 3 2 32 3 2 3 2 3 Æp_ F Æp ep !p pp Fpv Fp 6 7 6 76 7 6 7 6 7 (A.8) 4 Æ v_ 5 = 4 Fvp Fvv Fv 5 4 Æ v 5 + 4 ev 5 + 4 !v 5 : Æ_ Fp Fv F Æ e ! 2

2

2

2

3

37

The subcomponents of the F matrix can be de ned by inspection of eqn. (A.7). All error quantities are de ned to be the actual value minus the calculated (or measured) quantity (i.e., Æx = x x^). For the tangent plane implementation, the three components of nominal error state are de ned to be Æp = [Æn; Æe; Æh]T (A.9) Ævn = [ÆvN ; ÆvE ; ÆvD ]T (A.10) n T Æ = [ÆN ; ÆE ; ÆD ] : (A.11) The error state vector is de ned as Æx(t) = [Æp; Ævn ; Æn ]: Instrument and gravity modeling errors are represented by the three subvectors of (t). Equation (A.8) shows that the velocity error is driven by accelerometer and gravitational model errors denoted by ev . Let xa denote the state to be augmented to account for the accelerometer and gravitation errors. Equation (A.8) also shows that the attitude error is driven by gyro errors denoted by e . Let xg denote the state to be augmented to account for these gyro errors. Linear error models can be de ned [16] with matrices Fvxa and Fxg such that ev = Fvxa xa + a (A.12) e = Fxg xg + g (A.13) where a and g denote the accelerometer and gyro measurement noise, respectively. With these de nitions, eqn. (A.8) can be expanded as 2 3 2 32 3 2 3 2 3 " # Æp_ F Æp 0 0 !p pp Fpv Fp 6 7 6 76 7 6 0 75 xxag + 64 !v + a 75 (A.14) 4 Æ v_ 5 = 4 Fvp Fvv Fv 5 4 Æ v 5 + 4 Fvxa Æ_ Fp Fv F Æ 0 Fxg ! + g 2 3 2 32 3 2 3 Æp_ Fpp Fpv Fp 0 0 7 6 Æp 7 6 !p 7 6 7 6 6 Æ v_ 7 6 Fvp Fvv Fv Fvxa 0 77 66 Æv 77 66 !v + a 77 6 7 6 6 Æ _ 7 = 6 Fp Fv F (A.15) 0 Fxg 777 666 Æ 777 + 666 ! + g 777 : 6 7 6 6 7 6 0 0 Fxaxa 0 5 4 xa 5 4 !a 5 4 x_ a 5 4 0 x_ g 0 0 0 0 Fxg xg xg !g In the implementation for this project, xa and xg represent accelerometer and gyro biases respectively. Modeling these biases as random walk processes, Fxaxa and Fxg xg are identically zero. By the chain rule, p Fvxa = @@ef pv @@xf (A.16) =

Fxg =

a

p Rp2n @@xf a p @ e @!ip p @!ip @ xg p @!ip Rp2n @ xg

(A.17)

(A.18)

= (A.19) where the Æ on the error quantities has been dropped throughout to simplify the notation. Finally, with the p p @!ip @ f de nitions stated above, @xa = @xg = I. The power spectral densities for the driving noise processes !a and !g were determined by analysis instrument biases over an extended period of time. Similarly, the spectral density of the measurement noise processes a and g were determined by the analysis of measurement data. 38

A.1.3 Numeric Integration

The INS equations are updated at 150 Hz, using a predictor{corrector integration routine. Given the equation

x_ = f (x; u)

(A.20)

and the state x(k) at the k-th iteration, the predictor corrector algorithm calculates the state at the (k+1)-st iteration in three steps: Predictor Step: xp (k + 1) = x(k) + f (x(k); u(k))dt Corrector Step: xc(k + 1) = x(k) + f (xp (k + 1); u(k))dt Average: x(k + 1) = (xp (k + 1) + xc (k + 1)) =2 which is accurate to second order.

39

Appendix B

GPS Methodology B.1 Di erential Global Positioning (DGPS) B.1.1 GPS Observables

The two basic outputs of a GPS receiver are pseudo-range and carrier phase data. A two frequency receiver will output range and phase measurements for each carrier frequency, resulting in four basic outputs. Combinations of these basic outputs provide other (sometimes) useful signals. The basic outputs and their combinations are discussed in the following subsections. These sections will serve as background for the description in Section 3.2 of the aided INS approach used in this project. The rst subsection discusses the magnitude of various GPS measurement errors as a foundation for discussion in the subsequent sections.

GPS Error Sources

The discussion of the following sections will introduce various GPS measurement quantities and the errors that corrupt the measurements. This section provides a brief reference discussion of the various forms of error, to eliminate duplication in the following sections. Table B.1 (see Tables 3-1 and C-1 in [36]) lists typical standard deviations for the various sources of noise corrupting the GPS C/A code observables. Common mode error refers to those error sources that would be common to every receiver operating in a limited geographic region. Non-common mode errors refer to those errors that could be distinct to receivers operating even in close proximity. The actual amount of noncommon mode noise will be dependent on receiver type and techniques used to mitigate against multipath e ects. The magnitude of the non-common mode noise sources on the phase quantities would be signi cantly smaller than the values listed in Table B.1 for the C/A code observables. Based on the estimates of the standard deviation of the various noise sources contained in Table B.1, standard GPS with selective availability o produces a range standard deviation error on the order of 7.9 m from common mode errors. The corresponding approximate horizontal position error would be approximately 20 meters (for an HDOP of 2.5). Di erential GPS is discussed in Section B.1.2. Di erential operation achieves signi cant accuracy improvements by canceling the e ect of the common mode errors. Selective availability is the deliberate corruption of the GPS system accuracy by agencies of the U.S. government. Selective availability is currently turned o and is expected to remain o inde nitely . Ionospheric error and tropospheric error are caused by the deviation of the speed of light when not traveling in 40

Common Mode Errors Selective Availability Ionosphere Clock and Ephemeris Troposphere

Standard Deviation 0.0 m 7.0 m 3.6 m 0.7 m

Non-common Mode Errors Receiver Noise 0.1-0.7 m Multipath 0.1-3.0 m Table B.1: Error due to Noise Sources a vacuum. The nature of the two errors are di erent, since the ionosphere is a dispersive medium. Clock and ephemeris errors represent the di erence between the actual and computed satellite clock and position variables. All of these errors are slowly time varying and can be e ectively removed by the use of broadcast range corrections.

Pseudo-range Observables

Let the receiver clock o set from the satellite system time be denoted by tr . The receiver clock o set is the error between the GPS system time and time measured on the receiver. The receiver clock o set exists due to drift in the local oscillator of the receiver. With the three position variables and the clock error, there are four unknown quantities in the standard GPS positioning application. The GPS positioning problem is to determine these four unknowns based on the following system of equations, representing range measurements from four di erent satellites: ~ = ((X x) + (Y y ) + (Z z) ) : +ctr + ctsv + cta + SA + E + MP +  (B.1) : ~ = ((X x) + (Y y ) + (Z z) ) +ctr + ctsv + cta + SA + E + MP +  (B.2) : ~ = ((X x) + (Y y ) + (Z z) ) +ctr + ctsv + cta + SA + E + MP +  (B.3) : ~ = ((X x) + (Y y) + (Z z) ) +ctr + ctsv + cta + SA + E + MP +  (B.4) 1

(1)

(1)

2

(1)

(1)

(2)

(2)

2

(3)

(2)

2

(4)

2

(3)

2

(2)

(2)

2

(3)

(3)

(4)

(4)

(1)

(2)

(3)

(4)

(1)

(1)

(2)

(3)

2

(3)

2

(4)

(4)

(4)

2 05

(1)

(1)

(1)

(2)

(2)

(3)

(3)

(4)

(4)

2 05

(2)

2 05

(3)

2 05

(4)

where ~ , ~ , ~ ; and ~ are the measured pseudo-ranges, (X i ; Y i ; Z i ) are the ECEF position coordinates of satellite i, (x; y; z) are the ECEF position coordinates of the receiver antenna, tr is receiver clock bias, tsvi is the clock bias of the satellite vehicle, tai is the atmospheric delay, SA i represents the deliberate corruption of the satellite signals under the policy of selective availability, E i represents error in the broadcast ephemeris data, MP i represents multipath error,  i represents receiver tracking error noise, and c is the speed of light. The () i notation refers to the quantity in parenthesis referenced to the i-th satellite. (1)

(2)

(3)

(4)

( )

( )

( )

( )

( )

( )

( )

( )

( )

( )

1

Calculated as transit time multiplied by the speed of light.

41

Linearizing eqns. (B.1-B.4) gives 2 3 2 ~ (x) ~ 6 7 6 6 ~ (x) 7 6 ~ 6 7 6 6 ~ (x) 7 = 6 ~ 4 5 4 ~ (x) ~ (1)

(1)

(2)

(2)

(3)

(3)

(4)

(4)

3

2

3

2

3

(x ) 7 6 (x x ) 7 6  (x ) 77 + H 66 (y y ) 77 + 66  (x ) 75 64 (z z ) 75 64  (x ) ctr  0

0

0

0

0

(1)

7 7 7 7 5

(2) (3)

0

+ h:o:t:0s

(B.5)

(4)

0

where  i has been used as a shorthand to denote the terms ctsvi + ctai + SA i + E i + MP i +  i corrupting each measurement, (x ; y ; z ) is the point of linearization, h:o:t:0s represents the higher order terms in the expansion, ( )

( )

0

0

H= where Æ(i) Æx Æ(i) Æy Æ(i) Æz

( )

( )

( )

( )

0

(i) (x) = ((X (i) x)2 + (Y (i)

and

( )

2 Æ(1) Æx 6 Æ(2) 6 6 Æx 6 Æ(3) 4 Æx Æ(4) Æx

= ((X i = ((X i

( )

( )

Æ(1) Æy Æ(2) Æy Æ(3) Æy Æ(4) Æy

y)2 + (Z (i) z )2 )0:5 ;

Æ(1) Æz Æ(2) Æz Æ(3) Æz Æ(4) Æz

3

1 1 777 1 75 1 x ;y ;z ( 0

0

(B.7) 0)

x) y)2 + (Z (i) (Y (i) y) x)2 + (Y (i) y)2 + (Z (i) (Z (i) z) x)2 + (Y (i) y)2 + (Z (i) x)

2

+ (Y

(X i

( )

(i)

z )2 )0:5 z )2 )0:5

= ((X i z) ) : For solution, each component eqn. of (B.5) can be written as Æ^ i = ~ i (x) ^ i (x ) = h i Æx +  i + h:o:t:0 s ( )

(B.6)

2 05

:

2

( )

( )

( )

( )

0

(B.8)

( )

where the rst three components of h i (the i-th row of H) form a unit vector pointing from x to the satellite. In solving equations B.1-B.4, the e ective satellite vehicle (SV) positions are required. The position of the space vehicle antenna phase center in earth centered earth xed (ECEF) coordinates can be computed using data derived from the GPS navigation messages as described in [1, 16]. Note that the position estimation accuracy will depend on both the numeric properties of H and the magnitude of . ( )

0

Carrier Phase Observables

In addition to tracking the code, a receiver in phase lock on the carrier signal is able to track the relative phase shift in the carrier between any two time instants. Therefore, although the receiver cannot directly measure the number of carrier cycles between it and a given satellite, the receiver can measure the change in this number of cycles. 2

The measured pseudo-range should be corrected for satellite clock o set (tsv [1])

42

The carrier phase observable for the i-th satellite can be represented as (~ i + N i ) = ((X i x) + (Y i y) + (Z i z) ) : +ctr + ctsvi ctai + SA i + E i + mp i + i (B.9) =  i + ecmi + mp i + i (B.10) where N i is the integer phase ambiguity,  is the carrier wavelength, mp i is the carrier signal multipath, i is random measurement noise, and all other symbols are de ned in eqn. (B.1). The interest in the carrier signal stems from the fact that the non-common mode errors mp i and i are much smaller than the respective errors on the code observable (mp  2cm and   1mm). The common-mode errors are essentially the same as those on the code observable, except that the ionospheric error enters eqns. (B.1) and (B.9) with opposite signs. In the di erential mode of operation (see Section B.1.2), the common mode errors can be reduced to produce an observable accurate to a few centimeters. Let this di erentially corrected phase be described as 4 i  = (~ i ~oi )  (B.11)  = h i (x xo ) N i Noi  + (mp i mpoi ) + ( i oi ) (B.12) ( )

( )

( )

2

( )

( )

( )

( )

2

( )

( )

( )

( )

2 05

( )

( )

( )

( )

( )

( )

( )

( )

( )

( )

( )

( )

( )

( )

( )

( )

( )

( )

( )

where the subscript in ( )oi refers the generic quantity  to the DGPS base station. The di erentially corrected phase of eqn. (B.12) provides a very accurate measure of range, if the integer ambiguity can be determined. The integer phase ambiguity is the whole number of carrier phase cycles between the receiver and satellite at an initial measurement time. It is a (usually large) unknown integer constant (barring cycle slips). To make use of carrier phase observable as a range estimate, the integer ambiguity must be estimated. Alternatively, the carrier phase measurement could be di erenced across time epochs, eliminating the constant integer o set, to provide an accurate measurement of the change in range. ( )

Doppler Carrier Phase Processing

Note that if eqn. (B.12) is di erenced between two measurement times, an accurate measure of the change in satellite to receiver range results (4 i (k) 4 i (k 1)) = h i (k)4x(k) h i (k 1)4x(k 1) + Æmpi(k) + Æ i (k) (B.13) where 4x(k) = (x(k) xo(k)), and the relative clock error is included in 4x(k). If the relative acceleration of the base and rover is small during the di erencing time, then a set of four `Doppler' measurements can be processed to yield an accurate estimate of relative velocity (4 i (k) 4 i (k 1))=dt = h i (k)4v(k) + (Æmpi(k) + Æ i(k)) =dt (B.14) where dt is the di erencing time, and h i (k) = h i (k 1) has been assumed. This assumption is accurate for small dt. If one antenna (the base) is known to be at rest, then earth relative velocity of the second antenna (the rover) results. Carrier phase (once the integer ambiguity is resolved) gives a very accurate measure of range, but requires continuous phase lock. Loss of lock requires that the integers be re-estimated. Doppler gives a very accurate estimate of velocity only, and does not require continuous phase lock. Various methods have been suggested to utilize phase information (e.g., [5, 8, 20, 23, 24, 26, 29, 31]). ( )

( )

( )

( )

( )

( )

( )

( )

( )

43

Wide and Narrow Lane Variables

This section develops the equations for the so called narrow-lane and wide-lane phase variables [25]. The interest in the wide-lane signal is that its wavelength is larger enough that the wide-lane variable is often used to facilitate the problem of integer ambiguity resolution. The phase measurements of eqn. (B.12) at the L1 and L2 frequencies for a single satellite can be written as (~ + N ) = fc r fc Ia + fc (mp + ) (B.15) (B.16) (~ + N ) = fc r fc Ia + fc (mp + ) where  = fc and  = fc , the common mode errors have been eliminated through di erential operation, and the 4's have been dropped for convenience of notation. For small di erential distances, the residual ionospheric error Ia should be small. Forming the sum and di erence of equations (B.15-B.16) results in (~ + ~ ) + (N + N ) = ( fc + fc )r ( fc + fc )Ia + fc (mp + ) + fc (mp + ) (B.17) (~ ~ ) + (N N ) = ( f f )r ( f f )Ia + f (mp + ) f (mp + ) (B.18) 1

2

1

1

1

2

2

1

2

1

2

1

2

1

1

2

2

2

1

2

2

1

1

2

1

2

1

1

c

By de ning

2

2

2

2

c

w n

c

1

1

1

1

c

c

1

1

1

1

2

2

c

2

2

2

2

c f1 f2 c f1 + f2

= =

(B.19) (B.20) (B.21)

eqns. (B.17) and (B.18) are written in meters as (~ + ~ )n = r Ia (N + N )n + n (mp + ) + n (mp + ) (~ ~ )w = r + Ia (N N )w + w (mp + ) w (mp + ) 1

2

2

1

1

2

1

2

1

1

1

1

1

2

1

The pseudo-range estimates can be processed similarly yielding ( ~  )w = r Ia + w (MP +  ) 1

1 ( ~1 1

2

2 + 22 )n

(B.22)

2

2

(B.23)

2

w (MP2 + 2 ) 1 2   r + Ia + n (MP1 + 1 ) + n (MP2 + 2 ) 1 2 1

=

2

2

(B.24)

1

(B.25)

Note that the right hand sides of eqns. (B.23) and (B.25) are directly comparable. Since the residual ionospheric delay and the carrier noise are expected to be small and the coeÆcient of the code noise is signi cantly less than one, the di erence of the two equations, described as ( ~ + ~ )n (~ ~ )w = (N N )w + n (MP +  ) + n (MP +  ) 1

1

2

2

1

2

1

2

1

1

1

2

2

2

w  ( mp1 + 1 ) + w (mp2 + 2 ); (B.26) 1 2 should provide a basis for estimating the wide lane integer (N1 N2). The standard deviation of the noise on each estimate of (N1 N2) produced by eqn. (B.26) is approximately 0.7 times the code multipath in

44

meters. Therefore, the correct integer ambiguity can be reasonably expected to be within three integers of the estimate from eqn. (B.26). Averaging of the above estimate of (N N ) does not signi cantly decrease the estimation error unless the sampling time is long (i.e., minutes ) since the multipath is slowly time varying. Instead, an integer search will be required around the estimated value. Once this search is complete and (N N ) is determined, eqn. (B.23) is available for accurate carrier phase positioning and for aiding in direct estimation of the N and N variables. 1

1

2

2

1

2

Integer Ambiguity Resolution

Due to the dependence of eqn. (B.26) on the code measurement, which may be corrupted by signi cant levels of multipath, that equation is usually not used alone, but only to initialize an integer search. One such integer search approach based on least squares estimation and hypothesis testing [24, 42] is presented below. The method will be presented for a generic phase measurement and wavelength, and works equally well for any of the four phase variables previously discussed. Consider a set of n > 4 di erentially corrected phase measurements of wavelength  "

~p ~s

#

"

Np + Ns

#

"

#

Hp x + = Hs

"

p s

#

(B.27)

where the measurements are partitioned into a primary and a secondary measurement set. The ambiguity resolution algorithm proceeds by hypothesizing a set of integers for the primary set of measurements and using the secondary set of measurements as a test of each hypothesis. The primary set of measurements must have at least four satellites and these four should be selected to have a reasonable GDOP . Assume also that at the initial algorithm step, x = 0, var( ) = R =  I , and x   . These assumptions, although realistic are not necessary for the algorithm to work, but allow a theoretical basis for the algorithm derivation by the standard recursive least squares process. The search algorithm will be implemented as a nested triple `for' loop. Prior to initializing the loop, the following variables should be de ned to minimize calculations within the loop: 3

x = Hp ~p Bp = Hp   K = HT H Hs

(B.28) (B.29) (B.30)

1

1

1

The meaning of each variable is de ned below. The algorithm proceeds as follows: 1. Hypothesize a set of integers Np = [0; N ; N ; N ]T for the primary measurements. Each of the three `for' loops counts over a range of one of these three integers (i.e., Ni; i = 2; 3; 4). If m integer values on each side of a nominal integer are hypothesized, then the entire algorithm will iterated (2m + 1) times. The rst integer is arti cially set to zero to make the solution unique. Note that without this assumption, all the integers and the clock bias estimate could be increased by one wavelength without a ecting the position solution. The clock bias that results from the following algorithm is ctr N . If it is necessary that the code and phase clock estimates match, then N can be estimated based on the average of the discrepancy. 2

3

4

3

1

1

3

Geometric Dilution of Precision. Good GDOP implies that the measurement matrix H is well conditioned

45

2. For the hypothesized integer vector, calculate the resulting position xp = Hp (~p + Np) = x + BpNp: This position estimate has variance Pxp = HTp 1 I Hp Pxp =  (HTp Hp) : 1

(B.31)

1

1

3. Predict a value for the secondary measurement using xp 1 ^s = Hs xp : 

(B.32)

4. Calculate the residual between the predicted and measured secondary phase res ^ sp = (~s ^s ): (B.33) This quantity is biased by the secondary integer, which is estimated as N^ s = round(res ^ sp): (B.34) This choice of N^ s does not guarantee a minimal residual for the corrected position due to the possible ill-conditioning of H, but is still used as the alternative is another search over N^ s to nd the minimizing integer set. 5. Correct the secondary phase residual for the estimate secondary integer estimate and convert to meters res ^ sm = (res ^ sp + N^ s): (B.35) 6. Calculate the correction to xp due to the secondary measurements. This requires calculation of the gain vector K = Pxs HTs R . Using the standard recursive least squares equations, Pxs = Pxp + HTs 1 I Hs P = HT Hp 1 + HT Hs 1 1

1

1

1

xs

Therefore, and

Pxs =

p

 (H

T

s



H)

1

:



K = HT H HTs

(B.36)

4x = Kres ^ sm

(B.37)

1

7. Calculate the output residual of the position corrected by the secondary measurement 02 3 1 0 B6 C B6 0 777 C B6 C B 6 7 4y = B6 0 7 H4xC C B6 7 C 0 @4 5 A

res ^ sm

46

(B.38)

8. Calculate the -squared variable q associated with 4y and use it to evaluate the hypothesized integer Np . The properly de ned -squared variable q would account for the covariance matrix of 4y which is straightforward to calculate give Pxs. However, due to the required number of on-line calculations the covariance matrix is dropped in favor of the following simpler calculation q=

4yT 4y :

(B.39) 4 If the hypothesized integer set Np is correct, then q will be small. Depending on the number of satellites available, the noise level, and the satellite geometry it may take several iteration epochs to identify the correct integer set. In this process other criteria such as the previously estimated position, the position variance, and the size of Ns can also be used to eliminate candidate integer sets. Note that the algorithm is equally valid for the L1, L2, and wide-lane frequencies. In the implementation of this project, the algorithm is used rst to determine the wide-lane integers. Then, the increased position accuracy achieved using the wide-lane phase range facilitates the L1 or L2 integer initialization and search. In the implementation of this project, the above algorithm is processed (in each mode: widelane, L1, or L2) until only a single vector integer candidate remains. The algorithm is then processed again using the identi ed integer vector as the initial condition until a single vector integer candidate remains. The integer vector is only accepted as correct if the output vector identically con rms (i.e., matches) the initial condition. Once the correct integers are identi ed, a similar algorithm is run with q monitored to determine if cycle slips have occurred. Cycle slip detection is also implemented by comparing and monitoring the code, widelane phase, L1, and L2 ranges. n

B.1.2 Di erential GPS Operation

The common mode error sources described previously severely limit the accuracy attainable using GPS. These errors are generally not observable using a single receiver unless its position is already known. However, the common mode errors are the same for all receivers in a local area. Therefore, if the errors could be estimated by one receiver and broadcast to all other receivers, the GPS accuracy could be substantially improved. Three Di erential GPS (DGPS) techniques are described in [16]. Range based di erential GPS was selected for this implementation as this approach allows the most exibility in a system involving multiple roving vehicles. Di erential GPS involves a GPS receiver/antenna at a location (xo; yo; zo), a receiver/antenna at an unknown possibly changing position (x; y; z), a satellite at the calculated position (xsv ; ysv ; zsv ), and a communication medium from the rst receiver to the second receiver. In the discussion of this section, the former receiver will be referred to as the base. The latter receiver will be referred to as the rover. A typical DGPS setup is illustrated in Figure B.1. Note that one base station can service an unlimited number of roving vehicles. Also, wide area DGPS techniques are being developed which would allow a large area (e.g., the continental U.S.) to be served by a network of on the order of 10 base stations. The correction equations of this subsection will be derived for a single satellite. The process is identical for all satellites of interest. The derivation is also only presented for pseudo-range observables, but is also implementable for the L1, L2, and Doppler signals. The GPS observables at the base and rover are, respectively, the pseudo-range easurements ~o and ~r : ~o = ((X xo ) + (Y yo) + (Z zo) ) : + cto + ctsv + cta + SA + E + MPo + o (B.40) ~r = ((X xr ) + (Y yr ) + (Z zr ) ) : + ctr + ctsv + cta + SA + E + MPr + r (B.41) 2

2

2 05

2

2

2 05

47

   

         A  

A Z  Z        AA       Z      ZZ      H   A   Z S HH   AA   Z   S HH    Z   S HH A SS  HHH ZZZ AA  = ZZ Sw   ) HHHH A Z HH ZZ AA H HH XXXX ZZ AA x H X X @ H XXXX Z A x@@ H @ XXXX HHHH ZZ~U ? XXXXX Hj XXXz

    

  

Base Station

Di erential Corrections

c c

...... .................. ...... .

Rover vehicle

Figure B.1: Di erential GPS Scenario (Source: [22]) where the error terms are all de ned as in eqn. (B.1) and the subscripts o and r denote the non-common mode errors corresponding to he base and rover respectively. If the rover observable ~r is replaced with ~r ~o, the result is ~r ~o = (rr + ctsv + SA + E + cta + MPr + r ) (ro + ctsv + SA + E + cta + MPo + o ) = rr ro + (MPr + r ) (MPo + o) = rr ro +  (B.42) where rr = (xsv x) + (ysv y) + (zsv z)  : + ctr , ro = Ro + ctro , Ro = (xsv xo ) + (ysv yo ) + (zsv zo) : , and  = (MPr + r ) (MPo + o ). Note that ro is calculatable due to the known location of the base receiver. The ctro term can be estimated and removed at the base station. Since non-common mode noise  has a standard deviation between 0.1 and 4.0 m, depending on receiver design and multipath mitigation techniques, DGPS position accuracy is much better than standard GPS accuracy. Cancellation of the common mode noise sources in eqn. (B.42) assumes that the rover is suÆciently near (within 10-50 mi.) the base station and that the corrections are available at the rover in a timely fashion. Latency compensation is discussed in [37, 17]. Wide area DGPS techniques suitable for continent wide DGPS service are currently being developed [27]. The common mode noise sources are continuous and slowly time varying (see Figure B.2) and have signi cant short term correlation (see Figure B.3). Denote these common mode errors as DGP S (t) where DGP S (t) = (ctsv (t) + SA(t) + E (t) + cta(t)): (B.43) Note that DGP S (t) is straightforward to estimate at the base as ro (t) o(t). The correction DGP S is usually broadcast in range based di erential GPS (DGPS) instead of o, as it is compensated for base receiver clock bias and its range is much smaller than that of o. Given that the corrections DGP S are 2

2

2

2

2 05

2 05

48

100

Differential Correction, meters

80 60 40 20 0 -20 -40 -60 5.945

5.95

5.955

5.96 5.965 5.97 5.975 GPS Time of Week, seconds

5.98

5.985

5.99 x 10 5

Figure B.2: Example Di erential Corrections available at the rover for the satellites of interest, the DGPS positions are calculated as

x^ = (H^ T H^ ) H^ T (~ + DGPS) :

(B.44) To successfully implement a range space DGPS system, the base station must at a minimum broadcast the following set of information for each satellite: satellite id, range correction, ephemeris set identi er, and a reference time. A roving receiver then selects the most appropriate set of satellites for its circumstances. Normally, the base would also broadcast the range correction drift rate for each satellite to allow for latency compensation. For the implementation of this project, the base also broadcasts L1 and L2 phase and Doppler corrections. A main advantage of range space techniques is that the roving receiver can choose the best set of satellites for its circumstance. Implementation of a range space system however requires either that the user receiver is \DGPS compatible" or that the user handle raw GPS data. DGPS compatible implies that the receiver is capable of being connected to a communication medium to receive and process DGPS range corrections. 1

49

1

Normalized SV8 Correction Covariance

0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1

0

100

200 300 Covariance Time Lag, seconds

400

500

Figure B.3: Normalized Covariance Sequence for the DGPS Corrections for SV8.

50

Appendix C

Control State Calculation The purpose of this appendix is to describe the algorithms used to determine the control state based on the DGPS/INS navigation state information. The data ow for this calculation is shown in Figure C.1. The DGPS/INS state is an absolute quantity in the sense that it generates the absolute vehicle position in an earth relative frame. Since the control objective is to follow a trajectory, the control system utilizes a trajectory relative state vector. As Figure C.1 shows, a lane trajectory and the DGPS/INS state are inputs to an algorithm that outputs the control state vector as required for the existing PATH vehicle control algorithm. The structure of the PATH vehicle control algorithm was not changed, although the parameters of the control algorithm were tuned. The following section discuss a method for de ning the lane trajectory and a method for translating the DGPS/INS state into the control state. The discussion is introduced by a short review of a few necessary concepts from analytic geometry. Lane Trajectory IMU

INS

p,v, h(p)

 x EKF



Control State Calculation

d . d .

^  ~ +  + _ DGPS

Figure C.1: Architecture for the DGPS/INS control state calculation.

51

C.1 Review of Analytic Geometry Given a function p(s) de ning a two dimensional trajectory as a function of arclength s this section reviews various concepts from analytic geometry that will be used in the subsequent sections. Let u(t) denote the speed of travel. Then, ds(t) u(t) = = kv(t)k

(C.1) where v(t) is the velocity of a point along the trajectory. Since the point p is con ned to the trajectory, v(t) is tangent to the trajectory by de nition. Note that dt

s(t) =

Z t 0

u( )d

(C.2)

which will be used later in both the curve t and control algorithms. When the trajectory is de ned by p(s) = (s)T [n; e] = (s)T ; then the velocity along the trajectory satis es v(t) = dp(t)

(C.3) (C.4)

dt dp(t) ds ds dt  T d u(t): ds

= =

(C.5) (C.6)

Since u(t) is by de nition the magnitude of v(t), the vector  T T = vu((tt)) = d  (C.7) ds is a unit vector tangent to the trajectory. De ne the components of T as T = [T ; T ; 0]: Then, the trajectory heading is de ned by (C.8) p (s) = arctan2(T ; T ): The normal to the trajectory, de ned positive to the right in the direction of travel, is by the right hand rule ^i ^j k^ 2 T 3 N = T T 0 = 64 T 75 : (C.9) 0 0 1 0 The trajectory curvature vector K is the derivative of T with respect to s. Since T is a unit vector, only its direction can change. The scalar trajectory curvature  is the norm of K. Therefore, with the trajectory de ned as in eqn. (C.3),



T

dT

d 

 (C.10) = =

;

ds

ds 1

2

2

1

2

1

2

1

2

2

which has units of m . Therefore, for the curvature to be well de ned, the basis function must be at least twice di erentiable. Based on the curvature  and speed of travel along the trajectory vT , the magnitude should be RvT where R =  . The sign of the desired vehicle rotation of the vehicle rotation rate in radians s 1

1

52

rate depends on the direction in which the tangent is rotating and can be determined as the sign of the third component of T  K, so that rc

= ((T  K)  [0; 0; 1]) vT = kTkkKk sin(T K )vT = vT sin(T K ) = sin(T K ) RvT

(C.11) (C.12) (C.13) (C.14)

where rc denotes the desired yaw rate command and T K is the angle between T and K which is always 90 degrees.

C.2 Curve tting data set. Section C.3 presents a curve tting approach. Figure C.2 shows, for reference purposes, an example of the GPS/INS data used for the curve tting procedures. This data was acquired by GPS/INS at 15 Hz while the vehicle was driven along the trajectory de ned by the magnets. During the data acquisition, magnetometer control was used for lateral positioning. The speed was manually controlled at approximately 8 ms , resulting in trajectory points separated by approximately 0.5 m. The trajectory starts and ends with straight segments and has three turns each with a radius of curvature of approximately 800 m. The vehicle was driven in the north bound direction. The three turns are clearly evident in the position plots, the east velocity ve plot, the heading plot, and the z-axis gyro plot. Comparing the inertial signals of Figure C.2 with the corresponding plots of Section 5.1, it is clear that the inertial signals have signi cantly more energy when the vehicle is in motion. This is not a change in the system noise properties. The increased signal energy is the result of vibration primarily from the road surface. This is high frequency energy that is attenuated through the integration process of the INS to result in the accurate estimates of velocity and position. Figure C.3 shows the input data for the trajectory tting algorithms. This curve tting input data is derived from the position and velocity data of Figure C.2.

C.3 Curve tting This section presents aa algorithm to translate the GPS/INS navigation state into the control state (i.e., trajectory relative data to serve as the input to a control system). An example result of the algorithm is shown in Figure C.6. Given a set of data D = fti; ni; ei ; vni ; vei gNi corresponding to the time stamp, north and east position coordinates, and north and east components of the velocity acquired along a trajectory, this subsection describes a method for tting a smooth trajectory to D. The arclength de ned by eqn. (C.2) can be approximated for 1 < i  N as =1

ui si

= =

q

vn2 i + ve2i si 1 + ui (ti

where s is assumed to be zero. 1

53

ti

1

)

(C.15) (C.16)

Let the curve t trajectory be de ned as p(s) = [n(s); e(s)] = (s)T [n; e ] (C.17) where n; e 2