2つのGPSデバイスで見出しを計算する

Question

こんにちは私は屋外のロボットの位置と見出しを取得するために2つのRTK-GPSデバイスを使用する予定です。 2つのデバイスは、ロボットの左右に2mの距離を置いて配置されます。見出しは0と2 * PIの間の値でなければなりません。ロボットが北向きの場合は0。

私は次のことをしようとしています：

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); 

何とか角度は常に私がロボット

Answer 1

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); 

を回転させる場合でも、あなたはこれを試すことができます同じまま？

0 - 見出し北、 90 - 見出し東、 180 - 見出し南、 270 - 見出し西。