Tried to create a linearized map update model. Did not work
Changed the sensor model update_map to fill away from the robot