Така или иначе си я пуснал темата, да добавя малко и аз, защото на мен ми е интересна. Първо малко относно точността на изчисленията. Единственият линеен размер в сферичните координати е радиуса на земята r = 6371300 m, някъде съм го срещал и като r = 6378100 m. Която и цифра да се вземи грешката е в рамките на сантиметри за разстояния до 50 метра. Другите координати са ъглови, които се получават от GPS, т.е. северната ширина и дължината. Най-краткото разстояние между две точки, винаги минава през голямата окръжност на сферата. Дължината на дъгата по голямата окръжност (т.е. разстоянието между двете точки) е равна на d = r*phi, и сега за да имаме точност от порядъка на един метър ъгълът трябва да бъде отчетен с точност около 1/r=1x10-5 в градуси или 1.6x10-7 в радиани. Тези числа са на границата на точност на 32 битово число с плаваща запетая (float), според мен е добре да се работи с 64 битово число (double). Всякакъв опит да се направи библиотека, която може да използва 16 битово число, с цел пестене на време е безсмислена, защото това ще доведе до сериозни грешки.
Сега малко за функцията haversine, която се използва за намиране на разстоянието между две точки. Haversine не е име на човек, а име на тригонометрична функция и затова е неправилно да се казва формула на haversine. Тази формула може да се опрости като се знае, че при малки разстояния разликата в ъгловите координати е малка и, че sin (θ) = θ, за малки ъгли тогава формулата става:
d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + cos(phi1) * cos(phi2) * pow((l2 - l1) / 2, 2))
Може да се въведе и още едно опростяване свързано с косинусите от близки ъгли, което пак важи за малки разстояния ( тук под малки разстояние разбирам от порядъка 20, до 30 км), cos(phi1) * cos(phi2) = (cos((phi2 + phi1)/2)^2. Тогава формулата ще изглежда така:
d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + pow(cos((phi2 + phi1)/2), 2) * pow((l2 - l1) / 2, 2)).
Вижда се, че от 5 тригонометрични функции остава само една + още един квадрат.
За да се разбере предимството на опростяването съм направил няколко опита с различни контролери в Адруино среда
Първата група опити е свързана с разстояние 50.4 м, ширината на един магазин с координати в първата точка с.ш. = 43.418968, и.д. = 24.626770, и втора точка с.ш. = 43.419409, и.д.=24.626914.
Контролер Време, [μs] Разстояние, [m]
1 2 3 1 2 3
Due 194 103 73 50.4 50.4 50.4
Teensy 3.6 61 31 22 50.54 50.54 50.54
Uno 608 356 256 51.08 51.08 51.08
STM32F407ZET 67 34 25 50.4 50.4 50.4
Тук с едно 1, 2 и 3 е означено формулата която се използва, 1 е пълната формула, 2 е след първото опростяване и 3 след второто.
Няма никаква разлика във времената, ако се използва (float) вместо (double),освен при STM32F407ZET, където времената намаляват почти двойно, с изключение на третата формула, където времето остава не променено. Трябва да се има в предвид, че STM32F407ZET и Teensy 3.6 имат вградено хардуерно 32 битово FPU (float point unit).
Втората група опити е свързана с разстояние 15.79 м, ширината на една сграда с координати в първата точка с.ш. = 43.411376, и.д. = 24.627289, и втора точка с.ш. = 43.411325, и.д.=24.627470.
Контролер Време, [μs] Разстояние, [m]
1 2 3 1 2 3
Due 192 99 70 15.68 15.68 15.68
Teensy 3.6 58 30 21 15.8 15.8 15.8
Uno 600 352 256 15.82 15.82 15.82
STM32F407ZET 67 33 24 15.68 15.68 15.68
Координатите, както и разстоянията са взети от google map, нямам GPS модул.
Опростяването на формулата не води до загуба на точност, а времето за изчисление се намалява повече от два пъти.
От опитите се вижда, че Uno се справя много добре със задачата, изчислението отнема половин мили секунда, което означава, че до 10 милисекунди може да направиш още 15, 20 подобни изчисления. А 100 хертца sample rate е съвсем достатъчно за една лодка, дори и 50 хертца биха свършили работата. Мисля, че Uno като параметри и скорост би се справил с един автопилот, какъвто се търси.
Накрая искам да призова тези, които твърдят, че има по-лесна формула нека да я демонстрират, така както и аз, ще ми бъде много полезна и не само на мен.
Ето и кода ако има някой забележки относно измерването на времето, нека допълни, може и да не съм прав:
#define pi 3.14159265359
#define r 6371300 // radius
// coordinates of points in degree
// point 1
double phi1 = 43.411376;
double l1 = 24.627289;
// point 2
double phi2 = 43.411325;
double l2 = 24.627470;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
// coordinates of points in radians:
// point 1
phi1 = phi1 * pi / 180;
l1 = l1 * pi / 180;
// point 2
phi2 = phi2 * pi / 180;
l2 = l2 * pi / 180;
}
void loop() {
double time1;
time1 = micros();
// distance between points full formula
double d = 2 * r * asin(sqrt(pow(sin((phi2 - phi1) / 2), 2) + cos(phi1) * cos(phi2) * pow(sin((l2 - l1) / 2), 2)));
time1 = micros() - time1;
Serial.print(time1);
Serial.print (" ");
Serial.print(d);
Serial.println();
time1 = micros();
// distance between points first approximation
d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + cos(phi1) * cos(phi2) * pow((l2 - l1) / 2, 2));
time1 = micros() - time1;
Serial.print(time1);
Serial.print (" ");
Serial.print(d);
Serial.println();
time1 = micros();
//distance between points second approximation
d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + pow(cos((phi2 + phi1)/2), 2) * pow((l2 - l1) / 2, 2));
time1 = micros() - time1;
Serial.print(time1);
Serial.print (" ");
Serial.print(d);
Serial.println();
delay(5000);
}
