Navigatie Cu GPS

Navigation for Control of Ground Vehicles
David M. Bevly Assistant Professor Department of Mechanical Engineering Auburn University, AL 36849-5341 Director of Auburn University's GPS and Vehicle Dynamics Lab (GAVLAB)
Presentation Overview
Overview of GPS (73 slides)

History of GPS Signal Structure Measurements and Accuracy IMU errors Introduction of Kalman Filtering JD and DGC Examples Navigation Errors
IMU Modeling and Navigation (27 Slides)
GPS/INS Integration (84 slides)

Lidar and Vision Navigation (30 Slides) GPS/INS for Estimation of Vehicle States and Parameters (30 Slides)
GPS and Vehicle Dynamics Lab 2
Motivation
Future stability control systems for passenger vehicles will use a precision navigation solution

Currently, a need for more vehicle information Also a need to improve accuracy of information Lane keeping systems can use position information
Autonomous ground vehicles require accurate and robust navigation information

The DARPA Grand Challenge Military vehicles, especially Future Combat Systems (FCS)
Armored Robotic Vehicles (ARV) Robotic Armored Assault Systems (RAAS)
Eventually, highway vehicles might be automated
GPS and Vehicle Dynamics Lab
Control of Vehicles
need to know vehicle:

position velocity direction of travel orientation
above measurements can be made using GPS can use the measurements (for example) to:

control farm vehicles improve safety systems in passenger cars

Coordinate Nomenclature
Vz , r , p Vx Vy p , q
V = velocity r = yaw rate p = roll rate q = pitch rate = yaw angle = roll angle = pitch angle = road grade
Coordinate Nomenclature
V = velocity r = yaw rate = heading (or yaw) = vehicle course = steer angle = body sideslip angle = tire sideslip angle
Global Positioning System (GPS)

24+ satellites in well known orbits providing precise ranging source 6 orbital planes 55 inclinations 12 hour orbits 20,200 km orbits Ground track repeats every 23:56:04
How GPS Works

measure the transit time for a signal from SV to user
multiply by c to get range triangulate ranges to get position (and time)
GPS Facts
There are more than 100 times as many civilian users than military users. 5 million recreational GPS devices were shipped in 2003, with a projected growth rate of 31% each year through 2009. Economics:
The cost of maintaining the GPS satellite system is $750 million each year, including replacing aging satellites. The direct economic impact of GPS is projected to exceed $50 billion by 2010
http://gps.losangeles.af.mil/jpo/gpsoverview.htm
GPS Signal
(-160 dBw ~ 10-16 watts)

digital code:
GPS Carrier Wave: L1=1575.42 MHz
(satellite info)
19 cm
Encoded Digital Signal
satellite # time location velocity
Roughly equivalent to viewing a 25-watt light bulb from a distance of 10,000 miles.
GPS Time
The continuous atomic timescale used on the satellites and control stations
GPS time started on January 6, 1980 at 0h

Measured as seconds into the week Rolls over on Sunday at 0h
Does not account for leap seconds
Typically accurate to 50ns GPS weeks are numbered sequentially

Start from 0 at 0h on January 6, 1980 Increment every Sunday at 0h
Currently ahead of UTC time by 14 seconds Ex: UTC 10:34:25; GPS 10:34:39
11
3 Segments of GPS
Control Segment: 5 fixed location earth-based monitor stations

Stations located at: Colorado Springs, Ascension Island, Diego Garcia, Kwajalein, and Hawaii Responsible for maintain each of the satellites positions, clocks, etc. Track the GPS satellites and generate and upload the navigation data to each of the GPS satellites. Each satellite transmits at L1 (1575.4 MHz) and L2 (1227.6 MHz)
Space Segment: 29 satellite constellation
User Segment: all users, military and civilian, commercial and individual, who utilize the GPS signal
Current GPS Signals

L1 (1575.42 MHz)

Coarse/Acquisition (C/A) and P(Y) Code Civilian Use P(Y) Code Military Use
L2 (1227.60 MHz)

13
Future GPS Signals

NAVSTAR (http://gps.faa.gov/)
L2 Civil Signal: L2C

Broadcast as L2 with similar power spectrum to C/A Uses two PRN codes per satellite
L5
Civilian Signal broadcast at 1176.45 MHz Available 2015??
M-code
New military code
L1C (1st launch scheduled 2015) GALILEO (2010-2015 for full constellation) GLONASS (??) Australia, Japan, China, etc.
Other Countries

GPS Broadcast Signal Structure

Each satellite transmits the precise time (UTC-USNO), the complete parameters of its orbit, and the major parameters of all other satellites orbits
These parameters are collectively known as ephemeris data.
The Navigation message which includes the ephemeris data from the satellite is 30 secs. in duration and is transmitted in digital form at a rate of 50 bps. This data transmission modulates the GPS carrier wave using binary phase-shift keying (BPSK)
15
Gold Codes and Spread-Spectrum Transmission

Gold Codes are a family of unique binary sequences which have very low crosscorrelation with other sequences in the family and low auto-correlation as well. Modulating each GPS satellites signal by a unique Gold Code, known as the PRN number, spreads the signal over a wider bandwidth, which provides noise rejection and enables multiple access (CDMA).
Allows satellites transmit on the same frequency at the same time without interfering with each other
16
GPS Signal Structure

3 Components
Carrier Wave
L1, L2
Code Signal
C/A, P(Y)
Navigation Data
Satellite Information
17
Carrier Wave
L1 at 1575.42 MHz (154 x 10.23 MHz) L2 at 1227.60 MHz (120 x 10.23 MHz) Modulated with Code and Navigation Data using Binary Phase-Shift Keying (BPSK) C/A and P(Y) are transmitted orthogonally on L1
Code Signal
Code Division Multiple Access (CDMA) Course Acquisition - C/A

Gold Codes Code Period of 1 ms Anti-Spoofing Mode Code reset each week
Precision Code - P(Y)

19
Navigation Data
Navigation Data from the Data Bits

50 bits/s Bits(30) Words(10) Subframes(5) Frames (or Page) 5 Subframes:

1) 2) 3) 4) 5)
Clock Correction & Satellite Quality Ephemeris Ephemeris Almanac & Ionosphere & UTC Corrections Almanac
GPS Signal Structure

S Lli (t ) = 2 Pc XGi (t ) Di (t ) cos(1t + ) + 2 Pp XPi (t ) Di (t ) sin (1t + )
S Lli (t ) = 2 Pc XGi (t ) Di (t ) cos(1t + )
Signal = C/A x Data x Carrier
21
GPS position solution requires Raw Ephemeris, Pseudoranges, and Time

Inputs:
Satellite positions
deduced from Nav frame emphemeris & time
Pseudoranges
Measurement based on time delay from user to satellite
Outputs: Position (X,Y,Z)

2 1
4
22
Pseudorange
Satellite Positions vs. Time Pseudoranges

Definition: iT
Range calculated by taking propagation time multiplied by speed of light Since clocks unsynchronized, clock errors are present -> pseudorange Measurement epoch occurs by shifting replicated code until correlation achieved (C/A code repeats every 1 ms)
= c(tu t si ) + cbu
Pseudorange errors: i = Algorithm:
Note 1 s error in time = 300 m error
iT + Di cbi + c(Ti + I i + vi + vi )
iT =
(SatPos.x User.x )2 + (SatPos. y User. y )2 + (SatPos.z User.z )2

GPS Receiver
RF down conversion to IF GPS and Vehicle Dynamics Lab
24
The PVT Solution Process

1. 2. 3.
Received RF signal is down-converted to a lower, intermediate frequency (about 5 MHz) After signal acquisition, both the carrier (and any Doppler shifts) and the PRN code sequence are tracked. The outputs of the tracking loops are Doppler frequency (from the carrier loop), transport time delay (from the code loop), and the navigation message of the satellite.
4. 5. 6.
From the navigation message, satellite position is calculated Using the transport time delay, Doppler frequency, and satellite position, the range to the satellite and velocity towards the satellite are calculated. By using 4 satellites or more, an extended Kalman filter or Least Squares algorithm combines the range and velocity to compute user position.
GPS Measurement Model: Pseudorange as Relative Position

