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