Upload
gavin-thomas
View
233
Download
0
Tags:
Embed Size (px)
Citation preview
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.