Systems and methods for augmenting an inertial navigation system

Systems and methods for augmenting an inertial navigation system (INS) include outputting from the INS position information associated with the implement and adjusting the implement based upon a comparison of the position information of the implement and a desired position of the implement. The INS is periodically re-initialized using error estimates generated by a kalman filter as a function of position information from one or more positioning (or measuring) devices, such as a fan laser, an automatic total station (ATS), a GNSS receiver, or a ground based radio positioning system, to correct a drift of the position information that may be caused by inherent characteristics of the INS.