Pseudorange is distance from user to satellite
s = ( x s xu ) 2 + ( y s yu ) 2 + ( z s z u ) 2 + bs + s
Linearized measurement model is:
u , k 1 x s ,1 ) (x 1 1 , k 1 u , k 1 x s , 2 ) 2 (x . 2 , k 1 . . = . . . . x s ,m ) (x m u , k 1 m , k 1 u , k 1 y s ,1 ) (y u , k 1 y s , 2 ) (y u , k 1 z s ,1 ) (z u , k 1 z s , 2 ) (z
1 , k 1
1 , k 1
H k ( xu , x s ) s = * xk x k
2 , k 1
. . . u , k 1 y s , m ) (y
2 , k 1
. . . u , k 1 z s , m ) (z
m , k 1
m , k 1
1 1 x y 1 z 1 b 1 1
For 1,2m satellites -> m measurements Linearized about most current position estimate
Least Squares
xu y u x = zu ctu
x = H R H
T
H T R 1
1 = 2 3 4
a x1 a x2 H= ax3 a x 4
a y1 ay2 a y3 ay4
a z1 1 a z 2 1 a z 3 1 a z 4 1
u xj x axj = j r
i = r
u yj y a yj = j r
u zj z a zj = j r
(x
u )2 + ( y j y u )2 + (z j z u )2 x
j j =
27
User Solution Calculation

From the linearized pseudorange equation
= Hx
The position error can be estimated using LS as Calculating the covariance of x yields the 4x4 matrix D
x2 covariance terms 2 2 T y =2 D = UERE UERE H H 2 z 2 b covariance terms
x = H H
T
H T
28
Dilution of Precision (DOP)

Value based solely on satellite geometry High DOP value increases the negative effect of User Equivalent Range Errors (UERE) Ideal geometry: 1 satellite directly above, others (at least 3) equally spaced along the horizon
29
DOP Values
Geometric (GDOP) Position (PDOP) Horizontal (HDOP) Vertical (VDOP) Time (TDOP) Note: TDOP is in m, not s
2 2 GDOP UERE = x + y + z2 + b2
2 2 PDOP UERE = x + y + z2 2 2 HDOP UERE = x + y
VDOP UERE = z TDOP UERE = b
30
Position Accuracy
Horizontal position accuracy is often assumed to have a bivariate Gaussian (normal) distribution x + 2 xy y
2
PDF ( x, y ) =
2 x
x,y
2 2 x y 1 x ,y
e
6 4 2 0 -2 -4 -6 y
2 2 1 x ,y
2 y
This results in probability ellipses Parameters come from solution calculation covariance
Data 1- (39.3 % inside) 2.45- ( 95 % inside)
-5
0 x
31
Kaplan, E., Hegarty, C., Understanding GPS Principles and Applications
Radius Accuracy Standards

Distance Root Mean Square (DRMS)
2 2 DRMS = x + y = HDOP UERE
Contains ~63-69% of the samples 2 DRMS contains ~95-98.5% of the samples Exact percentage within radius depends on the circularity of the ellipse (correlation coefficient) Closely matches Gaussian probability
Kaplan, E., Hegarty, C., Understanding GPS Principles and Applications
Radius Accuracy Standards

Circular Error Probable (CEP)
Radius of a circle containing 50% of samples Originally used for military targeting accuracy
CEP 0.75 DRMS
As before, the exact ratio depends on the correlation coefficient

Radius CDF Accuracy

CDF % 1 - CEP DRMS 2 DRMS 39.3 50 69.9 99.2 Actual % 40.2 50 67.1 96.8 Approximate % 39.3 50 63-69 95-98.5
100 y 6 4 2 0 Data -2 1- -4 CEP DRMS -6 DRMS 2 -5 0 x 5
Note:

80
Data CDF
Plot differences due to non-circularity CEP = 0.76 DRPS
Probability
60
40
20
0 0
r /
34
35
GPS Errors from the Satellites

Ephemeris Errors
Difference in transmitted and actual satellite location (Slowly varying) SA contribution (now off) Based on stable atomic clocks
Satellite Clock Errors

36
GPS Errors from Atmosphere

Ionosphere Errors
Free electrons cause delay of signal proportional to inverse of carrier frequency squared Without SA, the largest error component Requires model to correct Highly variable Smaller contribution to error Affects both L1 and L2 equally
Troposphere Errors

GPS Errors from User

Multipath
Reflected signals masking actual correlation peak Reduce by using cut-off angle, good antenna location, antenna and signal processing techniques Thermal noise Software accuracy
Receiver Errors

Parkinson and Spilker, Global Positioning System: Theory and Applications Vol. 1, AIAA, 1996 Ted Driver, Statistical Analysis of Military and Civilian Navigation Error Data Services, Proceedings of the 2006 ION-GNSS Conference
1 GPS Error Budget (in meters)

UERE (User Equivalent Range Error)
C/A Code P Code Total 2.1 2.1 4.0 0.7 1.4 0.5 5.3 5.1 12.8 Bias 2.1 2.0 1.0 0.5 1.0 0.5 3.3 3.3 Random 0.0 0.7 0.5 0.5 1.0 0.2 1.4 0.4 Total 2.1 2.1 1.1 0.7 1.4 0.5 3.6 3.3 8.3 6.7 39 Bias 2.1 2.0 4.0 0.5 1.0 0.5 5.1 5.1 Random 0.0 0.7 0.5 0.5 1.0 0.2 1.4 0.4
Error Source Ephemeris Data Satellite Clock Ionosphere Troposphere Multipath Receiver Measurement UERE, rms Filtered UERE, rms
Vertical 1- errors (VDOP=2.5)
Horizontal 1- errors (HDOP=2.0) GPS and Vehicle Dynamics 10.2 Lab
Carrier Smoothing Synergizes Carrier and Code Phase Observations

Uses carrier phase observation and code phase observations to improve accuracy of pseudorange Smoothing algorithms use Doppler information from carrier frequency to correct raw code phase observation for more accuracy pseudorange Advantages: Mitigation of tracking noise and effects of multipath Smoothing of code phase pseudorange for pseudorange noise mitigation Dual-Frequency smoothing can improve the solution in terms of the ionospheric errors
GPS Errors
Receiver 1&2:RTD Receiver 3: Saphire Receiver 4: Starfire
41
GPS Errors
Common Mode Errors can easily be seen by two GPS receivers
42
GPS Errors (Effect of Ground Multi-path)

Common Mode Errors can easily be seen by two GPS receivers
Difference between two receivers on the ground Difference between two receivers with ground plane
43
Effect of Velocity/Acceleration on a Cheap GPS Receiver

Driving around test track at different speeds GPS Error=f(V) Possibly due to lack of carrier PLL
44
Relative Positioning
Determination of the baseline vector between a known receiver location and arbitrary receiver location

If receivers are in close proximity (50km), they are subjected to very similar errors Differencing measurements from receivers removes errors, providing accurate baseline measurement
Carrier single differencing removes atmospheric errors and
satellite clock biases Carrier double differencing removes receiver clock bias Code double differencing removes atmospheric errors, receiver and satellite clock biases, and cycle slip effects (more noisy) Carrier triple differencing removes cycle slip effects
45
Differential GPS Correction

Method to improve the positioning and timing performance of GPS Use base stations to measure error signals and calculate differential corrections DGPS can be categorized in 3 different ways

Absolute or relative differential positioning Local area, regional area, or wide Area Code based or carrier based
Kaplan Edition 2: Understanding GPS Principles and Applications
46
Differential GPS (DGPS)

Error Source Ionosphere Troposphere SV Clock SV Ephemeris Receiver Noise Multi-path SA Total GPS (m) 5 0.5 1 1 0.5 0.5 30 ~5-40 DGPS (m) ~0 ~0 ~0 ~0 0.5 0.5 ~0 ~1.0
Use a Base Station (at known location) to correct common GPS errors
DGPS Accuracy
10 meter accuracy based on Federal Radionavigation Systems (FRS) report published jointly by the U.S. DOT and Department of Defense (DoD) Dependent on users distance from transmission source In 1993, the US DOT estimated error growth of 0.67 m per 100 km from the broadcast site Measurements of accuracy in Portugal suggest a degradation of just 0.22 m per 100 km
http://en.wikipedia.org/wiki/Differential_GPS
Recent results have shown troposphere errors can be significant in RTK systems over short baselines:
David Lawrence, et.al, Decorrelation of Troposphere Across Short Baselines, Proceedings of the 2006 IEEE/ION Positioning, Location, and Navigation Symposium (PLANS) GPS and Vehicle Dynamics Lab 48
NDGPS Coverage
49
Starfire and Omnistar

http://www.oznet.ksu.edu/pr_prcag/StaticDF04.htm
Omnistar
Starfire
50
Starfire DGPS vs GPS

Starfire/Beeline East vs North 15 Beeline Starfire 10
North (m)
10
15
20 20
10
10 East (m)
20
30
40
51
Starfire DGPS vs GPS

Probability First Receiver
Receiver 1 (GPS) 1- CEP DRMS 2 DRMS 1.63 m 1.92 m 2.77 m 5.54 m
Receiver 2 (Starfire) 0.424 m 0.500 m 1.09 m 0.547 m
100
50 Data CDF 0 0 100 1 2 3 4 5 6 7
Probability Second Receiver
50
Note: Noise is not Gaussian (long term bias drift)

20 10 y (m) y (m) 0 -10 -20 -20 0 x (m) 20 0.5 0 -0.5
0 0
0.2
0.4
0.6 r (m)
0.8
1.2
Receiver 1 Receiver 2 1- CEP DRMS 2DRMS -0.5 0 x (m) 0.5
52
DGPS Position vs time
53
Carrier Phase DGPS (RTK)

East v North = R = f ( N , ) Up
N+
User
Reference Antenna
local reference station required solve for integer ambiguity track carrier phase phase at reference antenna is broadcast to user positioning v software calculates R 3-D accuracy* = 2 cm
*Actual depends on baseline length (1 cm + 1 ppm)

More on RTK
L1 f=1.5 GHz
RTK Real Time Kinematic GPS RTK GPS calculates the relative position, R, between a rover and fixed base station to sub centimeter accuracy Integer ambiguity (IA), N, must be calculated

c = = 19 cm for L1 f
2 1 R= N + 360
Many published algorithms available Can take 20 minutes New techniques utilizing L1 and L2 (wide laning) are nearly instantaneous
L1 Signal 1 19 cm 2
55
Integer Ambiguity
The number of whole carrier cycles between a receiver and satellite

Carrier based DGPS technique utilizing the accuracy of a receivers phase measurement Estimates number of carrier cycles, N
Single frequency Employs estimation scheme, can take up to
Cycle slip is a sudden shift in the value of N when communication between satellite and receiver is compromised
30 minutes Dual frequency A wide lane or narrow lane approach can limit the possibilities of N, drastically reducing search time Triple frequency N can be nearly instantaneously solved using a third frequency, such as L5, GLONASS, or GALILEO
56
What is DRTK?
Dynamic RTK is the idea to use a moving, or dynamic, base station to calculate relative position between it and a rover L1 Signal
1 19 cm 2
57
Differences
RTK (Fixed base station)
IA algorithms are published Method well studied Cycle slip is minimized with good receiver Speed and distance ranges known
East v North = R = f ( N , ) Up
DRTK (Dynamic base station)

IA algorithms differ Method not well studied Cycle slip can occur easier May have detrimental effect on system Fix with IMU? Delay time What is acceptable? FCS vehicles may not have direct com link Speed range unknown Distance range unknown
58
N+
User
Reference Antenna
Applications of Relative Navigation

Develop relative navigation scheme to improve ground vehicle convoys
59
Limitations of the DRTK Method

Rover position measurement is a position relative to the moving base vehicle, not global position

Relative position is single vector from follower to moving base vehicle Global position is needed for path following
In situations where a specific path needs to be followed:

A vehicle might drop a temporary static base station The base vehicle could stop, forming a static base station Techniques utilizing relative measurement might be able to keep track of lead vehicle motion
60
Building DRTK
Building a DRTK system will allow modification of internal carrier tracking and IA algorithms Initial development with Novatel Superstar II receiver

5 Hz carrier phase output Low cost ($300)
Place in PC-104 stack with transmitter
61
GPS Position Accuracy (1)

Military Stand Alone (No SA) ~3m,
global coverage global coverage not all are global, but almost full US coverage

Civil Stand Alone (w/ SA) ~30m,
Code Phase Differential (DGPS) ~0.1m-1m
local reference station ~0.3m Coast Guard differential corrections ~ 0.5m WAAS ~1-3m Nation Wide DGPS (NDGPS) ~ 1-3m OmniStar VBS (~1m) & Omnistar HP (~10cm) JohnDeere Starfire ~10cm
Carrier Phase Differential (RTK) ~2cm,
local (~10km) coverage

High Accuracy (HA) NDGPS ~10Lab cm GPS and Vehicle Dynamics
62
GPS Velocity Measurements

V ~
No Reference Station Required
Uses Doppler Shift of Difference in two carrier measurements
generally a sample delay associated with the measurements
Accuracy
0.2-0.5 m/s with SA 3-5 cm/s without SA
Provides accurate measurements to correct IMU errors

Accuracy of GPS Velocity (no SA)

East Velocity (cm/s)
10 5 0 5 10 0 10 5 0 5 10 0 5 10 15 Time (min) 20 25 30 10 10 5 0 East Velocity (cm/s) 5 10 5 10 15 20 25 30 Mean = 0.7 cm/sec 1 = 0.9 cm/sec 10 5
North Velocity (cm/s)
North Velocity (cm/s)
Mean = 0.1 cm/sec 1 = 1.8 cm/sec
30 minutes of static data 1 = 1.8 cm/sec in each axis

GPS Velocity Based Heading Accuracy

GPS
east VGPS = tan V north GPS 1
Heading accuracy based on E-N GPS velocity noises

4 3.5 3 2.5
Error (deg)
=
vel
vel
vel
V
GPS
V = tan V GPS
1 up GPS
Monte Carlo Experimental
0.05 (rad) V
2 1.5 1 0.5 0 0
0 .1 (rad) V
GPS and Vehicle Dynamics Lab Speed (m/s)
10
15
20
25
30
35
40
65
Cohen C.E., Parkinson, B.W., McNally, B.D., Flight Tests of Attitude Determination Using GPS Compared Against an Inertial Navigation Unit, Navigation: Journal of the Institute of Navigation, Vol 41, No. 1, Spring 1994.
GPS Attitude
No Reference Station Required Accuracy Depends on Antenna Spacing (Not Velocity)
GPS Wave Front
3 antennas roll pitch yaw (Common Clock)
66
GPS Attitude Accuracy

Accuracy ~ 0.3/L degrees (based on 3-4 mm carrier noise)
67
Uses of GPS
?
Measuring Plate Movement Using GPS
69
Traffic Monitoring
Mean traffic position lies along the centerline of the lane
Measurements using a Starfire DGPS Receiver
70
Pseudolites
Ground-based transmitter that emits GPS-like signals Pseudo-satellite -> pseudolite (PL) Many methods of implementation

C/A Codes Frequency Offset Pulsing Scheme Signal augmentation Data link enhancement
Objectives:

Pseudolite Types
Direct Ranging PL

Original (before GPS satellite) Satellite on the ground GPS scheme inverted (stationary receivers) Transmit data via GPS signal (max of 1000bps vs. 50bps GPS) Reflects message from GPS satellites
Mobile Pseudolite
Digital Datalink Pseudolite
Synchrolites
72
Other Positioning Systems

LORAN (LOng RAnge Navigation) VOR (VHF Omnidirectional Range) DME (Distance Measurement Equipment) TACAN (TACtical Air Navigation) Arc-Second Indoor GPS Ultra-Wide Band (UWB)
Debate on interference with GPS

GPS References
System: Theory and Applications Volume 1&2, AIAA, 1996. Kaplan, Understanding GPS Principles and Applications, Artech House Publishers, 1996 Misra and Enge, GPS: Signals, Measurements, and Performance, Ganga-Jamuna Press, 2001
Parkinson and Spilker, Global Positioning
74
Modeling IMU Navigation Performance for GPS Coupling Algorithms
75
Motivation
Initial investigation of IMU models and IMU error sources Predict navigational accuracy during loss of GPS (function of IMU and dynamics of the trajectory). Understand the limits of GPS/INS performance (especially in advanced integration techniques such as ultra-tightly coupled GPS/INS).
76
IMUs (Inertial Measurement Units)

Usually Consist of 3 accelerometers and 3 rate gyroscopes (MEMS, FOG, or RLG)
LN200 IMU
Sentera IMU with AD MEMS sensors
Analog Devices MEMS Accelerometer and Gyroscope

77
IMU Coordinate Transformation

r r r r Calculate a , V , , x in the Body Fixed Frame (On board IMUs)
Transfer to Earth Fixed Frame x
X y = [R ] Y z Z
[R ] = [R ][R ][R ]
Y
r r Calculate a sat , Vsat to determine GPS Doppler shifts in order to

compensate tracking loops Problem: IMU Errors
X
Y
Z
X
X
78
Simple IMU Model

Used to statistically simulate an IMU Assumes no scale factor error Three Main Error Sources:

Moving bias, Turn On Bias, and Random Noise Moving bias always initialized to zero (since offset bias exits)
g r = r + c r + br + w gyro
E[br ] = 0 E b =
2 r
& + c&x& + b&x& + waccel a=& x

E[b&x& ] = 0
2 E b&x2 = & accelbias
[ ]
2 gyrobias
& = b r
w gyro ~ N 0 ,
br + w gyrobias
fs
[ ]
1 & b&x& = b&x& + waccelbias
2 gyro
w accel ~ N 0 ,
&x&
2 accel
fs
wgyrobias ~ N 0,
2 gyrobias
2 waccelbias ~ N 0, accel bias
]
79
Other Accelerations
Accelerometers Measure specific force not true acceleration Must compensate for Gravity Field and Centripetal (and Coriolis) Accelerations
& + V y r + b x + Gc + waccel ax = & x
r = yaw rate = pitch = roll V = Velocity Gc = 9.81 m/s2
80
& + Vx r + by + Gc + waccel ay = & y

