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.navigation.frames.CoordinateTransformation;
19 import com.irurueta.navigation.frames.FrameType;
20 import com.irurueta.navigation.frames.NEDPosition;
21 import com.irurueta.navigation.inertial.BodyKinematics;
22 import com.irurueta.navigation.inertial.BodyMagneticFluxDensity;
23 import com.irurueta.navigation.inertial.wmm.EarthMagneticFluxDensityEstimator;
24 import com.irurueta.navigation.inertial.wmm.NEDMagneticFluxDensity;
25 import com.irurueta.navigation.inertial.wmm.WMMEarthMagneticFluxDensityEstimator;
26 import com.irurueta.navigation.inertial.wmm.WorldMagneticModel;
27 import com.irurueta.units.Acceleration;
28 import com.irurueta.units.AccelerationConverter;
29 import com.irurueta.units.AccelerationUnit;
30 import com.irurueta.units.Angle;
31 import com.irurueta.units.AngleConverter;
32 import com.irurueta.units.AngleUnit;
33 import com.irurueta.units.Distance;
34 import com.irurueta.units.DistanceConverter;
35 import com.irurueta.units.DistanceUnit;
36
37 import java.io.IOException;
38 import java.util.Date;
39 import java.util.GregorianCalendar;
40
41 /**
42 * Estimates the attitude of a body by taking into account both accelerometer
43 * and magnetometer measurements.
44 * Although {@link LevelingEstimator} and {@link LevelingEstimator2} can
45 * estimate body attitude just by using IMU measurements (accelerometer +
46 * gyroscope), they require the gyroscope to be accurate enough (aviation
47 * grade accuracy) to obtain reliable results.
48 * When the gyroscope is known to not be accurate (e.g. mobile devices),
49 * this class should be used instead to use a magnetometer to obtain a
50 * reliable attitude estimation.
51 */
52 public class AttitudeEstimator {
53
54 /**
55 * Estimator of the World Magnetic Model.
56 * This is used to determine magnetic declination angle at a given Earth
57 * position and instant.
58 */
59 private final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
60
61 /**
62 * Constructor.
63 * This constructor loads the default WMM, which is valid from
64 * 2020, January 1st until 2025, January 1st.
65 *
66 * @throws IOException if an I/O error occurs while loading the WMM
67 * model.
68 */
69 public AttitudeEstimator() throws IOException {
70 wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
71 }
72
73 /**
74 * Constructor.
75 *
76 * @param model a World Magnetic Model. The model is only valid for
77 * a certain timespan.
78 * @throws NullPointerException if provided model is null.
79 */
80 public AttitudeEstimator(final WorldMagneticModel model) {
81 wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(model);
82 }
83
84 /**
85 * Gets body attitude expressed in the local navigation frame.
86 * Internally this implementation uses a given World Magnetic Model (WMM)
87 * to estimate the magnetic declination angle at a given Earth position
88 * and timestamp.
89 * Besides, a leveling algorithm is used to estimate roll and pitch
90 * attitude angles, which can later be used to estimate yaw attitude angle
91 * with estimated declination angle and provided magnetometer measurements.
92 *
93 * @param latitude Earth position latitude expressed in radians (rad).
94 * @param longitude Earth position longitude expressed in radians (rad).
95 * @param height Earth position height respect average sea level expressed
96 * in meters (m).
97 * @param year a year expressed in decimal value.
98 * @param fx x-coordinate of measured body specific force
99 * expressed in meters per squared second (m/s^2).
100 * @param fy y-coordinate of measured body specific force
101 * expressed in meters per squared second (m/s^2).
102 * @param fz z-coordinate of measured body specific force
103 * expressed in meters per squared second (m/s^2).
104 * @param bx x coordinate of measured magnetic flux density resolved
105 * around body axes and expressed in Teslas (T).
106 * @param by y coordinate of measured magnetic flux density resolved
107 * around body axes and expressed in Teslas (T).
108 * @param bz z coordinate of measured magnetic flux density resolved
109 * around body axes and expressed in Teslas (T).
110 * @param result instance where body attitude will be stored.
111 * @see LevelingEstimator2
112 * @see WMMEarthMagneticFluxDensityEstimator
113 */
114 public void getAttitude(
115 final double latitude, final double longitude, final double height,
116 final double year, final double fx, final double fy, final double fz,
117 final double bx, final double by, final double bz, final CoordinateTransformation result) {
118 final var declination = wmmEstimator.getDeclination(latitude, longitude, height, year);
119 getAttitude(latitude, height, fx, fy, fz, bx, by, bz, declination, result);
120 }
121
122 /**
123 * Gets body attitude expressed in the local navigation frame.
124 * Internally this implementation uses a given World Magnetic Model (WMM)
125 * to estimate the magnetic declination angle at a given Earth position
126 * and timestamp.
127 * Besides, a leveling algorithm is used to estimate roll and pitch
128 * attitude angles, which can later be used to estimate yaw attitude angle
129 * with estimated declination angle and provided magnetometer measurements.
130 *
131 * @param latitude Earth position latitude expressed in radians (rad).
132 * @param longitude Earth position longitude expressed in radians (rad).
133 * @param height Earth position height respect average sea level expressed
134 * in meters (m).
135 * @param year a year expressed in decimal value.
136 * @param fx x-coordinate of measured body specific force
137 * expressed in meters per squared second (m/s^2).
138 * @param fy y-coordinate of measured body specific force
139 * expressed in meters per squared second (m/s^2).
140 * @param fz z-coordinate of measured body specific force
141 * expressed in meters per squared second (m/s^2).
142 * @param bx x coordinate of measured magnetic flux density resolved
143 * around body axes and expressed in Teslas (T).
144 * @param by y coordinate of measured magnetic flux density resolved
145 * around body axes and expressed in Teslas (T).
146 * @param bz z coordinate of measured magnetic flux density resolved
147 * around body axes and expressed in Teslas (T).
148 * @return a coordinate transformation containing body attitude.
149 * @see LevelingEstimator2
150 * @see WMMEarthMagneticFluxDensityEstimator
151 */
152 public CoordinateTransformation getAttitude(
153 final double latitude, final double longitude, final double height,
154 final double year, final double fx, final double fy, final double fz,
155 final double bx, final double by, final double bz) {
156 final var declination = wmmEstimator.getDeclination(latitude, longitude, height, year);
157 return getAttitude(latitude, height, fx, fy, fz, bx, by, bz, declination);
158 }
159
160 /**
161 * Gets body attitude expressed in the local navigation frame.
162 * Internally this implementation uses a given World Magnetic Model (WMM)
163 * to estimate the magnetic declination angle at a given Earth position
164 * and timestamp.
165 * Besides, a leveling algorithm is used to estimate roll and pitch
166 * attitude angles, which can later be used to estimate yaw attitude angle
167 * with estimated declination angle and provided magnetometer measurements.
168 *
169 * @param latitude Earth position latitude expressed in radians (rad).
170 * @param longitude Earth position longitude expressed in radians (rad).
171 * @param height Earth position height respect average sea level expressed
172 * in meters (m).
173 * @param calendar a calendar indicating a given timestamp.
174 * @param fx x-coordinate of measured body specific force
175 * expressed in meters per squared second (m/s^2).
176 * @param fy y-coordinate of measured body specific force
177 * expressed in meters per squared second (m/s^2).
178 * @param fz z-coordinate of measured body specific force
179 * expressed in meters per squared second (m/s^2).
180 * @param bx x coordinate of measured magnetic flux density resolved
181 * around body axes and expressed in Teslas (T).
182 * @param by y coordinate of measured magnetic flux density resolved
183 * around body axes and expressed in Teslas (T).
184 * @param bz z coordinate of measured magnetic flux density resolved
185 * around body axes and expressed in Teslas (T).
186 * @param result instance where body attitude will be stored.
187 * @see LevelingEstimator2
188 * @see WMMEarthMagneticFluxDensityEstimator
189 */
190 public void getAttitude(
191 final double latitude, final double longitude, final double height,
192 final GregorianCalendar calendar, final double fx, final double fy, final double fz,
193 final double bx, final double by, final double bz, final CoordinateTransformation result) {
194 final var declination = wmmEstimator.getDeclination(latitude, longitude, height, calendar);
195 getAttitude(latitude, height, fx, fy, fz, bx, by, bz, declination, result);
196 }
197
198 /**
199 * Gets body attitude expressed in the local navigation frame.
200 * Internally this implementation uses a given World Magnetic Model (WMM)
201 * to estimate the magnetic declination angle at a given Earth position
202 * and timestamp.
203 * Besides, a leveling algorithm is used to estimate roll and pitch
204 * attitude angles, which can later be used to estimate yaw attitude angle
205 * with estimated declination angle and provided magnetometer measurements.
206 *
207 * @param latitude Earth position latitude expressed in radians (rad).
208 * @param longitude Earth position longitude expressed in radians (rad).
209 * @param height Earth position height respect average sea level expressed
210 * in meters (m).
211 * @param calendar a calendar indicating a given timestamp.
212 * @param fx x-coordinate of measured body specific force
213 * expressed in meters per squared second (m/s^2).
214 * @param fy y-coordinate of measured body specific force
215 * expressed in meters per squared second (m/s^2).
216 * @param fz z-coordinate of measured body specific force
217 * expressed in meters per squared second (m/s^2).
218 * @param bx x coordinate of measured magnetic flux density resolved
219 * around body axes and expressed in Teslas (T).
220 * @param by y coordinate of measured magnetic flux density resolved
221 * around body axes and expressed in Teslas (T).
222 * @param bz z coordinate of measured magnetic flux density resolved
223 * around body axes and expressed in Teslas (T).
224 * @return a coordinate transformation containing body attitude.
225 * @see LevelingEstimator2
226 * @see WMMEarthMagneticFluxDensityEstimator
227 */
228 public CoordinateTransformation getAttitude(
229 final double latitude, final double longitude, final double height,
230 final GregorianCalendar calendar, final double fx, final double fy, final double fz,
231 final double bx, final double by, final double bz) {
232 final var declination = wmmEstimator.getDeclination(latitude, longitude, height, calendar);
233 return getAttitude(latitude, height, fx, fy, fz, bx, by, bz, declination);
234 }
235
236 /**
237 * Gets body attitude expressed in the local navigation frame.
238 * Internally this implementation uses a given World Magnetic Model (WMM)
239 * to estimate the magnetic declination angle at a given Earth position
240 * and timestamp.
241 * Besides, a leveling algorithm is used to estimate roll and pitch
242 * attitude angles, which can later be used to estimate yaw attitude angle
243 * with estimated declination angle and provided magnetometer measurements.
244 *
245 * @param latitude Earth position latitude expressed in radians (rad).
246 * @param longitude Earth position longitude expressed in radians (rad).
247 * @param height Earth position height respect average sea level expressed
248 * in meters (m).
249 * @param timestamp a timestamp.
250 * @param fx x-coordinate of measured body specific force
251 * expressed in meters per squared second (m/s^2).
252 * @param fy y-coordinate of measured body specific force
253 * expressed in meters per squared second (m/s^2).
254 * @param fz z-coordinate of measured body specific force
255 * expressed in meters per squared second (m/s^2).
256 * @param bx x coordinate of measured magnetic flux density resolved
257 * around body axes and expressed in Teslas (T).
258 * @param by y coordinate of measured magnetic flux density resolved
259 * around body axes and expressed in Teslas (T).
260 * @param bz z coordinate of measured magnetic flux density resolved
261 * around body axes and expressed in Teslas (T).
262 * @param result instance where body attitude will be stored.
263 * @see LevelingEstimator2
264 * @see WMMEarthMagneticFluxDensityEstimator
265 */
266 public void getAttitude(
267 final double latitude, final double longitude, final double height, final Date timestamp,
268 final double fx, final double fy, final double fz, final double bx, final double by, final double bz,
269 final CoordinateTransformation result) {
270 final var declination = wmmEstimator.getDeclination(latitude, longitude, height, timestamp);
271 getAttitude(latitude, height, fx, fy, fz, bx, by, bz, declination, result);
272 }
273
274 /**
275 * Gets body attitude expressed in the local navigation frame.
276 * Internally this implementation uses a given World Magnetic Model (WMM)
277 * to estimate the magnetic declination angle at a given Earth position
278 * and timestamp.
279 * Besides, a leveling algorithm is used to estimate roll and pitch
280 * attitude angles, which can later be used to estimate yaw attitude angle
281 * with estimated declination angle and provided magnetometer measurements.
282 *
283 * @param latitude Earth position latitude expressed in radians (rad).
284 * @param longitude Earth position longitude expressed in radians (rad).
285 * @param height Earth position height respect average sea level expressed
286 * in meters (m).
287 * @param timestamp a timestamp.
288 * @param fx x-coordinate of measured body specific force
289 * expressed in meters per squared second (m/s^2).
290 * @param fy y-coordinate of measured body specific force
291 * expressed in meters per squared second (m/s^2).
292 * @param fz z-coordinate of measured body specific force
293 * expressed in meters per squared second (m/s^2).
294 * @param bx x coordinate of measured magnetic flux density resolved
295 * around body axes and expressed in Teslas (T).
296 * @param by y coordinate of measured magnetic flux density resolved
297 * around body axes and expressed in Teslas (T).
298 * @param bz z coordinate of measured magnetic flux density resolved
299 * around body axes and expressed in Teslas (T).
300 * @return a coordinate transformation containing body attitude.
301 * @see LevelingEstimator2
302 * @see WMMEarthMagneticFluxDensityEstimator
303 */
304 public CoordinateTransformation getAttitude(
305 final double latitude, final double longitude, final double height, final Date timestamp,
306 final double fx, final double fy, final double fz, final double bx, final double by, final double bz) {
307 final var declination = wmmEstimator.getDeclination(latitude, longitude, height, timestamp);
308 return getAttitude(latitude, height, fx, fy, fz, bx, by, bz, declination);
309 }
310
311 /**
312 * Gets body attitude expressed in the local navigation frame.
313 * Internally this implementation uses a given World Magnetic Model (WMM)
314 * to estimate the magnetic declination angle at a given Earth position
315 * and timestamp.
316 * Besides, a leveling algorithm is used to estimate roll and pitch
317 * attitude angles, which can later be used to estimate yaw attitude angle
318 * with estimated declination angle and provided magnetometer measurements.
319 *
320 * @param position Earth position resolved in NED coordinates.
321 * @param year a year expressed in decimal value.
322 * @param kinematics body kinematics containing measured specific force.
323 * @param b measured body magnetic flux density.
324 * @param result instance where body attitude will be stored.
325 * @see LevelingEstimator2
326 * @see WMMEarthMagneticFluxDensityEstimator
327 */
328 public void getAttitude(
329 final NEDPosition position, final double year, final BodyKinematics kinematics,
330 final BodyMagneticFluxDensity b, final CoordinateTransformation result) {
331 getAttitude(position.getLatitude(), position.getLongitude(), position.getHeight(), year, kinematics.getFx(),
332 kinematics.getFy(), kinematics.getFz(), b.getBx(), b.getBy(), b.getBz(), result);
333 }
334
335 /**
336 * Gets body attitude expressed in the local navigation frame.
337 * Internally this implementation uses a given World Magnetic Model (WMM)
338 * to estimate the magnetic declination angle at a given Earth position
339 * and timestamp.
340 * Besides, a leveling algorithm is used to estimate roll and pitch
341 * attitude angles, which can later be used to estimate yaw attitude angle
342 * with estimated declination angle and provided magnetometer measurements.
343 *
344 * @param position Earth position resolved in NED coordinates.
345 * @param year a year expressed in decimal value.
346 * @param kinematics body kinematics containing measured specific force.
347 * @param b measured body magnetic flux density.
348 * @return a coordinate transformation containing body attitude.
349 * @see LevelingEstimator2
350 * @see WMMEarthMagneticFluxDensityEstimator
351 */
352 public CoordinateTransformation getAttitude(
353 final NEDPosition position, final double year, final BodyKinematics kinematics,
354 final BodyMagneticFluxDensity b) {
355 return getAttitude(position.getLatitude(), position.getLongitude(), position.getHeight(), year,
356 kinematics.getFx(), kinematics.getFy(), kinematics.getFz(), b.getBx(), b.getBy(), b.getBz());
357 }
358
359 /**
360 * Gets body attitude expressed in the local navigation frame.
361 * Internally this implementation uses a given World Magnetic Model (WMM)
362 * to estimate the magnetic declination angle at a given Earth position
363 * and timestamp.
364 * Besides, a leveling algorithm is used to estimate roll and pitch
365 * attitude angles, which can later be used to estimate yaw attitude angle
366 * with estimated declination angle and provided magnetometer measurements.
367 *
368 * @param position Earth position resolved in NED coordinates.
369 * @param calendar a calendar indicating a given timestamp.
370 * @param kinematics body kinematics containing measured specific force.
371 * @param b measured body magnetic flux density.
372 * @param result instance where body attitude will be stored.
373 * @see LevelingEstimator2
374 * @see WMMEarthMagneticFluxDensityEstimator
375 */
376 public void getAttitude(
377 final NEDPosition position, final GregorianCalendar calendar, final BodyKinematics kinematics,
378 final BodyMagneticFluxDensity b, final CoordinateTransformation result) {
379 getAttitude(position.getLatitude(), position.getLongitude(), position.getHeight(), calendar, kinematics.getFx(),
380 kinematics.getFy(), kinematics.getFz(), b.getBx(), b.getBy(), b.getBz(), result);
381 }
382
383 /**
384 * Gets body attitude expressed in the local navigation frame.
385 * Internally this implementation uses a given World Magnetic Model (WMM)
386 * to estimate the magnetic declination angle at a given Earth position
387 * and timestamp.
388 * Besides, a leveling algorithm is used to estimate roll and pitch
389 * attitude angles, which can later be used to estimate yaw attitude angle
390 * with estimated declination angle and provided magnetometer measurements.
391 *
392 * @param position Earth position resolved in NED coordinates.
393 * @param calendar a calendar indicating a given timestamp.
394 * @param kinematics body kinematics containing measured specific force.
395 * @param b measured body magnetic flux density.
396 * @return a coordinate transformation containing body attitude.
397 * @see LevelingEstimator2
398 * @see WMMEarthMagneticFluxDensityEstimator
399 */
400 public CoordinateTransformation getAttitude(
401 final NEDPosition position, final GregorianCalendar calendar, final BodyKinematics kinematics,
402 final BodyMagneticFluxDensity b) {
403 return getAttitude(position.getLatitude(), position.getLongitude(), position.getHeight(), calendar,
404 kinematics.getFx(), kinematics.getFy(), kinematics.getFz(), b.getBx(), b.getBy(), b.getBz());
405 }
406
407 /**
408 * Gets body attitude expressed in the local navigation frame.
409 * Internally this implementation uses a given World Magnetic Model (WMM)
410 * to estimate the magnetic declination angle at a given Earth position
411 * and timestamp.
412 * Besides, a leveling algorithm is used to estimate roll and pitch
413 * attitude angles, which can later be used to estimate yaw attitude angle
414 * with estimated declination angle and provided magnetometer measurements.
415 *
416 * @param position Earth position resolved in NED coordinates.
417 * @param timestamp a timestamp.
418 * @param kinematics body kinematics containing measured specific force.
419 * @param b measured body magnetic flux density.
420 * @param result instance where body attitude will be stored.
421 * @see LevelingEstimator2
422 * @see WMMEarthMagneticFluxDensityEstimator
423 */
424 public void getAttitude(
425 final NEDPosition position, final Date timestamp, final BodyKinematics kinematics,
426 final BodyMagneticFluxDensity b, final CoordinateTransformation result) {
427 getAttitude(position.getLatitude(), position.getLongitude(), position.getHeight(), timestamp,
428 kinematics.getFx(), kinematics.getFy(), kinematics.getFz(), b.getBx(), b.getBy(), b.getBz(), result);
429 }
430
431 /**
432 * Gets body attitude expressed in the local navigation frame.
433 * Internally this implementation uses a given World Magnetic Model (WMM)
434 * to estimate the magnetic declination angle at a given Earth position
435 * and timestamp.
436 * Besides, a leveling algorithm is used to estimate roll and pitch
437 * attitude angles, which can later be used to estimate yaw attitude angle
438 * with estimated declination angle and provided magnetometer measurements.
439 *
440 * @param position Earth position resolved in NED coordinates.
441 * @param timestamp a timestamp.
442 * @param kinematics body kinematics containing measured specific force.
443 * @param b measured body magnetic flux density.
444 * @return a coordinate transformation containing body attitude.
445 * @see LevelingEstimator2
446 * @see WMMEarthMagneticFluxDensityEstimator
447 */
448 public CoordinateTransformation getAttitude(
449 final NEDPosition position, final Date timestamp, final BodyKinematics kinematics,
450 final BodyMagneticFluxDensity b) {
451 return getAttitude(position.getLatitude(), position.getLongitude(), position.getHeight(), timestamp,
452 kinematics.getFx(), kinematics.getFy(), kinematics.getFz(), b.getBx(), b.getBy(), b.getBz());
453 }
454
455 /**
456 * Gets body attitude expressed in the local navigation frame.
457 * Internally this implementation uses a leveling algorithm to estimate
458 * roll and pitch attitude angles, which can later be used to find
459 * yaw attitude angle along with magnetometer measurements.
460 *
461 * @param fx x-coordinate of measured body specific force
462 * expressed in meters per squared second (m/s^2).
463 * @param fy y-coordinate of measured body specific force
464 * expressed in meters per squared second (m/s^2).
465 * @param fz z-coordinate of measured body specific force
466 * expressed in meters per squared second (m/s^2).
467 * @param bx x coordinate of measured magnetic flux density resolved
468 * around body axes and expressed in Teslas (T).
469 * @param by y coordinate of measured magnetic flux density resolved
470 * around body axes and expressed in Teslas (T).
471 * @param bz z coordinate of measured magnetic flux density resolved
472 * around body axes and expressed in Teslas (T).
473 * @param declination declination angle of Earth magnetic flux density
474 * at current Earth position and instant expressed in
475 * radians. Declination angle changes depending on
476 * instant and location. Declination angle can be
477 * obtained through
478 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
479 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
480 * @param result instance where body attitude will be stored.
481 * @see WMMEarthMagneticFluxDensityEstimator
482 * @see EarthMagneticFluxDensityEstimator
483 * @see LevelingEstimator
484 */
485 public static void getAttitude(
486 final double fx, final double fy, final double fz,
487 final double bx, final double by, final double bz, final double declination,
488 final CoordinateTransformation result) {
489 result.setSourceType(FrameType.LOCAL_NAVIGATION_FRAME);
490 result.setDestinationType(FrameType.BODY_FRAME);
491
492 final var roll = LevelingEstimator.getRoll(fy, fz);
493 final var pitch = LevelingEstimator.getPitch(fx, fy, fz);
494 final var yaw = getYaw(bx, by, bz, declination, roll, pitch);
495
496 result.setEulerAngles(roll, pitch, yaw);
497 }
498
499 /**
500 * Gets body attitude expressed in the local navigation frame.
501 * Internally this implementation uses a leveling algorithm to estimate
502 * roll and pitch attitude angles, which can later be used to find
503 * yaw attitude angle along with magnetometer measurements.
504 *
505 * @param fx x-coordinate of measured body specific force
506 * expressed in meters per squared second (m/s^2).
507 * @param fy y-coordinate of measured body specific force
508 * expressed in meters per squared second (m/s^2).
509 * @param fz z-coordinate of measured body specific force
510 * expressed in meters per squared second (m/s^2).
511 * @param bx x coordinate of measured magnetic flux density resolved
512 * around body axes and expressed in Teslas (T).
513 * @param by y coordinate of measured magnetic flux density resolved
514 * around body axes and expressed in Teslas (T).
515 * @param bz z coordinate of measured magnetic flux density resolved
516 * around body axes and expressed in Teslas (T).
517 * @param declination declination angle of Earth magnetic flux density
518 * at current Earth position and instant expressed in
519 * radians. Declination angle changes depending on
520 * instant and location. Declination angle can be
521 * obtained through
522 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
523 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
524 * @return a coordinate transformation containing body attitude.
525 * @see WMMEarthMagneticFluxDensityEstimator
526 * @see EarthMagneticFluxDensityEstimator
527 * @see LevelingEstimator
528 */
529 public static CoordinateTransformation getAttitude(
530 final double fx, final double fy, final double fz,
531 final double bx, final double by, final double bz, final double declination) {
532
533 final var roll = LevelingEstimator.getRoll(fy, fz);
534 final var pitch = LevelingEstimator.getPitch(fx, fy, fz);
535 final var yaw = getYaw(bx, by, bz, declination, roll, pitch);
536
537 return new CoordinateTransformation(roll, pitch, yaw, FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
538 }
539
540 /**
541 * Gets body attitude expressed in the local navigation frame.
542 * Internally this implementation uses a leveling algorithm to estimate
543 * roll and pitch attitude angles, which can later be used to find
544 * yaw attitude angle along with magnetometer measurements.
545 *
546 * @param fx x-coordinate of measured body specific force.
547 * @param fy y-coordinate of measured body specific force.
548 * @param fz z-coordinate of measured body specific force.
549 * @param bx x coordinate of measured magnetic flux density resolved
550 * around body axes and expressed in Teslas (T).
551 * @param by y coordinate of measured magnetic flux density resolved
552 * around body axes and expressed in Teslas (T).
553 * @param bz z coordinate of measured magnetic flux density resolved
554 * around body axes and expressed in Teslas (T).
555 * @param declination declination angle of Earth magnetic flux density
556 * at current Earth position and instant. Declination
557 * angle changes depending on instant and location.
558 * Declination angle can be obtained through
559 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
560 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
561 * @param result instance where body attitude will be stored.
562 * @see WMMEarthMagneticFluxDensityEstimator
563 * @see EarthMagneticFluxDensityEstimator
564 * @see LevelingEstimator
565 */
566 public static void getAttitude(
567 final Acceleration fx, final Acceleration fy, final Acceleration fz,
568 final double bx, final double by, final double bz, final Angle declination,
569 final CoordinateTransformation result) {
570 getAttitude(convertAcceleration(fx), convertAcceleration(fy), convertAcceleration(fz), bx, by, bz,
571 convertAngle(declination), result);
572 }
573
574 /**
575 * Gets body attitude expressed in the local navigation frame.
576 * Internally this implementation uses a leveling algorithm to estimate
577 * roll and pitch attitude angles, which can later be used to find
578 * yaw attitude angle along with magnetometer measurements.
579 *
580 * @param fx x-coordinate of measured body specific force.
581 * @param fy y-coordinate of measured body specific force.
582 * @param fz z-coordinate of measured body specific force.
583 * @param bx x coordinate of measured magnetic flux density resolved
584 * around body axes and expressed in Teslas (T).
585 * @param by y coordinate of measured magnetic flux density resolved
586 * around body axes and expressed in Teslas (T).
587 * @param bz z coordinate of measured magnetic flux density resolved
588 * around body axes and expressed in Teslas (T).
589 * @param declination declination angle of Earth magnetic flux density
590 * at current Earth position and instant. Declination
591 * angle changes depending on instant and location.
592 * Declination angle can be obtained through
593 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
594 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
595 * @return a coordinate transformation containing body attitude.
596 * @see WMMEarthMagneticFluxDensityEstimator
597 * @see EarthMagneticFluxDensityEstimator
598 * @see LevelingEstimator
599 */
600 public static CoordinateTransformation getAttitude(
601 final Acceleration fx, final Acceleration fy, final Acceleration fz,
602 final double bx, final double by, final double bz, final Angle declination) {
603 return getAttitude(convertAcceleration(fx), convertAcceleration(fy), convertAcceleration(fz), bx, by, bz,
604 convertAngle(declination));
605 }
606
607 /**
608 * Gets body attitude expressed in the local navigation frame.
609 * Internally this implementation uses a leveling algorithm to estimate
610 * roll and pitch attitude angles, which can later be used to find
611 * yaw attitude angle along with magnetometer measurements.
612 *
613 * @param kinematics body kinematics containing measured specific force.
614 * @param b measured body magnetic flux density.
615 * @param declination declination angle of Earth magnetic flux density
616 * at current Earth position and instant expressed in
617 * radians. Declination angle changes depending on
618 * instant and location. Declination angle can be
619 * obtained through
620 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
621 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
622 * @param result instance where body attitude will be stored.
623 * @see WMMEarthMagneticFluxDensityEstimator
624 * @see EarthMagneticFluxDensityEstimator
625 * @see LevelingEstimator
626 */
627 public static void getAttitude(
628 final BodyKinematics kinematics, final BodyMagneticFluxDensity b, final double declination,
629 final CoordinateTransformation result) {
630 getAttitude(kinematics.getFx(), kinematics.getFy(), kinematics.getFz(), b.getBx(), b.getBy(), b.getBz(),
631 declination, result);
632 }
633
634 /**
635 * Gets body attitude expressed in the local navigation frame.
636 * Internally this implementation uses a leveling algorithm to estimate
637 * roll and pitch attitude angles, which can later be used to find
638 * yaw attitude angle along with magnetometer measurements.
639 *
640 * @param kinematics body kinematics containing measured specific force.
641 * @param b measured body magnetic flux density.
642 * @param declination declination angle of Earth magnetic flux density
643 * at current Earth position and instant expressed in
644 * radians. Declination angle changes depending on
645 * instant and location. Declination angle can be
646 * obtained through
647 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
648 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
649 * @return a coordinate transformation containing body attitude.
650 * @see WMMEarthMagneticFluxDensityEstimator
651 * @see EarthMagneticFluxDensityEstimator
652 * @see LevelingEstimator
653 */
654 public static CoordinateTransformation getAttitude(
655 final BodyKinematics kinematics, final BodyMagneticFluxDensity b, final double declination) {
656 return getAttitude(kinematics.getFx(), kinematics.getFy(), kinematics.getFz(), b.getBx(), b.getBy(), b.getBz(),
657 declination);
658 }
659
660 /**
661 * Gets body attitude expressed in the local navigation frame.
662 * Internally this implementation uses a leveling algorithm to estimate
663 * roll and pitch attitude angles, which can later be used to find
664 * yaw attitude angle along with magnetometer measurements.
665 *
666 * @param kinematics body kinematics containing measured specific force.
667 * @param b measured body magnetic flux density.
668 * @param declination declination angle of Earth magnetic flux density
669 * at current Earth position and instant. Declination
670 * angle changes depending on instant and location.
671 * Declination angle can be obtained through
672 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
673 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
674 * @param result instance where body attitude will be stored.
675 * @see WMMEarthMagneticFluxDensityEstimator
676 * @see EarthMagneticFluxDensityEstimator
677 * @see LevelingEstimator
678 */
679 public static void getAttitude(
680 final BodyKinematics kinematics, final BodyMagneticFluxDensity b, final Angle declination,
681 final CoordinateTransformation result) {
682 getAttitude(kinematics, b, convertAngle(declination), result);
683 }
684
685 /**
686 * Gets body attitude expressed in the local navigation frame.
687 * Internally this implementation uses a leveling algorithm to estimate
688 * roll and pitch attitude angles, which can later be used to find
689 * yaw attitude angle along with magnetometer measurements.
690 *
691 * @param kinematics body kinematics containing measured specific force.
692 * @param b measured body magnetic flux density.
693 * @param declination declination angle of Earth magnetic flux density
694 * at current Earth position and instant. Declination
695 * angle changes depending on instant and location.
696 * Declination angle can be obtained through
697 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
698 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
699 * @return a coordinate transformation containing body attitude.
700 * @see WMMEarthMagneticFluxDensityEstimator
701 * @see EarthMagneticFluxDensityEstimator
702 * @see LevelingEstimator
703 */
704 public static CoordinateTransformation getAttitude(
705 final BodyKinematics kinematics, final BodyMagneticFluxDensity b, final Angle declination) {
706 return getAttitude(kinematics, b, convertAngle(declination));
707 }
708
709 /**
710 * Gets body attitude expressed in the local navigation frame.
711 * Internally this implementation uses a leveling algorithm to estimate
712 * roll and pitch attitude angles, which can later be used to find
713 * yaw attitude angle along with magnetometer measurements.
714 * Because latitude and height are also known, those are taken into
715 * account considering actual Earth shape to obtain a slightly more
716 * accurate estimation of roll and pitch attitude angles (and thus also
717 * a more accurate yaw angle estimation).
718 *
719 * @param latitude device latitude expressed in radians (rad).
720 * @param height device height expressed in meters (m).
721 * @param fx x-coordinate of measured body specific force
722 * expressed in meters per squared second (m/s^2).
723 * @param fy y-coordinate of measured body specific force
724 * expressed in meters per squared second (m/s^2).
725 * @param fz z-coordinate of measured body specific force
726 * expressed in meters per squared second (m/s^2).
727 * @param bx x coordinate of measured magnetic flux density resolved
728 * around body axes and expressed in Teslas (T).
729 * @param by y coordinate of measured magnetic flux density resolved
730 * around body axes and expressed in Teslas (T).
731 * @param bz z coordinate of measured magnetic flux density resolved
732 * around body axes and expressed in Teslas (T).
733 * @param declination declination angle of Earth magnetic flux density
734 * at current Earth position and instant expressed in
735 * radians. Declination angle changes depending on
736 * instant and location. Declination angle can be
737 * obtained through
738 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
739 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
740 * @param result instance where body attitude will be stored.
741 * @see WMMEarthMagneticFluxDensityEstimator
742 * @see EarthMagneticFluxDensityEstimator
743 * @see LevelingEstimator2
744 */
745 public static void getAttitude(
746 final double latitude, final double height, final double fx, final double fy, final double fz,
747 final double bx, final double by, final double bz, final double declination,
748 final CoordinateTransformation result) {
749
750 LevelingEstimator2.getPartialAttitude(latitude, height, fx, fy, fz, result);
751
752 // fix yaw angle
753 final var roll = result.getRollEulerAngle();
754 final var pitch = result.getPitchEulerAngle();
755 final var yaw = getYaw(bx, by, bz, declination, roll, pitch);
756
757 result.setEulerAngles(roll, pitch, yaw);
758 }
759
760 /**
761 * Gets body attitude expressed in the local navigation frame.
762 * Internally this implementation uses a leveling algorithm to estimate
763 * roll and pitch attitude angles, which can later be used to find
764 * yaw attitude angle along with magnetometer measurements.
765 * Because latitude and height are also known, those are taken into
766 * account considering actual Earth shape to obtain a slightly more
767 * accurate estimation of roll and pitch attitude angles (and thus also
768 * a more accurate yaw angle estimation).
769 *
770 * @param latitude device latitude expressed in radians (rad).
771 * @param height device height expressed in meters (m).
772 * @param fx x-coordinate of measured body specific force
773 * expressed in meters per squared second (m/s^2).
774 * @param fy y-coordinate of measured body specific force
775 * expressed in meters per squared second (m/s^2).
776 * @param fz z-coordinate of measured body specific force
777 * expressed in meters per squared second (m/s^2).
778 * @param bx x coordinate of measured magnetic flux density resolved
779 * around body axes and expressed in Teslas (T).
780 * @param by y coordinate of measured magnetic flux density resolved
781 * around body axes and expressed in Teslas (T).
782 * @param bz z coordinate of measured magnetic flux density resolved
783 * around body axes and expressed in Teslas (T).
784 * @param declination declination angle of Earth magnetic flux density
785 * at current Earth position and instant expressed in
786 * radians. Declination angle changes depending on
787 * instant and location. Declination angle can be
788 * obtained through
789 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
790 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
791 * @return a coordinate transformation containing body attitude.
792 * @see WMMEarthMagneticFluxDensityEstimator
793 * @see EarthMagneticFluxDensityEstimator
794 * @see LevelingEstimator2
795 */
796 public static CoordinateTransformation getAttitude(
797 final double latitude, final double height, final double fx, final double fy, final double fz,
798 final double bx, final double by, final double bz, final double declination) {
799
800 final var result = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
801 getAttitude(latitude, height, fx, fy, fz, bx, by, bz, declination, result);
802 return result;
803 }
804
805 /**
806 * Gets body attitude expressed in the local navigation frame.
807 * Internally this implementation uses a leveling algorithm to estimate
808 * roll and pitch attitude angles, which can later be used to find
809 * yaw attitude angle along with magnetometer measurements.
810 * Because latitude and height are also known, those are taken into
811 * account considering actual Earth shape to obtain a slightly more
812 * accurate estimation of roll and pitch attitude angles (and thus also
813 * a more accurate yaw angle estimation).
814 *
815 * @param latitude device latitude.
816 * @param height device height.
817 * @param fx x-coordinate of measured body specific force.
818 * @param fy y-coordinate of measured body specific force.
819 * @param fz z-coordinate of measured body specific force.
820 * @param bx x coordinate of measured magnetic flux density resolved
821 * around body axes and expressed in Teslas (T).
822 * @param by y coordinate of measured magnetic flux density resolved
823 * around body axes and expressed in Teslas (T).
824 * @param bz z coordinate of measured magnetic flux density resolved
825 * around body axes and expressed in Teslas (T).
826 * @param declination declination angle of Earth magnetic flux density
827 * at current Earth position and instant. Declination
828 * angle changes depending on instant and location.
829 * Declination angle can be obtained through
830 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
831 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
832 * @param result instance where body attitude will be stored.
833 * @see WMMEarthMagneticFluxDensityEstimator
834 * @see EarthMagneticFluxDensityEstimator
835 * @see LevelingEstimator2
836 */
837 public static void getAttitude(
838 final Angle latitude, final Distance height,
839 final Acceleration fx, final Acceleration fy, final Acceleration fz,
840 final double bx, final double by, final double bz, final Angle declination,
841 final CoordinateTransformation result) {
842 getAttitude(convertAngle(latitude), convertDistance(height), convertAcceleration(fx), convertAcceleration(fy),
843 convertAcceleration(fz), bx, by, bz, convertAngle(declination), result);
844 }
845
846 /**
847 * Gets body attitude expressed in the local navigation frame.
848 * Internally this implementation uses a leveling algorithm to estimate
849 * roll and pitch attitude angles, which can later be used to find
850 * yaw attitude angle along with magnetometer measurements.
851 * Because latitude and height are also known, those are taken into
852 * account considering actual Earth shape to obtain a slightly more
853 * accurate estimation of roll and pitch attitude angles (and thus also
854 * a more accurate yaw angle estimation).
855 *
856 * @param latitude device latitude.
857 * @param height device height.
858 * @param fx x-coordinate of measured body specific force.
859 * @param fy y-coordinate of measured body specific force.
860 * @param fz z-coordinate of measured body specific force.
861 * @param bx x coordinate of measured magnetic flux density resolved
862 * around body axes and expressed in Teslas (T).
863 * @param by y coordinate of measured magnetic flux density resolved
864 * around body axes and expressed in Teslas (T).
865 * @param bz z coordinate of measured magnetic flux density resolved
866 * around body axes and expressed in Teslas (T).
867 * @param declination declination angle of Earth magnetic flux density
868 * at current Earth position and instant. Declination
869 * angle changes depending on instant and location.
870 * Declination angle can be obtained through
871 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
872 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
873 * @return a coordinate transformation containing body attitude.
874 * @see WMMEarthMagneticFluxDensityEstimator
875 * @see EarthMagneticFluxDensityEstimator
876 * @see LevelingEstimator2
877 */
878 public static CoordinateTransformation getAttitude(
879 final Angle latitude, final Distance height,
880 final Acceleration fx, final Acceleration fy, final Acceleration fz,
881 final double bx, final double by, final double bz, final Angle declination) {
882 return getAttitude(convertAngle(latitude), convertDistance(height),
883 convertAcceleration(fx), convertAcceleration(fy), convertAcceleration(fz), bx, by, bz,
884 convertAngle(declination));
885 }
886
887 /**
888 * Gets body attitude expressed in the local navigation frame.
889 * Internally this implementation uses a leveling algorithm to estimate
890 * roll and pitch attitude angles, which can later be used to find
891 * yaw attitude angle along with magnetometer measurements.
892 * Because Earth position is also known, it is taken into
893 * account considering actual Earth shape to obtain a slightly more
894 * accurate estimation of roll and pitch attitude angles (and thus also
895 * a more accurate yaw angle estimation).
896 *
897 * @param position Earth position expressed in NED coordinates.
898 * @param kinematics body kinematics containing measured specific force.
899 * @param b measured body magnetic flux density.
900 * @param declination declination angle of Earth magnetic flux density
901 * at current Earth position and instant expressed in
902 * radians. Declination angle changes depending on
903 * instant and location. Declination angle can be
904 * obtained through
905 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
906 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
907 * @param result instance where body attitude will be stored.
908 * @see WMMEarthMagneticFluxDensityEstimator
909 * @see EarthMagneticFluxDensityEstimator
910 * @see LevelingEstimator2
911 */
912 public static void getAttitude(
913 final NEDPosition position, final BodyKinematics kinematics, final BodyMagneticFluxDensity b,
914 final double declination, final CoordinateTransformation result) {
915 getAttitude(position.getLatitude(), position.getHeight(),
916 kinematics.getFx(), kinematics.getFy(), kinematics.getFz(), b.getBx(), b.getBy(), b.getBz(),
917 declination, result);
918 }
919
920 /**
921 * Gets body attitude expressed in the local navigation frame.
922 * Internally this implementation uses a leveling algorithm to estimate
923 * roll and pitch attitude angles, which can later be used to find
924 * yaw attitude angle along with magnetometer measurements.
925 * Because Earth position is also known, it is taken into
926 * account considering actual Earth shape to obtain a slightly more
927 * accurate estimation of roll and pitch attitude angles (and thus also
928 * a more accurate yaw angle estimation).
929 *
930 * @param position Earth position expressed in NED coordinates.
931 * @param kinematics body kinematics containing measured specific force.
932 * @param b measured body magnetic flux density.
933 * @param declination declination angle of Earth magnetic flux density
934 * at current Earth position and instant expressed in
935 * radians. Declination angle changes depending on
936 * instant and location. Declination angle can be
937 * obtained through
938 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
939 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
940 * @return a coordinate transformation containing body attitude.
941 * @see WMMEarthMagneticFluxDensityEstimator
942 * @see EarthMagneticFluxDensityEstimator
943 * @see LevelingEstimator2
944 */
945 public static CoordinateTransformation getAttitude(
946 final NEDPosition position, final BodyKinematics kinematics, final BodyMagneticFluxDensity b,
947 final double declination) {
948 return getAttitude(position.getLatitude(), position.getHeight(),
949 kinematics.getFx(), kinematics.getFy(), kinematics.getFz(),
950 b.getBx(), b.getBy(), b.getBz(), declination);
951 }
952
953 /**
954 * Gets body attitude expressed in the local navigation frame.
955 * Internally this implementation uses a leveling algorithm to estimate
956 * roll and pitch attitude angles, which can later be used to find
957 * yaw attitude angle along with magnetometer measurements.
958 * Because Earth position is also known, it is taken into
959 * account considering actual Earth shape to obtain a slightly more
960 * accurate estimation of roll and pitch attitude angles (and thus also
961 * a more accurate yaw angle estimation).
962 *
963 * @param position Earth position expressed in NED coordinates.
964 * @param kinematics body kinematics containing measured specific force.
965 * @param b measured body magnetic flux density.
966 * @param declination declination angle of Earth magnetic flux density
967 * at current Earth position and instant. Declination
968 * angle changes depending on instant and location.
969 * Declination angle can be obtained through
970 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
971 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
972 * @param result instance where body attitude will be stored.
973 * @see WMMEarthMagneticFluxDensityEstimator
974 * @see EarthMagneticFluxDensityEstimator
975 * @see LevelingEstimator2
976 */
977 public static void getAttitude(
978 final NEDPosition position, final BodyKinematics kinematics, final BodyMagneticFluxDensity b,
979 final Angle declination, final CoordinateTransformation result) {
980 getAttitude(position, kinematics, b, convertAngle(declination), result);
981 }
982
983 /**
984 * Gets body attitude expressed in the local navigation frame.
985 * Internally this implementation uses a leveling algorithm to estimate
986 * roll and pitch attitude angles, which can later be used to find
987 * yaw attitude angle along with magnetometer measurements.
988 * Because Earth position is also known, it is taken into
989 * account considering actual Earth shape to obtain a slightly more
990 * accurate estimation of roll and pitch attitude angles (and thus also
991 * a more accurate yaw angle estimation).
992 *
993 * @param position Earth position expressed in NED coordinates.
994 * @param kinematics body kinematics containing measured specific force.
995 * @param b measured body magnetic flux density.
996 * @param declination declination angle of Earth magnetic flux density
997 * at current Earth position and instant. Declination
998 * angle changes depending on instant and location.
999 * Declination angle can be obtained through
1000 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
1001 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
1002 * @return a coordinate transformation containing body attitude.
1003 * @see WMMEarthMagneticFluxDensityEstimator
1004 * @see EarthMagneticFluxDensityEstimator
1005 * @see LevelingEstimator
1006 */
1007 public static CoordinateTransformation getAttitude(
1008 final NEDPosition position, final BodyKinematics kinematics, final BodyMagneticFluxDensity b,
1009 final Angle declination) {
1010 return getAttitude(position, kinematics, b, convertAngle(declination));
1011 }
1012
1013 /**
1014 * Returns magnetic heading (angle respect Earth magnetic north).
1015 *
1016 * @param bx x coordinate of measured magnetic flux density resolved
1017 * around body axes and expressed in Teslas (T).
1018 * @param by y coordinate of measured magnetic flux density resolved
1019 * around body axes and expressed in Teslas (T).
1020 * @param bz z coordinate of measured magnetic flux density resolved
1021 * around body axes and expressed in Teslas (T).
1022 * @param roll known body roll angle expressed in radians (rad).
1023 * @param pitch known body pitch angle expressed in radians (rad).
1024 * @return magnetic heading expressed in radians (rad).
1025 */
1026 @SuppressWarnings("DuplicatedCode")
1027 public static double getMagneticHeading(
1028 final double bx, final double by, final double bz, final double roll, final double pitch) {
1029
1030 final var sinRoll = Math.sin(roll);
1031 final var cosRoll = Math.cos(roll);
1032
1033 final var sinPitch = Math.sin(pitch);
1034 final var cosPitch = Math.cos(pitch);
1035
1036 final var tmp1 = -by * cosRoll + bz * sinRoll;
1037 final var tmp2 = bx * cosPitch + by * sinRoll * sinPitch + bz * cosRoll * sinPitch;
1038
1039 return Math.atan2(tmp1, tmp2);
1040 }
1041
1042 /**
1043 * Returns magnetic heading (angle respect Earth magnetic north).
1044 *
1045 * @param b measured magnetic flux density resolved around body axes.
1046 * @param roll known body roll angle expressed in radians (rad).
1047 * @param pitch known body pitch angle expressed in radians (rad).
1048 * @return magnetic heading expressed in radians (rad).
1049 */
1050 public static double getMagneticHeading(final BodyMagneticFluxDensity b, final double roll, final double pitch) {
1051 return getMagneticHeading(b.getBx(), b.getBy(), b.getBz(), roll, pitch);
1052 }
1053
1054 /**
1055 * Returns magnetic heading (angle respect Earth magnetic north).
1056 *
1057 * @param bx x coordinate of measured magnetic flux density resolved
1058 * around body axes and expressed in Teslas (T).
1059 * @param by y coordinate of measured magnetic flux density resolved
1060 * around body axes and expressed in Teslas (T).
1061 * @param bz z coordinate of measured magnetic flux density resolved
1062 * around body axes and expressed in Teslas (T).
1063 * @param roll known body roll angle.
1064 * @param pitch known body pitch angle.
1065 * @return magnetic heading expressed in radians (rad).
1066 */
1067 public static double getMagneticHeading(
1068 final double bx, final double by, final double bz, final Angle roll, final Angle pitch) {
1069 return getMagneticHeading(bx, by, bz, convertAngle(roll), convertAngle(pitch));
1070 }
1071
1072 /**
1073 * Returns magnetic heading (angle respect Earth magnetic north).
1074 *
1075 * @param b measured magnetic flux density resolved around body axes.
1076 * @param roll known body roll angle.
1077 * @param pitch known body pitch angle.
1078 * @return magnetic heading expressed in radians (rad).
1079 */
1080 public static double getMagneticHeading(
1081 final BodyMagneticFluxDensity b, final Angle roll, final Angle pitch) {
1082 return getMagneticHeading(b, convertAngle(roll), convertAngle(pitch));
1083 }
1084
1085 /**
1086 * Returns magnetic heading (angle respect Earth magnetic north).
1087 *
1088 * @param bx x coordinate of measured magnetic flux density resolved
1089 * around body axes and expressed in Teslas (T).
1090 * @param by y coordinate of measured magnetic flux density resolved
1091 * around body axes and expressed in Teslas (T).
1092 * @param bz z coordinate of measured magnetic flux density resolved
1093 * around body axes and expressed in Teslas (T).
1094 * @param roll known body roll angle expressed in radians (rad).
1095 * @param pitch known body pitch angle expressed in radians (rad).
1096 * @param result instance where magnetic heading will be stored.
1097 */
1098 public static void getMagneticHeadingAsAngle(
1099 final double bx, final double by, final double bz,
1100 final double roll, final double pitch, final Angle result) {
1101 result.setUnit(AngleUnit.RADIANS);
1102 result.setValue(getMagneticHeading(bx, by, bz, roll, pitch));
1103 }
1104
1105 /**
1106 * Returns magnetic heading (angle respect Earth magnetic north).
1107 *
1108 * @param bx x coordinate of measured magnetic flux density resolved
1109 * around body axes and expressed in Teslas (T).
1110 * @param by y coordinate of measured magnetic flux density resolved
1111 * around body axes and expressed in Teslas (T).
1112 * @param bz z coordinate of measured magnetic flux density resolved
1113 * around body axes and expressed in Teslas (T).
1114 * @param roll known body roll angle expressed in radians (rad).
1115 * @param pitch known body pitch angle expressed in radians (rad).
1116 * @return magnetic heading.
1117 */
1118 public static Angle getMagneticHeadingAsAngle(
1119 final double bx, final double by, final double bz, final double roll, final double pitch) {
1120 return new Angle(getMagneticHeading(bx, by, bz, roll, pitch), AngleUnit.RADIANS);
1121 }
1122
1123 /**
1124 * Returns magnetic heading (angle respect Earth magnetic north).
1125 *
1126 * @param b measured magnetic flux density resolved around body axes.
1127 * @param roll known body roll angle expressed in radians (rad).
1128 * @param pitch known body pitch angle expressed in radians (rad).
1129 * @param result instance where magnetic heading will be stored.
1130 */
1131 public static void getMagneticHeadingAsAngle(
1132 final BodyMagneticFluxDensity b, final double roll, final double pitch, final Angle result) {
1133 result.setUnit(AngleUnit.RADIANS);
1134 result.setValue(getMagneticHeading(b, roll, pitch));
1135 }
1136
1137 /**
1138 * Returns magnetic heading (angle respect Earth magnetic north).
1139 *
1140 * @param b measured magnetic flux density resolved around body axes.
1141 * @param roll known body roll angle expressed in radians (rad).
1142 * @param pitch known body pitch angle expressed in radians (rad).
1143 * @return magnetic heading.
1144 */
1145 public static Angle getMagneticHeadingAsAngle(
1146 final BodyMagneticFluxDensity b, final double roll, final double pitch) {
1147 return new Angle(getMagneticHeading(b, roll, pitch), AngleUnit.RADIANS);
1148 }
1149
1150 /**
1151 * Returns magnetic heading (angle respect Earth magnetic north).
1152 *
1153 * @param bx x coordinate of measured magnetic flux density resolved
1154 * around body axes and expressed in Teslas (T).
1155 * @param by y coordinate of measured magnetic flux density resolved
1156 * around body axes and expressed in Teslas (T).
1157 * @param bz z coordinate of measured magnetic flux density resolved
1158 * around body axes and expressed in Teslas (T).
1159 * @param roll known body roll angle.
1160 * @param pitch known body pitch angle.
1161 * @param result instance where magnetic heading will be stored.
1162 */
1163 public static void getMagneticHeadingAsAngle(
1164 final double bx, final double by, final double bz,
1165 final Angle roll, final Angle pitch, final Angle result) {
1166 result.setUnit(AngleUnit.RADIANS);
1167 result.setValue(getMagneticHeading(bx, by, bz, roll, pitch));
1168 }
1169
1170 /**
1171 * Returns magnetic heading (angle respect Earth magnetic north).
1172 *
1173 * @param bx x coordinate of measured magnetic flux density resolved
1174 * around body axes and expressed in Teslas (T).
1175 * @param by y coordinate of measured magnetic flux density resolved
1176 * around body axes and expressed in Teslas (T).
1177 * @param bz z coordinate of measured magnetic flux density resolved
1178 * around body axes and expressed in Teslas (T).
1179 * @param roll known body roll angle.
1180 * @param pitch known body pitch angle.
1181 * @return magnetic heading.
1182 */
1183 public static Angle getMagneticHeadingAsAngle(
1184 final double bx, final double by, final double bz, final Angle roll, final Angle pitch) {
1185 return new Angle(getMagneticHeading(bx, by, bz, roll, pitch), AngleUnit.RADIANS);
1186 }
1187
1188 /**
1189 * Returns magnetic heading (angle respect Earth magnetic north).
1190 *
1191 * @param b measured magnetic flux density resolved around body axes.
1192 * @param roll known body roll angle.
1193 * @param pitch known body pitch angle.
1194 * @param result instance where magnetic heading will be stored.
1195 */
1196 public static void getMagneticHeadingAsAngle(
1197 final BodyMagneticFluxDensity b, final Angle roll, final Angle pitch, final Angle result) {
1198 result.setUnit(AngleUnit.RADIANS);
1199 result.setValue(getMagneticHeading(b, roll, pitch));
1200 }
1201
1202 /**
1203 * Returns magnetic heading (angle respect Earth magnetic north).
1204 *
1205 * @param b measured magnetic flux density resolved around body axes.
1206 * @param roll known body roll angle.
1207 * @param pitch known body pitch angle.
1208 * @return magnetic heading.
1209 */
1210 public static Angle getMagneticHeadingAsAngle(
1211 final BodyMagneticFluxDensity b, final Angle roll, final Angle pitch) {
1212 return new Angle(getMagneticHeading(b, roll, pitch), AngleUnit.RADIANS);
1213 }
1214
1215 /**
1216 * Returns body yaw angle resolved around NED frame.
1217 * Yaw angle represents the true heading, and indicates the angle respect
1218 * the true geographic north.
1219 *
1220 * @param bx x coordinate of measured magnetic flux density resolved
1221 * around body axes and expressed in Teslas (T).
1222 * @param by y coordinate of measured magnetic flux density resolved
1223 * around body axes and expressed in Teslas (T).
1224 * @param bz z coordinate of measured magnetic flux density resolved
1225 * around body axes and expressed in Teslas (T).
1226 * @param declination declination angle of Earth magnetic flux density
1227 * at current Earth position and instant expressed in
1228 * radians. Declination angle changes depending on
1229 * instant and location. Declination angle can be
1230 * obtained through
1231 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
1232 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
1233 * @param roll known body roll angle expressed in radians (rad).
1234 * @param pitch known body pitch angle expressed in radians (rad).
1235 * @return body yaw angle (a.k.a. true heading) expressed in radians (rad).
1236 * @see WMMEarthMagneticFluxDensityEstimator
1237 * @see EarthMagneticFluxDensityEstimator
1238 */
1239 public static double getYaw(
1240 final double bx, final double by, final double bz, final double declination,
1241 final double roll, final double pitch) {
1242 return getMagneticHeading(bx, by, bz, roll, pitch) + declination;
1243 }
1244
1245 /**
1246 * Returns body yaw angle resolved around NED frame.
1247 * Yaw angle represents the true heading, and indicates the angle respect
1248 * the true geographic north.
1249 *
1250 * @param b measured magnetic flux density resolved around body axes.
1251 * @param declination declination angle of Earth magnetic flux density
1252 * at current Earth position and instant expressed in
1253 * radians. Declination angle changes depending on
1254 * instant and location. Declination angle can be
1255 * obtained through
1256 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
1257 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
1258 * @param roll known body roll angle expressed in radians (rad).
1259 * @param pitch known body pitch angle expressed in radians (rad).
1260 * @return body yaw angle (a.k.a. true heading) expressed in radians (rad).
1261 * @see WMMEarthMagneticFluxDensityEstimator
1262 * @see EarthMagneticFluxDensityEstimator
1263 */
1264 public static double getYaw(
1265 final BodyMagneticFluxDensity b, final double declination, final double roll, final double pitch) {
1266 return getYaw(b.getBx(), b.getBy(), b.getBz(), declination, roll, pitch);
1267 }
1268
1269 /**
1270 * Returns body yaw angle resolved around NED frame.
1271 * Yaw angle represents the true heading, and indicates the angle respect
1272 * the true geographic north.
1273 *
1274 * @param bx x coordinate of measured magnetic flux density resolved
1275 * around body axes and expressed in Teslas (T).
1276 * @param by y coordinate of measured magnetic flux density resolved
1277 * around body axes and expressed in Teslas (T).
1278 * @param bz z coordinate of measured magnetic flux density resolved
1279 * around body axes and expressed in Teslas (T).
1280 * @param declination declination angle of Earth magnetic flux density
1281 * at current Earth position and instant. Declination
1282 * angle changes depending on instant and location.
1283 * Declination angle can be obtained through
1284 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
1285 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
1286 * @param roll known body roll angle.
1287 * @param pitch known body pitch angle.
1288 * @return body yaw angle (a.k.a. true heading) expressed in radians (rad).
1289 * @see WMMEarthMagneticFluxDensityEstimator
1290 * @see EarthMagneticFluxDensityEstimator
1291 */
1292 public static double getYaw(
1293 final double bx, final double by, final double bz,
1294 final Angle declination, final Angle roll, final Angle pitch) {
1295 return getYaw(bx, by, bz, convertAngle(declination), convertAngle(roll), convertAngle(pitch));
1296 }
1297
1298 /**
1299 * Returns body yaw angle resolved around NED frame.
1300 * Yaw angle represents the true heading, and indicates the angle respect
1301 * the true geographic north.
1302 *
1303 * @param b measured magnetic flux density resolved around body axes.
1304 * @param declination declination angle of Earth magnetic flux density
1305 * at current Earth position and instant. Declination
1306 * angle changes depending on instant and location.
1307 * Declination angle can be obtained through
1308 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
1309 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
1310 * @param roll known body roll angle.
1311 * @param pitch known body pitch angle.
1312 * @return body yaw angle (a.k.a. true heading) expressed in radians (rad).
1313 * @see WMMEarthMagneticFluxDensityEstimator
1314 * @see EarthMagneticFluxDensityEstimator
1315 */
1316 public static double getYaw(
1317 final BodyMagneticFluxDensity b, final Angle declination, final Angle roll, final Angle pitch) {
1318 return getYaw(b, convertAngle(declination), convertAngle(roll), convertAngle(pitch));
1319 }
1320
1321 /**
1322 * Returns body yaw angle resolved around NED frame.
1323 * Yaw angle represents the true heading, and indicates the angle respect
1324 * the true geographic north.
1325 *
1326 * @param bx x coordinate of measured magnetic flux density resolved
1327 * around body axes and expressed in Teslas (T).
1328 * @param by y coordinate of measured magnetic flux density resolved
1329 * around body axes and expressed in Teslas (T).
1330 * @param bz z coordinate of measured magnetic flux density resolved
1331 * around body axes and expressed in Teslas (T).
1332 * @param declination declination angle of Earth magnetic flux density
1333 * at current Earth position and instant expressed in
1334 * radians. Declination angle changes depending on
1335 * instant and location. Declination angle can be
1336 * obtained through
1337 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
1338 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
1339 * @param roll known body roll angle expressed in radians (rad).
1340 * @param pitch known body pitch angle expressed in radians (rad).
1341 * @param result instance where body yaw angle (a.k.a. true heading)
1342 * will be stored.
1343 * @see WMMEarthMagneticFluxDensityEstimator
1344 * @see EarthMagneticFluxDensityEstimator
1345 */
1346 public static void getYawAsAngle(
1347 final double bx, final double by, final double bz,
1348 final double declination, final double roll, final double pitch, final Angle result) {
1349 result.setUnit(AngleUnit.RADIANS);
1350 result.setValue(getYaw(bx, by, bz, declination, roll, pitch));
1351 }
1352
1353 /**
1354 * Returns body yaw angle resolved around NED frame.
1355 * Yaw angle represents the true heading, and indicates the angle respect
1356 * the true geographic north.
1357 *
1358 * @param bx x coordinate of measured magnetic flux density resolved
1359 * around body axes and expressed in Teslas (T).
1360 * @param by y coordinate of measured magnetic flux density resolved
1361 * around body axes and expressed in Teslas (T).
1362 * @param bz z coordinate of measured magnetic flux density resolved
1363 * around body axes and expressed in Teslas (T).
1364 * @param declination declination angle of Earth magnetic flux density
1365 * at current Earth position and instant expressed in
1366 * radians. Declination angle changes depending on
1367 * instant and location. Declination angle can be
1368 * obtained through
1369 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
1370 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
1371 * @param roll known body roll angle expressed in radians (rad).
1372 * @param pitch known body pitch angle expressed in radians (rad).
1373 * @return body yaw angle (a.k.a. true heading).
1374 * @see WMMEarthMagneticFluxDensityEstimator
1375 * @see EarthMagneticFluxDensityEstimator
1376 */
1377 public static Angle getYawAsAngle(
1378 final double bx, final double by, final double bz, final double declination,
1379 final double roll, final double pitch) {
1380 return new Angle(getYaw(bx, by, bz, declination, roll, pitch), AngleUnit.RADIANS);
1381 }
1382
1383 /**
1384 * Returns body yaw angle resolved around NED frame.
1385 * Yaw angle represents the true heading, and indicates the angle respect
1386 * the true geographic north.
1387 *
1388 * @param b measured magnetic flux density resolved around body axes.
1389 * @param declination declination angle of Earth magnetic flux density
1390 * at current Earth position and instant expressed in
1391 * radians. Declination angle changes depending on
1392 * instant and location. Declination angle can be
1393 * obtained through
1394 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
1395 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
1396 * @param roll known body roll angle expressed in radians (rad).
1397 * @param pitch known body pitch angle expressed in radians (rad).
1398 * @param result instance where body yaw angle (a.k.a. true heading)
1399 * will be stored.
1400 * @see WMMEarthMagneticFluxDensityEstimator
1401 * @see EarthMagneticFluxDensityEstimator
1402 */
1403 public static void getYawAsAngle(
1404 final BodyMagneticFluxDensity b, final double declination, final double roll, final double pitch,
1405 final Angle result) {
1406 result.setUnit(AngleUnit.RADIANS);
1407 result.setValue(getYaw(b, declination, roll, pitch));
1408 }
1409
1410 /**
1411 * Returns body yaw angle resolved around NED frame.
1412 * Yaw angle represents the true heading, and indicates the angle respect
1413 * the true geographic north.
1414 *
1415 * @param b measured magnetic flux density resolved around body axes.
1416 * @param declination declination angle of Earth magnetic flux density
1417 * at current Earth position and instant expressed in
1418 * radians. Declination angle changes depending on
1419 * instant and location. Declination angle can be
1420 * obtained through
1421 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
1422 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
1423 * @param roll known body roll angle expressed in radians (rad).
1424 * @param pitch known body pitch angle expressed in radians (rad).
1425 * @return body yaw angle (a.k.a. true heading).
1426 * @see WMMEarthMagneticFluxDensityEstimator
1427 * @see EarthMagneticFluxDensityEstimator
1428 */
1429 public static Angle getYawAsAngle(
1430 final BodyMagneticFluxDensity b, final double declination, final double roll, final double pitch) {
1431 return new Angle(getYaw(b, declination, roll, pitch), AngleUnit.RADIANS);
1432 }
1433
1434 /**
1435 * Returns body yaw angle resolved around NED frame.
1436 * Yaw angle represents the true heading, and indicates the angle respect
1437 * the true geographic north.
1438 *
1439 * @param bx x coordinate of measured magnetic flux density resolved
1440 * around body axes and expressed in Teslas (T).
1441 * @param by y coordinate of measured magnetic flux density resolved
1442 * around body axes and expressed in Teslas (T).
1443 * @param bz z coordinate of measured magnetic flux density resolved
1444 * around body axes and expressed in Teslas (T).
1445 * @param declination declination angle of Earth magnetic flux density
1446 * at current Earth position and instant. Declination
1447 * angle changes depending on instant and location.
1448 * Declination angle can be obtained through
1449 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
1450 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
1451 * @param roll known body roll angle.
1452 * @param pitch known body pitch angle.
1453 * @param result instance where body yaw angle (a.k.a. true heading)
1454 * will be stored.
1455 * @see WMMEarthMagneticFluxDensityEstimator
1456 * @see EarthMagneticFluxDensityEstimator
1457 */
1458 public static void getYawAsAngle(
1459 final double bx, final double by, final double bz,
1460 final Angle declination, final Angle roll, final Angle pitch, final Angle result) {
1461 result.setUnit(AngleUnit.RADIANS);
1462 result.setValue(getYaw(bx, by, bz, declination, roll, pitch));
1463 }
1464
1465 /**
1466 * Returns body yaw angle resolved around NED frame.
1467 * Yaw angle represents the true heading, and indicates the angle respect
1468 * the true geographic north.
1469 *
1470 * @param bx x coordinate of measured magnetic flux density resolved
1471 * around body axes and expressed in Teslas (T).
1472 * @param by y coordinate of measured magnetic flux density resolved
1473 * around body axes and expressed in Teslas (T).
1474 * @param bz z coordinate of measured magnetic flux density resolved
1475 * around body axes and expressed in Teslas (T).
1476 * @param declination declination angle of Earth magnetic flux density
1477 * at current Earth position and instant. Declination
1478 * angle changes depending on instant and location.
1479 * Declination angle can be obtained through
1480 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
1481 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
1482 * @param roll known body roll angle.
1483 * @param pitch known body pitch angle.
1484 * @return body yaw angle (a.k.a. true heading).
1485 * @see WMMEarthMagneticFluxDensityEstimator
1486 * @see EarthMagneticFluxDensityEstimator
1487 */
1488 public static Angle getYawAsAngle(
1489 final double bx, final double by, final double bz,
1490 final Angle declination, final Angle roll, final Angle pitch) {
1491 return new Angle(getYaw(bx, by, bz, declination, roll, pitch), AngleUnit.RADIANS);
1492 }
1493
1494 /**
1495 * Returns body yaw angle resolved around NED frame.
1496 * Yaw angle represents the true heading, and indicates the angle respect
1497 * the true geographic north.
1498 *
1499 * @param b measured magnetic flux density resolved around body axes.
1500 * @param declination declination angle of Earth magnetic flux density
1501 * at current Earth position and instant. Declination
1502 * angle changes depending on instant and location.
1503 * Declination angle can be obtained through
1504 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
1505 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
1506 * @param roll known body roll angle.
1507 * @param pitch known body pitch angle.
1508 * @param result instance where body yaw angle (a.k.a. true heading)
1509 * will be stored.
1510 * @see WMMEarthMagneticFluxDensityEstimator
1511 * @see EarthMagneticFluxDensityEstimator
1512 */
1513 public static void getYawAsAngle(
1514 final BodyMagneticFluxDensity b, final Angle declination,
1515 final Angle roll, final Angle pitch, final Angle result) {
1516 result.setUnit(AngleUnit.RADIANS);
1517 result.setValue(getYaw(b, declination, roll, pitch));
1518 }
1519
1520 /**
1521 * Returns body yaw angle resolved around NED frame.
1522 * Yaw angle represents the true heading, and indicates the angle respect
1523 * the true geographic north.
1524 *
1525 * @param b measured magnetic flux density resolved around body axes.
1526 * @param declination declination angle of Earth magnetic flux density
1527 * at current Earth position and instant. Declination
1528 * angle changes depending on instant and location.
1529 * Declination angle can be obtained through
1530 * {@link WMMEarthMagneticFluxDensityEstimator#getDeclination(NEDPosition, Date)}
1531 * or using {@link EarthMagneticFluxDensityEstimator#getDeclination(NEDMagneticFluxDensity)}
1532 * @param roll known body roll angle.
1533 * @param pitch known body pitch angle.
1534 * @return body yaw angle (a.k.a. true heading).
1535 * @see WMMEarthMagneticFluxDensityEstimator
1536 * @see EarthMagneticFluxDensityEstimator
1537 */
1538 public static Angle getYawAsAngle(
1539 final BodyMagneticFluxDensity b, final Angle declination, final Angle roll, final Angle pitch) {
1540 return new Angle(getYaw(b, declination, roll, pitch), AngleUnit.RADIANS);
1541 }
1542
1543 /**
1544 * Converts a given distance instance into meters.
1545 *
1546 * @param distance distance to be converted.
1547 * @return converted value expressed in meters.
1548 */
1549 private static double convertDistance(final Distance distance) {
1550 return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
1551 }
1552
1553 /**
1554 * Converts a given angle instance into radians.
1555 *
1556 * @param angle angle to be converted.
1557 * @return converted value expressed in radians.
1558 */
1559 private static double convertAngle(final Angle angle) {
1560 return AngleConverter.convert(angle.getValue().doubleValue(), angle.getUnit(), AngleUnit.RADIANS);
1561 }
1562
1563 /**
1564 * Converts an instance of acceleration to meters per squared second (m/s^2).
1565 *
1566 * @param acceleration acceleration instance to be converted.
1567 * @return converted acceleration value.
1568 */
1569 private static double convertAcceleration(final Acceleration acceleration) {
1570 return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
1571 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1572 }
1573 }