Occupancy grid mapping basic code