# Calculating Azimuth/Bearing to determine which direction my robot should be facing.

• ### Question

• Ok, here is my problem: Determining Azimuth/Bearing.

After putting my GPS robot on hold for a while (long), I have decided
to finish it. I have everything working but am having some trouble with calculating the angle the robot needs to rotate in order to be facing the direction of it's
target.

I have point S and point F, representing Start and Finish respectively.

I need to work out how much the robot needs to rotate so that it is
facing it's target using only Decimal GPS coordinates.

Let's pretend we are going from Stonehenge, UK (S) to Big Ben, UK (F)

Using
S = Decimal (WGS84) 51.178891, -1.826225
F = Decimal (WGS84) 51.500782, -0.124669

Let's pass S into the getBearing() method.

cLat = 51.178891 | cLon = -1.826225

public float getBearing(float cLat, float cLon) {
float bearing = 0.0f;

float currentLat = cLat;
float currentLon = cLon;

float destinationLat = 51.500782f;// Big Ben Lat.
float destinationLon = -0.124669f;// Big Ben Lon.

float differenceLat = destinationLat - currentLat; // we don't need this yet, but may come in handy later.
float differenceLon = destinationLon - currentLon;

float Y = (float)(Math.sin(differenceLon) * Math.cos(destinationLat));
float X = (float)(Math.cos(currentLat) * Math.sin(destinationLat) - Math.sin(currentLat) *  Math.cos(destinationLat) * Math.cos(differenceLon));

bearing = toDegrees(Math.atan2(Y, X)); // our bearing calculation :)

return bearing;
}

This method should return the amount in degrees that the robot needs
to rotate to face it's target.

When I use an online calculator the results are somewhat different to what I get displayed on my robot. Could anyone have a quick look please and tell me if my calculation is correct?

Rich.

Saturday, June 11, 2011 1:40 PM

• Thanks, but I don't really understand how to use Haversine :(

Anyways, I did manage to correctly calculate the bearing. Here is the code written in JAVA for anyone else that comes across this problem:

```public double getBearing(double cLat, double cLon) {
double bearing = 0.0;

double currentLon = cLon;

double destinationLat = toRadians(51.500782);// Test: Big Ben lat - UK
double destinationLon = -0.124669;// Test: Big Ben lon - UK

double differenceLat = toRadians(destinationLat - currentLat); // we don't need this yet, but may come in handy later.
double differenceLon = toRadians(destinationLon - currentLon);

double Y = (Math.sin(differenceLon) * Math.cos(destinationLat));
double X = (Math.cos(currentLat) * Math.sin(destinationLat) - Math.sin(currentLat) * Math.cos(destinationLat) * Math.cos(differenceLon));

bearing = toDegrees(Math.atan2(Y, X)); // our bearing calculation :)

return bearing;
}

double degrees = 0.0f;
degrees = (radians * (180 / Math.PI));
return degrees;
}

radians = degrees * (Math.PI / 180);
}```

About the use of doubles rather than floats: Yeah, I was initially using doubles but for some reason on my NXT brick it was not displaying anything other than whole numbers.

Also, just to be 100% sure, it is the bearing that I need, isn't it? I've never done any navigation before and so I just want to be absolutely clear that I am on the right path before I move on.

Thanks,

Rich.

Sunday, June 12, 2011 4:40 PM

### All replies

• The formula you're looking for is here - http://en.wikipedia.org/wiki/Haversine_formula

One quick thing - I would suggest using doubles instead of floats.

Saturday, June 11, 2011 3:51 PM
• Thanks, but I don't really understand how to use Haversine :(

Anyways, I did manage to correctly calculate the bearing. Here is the code written in JAVA for anyone else that comes across this problem:

```public double getBearing(double cLat, double cLon) {
double bearing = 0.0;

double currentLon = cLon;

double destinationLat = toRadians(51.500782);// Test: Big Ben lat - UK
double destinationLon = -0.124669;// Test: Big Ben lon - UK

double differenceLat = toRadians(destinationLat - currentLat); // we don't need this yet, but may come in handy later.
double differenceLon = toRadians(destinationLon - currentLon);

double Y = (Math.sin(differenceLon) * Math.cos(destinationLat));
double X = (Math.cos(currentLat) * Math.sin(destinationLat) - Math.sin(currentLat) * Math.cos(destinationLat) * Math.cos(differenceLon));

bearing = toDegrees(Math.atan2(Y, X)); // our bearing calculation :)

return bearing;
}

double degrees = 0.0f;
degrees = (radians * (180 / Math.PI));
return degrees;
}

radians = degrees * (Math.PI / 180);