ecef和机体坐标系_WGS84和ECEF坐标的转换

#include "stdafx.h"

#include

#define  PI  3.141592653

/*

该程序根据GPS.G1-X-00006.pdf文档，实现了WGS84和ECEF坐标的转换

*/

void LLAtoECEF(double latitude, double longitude, double height, double &X, double &Y, double &Z)

{

double a = 6378137;

double b = 6356752.314245;// b=(1-f)

double E = (a*a - b*b)/(a*a);

double COSLAT = cos(latitude*PI/180);

double SINLAT = sin(latitude*PI/180);

double COSLONG = cos(longitude*PI/180);

double SINLONG = sin(longitude*PI/180);

double N = a /(sqrt(1 - E*SINLAT*SINLAT));

double NH = N + height;

X = NH * COSLAT * COSLONG;

Y = NH * COSLAT * SINLONG;

Z = (b*b*N/(a*a) + height) * SINLAT;

}

void ECEFtoLLA(int lx, int ly, int lz)

{

doubl