Calcul du cap avec deux appareils GPS

Question

Bonjour, Je prévois d'utiliser deux appareils RTK-GPS pour obtenir la position et le cap d'un robot extérieur. Les deux appareils sont placés à gauche et à droite du robot avec une distance de 2m. Le titre devrait être une valeur comprise entre 0 et 2 * PI. 0 si le robot est orienté vers le nord.

Je suis en train de ce qui suit:

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

en quelque sorte l'angle reste toujours la même, même si je fais tourner le robot

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

Pouvez-vous essayer?

0 - vers le nord, 90 - dirigeant vers l'est, 180 - en direction du sud, 270 - vers l'ouest.