1 /*
2 * Copyright (C) 2020 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.algebra.WrongSizeException;
19 import com.irurueta.navigation.frames.CoordinateTransformation;
20 import com.irurueta.navigation.frames.FrameType;
21 import com.irurueta.navigation.inertial.BodyMagneticFluxDensity;
22 import com.irurueta.navigation.inertial.wmm.NEDMagneticFluxDensity;
23 import com.irurueta.units.Angle;
24 import com.irurueta.units.AngleConverter;
25 import com.irurueta.units.AngleUnit;
26
27 /**
28 * Estimates magnetic flux density resolved around body coordinates for
29 * a given Earth magnetic flux density an a given body attitude.
30 */
31 public class BodyMagneticFluxDensityEstimator {
32
33 /**
34 * Private constructor to prevent instantiation.
35 */
36 private BodyMagneticFluxDensityEstimator() {
37 }
38
39 /**
40 * Computes expected measured body magnetic flux density for a given Earth
41 * magnetic flux density and a certain body attitude (a.k.a. orientation).
42 *
43 * @param magnitude magnitude (a.k.a. intensity) of Earth magnetic flux
44 * density expressed in Teslas (T).
45 * @param declination declination angle expressed in radians (rad).
46 * @param dip dip (a.k.a. inclination) angle expressed in radians
47 * (rad).
48 * @param roll body roll angle expressed in radians (rad).
49 * @param pitch body pitch angle expressed in radians (rad).
50 * @param yaw body yaw angle expressed in radians (rad).
51 * @param result instance where resulting magnetic flux density
52 * measured in body coordinates will be stored.
53 */
54 @SuppressWarnings("DuplicatedCode")
55 public static void estimate(
56 final double magnitude, final double declination, final double dip,
57 final double roll, final double pitch, final double yaw, final BodyMagneticFluxDensity result) {
58
59 final var magneticHeading = yaw - declination;
60
61 final var cosHeading = Math.cos(magneticHeading);
62 final var sinHeading = Math.sin(magneticHeading);
63
64 final var cosDip = Math.cos(dip);
65 final var sinDip = Math.sin(dip);
66
67 // notice that bn and be are not really pointing towards north and
68 // east, instead they are affected by the amount of declination.
69 final var bn = cosHeading * cosDip * magnitude;
70 final var be = sinHeading * cosDip * magnitude;
71 final var bd = sinDip * magnitude;
72
73 final var sinRoll = Math.sin(roll);
74 final var cosRoll = Math.cos(roll);
75 final var sinPitch = Math.sin(pitch);
76 final var cosPitch = Math.cos(pitch);
77
78 final var bx = cosPitch * bn - sinPitch * bd;
79 final var by = sinRoll * sinPitch * bn - cosRoll * be + sinRoll * cosPitch * bd;
80 final var bz = cosRoll * sinPitch * bn + sinRoll * be + cosRoll * cosPitch * bd;
81
82 result.setCoordinates(bx, by, bz);
83 }
84
85 /**
86 * Computes expected measured body magnetic flux density for a given Earth
87 * magnetic flux density and a certain body attitude (a.k.a. orientation).
88 *
89 * @param magnitude magnitude (a.k.a. intensity) of Earth magnetic flux
90 * density expressed in Teslas (T).
91 * @param declination declination angle expressed in radians (rad).
92 * @param dip dip (a.k.a. inclination) angle expressed in radians
93 * (rad).
94 * @param c coordinate transformation from NED to body
95 * coordinates.
96 * @param result instance where resulting magnetic flux density
97 * measured in body coordinates will be stored.
98 */
99 public static void estimate(
100 final double magnitude, final double declination, final double dip, final CoordinateTransformation c,
101 final BodyMagneticFluxDensity result) {
102 final var roll = c.getRollEulerAngle();
103 final var pitch = c.getPitchEulerAngle();
104 final var yaw = c.getYawEulerAngle();
105
106 estimate(magnitude, declination, dip, roll, pitch, yaw, result);
107 }
108
109 /**
110 * Computes expected measured body magnetic flux density for a given Earth
111 * magnetic flux density and a certain body attitude (a.k.a. orientation).
112 *
113 * @param earthB Earth magnetic flux density.
114 * @param roll body roll angle expressed in radians (rad).
115 * @param pitch body pitch angle expressed in radians (rad).
116 * @param yaw body yaw angle expressed in radians (rad).
117 * @param result instance where resulting magnetic flux density
118 * measured in body coordinates will be stored.
119 */
120 public static void estimate(
121 final NEDMagneticFluxDensity earthB, final double roll, final double pitch, final double yaw,
122 final BodyMagneticFluxDensity result) {
123
124 final var c = new CoordinateTransformation(roll, pitch, yaw, FrameType.LOCAL_NAVIGATION_FRAME,
125 FrameType.BODY_FRAME);
126 estimate(earthB, c, result);
127 }
128
129 /**
130 * Computes expected measured body magnetic flux density for a given Earth
131 * magnetic flux density and a certain body attitude (a.k.a. orientation).
132 *
133 * @param earthB Earth magnetic flux density.
134 * @param c coordinate transformation from NED to body coordinates.
135 * @param result instance where resulting magnetic flux density measured
136 * in body coordinates will be stored.
137 */
138 public static void estimate(
139 final NEDMagneticFluxDensity earthB, final CoordinateTransformation c,
140 final BodyMagneticFluxDensity result) {
141
142 try {
143 final var bm = earthB.asMatrix();
144 final var cbn = c.getMatrix();
145 cbn.multiply(bm);
146
147 // cbn now contains magnetic flux density in body coordinates
148 final var bx = cbn.getElementAtIndex(0);
149 final var by = cbn.getElementAtIndex(1);
150 final var bz = cbn.getElementAtIndex(2);
151
152 result.setCoordinates(bx, by, bz);
153 } catch (final WrongSizeException ignore) {
154 // never happens
155 }
156 }
157
158 /**
159 * Computes expected measured body magnetic flux density for a given Earth
160 * magnetic flux density and a certain body attitude (a.k.a. orientation).
161 *
162 * @param magnitude magnitude (a.k.a. intensity) of Earth magnetic flux
163 * density expressed in Teslas (T).
164 * @param declination declination angle.
165 * @param dip dip (a.k.a. inclination) angle.
166 * @param roll body roll angle expressed.
167 * @param pitch body pitch angle.
168 * @param yaw body yaw angle.
169 * @param result instance where resulting magnetic flux density
170 * measured in body coordinates will be stored.
171 */
172 public static void estimate(
173 final double magnitude, final Angle declination, final Angle dip,
174 final Angle roll, final Angle pitch, final Angle yaw, final BodyMagneticFluxDensity result) {
175 estimate(magnitude, convertAngle(declination), convertAngle(dip),
176 convertAngle(roll), convertAngle(pitch), convertAngle(yaw), result);
177 }
178
179 /**
180 * Computes expected measured body magnetic flux density for a given Earth
181 * magnetic flux density and a certain body attitude (a.k.a. orientation).
182 *
183 * @param magnitude magnitude (a.k.a. intensity) of Earth magnetic flux
184 * density expressed in Teslas (T).
185 * @param declination declination angle.
186 * @param dip dip (a.k.a. inclination) angle.
187 * @param c coordinate transformation from NED to body
188 * coordinates.
189 * @param result instance where resulting magnetic flux density
190 * measured in body coordinates will be stored.
191 */
192 public static void estimate(
193 final double magnitude, final Angle declination, final Angle dip,
194 final CoordinateTransformation c, final BodyMagneticFluxDensity result) {
195 estimate(magnitude, convertAngle(declination), convertAngle(dip), c, result);
196 }
197
198 /**
199 * Computes expected measured body magnetic flux density for a given Earth
200 * magnetic flux density and a certain body attitude (a.k.a. orientation).
201 *
202 * @param earthB Earth magnetic flux density.
203 * @param roll body roll angle.
204 * @param pitch body pitch angle.
205 * @param yaw body yaw angle.
206 * @param result instance where resulting magnetic flux density
207 * measured in body coordinates will be stored.
208 */
209 public static void estimate(
210 final NEDMagneticFluxDensity earthB,
211 final Angle roll, final Angle pitch, final Angle yaw, final BodyMagneticFluxDensity result) {
212 estimate(earthB, convertAngle(roll), convertAngle(pitch), convertAngle(yaw), result);
213 }
214
215 /**
216 * Computes expected measured body magnetic flux density for a given Earth
217 * magnetic flux density and a certain body attitude (a.k.a. orientation).
218 *
219 * @param magnitude magnitude (a.k.a. intensity) of Earth magnetic flux
220 * density expressed in Teslas (T).
221 * @param declination declination angle expressed in radians (rad).
222 * @param dip dip (a.k.a. inclination) angle expressed in radians
223 * (rad).
224 * @param roll body roll angle expressed in radians (rad).
225 * @param pitch body pitch angle expressed in radians (rad).
226 * @param yaw body yaw angle expressed in radians (rad).
227 * @return measured magnetic flux density resolved in body coordinates.
228 */
229 public static BodyMagneticFluxDensity estimate(
230 final double magnitude, final double declination, final double dip,
231 final double roll, final double pitch, final double yaw) {
232 final var result = new BodyMagneticFluxDensity();
233 estimate(magnitude, declination, dip, roll, pitch, yaw, result);
234 return result;
235 }
236
237 /**
238 * Computes expected measured body magnetic flux density for a given Earth
239 * magnetic flux density and a certain body attitude (a.k.a. orientation).
240 *
241 * @param magnitude magnitude (a.k.a. intensity) of Earth magnetic flux
242 * density expressed in Teslas (T).
243 * @param declination declination angle expressed in radians (rad).
244 * @param dip dip (a.k.a. inclination) angle expressed in radians
245 * (rad).
246 * @param c coordinate transformation from NED to body
247 * coordinates.
248 * @return measured magnetic flux density resolved in body coordinates.
249 */
250 public static BodyMagneticFluxDensity estimate(
251 final double magnitude, final double declination, final double dip, final CoordinateTransformation c) {
252 final var result = new BodyMagneticFluxDensity();
253 estimate(magnitude, declination, dip, c, result);
254 return result;
255 }
256
257 /**
258 * Computes expected measured body magnetic flux density for a given Earth
259 * magnetic flux density and a certain body attitude (a.k.a. orientation).
260 *
261 * @param earthB Earth magnetic flux density.
262 * @param roll body roll angle expressed in radians (rad).
263 * @param pitch body pitch angle expressed in radians (rad).
264 * @param yaw body yaw angle expressed in radians (rad).
265 * @return measured magnetic flux density resolved in body coordinates.
266 */
267 public static BodyMagneticFluxDensity estimate(
268 final NEDMagneticFluxDensity earthB, final double roll, final double pitch, final double yaw) {
269 final var result = new BodyMagneticFluxDensity();
270 estimate(earthB, roll, pitch, yaw, result);
271 return result;
272 }
273
274 /**
275 * Computes expected measured body magnetic flux density for a given Earth
276 * magnetic flux density and a certain body attitude (a.k.a. orientation).
277 *
278 * @param earthB Earth magnetic flux density.
279 * @param c coordinate transformation from NED to body coordinates.
280 * @return measured magnetic flux density resolved in body coordinates.
281 */
282 public static BodyMagneticFluxDensity estimate(
283 final NEDMagneticFluxDensity earthB, final CoordinateTransformation c) {
284 final var result = new BodyMagneticFluxDensity();
285 estimate(earthB, c, result);
286 return result;
287 }
288
289 /**
290 * Computes expected measured body magnetic flux density for a given Earth
291 * magnetic flux density and a certain body attitude (a.k.a. orientation).
292 *
293 * @param magnitude magnitude (a.k.a. intensity) of Earth magnetic flux
294 * density expressed in Teslas (T).
295 * @param declination declination angle.
296 * @param dip dip (a.k.a. inclination) angle.
297 * @param roll body roll angle expressed.
298 * @param pitch body pitch angle.
299 * @param yaw body yaw angle.
300 * @return measured magnetic flux density resolved in body coordinates.
301 */
302 public static BodyMagneticFluxDensity estimate(
303 final double magnitude, final Angle declination, final Angle dip,
304 final Angle roll, final Angle pitch, final Angle yaw) {
305 final var result = new BodyMagneticFluxDensity();
306 estimate(magnitude, declination, dip, roll, pitch, yaw, result);
307 return result;
308 }
309
310 /**
311 * Computes expected measured body magnetic flux density for a given Earth
312 * magnetic flux density and a certain body attitude (a.k.a. orientation).
313 *
314 * @param magnitude magnitude (a.k.a. intensity) of Earth magnetic flux
315 * density expressed in Teslas (T).
316 * @param declination declination angle.
317 * @param dip dip (a.k.a. inclination) angle.
318 * @param c coordinate transformation from NED to body
319 * coordinates.
320 * @return measured magnetic flux density resolved in body coordinates.
321 */
322 public static BodyMagneticFluxDensity estimate(
323 final double magnitude, final Angle declination, final Angle dip, final CoordinateTransformation c) {
324 final var result = new BodyMagneticFluxDensity();
325 estimate(magnitude, declination, dip, c, result);
326 return result;
327 }
328
329 /**
330 * Computes expected measured body magnetic flux density for a given Earth
331 * magnetic flux density and a certain body attitude (a.k.a. orientation).
332 *
333 * @param earthB Earth magnetic flux density.
334 * @param roll body roll angle.
335 * @param pitch body pitch angle.
336 * @param yaw body yaw angle.
337 * @return measured magnetic flux density resolved in body coordinates.
338 */
339 public static BodyMagneticFluxDensity estimate(
340 final NEDMagneticFluxDensity earthB, final Angle roll, final Angle pitch, final Angle yaw) {
341 final var result = new BodyMagneticFluxDensity();
342 estimate(earthB, roll, pitch, yaw, result);
343 return result;
344 }
345
346 /**
347 * Converts a given angle instance into radians.
348 *
349 * @param angle angle to be converted.
350 * @return converted value expressed in radians.
351 */
352 private static double convertAngle(final Angle angle) {
353 return AngleConverter.convert(angle.getValue().doubleValue(), angle.getUnit(), AngleUnit.RADIANS);
354 }
355 }