1 /* 2 * Copyright (C) 2017 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 package android.location.cts.gnss.pseudorange; 18 19 import org.apache.commons.math.linear.Array2DRowRealMatrix; 20 import org.apache.commons.math.linear.RealMatrix; 21 22 /** 23 * Converts ECEF (Earth Centered Earth Fixed) Cartesian coordinates to local ENU (East, North, 24 * and Up). 25 * 26 * <p> Source: reference from Navipedia: 27 * http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates 28 */ 29 30 public class Ecef2EnuConverter { 31 32 /** 33 * Converts a vector represented by coordinates ecefX, ecefY, ecefZ in an 34 * Earth-Centered Earth-Fixed (ECEF) Cartesian system into a vector in a 35 * local east-north-up (ENU) Cartesian system. 36 * 37 * <p> For example it can be used to rotate a speed vector or position offset vector to ENU. 38 * 39 * @param ecefX X coordinates in ECEF 40 * @param ecefY Y coordinates in ECEF 41 * @param ecefZ Z coordinates in ECEF 42 * @param refLat Latitude in Radians of the Reference Position 43 * @param refLng Longitude in Radians of the Reference Position 44 * @return the converted values in {@code EnuValues} 45 */ convertEcefToEnu(double ecefX, double ecefY, double ecefZ, double refLat, double refLng)46 public static EnuValues convertEcefToEnu(double ecefX, double ecefY, double ecefZ, 47 double refLat, double refLng){ 48 49 RealMatrix rotationMatrix = getRotationMatrix(refLat, refLng); 50 RealMatrix ecefCoordinates = new Array2DRowRealMatrix(new double[]{ecefX, ecefY, ecefZ}); 51 52 RealMatrix enuResult = rotationMatrix.multiply(ecefCoordinates); 53 return new EnuValues(enuResult.getEntry(0, 0), 54 enuResult.getEntry(1, 0), enuResult.getEntry(2 , 0)); 55 } 56 57 /** 58 * Computes a rotation matrix for converting a vector in Earth-Centered Earth-Fixed (ECEF) 59 * Cartesian system into a vector in local east-north-up (ENU) Cartesian system with respect to 60 * a reference location. The matrix has the following content: 61 * 62 * - sinLng cosLng 0 63 * - sinLat * cosLng - sinLat * sinLng cosLat 64 * cosLat * cosLng cosLat * sinLng sinLat 65 * 66 * <p> Reference: Pratap Misra and Per Enge 67 * "Global Positioning System: Signals, Measurements, and Performance" Page 137. 68 * 69 * @param refLat Latitude of reference location 70 * @param refLng Longitude of reference location 71 * @return the Ecef to Enu rotation matrix 72 */ getRotationMatrix(double refLat, double refLng)73 public static RealMatrix getRotationMatrix(double refLat, double refLng){ 74 RealMatrix rotationMatrix = new Array2DRowRealMatrix(3, 3); 75 76 // Fill in the rotation Matrix 77 rotationMatrix.setEntry(0, 0, -1 * Math.sin(refLng)); 78 rotationMatrix.setEntry(1, 0, -1 * Math.cos(refLng) * Math.sin(refLat)); 79 rotationMatrix.setEntry(2, 0, Math.cos(refLng) * Math.cos(refLat)); 80 rotationMatrix.setEntry(0, 1, Math.cos(refLng)); 81 rotationMatrix.setEntry(1, 1, -1 * Math.sin(refLat) * Math.sin(refLng)); 82 rotationMatrix.setEntry(2, 1, Math.cos(refLat) * Math.sin(refLng)); 83 rotationMatrix.setEntry(0, 2, 0); 84 rotationMatrix.setEntry(1, 2, Math.cos(refLat)); 85 rotationMatrix.setEntry(2, 2, Math.sin(refLat)); 86 return rotationMatrix; 87 } 88 89 /** 90 * A container for values in ENU (East, North, Up) coordination system. 91 */ 92 public static class EnuValues { 93 94 /** 95 * East Coordinates in local ENU 96 */ 97 public final double enuEast; 98 99 /** 100 * North Coordinates in local ENU 101 */ 102 public final double enuNorth; 103 104 /** 105 * Up Coordinates in local ENU 106 */ 107 public final double enuUP; 108 109 /** 110 * Constructor 111 */ EnuValues(double enuEast, double enuNorth, double enuUP)112 public EnuValues(double enuEast, double enuNorth, double enuUP){ 113 this.enuEast = enuEast; 114 this.enuNorth = enuNorth; 115 this.enuUP = enuUP; 116 } 117 } 118 119 } 120