fixed occupancy grid algorithm and added some sparse comments