Vfrdtyky

Hi I am planing to use two RTK-GPS devices to get the position and heading of a outdoor robot. The two devices are placed on the left and the right side of the robot with a distance of 2m. The heading should be a value between 0 and 2*PI. 0 if the robot is facing north.

I am trying the following:

double x1 = gps_left->latitude;

double y1 = gps_left->longitude;

double x2 = gps_right->latitude;

double y2 = gps_right->longitude;



double dx = x2-x1;

double dy = y2-y1;



// get the normal of the line

double nx = dy*-1;

double ny = dx;



double angle = atan2(ny ,nx);

somehow the angle always stays the same even if i rotate the robot

double x1 = gps_left->latitude;

double y1 = gps_left->longitude;

double x2 = gps_right->latitude;

double y2 = gps_right->longitude;



double dy = y2-y1;



double nx = cos(x2)*sin(dy);

double ny = cos(x1)*sin(dy) - sin(x1)*cos(x2)*cos(dy);



double angle = atan2(nx,ny);

Can you try this?

0 - heading north,
90 - heading east,
180 - heading south,
270 - heading west.

Like @Mark Booth the resolution might not be enough. We had a similar issue, so we used a magnetometer to fix this. Using this heading information from a magnetometer and GPS we were able to navigate successfully.