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.navigation.frames.NEDFrame;
19 import com.irurueta.navigation.frames.NEDPosition;
20 import com.irurueta.navigation.geodesic.Constants;
21 import com.irurueta.navigation.inertial.NEDGravity;
22 import com.irurueta.units.Angle;
23 import com.irurueta.units.AngleConverter;
24 import com.irurueta.units.AngleUnit;
25 import com.irurueta.units.Distance;
26 import com.irurueta.units.DistanceConverter;
27 import com.irurueta.units.DistanceUnit;
28
29 /**
30 * Calculates acceleration due to gravity resolved about north, east
31 * and down axes of a NED frame.
32 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multisensor
33 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
34 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/Gravity_NED.m">
35 * https://github.com/ymjdz/MATLAB-Codes/blob/master/Gravity_NED.m
36 * </a>
37 */
38 public class NEDGravityEstimator {
39
40 /**
41 * The equatorial radius of WGS84 ellipsoid (6378137 m) defining Earth's shape.
42 */
43 public static final double EARTH_EQUATORIAL_RADIUS_WGS84 = Constants.EARTH_EQUATORIAL_RADIUS_WGS84;
44
45 /**
46 * The polar radius of WGS84 ellipsoid (6356752.31425 m) defining Earth's shape.
47 */
48 public static final double EARTH_POLAR_RADIUS_WGS84 = Constants.EARTH_POLAR_RADIUS_WGS84;
49
50 /**
51 * Earth eccentricity as defined on the WGS84 ellipsoid.
52 */
53 public static final double EARTH_ECCENTRICITY = 0.0818191908425;
54
55 /**
56 * The flattening of WGS84 ellipsoid (1 / 298.257223563).
57 */
58 public static final double EARTH_FLATTENING_WGS84 = 1 / 298.257223563;
59
60 /**
61 * WGS84 Earth gravitational constant expressed in m^3 * s^-2
62 */
63 public static final double EARTH_GRAVITATIONAL_CONSTANT = 3.986004418E14;
64
65 /**
66 * Earth rotation rate expressed in radians per second (rad/s).
67 */
68 public static final double EARTH_ROTATION_RATE = 7.292115E-5;
69
70 /**
71 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
72 *
73 * @param latitude latitude expressed in radians (rad).
74 * @param height height expressed in meters (m).
75 * @param result instance where estimated acceleration due to gravity will be stored.
76 */
77 public void estimate(final double latitude, final double height, final NEDGravity result) {
78 estimateGravity(latitude, height, result);
79 }
80
81 /**
82 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
83 *
84 * @param latitude latitude expressed in radians (rad).
85 * @param height height expressed in meters (m).
86 * @return a new gravity instance containing estimated acceleration due to gravity.
87 */
88 public NEDGravity estimateAndReturnNew(final double latitude, final double height) {
89 return estimateGravityAndReturnNew(latitude, height);
90 }
91
92 /**
93 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
94 *
95 * @param frame a NED frame containing a given position.
96 * @param result instance where estimated acceleration due to gravity will be stored.
97 */
98 public void estimate(final NEDFrame frame, final NEDGravity result) {
99 estimateGravity(frame, result);
100 }
101
102 /**
103 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
104 *
105 * @param frame a NED frame containing a given position.
106 * @return a new gravity instance containing estimated acceleration due to gravity.
107 */
108 public NEDGravity estimateAndReturnNew(final NEDFrame frame) {
109 return estimateGravityAndReturnNew(frame);
110 }
111
112 /**
113 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
114 *
115 * @param latitude latitude.
116 * @param height height.
117 * @param result instance where estimated acceleration due to gravity will be stored.
118 */
119 public void estimate(final Angle latitude, final Distance height, final NEDGravity result) {
120 estimateGravity(latitude, height, result);
121 }
122
123 /**
124 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
125 *
126 * @param latitude latitude.
127 * @param height height.
128 * @return a new gravity instance containing estimated acceleration due to gravity.
129 */
130 public NEDGravity estimateAndReturnNew(final Angle latitude, final Distance height) {
131 return estimateGravityAndReturnNew(latitude, height);
132 }
133
134 /**
135 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
136 *
137 * @param position curvilinear position expressed in NED coordinates.
138 * @param result instance where estimated acceleration due to gravity will be stored.
139 */
140 public void estimate(final NEDPosition position, final NEDGravity result) {
141 estimateGravity(position, result);
142 }
143
144 /**
145 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
146 *
147 * @param position curvilinear position expressed in NED coordinates.
148 * @return a new gravity instance containing estimated acceleration due to gravity.
149 */
150 public NEDGravity estimateAndReturnNew(final NEDPosition position) {
151 return estimateGravityAndReturnNew(position);
152 }
153
154 /**
155 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
156 *
157 * @param latitude latitude expressed in radians (rad).
158 * @param height height expressed in meters (m).
159 * @param result instance where estimated acceleration due to gravity will be stored.
160 */
161 public static void estimateGravity(final double latitude, final double height, final NEDGravity result) {
162 // Calculate surface gravity using the Somigliana model (2.134)
163 final var sinsqL = Math.pow(Math.sin(latitude), 2.0);
164 final var e2 = EARTH_ECCENTRICITY * EARTH_ECCENTRICITY;
165 final var g0 = 9.7803253359 * (1.0 + 0.001931853 * sinsqL) / Math.sqrt(1.0 - e2 * sinsqL);
166
167 // Calculate north gravity using (2.140)
168 final var gn = -8.08E-9 * height * Math.sin(2.0 * latitude);
169
170 // Calculate down gravity using (2.139)
171 final var omegaIe2 = EARTH_ROTATION_RATE * EARTH_ROTATION_RATE;
172 final var r02 = EARTH_EQUATORIAL_RADIUS_WGS84 * EARTH_EQUATORIAL_RADIUS_WGS84;
173 final var height2 = height * height;
174 final var gd = g0 * (1.0 - (2.0 / EARTH_EQUATORIAL_RADIUS_WGS84) * (1.0 + EARTH_FLATTENING_WGS84 *
175 (1.0 - 2.0 * sinsqL) + (omegaIe2 * r02 * EARTH_POLAR_RADIUS_WGS84 / EARTH_GRAVITATIONAL_CONSTANT)) *
176 height + (3.0 * height2 / r02));
177
178 // NOTE: East gravity is zero
179 result.setCoordinates(gn, gd);
180 }
181
182 /**
183 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
184 *
185 * @param latitude latitude expressed in radians (rad).
186 * @param height height expressed in meters (m).
187 * @return a new gravity instance containing estimated acceleration due to gravity.
188 */
189 public static NEDGravity estimateGravityAndReturnNew(final double latitude, final double height) {
190 final var result = new NEDGravity();
191 estimateGravity(latitude, height, result);
192 return result;
193 }
194
195 /**
196 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
197 *
198 * @param frame a NED frame containing a given position.
199 * @param result instance where estimated acceleration due to gravity will be stored.
200 */
201 public static void estimateGravity(final NEDFrame frame, final NEDGravity result) {
202 estimateGravity(frame.getLatitude(), frame.getHeight(), result);
203 }
204
205 /**
206 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
207 *
208 * @param frame a NED frame containing a given position.
209 * @return a new gravity instance containing estimated acceleration due to gravity.
210 */
211 public static NEDGravity estimateGravityAndReturnNew(final NEDFrame frame) {
212 return estimateGravityAndReturnNew(frame.getLatitude(), frame.getHeight());
213 }
214
215 /**
216 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
217 *
218 * @param latitude latitude.
219 * @param height height.
220 * @param result instance where estimated acceleration due to gravity will be stored.
221 */
222 public static void estimateGravity(final Angle latitude, final Distance height, final NEDGravity result) {
223 estimateGravity(convertAngle(latitude), convertDistance(height), result);
224 }
225
226 /**
227 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
228 *
229 * @param latitude latitude.
230 * @param height height.
231 * @return a new gravity instance containing estimated acceleration due to gravity.
232 */
233 public static NEDGravity estimateGravityAndReturnNew(final Angle latitude, final Distance height) {
234 return estimateGravityAndReturnNew(convertAngle(latitude), convertDistance(height));
235 }
236
237 /**
238 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
239 *
240 * @param position curvilinear position expressed in NED coordinates.
241 * @param result instance where estimated acceleration due to gravity will be stored.
242 */
243 public static void estimateGravity(final NEDPosition position, final NEDGravity result) {
244 estimateGravity(position.getLatitude(), position.getHeight(), result);
245 }
246
247 /**
248 * Estimates acceleration due to gravity resolved about NED for a given position expressed in NED coordinates.
249 *
250 * @param position curvilinear position expressed in NED coordinates.
251 * @return a new gravity instance containing estimated acceleration due to gravity.
252 */
253 public static NEDGravity estimateGravityAndReturnNew(final NEDPosition position) {
254 final var result = new NEDGravity();
255 estimateGravity(position, result);
256 return result;
257 }
258
259 /**
260 * Converts angle to radians.
261 *
262 * @param angle angle to be converted.
263 * @return converted angle expressed in radians.
264 */
265 private static double convertAngle(final Angle angle) {
266 return AngleConverter.convert(angle.getValue().doubleValue(), angle.getUnit(), AngleUnit.RADIANS);
267 }
268
269 /**
270 * Converts distance to meters.
271 *
272 * @param distance distance to be converted.
273 * @return converted distance expressed in meters.
274 */
275 private static double convertDistance(final Distance distance) {
276 return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
277 }
278 }