Fixed heading state discretization mistake