fixed flow of slam, but final_slam still has errors