This is the complete navigation module code written in matlab. It has an A Star planner at high level. A hybrid controller and EKF SLAM routines at low level. The simulation is in V-REP using a Pioneer P3DX. The V-REP scene is t3.ttt For more details refer https://bijosebastian.wordpress.com/the-slam-diary/