manip sensor model tested in simulation- seems to work