Fixed localisation, but error is still to 1m