Inertial Sensor Error Modeling Using Allan Variance, by Hou and El-Sheimy, Proceedings of the 2003 ION-GPS Conference
Allan Variance
A time averaging technique used to determine error mechanisms. Viewed as the time domain equivalent of the power spectrum density
The PSD is the limiting mean square of a random variable Error Mechanism
Wide-Band Noise Exponentially Correlated Noise (First Order Markov Process) Rate Random Walk Linear Rate Ramp Quantization Noise Sinusoidal Input Flicker Noise GPS and Vehicle Dynamics Lab
AV Slope
- 1/2 1/2 1/2 1 -1 1 0
81
Autocorrelation Analysis
The expected value of the product of a random variable or signal realization with a time-shifted version of itself Used to determine time constant of stochastic process (bias drift) or a periodic nature in the signal
Simulated Sensor Data from MEMS Gyro
82
Gyro Parameter Identification (KVH 5000 FOG)

Gather Static Data Filter Output and remove constant offset bias Run Allan Variance to Determine Dominate Error sources Calculate Angular Random Walk
remove
g r = r + c r + br + wgyro
0 insignificant
=
gyro
fs
83
Accelerometer Parameter Identification (Low-cost Humphrey accelerometer)

Gather Static Data Filter Output and Remove Constant Offset Bias Run Allan Variance to Determine Dominate Error sources Calculate Angular Random Walk removed
& + c &x& + b&x& + waccel a=& x

0 Modeled as & = 1 b +w b & & & & x x accelbias &x&
Time (hours)
=
Time (hours)
gyro
fs
84
Accelerometer Parameter Identification (Low-cost Humphrey accelerometer)

Calculate the Variance of the Filtered Data
Take the Autocorrelation of the Filtered Data to determine the time constant of the Markov Process
Variance of the filtered data
2 E b&x2 = & accel bias
[ ]
bias
Time (hours)
accel
2 2 f s accel bias
&x&
& = 1 b +w b & & & & x x accelbias
&x&
Time (hours)
85
Validation of Simple Accelerometer Model

Humphrey Allan Variance Chart
Used determined coefficients to generate a simulated sensor output Use an Allan variance to compare the simulated and experimental sensor outputs Shows that simulations can be used to generate realistic simulated data for navigation analysis and design
86
Definition of Various Grade Sensors (by parameter specification)

Rate Gyro Consumer (Cheapest) Random walk Bias Time Constant Bias Variation Automotive (Cheap) Random walk Bias Time Constant Bias Variation Tactical (Expensive) Random walk Bias Time Constant Bias Variation /sec/Hz sec /hr .0017 100 0.35 /sec/Hz sec /hr .05 300 180 Tactical (Expensive) Random Walk Bias Time Constant Bias Variation g/Hz sec g .0005 60 50 10 5 /sec/Hz sec /hr .05 300 360 Automotive (Cheap) Random Walk Bias Time Constant Bias Variation g/Hz sec g .001 100
1.2 10 3
Attribute
Units
Spec
Accelerometer Consumer (Cheapest)
Attribute
Units
Specification
Random Walk Bias Time Constant Bias Variation
g/Hz sec g
.003 100 2.4 10 3
KVH-5000 Fog Humphrey Accelerometer
Tactical Category Consumer Category

87
Heading Integration Error Bounds

Previous research showed heading error bounds were governed by:
rotation = gyro Ts t
Neglected bias but can be considered a best case scenario. Monte Carlo Simulation: Static Data Offset Bias Removed 1000 Iterations Sample at 100 Hz Monte Carlo simulation shows the effects of the walking bias
Position Integration Error Bounds

1000 Iteration Monte Carlo Simulation
Static Data with offset bias removed Sample at 100 Hz
Equation derived to predict longitudinal and lateral error:
North
= waccel
1 3 Ts t 3
1 8
East
= waccel wgyro Ts t 2
Neglected bias but can be considered a best case scenario. Monte Carlo simulations validate the equations and show the effects of the walking bias
Six DOF IMU Model

Uses the same assumptions laid out in the previous slides: Constant Offset Biases Walking Biases (Modeled as a 1st Order Markov Process) Random Walk Noise
Longitudinal Accelerometer Model Roll Rate Gyro Model
& + c &x& + b&x& + waccel &x& a &x& = & x

Vertical Accelerometer Model
&+c +b + w g = gyro
Pitch Rate Gyro Model
& + c &y& + b&y& + waccel &y& a &y& = & y

Lateral Accelerometer Model
&+c +b + w g = gyro
Yaw Rate Gyro Model
& + c &z& + b&z& + waccel &z& a &z& = & z
& + c + b + wgyro g =
90
Six DOF Heading Error Bounds

Static Gyro with turn on bias = zero 1000 Interation Monte Carlo Simulation (1000 Iterations)
Tactical Grade IMU Consumer Grade IMU
Gravity Field has no effect on the heading accuracy

Six DOF Position Error Bounds

Static IMU with zero turn on bias 1000 Iterations Monte Carlo Simulation
Tactical Grade IMU Consumer Grade IMU
Gravity compensation reduces errors caused by accelerometers

Advanced IMU Model (Accelerometers)

Includes New Error Terms: Scale Factor, Scale Factor Asymmetry, and Scale Factor Nonlinearity Misalignment Nonorthogonality
& + SFA x & & + SFN x & &2 + sin ( Az + Az )& & + sin ( Ay Ay )& a &x& = (1 + SF x )& x x x y z& + c &x& + b&x& + w accel x & + SFA y & & + SFN y & & 2 + sin ( Az Az )& & + sin ( Ax + Ax )& a &y& = (1 + SF y )& y y y x z& + c &y& + b &y& + w accel y
Lateral Accelerometer Model Vertical Accelerometer Model Longitudinal Accelerometer Model
& + sin ( Ax Ax )& & & + SFAz & & + SFN z & &2 + sin ( Ay + Ay )& a&z& = (1 + SFz )& z z z x y + c&z& + b&&x&& + waccel z GPS and Vehicle Dynamics Lab
93
Advanced IMU Model (Rate Gyroscopes)

Includes New Error Terms: Scale Factor Misalignment Nonorthogonality
Other errors are not as common in rate gyros
& + sin ( + ) & + sin ( ) & g = (1 + SF ) Gy Gy Gz Gz + c + b + wgyro

& + sin ( + ) & + sin ( ) & g = (1 + SF ) Gy Gy Gx Gx + c + b + w gyro
& + sin ( + ) &+c +b +w & + sin ( Gz Gz ) g = (1 + SF ) Gx Gx gyro
Roll Rate Gyro Model
Pitch Rate Gyro Model
Yaw Rate Gyro Model
Advanced IMU Model (Misalignment)

Large arrows represent the nominal axis (X,Y, and Z) Smaller arrows represent the misalignment and scale factor errors
myy myz
Misalignment about X-axis

z = arcsin
m zy X m yx = arcsin Y
myx Y
Misalignment about Y-axis

y = arcsin
m m xz = arcsin zx Z X
mzy Z X mxz
Misalignment about Z-axis

x = arcsin
m yz Y m zy = arcsin Z
95
mzz
mzx
mxy
mxx
Nonorthogonality Errors
Large arrows represent the nominal axis (X,Y, and Z) Smaller arrows represent the nonorthogonality and scale factor errors
nyy
Nonorthogonality about X-axis

n yz nzy x = arcsin = arcsin Y Z
nyz Y
nyx
Nonorthogonality about Y-axis

nxy X
nzy
n y = arcsin xz X
n = arcsin zx Z
Z nxx
Nonorthogonality about Z-axis

nxz
nzz
nzx
n yx nzy arcsin = z = arcsin Y X

96
Input/Output Scale Factor Errors

Input/Output Errors Scaled Scale Factor Output = (1 + SF )Input
Scale Factor Asymmetry Output = ASF Input
Scale Factor Nonlinearity 2 Output = NSF (Input )

Simulation of Advanced IMU Model

Simulated Rocket Trajectory Rocket elevated to 55 degrees Impact point 86 km downrange
Trajectory Body Accelerations
98
Simulation of Advanced IMU Model

Maximum longitudinal velocity 550 m/s Impact velocity 600 m/s Flight Duration 165 seconds
Platform Heading NEU Velocities
99
Errors from Advanced IMU Model

Scatter Plot at Impact Point
1- sigma bounds Simple model Advanced Model
Scenario: Rocket Trajectory Duration is 165 seconds Monte Carlo Simulation 200 Iterations Constant Bias set to zero Initialization errors set to zero Shows that the additional terms in the advanced model only effect the mean impact point. Sigma bounds remain relatively equal
100
Contribution Level (meters) 500 450 400 350 300 250 200 150 100 50 0
Error Contribution in the Advanced IMU Model
The rocket trajectory was run with each error independently Errors that contribute the most are errors that see the accelerations and rotation rates
yr o Gy M i ro sa l M in Gy Gy r isa gm ro o M l ig en t Gy No isa nm ab o ro no r l ig e n t u t Gy No t ho nm a b o X ro no r go n en t u t No t ho a l a b Y no go it y ou t rt h na ab Z o li o R o go n t y a u t X ll ali b o Ac u P c e Y i t c Gy r t y a t Y a A le ro w h G o Sc b ou cc m Gy y ro al e t Z el et r o e e A S F Ac c c r om r M Sc a c a a c t e c e le e t e i sa l e le F o r F le ro r l a Ac ro m me Mi ign m a c to c t o c r r e te sa Ac e le r te r r M l ig ent Er c e o m No i sa nm a bo ro r le r e t no li e n ut o m e r rt gn t a X e te No h og men b ou r N no on t a t Y rt a b X o no h og li t y o ut Ac rth on a b Z a o Y c el e og o li t y u t X n Ac ro a a o Z c el m et li t y ut A er er ab Y c o X c e l m e S ca ou t Ac e r o t e r l e Z m S F c Y el e r e te ca l e a ct o r r A o Z c c m et Sc a Fa c A ele e r le to cc r A F r X e le om ss a c t Ac r o et e ym or m Y c el e et e r A s m et Ac ro r A sy ry m Z c el e m et ssy e t r A e cc rom r N mm y e le et on e t r o e r li n r y m N ea e t on ri er t l No i ne a y nl ri t in y e Al ar it lE y rro rs
Scatter Plot at Impact Point
101
GPS/INS: The Perfect Complement

