The calculations of distance_travelled, and estimatedState.velocity, which are both based on wheel rotation, should account for the angle of the steerable wheels and use the forward component of their movement, rather than their total movement.
For this calculation to be possible, state_estimator needs to know the currently requested steering angle. Ideally, state_estimator should estimate the true steering angle by applying a (rate limiting) low pass filter to the requested steering angle.
The calculations of
distance_travelled
, andestimatedState.velocity
, which are both based on wheel rotation, should account for the angle of the steerable wheels and use the forward component of their movement, rather than their total movement. For this calculation to be possible,state_estimator
needs to know the currently requested steering angle. Ideally,state_estimator
should estimate the true steering angle by applying a (rate limiting) low pass filter to the requested steering angle.