One paper accepted at IEEE Transactions on Intelligent Vehicles