GPS速度和航向计算

用法说明：

输入：两个时间点的经纬度坐标：(lat1, lon1), (lat2, lon2)

因海面应用，设定高程为0，
则两个时间点的大地坐标为(lat1, lon1, 0), (lat2, lon2, 0)：类型COORDBLH成员B－纬度，L－经度，H－高程
1.使用cc_ecef_land2right将(lat2, lon2, 0)转换为空间直角坐标(x2, y2, z2)，坐标系参数使用宏DECL_CSPARA_ARRAY_ELEM声明，宏参数a=6378137，f=1.0/298.257223563,omgedot=7.2921151467e-5
2.以(lat1, lon1)为计算基准位置，作为ccrc_ecef2tccs的lon、lat参数，将(x2, y2, z2)转换为站心坐标系(de, dn, du)
3.航速＝sqrt(de^2 + dn^2)/(t2-t1)，

4.真北航向＝arctan2(de, dn)，若结果<0，则在结果上加上360度

用法示例：

#define MAX_LIMITED_LAT 79.9
#define MIN_LIMITED_LAT -79.9
#define A_Earth 6378137
#define e_Earth 0.08181919104281579


const static double lfA1=1+3./4*pow(e_Earth,2)+45./64*pow(e_Earth,4)+175./256*pow(e_Earth,6)+11025./16384*pow(e_Earth,8);
const static double lfA2=-3./8*pow(e_Earth,2)-15./32*pow(e_Earth,4)-525./1024*pow(e_Earth,6)-2205./4096*pow(e_Earth,8);
const static double lfA3=15./256*pow(e_Earth,4)+105./1024*pow(e_Earth,6)+2205./16384*pow(e_Earth,8);
const static double lfA4=-35./3072*pow(e_Earth,6)-105./4096*pow(e_Earth,8);
const static double lfA5=315./131072*pow(e_Earth,8);


