Small fixes to mapper and filter. Mapper works well now. Need to output map for the path planner.