A control system for a vehicle using a forward-facing camera includes a look ahead module configured to determine a distance to a look ahead point. A lane center module determines a location of a lane center line. A vehicle center line module determines a location of a vehicle center line. A first lateral offset module determines a first lateral offset based on the look ahead point and the determined lane center line. A second lateral offset module determines a second lateral offset based on the determined lane center line and the vehicle center line. A yaw angle offset calculating module receives the first lateral offset, the second lateral offset and the distance to the look ahead point, calculates a yaw angle offset, and compensates a yaw angle error based on the yaw angle offset.