How To Build A Hybrid Fusion Module

Transcription

1 LIVIC CVIS-POMA presentation Hybrid Fusion module with IMM approach for Cooperative Vehicle Infrastructure Systems (CVIS) Sébastien Glaser Dominique Gruyer, Alexandre Ndjeng Ndjeng LIVIC (LCPC/INRET)

2 Summary Localisation module in CVIS Few theory on positionning IMM approach integration Experimental results

3 Objectives POMA Hybrid Fusion GPS WSN WLAN Specifications of SupplyPosition UC CV-UC-SP SupplyPosition CV-UC-SP SupplyLocationReference «include» Actor «POMA server» CV-UC-SP SupplyMap «include» CV-UC-SP MapMatching «extend» CV-UC-SP UpDateMap «include» CV-UC-SP GeospatialPlatform ServiceCentreMgr COMO Actors «applications» CINT CURB COMO EMAP «include» AGORA-C «include» Positionning problem: 1 Accuracy of the positionning 2 Confidence in the positionning

4 General architecture

5 Hybrid Fusion module Hybrid Fusion module EKF IMM

6 Hybrid Fusion module with IMM approach for Cooperative Vehicle Infrastructure Systems (CVIS)

7 Traditional Hybridization (single model estimation) Probabilistic approach Vehicle state vector Process Model : integration of proprioceptive sensors data ( x, y, θ v ) Loc k / k = k / k k / k k / k, k / k = Loc f (, 1 k 1/ k 1 uk wk ) Observation Model : update by ( Loc y ) k = h k / k 1, vk, Loc k / k Kalman Filter exteroceptive sensors data Current Nonlinear model l f ( θk 1/ k 1 ) cos( δ r ) ( θk 1/ k 1 ) cos( δ r ) tan( ) xk / k 1 = xk 1/ k 1 + vk 1/ k 1 T cos yk / k 1 = yk 1/ k 1 + vk 1/ k 1 T sin : a 1 θk / k 1 = θk 1/ k 1 + T vk / k 1 δ r l pip vk / k 1 = dist T Convenient o System can diverge in case of strong nonlinearities o Specific domain of validity low confidence in the localization function regarding driving situations o Computational cost increases with an increasing number of parameters

8 Vehicle Dynamic Space Decomposition ( i ) k 1/ k 1??? ( CT ) Λ k ( i) k / k GPS Data CV CT Prediction for CA model CA past ego position ( CV ) Λ ( CA) k Λ k : Constant Acceleration : Constant Acceleration : : Constant Constant Turning Turning : Constant : Constant Velocity Velocity Simple linear models for specific dynamics Simple linear models for specific dynamics

9 IMM (Interacting Multiple Models) with static or dynamic structure Better complexity Robust method in case of variations or modes incompatibility Better compromise complexity computation time ( i) k 1/ k 1 ( i) k 1/ k 1 ( i ) k 1/ k 1 Model 1 Fusion Fusion k / k µ Interaction Interaction ( i / j ) k 1/ k 1 Mixed Mixed Probabilities Probabilities ( i ) k 1/ k 1 Π ji Model 2 Model n Function Function of of likelihoods likelihoods Probabilities Probabilities updating updating ( i) Λ k ( i) µ k / k ( i) µ k / k Markov Markov transition transition model model

10 IMM (Interacting Multiple Models) : Example ( CR ) k / k 1 Correction ( CR) k / k ( CT ) k / k Correction ( CT ) k / k 1 GPS Data GPS Data ( CR ) µ k / k ( CT ) µ k / k ( CR) Λ k ( CT ) Λ k CR ( ) CR k / k 1 k 1/ k 1 ( ) CS k / k 1 CS past ego position CV ( ) CV k / k 1 CT ( CT ) k / k 1 Prediction for CA model CA ( CA) k / k 1 ( CV ) Λ k ( CA) Λ k Probability updating ( CV ) µ k / k ( CA) µ k / k ( CV ) k / k 1 GPS Data Correction ( CV ) k / k Assessment of the current ego position for CV model Fusion ( i) k / k ( CA) k / k Assessment of the current ego position Correction ( CA ) k / k 1 GPS Data

11 IMM (Interacting Multiple Models) : Results and conclusion Results Positioning with IMM approach Probabilities by model Conclusion Filters run in parallel Final output computed from the merging of filters outputs assess the most likely models Take into account all the possible configurations Better robustness to the data noise and corrupted data

12 Hybrid Fusion components In RTMaps

13 IMM (Interacting Multiple Models) : Embedded architecture Architecture for multiple sensors with standardized structure With FifoReader Management of multiple inputs Sensor i Interface Sensor Management of multiple inputs Sensor j Interface Sensor Sensor structure Sensor structure Sensor Standardized Structure (multiplexor) Sensor structure Hybrid Hybrid Fusion Fusion IMM Positioning EKF Positioning Sensor k Interface Sensor Sensor structure Dynamic number of input

14 IMM (Interacting Multiple Models) : RTMaps diagram General diagram for the POMA Hybrid Fusion module Hybrid Fusion module: Outputs either IMM approach or EKF approach Sensors data base Sensors Interfaces

15 IMM (Interacting Multiple Models) : RTMaps diagram The properties window for Hybrid Fusion module Hybrid Fusion TopView Hybrid Fusion Package Sensors

16 Beta test in Versailles May 2009

17 Available sensors the CAN data the POMA DR Sensor Board to Roof Antenna Platform 1a Speed data Odometer sensor Yaw-Rate (Z-)Gyro -Y- Magnetometer PCB L1 GPS Receiver PPS I/O Serial Port I/O Serial Port Serial Port µc Time stamp & I/F Driver S/W PCI Bus 1 Control I/F Test with Isea reference sensors

18 Observations Identification of the sensors configurations and noises Problem of synchronization detected Problem of CAN data only positive speed trouble with rear maneuvers we need to add 2 new models to the IMM approach: CS and CR With the current sensor (< 10hz) we need to provide positioning with a 10 Hz frequency Validation in Versailles with a data base (1 hour) included: motorway (high speed) sub urban areas (low speed and turn maneuver) tunnels (GPS looses) traffic jam (very slow speed) stop and go maneuvers Test tracks (Satory s tracks)

19 Versailles scenarios (Satory s s track, Miniere road, A86) Satory s s Track Minière Road

20 Versailles scenarios (Satory s s track, Miniere road, A86) A86 (Highway with tunnels)

21 Versailles scenarios (Satory s s track) Reference track

22 Versailles scenarios (Satory s s track)

23 Versailles scenarios (Satory s s track)

24 Versailles scenarios (Miniere Road round about) Reference track

25 Versailles scenarios (Miniere Road round about) Loss of good EGNOS data (low integrity) More than 35 seconds

26 Versailles scenarios (Miniere Road round about)

27 Versailles scenarios (A86 Tunnels)

28 1 2 Problem when EGNOS Has a good confidence!!! Loss of EGNOS GPS data!!! Green color: Hybrid Fusion results 3 4 Bad EGNOS GPS data!!! Loss of EGNOS GPS data!!!

29 Loss of good EGNOS data (low integrity) More than 40 seconds EGNOS data with good integrity) But bad values

30