GPS (Low Frequency Sensor) INS (High Frequency Sensor) Limited to 1-20 Hz Stable over long periods of time Stochastic zero mean noise Unbiased Noisy Higher output rates available Drift over long periods Noise due to vehicle dynamics Biased
The combination provides a high update rate, low noise, unbiased measurement solution
Allan Variance of GPS Velocity

Dominated by random noise (no drift)
103
Allan Variance of GPS Position

Error has short term drift Note: 1 position error does not equal 1 from AV due to drift
GPS Position (=2.5 m) Starfire DGPS Position(=0.2 m)
WB=1.0 m WB=0.007 m
Methods of GPS/INS Integration

GPS Aiding the INS

Loosely Coupled Closely Coupled (Tightly Coupled w/out aiding) Tightly Coupled (w/aiding) Ultra-Tightly Coupled or Deeply Integrated
INS Aiding GPS

The methods differ in the type of information that is shared between individual units
Saurabh Godha, Strategies for GPS/INS Integration, http://www.geomatics.ucalgary.ca/~sgodha/Photos/GPS-INS.ppt
105
Loosely Coupled Integration

Introduction
Combines computed GPS PVT Solution with IMU measurements GPS Solution is completely independent from the IMU measurements
GPS corrects IMU drift
Utilized a decentralized or cascaded Kalman Filter approach:

Local GPS navigation processing filter Master INS filter (GPS+IMU)

106
Loosely-Coupled Block Diagram

The GPS PVT Navigation Solution is combined with the IMU at the navigation level (compensates IMU drift)
Ant.
GPS Nav. Solution GPS Receiver Kalman Inertial Nav. Solution INS Filter Combined Nav. Solution
107
Loosely Coupled Integration

Advantages
Cascaded architecture reduces the dimension of each state vector (less processing overhead) Easy to implement combine outputs from any commercial GPS receiver and IMU (dont need access to raw GPS measurements) Navigation solution relies on pure IMU measurements if GPS PVT is not available (number of satellites is less than 4) Correlated systems treated independently
Provides sub-optimal solution
Disadvantages
Tightly Coupled Integration

Introduction
Uses raw GPS measurements (pseudorange and Doppler) combine with the inertial measurements in one integrated Kalman filter Error states include the navigation solution as well as INS and GPS sensor errors Provides a more optimal navigation solution Utilizes a Centralized Kalman Filter INS may be used to aid tracking loops
109
Tightly-Coupled Block Diagram

IMU measurements are combined with individual GPS satellite phase measurements at the Positioning level
Ant. GPS Receiver Tracking Loops Kalman Range & Rate Estimate Filter Nav. Solution
Pseudo-range & Rate
INS
110
Tightly Coupled Integration

Advantages
Allows for continuous (degraded) positioning even when the number of satellites of GPS drops below 4 Allows for monitoring of individual GPS measurements from each satellite IMU aiding of tracking loops can improve GPS tracking in high dynamic environments More difficult to implement Larger size of state vector in centralized KF requires more computation time Provides no long-term noise immunity to GPS reception
Disadvantages

Deeply Coupled Integration Introduction

IMU is directly used to aid GPS signal tracking
Inertial measurements are combined with the GPS signal measurements at the tracking level Raw GPS and IMU measurements are combined in a centralized navigation filter Filter operates on the receiver tracking loop I and Q signals and the IMU measurements in order to estimate navigation information
Requires access to receiver tracking loops or raw IF Also known as Ultra Tight Coupling
Deeply Coupled Integration

Block Diagram
IMU measurements are combined with the GPS signal measurements (IF) at the tracking level
Ant. GPS Receiver RF Frontend & Correlators
Correlator Samples Kalman Filter Nav. Solution
NCO
Frequency Estimate
INS
113
Deeply Coupled Integration

Advantages

Can provide improved accuracy Increased jamming resistance Allows faster signal acquisition and reacquisition Provides improved tracking of GPS signal in the presence of high noise and/or high dynamics Currently not robust (no method for integrity monitoring of individual satellites since raw data is fused a in single navigation filter) Extremely cumbersome Sensitive to IMU noise and bias as well as method of implementation
Disadvantages
GPS/INS Integration
GPS/INS integration is predominately performed using Kalman Filters

Optimal fusion of data given sensor statistics Extended Kalman Filters (EKFs) required when problem becomes non-linear
Coordinate Transformations Estimating Certain IMU Scale Factors
Linear Kalman Filters can be used on ground vehicles

Linear about small orientation angles Only estimated additive IMU errors (bias drift) Unscented Kalman Filters Particle Filtering Etc.
Other methods currently being explored

Linear Kalman Filter

Assumes the following Model Form
Continuous
& = Ax + Bu u + Bw w x y = Cx + v
E vvT = Rc
T
[ ] E [ww ] = Q
T T
w R d 1 Qc R d d v R m1 Rc R mm wk R n1 Qd R nn vk R m1 Rd R mm
d: # of disturbances n: # of states m: # of measurements
Discrete/Sampled
xk +1 = Ad xk + Bd u + wk yk +1 = Cxk + vk
E vk v k
k
[ ]= R E [w w ] = Q
d k
116
Process Disturbance
Ew
[ ]
2 vel
Rwpsd = Qc = 0
0 b2a
For Small Ts:

Qd w
1 Qd Ts 0
2 Rwpsd = Ts accel
tk
Qc T T w Ts Bw Qc Bw Ts
2 0 Ts accel 1 Tb 1
Qd =
t k 1
BwQc B (e ) d
T w
A T
0 1 b2a 0 0
0 1 Tb
Byrsons Trick:
c11 c12 c= =e 0 c22
T Ad = c22
T A BwQc Bw Ts AT 0
2 Ts accel 1 0 = Ts 1 0 1
1 0 2 ba 0 1 2 Tb
T Qd = c22 c12
Qbias =
b2
Tb2
Measure of the Sensor Stability
117
Linear Kalman Filter Equations

Measurement Update T T L k = Pk C d (Cd Pk C d + R d ) 1 + = + L k ( ) x x k k Pk+ = (I L k C d est )PkTime Update + -k +1 = A d x k x + Bd u k Pk- +1 = A d Pk+ A T d + Qd where k = residuals = y meas C d x
k )(xk x k ) Pk = E (xk x
]
118
Kalman-Bucy EKF Equations

Assumed form
& = f ( x, u ) + w x y = h( x ) + v
Kalman filter recursive equations

