Page 1

2-2-2. Localization

Page 2

Output DataComputation

Sensor Data Processing

Velocity Computation

DGPS Data

Processing

Latitude, Longitude

Kalman Filter I

Heading Angle

Conversion

Heading AnglePrediction

Gyroscope Data

Processing

Heading AngleUpdate

Yaw RateHeading Angle

Heading Angle

Heading Angle

Coordinate Conversion(GRS-TM) (X, Y)

Kalman Filter II

PositionConversion

PositionUpdate

PositionPrediction

Velocity (X, Y)(X, Y)Wheel Speed Data

Processing

Flow:

Input Processing • Compensation the

Yaw rate offset • Analyze NMEA proto-

col• Revise OBD II velocity

Prediction• Predict heading angle using in-

tegration the Yaw rate (100Hz)• Predict the position using inte-

gration Velocity (50Hz)

Update• Update GPS heading

angle (10Hz) • Update GPS position

(10Hz)

Output Computation• Transformation Path

planning/tracking coordi-

nate

2-2-2. Localization

Page 3

0 0.30.60.91.21.51.82.12.42.7 3 3.33.63.94.24.54.8175

176

177

178

179

180

181

He

ad

ing

An

gle

(d

eg

ree

)

Time (s)

DGPS B20• 1.5m RMS horizontal error• 10 Hz output rate

< Heading Angle error in low velocity section>

GPSTrue

< GPS error with path curvature >

2-2-2. Localization

Page 4

Ave

rage

hea

ding

er-

ror

(deg

ree)

Yaw rate (degree/s)

Dynamic Adjustment of Noise Covariance

Apply a vehicle velocity and yaw rate to noise covariance

– • : Kalman gain • : error covariance at time k• : observation model between the state and the observation• R: measurement noise covariance matrix

– ) + ) +

• , : coefficient• : velocity of vehicle• : yaw rate of vehicle• : threshold of velocity• : threshold of yaw rate

Ave

rage

hea

ding

er-

ror

(deg

ree)

Velocity from wheel speed sensor (m/s)

2-2-2. Localization