const static double lfB1=3./8*pow(e_Earth,2)+3./16*pow(e_Earth,4)+213./2048*pow(e_Earth,6)+255./4096*pow(e_Earth,8);
const static double lfB2=21./256*pow(e_Earth,4)+21./256*pow(e_Earth,6)+533./8192*pow(e_Earth,8);
const static double lfB3=151./6144*pow(e_Earth,6)+151./4096*pow(e_Earth,8);
const static double lfB4=1097./131082*pow(e_Earth,8);
void GetDistAndAngleBetweenTwoPointForExact(double lfBeginLat,double lfBeginLon,double lfEndLat,double lfEndLon,double& lfOrient,double& lfDist)
{


double lfLon1,lfLon2;
// 判断值的有效性
if(lfBeginLat<-90||lfBeginLat>90||lfBeginLon<=-180||lfBeginLon>=360)
{
return ;
}
if(lfEndLat<-90||lfEndLat>90||lfEndLon<=-180||lfEndLon>=360)
{
return ;
}
if(fabs(lfBeginLat-lfEndLat)<1.0e-8&&fabs(lfBeginLon-lfEndLon)<1.0e-8)
{
lfDist=0;
lfOrient=0;
return ;
}

//转换为0-360
if(lfBeginLon<0)
{
lfLon1=360+lfBeginLon;
}
else
{
lfLon1=lfBeginLon;
}
if(lfEndLon<0)
{
lfLon2=360+lfEndLon;
}
else
{
lfLon2=lfEndLon;
}
if(fabs(lfLon1-lfLon2)>180)
{
//第一个点是0-180
if(lfLon1<180)
{
//确保第一个点是0-180，第二个点是-180到0
lfBeginLon=lfLon1;
lfEndLon=lfLon2-360;
}
//第一个点是180-360
else
{
//确保第二个点是0-180，第一个点是-180到0
lfBeginLon=lfLon1-360;
lfEndLon=lfLon2;
}
}
else
{
lfBeginLon=lfLon1;
lfEndLon=lfLon2;
}
if(fabs(lfBeginLat-lfEndLat)<=0.000001)
{
lfDist=(A_Earth*fabs(DTOR(lfEndLon)-DTOR(lfBeginLon))*cos(DTOR(lfBeginLat)))/(pow(1-e_Earth*e_Earth*pow(sin(DTOR(lfBeginLat)),2),0.5));
lfDist/=1852;
if(lfEndLon>lfBeginLon)
{
lfOrient=90;
}
else
{
lfOrient=270;
}
return ;
}
double lfX1=A_Earth*(1-e_Earth*e_Earth)*(lfA1*DTOR(lfBeginLat)+lfA2*sin(DTOR(2*lfBeginLat))+lfA3*sin(DTOR(4*lfBeginLat))+lfA4*sin(DTOR(6*lfBeginLat))+lfA5*sin(DTOR(8*lfBeginLat)));
double lfX2=A_Earth*(1-e_Earth*e_Earth)*(lfA1*DTOR(lfEndLat)+lfA2*sin(DTOR(2*lfEndLat))+lfA3*sin(DTOR(4*lfEndLat))+lfA4*sin(DTOR(6*lfEndLat))+lfA5*sin(DTOR(8*lfEndLat)));
double lfSin1=sin(DTOR(lfBeginLat));
double lfSin2=sin(DTOR(lfEndLat));
double lfQ1=log( (1+lfSin1) / ((1-lfSin1) ))/2 -e_Earth*log( (1+lfSin1*e_Earth) / ((1-lfSin1*e_Earth) ))/2;
double lfQ2=log( (1+lfSin2) / ((1-lfSin2)))/2-e_Earth*log( (1+lfSin2*e_Earth) / ((1-lfSin2*e_Earth)))/2;
if(lfBeginLat==lfEndLat)
{
if(lfEndLon>lfBeginLon)
{
lfOrient=PI/2;
}
else
{
lfOrient=3*PI/2;
}
}
else if(lfEndLat>lfBeginLat)
{
if(lfEndLon>=lfBeginLon)
{
lfOrient=atan(DTOR((lfEndLon-lfBeginLon)/(lfQ2-lfQ1)));
}
else
{
lfOrient=2*PI+atan(DTOR((lfEndLon-lfBeginLon)/(lfQ2-lfQ1)));
}
}
else
{
lfOrient=PI+atan(DTOR((lfEndLon-lfBeginLon)/(lfQ2-lfQ1)));
}
lfDist=(lfX2-lfX1)/cos(lfOrient);
lfOrient=RTOD(lfOrient);
lfDist/=1852;
}
// 功  能：已知起点经纬度和航向（方位）、航程（距离），求终点经纬度
// 输入值：起点经纬度和航向（方位）、航程（距离）
// 返回值：终点经纬度(引用形式)
void CalAPointByDistAndAngleForExact(double lfBeginLat,double lfBeginLon,double dOrient,
double lfDist,double& lfEndLat,double& lfEndLon)
{
lfDist=lfDist*1852;
//当90度时时特殊情况
if(fabs(dOrient-90)<=0.0001||fabs(dOrient-270)<=0.0001)
{
lfEndLat=lfBeginLat;
double lfLonDist;
lfLonDist=(pow(1-e_Earth*e_Earth*pow(sin(DTOR(lfBeginLat)),2),0.5)*lfDist)/(A_Earth*cos(DTOR(lfBeginLat)));

if(fabs(dOrient-90)<=0.000001)
{
lfEndLon=RTOD(DTOR(lfBeginLon)+lfLonDist);
}
else
{
lfEndLon=RTOD(DTOR(lfBeginLon)-lfLonDist);
}
return ;
}
else
{
double lfX1=A_Earth*(1-e_Earth*e_Earth)*(lfA1*DTOR(lfBeginLat)+lfA2*sin(DTOR(2*lfBeginLat))+lfA3*sin(DTOR(4*lfBeginLat))+lfA4*sin(DTOR(6*lfBeginLat))+lfA5*sin(DTOR(8*lfBeginLat)));
double lfX2=lfX1+lfDist*cos(DTOR(dOrient));
double lfB0=lfX2/((1-e_Earth*e_Earth)*A_Earth*lfA1);

lfEndLat=(lfB0)+lfB1*sin((2*lfB0))+lfB2*sin((4*lfB0))+lfB3*sin((6*lfB0))+lfB4*sin((8*lfB0));
lfEndLat=RTOD(lfEndLat);
double lfSin1=sin(DTOR(lfBeginLat));
double lfSin2=sin(DTOR(lfEndLat));
double lfQ1=log( (1+lfSin1) / ((1-lfSin1) ))/2 -e_Earth*log( (1+lfSin1*e_Earth) / ((1-lfSin1*e_Earth) ))/2;
double lfQ2=log( (1+lfSin2) / ((1-lfSin2)))/2-e_Earth*log( (1+lfSin2*e_Earth) / ((1-lfSin2*e_Earth)))/2;
lfEndLon=DTOR(lfBeginLon)+(lfQ2-lfQ1)*tan(DTOR(dOrient));
lfEndLon=RTOD(lfEndLon);
}
}

资源下载地址：https://download.csdn.net/download/gaojy19881225/10365946