3D estimation of road cartography using vehicle localization and observers

Driving safety enhancement could be achieved by better understanding of risk situations from the knowledge of vehicle dynamic states as well as road geometry. Among the parameters of the road that have an impact on vehicle dynamics, one can find the bank and the slope angles, which can not however be measured by mean of low cost onboard sensors. This work of this paper is aimed to improve the controllability of the vehicle and the development of a low cost mapping system of the roadway. Two observers are developed firstly to estimate these two variables and secondly to localize the vehicle. The road attributes are estimated using an extended Kalman filter (EKF) and a Proportional Integral (PI) observer with unknown inputs based vehicle models and the measurements obtained from inertial (INS) and ABS sensors. The vehicle localization is performed at each time sample using an algorithm based on the IMM technique (Interacting Multiple Models), it allows to reconstruct the road shape in 3 dimensions. Testing on measurements obtained with a prototype vehicle show the good behavior of the proposed estimation scheme.