navigatie cu gps

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)


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)


3/244


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


4/244


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


5/244


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


6/244


Coordinate Nomenclature

V = velocityr = yaw rate

= heading (or yaw)

= vehicle course = steer angle = body sideslip angle = tire sideslip angle


7/244


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/244


How GPS Works

measure thetransit time for asignal from SV touser multiply by c to

get range

triangulateranges to getposition (andtime)


9/244


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


10/244


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.


11/244


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


12/244


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


13/244


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


14/244


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.


15/244


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)


16/244


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


17/244


GPS Signal Structure

3 Components Carrier Wave

L1, L2

Code Signal C/A, P(Y)

Navigation Data

Satellite Information


18/244


19/244


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


20/244


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


21/244


GPS Signal Structure( ) ( ) +++= ttDtXPPttDtXGPtS iipiicLli 11 sin)()(2cos)()(2)(

( ) += ttDtXGPtS iicLli 1cos)()(2)(

Signal =C/A x Data xCarrier


22/244


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


23/244


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 +++++=


24/244


GPS Receiver

RF down conversion to IF


25/244


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.


26/244


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


27/244


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


28/244


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


29/244


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


30/244


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 +++=


31/244


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)


32/244


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


33/244


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


34/244


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

%


35/244


36/244


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


37/244


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


38/244


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


39/244


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


40/244


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


41/244


GPS Errors

Receiver 1&2:RTDReceiver 3: Saphire

Receiver 4: Starfire


42/244


GPS Errors

Common Mode Errors can easily beseen by two GPS receivers

GPS Errors (Effect of Ground


43/244


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


44/244


Effect of Velocity/Acceleration

on a Cheap GPS ReceiverDriving aroundtest track atdifferent speeds

GPS Error=f(V)

Possibly due tolack of carrier PLL


45/244


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


46/244


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


47/244


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


48/244


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)


49/244


NDGPS Coverage


50/244


Starfire and Omnistar

Omnistar Starfire

http://www.oznet.ksu.edu/pr_prcag/StaticDF04.htm


51/244


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


52/244


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


53/244


DGPS Position vs time


54/244


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)


55/244


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


56/244


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


58/244


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


59/244


pp

NavigationDevelop relative navigation scheme to improve ground

vehicle convoys


60/244


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


61/244


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


62/244


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


63/244


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


64/244


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


65/244


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.


66/244


GPS Attitude

3 antennas

roll

pitchyaw

(Common Clock)

No Reference Station Required

Accuracy Depends on Antenna Spacing (Not Velocity)

GPS Wave Front


67/244


GPS Attitude AccuracyAccuracy ~ 0.3/L degrees

(based on 3-4 mm carrier noise)


68/244


Uses of GPS

?

Measuring Plate Movement


69/244


Using GPS


70/244


Traffic MonitoringMean traffic position lies along the

centerline of the lane

Measurementsusing a Starfire

DGPS Receiver


71/244


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


72/244


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


73/244


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


74/244


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


75/244


Modeling IMU NavigationPerformance for GPS

Coupling Algorithms


76/244


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


77/244


Units)

Usually Consist of 3 accelerometers and3 rate gyroscopes (MEMS, FOG, or RLG)

Analog Devices MEMSAccelerometer andGyroscope

Sentera IMU with ADMEMS sensorsLN200 IMU


78/244


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


79/244


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


80/244


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


81/244


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


82/244


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


83/244


(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


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


85/244


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


86/244


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 )


87/244


(by parameter specification)

0.35/hrBias Variation

100secBias Time Constant

.0017/sec/HzRandom walk(Expensive)

Tactical

180/hrBias Variation


.05/sec/HzRandom walk(Cheap)

Automotive

360/hrBias Variation


.05/sec/HzRandom walk(Cheapest)

Consumer

SpecUnitsAttributeRate Gyro

3104.2

3102.1

51050gBias Variation


