methee srisupundit final defense. intelligent vehicle localization observer (estimator) kalman...

Methee SrisupunditFinal Defense

Intelligent Vehicle Localization Observer (Estimator)

• Kalman Filter• Particle Filter

Methodology• Control Input Recognition• Mathematic Model Identification• Testbed• Algorithm Structure & Detail

Experiment & Result• Testbed Result• Control Result

Unman Transport Vehicle•Automated Navigation•Traffic Obedience•Accident Avoidance

We have a

DREAM!

we called…

What is Localization?• ““an ability to identify the location of itself in a coordinate an ability to identify the location of itself in a coordinate

frame”frame”

How to do it?• Considered Coordinate Frame

• Sensory ToolGPS

Electronic Compass

Control Architecture

PLANTPLANToror

SYSTEMSYSTEM

SensorSensor

Control Control AlgorithAlgorith

mm

• Refresh Rate• Disturbance & Noise• Uncertainty

• Refresh Rate• Disturbance & Noise• Uncertainty

Sensor Problem• GPSGPS Satellite AbsenceSatellite Absence MultipathMultipath 10Hz refresh rate10Hz refresh rate

• Electronic CompassElectronic Compass Magnetic DistortionMagnetic Distortion

EnvironmentEnvironment Vehicle AccelerationVehicle Acceleration 13Hz refresh rate13Hz refresh rate

Satellite Layout

Multipath

Observer Integrated Architecture

PLANTPLANToror

SYSTEMSYSTEM

SensorSensor

Control Control AlgorithAlgorith

mm

ESTIMATORESTIMATOR

CONTROL SIGNAL

SENSOR SIGNAL

Mathematic Model

Sensor Model

Probabilistic Model

ESTIMATE

UPDATE

ESTIMATION SIGNAL

OBSERVEOBSERVERR

Observer Selection

UNSCENTED KALMAN FILTER

PARTICLE FILTER

EXTENDED KALMAN FILTER

Algorithm Concept• Gaussian Distribution• Linear Model

X1

X2

Observer Result

Measurement

Estimation

Solution for Non-Linear System• 1st Order Taylor-Series : Extended Kalman Filter• Unscented Kalman Filter

Limitation• Still in Gaussian• Depends on Complexity

Of System

Use concept of Particle(sample) to represent state distribution• No distribution assumption (Gaussian or Multi-Modal)

• Use weight sum to find estimation• Each particle consist of state & weight• Used “Sequential Importance Sampling with Resampling”

to maintain particle population

Resampling & transform

distribute

updateMeasurement Distribution

N = 12 Particles

Objective• The Main objective is to develop and find an appropriate localization

algorithm between Extended Kalman Filter, Unscented Kalman Filter and Particle Filter, for an intelligent vehicle. The sub-objectives are defined as following: To compare the performance each estimation technique which is Extended

Kalman Filter, Unscented Kalman Filter and Particle Filter . 

Scope and Limitation• Investigate the performance of sensor-fusion of GPS, digital compass

and odometer in Extended Kalman Filter, Unscented Kalman Filter and Particle Filter.

• The driving situation will be in low velocity(<15 km/h).• Path adopted in the experiment are in urban environment which is tree

shrouded rectangle path.

Requirement for Implementation• Control Input Recognition

Steering Angle Speed / Distance which the vehicle moved

• Mathematic Model of Vehicle Non-Slippery Bicycle Model

• Testbed for algorithm testing• Algorithm Structure & Detail

• based on center of curvature concept• Need 3 parameter

1. Axle Length 2. Distance Ratio3. Steering Ratio

• Model Identification• Steepest Descent to identify the parameter• Using Sum-Square of Euclidian Distance as an

Error• Considered each parameter separately until

converge• Cannot calculate all parameter together because they are dependent

• Implemented on MATLAB

ELECTRIC GOLF CARELECTRIC GOLF CAR

Axle Length 1,500mm

Steer Ratio 105pulse/deg

Distance Ratio

4100pulse/m

MITSUBISHI GALANTMITSUBISHI GALANT

Axle Length 2,700mm

Steer Ratio 780pulse/deg

Distance Ratio

92pulse/m

EXTENDED KALMAN FILTER (EKF)EXTENDED KALMAN FILTER (EKF)

Denman – Beavers Square Root

Apply Sequence UKF• reduce calculation complexity

Adaptive Covariance• improve uncertainty level of

estimation

UNSCENTED KALMAN FILTER (UKF)UNSCENTED KALMAN FILTER (UKF)

PARTICLE FILTER (PF)PARTICLE FILTER (PF)

Estimate• uniformly distribute weight

in estimation process Update

• Use Euclidean distance and error of orientation to compute weight

Adaptive Covariance• GPS Covariance depends on environment• We cannot measure covariance of dynamic

