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.inertial.estimators;
17
18 import com.irurueta.geometry.Point3D;
19 import com.irurueta.navigation.frames.ECEFFrame;
20 import com.irurueta.navigation.geodesic.Constants;
21 import com.irurueta.navigation.inertial.ECEFGravity;
22 import com.irurueta.units.Distance;
23 import com.irurueta.units.DistanceConverter;
24 import com.irurueta.units.DistanceUnit;
25
26 /**
27 * Calculates acceleration due to gravity resolved about ECEF frame.
28 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multisensor
29 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
30 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/Gravity_ECEF.m">
31 * https://github.com/ymjdz/MATLAB-Codes/blob/master/Gravity_ECEF.m
32 * </a>
33 */
34 public class ECEFGravityEstimator {
35
36 /**
37 * The equatorial radius of WGS84 ellipsoid (6378137 m) defining Earth's shape.
38 */
39 public static final double EARTH_EQUATORIAL_RADIUS_WGS84 = Constants.EARTH_EQUATORIAL_RADIUS_WGS84;
40
41 /**
42 * WGS84 Earth gravitational constant expressed in m^3 * s^-2
43 */
44 public static final double EARTH_GRAVITATIONAL_CONSTANT = Constants.EARTH_GRAVITATIONAL_CONSTANT;
45
46 /**
47 * WGS84 Earth's second gravitational constant.
48 */
49 public static final double EARTH_SECOND_GRAVITATIONAL_CONSTANT = Constants.EARTH_SECOND_GRAVITATIONAL_CONSTANT;
50
51 /**
52 * Earth rotation rate expressed in radians per second (rad/s).
53 */
54 public static final double EARTH_ROTATION_RATE = Constants.EARTH_ROTATION_RATE;
55
56 /**
57 * Estimates acceleration due to gravity resolved about ECEF for a given position expressed in ECEF coordinates.
58 *
59 * @param x cartesian x coordinate of body position expressed in meters (m) with respect ECEF frame, resolved
60 * along ECEF-frame axes.
61 * @param y cartesian y coordinate of body position expressed in meters (m) with respect ECEF frame, resolved
62 * along ECEF-frame axes.
63 * @param z cartesian z coordinate of body position expressed in meters (m) with respect ECEF frame, resolved
64 * along ECEF-frame axes.
65 * @param result instance where estimated acceleration due to gravity will be stored.
66 */
67 public void estimate(final double x, final double y, final double z, final ECEFGravity result) {
68 estimateGravity(x, y, z, result);
69 }
70
71 /**
72 * Estimates acceleration due to gravity resolved about ECEF for a given position expressed in ECEF coordinates.
73 *
74 * @param x cartesian x coordinate of body position expressed in meters (m) with respect ECEF frame, resolved along
75 * ECEF-frame axes.
76 * @param y cartesian y coordinate of body position expressed in meters (m) with respect ECEF frame, resolved along
77 * ECEF-frame axes.
78 * @param z cartesian z coordinate of body position expressed in meters (m) with respect ECEF frame, resolved along
79 * ECEF-frame axes.
80 * @return a new gravity instance containing estimated acceleration due to gravity.
81 */
82 public ECEFGravity estimateAndReturnNew(final double x, final double y, final double z) {
83 return estimateGravityAndReturnNew(x, y, z);
84 }
85
86 /**
87 * Estimates acceleration due to gravity resolved about ECEF for a position on a given ECEF frame.
88 *
89 * @param frame an ECEF frame containing a given position.
90 * @param result instance where estimated acceleration due to gravity will be stored.
91 */
92 public void estimate(final ECEFFrame frame, final ECEFGravity result) {
93 estimateGravity(frame, result);
94 }
95
96 /**
97 * Estimates acceleration due to gravity resolved about ECEF for a position on a given ECEF frame.
98 *
99 * @param frame an ECEF frame containing a given position.
100 * @return a new gravity instance containing estimated acceleration due to gravity.
101 */
102 public ECEFGravity estimateAndReturnNew(final ECEFFrame frame) {
103 return estimateGravityAndReturnNew(frame);
104 }
105
106 /**
107 * Estimates acceleration due to gravity resolved about ECEF for a given position expressed in ECEF coordinates.
108 *
109 * @param position cartesian body position expressed in meters (m) with respect ECEF frame, resolved along
110 * ECEF-frame axes.
111 * @param result instance where estimated acceleration due to gravity will be stored.
112 */
113 public void estimate(final Point3D position, final ECEFGravity result) {
114 estimateGravity(position, result);
115 }
116
117 /**
118 * Estimates acceleration due to gravity resolved about ECEF for a given position expressed in ECEF coordinates.
119 *
120 * @param position cartesian body position expressed in meters (m) with respect ECEF frame, resolved along
121 * ECEF-frame axes.
122 * @return a new gravity instance containing estimated acceleration due to gravity.
123 */
124 public ECEFGravity estimateAndReturnNew(final Point3D position) {
125 return estimateGravityAndReturnNew(position);
126 }
127
128 /**
129 * Estimates acceleration due to gravity resolved about ECEF for a given position expressed in ECEF coordinates.
130 *
131 * @param x cartesian x coordinate of body position with respect ECEF frame, resolved along ECEF-frame axes.
132 * @param y cartesian y coordinate of body position with respect ECEF frame, resolved along ECEF-frame axes.
133 * @param z cartesian z coordinate of body position with respect ECEF frame, resolved along ECEF-frame axes.
134 * @param result instance where estimated acceleration due to gravity will be stored.
135 */
136 public void estimate(final Distance x, final Distance y, final Distance z, final ECEFGravity result) {
137 estimateGravity(x, y, z, result);
138 }
139
140 /**
141 * Estimates acceleration due to gravity resolved about ECEF for a given position expressed in ECEF coordinates.
142 *
143 * @param x cartesian x coordinate of body position with respect ECEF frame, resolved along ECEF-frame axes.
144 * @param y cartesian y coordinate of body position with respect ECEF frame, resolved along ECEF-frame axes.
145 * @param z cartesian z coordinate of body position with respect ECEF frame, resolved along ECEF-frame axes.
146 * @return a new gravity instance containing estimated acceleration due to gravity.
147 */
148 public ECEFGravity estimateAndReturnNew(final Distance x, final Distance y, final Distance z) {
149 return estimateGravityAndReturnNew(x, y, z);
150 }
151
152 /**
153 * Estimates acceleration due to gravity resolved about ECEF for a given position expressed in ECEF coordinates.
154 *
155 * @param x cartesian x coordinate of body position expressed in meters (m) with respect ECEF frame, resolved
156 * along ECEF-frame axes.
157 * @param y cartesian y coordinate of body position expressed in meters (m) with respect ECEF frame, resolved
158 * along ECEF-frame axes.
159 * @param z cartesian z coordinate of body position expressed in meters (m) with respect ECEF frame, resolved
160 * along ECEF-frame axes.
161 * @param result instance where estimated acceleration due to gravity will be stored.
162 */
163 public static void estimateGravity(final double x, final double y, final double z, final ECEFGravity result) {
164 // Calculate distance from center of the Earth
165 final var magR = Math.sqrt(x * x + y * y + z * z);
166
167 if (magR == 0.0) {
168 // If the input position is 0,0,0, produce a dummy output
169 result.setCoordinates(0.0, 0.0, 0.0);
170 } else {
171 // Calculate gravitational acceleration using (2.142)
172 final var zScale = 5.0 * Math.pow(z / magR, 2.0);
173 final var tmp1 = -EARTH_GRAVITATIONAL_CONSTANT / Math.pow(magR, 3.0);
174 final var tmp2 = 1.5 * EARTH_SECOND_GRAVITATIONAL_CONSTANT * Math.pow(EARTH_EQUATORIAL_RADIUS_WGS84
175 / magR, 2.0);
176 final var tmp3 = tmp1 * (1.0 + tmp2 * (1.0 - zScale));
177
178 final var gammaX = tmp3 * x;
179 final var gammaY = tmp3 * y;
180 final var gammaZ = tmp1 * (1.0 + tmp2 * (3.0 - zScale)) * z;
181
182 // Add centripetal acceleration using (2.133)
183 final var omega2 = EARTH_ROTATION_RATE * EARTH_ROTATION_RATE;
184 final var gx = gammaX + omega2 * x;
185 final var gy = gammaY + omega2 * y;
186
187 result.setCoordinates(gx, gy, gammaZ);
188 }
189 }
190
191 /**
192 * Estimates acceleration due to gravity resolved about ECEF for a given position expressed in ECEF coordinates.
193 *
194 * @param x cartesian x coordinate of body position expressed in meters (m) with respect ECEF frame, resolved along
195 * ECEF-frame axes.
196 * @param y cartesian y coordinate of body position expressed in meters (m) with respect ECEF frame, resolved along
197 * ECEF-frame axes.
198 * @param z cartesian z coordinate of body position expressed in meters (m) with respect ECEF frame, resolved along
199 * ECEF-frame axes.
200 * @return a new gravity instance containing estimated acceleration due to gravity.
201 */
202 public static ECEFGravity estimateGravityAndReturnNew(final double x, final double y, final double z) {
203 final var result = new ECEFGravity();
204 estimateGravity(x, y, z, result);
205 return result;
206 }
207
208 /**
209 * Estimates acceleration due to gravity resolved about ECEF for a position on a given ECEF frame.
210 *
211 * @param frame an ECEF frame containing a given position.
212 * @param result instance where estimated acceleration due to gravity will be stored.
213 */
214 public static void estimateGravity(final ECEFFrame frame, final ECEFGravity result) {
215 estimateGravity(frame.getX(), frame.getY(), frame.getZ(), result);
216 }
217
218 /**
219 * Estimates acceleration due to gravity resolved about ECEF for a position on a given ECEF frame.
220 *
221 * @param frame an ECEF frame containing a given position.
222 * @return a new gravity instance containing estimated acceleration due to gravity.
223 */
224 public static ECEFGravity estimateGravityAndReturnNew(final ECEFFrame frame) {
225 return estimateGravityAndReturnNew(frame.getX(), frame.getY(), frame.getZ());
226 }
227
228 /**
229 * Estimates acceleration due to gravity resolved about ECEF for a given position expressed in ECEF coordinates.
230 *
231 * @param position cartesian body position expressed in meters (m) with respect ECEF frame, resolved along
232 * ECEF-frame axes.
233 * @param result instance where estimated acceleration due to gravity will be stored.
234 */
235 public static void estimateGravity(final Point3D position, final ECEFGravity result) {
236 estimateGravity(position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
237 }
238
239 /**
240 * Estimates acceleration due to gravity resolved about ECEF for a given position expressed in ECEF coordinates.
241 *
242 * @param position cartesian body position expressed in meters (m) with respect ECEF frame, resolved along
243 * ECEF-frame axes.
244 * @return a new gravity instance containing estimated acceleration due to gravity.
245 */
246 public static ECEFGravity estimateGravityAndReturnNew(final Point3D position) {
247 return estimateGravityAndReturnNew(position.getInhomX(), position.getInhomY(), position.getInhomZ());
248 }
249
250 /**
251 * Estimates acceleration due to gravity resolved about ECEF for a given position expressed in ECEF coordinates.
252 *
253 * @param x cartesian x coordinate of body position with respect ECEF frame, resolved along ECEF-frame axes.
254 * @param y cartesian y coordinate of body position with respect ECEF frame, resolved along ECEF-frame axes.
255 * @param z cartesian z coordinate of body position with respect ECEF frame, resolved along ECEF-frame axes.
256 * @param result instance where estimated acceleration due to gravity will be stored.
257 */
258 public static void estimateGravity(final Distance x, final Distance y, final Distance z, final ECEFGravity result) {
259 estimateGravity(convertToMeters(x), convertToMeters(y), convertToMeters(z), result);
260 }
261
262 /**
263 * Estimates acceleration due to gravity resolved about ECEF for a given position expressed in ECEF coordinates.
264 *
265 * @param x cartesian x coordinate of body position with respect ECEF frame, resolved along ECEF-frame axes.
266 * @param y cartesian y coordinate of body position with respect ECEF frame, resolved along ECEF-frame axes.
267 * @param z cartesian z coordinate of body position with respect ECEF frame, resolved along ECEF-frame axes.
268 * @return a new gravity instance containing estimated acceleration due to gravity.
269 */
270 public static ECEFGravity estimateGravityAndReturnNew(final Distance x, final Distance y, final Distance z) {
271 final var result = new ECEFGravity();
272 estimateGravity(x, y, z, result);
273 return result;
274 }
275
276 /**
277 * Converts distance to meters.
278 *
279 * @param distance distance to be converted.
280 * @return converted distance expressed in meters.
281 */
282 private static double convertToMeters(final Distance distance) {
283 return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
284 }
285 }