locked
Calculating Azimuth/Bearing to determine which direction my robot should be facing. RRS feed

  • 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.

    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.

    Sunday, June 12, 2011 4:40 PM