(t ) = x (t + ) + x k k 1 P (t ) = P (t + ) + k k 1 t t t k ( ), u ( ), ]d f [x
E wwT = Q( )
T k)
] E [vv ] = R(t
T
& f x =A = x x h =C x
k 1
k T T d A( ) P( ) + P( ) A( ) + BwQ( ) Bw t k 1
1
L(t ) = P(t )C (t ) T C (t )P(t )C (t ) T + R(t ) k k k k k k k (t + ) = x (t ) + L(t ) y (t ) C (t ) x (t ) x k k k k k k P(t + ) = I L(t )C (t ) P(t ) k k k k
)( x x ) = P(t k ) E (x x
Use numerical integration (Euler, Runge-Kutta, etc.) to propagate states and error covariance matrices
GPS/INS Update Rates

GPS Updates generally at 1-10 Hz IMU Updates at >50 Hz Therefore KF is technically unobservable between GPS updates

Time update integrates IMU measurements between GPS updates KF filters integrated IMU and GPS measurement at every GPS measurement
Based on predicted error from propagated IMU between GPS
measurements and predicted GPS error

Time Update
Measurement Update
1 T T Lk = Pk C (CP C + R ) k v X k = X k + Lk ( y meas CX k n ) Pk = ( I Lk C ) Pk
k +1 = x k + u k x Pk +1 = Pk T + Qd
120
1 DOF GPS/INS Example

System Model
& 0 1 x & 1 x 1 & b & = 0 1T b + 0 a x + 0 x b x
2 E wvel
If GPS is available:
0 w 1 vel Tb
C = [1 0]
# Lk = #
[ ]
Rwpsd = Qc = 0
0 b2a
2 Rwpsd = Ts accel
If GPS is not available: 0 C = [0 0] L =

k
VGPS
& x = C + vGPS bx
2 2 E vGPS = Rd = GPS
[ ]
P P = 11 P21
2 & E x P12 = P22 x Eb & x
[ ] ) [ ] E ( b
( )
b & Ex x
x
121
1 DOF Yaw Example

System Model (assumes
& vel 0 1 vel 1 1 b & = 0 1T b + 0 g r + 0 r r b
= vel =
Ew
)
0 b2g
0 w 1 hd Tb
2 = Rv = E v
[ ]
2 hd
2 Ts gyro = Qc = 0
GPS = GPS
= [1 0] vel + v g bias
[ ]
2 GPS
To Estimate Sideslip ():
vel Turn off KF during turning/periods of changing sideslip: = GPS (compares GPS velocity, course, with integrated gyro)
122
Longitudinal Dynamics
System Model
Cannot distinguish pitch and longitudinal accelerometer bias
& V x & g & & +b p x & bq
0 G c 0 = 0 0 0 0 0
Gc 0 0 0
0 V x 1 0 g + 0 1 ( p + bx ) 0 1 b Tb q 0
0 1 a x 0 0 + 1 g q 0 0 0
0 0 w 1 0 long 0 1Tb 0 0
Vx VGPS 1 0 0 0 g = 0 1 0 0 ( + b ) + vGPS x p GPS bq
GPS
up VGPS = tan V GPS 1

0 0 0 b2g
123
2 E vGPS
[ ]
2 GPS = Rv = 0
2 GPS ( up ) V 0
2 E wlat
[ ]
2 Ts accel = Qc = 0 0
2 Ts gyro 0
Lateral Dynamics
System Model

Cannot distinguish pitch and longitudinal accelerometer bias Must account for centripetal acceleration
& V y & & + by & b p
0 G C 0 = 0 0 0
1 0 V y 1 0 ( ) ( ) a g b V r r x y 1 ( + b y ) + + 0 1 0 g p 0 1 bp 0 0 Tb
0
1 Tb
wlat 1 Tb 0 0
Vy = [1 0 0]( + b ) + v VGPS y GPS bp
= GPS vel
2 E wlat
[ ]
2 2 Ts accel + V 2 gyro = Qc = 0 0
0
2 Ts gyro
0 0 b2g
2 2 E vGPS = Rv = GPS
[ ]
124
Complimentary Filters
Can use complimentary filters to separate low frequency accelerometer bias from higher frequency vehicle dynamics KF Corrects IMU Errors
= Tb s ( + b ) y Tb s + 1 = b y 1 ( + b y ) Tb s + 1
Ts = b ( + bx ) Tb s + 1 = b x 1 ( + bx ) Tb s + 1
= g r br r = g p bp p = g q bq q & & = a x Gc ( g + p + bx ) x & & = a G ( + b ) y

y c y
125
GPS/INS KF Closed-Loop Eigenvalues and Bandwidth
126
GPS/INS Velocity Accuracy

Determined using a covariance analysis based on sensor noise statistics
127
GPS/INS Velocity Based Heading Accuracy

& = & Assumes
Determined using a Covariance analysis based on sensor noise statistics
& (
& =0 ) = 0 or V y
128
GPS Velocity Based Heading Accuracy

Multi-Antenna GPS/INS Attitude Accuracy

Multi-Antenna GPS/INS Attitude Accuracy (with Short Baseline)

GPS/INS Estimation of Vehicle Roll

Lateral accelerometer bias resembles roll
3 2 Actual Esimtated (No Roll Gyro)
Roll gyro reduces the latency in the roll estimate
(deg)
1 0
1 0 3 2
10
20
30
40
50
60
70
Actual Esimtated (w/ Roll Gyro)
(deg)
1 0
1 0
10
20
30 Time (s)
40
50
60
70
Recall: Lateral accelerometer bias is not distinguishable from roll

Experimental Results of the Lateral Estimator on a Test Vehicle

Lane Change Maneuvers
2 0.5
Driving Around a Banked Turn

0.4 0.2 Model Estimated
est (vGPS) (deg)
est (vGPS) (deg)
Model Estimated
0.4
V (m/s)
0.2 0
V (m/s)
10 20 30
0 0.2 0.4 0 0.04 10 20 30
0.5 1
0.2 10 20 30 40 0 0.2 10 20 30 40
1 0
1.5 0 2
b and b (deg/s)
b and b (deg/s)
10 Time (s) 20 30
+ bx (deg)
+ bx (deg)
0.02 0 0.02 0.04 0.06 0 10 Time (s) 20 30
0.2
5 0 10 20 Time (s) 30 40
0.4 0
10
20 Time (s)
30
40
1 0
133
Experimental Estimator Results on a Test Vehicle

GPS Velocity Measurement (m/s)
Results from the Longitudinal Estimator

=0.03 m/s 0.1 0 0.1 0.2 0 1.4 1.2 1 0.8 0.6 0.4 0 500 Time (s) 1000 500 1000
Results from the Lateral Estimator

0.4 0.08 = 0.1 deg 0.06 sqrt(P11)
Velocity Estimate (m/s)
0.2
0.2 =0.009 m/s
(deg)
0.1 0 0.1 0.2 0 1.4 500 1000
0.2 0
P0.5
GPS
0.04 0.02 sqrt(P )

22
0.2 0.4 0 0.4 = 0.04 deg

=0.0022 m/s
2
10
20
30
40
50
0 0
10
20
30
40
50
Accelerometer Measurement (m/s2)
Bias Estimate (m/s2)
=0.05 m/s2
1.2 1 0.8 0.6 0.4 0
0.2
Gyro Bias Estimate (deg/s)

40 50
(deg)
0.05
est
0.2 0.4 0 10 20 30 Time (s)
0.1
500 Time (s)
1000
10
20 30 Time (s)
40
50
134
User with High Dynamics requires the Fusion of GPS & IMU
GPS gives:

User position in global coordinates (loosely coupled) Pseudorange measurements, satellite positions, & time all in global coordinates (tightly coupled) Acceleration Rotation Rate
IMU gives inertial measurements in body frame

Requires additional modeling 1

135
Mode of GPS Measurement Gives Name to the Fusion

Loosely Coupled Measurement Model

Pos measurement IMU states
x 1 y = 0 0 z
0 1 0
0 0 1
0 0 0
0 0 0
Tightly Coupled Measurement Model
x y 0 z 0 + . 0 . .
s =

H k ( xu , x s ) * xk x k
i = 1, 2 ,... m
For m satellites in view (w/ valid pseudorange observations) State vector x includes states for the additional IMU dynamics
Summary of Methods
Loosely Coupled
GPS
, rsats
Least-Squares
User r, v Kalman Filter
IMU
a,
User States
Tightly Coupled
GPS
, rsats Kalman Filter
IMU
User States a,
137
Full Model Prepared for Planar TC Kalman Filter Estimation

Linearized (about current estimates) dynamic model Inputs are IMU measurements (derivatives of states) Sensor biases modelled as 1st-order Markov process
0 N 0 E 0 d V 0 = dt 0 ba 0 b 0 cos( k 1 ) V sin( k 1 ) 0 sin( k 1 ) V cos( k 1 ) 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 0 N 0 0 E 0 0 V 1 + 1 0 0 ba 0 1 b 0 0 0 0 0 0 0 0 a 1 0 + 1 r 0 1 0 0 0 0 0 0 0 0 a 0 0 0 0 0 0 ba 1 0 b 0 1
138
TC KF Measurement Model: Two Dimensional Case

Measurement model (pseudoranges) linearized w/ current estimate Size of measurement model depends on satellites in view
u , k 1 , x s ,1 ) (x 1 , k 1 1 (x u , k 1 , x s , 2 ) 2 2 , k 1 . . = . . . . . (x u , k 1 , x s , m ) m m , k 1 u , k 1 , y s ,1 ) (y u , k 1 , y s , 2 ) (y
1 , k 1
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0
2 , k 1
. . . u , k 1 , y s , m ) (y
m , k 1
1 N E 1 V 1 1 b a 1 b 1 b gps 1
1 2 . + . . . m
139
Noise Statistics Known from Simulation

Disturbance Covariance approx. by c = d t
2 2
a 2 0 Qc = 0 0
2
0 0
0 0
ba 2
0
0 0 0 b 2
Discrete Qd found using trick method
Qd discrete state disturbance covariance
Noise covariance derived directly from GPS output statistics 2 0

Rd = N 0

140
Motivation for GPS Guided Tractors

1999 tractor sales

North America - 108K World - 590K
good satellite visibility on farms relieve drivers from tedious & monotonous labor provide farm operation during poor visibility open doors for new agricultural techniques
Automated Steering
Cm-level control for row crops Reduced overlap for tillage Cooperating Vehicles
142
GPS Guided Farm Tractor

4 - antenna carrier phase DGPS 3-D position (1 = 2 cm) 3 axis attitude (1 = 0.1) 5 Hz update rate steer angle potentiometer
143
144
Roll and Lever Arm Correction

Rolls Off at 0.12 Hz Higher Frequency Resonant Peak @ ~ 1Hz Can filter INS to measure Roll
0 Amplitude (dB) -20 -40 -60 -80 -100 -120 0.0 0.1 1.0 10.0 Frequency (Hz) GPS and Vehicle Dynamics Lab
~ 3 meter lever arm 0.4 roll accuracy required to utilize 2 cm DGPS position accuracy
145
Various Yaw Dynamic Models

Bicycle Model
R(s) = (s) mV c0 c2 c1mV 2 c12 c0 I Z + mc2 2 IZ s + s+ 2 mV mV
R ss = Vx L + K US V x
2
a C f s +
a C f c1C f
DC Gain: Neutral Steer Model (Kus=0)

a C f R( s) = ( s ) I s + c2 Z V
Kinematic Model
R= Vx L
146
Box-Jenkins Model Fit

10
Magnitude
0 10 20 0 10 0
ETFE BJ(2,2,2,2,1) Fit 10

1
10
Phase (deg)
50 100 150 10
0
ETFE BJ(2,2,2,2,1) Fit 10 Frequency (rad/sec)

1
10
Vx= 4 m/s
R (t ) =
C (q) B(q) (t ) + e(t ) D(q ) A(q )
147
Line Tracking
0.9 Later Error (m) 0.6 0.3 0.0 -0.3 0 50 100 Time (sec) 150 200 Mean = 5 mm 1=3 cm 1 Foot
148
Advanced Trajectories
149
High Speed Control

Accurate control at full range of tractor speeds
Vx=5 m/s
0.4
0.4
Vx=8 m/s
Lateral Error (m)
0.2 0 0.2 0 0.4 Mean=3.5 cm 1=4.0 cm
Lateral Error (m)
0.2 0 0.2 0 0.4
Mean=2 cm
1=2 cm
2 1=0.08
10
10
15
20
Control Input
0.2 0 0.2 0.4 0
Control Input
0.2 0 0.2 0.4
1=0.10
4 Time (sec)
8 and Vehicle 10 0 GPS Dynamics Lab 5
10 Time (sec)
15
20
150
Full State Estimation: 12 Tractor States

X est = E
N VX
& && b
& b
gb
rb
E = tractor east position N = tractor north position V x = forward velocity = heading & = yaw rate && = yaw acceleration b = slip angle (heading bias or " crab angle" ) = steer angle & = steering slew rate b = steer angle bias g b = gyro bias rb = radar bias
Cascaded (KF) Estimation

GPS Estimates of Position & Velocity
INS Radar Gyro
Bias Estimate
Tractor Model
Dead Reckoning Filter Control States Filter
Esimates of Tractor States
LQR Control
Natural Separation of the Estimators

Separate Estimators
Dead Reckoning Equations
& = ( radar r ) sin( ) e b & = ( radar r cos( ) n b) & = gyro g b
Inputs: Radar, Gyro
X 1 = e n rb
Tractor Equations
&&& = 2 && &+K n K d v & & & = + vu I I v v 2 n 2 R n
Input: u = Steering Slew Rate
gb b
&& X2 =
& VX
& b
153
Demonstration of Separate Bias Estimators

15 10 5 0 -5 -10 -15 -20 -25 0 Gyro Gyro Bias Deg, Deg/Sec
VX=2 m/s Line Tracking

Steer Angle 20 Time (sec) Steer Angle Bias 40 60
Able to accurately estimate both the gyro and steer angle bias independently
154
Lateral Errors When Dead Reckoning (No GPS)

1.5 Lateral Error (m) 1.0 0.5 0.0 -0.5 -1.0 -1.5 0 20 40 60 Time After GPS is Off (Sec) 1 0.3 m
3 Tests VX=2 m/s Line Tracking Results Errors < 0.3m for 40 sec Errors <
Error Analysis:
& E = V X E = V X & TS t y
y (t ) = V X & TS * t
2 3
3 2
155
Dead Reckoning Positioning Performance (No GPS)

0.4 0.3 Longitudinal Error (m) 0.2 0.1 0.0 -0.1 -0.2 -0.3 -0.4 -0.4 -0.2 0.0 Lateral Error (m) 0.2 0.4 9 cm = 1/2
0-8 sec 8-42 sec
0.3 m
Test VX=2 m/s Line Tracking Results 8 sec < 9 cm error 42 sec < 30 cm error => % Error Lateral Errors > Longitudinal Errors
156
Lateral Errors = f ( E ) 2 Long. Errors = f ( E )
Effect of Crab Angle

Three Major Errors

Gyro Heading Crab Angle Velocity Integration

Vx Heading
Veast 1 ek ek 1 = tan ( ) = tan ( ) Vnorth nk nk 1

1
0.5 Lateral Error (m) 0.0 -0.5 -1.0 -1.5 -2.0 0 20 Gyro Heading
GPS Heading
GPS vs. GYRO < 0.3 m Crab Angle < 1 @ t=20 sec
40
60
80
157
Time after GPS is Off (sec)
Total Dead Reckoning Control Performance

-795 -800 -805 North (m) -810 -815 -820 -825 -830 -835 -50 -30 -10 East (m) 10 30 50 Lap 4 Lap 1
1st Lap With GPS 3 Laps DR (NO GPS)
4 Minutes of Dead Reckoning
Errors Position Heading
Mean 0.2 m -0.05
1 0.23 m 0.87
Max <1m < 2

158
Implement Control
4 Added States
X = e
n b ]
GPS Measurement
( s)
a 1+ b KI = = R( s ) s + VbX s + I
& & I = V X + L& + (a + L) y

159
Implement Control
Implement: 7.9 m (26 foot) Wide Chisel Plow Implement Position: Carrier Phase DGPS 3-D Position (1 = 2 cm)
GPS Antenna (L=6.5 m)
160
Experimental Control of Implement

Experiments performed at 4.5 mph Notice difference in position of tractor and implement
5 Implement Tractor 4 3
70 65 60 55
North (m)
Implement Tractor
North (m)
2 1 0 1 2 10
ARC: = 2.5 cm 1 = 3.5 cm 50 45 40 35
10
20 30 East (m)
40
20 15 Lab10 60 GPS 50 and Vehicle Dynamics
30
0 East (m)
10
15
20
161
Analytical Models
Can a model capture the behaviors actually seen from real data? Models useful to steering control describe the turning rate of the tractor from a given steering angle
c Fyi Fyr b
Implement described using a tire model
Fy = C
a Fyf Vx
i
Vit
r
Vrt Vy
V
f
Vft
Fxf
162
Empirical Models
Used to determine how a tractor really behaves with changes in implement Determine appropriate analytical model (and parameter variations) using experimental data
163
On-line Estimation of Hitch Parameter

