Определение расстояние по координатам GPS, Необходим алгоритм Опции

[Newegor]

Есть задача определения пробега авто по координатам получаемым с GPS модуля по протоколу NMEA. Все работает, вот только осталось сделать подсчет расстояния. Какие есть алгоритмы. Сейчас считаю очень просто по теореме Пифагора, но это для маленьких расстояний подходит, плюс еще не определился как исключить погрешность определения координат, когда авто стоит. Они прыгают иногда и на десятки метров, а авто в это время стоит. В общем поиском по инету ничего путного не нашел (может и плохо искал). Если есть какие-нибудь стандартные методики, то буду благодарен за них.

[alexQ]

да ... 3й класс.. вторая четверть ...


/********************************************************************************
***
* Рассчитываем дистанцию (между точками 1 и 2) и отклонение *
*(линия 1-2 относительно севера). navcalc() *
********************************************************************************
****/
#define RAD_CONVERT 57.300000000
#define KMETERS_CONVERT 3437.7387000*0.62500000*4.96728972 //коэффициент преобразования в километры
void navcalc(void)
{
long double t1,t2,t3,t4,t5;
long double Lat1,Lon1,Lat2,Lon2;
long double rad_dist,dist, rad_bearing,bearing;
/*****************************
* Начальные переменные в ноль*
******************************/
t1 = t2 = t3 = t4 = t5 = dist = bearing = 0.0000000;
/************************************************************
* Задаем коориданаты двух точек (тест)
*************************************************************/
//1063 meters 30,5град истинный угол
Lat2=59.42811; //Эрмитаж
Lon2=30.24195;
Lat1=59.42318; // собор
Lon1=30.23625;
/**********************************
* Преобразуем координаты в радианы*
***********************************/
Lat1 = Lat1 / RAD_CONVERT;
Lon1 = Lon1 / RAD_CONVERT;
Lat2 = Lat2 / RAD_CONVERT;
Lon2 = Lon2 / RAD_CONVERT;
t1 = sin(Lat1) * sin(Lat2);
t2 = cos(Lat1) * cos(Lat2);
t3 = cos(Lon1 - Lon2);
t4 = t2 * t3;
t5 = t1 + t4;
rad_dist = atan(-t5 / sqrt(-t5 * t5 + 1.0000))+2.0000* atan(1.0000);
/*****************************************
* Преобразуем в километры (метры) *
******************************************/
dist = rad_dist * KMETERS_CONVERT;
// send_decimal_uart(dist*1000); putchar(' '); //метры
send_decimal_uart(dist); putchar(' '); //километры
/**************************************************************************
* Рассчитываем отклонение
* b = acos(sin(Lat2) - sin(Lat1)* cos(rad_dist)/sin(rad_dist) * cos(lat1));
**************************************************************************/
if(sin(Lon2 - Lon1) < 0.0000)
{
t1 = sin(Lat2) - sin(Lat1) * cos(rad_dist);
t2 = cos(Lat1) * sin(rad_dist);
t3 = t1 / t2;
t4 = atan(-t3 / sqrt(-t3 * t3 + 1.0000)) + 2.0000 * atan(1.000);
rad_bearing = t4;
}
else
{
t1 = sin(Lat2) - sin(Lat1) * cos(rad_dist);
t2 = cos(Lat1) * sin(rad_dist);
t3 = t1 / t2;
t4 = -t3 * t3 + 1.0000;
t5 = 2.0000 * 3.1415926535 - (atan(-t3 / sqrt(-t3 * t3 + 1.0000)) + 2.0000 * atan(1.0000));
rad_bearing = t5;
}
/***********************************
* Преобразуем радианы в градусы *
************************************/
bearing = rad_bearing * RAD_CONVERT;
// Отклонение в градусах
send_decimal_uart(bearing);putchar(' ');
return;
}

[AndrewGT]

//v - скорость из VTG, узел = 1852 м/час
S += v*dt
if(v<V0) v = 0; // V0 поставьте, скажем 0.2 м/с