Haversine 公式错误 - 不正确的距离 - Arduino

Question

目前参与的项目需要从 gps 模块获取读数，然后使用这些读数计算读数与固定航路点之间的距离。 Gps 工作并给出 LAT 的值 - 54.9289 和 LON - -1.368 这应该给出大约 3,200 米的距离。但是它给出了大约6105。我也有一种感觉，即6105是km到哈哈。我想知道它是否没有正确地取负数，或者我在代码中是否有一些变量冲突。如果能对此有所了解，那就太好了，谢谢。

#include <TinyGPS.h>
#include <SoftwareSerial.h>
#include <rgb_lcd.h>
#include <Wire.h>

//Sets TX And RX Pins
SoftwareSerial GPS(2,3);
TinyGPS gps;
void gpsdump(TinyGPS &gps);
bool feedgps();
void CheckGPS();
void GetCoords();
 long lat, lon;
 float LAT, LON; // Latitude is gained from GPS and stored in another variable to avoid errors - Should change with changing GPS value - Which would alter distance to waypoint.
 float LAT1,LON1;
rgb_lcd lcd;


void setup()
{
// Sets Baud Rate
  GPS.begin(9600);
  Serial.begin(115200);
}
// Determines The Distance Between Current Location And Waypoint

void GetDistance() 
{
  // Calculating Distance Between Waypoints

  double Distance_Lat; // Distance between Lattitude values
  double Distance_Lon; // Distance between Lonitude values
  double Distance_Total = 0;// Total Distance 
  double val,val2; // Subsidary variable for holding numbers. - No actual value represented.
  double fLAT1,fLAT2;
  double LAT2 = 54.900000; // Waypoint Latitude
  double LON2 = -1.368072; // Waypoint Longitude

 // Initialising Calculation
  Distance_Lat = radians(LAT2-LAT1); // Must be done in radians
  fLAT1 = radians(LAT1);
  fLAT2 = radians(LAT2);
  Distance_Lon = radians((LON2)-(LON1));

  // Calculating Distance - Using Haversines Formulae
  Distance_Total = (sin(Distance_Lat/2.0)*sin(Distance_Lat/2.0));
  val = cos(fLAT1);
  val = val*(cos(fLAT2));
  val = val*(sin(Distance_Lon/2.0));
  val = val*(sin(Distance_Lon/2.0));
  Distance_Total = Distance_Total + val;

  Distance_Total = 2*atan2(sqrt(Distance_Total),sqrt(1.0-Distance_Total)); 
  Distance_Total = Distance_Total*6371.0000; // Converting to meters.
Serial.println("Distance: ");
Serial.println(Distance_Total);

  //---------------------------------------------------------------------------------
}

// Returns Latitude And Longitude As Decimal Degrees (DD).
void GetCoords()
{
 long lat, lon;
  CheckGPS();
  Serial.print("Latitude : ");
  Serial.print(LAT/1000000,7);
  Serial.print(" :: Longitude : ");
  Serial.println(LON/1000000,7);

}

void CheckGPS()

  {
  bool newdata = false;
  unsigned long start = millis();
 // Every 1 seconds, Print an update
  while (millis() - start < 1000)
    {
    if (feedgps ())
    newdata = true;
    if (newdata)
    gpsdump(gps);
    }
  }

// Checks If The GPS Has Any Data To Transmit
  bool feedgps()
  {
    while (GPS.available())
    if (gps.encode(GPS.read()))
      return true;
    else
      return false;  
  }

// Transmits GPS Data And Gets Latitude And Longitude Positions.
  void gpsdump(TinyGPS &gps)

{    gps.get_position(&lat, &lon);
     LAT = lat;
     LON = lon;
     //Keeps The GPS Fed To Avoid Checksum Errors.
     feedgps();

}

void loop()
{
// Function That Returns The GPS Coordinates In DD.
  GetCoords();
  GetDistance();
}

Answer 1

我现在在维基百科上看到的半正弦公式，https://en.wikipedia.org/wiki/Haversine_formula，在你有 atan2 的地方有 arcsin(sqrt(Distance_Total))。

Haversine 公式错误 - 不正确的距离 - Arduino

Haversine Formula Error - Incorrect Distance - Arduino

arduino

gps

haversine