Fixed out by one error in position estimate