1. Methee Srisupundit Final Defense

2.  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

4.  Unman Transport Vehicle Automated Navigation Traffic Obedience Accident Avoidance We have a DREAM! we called…

5.  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

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

7.  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

8.  Observer Integrated Architecture PLANTorSYSTEMPLANTorSYSTEM SensorSensor Control Algorithm ESTIMATORESTIMATOR

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

10.  Observer Selection UNSCENTED KALMAN FILTER PARTICLE FILTER EXTENDED KALMAN FILTER

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

12.  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

13.  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

14. Resampling & transform distribute update Measurement Distribution N = 12 Particles

15.  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.

17.  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

18. based on center of curvature concept Need 3 parameter 1. Axle Length 2. Distance Ratio 3. Steering Ratio

19. 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

20. ELECTRIC GOLF CAR Axle Length1,500mm Steer Ratio105pulse/deg Distance Ratio 4100pulse/m

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

22. EXTENDED KALMAN FILTER (EKF)

23.  Denman – Beavers Square Root  Apply Sequence UKF reduce calculation complexity  Adaptive Covariance improve uncertainty level of estimation UNSCENTED KALMAN FILTER (UKF)

24. PARTICLE FILTER (PF)  Estimate uniformly distribute weight in estimation process  Update Use Euclidean distance and error of orientation to compute weight

25.  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.

26.  Adaptive Covariance

27. 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

29.  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

30.  Advantage Same sensor data for all algorithm. Can perform on single PC without hardware Good for comparing algorithm  Disadvantage Cannot perform vehicle control test

31. TESTBED DEMONSTRATION

32.  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

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

34. Unscented Kalman Filter Static Covariance of 0.1m ErrorGPSUKF Average2.442.49 Cov65.3463.46 Max134.62119.70 GPSUKF

35. Unscented Kalman Filter Static Covariance of 1.0m ErrorGPSUKF Average2.432.39 Cov64.1351.64 Max134.62106.34 GPSUKF

36. Unscented Kalman Filter Static Covariance of 5.0m ErrorGPSUKF Average2.432.29 Cov64.2839.22 Max134.6284.73 GPSUKF

37. Unscented Kalman Filter Adaptive Covariance ErrorGPSUKF Average2.411.37 Cov61.631.16 Max134.625.76 GPSUKF

38. Extended Kalman Filter Adaptive Covariance ErrorGPSEKF Average2.441.37 Cov65.031.16 Max134.625.78 GPSEKF

39. ParticleFilter Static Covariance of 0.1m ErrorGPSPF Average2.431.56 Cov64.172.02 Max134.628.73 GPSPF

40. ParticleFilter Static Covariance of 1.0m ErrorGPSPF Average2.491.43 Cov70.491.49 Max134.628.93 GPSPF

41. ParticleFilter Static Covariance of 5.0m ErrorGPSPF Average2.461.63 Cov68.141.98 Max134.628.93 GPSPF

42. ParticleFilter Adaptive Covariance ErrorGPSPF Average2.441.51 Cov64.881.47 Max134.626.50 GPSPF

43. Error LEFTRIGHTG LEFTG RIGHT avgcovmaxavgcovmaxavgcovmaxavgcovmax UKF 0.1 2.4963.46119.701.858.6026.251.732.279.132.319.1634.59 1.0 2.3951.64106.341.858.3023.931.671.919.052.146.5522.82 5.0 2.2939.2284.731.716.6918.801.601.589.322.044.4714.62 Ad 1.371.165.761.371.878.711.762.2513.471.852.839.13 EKF 0.1 2.5265.72119.701.838.3026.281.732.259.132.319.2634.59 1.0 2.3851.01105.991.787.4123.951.651.899.052.146.5522.82 5.0 2.2938.7884.701.656.0419.071.581.549.332.044.4714.02 Ad 1.371.165.781.452.6010.011.772.1213.441.802.809.49 PF 0.1 1.562.028.733xx 5xxxx 6xx1.822.549.097.1061.8725.12 1.0 1.431.498.931.431.878.911.742.0810.771.953.4910.03 5.0 1.631.988.931.461.608.781.681.8914.892.093.1810.57 Ad 1.511.476.501.582.449.422.477.6123.492.042.779.89

44.  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.

45.  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 20.4520.2621.18 Standard Deviation 8.868.348.51

46.  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)

47. CONTROL DEMONSTRATION

50.  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)

51.  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

52.  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.