WGS84坐标系转ENU坐标

1、第一步将WGS84坐标系转为地心坐标系

	public static double[] wgs84ToEcef(double lat, double lon, double h) {
		double a = 6378137;
		double b = 6356752.3142;
		double f = (a - b) / a;
		double e_sq = f * (2 - f);
		double lamb = Math.toRadians(lat);
		double phi = Math.toRadians(lon);
		double s = Math.sin(lamb);
		double N = a / Math.sqrt(1 - e_sq * s * s);

		double sin_lambda = Math.sin(lamb);
		double cos_lambda = Math.cos(lamb);
		double sin_phi = Math.sin(phi);
		double cos_phi = Math.cos(phi);

		double x = (h + N) * cos_lambda * cos_phi;
		double y = (h + N) * cos_lambda * sin_phi;
		double z = (h + (1 - e_sq) * N) * sin_lambda;
		return new double[]{x,y,z};
	}

2、第二步输入站点经纬度进行转换

	public static double[] ecefToEnu(double x, double y, double z, double lat, double lng, double height) {
		double a = 6378137;
		double b = 6356752.3142;
		double f = (a - b) / a;
		double e_sq = f * (2 - f);
		double lamb = Math.toRadians(lat);
		double phi = Math.toRadians(lng);
		double s = Math.sin(lamb);
		double N = a / Math.sqrt(1 - e_sq * s * s);
		double sin_lambda = Math.sin(lamb);
		double cos_lambda = Math.cos(lamb);
		double sin_phi = Math.sin(phi);
		double cos_phi = Math.cos(phi);

		double x0 = (height + N) * cos_lambda * cos_phi;
		double y0 = (height + N) * cos_lambda * sin_phi;
		double z0 = (height + (1 - e_sq) * N) * sin_lambda;

		double xd = x - x0;
		double yd = y - y0;
		double zd = z - z0;

		double t = -cos_phi * xd - sin_phi * yd;

		double xEast = -sin_phi * xd + cos_phi * yd;
		double yNorth = t * sin_lambda + cos_lambda * zd;
		double zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd;
		return new double[] { xEast, yNorth, zUp };
	}

3、测试代码

	public static void main(String[] args) {
		double[] arr1 = wgs84ToEcef(30.7335905, 103.968191, 493.080);//此处经纬度是需要比对的偏移经纬度
		ecefToEnu(arr1[0],arr1[1], arr1[2],30.7335905, 103.968191,  493.080);//此处经纬度是站点经纬度
	}