A method for steering an agricultural vehicle comprising: receiving
global positioning system (GPS) data including position and velocity information corresponding to at least one of a position, velocity, and course of the vehicle; receiving a
yaw rate
signal; and computing a compensated heading, the compensated heading comprising a blend of the
yaw rate
signal with heading information based on the
GPS data. For each desired swath comprising a plurality of desired positions and desired headings, the method also comprises: computing an actual track and a cross track error from the desired swath based on the compensated heading and the position; calculating a desired
radius of curvature to arrive at the desired track with a desired heading; and generating a steering command based on the desired
radius of curvature to a steering mechanism, the steering mechanism configured to direct the vehicle.