The present disclosure relates to lane-level map matching for a vehicle. The method includes receiving vehicle data including a geographical position of the vehicle, a heading of the vehicle, and a speed of the vehicle and receiving sensor data from a perception system of the vehicle. The sensor data includes information about a position of at least one road reference in a surrounding environment of the vehicle. Operations include receiving map data including a lane geometry of the surrounding environment of the vehicle, the lane geometry including a set of candidate lanes. Operations include forming a state space model including a set of states. Each state of the set of states represents a candidate lane of the set of candidate lanes, and defining a cost for going from each state to every other state of the set of states based on the received vehicle data and the received sensor data.