大地坐标系和地心地固直角坐标ECEF转换公式和C语言函数代码

最新推荐文章于 2024-08-17 20:47:40 发布

yx-1230

最新推荐文章于 2024-08-17 20:47:40 发布

阅读量4.3k

点赞数 4

文章标签： GNSS卫星导航定位 RTK

本文链接：https://blog.csdn.net/qyx635804080/article/details/87907119

版权

ECEF（x,y,z）转成大地坐标系（φ,λ,h）
λ=tan^(-1)⁡〖(y/x)〗
h=p/cos⁡∅ -N
∅=tan^(-1)⁡〖[z/p 〖(1-e^2 N/(N+h))〗^(-1)]〗
p=√(x^2+y2 )

#define RE_WGS84    6378137.0           /* earth semimajor axis (WGS84) (m) */
#define FE_WGS84    (1.0/298.257223563) /* earth flattening (WGS84) */
#define PI          3.1415926535897932  /* pi */
/* inner product ---------------------------------------------------------------
* inner product of vectors
* args   : double *a,*b     I   vector a,b (n x 1)
*          int    n         I   size of vector a,b
* return : a'*b
*-----------------------------------------------------------------------------*/
extern double dot(const double *a, const double *b, int n)
{
    volatile double c=0.0000;
    
    while (--n>=0) c+=a[n]*b[n];
    return c;
}
/* transform ecef to geodetic postion ------------------------------------------
* transform ecef position to geodetic position
* args   : double *r        I   ecef position {x,y,z} (m)
*          double *pos      O   geodetic position {lat,lon,h} (rad,m)
* return : none
* notes  : WGS84, ellipsoidal height
*-----------------------------------------------------------------------------*/

extern void ecef2pos(const double *r, double *pos)
{
    double e2=FE_WGS84*(2.0-FE_WGS84),r2=dot(r,r,2),z,zk,v=RE_WGS84,sinp;
    
    for (z=r[2],zk=0.0;fabs(z-zk)>=1E-4;) {
        zk=z;
        sinp=z/sqrt(r2+z*z);
        v=RE_WGS84/sqrt(1.0-e2*sinp*sinp);
        z=r[2]+v*e2*sinp;
    }
    pos[0]=r2>1E-12?atan(z/sqrt(r2)):(r[2]>0.0?PI/2.0:-PI/2.0);
    pos[1]=r2>1E-12?atan2(r[1],r[0]):0.0;
    pos[2]=sqrt(r2+z*z)-v;
}