Den einzigen meines Wissens nach möglichen Weg mit den vorhandenen Sensoren um eben den Drift zu kompensieren ist die Messung einer bekannten Größe, also der Weg den du bereits vorgeschlagen hast.

Was daran aber zu optimieren möglich wäre, wäre eine Messung und Map Bildung an einer beliebigen Stelle am Raum. Wenn du nun z.B. deine Lidar Messung durch Filtern vieler einzelner Messwerte an dieser Stelle möglichst präzise machst (Da gibt es schöne Ansätze die von einer Gausverteilung der Lidarmesswerte ausgehen) kannst du versuchen die Messwerte die du an der Stelle hast eindeutig in deiner Map zu verorten. Dadurch könntest du Fehler bezüglich deiner derzeitigen Position beheben.