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.