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.NEDFrame;
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 curvilinear to cartesian position and velocity resolving
30 * axes from NED to ECEF.
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_NED_to_ECEF.m">
34 * https://github.com/ymjdz/MATLAB-Codes/blob/master/pv_NED_to_ECEF.m
35 * </a>
36 */
37 public class NEDtoECEFPositionVelocityConverter {
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 curvilinear to cartesian position and velocity resolving axes
51 * from NED to ECEF.
52 *
53 * @param sourcePosition source position resolved on NED frame.
54 * @param sourceVelocity source velocity resolved on NED frame.
55 * @param destinationPosition instance where position resolved on ECEF frame will
56 * be stored.
57 * @param destinationVelocity instance where velocity resolved on ECEF frame will
58 * be stored.
59 */
60 public void convert(final NEDPosition sourcePosition, final NEDVelocity sourceVelocity,
61 final ECEFPosition destinationPosition, final ECEFVelocity destinationVelocity) {
62 convertNEDtoECEF(sourcePosition, sourceVelocity, destinationPosition, destinationVelocity);
63 }
64
65 /**
66 * Converts curvilinear to cartesian position and velocity resolving axes
67 * from NED to ECEF.
68 *
69 * @param latitude latitude expressed in radians (rad).
70 * @param longitude longitude expressed in radians (rad).
71 * @param height height expressed in meters (m).
72 * @param vn north coordinate of velocity of body frame expressed
73 * in meters per second (m/s).
74 * @param ve east coordinate of velocity of body frame expressed
75 * in meters per second (m/s).
76 * @param vd down coordinate of velocity of body frame expressed
77 * in meters per second (m/s).
78 * @param destinationPosition instance where position resolved on ECEF frame will
79 * be stored.
80 * @param destinationVelocity instance where velocity resolved on ECEF frame will
81 * be stored.
82 */
83 public void convert(final double latitude, final double longitude, final double height, final double vn,
84 final double ve, final double vd, final ECEFPosition destinationPosition,
85 final ECEFVelocity destinationVelocity) {
86 convertNEDtoECEF(latitude, longitude, height, vn, ve, vd, destinationPosition, destinationVelocity);
87 }
88
89 /**
90 * Converts curvilinear to cartesian position and velocity resolving axes
91 * from NED to ECEF.
92 *
93 * @param sourcePosition source position resolved on NED frame.
94 * @param sourceVelocity source velocity resolved on NED frame.
95 * @param destinationPosition instance where position resolved on ECEF frame will
96 * be stored.
97 * @param destinationVelocity instance where velocity resolved on ECEF frame will
98 * be stored.
99 */
100 public static void convertNEDtoECEF(final NEDPosition sourcePosition,
101 final NEDVelocity sourceVelocity,
102 final ECEFPosition destinationPosition,
103 final ECEFVelocity destinationVelocity) {
104 final var latitude = sourcePosition.getLatitude();
105 final var longitude = sourcePosition.getLongitude();
106 final var height = sourcePosition.getHeight();
107
108 final var vn = sourceVelocity.getVn();
109 final var ve = sourceVelocity.getVe();
110 final var vd = sourceVelocity.getVd();
111
112 convertNEDtoECEF(latitude, longitude, height, vn, ve, vd, destinationPosition, destinationVelocity);
113 }
114
115 /**
116 * Converts curvilinear to cartesian position and velocity resolving axes
117 * from NED to ECEF.
118 *
119 * @param latitude latitude expressed in radians (rad).
120 * @param longitude longitude expressed in radians (rad).
121 * @param height height expressed in meters (m).
122 * @param vn north coordinate of velocity of body frame expressed
123 * in meters per second (m/s).
124 * @param ve east coordinate of velocity of body frame expressed
125 * in meters per second (m/s).
126 * @param vd down coordinate of velocity of body frame expressed
127 * in meters per second (m/s).
128 * @param destinationPosition instance where position resolved on ECEF frame will
129 * be stored.
130 * @param destinationVelocity instance where velocity resolved on ECEF frame will
131 * be stored.
132 */
133 @SuppressWarnings("DuplicatedCode")
134 public static void convertNEDtoECEF(final double latitude, final double longitude, final double height,
135 final double vn, final double ve, final double vd,
136 final ECEFPosition destinationPosition,
137 final ECEFVelocity destinationVelocity) {
138 try {
139
140 // Calculate transverse radius of curvature using (2.105)
141 final var re = EARTH_EQUATORIAL_RADIUS_WGS84
142 / Math.sqrt(1.0 - Math.pow(EARTH_ECCENTRICITY * Math.sin(latitude), 2.0));
143
144 // Convert position using (2.112)
145 final var cosLat = Math.cos(latitude);
146 final var sinLat = Math.sin(latitude);
147 final var cosLong = Math.cos(longitude);
148 final var sinLong = Math.sin(longitude);
149
150 final var x = (re + height) * cosLat * cosLong;
151 final var y = (re + height) * cosLat * sinLong;
152 final var z = ((1.0 - EARTH_ECCENTRICITY * EARTH_ECCENTRICITY) * re + height) * sinLat;
153
154 destinationPosition.setCoordinates(x, y, z);
155
156 // Calculate NED to ECEF coordinate transformation matrix
157 final var cne = CoordinateTransformation.nedToEcefMatrix(latitude, longitude);
158
159 // Transform velocity using (2.73)
160 final var vEbn = new Matrix(NEDFrame.NUM_VELOCITY_COORDINATES, 1);
161 vEbn.setElementAtIndex(0, vn);
162 vEbn.setElementAtIndex(1, ve);
163 vEbn.setElementAtIndex(2, vd);
164
165 final var vEbe = cne.multiplyAndReturnNew(vEbn);
166 final var vx = vEbe.getElementAtIndex(0);
167 final var vy = vEbe.getElementAtIndex(1);
168 final var vz = vEbe.getElementAtIndex(2);
169
170 destinationVelocity.setCoordinates(vx, vy, vz);
171 } catch (final WrongSizeException ignore) {
172 // never happens
173 }
174 }
175 }