Hi every one. We can obtain spherical distance (direct distance) between two points by knowing their latitude and longitude through Haversine formula. But in control and navigation works, we want to know the distance vector along the north and the east between a **desired** and **current** point. So, we should obtain the **distance vector (spherical distance vector)** along the y direction considered from the south to the north and the x direction considered from the west to the east. So, I devised a method to obtain the **distance vector** along the y and x direction and described it completely on the pdf in the attachment.

According to this method, I wrote this code to find direct distance and distance vector along the north and the east by using Ublox 6M nodule:

```
#include <TinyGPS++.h>
#include <SoftwareSerial.h>
static const int RXPin = 69, TXPin = 68; // Software Serial ports
static const uint32_t GPSBaud = 9600;
double Cur_Long, Cur_Lat; // Current Longitude and Latitude
double Des_Long=40.501122*PI/180, Des_Lat=45.145577*PI/180; // Insert Desired Longitude and Latitude here (in Radian)
double d_direct, d_y, d_x; // Direct distance, Distance vector along the north and the east
double r = 6371000; // Radius of the earth in meters
double Delta_Lat,Delta_Long;
TinyGPSPlus gps; // TinyGPS++ object
SoftwareSerial ss(RXPin, TXPin); // Define Software Serial ports
void setup()
{
Serial.begin(115200);
ss.begin(GPSBaud); // Activating of Software Serial
}
void loop()
{
while (ss.available() > 0){
if (gps.encode(ss.read())){
if (gps.location.isValid()){
Cur_Lat=gps.location.lat()*PI/180; // Current Latitude in Radians
Cur_Long=gps.location.lng()*PI/180; // Current Longitude in Radians
Delta_Lat=Des_Lat-Cur_Lat;
Delta_Long=Des_Long-Cur_Long;
d_y=sign(Delta_Lat)*2*r*asin(abs(sin(Delta_Lat/2))); // Distance vector along the north (y direction)
d_x=sign(Delta_Long)*2*r*asin(abs(cos(Des_Lat)*sin(Delta_Long/2))); // Distance vector along the east (x direction)
d_direct=2*r*asin(sin(Delta_Lat/2)*sin(Delta_Lat/2)+cos(Cur_Lat)*cos(Des_Lat)*sin(Delta_Long/2)*sin(Delta_Long/2)); // spherical distance (direct distance)
Serial.print("Current Latitude=");
Serial.print(Cur_Lat);
Serial.print("; Current Longitude=");
Serial.print(Cur_Long);
Serial.print("; Distance vector x=");
Serial.print(d_x);
Serial.print("; Distance vector y=");
Serial.print(d_y);
Serial.print("; Direct Distance=");
Serial.println(d_direct);
} // end of if (gps.location.isValid())
else{ // else of if (gps.location.isValid())
Serial.println("invalid data");
}
} // end of if (gps.encode(ss.read()))
} // end of while (ss.available() > 0)
if (millis() > 5000 && gps.charsProcessed() < 10){
Serial.println("No GPS detected: check wiring.");
}
} // End of void loop
int sign( double n){
if (n>0)
return 1;
else if(n<0)
return -1;
else
return 0;
}
```

If you have any question, you can ask me in comments or by a pm.

if you want to get Euler Angles directly and exactly you can see this:

https://forum.arduino.cc/index.php?topic=563091.0

Obtaining Distance Vector Using GPS.pdf (326.3 KB)