Answered by:
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
I have made this method:
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?
Thanks for your time,
Rich.Saturday, June 11, 2011 1:40 PM
Answers

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 currentLat = toRadians(cLat); 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; } private double toDegrees(double radians) { double degrees = 0.0f; degrees = (radians * (180 / Math.PI)); return degrees; } private double toRadians(double degrees) { double radians = 0.0; radians = degrees * (Math.PI / 180); return radians; }
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.
 Marked as answer by Luke Thompson  MSFTModerator Wednesday, November 16, 2011 7:31 AM
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 currentLat = toRadians(cLat); 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; } private double toDegrees(double radians) { double degrees = 0.0f; degrees = (radians * (180 / Math.PI)); return degrees; } private double toRadians(double degrees) { double radians = 0.0; radians = degrees * (Math.PI / 180); return radians; }
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.
 Marked as answer by Luke Thompson  MSFTModerator Wednesday, November 16, 2011 7:31 AM
Sunday, June 12, 2011 4:40 PM