An integrated algorithm for ego-vehicle and obstacles state estimation for autonomous driving

Understanding of the driving scenario represents a necessary condition for autonomous driving. Within the control routine of an autonomous vehicle, it represents the preliminary step for the motion planning system. Estimation algorithms hence need to handle a considerable number of information coming from multiple sensors, to provide estimates regarding the motion of ego-vehicle and surrounding obstacles. Furthermore, tracking is crucial in obstacles state estimation, because it ensures obstacles recognition during time. This paper presents an integrated algorithm for the estimation of ego-vehicle and obstacles’ positioning and motion along a given road, modeled in curvilinear coordinates. Sensor fusion deals with information coming from two Radars and a Lidar to identify and track obstacles. The algorithm has been validated through experimental tests carried on a prototype of an autonomous vehicle.