Got inverse sensor model working