Estimate parameter using GPS/INS and Steer Angle measurements when enough excitation exists
164
DARPA Grand Challenge (Overview)

Autonomous ground vehicle race in February 2004 and October 2005 130+ miles across desert terrain No human intervention Avoid obstacles in path while remaining in corridor Auburn University partnered with SciAutonics and Team Terramax 2004 race did not prove successful
SciAutonics
Furthest competitor reached 7 out of 142 miles
Terramax
2005 race demonstrated some capability of autonomous vehicles
Five teams finished race

DARPA Grand Challenge

Critical navigation states:

Navigation System Development

Other vehicle information:

Velocity Direction of travel Position
Roll angle Pitch angle Road grade
Used by vehicle controller to drive the vehicle
Used by obstacle detection system to properly orient obstacles
166
Effect of Model Error on UGV Control

Controller feeds back all available measurements
North (m)
Aggressive Autonomous Lane Change (V=20 mph)

2 0 -2 -4 -6 -8 -10 -150 -100 East (m) -50 0 Incorrect Parameters Correct Parameters Desired Path
The incorrect model results in instability A better model is needed before the other measurements can be utilized
167

Navigation Sensor Suite
Differential GPS was cornerstone of vehicle navigation
Navcom Starfire (<10cm position accuracy)
Tactical grade inertial measurement unit provided high update accelerations and angular rates
Rockwell Collins GIC-100 (<1/hr drift rate) TCM2 (16 Hz output) Microstrain 3DM-GX1 (50 Hz output)
Magnetometers output vehicle orientation information

The vehicles on board speedometer provided additional speed measurement
Navcom Starfire Rockwell Collins GIC-100

168
Navigation
Navigation solution blends measurements from various sensors -DGPS -Steer angle -Multi-antenna GPS -IMU -Wheel speed -Lidar -Doppler radar State estimates provided to controller: -Velocity -Course -Position -Pitch -Roll -Measurement biases Other sensors can be easily added -Range radar -Camera -Ultrasonic
169
Navigation Errors from Longitudinal Slip

Longitudinal wheel slip provides navigation algorithm with incorrect measurement Corrupts estimates of velocity, accelerometer bias, and position Can estimate wheel slip as a bias, but only when GPS is available Doppler radar is an alternative speed measurement
Bias can change with terrain


m = + c + b + w
& = 1 b + b
Sensor Capabilities and Limitations
2 2 , t
Sensors can be modeled with a turn on bias, a Markov bias, and white noise
~ [0,1]
GPS offers precise information, but only at low update rates of 5 Hz IMU provides 50 Hz measurements, but suffers from bias drift Magnetometers provide roll, pitch and yaw measurements, but contained quickly drifting bias TCM2 output at 16 Hz Speedometer had a calibration error and was susceptible to wheel slip Also output at a variable rate A Kalman filter was used to blend the various sensor measurements and provide reliable information based on the strengths of each sensor while compensating for their inadequacies
171