object without good ground-truth Q: How to get a good covariance?

• A: Estimate from behavior of system.

Adaptive Covariance

Adaptive Covariance

Traveled distance

Lateral Error

Longitudinal

Error

accLat = abs(accLat – abs( lat_error) )

R = ( 3 * ( accLat + lon_error ) )2

Remark: 1/3 times of Standard Deviation is 99.98% of occurrence

Concept• Developed on VC++ .Net 2005• Use Time-Stamp[ms] to separate each event• Use “com0com” as a serial port emulator

Limitation• Cannot response to control signal• Testbed only transmit good data, cannot send empty

data as actual device Result (repeat logging)

Average Time Stamp Error = 1.99ms Standard Deviation = 5.69ms

Advantage• Same sensor data for all algorithm.• Can perform on single PC without hardware• Good for comparing algorithm

Disadvantage• Cannot perform vehicle control test

TESTBED DEMONSTRATION

Concept• Developed on VC++ .Net 2005• Similar object structure for every algorithm• Localization run on separate Thread• Localize Thread and Frontend Thread use shared

resources which controlled by Mutex• Estimation Logging will sampling every 10ms for updated

dataGPS

COMP

ODO Frontend Thread

SENSOR BUFFER

MUTEX

Localize Thread

NAVI BUFFER

MUTEX

UpdateUpdate

Estimate

Estimate

ODO Data

GPS DataAdaptive covarianc

e

COMP DataAdaptive covarianc

e

Localization thread

Unscented Kalman Filter Static Covariance of 0.1m

Error GPS UKF

Average 2.44 2.49

Cov 65.34 63.46

Max 134.62 119.70

GPS UKF

Unscented Kalman Filter Static Covariance of 1.0m

Error GPS UKF

Average 2.43 2.39

Cov 64.13 51.64

Max 134.62 106.34

GPS UKF

Unscented Kalman Filter Static Covariance of 5.0m

Error GPS UKF

Average 2.43 2.29

Cov 64.28 39.22

Max 134.62 84.73

GPS UKF

Unscented Kalman Filter Adaptive Covariance

Error GPS UKF

Average 2.41 1.37

Cov 61.63 1.16

Max 134.62 5.76

GPS UKF

Extended Kalman Filter Adaptive Covariance

Error GPS EKF

Average 2.44 1.37

Cov 65.03 1.16

Max 134.62 5.78

GPS EKF

ParticleFilter Static Covariance of 0.1m

Error GPS PF

Average 2.43 1.56

Cov 64.17 2.02

Max 134.62 8.73

GPS PF

ParticleFilter Static Covariance of 1.0m

Error GPS PF

Average 2.49 1.43

Cov 70.49 1.49

Max 134.62 8.93

GPS PF

ParticleFilter Static Covariance of 5.0m

Error GPS PF

Average 2.46 1.63

Cov 68.14 1.98

Max 134.62 8.93

GPS PF

ParticleFilter Adaptive Covariance

Error GPS PF

Average 2.44 1.51

Cov 64.88 1.47

Max 134.62 6.50

GPS PF

Adaptive Covariance dramatically increase the performance of both EKF & UKF.

EKF & UKF give a similar result.• 1st Order Linearization is enough for current situation(low

speed).• Complexity of Model is in 1st Order.

PF doesn’t affect much from the difference of covariance changes.

Data Refresh Rate• Use 10ms Timer to collect data from “Navigation Buffer”.• Only the updated data will be written into the logger with Timestamp.

Refresh Rate[ms] UKF EKF PF

Mean 20.45 20.26 21.18

Standard Deviation 8.86 8.34 8.51

From Localization Performance we choose UKF for integrated with control algorithm• Compare between pure sensor control and UKF integrated

control• Low speed control (7 km/h)

CONTROL DEMONSTRATION

Development• Wheel encoder installation for golf car• Wheel odometer installation for mitsubishi galant• Control input recognition board (Odometer Board)

for both vehicle• Localization software which compatible with both

vehicle and with all algorithm. (same thread structure)

• Testbed software for comparison of algorithm• Integration of localization algorithm into control

software• Adaptive covariance algorithm for improve kalman

filter performance (both UKF & EKF)

Result• All observer can decrease the uncertainty of localization.• UKF perform best when apply adaptive covariance

algorithm.• All observer consume nearly the same computational

time which not affect the delay of data.• For current situation (low speed) 1st order expansion is

enough for estimate the system.• Particle Filter show a robustness over varying covariance

of sensor data

Future Work• Increase the reliability of mathematic model by

considering slippery, speed and acceleration of each control signal.

• Integrated more sensor such as IMU(Inertial Measurement Unit).

• Improve & Test Adaptive Covariance Algorithm for various condition and prove it with mathematical tool.