자율항법시스템_4_두 지점간 거리 Bearing각 구하기 (vincenty arduino) - cchamchi/cansat GitHub Wiki
지구상의 두 좌표(longitude,latidude) 간의 거리를 구해보자
지점 A(a1,b1) 지점 B(a2,b2) 거리 d
- 내적을 이용하여 벡터간 사이각을 구한다.
각 좌표를 직교 좌표계로 변환 하면
(a1,b1) -> (x1,y1,z1) , (a2,b2) -> (x2,y2,z2)
여기서 x=cosacosb, y=sinacosb, z=sinb
3차원 좌표계에서 두 벡터의 내적은
A.B = x1x2 + y1y2 + z1*z2 =
자세한 것은 아래 그림을 볼것
거리 구하기 프로그램
function degreesToRadians(degrees) {
return degrees * Math.PI / 180;
}
function distanceInKmBetweenEarthCoordinates(Lat1,Lon1,Lat2,Lon2) {
var earthRadiusKm = 6371;
var angle;
Lon1 = degreesToRadians(Lon1);
Lat1 = degreesToRadians(lat1);
Lon2 = degreesToRadians(Lon2);
Lat2 = degreesToRadians(Lat2);
angle= acos( sin(Lat1) x sin(Lat2) + cos(Lat1) x cos(Lat2)x cos(Lon1-Lon2) . )
return earthRadiusKm * angle;
}
지점 1 ( 37.359437, 127.105278) 지점 2(37.384857, 127.123429)
http://boulter.com/gps/distance/
http://www.gpsvisualizer.com/calculators
아두이노 프로그램
#define PI 3.14159265358979
float p1[2]={37.359437, 127.105278};
float p2[2]={37.384857, 127.123429};
float degreesToRadians(float degrees) {
return degrees *(PI/180.); // () 가 매우 중요
}
float distanceInKmBetweenEarthCoordinates(float Lat1,float Lon1,float Lat2,float Lon2) {
float earthRadiusKm = 6371.0;
float angle;
Lon1 = degreesToRadians(Lon1);
Lat1 = degreesToRadians(Lat1);
Lon2 = degreesToRadians(Lon2);
Lat2 = degreesToRadians(Lat2);
angle= acos( sin(Lat1) * sin(Lat2) + cos(Lat1) * cos(Lat2) * cos(Lon1-Lon2) );
return earthRadiusKm * angle;
}
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
float distance;
distance=distanceInKmBetweenEarthCoordinates(p1[0],p1[1],p2[0],p2[1]);
Serial.println(distance,4);
}
void loop() {
// put your main code here, to run repeatedly:
}
계산 결과
3.1108
Vincenty solutions of geodesics on the ellipsoid
삼각함수 방식은 두 지점간의 거리가 가까운 경우 각도차이가 너무나 작기 때문에 100m 이상의 상당히 큰 오차를 발생 시킨다. 1976년에 vinventy가 연구한 방식을 사용해 보자 아래는 이론적 설명이 나와 있는 논문이다.
https://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
잘 설명된 웹페이지
https://www.movable-type.co.uk/scripts/latlong-vincenty.html
java로 구현한 사이트
ftp://ftp.keck.hawaii.edu/pub/ltcs/Mar06release_P16/vincenty.c
bearing 각이 initial 과 final 두개를 구하는 이유이다
http://www.onlineconversion.com/map_greatcircle_bearings.htm
#define PI 3.14159265358979
float degreesToRadians(float degrees) {
return degrees *(PI/180.); // () 가 매우 중요
}
float radiansToDegrees(float radians) {
return radians *(180./PI); // () 가 매우 중요
}
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* Vincenty Inverse Solution of Geodesics on the Ellipsoid (c) Chris Veness 2002-2010 */
/* */
/* from: Vincenty inverse formula - T Vincenty, "Direct and Inverse Solutions of Geodesics on the */
/* Ellipsoid with application of nested equations", Survey Review, vol XXII no 176, 1975 */
/* http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/**
* Calculates geodetic distance between two points specified by latitude/longitude using
* Vincenty inverse formula for ellipsoids
*
* @param {Number} lat1, lon1: first point in decimal degrees
* @param {Number} lat2, lon2: second point in decimal degrees
* @returns (Number} distance in metres between points
*/
int distVincenty(float point1[],float point2[],float results[]){
float lat1=point1[0];float lon1=point1[1];
float lat2=point2[0];float lon2=point2[1];
float a = 6378137, b = 6356752.314245, f = 1/298.257223563; // WGS-84 ellipsoid params
float L = degreesToRadians((lon2-lon1));
float U1 = atan((1-f) * tan(degreesToRadians(lat1)));
float U2 = atan((1-f) * tan(degreesToRadians(lat2)));
float sinU1 = sin(U1), cosU1 = cos(U1);
float sinU2 = sin(U2), cosU2 = cos(U2);
float cosSqAlpha,cos2SigmaM,sinSigma,sinLambda,cosLambda,cosSigma,sigma ;
float lambda = L, lambdaP, iterLimit = 100;
do {
sinLambda = sin(lambda);
cosLambda = cos(lambda);
sinSigma = sqrt((cosU2*sinLambda) * (cosU2*sinLambda) +
(cosU1*sinU2-sinU1*cosU2*cosLambda) * (cosU1*sinU2-sinU1*cosU2*cosLambda));
if (sinSigma==0) return 0; // co-incident points
cosSigma = sinU1*sinU2 + cosU1*cosU2*cosLambda;
sigma = atan2(sinSigma, cosSigma);
float sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
cosSqAlpha = 1 - sinAlpha*sinAlpha;
cos2SigmaM = cosSigma - 2*sinU1*sinU2/cosSqAlpha;
if (isnan(cos2SigmaM)) cos2SigmaM = 0; // equatorial line: cosSqAlpha=0 (§6)
float C = (f/16)*cosSqAlpha*(4+f*(4-3*cosSqAlpha));
lambdaP = lambda;
lambda = L + (1-C) * f * sinAlpha *
(sigma + C*sinSigma*(cos2SigmaM+C*cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)));
} while ((fabs(lambda-lambdaP) > 1e-12) && (--iterLimit > 0));
if (iterLimit==0) return 0; // formula failed to converge
float uSq = cosSqAlpha * (a*a - b*b) / (b*b);
float A = 1 + uSq/16384*(4096+uSq*(-768+uSq*(320-175*uSq)));
float B = uSq/1024 * (256+uSq*(-128+uSq*(74-47*uSq)));
float deltaSigma = B*sinSigma*(cos2SigmaM+B/4*(cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)-
B/6*cos2SigmaM*(-3+4*sinSigma*sinSigma)*(-3+4*cos2SigmaM*cos2SigmaM)));
float s = b*A*(sigma-deltaSigma);
//s = s.toFixed(3); // round to 1mm precision
results[0]= s;
// note: to return initial/final bearings in addition to distance, use something like:
float fwdAz = atan2(cosU2*sinLambda, cosU1*sinU2-sinU1*cosU2*cosLambda);
float revAz = atan2(cosU1*sinLambda, -sinU1*cosU2+cosU1*sinU2*cosLambda);
results[1]=radiansToDegrees(fwdAz);
results[2]=radiansToDegrees(revAz);
//return { distance: s, initialBearing: fwdAz.toDeg(), finalBearing: revAz.toDeg() };
return 1;
}
/**************************************************************************
* Module: vincenty (direct).
*
* Calculate WGS 84 destination given starting lat/long (degrees),
* bearing (degrees) & distance (Meters).
*
* from: Vincenty direct formula - T Vincenty, "Direct and Inverse
* Solutions of Geodesics on the Ellipsoid with application of
* nested equations", Survey Review, vol XXII no 176, 1975
* http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
*
* Ported from web java script implementation. Code standard is a bit
* odd, but it's efficient and closely parallels the alg equations.
*
* Doug Summers Nov 2010
*
************************************************************/
void destVincenty(float point1[],float dest[],float bearing, float dist){
//double lat1, double lon1, double bearing, double dist,
// double *lat2out, double *lon2out)
// WGS-84 ellipsiod
float a=6378137.0, b=6356752.3142, f=1/298.257223563;
float alpha1,sinAlpha, sinAlpha1, cosAlpha1, cosSqAlpha;
float sigma, sigma1, cos2SigmaM, sinSigma, cosSigma, deltaSigma, sigmaP;
float tanU1, cosU1, sinU1, uSq;
float A, B, C, L, lambda;
float tmp, lat2;
float revAz; /* unused but retained for alg completeness */
float lat1=point1[0],lon1=point1[1];
/* code body */
alpha1 = degreesToRadians(bearing);
sinAlpha1 = sin(alpha1);
cosAlpha1 = cos(alpha1);
tanU1 = (1-f) * tan(degreesToRadians(lat1));
cosU1 = 1 / sqrt((1 + tanU1*tanU1));
sinU1 = tanU1*cosU1;
sigma1 = atan2(tanU1, cosAlpha1);
sinAlpha = cosU1 * sinAlpha1;
cosSqAlpha = 1 - sinAlpha*sinAlpha;
uSq = cosSqAlpha * (a*a - b*b) / (b*b);
A = 1 + uSq/16384*(4096+uSq*(-768+uSq*(320-175*uSq)));
B = uSq/1024 * (256+uSq*(-128+uSq*(74-47*uSq)));
sigma = dist / (b*A);
sigmaP = 2*PI;
while (fabs(sigma-sigmaP) > 1e-12) {
cos2SigmaM = cos(2*sigma1 + sigma);
sinSigma = sin(sigma);
cosSigma = cos(sigma);
deltaSigma = B*sinSigma*(cos2SigmaM+B/4*(cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)-
B/6*cos2SigmaM*(-3+4*sinSigma*sinSigma)*(-3+4*cos2SigmaM*cos2SigmaM)));
sigmaP = sigma;
sigma = dist / (b*A) + deltaSigma;
}
tmp = sinU1*sinSigma - cosU1*cosSigma*cosAlpha1;
lat2 = atan2(sinU1*cosSigma + cosU1*sinSigma*cosAlpha1,
(1-f)*sqrt(sinAlpha*sinAlpha + tmp*tmp));
lambda = atan2(sinSigma*sinAlpha1,
cosU1*cosSigma - sinU1*sinSigma*cosAlpha1);
C = f/16*cosSqAlpha*(4+f*(4-3*cosSqAlpha));
L = lambda - (1-C)*f*sinAlpha*(sigma+C*sinSigma*(cos2SigmaM+C*cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)));
// final bearing
dest[2] = atan2(sinAlpha, -tmp);
// algorithm convention uses Deg outputs
dest[0] = radiansToDegrees(lat2);
dest[1] = lon1+(radiansToDegrees(L));
}
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
float p1[2]={40.4268903,-3.7117107}; //spain madrid
//float p1[2]={37.359437, 127.105278};
float p2[2]={37.384857, 127.123429};
float dest[3]; //[lat,lon,finalBearing]
void setup() {
Serial.begin(9600);
// put your setup code here, to run once:
float results[3];
Serial.println("Start Vincenty Inverse algoritm");
if((distVincenty(p1,p2,results)) ==1){
Serial.print("Distance(m):");
Serial.println(results[0]);
Serial.print("Init Bearing(deg):");
Serial.println(results[1]);
Serial.print("final Bearing(deg):");
Serial.println(results[2]);
}
Serial.println("Start Vincenty Destination algoritm");
destVincenty(p1,dest,results[1],results[0]);
Serial.print("latitude:");
Serial.println(dest[0],6);
Serial.print("Longitude:");
Serial.println(dest[1],6);
Serial.print("Final Bearing:");
Serial.println(dest[2]);
}
void loop() {
// put your main code here, to run repeatedly:
}
결과 - 각각 5ms 정도 소요됨(uno기준)
Start Vincenty Inverse algoritm
Distance(m):3247.57
Init Bearing(deg):29.67
final Bearing(deg):29.68
Start Vincenty Destination algoritm
latitude:37.384860
Longitude:127.123428
Final Bearing:0.52
여기도 쓸만한것 같음 NeoGPS에서 사용함