Code added to take error into account when determining robot position