GPS经纬度转84坐标系

    一般来讲，GPS直接提供的坐标（B,L,H）是1984年世界大地坐标系(Word Geodetic System 1984即WGS-84)的坐标，其中B为纬度，L为经度，H为大地高即是到WGS-84椭球面的高度。而在实际应用中，我国地图采用的是1954北京坐标系或者1980西安坐标系下的高斯投影坐标（x,y,），不过也有一些电子地图采用1954北京坐标系或者1980西安坐标系下的经纬度坐标（B,L），高程一般为海拔高度h。

    GPS的测量结果与我国的54系或80系坐标相差几十米至一百多米，随区域不同，差别也不同，经粗落统计，我国西部相差70米左右，东北部140米左右，南部75米左右，中部45米左右。

    1984世界大地坐标系WGS-84坐标系是美国国防部研制确定的大地坐标系，是一种协议地球坐标系。WGS-84坐标系的定义是：原点是地球的质心，空间直角坐标系的Z轴指向BIH（1984.0）定义的地极（CTP）方向，即国际协议原点CIO，它由IAU和IUGG共同推荐。X轴指向BIH定义的零度子午面和CTP赤道的交点，Y轴和Z，X轴构成右手坐标系。WGS-84椭球采用国际大地测量与地球物理联合会第17届大会测量常数推荐值，采用的两个常用基本几何参数： 长半轴a=6378137m；扁率f=1:298.257223563。

GPS.cpp

#include 
#include 
#include "gps.h"


void GeodeticToCartesian (PCRDCARTESIAN &pcc, PCRDGEODETIC &pcg)
{
        double B;        //纬度度数
        double L;        //经度度数
        double L0;        //中央经线度数
        double l;        //L-L0
        double t;        //tanB
        double m;        //ltanB
        double N;        //卯酉圈曲率半径 
        double q2;
        double x;        //高斯平面纵坐标
        double y;        //高斯平面横坐标
        double s;        //赤道至纬度B的经线弧长
        double f;        //参考椭球体扁率
        double e2;        //椭球第一偏心率
        double a;        //参考椭球体长半轴
        //double b;        //参考椭球体短半轴
        double a1;
        double a2;
        double a3;
        double a4;
        double b1;
        double b2;
	double b3;
	double b4;
        double c0;
        double c1;
        double c2;
        double c3;
        int Datum=84;        //投影基准面类型：北京54基准面为54，西安80基准面为80，WGS84基准面为84
        int prjno=0;        //投影带号
        int zonewide=3;        
        double IPI=0.0174532925199433333333;        //3.1415926535898/180.0
        B=pcg.latitude ; //纬度
        L=pcg.longitude ; //经度
        if (zonewide==6) 
        {
                 prjno=(int)(L/zonewide)+1;
                 L0=prjno*zonewide-3;
        }
        else
        {
                prjno=(int)((L-1.5)/3)+1;
                L0=prjno*3;
        }
        
        if(Datum==54)
        {
                 a=6378245;
                 f=1/298.3;
        }        
        else if(Datum==84)
        {
                a=6378137;
                f=1/298.257223563;
        }
        L0=L0*IPI;
        L=L*IPI;
        B=B*IPI;

        e2=2*f-f*f;//(a*a-b*b)/(a*a);
        l=L-L0;
        t=tan(B);
        m=l * cos(B);
        N=a/sqrt(1-e2* sin(B) * sin(B));
        q2=e2/(1-e2)* cos(B)* cos(B);
        a1=1+(double)3/4*e2+(double)45/64*e2*e2+(double)175/256*e2*e2*e2+(double)11025/16384*e2*e2*e2*e2+(double)43659/65536*e2*e2*e2*e2*e2;
        a2=(double)3/4*e2+(double)15/16*e2*e2+(double)525/512*e2*e2*e2+(double)2205/2048*e2*e2*e2*e2+(double)72765/65536*e2*e2*e2*e2*e2;
        a3=(double)15/64*e2*e2+(double)105/256*e2*e2*e2+(double)2205/4096*e2*e2*e2*e2+(double)10359/16384*e2*e2*e2*e2*e2;
        a4=(double)35/512*e2*e2*e2+(double)315/2048*e2*e2*e2*e2+(double)31185/13072*e2*e2*e2*e2*e2;
        b1=a1*a*(1-e2);
        b2=(double)-1/2*a2*a*(1-e2);
        b3=(double)1/4*a3*a*(1-e2);
        b4=(double)-1/6*a4*a*(1-e2);
        c0=b1;
        c1=2*b2+4*b3+6*b4;
        c2=-(8*b3+32*b4);
        c3=32*b4;
        s=c0*B+cos(B)*(c1*sin(B)+c2*sin(B)*sin(B)*sin(B)+c3*sin(B)*sin(B)*sin(B)*sin(B)*sin(B));
        x=s+(double)1/2*N*t*m*m+(double)1/24*(5-t*t+9*q2+4*q2*q2)*N*t*m*m*m*m+(double)1/720*(61-58*t*t+t*t*t*t)*N*t*m*m*m*m*m*m;
        y=N*m+(double)1/6*(1-t*t+q2)*N*m*m*m+(double)1/120*(5-18*t*t+t*t*t*t-14*q2-58*q2*t*t)*N*m*m*m*m*m;
        y=y+1000000*prjno+500000;
        pcc.x=x;
        pcc.y=y-38000000;
        pcc.z=pcg.height;

}

GPS.h

#ifndef GPS_H
#define GPS_H

//笛卡尔坐标系
typedef struct tagCRDCARTESIAN{
double x;
double y;
double z;
}PCRDCARTESIAN;

//大地坐标系
typedef struct tagCRDGEODETIC{
double longitude; //经度
double latitude;  //纬度 
double height;    //大地高,可设为0
}PCRDGEODETIC;


void GeodeticToCartesian (PCRDCARTESIAN &pcc, PCRDGEODETIC &pcg);

#endif

关于调用：

  //初始位置
  PCRDCARTESIAN s_x_y;
  PCRDGEODETIC s_l_l;
  s_l_l.latitude = light_data.begin()->imu_data[0];	
  s_l_l.longitude = light_data.begin()->imu_data[1];
  GeodeticToCartesian(s_x_y, s_l_l);
  //得到相对于初始状态的位移值，单位m
  for(auto it_light = light_data.begin(); it_light != light_data.end(); it_light++)
  {
    PCRDCARTESIAN x_y;
    PCRDGEODETIC l_l;
    
    l_l.latitude = it_light->imu_data[0];
    l_l.longitude = it_light->imu_data[1];
    GeodeticToCartesian(x_y, l_l);
    //与自己的坐标系相对应
    it_light->p_y = x_y.x - s_x_y.x;
    it_light->p_x = x_y.y - s_x_y.y;
  }

通过以上的转换，就可以为里程计提供初始定位信息，提高配准成功率