1 /*
2 * Copyright (C) 2019 Alberto Irurueta Carro (alberto@irurueta.com)
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 package com.irurueta.navigation.frames.converters;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20 import com.irurueta.navigation.frames.CoordinateTransformation;
21 import com.irurueta.navigation.frames.ECEFPosition;
22 import com.irurueta.navigation.frames.ECEFVelocity;
23 import com.irurueta.navigation.frames.ECIorECEFFrame;
24 import com.irurueta.navigation.frames.NEDPosition;
25 import com.irurueta.navigation.frames.NEDVelocity;
26 import com.irurueta.navigation.geodesic.Constants;
27
28 /**
29 * Converts cartesian to curvilinear position and velocity resolving axes
30 * from ECEF to NED.
31 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multi-sensor
32 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
33 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/pv_ECEF_to_NED.m">
34 * https://github.com/ymjdz/MATLAB-Codes/blob/master/pv_ECEF_to_NED.m
35 * </a>
36 */
37 public class ECEFtoNEDPositionVelocityConverter {
38
39 /**
40 * The equatorial radius of WGS84 ellipsoid (6378137 m) defining Earth's shape.
41 */
42 public static final double EARTH_EQUATORIAL_RADIUS_WGS84 = Constants.EARTH_EQUATORIAL_RADIUS_WGS84;
43
44 /**
45 * Earth eccentricity as defined on the WGS84 ellipsoid.
46 */
47 public static final double EARTH_ECCENTRICITY = Constants.EARTH_ECCENTRICITY;
48
49 /**
50 * Converts cartesian to curvilinear position and velocity resolving axes
51 * from NED to ECEF.
52 *
53 * @param sourcePosition source position resolved on ECEF frame.
54 * @param sourceVelocity source velocity resolved on ECEF frame.
55 * @param destinationPosition instance where position resolved on NED frame will
56 * be stored.
57 * @param destinationVelocity instance where velocity resolved on NED frame will
58 * be stored.
59 */
60 public void convert(final ECEFPosition sourcePosition,
61 final ECEFVelocity sourceVelocity,
62 final NEDPosition destinationPosition,
63 final NEDVelocity destinationVelocity) {
64 convertECEFtoNED(sourcePosition, sourceVelocity, destinationPosition, destinationVelocity);
65 }
66
67 /**
68 * Converts cartesian to curvilinear position and velocity resolving axes
69 * from NED to ECEF.
70 *
71 * @param x x cartesian coordinate of body frame expressed in meters (m).
72 * @param y y cartesian coordinate of body frame expressed in meters (m).
73 * @param z z cartesian coordinate of body frame expressed in meters (m).
74 * @param vx x coordinate of body velocity expressed in meters per second (m/s).
75 * @param vy y coordinate of body velocity expressed in meters per second (m/s).
76 * @param vz z coordinate of body velocity expressed in meters per second (m/s).
77 * @param destinationPosition instance where position resolved on NED frame will
78 * be stored.
79 * @param destinationVelocity instance where velocity resolved on NED frame will
80 * be stored.
81 */
82 public void convert(final double x, final double y, final double z,
83 final double vx, final double vy, final double vz,
84 final NEDPosition destinationPosition, final NEDVelocity destinationVelocity) {
85 convertECEFtoNED(x, y, z, vx, vy, vz, destinationPosition, destinationVelocity);
86 }
87
88 /**
89 * Converts cartesian to curvilinear position and velocity resolving axes
90 * from NED to ECEF.
91 *
92 * @param sourcePosition source position resolved on ECEF frame.
93 * @param sourceVelocity source velocity resolved on ECEF frame.
94 * @param destinationPosition instance where position resolved on NED frame will
95 * be stored.
96 * @param destinationVelocity instance where velocity resolved on NED frame will
97 * be stored.
98 */
99 public static void convertECEFtoNED(final ECEFPosition sourcePosition,
100 final ECEFVelocity sourceVelocity,
101 final NEDPosition destinationPosition,
102 final NEDVelocity destinationVelocity) {
103 convertECEFtoNED(sourcePosition.getX(), sourcePosition.getY(),
104 sourcePosition.getZ(), sourceVelocity.getVx(), sourceVelocity.getVy(),
105 sourceVelocity.getVz(), destinationPosition, destinationVelocity);
106 }
107
108 /**
109 * Converts cartesian to curvilinear position and velocity resolving axes
110 * from NED to ECEF.
111 *
112 * @param x x cartesian coordinate of body frame expressed in meters (m).
113 * @param y y cartesian coordinate of body frame expressed in meters (m).
114 * @param z z cartesian coordinate of body frame expressed in meters (m).
115 * @param vx x coordinate of body velocity expressed in meters per second (m/s).
116 * @param vy y coordinate of body velocity expressed in meters per second (m/s).
117 * @param vz z coordinate of body velocity expressed in meters per second (m/s).
118 * @param destinationPosition instance where position resolved on NED frame will
119 * be stored.
120 * @param destinationVelocity instance where velocity resolved on NED frame will
121 * be stored.
122 */
123 @SuppressWarnings("DuplicatedCode")
124 public static void convertECEFtoNED(final double x, final double y, final double z,
125 final double vx, final double vy, final double vz,
126 final NEDPosition destinationPosition, final NEDVelocity destinationVelocity) {
127
128 try {
129 // Convert position using Borlowski closed-form exact solution
130 // From (2.113)
131 final var longitude = Math.atan2(y, x);
132
133 // From (C.29) and (C.30)
134 final var ecc2 = EARTH_ECCENTRICITY * EARTH_ECCENTRICITY;
135 final var k1 = Math.sqrt(1.0 - ecc2) * Math.abs(z);
136 final var k2 = ecc2 * EARTH_EQUATORIAL_RADIUS_WGS84;
137 final var beta = Math.sqrt(x * x + y * y);
138 final var e = (k1 - k2) / beta;
139 final var f = (k1 + k2) / beta;
140
141 // From (C.31)
142 final var p = 4.0 / 3.0 * (e * f + 1.0);
143
144 // From (C.32)
145 final var e2 = e * e;
146 final var f2 = f * f;
147 final var q = 2.0 * (e2 - f2);
148
149 // From (C.33)
150 final var p3 = p * p * p;
151 final var q2 = q * q;
152 final var d = p3 + q2;
153
154 // From (C.34)
155 final var v = Math.pow(Math.sqrt(d) - q, 1.0 / 3.0) - Math.pow(Math.sqrt(d) + q, 1.0 / 3.0);
156
157 // From (C.35)
158 final var g = 0.5 * (Math.sqrt(e2 + v) + e);
159
160 // From (C.36)
161 final var g2 = g * g;
162 final var t = Math.sqrt(g2 + (f - v * g) / (2.0 * g - e)) - g;
163
164 // From (C.37)
165 final var t2 = t * t;
166 final var latitude = Math.signum(z) * Math.atan((1 - t2) / (2.0 * t * Math.sqrt(1.0 - ecc2)));
167
168 // From (C.38)
169 final var height = (beta - EARTH_EQUATORIAL_RADIUS_WGS84 * t)
170 * Math.cos(latitude) + (z - Math.signum(z)
171 * EARTH_EQUATORIAL_RADIUS_WGS84 * Math.sqrt(1.0 - ecc2))
172 * Math.sin(latitude);
173
174 // Calculate ECEF to NED coordinate transformation matrix
175 final var cen = CoordinateTransformation.ecefToNedMatrix(latitude, longitude);
176
177 // Transform velocity using (2.73)
178 final var vEbe = new Matrix(ECIorECEFFrame.NUM_VELOCITY_COORDINATES, 1);
179 vEbe.setElementAtIndex(0, vx);
180 vEbe.setElementAtIndex(1, vy);
181 vEbe.setElementAtIndex(2, vz);
182
183 final var vEbn = cen.multiplyAndReturnNew(vEbe);
184 final var vn = vEbn.getElementAtIndex(0);
185 final var ve = vEbn.getElementAtIndex(1);
186 final var vd = vEbn.getElementAtIndex(2);
187
188 destinationPosition.setCoordinates(latitude, longitude, height);
189 destinationVelocity.setCoordinates(vn, ve, vd);
190
191 } catch (final WrongSizeException ignore) {
192 // never happens
193 }
194 }
195 }