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

    原文作者:一墨一飞花
    原文地址: https://blog.csdn.net/gaojy19881225/article/details/80036938
    本文转自网络文章,转载此文章仅为分享知识,如有侵权,请联系博主进行删除。
点赞