navigatie cu gps
Post on 06-Apr-2018
256 Views
Preview:
TRANSCRIPT
-
8/3/2019 Navigatie Cu GPS
1/244
Navigation for Control of Ground
VehiclesDavid M. Bevly
Assistant Professor
Department of Mechanical Engineering
Auburn University, AL 36849-5341
Director of Auburn University's
GPS and Vehicle Dynamics Lab (GAVLAB)
-
8/3/2019 Navigatie Cu GPS
2/244
GPS and Vehicle Dynamics Lab2
Presentation OverviewOverview of GPS (73 slides)
History of GPS Signal Structure
Measurements and Accuracy
IMU Modeling and Navigation (27 Slides) IMU errors
GPS/INS Integration (84 slides) Introduction of Kalman Filtering
JD and DGC Examples
Navigation Errors
Lidar and Vision Navigation (30 Slides)
GPS/INS for Estimation of Vehicle States andParameters (30 Slides)
-
8/3/2019 Navigatie Cu GPS
3/244
GPS and Vehicle Dynamics Lab3
MotivationFuture stability control systems for passenger vehicles will use aprecision 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
-
8/3/2019 Navigatie Cu GPS
4/244
GPS and Vehicle Dynamics Lab4
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
-
8/3/2019 Navigatie Cu GPS
5/244
GPS and Vehicle Dynamics Lab5
Coordinate Nomenclature
V = velocityr = yaw ratep = roll rate
q = pitch rate = yaw angle= roll angle
= pitch angle= road grade
p, q
, r
, p
g
Vz
Vy
Vx
-
8/3/2019 Navigatie Cu GPS
6/244
GPS and Vehicle Dynamics Lab6
Coordinate Nomenclature
V = velocityr = yaw rate
= heading (or yaw)
= vehicle course = steer angle = body sideslip angle = tire sideslip angle
-
8/3/2019 Navigatie Cu GPS
7/244
GPS and Vehicle Dynamics Lab7
Global Positioning System (GPS)
24+ satellites inwell known orbitsproviding preciseranging source
6 orbital planes55 inclinations12 hour orbits
20,200 km orbits
Ground trackrepeats every23:56:04
-
8/3/2019 Navigatie Cu GPS
8/244
GPS and Vehicle Dynamics Lab8
How GPS Works
measure thetransit time for asignal from SV touser multiply by c to
get range
triangulateranges to getposition (andtime)
-
8/3/2019 Navigatie Cu GPS
9/244
GPS and Vehicle Dynamics Lab9
GPS FactsThere are more than 100 times as many
civilian users than military users.5 million recreational GPS devices wereshipped 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 2010http://gps.losangeles.af.mil/jpo/gpsoverview.htm
-
8/3/2019 Navigatie Cu GPS
10/244
GPS and Vehicle Dynamics Lab10
GPS Signal (-160 dBw ~ 10-16 watts)
digital code:(satellite
info)
satellite #time
location
velocity
19 cm
GPS Carrier Wave: L1=1575.42 MHz
Encoded Digital Signal
Roughly equivalent to viewing a 25-wattlight bulb from a distance of 10,000 miles.
-
8/3/2019 Navigatie Cu GPS
11/244
GPS and Vehicle Dynamics Lab11
GPS Time
The continuous atomic timescale used on thesatellites 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 Currently ahead of UTC time by 14 seconds Ex: UTC 10:34:25; GPS 10:34:39
Typically accurate to 50ns GPS weeks are numbered sequentially
Start from 0 at 0h on January 6, 1980 Increment every Sunday at 0h
-
8/3/2019 Navigatie Cu GPS
12/244
GPS and Vehicle Dynamics Lab12
3 Segments of GPSControl 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 thenavigation data to each of the GPS satellites.
Space Segment: 29 satellite constellation Each satellite transmits at L1 (1575.4 MHz) and L2 (1227.6
MHz)
User Segment: all users, military and civilian,
commercial and individual, who utilize the GPS signal
-
8/3/2019 Navigatie Cu GPS
13/244
GPS and Vehicle Dynamics Lab13
Current GPS Signals
L1 (1575.42 MHz) Coarse/Acquisition (C/A) and P(Y) Code
Civilian Use
L2 (1227.60 MHz)
P(Y) Code
Military Use
-
8/3/2019 Navigatie Cu GPS
14/244
GPS and Vehicle Dynamics Lab14
Future GPS SignalsNAVSTAR (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)
Other Countries GALILEO (2010-2015 for full constellation)
GLONASS (??)
Australia, Japan, China, etc.
-
8/3/2019 Navigatie Cu GPS
15/244
GPS and Vehicle Dynamics Lab15
GPS Broadcast Signal
StructureEach satellite transmits the precise time (UTC-USNO),
the complete parameters of its orbit, and the majorparameters 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. induration and is transmitted in digital form at a rate of50 bps.
This data transmission modulates the GPS carrierwave using binary phase-shift keying(BPSK)
-
8/3/2019 Navigatie Cu GPS
16/244
GPS and Vehicle Dynamics Lab16
Gold Codes and
Spread-Spectrum Transmission
Gold Codes are a family of unique binarysequences which have very low cross-correlation with other sequences in the familyand low auto-correlation as well.
Modulating each GPS satellites signal by aunique Gold Code, known as the PRNnumber, spreads the signal over a widerbandwidth, which provides noise rejection
and enables multiple access (CDMA). Allows satellites transmit on the same frequency
at the same time without interfering with eachother
-
8/3/2019 Navigatie Cu GPS
17/244
GPS and Vehicle Dynamics Lab17
GPS Signal Structure
3 Components Carrier Wave
L1, L2
Code Signal C/A, P(Y)
Navigation Data
Satellite Information
-
8/3/2019 Navigatie Cu GPS
18/244
-
8/3/2019 Navigatie Cu GPS
19/244
GPS and Vehicle Dynamics Lab19
Code Signal
Code Division Multiple Access (CDMA)Course Acquisition - C/A Gold Codes
Code Period of 1 ms
Precision Code - P(Y)
Anti-Spoofing Mode Code reset each week
-
8/3/2019 Navigatie Cu GPS
20/244
GPS and Vehicle Dynamics Lab20
Navigation Data
Navigation Data from the Data Bits 50 bits/s Bits(30) Words(10) Subframes(5)
Frames (or Page)
5 Subframes:1) Clock Correction & Satellite Quality
2) Ephemeris
3) Ephemeris4) Almanac & Ionosphere & UTC Corrections
5) Almanac
-
8/3/2019 Navigatie Cu GPS
21/244
GPS and Vehicle Dynamics Lab21
GPS Signal Structure( ) ( ) +++= ttDtXPPttDtXGPtS iipiicLli 11 sin)()(2cos)()(2)(
( ) += ttDtXGPtS iicLli 1cos)()(2)(
Signal =C/A x Data xCarrier
-
8/3/2019 Navigatie Cu GPS
22/244
GPS and Vehicle Dynamics Lab22
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)
1
23
4
-
8/3/2019 Navigatie Cu GPS
23/244
GPS and Vehicle Dynamics Lab23
PseudorangeSatellite Positions vs. Time Pseudoranges
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 untilcorrelation achieved (C/A code repeats every 1 ms)
Definition: Note 1 s error in time = 300 m error
Pseudorange errors:
Algorithm:
( ) ( ) ( )222 ...... zUserzSatPosyUserySatPosxUserxSatPosiT ++=
( ) usiuiT cbttc +=
( )iiiiiiiTi vvITcbcD +++++=
-
8/3/2019 Navigatie Cu GPS
24/244
GPS and Vehicle Dynamics Lab24
GPS Receiver
RF down conversion to IF
-
8/3/2019 Navigatie Cu GPS
25/244
GPS and Vehicle Dynamics Lab25
The PVT Solution Process1. Received RF signal is down-converted to a lower, intermediate
frequency (about 5 MHz)
2. After signal acquisition, both the carrier (and any Doppler shifts) andthe PRN code sequence are tracked.
3. The outputs of the tracking loops are Doppler frequency (from thecarrier loop), transport time delay (from the code loop), and the
navigation message of the satellite.4. From the navigation message, satellite position is calculated
5. Using the transport time delay, Doppler frequency, andsatellite position, the range to the satellite and velocity
towards the satellite are calculated.6. By using 4 satellites or more, an extended Kalman filter or
Least Squares algorithm combines the range and velocity tocompute user position.
-
8/3/2019 Navigatie Cu GPS
26/244
GPS and Vehicle Dynamics Lab26
GPS Measurement Model:
Pseudorange as Relative PositionPseudorange is distance from user to satellite
ssusususs bzzyyxx ++++=222
)()()(Linearized measurement model is:
For 1,2m satellites -> m measurements
Linearized about most current position estimate
k
k
suk
s xx
xxH*
),(
=
=
b
z
y
x
zzyyxx
zzyyxx
zzyyxx
km
msku
km
msku
km
msku
k
sku
k
sku
k
sku
k
sku
k
sku
k
sku
m 1)
()()(
1...
1...
1...
1)()()(
1)()()(
.
.
.
.
1,
,1,
1,
,1,
1,
,1,
1,2
2,1,
1,2
2,1,
1,2
2,1,
1,1
1,1,
1,1
1,1,
1,1
1,1,
2
1
-
8/3/2019 Navigatie Cu GPS
27/244
GPS and Vehicle Dynamics Lab27
Least Squares
j
uj
xjr
xxa
=
j
uj
yjr
yya
=
j
uj
zjr
zza
=
( ) ( ) ( )222 ujujuji zzyyxxr ++= jj =
=
1
1
1
1
444
333
222
111
zyx
zyx
zyx
zyx
aaa
aaa
aaa
aaa
H
=
u
u
u
u
tc
z
y
x
x
=
4
3
2
1
( ) = 111 RHHRHx TT
-
8/3/2019 Navigatie Cu GPS
28/244
GPS and Vehicle Dynamics Lab28
User Solution Calculation
From the linearized pseudorange equation
The position error can be estimated using LS as
Calculating the covariance ofxyields the 4x4matrix D
xH=
( ) =
TT HHHx1
( ) 12
2
2
2
2
2
termscovariance
termscovariance
=
= HHD TUERE
b
z
y
x
UERE
-
8/3/2019 Navigatie Cu GPS
29/244
GPS and Vehicle Dynamics Lab29
Dilution of Precision (DOP)
Value based solely on satellite geometryHigh DOP value increases the negativeeffect of User Equivalent Range Errors
(UERE)Ideal geometry: 1 satellite directlyabove, others (at least 3) equallyspaced along the horizon
-
8/3/2019 Navigatie Cu GPS
30/244
GPS and Vehicle Dynamics Lab30
DOP Values
Geometric (GDOP)
Position (PDOP)
Horizontal (HDOP)
Vertical (VDOP)
Time (TDOP)
222
zyxUEREPDOP ++=
bUERETDOP =
22
yxUEREHDOP +=
zUEREVDOP =
Note: TDOP is in m, not s
2222
bzyxUEREGDOP +++=
-
8/3/2019 Navigatie Cu GPS
31/244
GPS and Vehicle Dynamics Lab31
Position AccuracyHorizontal position accuracy is often
assumed to have a bivariate Gaussian(normal) distribution
This results in probabilityellipses
Parameters come fromsolution calculationcovariance
( )( )
2 2 2 2,
2,
2
2 1
2
,
1,
2 1
x x y x y y
x y
x xy y
x y x y
PDF ex y
+
=
-5 0 5
-6
-4
-2
0
2
4
6
x
y
Data
1-(39.3 % inside)
2.45-( 95 % inside)
-
8/3/2019 Navigatie Cu GPS
32/244
GPS and Vehicle Dynamics Lab32
Radius Accuracy StandardsDistance Root Mean Square (DRMS)
Contains ~63-69% of the samples
2 DRMS contains ~95-98.5% of thesamples
Exact percentage within radius depends on
the circularity of the ellipse (correlationcoefficient)
Closely matches Gaussian probability
2 2
y UERE DRMS HDOP = + =
Kaplan, E., Hegarty, C., Understanding GPS Principles and Applications
-
8/3/2019 Navigatie Cu GPS
33/244
GPS and Vehicle Dynamics Lab33
Radius Accuracy StandardsCircular Error Probable (CEP)
Radius of a circle containing 50% ofsamples
Originally used for military targeting
accuracy
As before, the exact ratio depends on thecorrelation coefficient
0.75CEP DRMS
Kaplan, E., Hegarty, C., Understanding GPS Principles and Applications
-
8/3/2019 Navigatie Cu GPS
34/244
GPS and Vehicle Dynamics Lab34
Radius CDF Accuracy
Note:
Plot differences due tonon-circularity
CEP= 0.76DRPS
0 1 2 3 4 50
20
40
60
80
100
r/
Probability
Data
CDF
-5 0 5
-6
-4
-2
0
2
4
6
x
y
Data
1-
CEP
DRMS
2DRMS
1 -
96.8
67.1
50
40.2
Actual%
95-98.599.22DRMS
63-6969.9DRMS
5050CEP
39.339.3
Approximate%
CDF
%
-
8/3/2019 Navigatie Cu GPS
35/244
-
8/3/2019 Navigatie Cu GPS
36/244
GPS and Vehicle Dynamics Lab36
GPS Errors from the Satellites
Ephemeris Errors Difference in transmitted and actual
satellite location (Slowly varying)
Satellite Clock Errors SA contribution (now off)
Based on stable atomic clocks
-
8/3/2019 Navigatie Cu GPS
37/244
GPS and Vehicle Dynamics Lab37
GPS Errors from Atmosphere
Ionosphere Errors
Free electrons cause delay of signal proportionalto inverse of carrier frequency squared
Without SA, the largest error component
Requires model to correct
Troposphere Errors
Highly variable
Smaller contribution to error
Affects both L1 and L2 equally
-
8/3/2019 Navigatie Cu GPS
38/244
GPS and Vehicle Dynamics Lab38
GPS Errors from User
Multipath Reflected signals masking actual
correlation peak
Reduce by using cut-off angle, goodantenna location, antenna and signalprocessing techniques
Receiver Errors Thermal noise
Software accuracy
Parkinson and Spilker, Global Positioning System: Theory and Applications Vol. 1, AIAA, 1996
-
8/3/2019 Navigatie Cu GPS
39/244
GPS and Vehicle Dynamics Lab39
1 GPS Error Budget (in meters)
P CodeC/A Code
10.2
12.8
5.1
5.3
0.5
1.4
0.7
4.0
2.1
2.1
Total
3.3
3.3
0.5
1.0
0.5
1.0
2.0
2.1
Bias
0.4
1.4
0.2
1.0
0.5
0.5
0.7
0.0
Random
6.7Horizontal 1- errors (HDOP=2.0)
8.3Vertical 1-errors (VDOP=2.5)
3.30.45.1Filtered UERE, rms
3.61.45.1UERE, rms
0.50.20.5Receiver Measurement
1.41.01.0Multipath
0.70.50.5Troposphere
1.10.54.0Ionosphere
2.10.72.0Satellite Clock
2.10.02.1Ephemeris Data
TotalRandomBiasError Source
UERE (User Equivalent Range Error)
Ted Driver, Statistical Analysis of Military and Civilian Navigation Error Data Services,
Proceedings of the 2006 ION-GNSS Conference
-
8/3/2019 Navigatie Cu GPS
40/244
GPS and Vehicle Dynamics Lab40
Carrier Smoothing Synergizes Carrier
and Code Phase Observations
Uses carrier phase observation and code phaseobservations to improve accuracy of pseudorange
Smoothing algorithms use Doppler information fromcarrier frequency to correct raw code phaseobservation 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
-
8/3/2019 Navigatie Cu GPS
41/244
GPS and Vehicle Dynamics Lab41
GPS Errors
Receiver 1&2:RTDReceiver 3: Saphire
Receiver 4: Starfire
-
8/3/2019 Navigatie Cu GPS
42/244
GPS and Vehicle Dynamics Lab42
GPS Errors
Common Mode Errors can easily beseen by two GPS receivers
GPS Errors (Effect of Ground
-
8/3/2019 Navigatie Cu GPS
43/244
GPS and Vehicle Dynamics Lab43
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
Effect of Velocity/Acceleration
-
8/3/2019 Navigatie Cu GPS
44/244
GPS and Vehicle Dynamics Lab44
Effect of Velocity/Acceleration
on a Cheap GPS ReceiverDriving aroundtest track atdifferent speeds
GPS Error=f(V)
Possibly due tolack of carrier PLL
-
8/3/2019 Navigatie Cu GPS
45/244
GPS and Vehicle Dynamics Lab45
Relative Positioning
Determination of the baseline vector between a
known receiver location and arbitrary receiverlocation 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 (morenoisy) Carrier triple differencing removes cycle slip effects
-
8/3/2019 Navigatie Cu GPS
46/244
GPS and Vehicle Dynamics Lab46
Differential GPS Correction
Method to improve the positioning and timingperformance of GPS
Use base stations to measure error signalsand 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
-
8/3/2019 Navigatie Cu GPS
47/244
GPS and Vehicle Dynamics Lab47
Differential GPS (DGPS)
Use a Base Station (at known location)to correct common GPS errors
~1.0~5-40Total
~030SA
0.50.5Multi-path
0.50.5Receiver Noise
~01SV Ephemeris
~01SV Clock
~00.5Troposphere
~05Ionosphere
DGPS (m)GPS (m)Error Source
-
8/3/2019 Navigatie Cu GPS
48/244
GPS and Vehicle Dynamics Lab48
DGPS Accuracy10 meter accuracy based on Federal RadionavigationSystems (FRS) report published jointly by the U.S.
DOT and Department of Defense (DoD)Dependent on users distance from transmissionsource
In 1993, the US DOT estimated error growth of 0.67
m per 100 km from the broadcast siteMeasurements of accuracy in Portugal suggest adegradation of just 0.22 m per 100 kmhttp://en.wikipedia.org/wiki/Differential_GPS
Recent results have shown troposphere errors can besignificant in RTK systems over short baselines:
David Lawrence, et.al, Decorrelation of Troposphere Across Short Baselines, Proceedingsof the 2006 IEEE/ION Positioning, Location, and Navigation Symposium (PLANS)
-
8/3/2019 Navigatie Cu GPS
49/244
GPS and Vehicle Dynamics Lab49
NDGPS Coverage
-
8/3/2019 Navigatie Cu GPS
50/244
GPS and Vehicle Dynamics Lab50
Starfire and Omnistar
Omnistar Starfire
http://www.oznet.ksu.edu/pr_prcag/StaticDF04.htm
-
8/3/2019 Navigatie Cu GPS
51/244
GPS and Vehicle Dynamics Lab51
Starfire DGPS vs GPS
20 10 0 10 20 30 4020
15
10
5
0
5
10
15
North(m)
East (m)
Starfire/Beeline East vs North
Beeline
Starfire
-
8/3/2019 Navigatie Cu GPS
52/244
GPS and Vehicle Dynamics Lab52
Starfire DGPS vs GPS
1 -
1.09 m
0.547 m
0.500 m0.424 m
Receiver 2
(Starfire)
5.54 m2DRMS
2.77 mDRMS
1.92 mCEP1.63 m
Receiver 1
(GPS)
Note: Noise is not Gaussian(long term bias drift)
-20 0 20-20
-10
0
10
20
x (m)
y(
m)
-0.5 0 0.5
-0.5
0
0.5
x (m)
y(
m)
Receiver 1
Receiver 2
1-
CEP
DRMS
2DRMS
0 1 2 3 4 5 6 70
50
100
Probability
FirstReceiver
Data
CDF
0 0.2 0.4 0.6 0.8 1 1.20
50
100
r(m)
Probability
SecondReceiver
-
8/3/2019 Navigatie Cu GPS
53/244
GPS and Vehicle Dynamics Lab53
DGPS Position vs time
-
8/3/2019 Navigatie Cu GPS
54/244
GPS and Vehicle Dynamics Lab54
Carrier Phase DGPS (RTK)local reference stationrequired
solve for integerambiguity
track carrier phase
phase at referenceantenna is broadcastto user
positioning software
calculates3-D accuracy* = 2 cm
UserReference
Antenna
R
+
),( NfR
Up
North
East
==
v
v
*Actual depends on baseline length (1 cm + 1 ppm)
-
8/3/2019 Navigatie Cu GPS
55/244
GPS and Vehicle Dynamics Lab55
More on RTKRTK Real Time Kinematic GPS
RTK GPS calculates the relative position, R, between a
rover and fixed base station to sub centimeter accuracyInteger ambiguity (IA), N, must be calculated Many published algorithms available
Can take 20 minutes
New techniques utilizing L1 and L2 (wide laning) are nearlyinstantaneous
+=360
12NR
19 cm1 2
L1 Signal
R
L1forcm19==f
c
L1 f=1.5 GHz
-
8/3/2019 Navigatie Cu GPS
56/244
-
8/3/2019 Navigatie Cu GPS
57/244
GPS and Vehicle Dynamics Lab 57
What is DRTK?
Dynamic RTK is the idea to use amoving, or dynamic, base station tocalculate relative position between it
and a rover 19 cm1 2
L1 Signal
R
-
8/3/2019 Navigatie Cu GPS
58/244
GPS and Vehicle Dynamics Lab 58
DifferencesRTK (Fixed base station)
IA algorithms are published
Method well studied
Cycle slip is minimized withgood receiver
Speed and distance rangesknown
DRTK (Dynamic base station)
IA algorithms differ
Method not well studied
Cycle slip can occur easier
May have detrimental effecton system
Fix with IMU?
Delay time
What is acceptable?
FCS vehicles may not havedirect com link
Speed range unknown
Distance range unknown
UserReference
Antenna
R
N+
),( NfR
Up
North
East
==
v
Applications of Relative
-
8/3/2019 Navigatie Cu GPS
59/244
GPS and Vehicle Dynamics Lab 59
pp
NavigationDevelop relative navigation scheme to improve ground
vehicle convoys
-
8/3/2019 Navigatie Cu GPS
60/244
GPS and Vehicle Dynamics Lab 60
Limitations of the DRTK MethodRover position measurement is a position relative to themoving base vehicle, not global position
Relative position is single vector from follower to moving basevehicle
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
-
8/3/2019 Navigatie Cu GPS
61/244
GPS and Vehicle Dynamics Lab 61
Building DRTKBuilding a DRTK system will allow modification ofinternal 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
-
8/3/2019 Navigatie Cu GPS
62/244
GPS and Vehicle Dynamics Lab 62
GPS Position Accuracy (1)Military Stand Alone (No SA) ~3m, global coverage
Civil Stand Alone (w/ SA) ~30m, global coverage
Code Phase Differential (DGPS) ~0.1m-1m not all are global, but almost full US coverage
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 ~10 cm
-
8/3/2019 Navigatie Cu GPS
63/244
GPS and Vehicle Dynamics Lab 63
GPS Velocity MeasurementsNo Reference Station Required
Uses Doppler Shift of Difference
in two carrier measurements generally a sample delay
associated with themeasurements
Accuracy
0.2-0.5 m/s with SA
3-5 cm/s without SA
Provides accurate measurements tocorrect IMU errors
~V
-
8/3/2019 Navigatie Cu GPS
64/244
GPS and Vehicle Dynamics Lab 64
Accuracy of GPS Velocity (no SA)
30 minutes of static data1 = 1.8 cm/sec in each axis
0 5 10 15 20 25 3010
5
0
5
10
EastVelo
city(cm/s)
Mean = 0.7 cm/sec 1 = 0.9 cm/sec
0 5 10 15 20 25 3010
5
0
5
10
NorthVelocity(cm/s)
Time (min)
Mean = 0.1 cm/sec 1 = 1.8 cm/sec
10 5 0 5 1010
5
0
5
10
East Velocity (cm/s)
NorthVelocity(cm/s)
GPS Velocity Based Heading
-
8/3/2019 Navigatie Cu GPS
65/244
GPS and Vehicle Dynamics Lab 65
y g
Accuracy Heading accuracy based onE-N GPS velocity noises V
vel =
0 5 10 15 20 25 30 35 400
0.5
1
1.5
2
2.5
3
3.5
4
Speed (m/s)
Error(deg)
vel
vel
Monte CarloExperimental
(rad)
1.0
V
(rad)05.0
V
=
north
GPS
east
GPSGPS
V
V1tan
=
GPS
up
GPSGPS
V
V1tan
Cohen C.E., Parkinson, B.W., McNally, B.D., Flight Tests of Attitude Determination Using GPS Compared Againstan Inertial Navigation Unit, Navigation: Journal of the Institute of Navigation, Vol 41, No. 1, Spring 1994.
-
8/3/2019 Navigatie Cu GPS
66/244
GPS and Vehicle Dynamics Lab 66
GPS Attitude
3 antennas
roll
pitchyaw
(Common Clock)
No Reference Station Required
Accuracy Depends on Antenna Spacing (Not Velocity)
GPS Wave Front
-
8/3/2019 Navigatie Cu GPS
67/244
GPS and Vehicle Dynamics Lab 67
GPS Attitude AccuracyAccuracy ~ 0.3/L degrees
(based on 3-4 mm carrier noise)
-
8/3/2019 Navigatie Cu GPS
68/244
GPS and Vehicle Dynamics Lab 68
Uses of GPS
?
Measuring Plate Movement
-
8/3/2019 Navigatie Cu GPS
69/244
GPS and Vehicle Dynamics Lab 69
Using GPS
-
8/3/2019 Navigatie Cu GPS
70/244
GPS and Vehicle Dynamics Lab 70
Traffic MonitoringMean traffic position lies along the
centerline of the lane
Measurementsusing a Starfire
DGPS Receiver
-
8/3/2019 Navigatie Cu GPS
71/244
GPS and Vehicle Dynamics Lab 71
PseudolitesGround-based transmitter that emits GPS-likesignals
Pseudo-satellite -> pseudolite (PL)
Many methods of implementation
C/A Codes
Frequency Offset
Pulsing Scheme
Objectives:
Signal augmentation
Data link enhancement
-
8/3/2019 Navigatie Cu GPS
72/244
GPS and Vehicle Dynamics Lab 72
Pseudolite TypesDirect Ranging PL Original (before GPS satellite) Satellite on the ground
Mobile Pseudolite GPS scheme inverted (stationary receivers)
Digital Datalink Pseudolite Transmit data via GPS signal (max of 1000bps vs.
50bps GPS)
Synchrolites Reflects message from GPS satellites
-
8/3/2019 Navigatie Cu GPS
73/244
GPS and Vehicle Dynamics Lab 73
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
-
8/3/2019 Navigatie Cu GPS
74/244
GPS and Vehicle Dynamics Lab 74
GPS References
Parkinson and Spilker, Global Positioning
System: Theory and Applications Volume1&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
-
8/3/2019 Navigatie Cu GPS
75/244
GPS and Vehicle Dynamics Lab 75
Modeling IMU NavigationPerformance for GPS
Coupling Algorithms
-
8/3/2019 Navigatie Cu GPS
76/244
GPS and Vehicle Dynamics Lab 76
MotivationInitial investigation of IMU models and IMU
error sourcesPredict navigational accuracy during loss ofGPS (function of IMU and dynamics of the
trajectory).Understand the limits of GPS/INSperformance (especially in advancedintegration techniques such as ultra-tightlycoupled GPS/INS).
IMUs (Inertial Measurement
-
8/3/2019 Navigatie Cu GPS
77/244
GPS and Vehicle Dynamics Lab 77
Units)
Usually Consist of 3 accelerometers and3 rate gyroscopes (MEMS, FOG, or RLG)
Analog Devices MEMSAccelerometer andGyroscope
Sentera IMU with ADMEMS sensorsLN200 IMU
-
8/3/2019 Navigatie Cu GPS
78/244
GPS and Vehicle Dynamics Lab 78
Calculate in the Body Fixed Frame (On board IMUs)
Transfer to Earth Fixed Frame
Calculate to determine GPS Doppler shifts in order tocompensate tracking loops
Problem: IMU Errors
xVarrrr
,,,
[ ]
=
Z
Y
X
R
z
y
x
[ ] [ ] RRRR =
satsat Varr
,
Y
Z
XX
Y
Z
Z
Y
X
Y
Z
X
IMU Coordinate Transformation
-
8/3/2019 Navigatie Cu GPS
79/244
GPS and Vehicle Dynamics Lab 79
Simple IMU ModelUsed 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)
sgyrogyro fNw2,0~
2,0~biasgyrobiasgyro
Nw
gyrorrr wbcrg +++=[ ][ ] 22
0
biasgyror
r
bE
bE
=
=
biasgyror
r
r wbb +=
1&
2,0~biasbias accelaccel
Nw
saccelaccel fNw 2,0~
accelxx wbcxa +++= &&&&&&[ ][ ] 22
0
biasaccelx
x
bE
bE
=
=
&&
&&
biasaccelx
x
x wbb += &&&&
&&
&
1
-
8/3/2019 Navigatie Cu GPS
80/244
GPS and Vehicle Dynamics Lab 80
Other AccelerationsAccelerometers Measure specific force
not true accelerationMust compensate for Gravity Field andCentripetal (and Coriolis) Accelerations
accelcxyx wGbrVxa ++++= &&
accelcyxy wGbrVya ++++= &&
r = yaw rate = pitch = roll
V= VelocityGc = 9.81 m/s
2
Inertial Sensor Error Modeling Using Allan Variance, by Hou and El-Sheimy, Proceedings of the 2003 ION-GPS Conference
-
8/3/2019 Navigatie Cu GPS
81/244
GPS and Vehicle Dynamics Lab 81
A time averaging technique used to determine errormechanisms.
Viewed as the time domain equivalent of the powerspectrum density
The PSD is the limiting mean square of a random variable
Allan Variance
0Flicker Noise
1Sinusoidal Input
-1Quantization Noise
1Linear Rate Ramp1/2Rate Random Walk
1/2Exponentially Correlated Noise (First Order Markov Process)
- 1/2Wide-Band Noise
AV SlopeError Mechanism
-
8/3/2019 Navigatie Cu GPS
82/244
GPS and Vehicle Dynamics Lab 82
Autocorrelation AnalysisThe expected value of the product of a random variableor 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
Gyro Parameter Identification
-
8/3/2019 Navigatie Cu GPS
83/244
GPS and Vehicle Dynamics Lab 83
(KVH 5000 FOG)
gyrorrr
wbcrg +++=
0
remove
insignificant
s
gyro
f
=
Gather Static Data
Filter Output and remove constant offset bias
Run Allan Variance to Determine Dominate Error sources
Calculate Angular Random Walk
Accelerometer Parameter Identification
-
8/3/2019 Navigatie Cu GPS
84/244
GPS and Vehicle Dynamics Lab 84Time (hours)
Time (hours)
(Low-cost Humphrey accelerometer)
accelxx wbcxa +++= &&&&&&
biasaccelx
x
x wbb += &&&&
&&
&
1
s
gyro
f
=
0
removed
Modeled as
Gather Static Data
Filter Output and Remove Constant Offset Bias
Run Allan Variance to Determine Dominate Error sources
Calculate Angular Random Walk
Accelerometer Parameter Identification
-
8/3/2019 Navigatie Cu GPS
85/244
GPS and Vehicle Dynamics Lab 85
biasaccelx
x
x wbb += &&&&
&&
&
1
22
biasaccelxbE =
&&
vf
x
accels
accelbias
bias
&&
22=
Variance of the filtered data
Calculate the Variance of the Filtered Data
Take the Autocorrelation of the Filtered Data to
determine the time constant of the Markov Process
Time (hours)
Time (hours)
(Low-cost Humphrey accelerometer)
Validation of Simple
-
8/3/2019 Navigatie Cu GPS
86/244
GPS and Vehicle Dynamics Lab 86
Accelerometer ModelHumphrey 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 fornavigation analysis and
design
Definition of Various Grade Sensors
(b f )
-
8/3/2019 Navigatie Cu GPS
87/244
GPS and Vehicle Dynamics Lab87
(by parameter specification)
0.35/hrBias Variation
100secBias Time Constant
.0017/sec/HzRandom walk(Expensive)
Tactical
180/hrBias Variation
300secBias Time Constant
.05/sec/HzRandom walk(Cheap)
Automotive
360/hrBias Variation
300secBias Time Constant
.05/sec/HzRandom walk(Cheapest)
Consumer
SpecUnitsAttributeRate Gyro
3104.2
3102.1
51050gBias Variation
60secBias Time Constant
.0005g/HzRandom Walk(Expensive)
Tactical
gBias Variation
100secBias Time Constant
.001g/HzRandom Walk(Cheap)
Automotive
gBias Variation
100secBias Time Constant
.003g/HzRandom Walk(Cheapest)
Consumer
SpecificationUnitsAttributeAccelerometer
KVH-5000 Fog Tactical Category
Humphrey Accelerometer Consumer Category
H di I i E B d
-
8/3/2019 Navigatie Cu GPS
88/244
GPS and Vehicle Dynamics Lab88
tTsgyrorotation =
Previous research showed
heading error bounds were
governed by:
Monte Carlo Simulation:
Static Data
Offset Bias Removed
1000 Iterations
Sample at 100 Hz
Heading Integration Error Bounds
Monte Carlo simulation shows
the effects of the walking bias
Neglected bias but can be
considered a best case
scenario.
P iti I t ti E B d
-
8/3/2019 Navigatie Cu GPS
89/244
GPS and Vehicle Dynamics Lab89
Position Integration Error Bounds
8
12tTswwP gyroaccelEast =
3
3
1tTswP accelNorth =
Equation derived to predict
longitudinal and lateral error:
1000 Iteration Monte Carlo Simulation
Static Data with offset bias removed
Sample at 100 Hz
Neglected bias but can be
considered a best case
scenario.
Monte Carlo simulationsvalidate the equations and
show the effects of the
walking bias
Si DOF IMU M d l
-
8/3/2019 Navigatie Cu GPS
90/244
GPS and Vehicle Dynamics Lab90
Six DOF IMU Model
gyrowbcg +++= &
gyrowbcg +++= &
gyrowbcg +++= &
xaccelxxxwbcxa
&&&&&&&&&& +++=
yaccelyyywbcya
&&&&&&&&&& +++=
zaccelzzzwbcza
&&&&&&&&&& +++=
Longitudinal Accelerometer Model
Vertical Accelerometer Model
Lateral Accelerometer Model
Roll Rate Gyro Model
Pitch Rate Gyro Model
Yaw Rate Gyro 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
Si DOF H di E B d
-
8/3/2019 Navigatie Cu GPS
91/244
GPS and Vehicle Dynamics Lab91
Six DOF Heading Error Bounds
Tactical Grade IMU Consumer Grade IMU
Static Gyro with turn on bias = zero
1000 Interation Monte Carlo Simulation (1000 Iterations)
Gravity Field has no effect on the heading accuracy
Si DOF P iti E B d
-
8/3/2019 Navigatie Cu GPS
92/244
GPS and Vehicle Dynamics Lab92
Six DOF Position Error Bounds
Tactical Grade IMU Consumer Grade IMU
Static IMU with zero turn on bias
1000 Iterations Monte Carlo Simulation
Gravity compensation reduces errors caused by accelerometers
Advanced IMU Model
(A l t )
-
8/3/2019 Navigatie Cu GPS
93/244
GPS and Vehicle Dynamics Lab93
(Accelerometers)
( ) ( ) ( )
xaccelxx
AyAyAzAzxxxx
wbc
zyxSFNxSFAxSFa
+++
++++++=
&&&&
&&&&&&&&&&&& sinsin1 2
( ) ( ) ( )
yaccelyy
AxAxAzAzyyyy
wbc
zxySFNySFAySFa
+++
++++++=
&&&&
&&&&&&&&&&&& sinsin1 2
( ) ( ) ( )
zaccelxz
AxAxAyAyzzzz
wbc
yxzSFNzSFAzSFa
+++
++++++=
&&&&&&
&&&&&&&&&&&& sinsin1 2
Longitudinal Accelerometer Model
Vertical Accelerometer Model
Lateral Accelerometer Model
Includes New Error Terms:
Scale Factor, Scale Factor Asymmetry, and Scale Factor Nonlinearity
Misalignment
Nonorthogonality
Advanced IMU Model (Rate
Gyroscopes)
-
8/3/2019 Navigatie Cu GPS
94/244
GPS and Vehicle Dynamics Lab94
Gyroscopes)
Roll Rate Gyro Model
Pitch Rate Gyro Model
Yaw Rate Gyro Model
( ) ( ) ( ) gyroGzGzGyGy wbcSFg +++++++= &&&
sinsin1
( ) ( ) ( )
gyroGxGxGyGy wbcSFg +++++++= &&& sinsin1
( ) ( ) ( )
gyroGxGxGzGz wbcSFg +++++++= &&& sinsin1
Includes New Error Terms:
Scale Factor
Misalignment Nonorthogonality
Other errors are not as
common in rate gyros
Advanced IMU Model
(Misalignment)
-
8/3/2019 Navigatie Cu GPS
95/244
GPS and Vehicle Dynamics Lab95
(Misalignment)
Z
Y
X
mxy
mxz
mzx
mzy
myx
myz
mxxmzz
myy
=
=
Z
m
Y
m zyyzx arcsinarcsin
=
=
Z
m
X
m zxxzy arcsinarcsin
=
=
Y
m
X
m yxzyz arcsinarcsin
Misalignment about X-axis
Misalignment about Y-axis
Misalignment about Z-axis
Large arrows represent the nominal axis (X,Y, and Z)
Smaller arrows represent the misalignment and scale factor errors
Nonorthogonality Errors
-
8/3/2019 Navigatie Cu GPS
96/244
GPS and Vehicle Dynamics Lab96
Nonorthogonality Errors
Z
Y
X
nyy
nyxnyz
nzy
nzxnzz
nxy
nxxnxz
=
=
Z
n
Y
n zyyzx arcsinarcsin
=
=
Z
n
X
n zxxzy arcsinarcsin
=
=
Y
n
X
n yxzyz arcsinarcsin
Nonorthogonality about X-axis
Nonorthogonality about Y-axis
Nonorthogonality about Z-axis
Large arrows represent the nominal axis (X,Y, and Z)
Smaller arrows represent the nonorthogonality and scale factor errors
Input/Output Scale Factor Errors
-
8/3/2019 Navigatie Cu GPS
97/244
GPS and Vehicle Dynamics Lab97
Input/Output Scale Factor Errors
( )InputSFOutput += 1
( )2InputNSFOutput =
Input/Output Errors
InputASFOutput =
Scaled Scale Factor
Scale Factor Asymmetry
Scale Factor Nonlinearity
Simulation of Advanced IMU Model
-
8/3/2019 Navigatie Cu GPS
98/244
GPS and Vehicle Dynamics Lab98
Simulation of Advanced IMU Model
Trajectory Body Accelerations
Simulated Rocket Trajectory
Rocket elevated to 55 degrees
Impact point 86 km downrange
Simulation of Advanced IMU Model
-
8/3/2019 Navigatie Cu GPS
99/244
GPS and Vehicle Dynamics Lab99
Simulation of Advanced IMU Model
Platform Heading NEU Velocities
Maximum longitudinal velocity 550 m/s Impact velocity 600 m/s
Flight Duration 165 seconds
Errors from Advanced IMU Model
-
8/3/2019 Navigatie Cu GPS
100/244
GPS and Vehicle Dynamics Lab
100
Errors from Advanced IMU Model
Scatter Plot at Impact Point
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
1- sigma boundsSimple model
Advanced Model
Error Contribution in the
Advanced IMU Model
-
8/3/2019 Navigatie Cu GPS
101/244
GPS and Vehicle Dynamics Lab
101
Advanced IMU Model
Scatter Plot at Impact Point
0
50
100150
200
250
300
350
400
450
500
GyroMisalingmentaboutX
GyroMisalignmentaboutY
GyroMisalignmentaboutZ
GyroNonorthogonalityaboutX
GyroNonorthogonalityaboutY
GyroNonorthogonalityaboutZ
RollGyroScaleFactor
PitchGyroScaleFactor
YawGyroScaleFactorError
AccelerometerMisalignmentaboutX
AccelerometerMisalignmenta
boutY
AccelerometerMisalignmentaboutZ
AccelerometerNonorthogonalityaboutX
AccelerometerNonorthogonalityaoutY
AccelerometerNonorthogonalityaboutZ
XAccelerometerScaleFactor
YAccelerometerScaleFactor
ZAccelerometerScaleFactor
XAccelerometerAssymmetry
YAccelerometerAssymetry
ZAccelerometerAssymmetry
XAccelerometerNonlinearity
YAccelerometerNonlinearity
ZAccelerometerNonlinearity
AllErrors
ContributionLevel
(meters)
The rocket trajectory was run with each error independently
Errors that contribute the most are errors that see the accelerations and rotation rates
GPS/INS:
The Perfect Complement
-
8/3/2019 Navigatie Cu GPS
102/244
GPS and Vehicle Dynamics Lab
102
The Perfect Complement
Higher output ratesavailable
Drift over long periods
Noise due to vehicledynamics
Biased
Limited to 1-20 Hz Stable over long periods of
time
Stochastic zero mean noise
Unbiased
Noisy
INS (High Frequency Sensor)GPS (Low Frequency Sensor)
The combination provides a high update rate,low noise, unbiased measurement solution
Allan Variance of GPS Velocity
-
8/3/2019 Navigatie Cu GPS
103/244
GPS and Vehicle Dynamics Lab
103
Allan Variance of GPS VelocityDominated by random noise (no drift)
Allan Variance of GPS Position
-
8/3/2019 Navigatie Cu GPS
104/244
GPS and Vehicle Dynamics Lab
104
Allan Variance of GPS Position
GPS Position (=2.5 m) Starfire DGPS Position(=0.2 m)
Error has short term driftNote: 1 position error does not equal 1 from AV
due to drift
WB=0.007 m
WB=1.0 m
Methods of GPS/INS
Integration
-
8/3/2019 Navigatie Cu GPS
105/244
GPS and Vehicle Dynamics Lab
105
IntegrationGPS Aiding the INS Loosely Coupled
Closely Coupled (Tightly Coupled w/out aiding)INS Aiding GPS Tightly Coupled (w/aiding)
Ultra-Tightly Coupled or Deeply Integrated
The methods differ in the type of informationthat is shared between individual units
The methods differ in the type of informationThe methods differ in the type of information
that is shared between individual unitsthat is shared between individual unitsSaurabh Godha, Strategies for GPS/INS Integration, http://www.geomatics.ucalgary.ca/~sgodha/Photos/GPS-INS.ppt
Loosely Coupled Integration
Introduction
-
8/3/2019 Navigatie Cu GPS
106/244
GPS and Vehicle Dynamics Lab
106
Combines computed GPS PVT Solution withIMU measurements
GPS Solution is completely independent fromthe IMU measurements
GPS corrects IMU drift
Utilized a decentralized or cascaded KalmanFilter approach:
Local GPS navigation processing filter
Master INS filter (GPS+IMU)
Loosely-Coupled Block Diagram
-
8/3/2019 Navigatie Cu GPS
107/244
GPS and Vehicle Dynamics Lab
107
Loosely Coupled Block DiagramThe GPS PVT Navigation Solution is combined with theIMU at the navigation level (compensates IMU drift)
Ant.
GPS Receiver
Kalman
Filter
Combined
Nav. Solution
INS
GPS Nav.Solution
Inertial Nav.
Solution
Loosely Coupled Integrationd
-
8/3/2019 Navigatie Cu GPS
108/244
GPS and Vehicle Dynamics Lab
108
Loosely Coupled IntegrationAdvantages 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 accessto raw GPS measurements)
Disadvantages Navigation solution relies on pure IMU measurements if
GPS PVT is not available (number of satellites is lessthan 4)
Correlated systems treated independently Provides sub-optimal solution
-
8/3/2019 Navigatie Cu GPS
109/244
Tightly-Coupled Block Diagram
-
8/3/2019 Navigatie Cu GPS
110/244
GPS and Vehicle Dynamics Lab
110
Tightly Coupled Block DiagramIMU measurements are combined with individual GPSsatellite phase measurements at the Positioning level
GPS Receiver
Ant.
Pseudo-range
& Rate
Range & Rate
Estimate
Tracking Loops
Kalman
Filter
Nav. Solution
INS
Tightly Coupled IntegrationAd t
-
8/3/2019 Navigatie Cu GPS
111/244
GPS and Vehicle Dynamics Lab
111
Tightly Coupled IntegrationAdvantages 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 environmentsDisadvantages More difficult to implement
Larger size of state vector in centralized KF requiresmore computation time
Provides no long-term noise immunity to GPSreception
Deeply Coupled Integration
Introduction
-
8/3/2019 Navigatie Cu GPS
112/244
GPS and Vehicle Dynamics Lab
112
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 andQ signals and the IMU measurements in order toestimate navigation information
Requires access to receiver tracking loops or
raw IFAlso known as Ultra Tight Coupling
Deeply Coupled Integration
Block DiagramIMU bi d i h h GPS i l
-
8/3/2019 Navigatie Cu GPS
113/244
GPS and Vehicle Dynamics Lab
113
IMU measurements are combined with the GPS signalmeasurements (IF) at the tracking level
Ant.
Frequency
Estimate
GPS Receiver
Kalman
Filter
Nav. Solution
INS
RF Frontend &
Correlators
NCO
Correlator
Samples
Deeply Coupled IntegrationAd
-
8/3/2019 Navigatie Cu GPS
114/244
GPS and Vehicle Dynamics Lab
114
p y p gAdvantages
Can provide improved accuracy
Increased jamming resistance Allows faster signal acquisition and reacquisition
Provides improved tracking of GPS signal in the presence ofhigh noise and/or high dynamics
Disadvantages Currently not robust (no method for integrity monitoring of
individual satellites since raw data is fused a in singlenavigation filter)
Extremely cumbersome
Sensitive to IMU noise and bias as well as method ofimplementation
GPS/INS Integration/ d l f d
-
8/3/2019 Navigatie Cu GPS
115/244
GPS and Vehicle Dynamics Lab
115
/ gGPS/INS integration is predominately performedusing 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)
Other methods currently being explored Unscented Kalman Filters
Particle Filtering
Etc.
Linear Kalman Filter
-
8/3/2019 Navigatie Cu GPS
116/244
GPS and Vehicle Dynamics Lab
116
Assumes the following Model Form
vCxy
wBuBAxx wu
+=
++=&
kkk
kdkdk
vCxy
wuBxAx
+=
++=
+
+
1
1
Continuous
Discrete/Sampled
[ ] cTc
T
QwwE
RvvE
=
=
[ ] dTkk
d
T
kk
QwwE
RvvE
=
=
mm
d
m
k
nn
d
n
k
mm
c
m
dd
c
d
RR
Rv
RQ
Rw
RRRv
RQRw
1
1
1
1
d: # of disturbancesn: # of statesm: # of measurements
Process Disturbance 0R
-
8/3/2019 Navigatie Cu GPS
117/244
GPS and Vehicle Dynamics Lab
117
2
2
b
b
biasT
Q a
=
=
10
01
1
0
10
01
0
01
1
0
0
01
2
2
2
12
2
1
b
b
accels
s
Tb
accels
Tsd
T
TT
TTQ
a
bab
T
wcws
T
ws
c
wd BQBTT
Q
Q
[ ]
== 2
2
0
0
ab
wpsd
cvel
RQwE
2accelswpsd TR =
Measure of theSensor Stability
=k
k
t
t
TAT
wcw
A
d deBQBeQ
1
)(
sT
Twcw T
A
BQBA
ec
cc
=
= 0
22
1211
0c
T
d cA 22= 1222ccQT
d =
Byrsons Trick:
For Small Ts:
Linear Kalman Filter Equationst U d tM
-
8/3/2019 Navigatie Cu GPS
118/244
GPS and Vehicle Dynamics Lab
118
q
kdmeas
d
T
dkd
-
1k
kdkd-
1k
-
estdk
k
-
1
d
T
dkd
T
dkk
xCyresiduals
QAPAP
uBxAxUpdateTime
)PCL(IP)(Lxx
)RCP(CCPL
t UpdateMeasuremen
kk
kk
==
+=+=
=+=
+=
++
++
+
+
where ( )( )[ ]Tkkkkk xxxxEP =
Kalman filter recursive equationsAssumed formKalman-Bucy EKF Equations
-
8/3/2019 Navigatie Cu GPS
119/244
GPS and Vehicle Dynamics Lab
119
Kalman filter recursive equationsAssumed formy q
( )
( ) vxhy
wuxfx
+=
+= ,&
Ax
x
x
f=
= &
Cx
h=
)(QwwE T =
)( kT tRvvE =
( )( ) )( kT
tPxxxxE =
[ ]
++
=
kt
kt
duxfk
txk
tx
1
),(),()1
()(
dTw
BQw
Bk
t
kt
TAPPAk
tPk
tP +
+++
= )(
1
)()()()()1
()(
1
)()()()()()()(
+= ktRktCktPktCktCktPktLTT
+=+ )()()()()()(
ktx
ktC
kty
ktL
ktx
ktx
[ ] )()()()( =+k
tPk
tCk
tLIk
tP
Use numerical integration (Euler, Runge-Kutta, etc.)to propagate states and error covariance matrices
GPS/INS Update RatesGPS Updates generally at 1-10 Hz
-
8/3/2019 Navigatie Cu GPS
120/244
GPS and Vehicle Dynamics Lab
120
p g y
IMU Updates at >50 Hz
Therefore KF is technically unobservable betweenGPS 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
kPC
kLI
kP
nkCXmeasyk
Lk
Xk
X
vRTTC
kP
kL C
kCP
)(
)(
1)(
=+=
+=
d
T
kk
kkk
QPP
uxx
+=+=
+
+
1
1
Time Update Measurement Update
1 DOF GPS/INS Example
-
8/3/2019 Navigatie Cu GPS
121/244
GPS and Vehicle Dynamics Lab
121
System Model
velT
x
xTx
wab
x
b
x
bb
+
+
=
11 0
01
0
1
0
10 &
&
&&
[ ]
==
2
2
0
0
ab
wpsd
cvel
RQwE
2
accelswpsd TR =
GPS
x
GPS vb
xCV +
=
&22
GPSdGPS RvE ==
( ) [ ][ ] ( )
=
=
2
2
2221
1211
xx
x
bExbE
bxExE
PP
PPP
&
&&
[ ]01=C
[ ]00=C
= 0
0kL
= #
#kL
If GPS is available:
If GPS is not available:
1 DOF Yaw ExampleSystem Model (assumes )
-
8/3/2019 Navigatie Cu GPS
122/244
GPS and Vehicle Dynamics Lab
122
System Model (assumes )
hdT
r
r
vel
Tr
vel wgbb
bb
+
+
=
11 0
0101
010
&
&
[ ]
== 2
2
2
00
gb
gyroschd TQwE
[ ]
v
gbias
vel
GPSGPS +
== 01 [ ]
V
RvE GPSv
22
==
velGPS =Turn off KF during turning/periods of changing sideslip:
(compares GPS velocity, course, with integrated gyro)
== vel
To Estimate Sideslip ():
Longitudinal DynamicsSystem Model
-
8/3/2019 Navigatie Cu GPS
123/244
GPS and Vehicle Dynamics Lab
123
System Model
Cannot distinguish pitch and longitudinal accelerometer bias
( ) ( ) long
T
q
x
q
xp
g
x
T
cc
q
xp
g
x
wg
a
b
b
VGG
b
b
V
bb
+
+
+
=
+ 11 00
010
000001
00
10
0001
000
1000
000000
&
&&
&&
[ ]
==
2
2
2
2
00
00
00
gb
gyros
accels
clat T
T
QwE
( ) GPS
q
xp
g
x
GPS
GPSv
b
b
V
V+
+
=
0010
0001
[ ]
==
V
RvEupGPS
GPS
vGPS2
)(
2
2
0
0
=
GPS
up
GPSGPS
V
V1tan
Lateral DynamicsSystem Model
-
8/3/2019 Navigatie Cu GPS
124/244
GPS and Vehicle Dynamics Lab
124
y Cannot distinguish pitch and longitudinal accelerometer bias
Must account for centripetal acceleration
( ) ( )( )( )
lat
T
T
p
xrry
p
y
y
T
C
p
y
y
wg
Vbga
b
b
VG
b
b
V
b
b
b
+
+
+
=
+
1
1
1 00
00
001
00
10
01
00
100
00
&
&&
&
[ ]( )
+==
2
2
222
2
00
00
00
gb
gyros
gyroaccels
clat T
VT
QwE
[ ] ( ) GPSp
y
y
GPS v
b
b
V
V +
+= 001
22
GPSvGPS RvE ==
velGPS =
Complimentary FiltersCan use complimentary filters to
-
8/3/2019 Navigatie Cu GPS
125/244
GPS and Vehicle Dynamics Lab
125
Can use complimentary filters to
separate low frequency accelerometerbias from higher frequency vehicledynamics
KF Corrects IMU Errors
( )( )ycy
xpgcx
qq
pp
rr
bGay
bGax
bgq
bgp
bgr
+=++=
==
=
&&
&&
( )
( )yb
y
y
b
b
bsT
b
bsT
sT
++=
++
=
11
1
( )
( )xb
x
x
b
b
bsT
b
bsT
sT
++=
++
=
11
1
GPS/INS KF Closed-Loop
Eigenvalues and Bandwidth
-
8/3/2019 Navigatie Cu GPS
126/244
GPS and Vehicle Dynamics Lab
126
GPS/INS Velocity AccuracyDetermined using a covariance analysis
-
8/3/2019 Navigatie Cu GPS
127/244
GPS and Vehicle Dynamics Lab
127
Determined using a covariance analysisbased on sensor noise statistics
GPS/INS Velocity Based Heading
Accuracy Assumes ( or ) && = 0=& 0=yV&
-
8/3/2019 Navigatie Cu GPS
128/244
GPS and Vehicle Dynamics Lab
128
( ) 0yV
Determined
using a
Covariance
analysis basedon sensor noise
statistics
GPS Velocity Based Heading
Accuracy
-
8/3/2019 Navigatie Cu GPS
129/244
GPS and Vehicle Dynamics Lab
129
Determined using a covariance analysisbased on sensor noise statistics
Multi-Antenna GPS/INS Attitude
Accuracy
-
8/3/2019 Navigatie Cu GPS
130/244
GPS and Vehicle Dynamics Lab
130
Determined using a covariance analysisbased on sensor noise statistics
Multi-Antenna GPS/INS Attitude
Accuracy (with Short Baseline)
-
8/3/2019 Navigatie Cu GPS
131/244
GPS and Vehicle Dynamics Lab131
Determined using a covariance analysisbased on sensor noise statistics
GPS/INS Estimation of Vehicle
RollRoll gyro reduces thelatency in the roll estimate
Lateral accelerometer
bi bl ll
-
8/3/2019 Navigatie Cu GPS
132/244
GPS and Vehicle Dynamics Lab132
latency in the roll estimate
0 10 20 30 40 50 60 701
0
1
2
3
(deg)
ActualEsimtated (No Roll Gyro)
0 10 20 30 40 50 60 701
0
1
2
3
Time (s)
(deg)
ActualEsimtated (w/ Roll Gyro)
bias resembles roll
Recall: Lateral accelerometer bias is not distinguishable from roll
Experimental Results of the Lateral
Estimator on a Test VehicleLane ChangeM
Driving Around aB k d T
-
8/3/2019 Navigatie Cu GPS
133/244
GPS and Vehicle Dynamics Lab133
0 10 20 30 401
0
1
2
est
(v
GPS
)(deg)
ModelEstimated
0 10 20 30 40
0.2
0
0.2
0.4
Vy
(m/s)
0 10 20 30 40
5
0
5
Time (s)
+b
x(deg)
0 10 20 30 400.4
0.2
0
0.2
Time (s)
bp
andb
r(deg/s)
0 10 20 301.5
1
0.5
0
0.5
est
(v
GPS
)(deg)
0 10 20 30
0.4
0.2
0
0.2
0.4
Vy
(m/s)
ModelEstimated
0 10 20 301
0
1
2
Time (s)
+b
x(deg)
0 10 20 300.06
0.04
0.02
0
0.02
0.04
Time (s)
bp
andb
r(deg/s)
Maneuvers Banked Turn
Experimental Estimator Results
on a Test VehicleResults from the Results from the
-
8/3/2019 Navigatie Cu GPS
134/244
GPS and Vehicle Dynamics Lab134
0 500 10000.2
0.1
0
0.1
0.2
GPSVelocityMeasurement(m/s)
=0.03 m/s
0 500 10000.2
0.1
0
0.1
0.2=0.009 m/s
VelocityEstimate(m/s)
0 500 10000.4
0.6
0.8
1
1.2
1.4=0.05 m/s
2
Time (s)AccelerometerMeasurement(m/s
2)
0 500 10000.4
0.6
0.8
1
1.2
1.4
=0.0022 m/s2
Time (s)
BiasEstimate(m/s
2)
0 10 20 30 40 500.4
0.2
0
0.2
0.4
= 0.1 deg
GPS
(deg)
0 10 20 30 40 500
0.02
0.04
0.06
0.08
sqrt(P22
)
sqrt(P11)
P0.5
0 10 20 30 40 500.4
0.2
0
0.2
0.4
= 0.04 deg
est
(deg)
Time (s)0 10 20 30 40 50
0.1
0.05
0
Time (s)
G
yroBiasEstimate(deg/s)
Longitudinal Estimator Lateral Estimator
User with High Dynamics requires
the Fusion of GPS & IMUGPS gives:
-
8/3/2019 Navigatie Cu GPS
135/244
GPS and Vehicle Dynamics Lab135
User position in global coordinates (loosely coupled)
Pseudorange measurements, satellite positions, &time all in global coordinates (tightly coupled)
IMU gives inertial measurements in body frame
Acceleration Rotation Rate
Requires additional modeling
1
23
Mode of GPS Measurement Gives
Name to the FusionLoosely Coupled Measurement Model Pos measurement
x
-
8/3/2019 Navigatie Cu GPS
136/244
GPS and Vehicle Dynamics Lab136
Pos measurement
IMU states
Tightly Coupled Measurement Model
For m satellites in view (w/ valid pseudorange observations)
State vector x includes states for the additional IMU dynamics
k
k
suks x
x
xxH*
),(
= mi ,...2,1=
+
=
.
.
.000100
000010
000001
z
y
z
y
x
Summary of MethodsLoosely Coupled
-
8/3/2019 Navigatie Cu GPS
137/244
GPS and Vehicle Dynamics Lab137
Tightly Coupled
IMU
GPS Least-Squares
Kalman Filter
, rsats
User Statesa,
User r, v
IMU
GPS
Kalman Filter
, rsats
User States
a,
Full Model Prepared for Planar TC
Kalman Filter EstimationLinearized (about current estimates) dynamic model
-
8/3/2019 Navigatie Cu GPS
138/244
GPS and Vehicle Dynamics Lab138
Inputs are IMU measurements (derivatives of states)
Sensor biases modelled as 1st-order Markov process
+
+
=
b
ba
a
aa
kk
kk
a
r
a
b
b
V
E
NV
V
b
b
V
E
N
dt
d
1000
0100
0010
0001
0000
0000
00
00
10
01
00
00
100000
01
0000
100000
010000
00)cos()sin(00
00)sin()cos(00
11
11
TC KF Measurement Model: Two
Dimensional CaseMeasurement model (pseudoranges) linearized w/current estimate
-
8/3/2019 Navigatie Cu GPS
139/244
GPS and Vehicle Dynamics Lab139
current estimate
Size of measurement model depends on satellites in view
+
=
mgps
a
km
msku
km
msku
k
sku
k
sku
k
sku
k
sku
mbb
b
V
E
N
yyxx
yyxx
yyxx
.
.
.
.
10000
10000),(),(
10000..
10000..
10000..
10000
),(),(
10000),(),(
.
.
.
.
2
1
1,
,1,
1,
,1,
1,2
2,1,
1,2
2,1,
1,1
1,1,
1,1
1,1,
2
1
Noise Statistics Known from
SimulationDisturbance Covariance approx. by
2tdc =
22
-
8/3/2019 Navigatie Cu GPS
140/244
GPS and Vehicle Dynamics Lab140
Discrete found using trick method Qd discrete state disturbance covariance
Noise covariance derived directly from
GPS output statistics
=
2
2
2
2
000
000000
000
b
ba
a
Qc
=2
2
0
0
E
NdR
dQ
Motivation for GPS Guided
Tractors1999 tractor salesN th A i 108K
-
8/3/2019 Navigatie Cu GPS
141/244
GPS and Vehicle Dynamics Lab141
North America - 108K
World - 590K
good satellite visibility on farms
relieve drivers from tedious & monotonouslabor
provide farm operation during poor visibility
open doors for new agricultural techniques
Automated Steering
-
8/3/2019 Navigatie Cu GPS
142/244
GPS and Vehicle Dynamics Lab142
Cm-level control for row cropsReduced overlap for tillage
Cooperating Vehicles
GPS Guided Farm Tractor4 - antenna carrierphase DGPS
-
8/3/2019 Navigatie Cu GPS
143/244
GPS and Vehicle Dynamics Lab143
phase DGPS
3-D position(1 = 2 cm)
3 axis attitude(1 = 0.1)
5 Hz update rate
steer anglepotentiometer
-
8/3/2019 Navigatie Cu GPS
144/244
GPS and Vehicle Dynamics Lab144
Roll and Lever Arm Correction~ 3 meter lever arm0.4 roll accuracy
Rolls Off at 0.12 Hz
Higher Frequency Resonant
-
8/3/2019 Navigatie Cu GPS
145/244
GPS and Vehicle Dynamics Lab145
yrequired to utilize 2cm DGPS positionaccuracy
-120
-100
-80
-60
-40
-20
0
0.0 0.1 1.0 10.0
Frequency (Hz)
Amplitude(dB)
Higher Frequency Resonant
Peak @ ~ 1HzCan filter INS to measureRoll
Various Yaw Dynamic Models
Bicycle ModelCcCa
-
8/3/2019 Navigatie Cu GPS
146/244
GPS and Vehicle Dynamics Lab146
+
++
+
=2
2
1
2
120202
1
)(
)(
mV
cmVcccs
mV
mcIcsI
mV
CcCasCa
s
sR
ZZ
ff
f
DC Gain: 2xUS
x
ss
VKL
VR
+
=
Neutral Steer Model (Kus=0)
V
csI
Ca
s
sR
Z
f
2)(
)(
+=
Kinematic Model
LVR x=
Box-Jenkins Model Fit
0
10
itude
-
8/3/2019 Navigatie Cu GPS
147/244
GPS and Vehicle Dynamics Lab147
100
101
102
20
10Magni
ETFEBJ(2,2,2,2,1) Fit
100
101
102
150
100
50
0
Frequency (rad/sec)
Ph
ase(deg)
ETFEBJ(2,2,2,2,1) Fit
Vx= 4 m/s )()(
)(
)()(
)(
)( teqD
qC
tqA
qB
tR +=
Line Tracking
0.9
-
8/3/2019 Navigatie Cu GPS
148/244
GPS and Vehicle Dynamics Lab148
-0.3
0.0
0.3
0.6
0 50 100 150 200
Time (sec)
LaterError(m)
1 FootMean = 5 mm 1=3 cm
Advanced Trajectories
-
8/3/2019 Navigatie Cu GPS
149/244
GPS and Vehicle Dynamics Lab149
High Speed Control
Accurate control at full range of tractorspeeds
-
8/3/2019 Navigatie Cu GPS
150/244
GPS and Vehicle Dynamics Lab150
speeds
0 2 4 6 8 100.2
0
0.2
0.4
Mean=2 cm 1=2 cm
LateralError(m)
0 2 4 6 8 100.4
0.2
0
0.2
0.41=0.08
ControlInput
Time (sec)
0 5 10 15 200.2
0
0.2
0.4
LateralError(m)
Mean=3.5 cm 1=4.0 cm
0 5 10 15 200.4
0.2
0
0.2
0.4
ControlInput
1=0.10
Time (sec)
Vx=5 m/sVx=8 m/s
[ ]TrgVNEestX bbbbX &&&&=ititt tE
Full State Estimation:12 Tractor States
-
8/3/2019 Navigatie Cu GPS
151/244
GPS and Vehicle Dynamics Lab151
biasradar
biasgyrobiasanglesteer
rateslewsteering
anglesteer
)angle"crab"orbias(headinganglesliponacceleratiyaw
rateyaw
heading
velocityforwardpositionnorthtractor
positioneasttractor
===
==
==
===
=
=
br
bg
b
b
xVN
E
&
&&
&
Cascaded (KF) Estimation
GPSEstimates of
Position &Velocity
-
8/3/2019 Navigatie Cu GPS
152/244
GPS and Vehicle Dynamics Lab152
INS
Radar Gyro
Tractor Model
LQR Control
Bias
Estimate
Esimates ofTractor States
u
Control States Filter
ead Reckoning Filter
+
-
Natural Separation of the Estimators
Separate Estimators
d )i ()(
Tractor EquationsDead Reckoning Equations
-
8/3/2019 Navigatie Cu GPS
153/244
GPS and Vehicle Dynamics Lab153
u
vI
vK
vI
vd
nRKnn
+=
+=
&&&
&&&&&&
222
bggyro
brradarnb
rradare
=
=
=
&
&
&
)cos()
(
)sin()(
Inputs: Radar, Gyro
[ ]Tbb
gb
rneX =1 [ ]T
bXVX &&&&=2
Input: u = Steering Slew Rate
Demonstration of Separate Bias
Estimators
5
10
15
ec
Gyro Gyro Bias
-
8/3/2019 Navigatie Cu GPS
154/244
GPS and Vehicle Dynamics Lab154
-25
-20
-15
-10
-5
0
0 20 40 60
Time (sec)
Deg,Deg
/Se
Steer Angle Steer Angle Bias
VX=2 m/sLine Tracking
Able to accurately estimate both the gyro and steer angle bias independently
Lateral Errors When DeadReckoning (No GPS)
1.0
1.5
m)
13 Tests
VX=2 m/s
-
8/3/2019 Navigatie Cu GPS
155/244
GPS and Vehicle Dynamics Lab155
-1.5
-1.0
-0.5
0.00.5
0 20 40 60
Time After GPS is Off (Sec)
LateralError(m
0.3 m
X
Line Tracking
Results
Errors < 0.3mfor 40 sec
Errors < y
tTVVy SXEXE && ==2
3
*)(32 tTVt SXy &=
Error Analysis:
Dead Reckoning Positioning
Performance (No GPS)
0 2
0.3
0.4
m)
0-8 sec
8-42 sec0.3 m
Test
VX=2 m/s Line Tracking
-
8/3/2019 Navigatie Cu GPS
156/244
GPS and Vehicle Dynamics Lab156
-0.4
-0.3
-0.2
-0.1
0.0
0.1
0.2
-0.4 -0.2 0.0 0.2 0.4
Lateral Error (m)
LongitudinalError(m
9 cm = 1/2
Line Tracking
Results
8 sec < 9 cm error 42 sec < 30 cm
error=> % Error
)(ErrorsLong.
)(ErrorsLateral2
E
E
ff
== Lateral Errors >
Longitudinal Errors
Effect of Crab AngleThree Major Errors
Gyro Heading
Crab Angle
)(tan)(tan1
111
==kk
kk
north
eastV
nn
ee
V
VX
-
8/3/2019 Navigatie Cu GPS
157/244
GPS and Vehicle Dynamics Lab157
Crab Angle
Velocity Integration
-2.0
-1.5
-1.0
-0.50.0
0.5
0 20 40 60 80
Time after GPS is Off (sec)
LateralError(m
)
Gyro Heading GPS Heading
Vx Heading
1kknorth nnV
GPS vs. GYRO < 0.3 m
Crab Angle < 1 @ t=20 sec
Total Dead Reckoning Control
Performance
-810
-805
-800
-795
)1 L Wi h GPS
Lap 1
-
8/3/2019 Navigatie Cu GPS
158/244
GPS and Vehicle Dynamics Lab158
-835
-830
-825
-820
-815
810
-50 -30 -10 10 30 50
East (m)
North
(m)
1st Lap With GPS
3 Laps DR (NO GPS)
Lap 4
Errors Mean 1 Max
Position 0.2 m 0.23 m < 1 mHeading -0.05 0.87 < 2
4 Minutes of DeadReckoning
Implement Control
-
8/3/2019 Navigatie Cu GPS
159/244
GPS and Vehicle Dynamics Lab159
I
I
b
V
ba
s
K
ssR
sX
+=
++= 1
)(
)( &&& )( LaLVy XI +++=
4 Added States
GPS Measurement
[ ]TX bne =
Implement Control
Implement: 7.9 m (26 foot) Wide Chisel Plow
Implement Position: Carrier Phase DGPS 3-D
-
8/3/2019 Navigatie Cu GPS
160/244
GPS and Vehicle Dynamics Lab160
Position (1 = 2 cm)
GPS Antenna (L=6.5 m)
Experimental Control ofImplement
Experiments performed at 4.5 mph
Notice difference in position of tractor
-
8/3/2019 Navigatie Cu GPS
161/244
GPS and Vehicle Dynamics Lab161
Notice difference in position of tractorand implement
10 0 10 20 30 40 50 602
1
0
1
2
3
4
5
East (m)
North(m)
ImplementTractor
20 15 10 5 0 5 10 15 2030
35
40
45
50
55
60
65
70
ARC: = 2.5 cm 1 = 3.5 cm
East (m)
North(m)
ImplementTractor
-
8/3/2019 Navigatie Cu GPS
162/244
Empirical Models
Used to determine how a tractorreally behaves with changes inimplement
-
8/3/2019 Navigatie Cu GPS
163/244
GPS and Vehicle Dynamics Lab163
Determine appropriate analyticalmodel (and parameter variations)using experimental data
On-line Estimation of HitchParameter
Estimate parameter using GPS/INS and Steer Angle measurements whenenough excitation exists
-
8/3/2019 Navigatie Cu GPS
164/244
GPS and Vehicle Dynamics Lab164
DARPA Grand Challenge(Overview)
Autonomous ground vehicle race inFebruary 2004 and October 2005
130+ miles across desert terrain
No human intervention
SciAutonics
-
8/3/2019 Navigatie Cu GPS
165/244
GPS and Vehicle Dynamics Lab165
Avoid obstacles in path whileremaining in corridor
Auburn University partnered withSciAutonics and Team Terramax
2004 race did not prove successful
Furthest competitor reached 7 out of142 miles
2005 race demonstrated somecapability of autonomous vehicles Five teams finished race
Terramax
DARPA Grand ChallengeNavigation System Development
Critical navigation states:
Velocity
Other vehicle information:
Roll angle
-
8/3/2019 Navigatie Cu GPS
166/244
GPS and Vehicle Dynamics Lab166
Direction of travel Position
Used by vehicle controller todrive the vehicle
Pitch angle Road grade
Used by obstacle detection systemto properly orient obstacles
Effect of Model Error on UGVControl
Controller feeds backall available 2 Incorrect Parameters
Aggressive AutonomousLane Change (V=20 mph)
-
8/3/2019 Navigatie Cu GPS
167/244
GPS and Vehicle Dynamics Lab167
measurements
The incorrect modelresults in instability
A better model isneeded before theother measurementscan be utilized
-150 -100 -50 0-10
-8
-6
-4
-2
0
East (m)
Nor
th(m)
Correct Parameters
Desired Path
DARPA Grand ChallengeNavigation Sensor Suite
Differential GPS was cornerstone of vehicle navigation
Navcom Starfire (
-
8/3/2019 Navigatie Cu GPS
168/244
GPS and Vehicle Dynamics Lab168
g
Rockwell Collins GIC-100 (
-
8/3/2019 Navigatie Cu GPS
169/244
GPS and Vehicle Dynamics Lab169
-Multi-antenna GPS -IMU-Wheel speed -Lidar
-Doppler radar
State estimates provided to controller:
-Velocity -Course-Position -Pitch
-Roll -Measurementbiases
Other sensors can be easily added
-Range radar -Camera
-Ultrasonic
Navigation Errors fromLongitudinal Slip
Longitudinal wheel slip providesnavigation algorithm withincorrect measurement
-
8/3/2019 Navigatie Cu GPS
170/244
GPS and Vehicle Dynamics Lab170
Corrupts estimates of velocity,accelerometer bias, andposition
Can estimate wheel slip as abias, but only when GPS isavailable
Doppler radar is an alternative
speed measurement Bias can change with terrain
DARPA Grand ChallengeSensor Capabilities and Limitations
Sensors can be modeled with a turn on bias, a Markov bias, and white noise
GPS offers precise information, but only at low update rates of 5 Hz
wbcm +++= ]1,0[~,21
2
t
bb
+=&
-
8/3/2019 Navigatie Cu GPS
171/244
GPS and Vehicle Dynamics Lab171
IMU provides 50 Hz measurements, but suffers from bias drift
Magnetometers provide roll, pitch and yaw measurements, but containedquickly 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 whilecompensating for their inadequacies
DARPA Grand ChallengeNavigation Model
Inputs were from IMU
Biases were not modeled as a function of time,but the noise driving the drift was modeled inthe process covariance matrix
0
br
gga
N
b
V
r
gx
r
&
&
&
&
[ 22222222
-
8/3/2019 Navigatie Cu GPS
172/244
GPS and Vehicle Dynamics Lab172
Velocity state accounts for vehicle pitch andlongitudinal road grade
Vehicle pitch estimate contains longitudinalaccelerometer bias
Roll estimate is vehicle roll and lateral roadgrade, and contains lateral accelerometer bias
wbb +=
1&
=
=
0
0
00
0
0
0
0
0
)sin(
)cos(
2
2
2
1
1
1
b
b
V
V
b
b
b
b
b
b
b
b
E
N
x IMU
IMU
M
M
M
M
M
M
g
&
&
&
&
&
&
&
&
&
&
&
&
&
&
&
[]22222221212122b
22
b
22
E
2
N
2
br
2
r
2
...
bMbMbMbMbMbMg
axd diagQ =
[
]TMAGMAGgMAGMAGMMAGMAGGPSGPSGPSWSGPS ENVVy
2121
21
...
=
DARPA Grand ChallengeNavigation System Performance
The system successfullytracked GPS measurementswhen they were available
-
8/3/2019 Navigatie Cu GPS
173/244
GPS and Vehicle Dynamics Lab173
Bridged brief erroneous GPSmeasurements
DARPA Grand ChallengeNavigation System Initialization
Initialization of the Kalman filter iscritical to its performance
Settle time when GPS is acquired ~ 3seconds
Algorithmic logic in the loop leads to
-
8/3/2019 Navigatie Cu GPS
174/244
GPS and Vehicle Dynamics Lab174
Algorithmic logic in the loop leads tobetter navigation solutions
Some commercial systems take >30minutes to initialize
Magnetometers aided initialization, butwere statistically weighted out of thefilter when the vehicle was moving
Fast bias drift makes measurements
unreliable Passing metallic objects also degrade
quality of measurement
70 80 90 100 110 12020
40
60
80
100
120
140
160
Time (s)
Yaw(deg)
TCM2
Microstrain
GPS
DARPA Grand ChallengeNational Qualification Event
Failed to finish first run becauseof GPS receiver malfunction
Successfully completed nextthree runs
-
8/3/2019 Navigatie Cu GPS
175/244
GPS and Vehicle Dynamics Lab175
One of ten cars granted earlyentry into DARPA GrandChallenge
DARPA Grand ChallengeGrand Challenge
Completed 16miles beforeUSB hub failed
-
8/3/2019 Navigatie Cu GPS
176/244
GPS and Vehicle Dynamics Lab176
USB hub failedand crashed acomputer
Vehicleperformed verywell while on
course
DARPA Grand ChallengeNavigation Error Sources
The absence of GPS measurements makes somebiases unobservable Bias estimates remain constant when no measurements
exist to update them
-
8/3/2019 Navigatie Cu GPS
177/244
GPS and Vehicle Dynamics Lab177
exist to update them Integration of constant offset linearly degrades heading
estimate
Tactical grade IMU with low bias drift allowed for relativelylong dead reckoning periods when the bias was correctly
estimated before the outage
Calibration error in wheel speed sensor created anoffset in the velocity estimate Wheel slip would also introduce estimate error
If the wheel speed bias was estimated, the calibration errorand wheel slip would corrupt it as well
DARPA Grand ChallengeNavigation 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
-
8/3/2019 Navigatie Cu GPS
178/244
GPS and Vehicle Dynamics Lab178
~1m error after 25 secondsError Caused by neglected sideslip generatedduring cornering
380 390 400 410 420 430 440 450
-0.2
0
0.2
0.4
0.6
0.8
1
Error(m)
Time (s)
40 50 60 70 80 90 100 11
-40
-35
-30
-25
-20
-15
-10
East (m)
North(m)
GPS EKF
DARPA Grand ChallengeNavigation Error Sources
The effect of sideslip (generation of lateral velocity) during turningcan be seen in the heading estimate
Sideslip is the difference between the direction the vehicle is pointingand the direction the vehicle is traveling
A single GPS antenna measures the direction of travel
-
8/3/2019 Navigatie Cu GPS
179/244
GPS and Vehicle Dynamics Lab179
VF
F
VR
R
Vx
Vy
Vr
N
E
85 90 95 100 105 110 115
50
100
150
Head
ing(deg)
GPS
KF
85 90 95 100 105 110 115-20
-10
0
10
20
Error(deg
)
Time
A single GPS antenna measures the direction of travel An integrated yaw rate gyro yields the direction the vehicle is pointing
GPS course is denoted by Heaind is deno
top related