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.ECIFrame;
20 import com.irurueta.navigation.geodesic.Constants;
21 import com.irurueta.navigation.inertial.ECIGravitation;
22 import com.irurueta.units.Distance;
23 import com.irurueta.units.DistanceConverter;
24 import com.irurueta.units.DistanceUnit;
25
26 /**
27 * Calculates gravitational acceleration resolved about ECI-frame axes.
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/Gravitation_ECI.m">
31 * https://github.com/ymjdz/MATLAB-Codes/blob/master/Gravitation_ECI.m
32 * </a>
33 */
34 public class ECIGravitationEstimator {
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 * Estimates gravitational acceleration resolved about ECI-frame axes.
53 *
54 * @param x cartesian x coordinate of body position expressed in meters (m) with respect ECI frame, resolved
55 * along ECI-frame axes.
56 * @param y cartesian y coordinate of body position expressed in meters (m) with respect ECI frame, resolved
57 * along ECI-frame axes.
58 * @param z cartesian z coordinate of body position expressed in meters (m) with respect ECI frame, resolved
59 * along ECI-frame axes.
60 * @param result instance where estimated acceleration due to gravity will be stored.
61 */
62 public void estimate(final double x, final double y, final double z, final ECIGravitation result) {
63 estimateGravitation(x, y, z, result);
64 }
65
66 /**
67 * Estimates gravitational acceleration resolved about ECI-frame axes.
68 *
69 * @param x cartesian x coordinate of body position expressed in meters (m) with respect ECI frame, resolved along
70 * ECI-frame axes.
71 * @param y cartesian y coordinate of body position expressed in meters (m) with respect ECI frame, resolved along
72 * ECI-frame axes.
73 * @param z cartesian z coordinate of body position expressed in meters (m) with respect ECI frame, resolved along
74 * ECI-frame axes.
75 * @return a new gravitation instance containing estimated acceleration due to gravity.
76 */
77 public ECIGravitation estimateAndReturnNew(final double x, final double y, final double z) {
78 return estimateGravitationAndReturnNew(x, y, z);
79 }
80
81 /**
82 * Estimates gravitational acceleration resolved about ECI-frame axes for a position on a given ECI frame.
83 *
84 * @param frame an ECI frame containing a given position.
85 * @param result instance where estimated acceleration due to gravity will be stored.
86 */
87 public void estimate(final ECIFrame frame, final ECIGravitation result) {
88 estimateGravitation(frame, result);
89 }
90
91 /**
92 * Estimates gravitational acceleration resolved about ECI-frame axes for a position on a given ECI frame.
93 *
94 * @param frame an ECI frame containing a given position.
95 * @return a new gravitation instance containing estimated acceleration due to gravity.
96 */
97 public ECIGravitation estimateAndReturnNew(final ECIFrame frame) {
98 return estimateGravitationAndReturnNew(frame);
99 }
100
101 /**
102 * Estimates gravitational acceleration resolved about ECI-frame axes for a given position expressed in
103 * ECI coordinates.
104 *
105 * @param position cartesian body position expressed in meters (m) with respect ECI frame, resolved along
106 * ECI-frame axes.
107 * @param result instance where estimated acceleration due to gravity will be stored.
108 */
109 public void estimate(final Point3D position, final ECIGravitation result) {
110 estimateGravitation(position, result);
111 }
112
113 /**
114 * Estimates gravitation acceleration resolved about ECI-frame axes for a given position expressed in
115 * ECI coordinates.
116 *
117 * @param position cartesian body position expressed in meters (m) with respect ECI frame, resolved along
118 * ECI-frame axes.
119 * @return a new gravitation instance containing estimated acceleration due to gravity.
120 */
121 public ECIGravitation estimateAndReturnNew(final Point3D position) {
122 return estimateGravitationAndReturnNew(position);
123 }
124
125 /**
126 * Estimates gravitation acceleration resolved about ECI-frame axes for a given position expressed in
127 * ECI coordinates.
128 *
129 * @param x cartesian x coordinate of body position with respect ECI frame, resolved along
130 * ECI-frame axes.
131 * @param y cartesian y coordinate of body position with respect ECI frame, resolved along
132 * ECI-frame axes.
133 * @param z cartesian z coordinate of body position with respect ECI frame, resolved along
134 * ECI-frame axes.
135 * @param result instance where estimated acceleration due to gravity will be stored.
136 */
137 public void estimate(final Distance x, final Distance y, final Distance z, final ECIGravitation result) {
138 estimateGravitation(x, y, z, result);
139 }
140
141 /**
142 * Estimates gravitation acceleration resolved about ECI-frame axes for a given position
143 * expressed in ECI coordinates.
144 *
145 * @param x cartesian x coordinate of body position with respect ECI frame, resolved along
146 * ECI-frame axes.
147 * @param y cartesian y coordinate of body position with respect ECI frame, resolved along
148 * ECI-frame axes.
149 * @param z cartesian z coordinate of body position with respect ECI frame, resolved along
150 * ECI-frame axes.
151 * @return a new gravitation instance containing estimated acceleration due to gravity.
152 */
153 public ECIGravitation estimateAndReturnNew(final Distance x, final Distance y, final Distance z) {
154 return estimateGravitationAndReturnNew(x, y, z);
155 }
156
157 /**
158 * Estimates gravitational acceleration resolved about ECI-frame axes.
159 *
160 * @param x cartesian x coordinate of body position expressed in meters (m) with respect ECI frame, resolved
161 * along ECI-frame axes.
162 * @param y cartesian y coordinate of body position expressed in meters (m) with respect ECI frame, resolved
163 * along ECI-frame axes.
164 * @param z cartesian z coordinate of body position expressed in meters (m) with respect ECI frame, resolved
165 * along ECI-frame axes.
166 * @param result instance where estimated acceleration due to gravity will be stored.
167 */
168 public static void estimateGravitation(
169 final double x, final double y, final double z, final ECIGravitation result) {
170
171 // Calculate distance from center of the Earth
172 final var magR = Math.sqrt(x * x + y * y + z * z);
173
174 // If the input position is 0,0,0, produce a dummy output
175 if (magR == 0.0) {
176 result.setCoordinates(0.0, 0.0, 0.0);
177 } else {
178 final var zScale = 5.0 * Math.pow(z / magR, 2.0);
179 final var tmp1 = -EARTH_GRAVITATIONAL_CONSTANT / Math.pow(magR, 3.0);
180 final var tmp2 = 1.5 * EARTH_SECOND_GRAVITATIONAL_CONSTANT * Math.pow(EARTH_EQUATORIAL_RADIUS_WGS84 / magR,
181 2.0);
182 final var gx = tmp1 * (1.0 + tmp2 * (1.0 - zScale)) * x;
183 final var gy = tmp1 * (1.0 + tmp2 * (1.0 - zScale)) * y;
184 final var gz = tmp1 * (1.0 + tmp2 * (3.0 - zScale)) * z;
185
186 result.setCoordinates(gx, gy, gz);
187 }
188 }
189
190 /**
191 * Estimates gravitational acceleration resolved about ECI-frame axes.
192 *
193 * @param x cartesian x coordinate of body position expressed in meters (m) with respect ECI frame, resolved along
194 * ECI-frame axes.
195 * @param y cartesian y coordinate of body position expressed in meters (m) with respect ECI frame, resolved along
196 * ECI-frame axes.
197 * @param z cartesian z coordinate of body position expressed in meters (m) with respect ECI frame, resolved along
198 * ECI-frame axes.
199 * @return a new gravitation instance containing estimated acceleration due to gravity.
200 */
201 public static ECIGravitation estimateGravitationAndReturnNew(final double x, final double y, final double z) {
202 final var result = new ECIGravitation();
203 estimateGravitation(x, y, z, result);
204 return result;
205 }
206
207 /**
208 * Estimates gravitational acceleration resolved about ECI-frame axes for a position on a given ECI frame.
209 *
210 * @param frame an ECI frame containing a given position.
211 * @param result instance where estimated acceleration due to gravity will be stored.
212 */
213 public static void estimateGravitation(final ECIFrame frame, final ECIGravitation result) {
214 estimateGravitation(frame.getX(), frame.getY(), frame.getZ(), result);
215 }
216
217 /**
218 * Estimates gravitational acceleration resolved about ECI-frame axes for a position on a given ECI frame.
219 *
220 * @param frame an ECI frame containing a given position.
221 * @return a new gravitation instance containing estimated acceleration due to gravity.
222 */
223 public static ECIGravitation estimateGravitationAndReturnNew(final ECIFrame frame) {
224 return estimateGravitationAndReturnNew(frame.getX(), frame.getY(), frame.getZ());
225 }
226
227 /**
228 * Estimates gravitational acceleration resolved about ECI-frame axes for a given position expressed in
229 * ECI coordinates.
230 *
231 * @param position cartesian body position expressed in meters (m) with respect ECI frame, resolved along
232 * ECI-frame axes.
233 * @param result instance where estimated acceleration due to gravity will be stored.
234 */
235 public static void estimateGravitation(final Point3D position, final ECIGravitation result) {
236 estimateGravitation(position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
237 }
238
239 /**
240 * Estimates gravitation acceleration resolved about ECI-frame axes for a given position expressed in
241 * ECI coordinates.
242 *
243 * @param position cartesian body position expressed in meters (m) with respect ECI frame, resolved along
244 * ECI-frame axes.
245 * @return a new gravitation instance containing estimated acceleration due to gravity.
246 */
247 public static ECIGravitation estimateGravitationAndReturnNew(final Point3D position) {
248 return estimateGravitationAndReturnNew(position.getInhomX(), position.getInhomY(), position.getInhomZ());
249 }
250
251 /**
252 * Estimates gravitation acceleration resolved about ECI-frame axes for a given position
253 * expressed in ECI coordinates.
254 *
255 * @param x cartesian x coordinate of body position with respect ECI frame, resolved along
256 * ECI-frame axes.
257 * @param y cartesian y coordinate of body position with respect ECI frame, resolved along
258 * ECI-frame axes.
259 * @param z cartesian z coordinate of body position with respect ECI frame, resolved along
260 * ECI-frame axes.
261 * @param result instance where estimated acceleration due to gravity will be stored.
262 */
263 public static void estimateGravitation(
264 final Distance x, final Distance y, final Distance z, final ECIGravitation result) {
265 estimateGravitation(convertToMeters(x), convertToMeters(y), convertToMeters(z), result);
266 }
267
268 /**
269 * Estimates gravitation acceleration resolved about ECI-frame axes for a given position
270 * expressed in ECI coordinates.
271 *
272 * @param x cartesian x coordinate of body position with respect ECI frame, resolved along
273 * ECI-frame axes.
274 * @param y cartesian y coordinate of body position with respect ECI frame, resolved along
275 * ECI-frame axes.
276 * @param z cartesian z coordinate of body position with respect ECI frame, resolved along
277 * ECI-frame axes.
278 * @return a new gravitation instance containing estimated acceleration due to gravity.
279 */
280 public static ECIGravitation estimateGravitationAndReturnNew(final Distance x, final Distance y, final Distance z) {
281 final var result = new ECIGravitation();
282 estimateGravitation(x, y, z, result);
283 return result;
284 }
285
286 /**
287 * Converts distance to meters.
288 *
289 * @param distance distance to be converted.
290 * @return converted distance expressed in meters.
291 */
292 private static double convertToMeters(final Distance distance) {
293 return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
294 }
295 }