.0005g/HzRandom Walk(Expensive)

Tactical

gBias Variation


.001g/HzRandom Walk(Cheap)

Automotive

gBias Variation


.003g/HzRandom Walk(Cheapest)

Consumer

SpecificationUnitsAttributeAccelerometer

KVH-5000 Fog Tactical Category

Humphrey Accelerometer Consumer Category

H di I i E B d


88/244


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


89/244


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


90/244


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


91/244


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


92/244


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 )


93/244


(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)


94/244


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)


95/244


(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


96/244


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


97/244


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


98/244



Trajectory Body Accelerations

Simulated Rocket Trajectory

Rocket elevated to 55 degrees

Impact point 86 km downrange



99/244



Platform Heading NEU Velocities

Maximum longitudinal velocity 550 m/s Impact velocity 600 m/s

Flight Duration 165 seconds

Errors from Advanced IMU Model


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


101/244


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


102/244


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


103/244


103

Allan Variance of GPS VelocityDominated by random noise (no drift)

Allan Variance of GPS Position


104/244


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


105/244


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


106/244


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


107/244


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


108/244


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


109/244

Tightly-Coupled Block Diagram


110/244


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


111/244


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


112/244


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


113/244


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


114/244


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


115/244


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


116/244


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


117/244


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


118/244


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


119/244


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


120/244


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


121/244


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 )


122/244


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


123/244


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


124/244


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


125/244


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


126/244


126

GPS/INS Velocity AccuracyDetermined using a covariance analysis


127/244


127

Determined using a covariance analysisbased on sensor noise statistics

GPS/INS Velocity Based Heading

Accuracy Assumes ( or ) && = 0=& 0=yV&


128/244


128

( ) 0yV

Determined

using a

Covariance

analysis basedon sensor noise

statistics

GPS Velocity Based Heading

Accuracy


129/244


129


Multi-Antenna GPS/INS Attitude

Accuracy


130/244


130


Multi-Antenna GPS/INS Attitude

Accuracy (with Short Baseline)


131/244



GPS/INS Estimation of Vehicle

RollRoll gyro reduces thelatency in the roll estimate

Lateral accelerometer

bi bl ll


132/244


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


133/244


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


134/244


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:


135/244


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


136/244


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


137/244


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


138/244


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


139/244


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


140/244


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


141/244


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


142/244


Cm-level control for row cropsReduced overlap for tillage

Cooperating Vehicles

GPS Guided Farm Tractor4 - antenna carrierphase DGPS


143/244


phase DGPS

3-D position(1 = 2 cm)

3 axis attitude(1 = 0.1)

5 Hz update rate

steer anglepotentiometer


144/244


Roll and Lever Arm Correction~ 3 meter lever arm0.4 roll accuracy

Rolls Off at 0.12 Hz

Higher Frequency Resonant


145/244


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


146/244


+

++

+

=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


147/244


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


148/244


-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


149/244


High Speed Control

Accurate control at full range of tractorspeeds


150/244


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


151/244


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


152/244


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


153/244


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


154/244


-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


155/244


-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


156/244


-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


157/244


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


158/244


-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


159/244


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


160/244


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


161/244


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


162/244

Empirical Models

Used to determine how a tractorreally behaves with changes inimplement


163/244


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


164/244


DARPA Grand Challenge(Overview)

Autonomous ground vehicle race inFebruary 2004 and October 2005

130+ miles across desert terrain

No human intervention

SciAutonics


165/244


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


166/244


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)


167/244


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 (


168/244


g

Rockwell Collins GIC-100 (


169/244


-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


170/244


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

+=&


171/244


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


172/244


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


173/244


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


174/244


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


175/244


One of ten cars granted earlyentry into DARPA GrandChallenge

DARPA Grand ChallengeGrand Challenge

Completed 16miles beforeUSB hub failed


176/244


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


177/244


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


178/244


~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


179/244


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

navigatie cu gps

Documents