Put lidar and joint data .m file into folder .\train\data
Modify data_idx and dir in SLAM.py
Run SLAM.py, the tunning on map and particles can be done in init_SLAM()
a sequence of map plot will be shown, the last plot will be saved occupied grid is in black, free grid is in white, undetermined is in gray, robot trajectory in blue and lidar scan of the best particle in green