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

GPS速度和航向计算相关推荐

GPS卫星位置的计算
1. GPS卫星位置的计算 1.1 用广播星历计算卫星位置 1.1.1. 计算卫星运动的平均角速度 nnn 首先根据广播星历中给出的参数 A\sqrt{A}A 计算参考时刻 toet_{\mathr ...
基于GPS与经纬度距离计算
基于GPS与经纬度距离计算 # -*- coding:utf-8 -*- # /usr/bin/pythonimport warnings warnings.filterwarnings(" ...
本实战将实现根据当天的总步数，计算消耗的热量值。由于行走速度不同，计算卡路里的消耗也不同，这里假设走一步消耗28卡路里。实现效果如图4所示
本实战将实现根据当天的总步数,计算消耗的热量值.由于行走速度不同,计算卡路里的消耗也不同,这里假设走一步消耗28卡路里.实现效果如图4所示 x=int(input("请输入当天的步数:\n& ...
GPS卫星坐标用计算机计算,GPS卫星位置的计算
[摘要]针对GPS卫星导航电文的特点和里边各种参数含义及用法,阐述了利用导航电文提供的数据计算卫星位置的方法,为进行基线解算时提供准确的卫星坐标数据. 0前言全球卫星定位系统(GPS)是一个实时的 ...
html GPS坐标实现,JavaScript 实现GPS坐标点距离计算（两个经/纬度间的距离计算）...
在LBS(基于位置服务)的一些应用中,有时我们会需要计算两个用户或两个坐标点之间的距离.要解决这类问题,就要了解空间几何的概念并结合数学中在三角函数公式计算两点之间的值.本文介绍基于经度/纬度的,两个 ...
GPS网的基线计算以及平差处理（使用Python绘制误差椭圆，C++处理数据并输出Qxx矩阵）
题目如下: 如下图所示为一简单GPS网,用两台GPS接收机观测,测得5条基线向量,每一条基线向量中三个坐标差观测值相关,由于只用两台GPS接收机观测,所以各观测基线向量互相独立.观测基线向量信息见表1 ...
c语言乘法计算速度,C++ 基本计算的速度
前言 ACM 时,曾经测试过 Lutece 的速度并记录在这篇博客,毕竟 OJ 运算速度直接关系到了每一次写的代码的复杂度. 脱坑 ACM 以后,在编程珠玑里面看到了自己的电脑的运算速度应该作为常识记 ...
加速度速度位移的计算
加速度g: 1个g其近似标准值通常取为980厘米/秒的二次方或9.8米/秒的二次方. 1.S=Vot+1/2at² = 1/2(V0+Vt) 2.Vt = V0 + at; 固 ...
GPS坐标间距离计算
# -*- coding:utf-8 -*- # /usr/bin/pythonimport warnings warnings.filterwarnings("ignore") ...

GPS速度和航向计算

GPS速度和航向计算相关推荐

最新文章

热门文章