Navigation Model
& V g & a x g g & r b r br & 0 N ) & V cos( E sin( ) V & & IMU b & b 0 & &IMU b & = x = & 0 b & 0 g 0 & b M 1 0 & b 1 M 0 & b M 1 0 & bM 2 0 & bM 2 0 & bM 2
Inputs were from IMU Biases were not modeled as a function of time, but the noise driving the drift was modeled in the process covariance matrix
Q d = diag ax r & = 12 b + w2 b
2
br 2 N 2 E 2 2 b 2 2 ... bM 1 2 bM 2 2 bM 2 2 bM 2 2
b g
bM 1 2 bM 1 2
y = [VGPS
MAG1 MAG 2 g MAG1 MAG 2 ]T
VWS GPS
N GPS
EGPS
MAG1 MAG 2 M ...
Velocity state accounts for vehicle pitch and longitudinal road grade Vehicle pitch estimate contains longitudinal accelerometer bias Roll estimate is vehicle roll and lateral road grade, and contains lateral accelerometer bias

The system successfully tracked GPS measurements when they were available Bridged brief erroneous GPS measurements
Navigation System Performance
173

Initialization of the Kalman filter is critical to its performance
Navigation System Initialization
Settle time when GPS is acquired ~ 3 seconds Algorithmic logic in the loop leads to better navigation solutions
Some commercial systems take >30 minutes to initialize Magnetometers aided initialization, but were statistically weighted out of the filter when the vehicle was moving
Yaw (deg)
160 140 120 100 80 60 40
TCM2 Microstrain GPS
Fast bias drift makes measurements unreliable Passing metallic objects also degrade quality of measurement
20
70
80
90
100 Time (s)
110
120
174

National Qualification Event
Failed to finish first run because of GPS receiver malfunction Successfully completed next three runs One of ten cars granted early entry into DARPA Grand Challenge
175

Grand Challenge
Completed 16 miles before USB hub failed and crashed a computer Vehicle performed very well while on course

Navigation Error Sources
The absence of GPS measurements makes some biases unobservable

Bias estimates remain constant when no measurements exist to update them Integration of constant offset linearly degrades heading estimate Tactical grade IMU with low bias drift allowed for relatively long dead reckoning periods when the bias was correctly estimated before the outage
Calibration error in wheel speed sensor created an offset in the velocity estimate

Wheel slip would also introduce estimate error If the wheel speed bias was estimated, the calibration error and wheel slip would corrupt it as well

Navigation System Performance
Performance assessed by simulating a GPS outage

Dead reckoning performance critical in Grand Challenge Outage starts and stops at the circles in the figure below
~1m error after 25 seconds Error Caused by neglected sideslip generated during cornering
-10 -15 -20 North (m)
Error (m) 1 0.8
-25 -30 -35 -40 40 50 60
0.6
0.4
0.2
70 80 90 100 110 -0.2 East (m) 380 390 GPS and Vehicle Dynamics Lab GPS EKF
400
410 420 Time (s)
430
440
450
178

Navigation Error Sources
The effect of sideslip (generation of lateral velocity) during turning can be seen in the heading estimate Sideslip is the difference between the direction the vehicle is pointing and the direction the vehicle is traveling

A single GPS antenna measures the direction of travel An integrated yaw rate gyro yields the direction the vehicle is pointing
150
N
Heading (deg)
100
GPS KF
VF
F
Vx r VR
R
50 85 90 95 100 105 110 115
20 Error (deg)
E
10 0
Vy
-10 GPS course is denoted by -20 Heaind is denoted by and Vehicle Dynamics 85 GPS Lab
90
95
100 Time
105
110
179
115
Navigation Errors

Typical Navigation Models
Kinematic navigation models are sometimes preferred

Rely on the kinematic relationships between sensors Vehicle model based navigation models require knowledge of parameters, which are hard to find or apt to change
Typical kinematic navigation models neglect the lateral & = 0 and = 0 r dt vehicle dynamics and assume

Estimates velocity, course, position, and sensor biases (sometimes roll) Examples shown use IMU as input, and GPS as measurement
a x b ax 0 r b r & = x 0 cos( ) V ) V sin(
Model 1:
V b ax = x b r N E
V y= N E
Model 2:
ax b ax 0 r b r 0 &= x & b IMU 0 ) V cos( ) V sin(
V b ax b r = x +b ay b N E
V y = M N E
M =
1 ) a y V (r b r g
180
Navigation Errors
Expanded Navigation Model
When sideslip is present, typical navigation models break down Expanding the model to include the lateral dynamics reduces estimation error in the presence of sideslip

Requires measurement of sideslip (2 antenna GPS receiver) Also provides lateral velocity estimate, which can be useful to stability control systems
+V ) (r b ax b ax y r 0 V ) g sin( a b ) (r b ay x r y 0 r b r & = x 0 & b IMU 0 2 2 1/ 2 /V )) + atan (V y x (V x + V y ) cos( 2 +V 2 )1 / 2 sin( (V + atan (V y / V x )) y x
V x bax V y bay = x b r b N E
Model 3:
V x V y y= V 1 y = tan V N x E
181
Navigation Errors
Model Comparison
Models 1 & 2 do not track vehicle course or vehicle heading Model 3 tracks vehicle heading well Can effect dead reckoning performance
1&2 3 True Course True Heading
2 1.5 1 0.5 0 -0.5 1&2 3 True br
A step lane change maneuver was input into a simulation at 40mph

Error also shows up in yaw gyro bias estimate
10
0 33 33.5 34 34.5 35 35.5 Time (s) 36
Yaw Rate Gyro Bias (deg/s)
15 Yaw Estimate (deg)
GPS and 36.5 37
Vehicle Dynamics Lab
10
15
20
25
30 Time (s)
35
40
45
50
182
Navigation Errors
Model Comparison
Dead reckoning performance of Model 3 is also better GPS artificially removed during the maneuver generating sideslip
Worst case scenario because bias estimates contain large errors Position model incorrect because it is integrating heading, not course
70 60 50 Position Error (m) 40 30 20 10 0 GPS and Vehicle Dynamics 0 Lab 10 20 Time (s) 30 40 50
GPS also removed before the maneuver was executed
Model 1&2:
= V N cos( ) = V E sin( )
Model 3:
= V N cos( + ) = V E sin( + )
1&2 - Outage During Slip 1&2 - Outage Before Slip 3 - Outage During Slip 3 - Outage Before Slip
183
Navigation Errors
Model Comparison
Similar errors are seen with experimental data

Errors are not as well defined because of inexact sensor characteristic models, and slightly offset measurement times
Model 3 tracks heading relatively well, but models 1&2 break down when the slip first occurs
-75 Yaw Estimate (deg) 1&2 3 True Course True Heading
-80
-85
-90
-95
GPS 197.5
and 198 Vehicle Dynamics Lab 198.5 199

Time (s)
199.5
184
Navigation Errors

Model Comparison
Dead reckoning performance improved with model 3

Course error corrupted by incorrect bias estimate Position error experiences smaller growth rate when dead reckoning through an extreme maneuver with model 3
To improve navigation accuracy, the model must account for lateral vehicle dynamics
95 Yaw Estimate (deg) 90 85 80 75 1&2 3 True Course True Heading Position Error (m) 120 100 80 60 40 20 0 1&2 3
116
117
118
119 120 Time (s)
121
122
GPS Lab 123 and Vehicle Dynamics 0 20
40
60 80 Time (s)
100
120
185
Laser Scanner Navigation

Concept
Laser scanners provide environment information which can be used to navigate in a defined corridor Provides ability to estimate the following:

Velocity, local heading Local lateral error (for use in some controllers) Lateral vehicle movement (also makes vehicle sideslip and/or lateral velocity observable)
A corridor is defined using obstacles along the path
Buildings, trees, signs, road edge
Navigate relative to the defined corridor Preliminary experiment was run indoors in a hallway

Estimating Sideslip
With alternate laser scanner setup, lateral accelerometer bias is observable
ylat2 2 1
ylat1
No need to use ideal accelerometer input to estimate lateral velocity

ydl, 0
ydr, 0
Using the 0 measurement, lateral position (in the vehicle y = [y ] frame) to the wall is directly measured u = [a x ]
d1
& a y b y V ay & = 0 x , x = bay & y y V V

187

Navigation Models
Two navigation models were developed in this study

One did not use a lateral accelerometer input The other used an ideal accelerometer input Both used the same measurements
The same Kalman-Bucy filter algorithm was used

V x y= y lat
Lidar/IMU Equations
No Lateral Accelerometer (Model 1)
& V x a x b ax & b 0 ax & & = r b 1 = x r & 0 b r sin( ) V & y lat x
ax 2 0 Q1 = 0 0 0 0 0
2
With Lateral Accelerometer (Model 2)

& V x a x b ax & b 0 ax & r b r & 2 = = x & 0 b r sin( cos( + V ) V ) & x y y lat & a y V y
a u1 = x r
a x u 2 = r a y
0
2
bax
0 0 0
r
0 0
0 0
br
0
0 0 0 0 ylat 2
ax 2 0 2 0 bax 0 0 Q2 = 0 0 0 0 0 0
0 0
0
2
0 0 0 0
r
0 0 0
0 0
br 2
0 0
ylat 2
0
0 0 0 0 0 2 ay
189

Model 1 Solution
1 0.5 0 -0.5 -1 -1.5 -2 0 0.1 Heading (deg)
With the measurements from the laser scanner, usable estimates are obtained
2 Lidar r KF 1.5
Lidar ax KF
Velocity (m/s)
0.5
Removes inertial sensor errors to provide unbiased solution Velocity and heading cleanly estimated
10 Time (s)
15
-0.5 20 0 2.5 Longitudinal Accelerometer Bias (m/s)

2
10 Time (s)
15
20
2 1.5 1 0.5 0 -0.5 -1 -1.5 20 0 5 10 Time (s) 15 20
0.05 Yaw Gyro Bias (deg/s)
-0.05
-0.1
-0.15 0
10 Time (s)
15
190

Model 1 Solution
Lateral error estimate can be tuned by varying value for ylat

Accounts for unmodeled dynamics and disturbances, such as lateral velocity Level of filtering on estimate decreased with higher values for ylat
0.1 0 -0.1 Lateral Error, ylat (m) -0.2 -0.3 -0.4 -0.5
0.1 0 -0.1 Lateral Error, ylat (m) -0.2 -0.3 -0.4 -0.5 -0.6 -0.7 0 Lidar KF 5 10 Time (s) 15
-0.6
-0.7 GPS and Lab 0 5 20 Vehicle Dynamics
Lidar KF
10 Time (s)
15
20
191

Model 2 Solution
Model 2 displayed the same estimates for velocity and heading since there were no model differences from model 1 Lateral velocity observable with laser scanner measurements if a lateral accelerometer is used as an input Lateral offset estimate now accounts for lateral motion ylat is set to zero, and navigation system still tracks lateral error
0.1 0 -0.1 Lateral Error, ylat (m) -0.2 -0.3 -0.4 -0.5 -0.6 -0.7 0 Lidar KF
GPS and5 Vehicle Dynamics Lab 10 15

Time (s)
20
192
GPS/INS/MAP Enhancement of a Lane Tracking Camera System
Research Motivation
Vehicle lane departure major cause of highway fatalities
42,000 roadway fatalities in 2004, 50% resulting from vehicle lane departure ADAS Advanced Driver Assistance Systems LDW- Lane Departure Warning
Use camera to detect a lane marker Send warning to driver if lane is being approach Helps to prevent un-intended lane departure
ITS Research

Applications also include

Military/Defense General Robotics

Basic Edge Detection

Filter the image (E) in orthogonal directions (x and y) with convolution masks Sobel operator 2D gradient filter (2D convolution
1 0 1 G x = E 2 0 2 1 0 1
1 2 1 Gy = E 0 0 0 1 2 1
195
Edge Detection
Compute a gradient magnitude
G = Gx + G y
2 2
Threshold the magnitude to define a detected edge 1, G > t g E= o . w. 0, Canny edge detection extension of Sobel operator, most common method
Corner Detection
Takes edge detection one step further General method
1. 2. 3.
Gradient filtering (2D-convlution) Extract the edge image Find corners along detected edges
Detected corners can be used as features for other calculations

197
Demonstration
Stop motion sequence Top frame corners marked with white dots Bottom frame edge detection with Sobel operator
198
Optical Flow
Optical flow computation gives a velocity estimation of objects within an image frame Horn and Schunck - Least squares minimization with constraints on smoothness and brightness Three dimensional movement projected onto 2D image plane
H&S Algorithm Demo
200
Performance Limitations
Current LTCSs suffer from:
Environmental constraints Occluded markers: rain, mud, snow Lighting: poor lighting (night), glare from sun Highway conditions Poor/non-existent lane markings Markers occluded by another vehicle
Our Research
Improved LTCS Fuse GPS/INS/MAP with camera system Assist and/or compensate when lane detection fails
201
Simulation Experiment
Vehicle model generate, East, North and heading Create GPS, IMU and camera measurements from data Simulate vehicle driving in a mapped road lane
202
Description of GPS/INS/LDW Lane Tracking System

bGPS = y cam y ldw
1. 2. 3. 4. 5.
Collect lane offset reading from camera Create an estimated offset from GPS position w.r.t. map Difference is a measurement of GPS bias Setup a KF estimation for lateral dynamics (offset, velocity and heading) and sensor biases If bias is known, can the lane be tracked with GPS/IMU/MAP if camera fails?
Vehicle Navigation Using GPS/INS/Vision

Infinity M45:

LDW Camera GPS and IMU Steer angle Wheel Speed
Tests conducted at National Center for Asphalt Technology Test Track (w/ Road Database
Preliminary Data (LDW and GPS)
Differential GPS
Non-Differential GPS
205
DGPS, MAP, LTC data

Good lane markings
DGPS/LDW VS MAP (north,inside lane), 2/15/06 15
Camera not detecting any right marker
10
lateral distance from reference, meters
10
15 800
Poor left lane markings camera detecting road edge

700 600 500 400 300 200 longitudinal distance from reference, meters DGPS Center lane marker Inside lane marker LDW right lane estimate LDW left lane estimate 100 0 100
206
DGPS, MAP, LTC data

Lateral offest (left lane) with error 0 0.5 DGPS & MAP LDW
Lateral lane offset
1 1.5 2 2.5 3 3.5 800 700 600 500 400 300 200 Longitudinal distance from reference (m) 100 0 100
2 Error 1.5 1
High error due to poor lane markings
Error
0.5 0 0.5 1 800 700 600
GPS and Vehicle Dynamics Lab 500 400 300 200 100 Longitudinal distance from reference (m)
Small error with good lane markings

0 100
207
Lane Tracking Estimation -6 state

KF states

Inputs

Lateral position and velocity Heading angle Biases (accelerometer, gyro, GPS) Accelerometer Gyro Lateral offset (camera) Lateral offset (GPS + map) Heading (GPS)
State Equations: & =a V y accel ba c a + wa

& = Vy y & = rgyro br cr + wr & =w b a ba & =w b r br & = 1 b + w b GPS GPS GPS
Measurements
yLTC = y +LTC yGPS = y + bGPS +GPSy
GPS = +GPS
Measurements:

Inputs:
a accel = a y + c a + b a + w a rgyro = r + b r + c r + w r
208
Estimation Model
& 0 V y 1 & y & 0 & = 0 ba b & 0 r 0 & bGPS 0 0 0 0 0 0 0 0 0 0 0 0 0 V 1 y 0 0 y 0 0 1 0 0 0 0 0 b + 0 a 0 0 0 b 0 1 r 0 0 bGPS 0 Vy y 0 0 LTC 0 1 + GPS 1 b a 0 0 b GPS 2 r b GPS 0 1 0 0 0 1 0 0 0
& = Ax + B u u + B w w x
y = Cx +
ca 1 0 0 a y cr 0 r + 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 wa 0 w r 0 wb 0 a wb 0 r wGPS 1
0 y= 0 0
1 1 0
0 0 1
0 0 0
2 a 0 0 0 0 y 2 0 0 0 0 r 2 Qc = 0 0 ba 0 0 2 0 0 0 0 br 2 0 0 0 0 bGPS
2 LTC Rd = 0 0
0
2 GPS
y
0 0 2 GPS
209
Experimental Data
Data collected on south side of NCAT
Data logged at 70 Hz
210
GPS/LDW/IMU Measurements
Course/Yaw
Left Lane
IMU (ay & r)
Right Lane
211
Results
Estimation of IMU Biases and GPS/Lane Database Offset
212
Camera failure
Camera failure (30 s outage rely on corrected GPS/Map database) Incorrect lane detection
Loss of Lane Marking Incorrect Lane Detection (detecting road edge)
213
Orientation Accuracy
IMU Only
0.15
IMU/Vision
0.08 0.06
0.1
0.04
0.05
0.02
(deg)
(deg)
0 0.02 0.04
0.05
0.1
0.06
0.15
0.08 0 10 Lab 20 80 90 100 GPS and Vehicle Dynamics 0.1
0.2 0
10
20
30
40
50 60 Time (s)
70
30
40
50 60 Time (s)
70
80
90
100
214
Velocity Accuracy
IMU Only
2.5 2
0.3 0.4
IMU/Vision
1.5
0.2
1 0.5
0.1
(m/s)
V (m/s)
0 0.5 1
0 0.1 0.2
1.5 2 2.5 0 10 20 30 40 50 60 Time (s) 70

0.3
80 90 100 0 10 GPS and Vehicle Dynamics Lab 20
0.4
30
40
50 60 Time (s)
70
80
90
100
215
Position Accuracy
Determined using a covariance analysis based on sensor noise statistics Assumes data is available (data base) to produce a vision based position solution
IMU Only
80 60
IMU/Vision
1.5 1
40
0.5
20
(m)
(m)
20
0.5
40
1
60 80 0
10
20
30
40
50 60 Time (s)
70
0 10 80 90 GPS and 100 Vehicle Dynamics Lab 20
1.5
30
40
50 60 Time (s)
70
80
90
100
216
Vehicle State Estimation and Control

Control systems utilize estimates of states such as wheel slip and sideslip
Can provide information on tires, traction, etc. integration of inertial sensors

drifts due to bias and sensor noise errors from vehicle roll and pitch
Traditional Methods Rely on:
estimation using models

model must be known very well extensive modeling required dependent on varying environment and parameters such as tire
cornering stiffness, CG location, ground conditions, etc.
State estimation can then be used to estimate critical vehicle parameters for steering control systems
Typical Yaw Vehicle Model

FyOR VOR OR FxOR b r T FyIR VIR IR FxIR Vy y a Vx
FyOF
x FyIF
V OF OF FxOF
V VIF IF FxIF
V = velocity r = yaw rate = vehicle heading = steer angle = body side slip = tire side slip C = cornering stiffness
Fy = C
C0 & mV = C1 & r IZ
1 + mV 2
C2 I ZV
C1
f C mV + aCf r IZ
218
Tire Behavior
Fx = C s s F y = C
Linear for small slip angles Saturates at higher slip angles (vehicle slides) Varies with loading Corning stiffness depends on tire only Peak force changes with surface () and tire
Pacejka Magic Tire Model (SAE Paper 870421 )

Analytical Tire Behavior

Pacejka Model
7000 6000 5000 Lateral Force (N) 4000 3000 2000 1000 0 -60
Fz = 2 kN Fz = 4 kN Fz = 6 kN Fz = 8 kN
Dugoff Model
7000 6000 5000 Lateral Force (N) 4000 3000 2000 1000 0 -60
Fz = 2 kN Fz = 4 kN Fz = 6 kN Fz = 8 kN
-50
-40
-30 -20 Slip Angle (deg)
-10
-50
-40
Difficult to determine parameters for a particular tire
Fewer parameters to determine Inherent inaccuracies

220
-30 -20 Slip Angle (deg)
-10
Calculation of Vehicle States

Body Side Slip: Correct for Angular Velocities:
VEL GPS
v v VP = VA + rA / P
Must Move GPS Velocity into Vehicle Frame

integration of a gyro 2 antenna GPS System
Tire Sideslip and wheel slip

tire VX rtire ABS % slip = 100% tire VX
Side Slip at any point on the Vehicle: P V 1 Y P = tan V P X Tire Force & Cornering Stiffness
& Fx = m& x & = Fyf + Fyr cos( ) Fy = m& y && = aFyf bFyr cos( ) M z = I z
Fyf
f = r =
tire f tire r
C f =
, C r =
Fyr
, Cx =
Fx % slip
221
Small Low-Cost Vehicle GPS/INS System

ESC Scaled Experiment Testbed
CG Relocator GAVLAB GPS/INS System

IMU at 60 Hz GPS at 4 Hz Wireless Data Acquisition Prototype Cost < $500
222
ESC II in Action (Using GAVLAB GPS/INS System)
Without ESC II
With ESC II
223
Vehicle Estimation
Infinity G35:

GPS Velocity & Position 6 DOF IMU Steer angle Wheel Speed
Tests conducted at National Center for Asphalt Technology Test Track

224
G35 Data Acquisition Setup

CrossBow IMU

Starfire/Beeline GPS

3 Axis Acceleration 3 Axis Rotation Rate Longitudinal Speed Lateral Speed

Wheel Speeds Steer Angle
DATRON Velocity

Position & Velocity Course Heading & Roll Data Logging Real Time Analysis
On Board PC

CAN

Dash Center Console Trunk
225
Vehicle State and Parameter Estimation

Estimate vehicle parameters and states that may otherwise:

be difficult to measure require expensive sensors
Uses GPS and low cost inertial sensors with a vehicle test-bed
Estimation of Sideslip and Yaw Rate Estimation of Understeer gradient
Offset due to road bank

226
Wheel Slip Measurement

Generally limited by wheel speed resolution (not GPS) Accurate in the linear ( Fx = C x slip = m& & ) and non-linear x tire region ( Fx = f ( slip ) = m& &) x V r
% slip =
tire X tire tire X ABS
100 %
20
Velocity (m/s)
Moderate Acceleration/Braking
Velocity (m/s)
20 15 10 5 0 0 20 0
% slip
Heavy Braking
Wheel Speed Sensor GPS
15 Wheel Speed Sensor GPS 10 0 6 4 10 20 30 40 50 60
10
15
% slip
2 0 2 4 0 10 20 30 Time (sec) 40
20 40 ABS Engages 5 Time (sec) 10 15
60 50and Vehicle 60 0 GPS Dynamics Lab
227
Sideslip Estimation
Parking Lot
5 Steer Angle (deg) 0 -5 -10 -15 0 2 Slip Angle (deg) 0 -2 -4 0 5 10 15 20 25 30 Datron GPS/IMU Bicycle Model Slip Angle (deg) 35 40 Steer Angle (deg) 2 0 -2 -4 -6 0 3 2 1 0 -1 0 20 40 60 Time (s) 80 100 120 20 40 60 80 Datron 100 GPS/IMU Bicycle Model 120
Test Track
10
15
20 Time (s)
25
30
35
40
Linear tire used in analytic model
Works well for this scenario

Slip angle matches even with bank angle
Analytic model slightly modified
228
Tire Curve on Wet Pavement

1 x 10
4
Front Tire
1
Rear Lateral Force, Fyr (N)
x 10
Rear Tire
Front Lateral Force, Fyf (N)
0.5
0.5
0.5
0.5
1 10
5 0 5 Front Tire Slip Angle, f (deg)
10
1 10
5 0 5 Rear Tire Slip Angle, r (deg)
10
Still in linear region Measurement does not rely on any tire model
Saturation Present Caused by longitudinal coupling

229
Linear Tire Model Breakdown

Linear Tire Model
20
(deg) (deg)
20
Dugoff Tire Model
20 0 100
r (deg/s)
20 0 5 10 15 20 25 30 100
r (deg/s)
10
15
20
25
30
100 0
100 0 5 10 15 Time (s) 20 25 30
10
15 Time (s)
20
25
30
Simulation does not match experimental
Simulation matches longer Eventually small angle assumption fails

230
Tire Curve Estimation

Simulated
Front Lateral Force (kN) 10
Experimental
Measured Dugoff Fit -10 -5 0 5 Front Slip Angle (deg) 10 15
-10 -15 10
Rear Lateral Force (kN)
-10 -10
-5
0 Rear Slip Angle (deg)
10
Actual lateral force is lower than model predicts (weight transfer)
No filtering done on measurements
231
GPS vs. Datron Tire Curve Estimation

20 0 -20 -40 290 Sideslip Angle (deg) 8 6 4 2 0 -2 290 295 300 305 Time (s) 310 295 300 305 310 315 Rear Lateral Force (kN) Front Lateral Force (kN) 40 Yaw Rate (deg/s) 10 0
-10 -10 10
-5
0 Front Slip Angle (deg)
10
GPS DATRON
GPS DATRON Dugoff Fit 0 5 Rear Slip Angle (deg) 10
315
-10 -5
Yaw Rate and Sideslip
Estimated Fy vs. Sideslip
232
Measurements on Two Different Road Types

Steer Angle (deg)
20 12 10 0 8
Sideslip Angle (deg)
Asphalt
-20 0 5 10 15 20 25 30 35 40 45
6 4 2 0 -2 -4 GPS Datron 5 10 15 Time (s) 20 25 30
Yaw Rate (deg/s)
50 0 -50 0
10
15
20 25 Time (s)
30
35
40
45
Steer Angle (deg)
20
10 5
Sideslip Angle (deg)
Gravel
-20 0 60 5 10 15 20 25
-5 -10 -15 -20 -25 -30
Yaw Rate (deg/s)
40 20 0 -20 0 5 10
Time (s)
-35 Lab GPS and 15 20Vehicle 25 Dynamics 0
10
Time (s)
15
20
25
233
On-line Tire Curve Estimation

10 10 Experimental Dugoff Fit
Front Tire Lateral Force (kN)
Rear Tire Lateral Force (kN)

-5 0 5 Front Tire Slip Angle (deg) 10
Asphalt
-5
-5
-10 -10
-10 -25
-20
-15
-10 -5 0 5 Rear Tire Slip Angle (deg)
10
15
10
10
Front Tire Lateral Force (kN)
Gravel
Rear Tire Lateral Force (kN)
0 Filtered Experiment Experimental Dugoff Fit -40 -30 -20 -10 Rear Tire Slip Angle (deg) 0
-5 -35
-30
-25
Dynamics -20 GPS -15 and -10 Vehicle -5 0 5 Front Tire Slip Angle (deg)
Lab
-5
234
EKF On-line Estimation of Tire Curve Parameters

2 1.8 x 10
5
11.5 11
Cornering Stiffness (N/rad)
10.5
Peak Lateral Force (kN)

45
Asphalt
1.6 1.4 1.2 1 0.8 0 x 10

4
10 9.5 9 8.5 8 7.5 7
Front - Recursive Front - Batch Rear - Recursive Rear - Batch
10
15
20 25 Time (s)
30
35
40
6.5 0
10
15
20 25 Time (s)
30
35
40
45
9 8
7 Front - Recursive Front - Batch Rear - Recursive Rear - Batch 6.5
Cornering Stiffness (N/rad)
7 6 5 4 3 2 0 5 10
Peak Lateral Force (kN)
Gravel
6 5.5 5 4.5
Time (s)
4 GPS Dynamics Lab 15 and Vehicle 20 25 0
10
Time (s)
15
20
25
235
Rolling Resistance Estimation

Estimate a change in Longitudinal Force
Monitor changes of speed as trucks pass over differing sections of asphalt
Deduce the relative change in rolling resistance and correlate to fuel consumption
Data Acquisition and Instrumentation
& FDrive- FBrake- FSlope - FRollingResistance- FAirDrag = m& x

Force and Mass Estimation

Use vehicle measurements to estimate:

Drive Force Vehicle Mass Air Drag Rolling Resistance
Fengine =
engine N transmission N final drive mechanical

Rtire
& V2 Fdrive = & x m 1 C df Frr
237
What if GPS is not Always Available

Use GPS to evaluate the performance and model validity of vehicle systems when GPS is available
Leads to levels of confidence of performance when GPS is not available Leads to improved performance when GPS is not available
Use GPS to estimate critical parameters
GPS Model Based Estimator

C0 & mV C 1 & = r & Iz 0 & gyro Bias 0 C1 1 mV 2 C2 VI z 1 0 C 0 0 f mV 0 0 r + aCf Iz 0 0 0 Bias gyro 0 0 0
Uses vehicle dynamic model
Requires knowledge of vehicle parameters

Estimation accuracy
0 1 0 1 y= r 1 0 1 0 Bias gyro
2 0 Qw = 2 0 bias
function of model accuracy
Observable with GPS only
[ y = [
y = rgyro
gps
gps
for 1 antenna rgyro heading
heading
for 2 antenna
239
Simulation with Correct Model
Sideslip and yaw rate using the correct estimator model
Residuals from the estimator using the correct estimator model

240
Simulation with Incorrect Model

Sideslip and yaw rate using correct vehicle understeer but incorrect tire cornering stiffness Estimator residuals using correct vehicle understeer but incorrect tire cornering stiffness
241
Model-Based Estimator Experimental Results
Sideslip and yaw rate from extreme cornering experiment
Residuals from yaw rate and GPS for extreme cornering experiment with incorrect vehicle parameters
242
Experimental Results with Modified Vehicle Parameters
Sideslip and yaw rate estimate improved using modified tire cornering stiffness
Estimator residuals from parking lot test with modified tire cornering stiffness
243
AUBURN UNIVERSITY GPS AND VEHICLE DYNAMICS LAB

www.eng.auburn.edu/~dmbevly/gavlab

Navigatie Cu GPS

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Navigatie Cu GPS

Uploaded by

Copyright:

Available Formats

Navigation for Control of Ground Vehicles

IMU Modeling and Navigation (27 Slides)

GPS/INS Integration (84 slides)

Autonomous ground vehicles require accurate and robust navigation information

Eventually, highway vehicles might be automated

GPS and Vehicle Dynamics Lab

position velocity direction of travel orientation

control farm vehicles improve safety systems in passenger cars

GPS and Vehicle Dynamics Lab

GPS and Vehicle Dynamics Lab

Global Positioning System (GPS)

How GPS Works

multiply by c to get range triangulate ranges to get position (and time)

GPS and Vehicle Dynamics Lab

GPS and Vehicle Dynamics Lab

(-160 dBw ~ 10-16 watts)

GPS Carrier Wave: L1=1575.42 MHz

Encoded Digital Signal

satellite # time location velocity

GPS time started on January 6, 1980 at 0h

Does not account for leap seconds

Typically accurate to 50ns GPS weeks are numbered sequentially

Space Segment: 29 satellite constellation

Current GPS Signals

GPS and Vehicle Dynamics Lab

Future GPS Signals

L2 Civil Signal: L2C

GPS Broadcast Signal Structure

These parameters are collectively known as ephemeris data.

GPS and Vehicle Dynamics Lab

Gold Codes and Spread-Spectrum Transmission

GPS Signal Structure

GPS and Vehicle Dynamics Lab

Precision Code - P(Y)

50 bits/s Bits(30) Words(10) Subframes(5) Frames (or Page) 5 Subframes:

GPS Signal Structure

S Lli (t ) = 2 Pc XGi (t ) Di (t ) cos(1t + )

Signal = C/A x Data x Carrier

GPS and Vehicle Dynamics Lab

GPS position solution requires Raw Ephemeris, Pseudoranges, and Time

Outputs: Position (X,Y,Z)

Pseudorange errors: i = Algorithm:

Note 1 s error in time = 300 m error

(SatPos.x User.x )2 + (SatPos. y User. y )2 + (SatPos.z User.z )2

RF down conversion to IF GPS and Vehicle Dynamics Lab

The PVT Solution Process

GPS Measurement Model: Pseudorange as Relative Position

User Solution Calculation

GPS and Vehicle Dynamics Lab

Dilution of Precision (DOP)

2 2 PDOP UERE = x + y + z2 2 2 HDOP UERE = x + y

VDOP UERE = z TDOP UERE = b

Data 1- (39.3 % inside) 2.45- ( 95 % inside)

Kaplan, E., Hegarty, C., Understanding GPS Principles and Applications

Radius Accuracy Standards

Kaplan, E., Hegarty, C., Understanding GPS Principles and Applications

Radius Accuracy Standards

As before, the exact ratio depends on the correlation coefficient

Radius CDF Accuracy

Plot differences due to non-circularity CEP = 0.76 DRPS

GPS and Vehicle Dynamics Lab

GPS and Vehicle Dynamics Lab

GPS Errors from the Satellites