changed occupied points to NaN likelihood
made main in locate deal with incorrect angle estimates