[sloved]autonomus car(help needed)

Sorry for bad English.
I am trying to build an autonomus rc car which will go from point a to b to c and so on based on given co ordinate inputs.(my goal)
I used neo 6m gps module and qmc588l magnetometer.The problem is ,even with 8-10 gps lock,the car wont drive straight (the chasis is checked ok and uses differential drive).While testing I found out a few issues-
A.Magnetometer issue
1.The magnetometer reading doesn't align with the heading from my cellphone,(which i used to compare) with deviation upto 20 degrees,even after calibration.(declination was taken into consideration.)
2.The magnetometer doesn't show 90 degree turns when i turn 90 degrees ,like when i turn from north to east it shows 360- 60 degree,then when i turn from east to south it goes around 190 degrees(calibration issue?)but shows around 360-1 or 2 degrees when i face north again.
B.GPS issue
1.While stationary,I took the gps co-ordinates and copied them to google maps to see my position.I found out that from the gps ,even with more than 6 gps lock,my position error was around 5-15 meters (15-45 feet) off the real position and the position problems changed from time to time as the number of gps lock changed.
So with my goal in mind,my questions are:
1.Am I on the right track?
2.How can i solve my problem using this gps and compass or
3.Do I need other sensors like wheel encodes or imu,If so then how to integrate these input?
4.Is it possible to achieve accuracy around 2 meters ,how much it might cost?
The code i used is attached below
I apologize for my English again.

Magnetometers must be carefully calibrated in the vehicle in order to be useful as a compass. Here is a good overview of the best way to do that.

GPS accuracy varies from +/- 3 m to useless, depending on the environmental conditions and number of satellites in view. You must be outdoors under a clear sky, with no buildings nearby for it to work well.

Solve your magnetometer problem before worrying about increased accuracy.

One step at a time brings us forward.

Start by getting the magnetometer well away from any magnetic things such as motors or power wires. Put it on a stalk at least 30cm long away from the body of the vehicle.

Have a look at the winners of autonomous vehicle competitions, such as the SparkFun one. They publish lots of details on their winning strategies.

The following functions are the problem.

Single precision float values and functions (about 6 digits of accuracy) are far too inaccurate for distance and bearing calculations with latitude and longitude coordinates. Since Arduino does not support the double data type, use NeoGPS instead of TinyGPS, and the distance/bearing functions in the NeoGPS library instead. Those functions support about 9 digits of accuracy.

float getdistance(float lan1,float long1,float lan2,float long2)//xlat,xlon,flat,flon
  return 1000*2*R*asin(sqrt(pow(sin(rad*(lan1-lan2)/2),2)+cos(rad*lan1)*cos(rad*lan2)*pow(sin(rad*(long1-long2)/2),2)));
float getbearing(float lat2,float lon2,float lat1,float lon1)//xlat,xlon,flat,flon
{float T;
T=atan2(sin((lon2-lon1)*rad)*cos(rad*(lat2)),((cos(rad*(lat1))*sin(rad*(lat2)))-( sin(rad*(lat1))*cos(rad*(lon2-lon1))*cos(rad*(lat2)))));
return T;