Hi,
What interval, distance or time are you trying to update the robots position and correct its direction of travel.
How precise do you think GPS is?
If you are looking for 0.0001 degree change in direction, over a few cms, I think you need to reconsider your methodology.
You are working out the new direction to travel, but do not know in what direction your robot is pointing.
Check how accurate GPS first, display readings as you move and see if they correlate to the real world movement.
I fear you are over estimating the precision of the GPS system in a small scale world.
You are better to use encoders on each drive motor to make sure you have instantaneous straight line movement.
Tom.. 
