Methee Srisupundit Final 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 frame” “an ability to identify the location of itself in a coordinate frame”  How to do it? Considered Coordinate Frame Sensory Tool GPS Electronic Compass

 Control Architecture PLANTorSYSTEMPLANTorSYSTEM SensorSensor Control Algorithm Refresh Rate Disturbance & Noise Uncertainty Refresh Rate Disturbance & Noise Uncertainty

 Sensor Problem GPS GPS  Satellite Absence  Multipath  10Hz refresh rate Electronic Compass Electronic Compass  Magnetic Distortion  Environment  Vehicle Acceleration  13Hz refresh rate Satellite Layout Multipath

 Observer Integrated Architecture PLANTorSYSTEMPLANTorSYSTEM SensorSensor Control Algorithm ESTIMATORESTIMATOR

CONTROL SIGNAL SENSOR SIGNAL Mathematic Model Sensor Model Probabilistic Model ESTIMATE UPDATE ESTIMATION SIGNAL OBSERVER

 Observer Selection UNSCENTED KALMAN FILTER PARTICLE FILTER EXTENDED KALMAN FILTER

 Algorithm Concept Gaussian Distribution Linear Model X1X1 X2X2 Observer Result Measurement Estimation

 Solution for Non-Linear System 1 st 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 update Measurement 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 Ratio 3. 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 CAR Axle Length1,500mm Steer Ratio105pulse/deg Distance Ratio 4100pulse/m

MITSUBISHI GALANT Axle Length2,700mm Steer Ratio780pulse/deg Distance Ratio 92pulse/m

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)

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

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 data GPS COMP ODO Frontend Thread SENSOR BUFFER MUTEX Localize Thread NAVI BUFFER MUTEX

Update Estimate ODO Data GPS Data Adaptive covariance COMP Data Adaptive covariance  Localization thread

Unscented Kalman Filter Static Covariance of 0.1m ErrorGPSUKF Average Cov Max GPSUKF

Unscented Kalman Filter Static Covariance of 1.0m ErrorGPSUKF Average Cov Max GPSUKF

Unscented Kalman Filter Static Covariance of 5.0m ErrorGPSUKF Average Cov Max GPSUKF

Unscented Kalman Filter Adaptive Covariance ErrorGPSUKF Average Cov Max GPSUKF

Extended Kalman Filter Adaptive Covariance ErrorGPSEKF Average Cov Max GPSEKF

ParticleFilter Static Covariance of 0.1m ErrorGPSPF Average Cov Max GPSPF

ParticleFilter Static Covariance of 1.0m ErrorGPSPF Average Cov Max GPSPF

ParticleFilter Static Covariance of 5.0m ErrorGPSPF Average Cov Max GPSPF

ParticleFilter Adaptive Covariance ErrorGPSPF Average Cov Max GPSPF

Error LEFTRIGHTG LEFTG RIGHT avgcovmaxavgcovmaxavgcovmaxavgcovmax UKF Ad EKF Ad PF xx 5xxxx 6xx Ad

 Adaptive Covariance dramatically increase the performance of both EKF & UKF.  EKF & UKF give a similar result. 1 st Order Linearization is enough for current situation (low speed). Complexity of Model is in 1 st 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] UKFEKFPF Mean Standard Deviation

 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) 1 st 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.