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.calibration.bias;
17
18 import com.irurueta.geometry.Point3D;
19 import com.irurueta.navigation.LockedException;
20 import com.irurueta.navigation.frames.CoordinateTransformation;
21 import com.irurueta.navigation.frames.ECEFFrame;
22 import com.irurueta.navigation.frames.ECEFPosition;
23 import com.irurueta.navigation.frames.FrameType;
24 import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
25 import com.irurueta.navigation.frames.NEDFrame;
26 import com.irurueta.navigation.frames.NEDPosition;
27 import com.irurueta.navigation.frames.converters.ECEFtoNEDFrameConverter;
28 import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
29 import com.irurueta.navigation.inertial.BodyMagneticFluxDensity;
30 import com.irurueta.navigation.inertial.calibration.MagneticFluxDensityTriad;
31 import com.irurueta.navigation.inertial.estimators.BodyMagneticFluxDensityEstimator;
32 import com.irurueta.navigation.inertial.wmm.WMMEarthMagneticFluxDensityEstimator;
33 import com.irurueta.navigation.inertial.wmm.WorldMagneticModel;
34 import com.irurueta.units.Angle;
35 import com.irurueta.units.Distance;
36 import com.irurueta.units.MagneticFluxDensity;
37 import com.irurueta.units.MagneticFluxDensityUnit;
38 import com.irurueta.units.Time;
39 import com.irurueta.units.TimeConverter;
40 import com.irurueta.units.TimeUnit;
41
42 import java.io.IOException;
43 import java.util.Date;
44 import java.util.GregorianCalendar;
45
46 /**
47 * Approximately estimated magnetometer biases (hard iron) and noise PSD's by
48 * averaging all provided samples when instant, body position and orientation
49 * is known while assuming that any soft iron cross coupling errors can be
50 * neglected.
51 * <p>
52 * The estimator must be used when the body where the magnetometer is attached to
53 * remains static on the same position with zero velocity and no rotation speed
54 * while capturing data.
55 * <p>
56 * To compute PSD's this estimator assumes that magnetometer samples are obtained at
57 * a constant provided rate equal to {@link #getTimeInterval()} seconds.
58 * If not available, magnetometer sampling rate average can be estimated using
59 * {@link com.irurueta.navigation.inertial.calibration.TimeIntervalEstimator}.
60 * <p>
61 * Notice that in order to compute magnetometer biases (hard iron), instant, body
62 * position and orientation must be known to account for expected magnetic field
63 * to be sensed.
64 * <p>
65 * Even though this estimator obtains approximate bias values, the obtained
66 * result can be used to initialize some non-linear calibrators to obtain
67 * more accurate results, by using bias values as initial hard iron values.
68 * Such calibrators are:
69 * - com.irurueta.navigation.inertial.calibration.magnetometer.KnownFrameMagnetometerNonLinearLeastSquaresCalibrator
70 * - com.irurueta.navigation.inertial.calibration.magnetometer.KnownPositionAndInstantMagnetometerCalibrator
71 * - com.irurueta.navigation.inertial.calibration.magnetometer.RobustKnownFrameMagnetometerCalibrator and any
72 * of its subclasses.
73 * - com.irurueta.navigation.inertial.calibration.magnetometer.RobustKnownPositionAndInstantMagnetometerCalibrator
74 * and any of its subclasses.
75 * <p>
76 * Even though this estimator can compute noise PSD's, if only noise PSD's levels
77 * are required, estimators in {@link com.irurueta.navigation.inertial.calibration.noise} package should
78 * be used instead.
79 * <p>
80 * This estimator does NOT compute average bias values over a period of time, it only
81 * computes accumulated averages.
82 */
83 public class BodyMagneticFluxDensityBiasEstimator {
84
85 /**
86 * Default time interval between accelerometer samples expressed in seconds (s).
87 */
88 public static final double DEFAULT_TIME_INTERVAL_SECONDS = 0.02;
89
90 /**
91 * Time interval expressed in seconds (s) between body kinematics samples.
92 */
93 private double timeInterval = DEFAULT_TIME_INTERVAL_SECONDS;
94
95 /**
96 * Contains body position, velocity (which will always be zero) and orientation
97 * resolved around ECEF axes.
98 * By default it is assumed that body is located at zero NED coordinates (latitude,
99 * longitude and height) and with zero Euler angles representing rotation (roll = 0,
100 * pith = 0, yaw = 0), which for Android devices it means that the device is flat
101 * on a horizontal surface with the screen facing down.
102 */
103 private final ECEFFrame frame;
104
105 /**
106 * Contains year expressed in decimal format.
107 */
108 private double year;
109
110 /**
111 * Listener to handle events raised by this estimator.
112 */
113 private BodyMagneticFluxDensityBiasEstimatorListener listener;
114
115 /**
116 * Contains Earth's magnetic model.
117 */
118 private WorldMagneticModel magneticModel;
119
120 /**
121 * World Magnetic Model of Earth.
122 */
123 private WMMEarthMagneticFluxDensityEstimator wmmEstimator;
124
125 /**
126 * Last provided body magnetic flux density values.
127 */
128 private BodyMagneticFluxDensity lastBodyMagneticFluxDensity;
129
130 /**
131 * Contains estimated bias of x coordinate of body magnetic flux density
132 * expressed in Teslas (T). Notice that bias is equivalent to hard iron
133 * component on a magnetometer calibrator.
134 */
135 private double biasX;
136
137 /**
138 * Contains estimated bias of y coordinate of body magnetic flux density
139 * expressed in Teslas (T). Notice that bias is equivalent to hard iron
140 * component on a magnetometer calibrator.
141 */
142 private double biasY;
143
144 /**
145 * Contains estimated bias of z coordinate of body magnetic flux density
146 * expressed in Teslas (T). Notice that bias is equivalent to hard iron
147 * component on a magnetometer calibrator.
148 */
149 private double biasZ;
150
151 /**
152 * Contains estimated variance of x coordinate of body magnetic flux density
153 * expressed in squared Teslas (T^2).
154 */
155 private double varianceX;
156
157 /**
158 * Contains estimated variance of y coordinate of body magnetic flux density
159 * expressed in squared Teslas (T^2).
160 */
161 private double varianceY;
162
163 /**
164 * Contains estimated variance of z coordinate of body magnetic flux density
165 * expressed in squared Teslas (T^2).
166 */
167 private double varianceZ;
168
169 /**
170 * Number of processed magnetometer samples.
171 */
172 private int numberOfProcessedSamples;
173
174 /**
175 * Number of processed magnetometer samples plus one.
176 */
177 private int numberOfProcessedSamplesPlusOne = 1;
178
179 /**
180 * Indicates that estimator is running.
181 */
182 private boolean running;
183
184 /**
185 * Theoretical expected body magnetic flux density for provided instant,
186 * body position and orientation, assuming that body remains at the same
187 * position (zero velocity).
188 * When body remains static, sensed magnetic flux density will remain constant
189 * for a few minutes respect to provided time instant.
190 */
191 private BodyMagneticFluxDensity expectedBodyMagneticFluxDensity;
192
193 /**
194 * Constructor.
195 * It is assumed that body is located at zero NED coordinates (latitude = 0,
196 * longitude = 0 and height = 0) and with zero Euler angles representing rotation
197 * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
198 * device is flat on a horizontal surface with the screen facing down.
199 * This constructor assumes that time is current time instant.
200 *
201 * @throws IOException if initialization of world magnetic model fails.
202 */
203 public BodyMagneticFluxDensityBiasEstimator() throws IOException {
204 this(new Date(), (WorldMagneticModel) null);
205 }
206
207 /**
208 * Constructor.
209 * It is assumed that body is located at zero NED coordinates (latitude = 0,
210 * longitude = 0, and height = 0) with provided orientation.
211 * This constructor assumes that time is current time instant.
212 *
213 * @param nedC coordinate transformation from body to local navigation
214 * (NED) coordinates. This contains orientation respect the horizon
215 * at current body location.
216 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
217 * transformation is not
218 * from body to local
219 * navigation coordinates.
220 * @throws IOException if initialization of
221 * world magnetic model
222 * fails.
223 */
224 public BodyMagneticFluxDensityBiasEstimator(final CoordinateTransformation nedC)
225 throws InvalidSourceAndDestinationFrameTypeException, IOException {
226 this(nedC, new Date(), (WorldMagneticModel) null);
227 }
228
229 /**
230 * Constructor.
231 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
232 * pitch = 0, yaw = 0) respect the horizon at provided body location.
233 * For Android devices this means that the device is flat on a horizontal surface
234 * with the screen facing down.
235 * This constructor assumes that time is current time instant.
236 *
237 * @param latitude latitude expressed in radians (rad).
238 * @param longitude longitude expressed in radians (rad).
239 * @param height height expressed in meters (m).
240 * @throws IOException if initialization of world magnetic model fails.
241 */
242 public BodyMagneticFluxDensityBiasEstimator(final double latitude, final double longitude, final double height)
243 throws IOException {
244 this(latitude, longitude, height, new Date(), (WorldMagneticModel) null);
245 }
246
247 /**
248 * Constructor.
249 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
250 * pitch = 0, yaw = 0) respect the horizon at provided body location.
251 * For Android devices this means that the device is flat on a horizontal surface
252 * with the screen facing down.
253 * This constructor assumes that time is current time instant.
254 *
255 * @param latitude latitude.
256 * @param longitude longitude.
257 * @param height height expressed in meters (m).
258 * @throws IOException if initialization of world magnetic model fails.
259 */
260 public BodyMagneticFluxDensityBiasEstimator(final Angle latitude, final Angle longitude, final double height)
261 throws IOException {
262 this(latitude, longitude, height, new Date(), (WorldMagneticModel) null);
263 }
264
265 /**
266 * Constructor.
267 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
268 * pitch = 0, yaw = 0) respect the horizon at provided body location.
269 * For Android devices this means that the device is flat on a horizontal surface
270 * with the screen facing down.
271 * This constructor assumes that time is current time instant.
272 *
273 * @param latitude latitude.
274 * @param longitude longitude.
275 * @param height height.
276 * @throws IOException if initialization of world magnetic model fails.
277 */
278 public BodyMagneticFluxDensityBiasEstimator(final Angle latitude, final Angle longitude, final Distance height)
279 throws IOException {
280 this(latitude, longitude, height, new Date(), (WorldMagneticModel) null);
281 }
282
283 /**
284 * Constructor.
285 * This constructor assumes that time is current time instant.
286 *
287 * @param position body position expressed in NED coordinates.
288 * @param nedC coordinate transformation from body to local navigation
289 * (NED) coordinates. This contains orientation respect the
290 * horizon at current body location.
291 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
292 * transformation is not
293 * from body to local
294 * navigation coordinates.
295 * @throws IOException if initialization of
296 * world magnetic model
297 * fails.
298 */
299 public BodyMagneticFluxDensityBiasEstimator(final NEDPosition position, final CoordinateTransformation nedC)
300 throws InvalidSourceAndDestinationFrameTypeException, IOException {
301 this(position, nedC, new Date(), (WorldMagneticModel) null);
302 }
303
304 /**
305 * Constructor.
306 * This constructor assumes that time is current time instant.
307 *
308 * @param position body position expressed in ECEF coordinates.
309 * @param nedC coordinate transformation from body to local navigation
310 * (NED) coordinates. This contains orientation respect the
311 * horizon at current body location.
312 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
313 * transformation is not
314 * from body to local
315 * navigation coordinates.
316 * @throws IOException if initialization of
317 * world magnetic model
318 * fails.
319 */
320 public BodyMagneticFluxDensityBiasEstimator(final ECEFPosition position, final CoordinateTransformation nedC)
321 throws InvalidSourceAndDestinationFrameTypeException, IOException {
322 this(position, nedC, new Date(), (WorldMagneticModel) null);
323 }
324
325 /**
326 * Constructor.
327 * It is assumed that body is located at zero NED coordinates (latitude = 0,
328 * longitude = 0 and height = 0) and with zero Euler angles representing rotation
329 * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
330 * device is flat on a horizontal surface with the screen facing down.
331 * This constructor assumes that time is current time instant.
332 *
333 * @param listener listener to handle events raised by this estimator.
334 * @throws IOException if initialization of world magnetic model fails.
335 */
336 public BodyMagneticFluxDensityBiasEstimator(
337 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
338 this(new Date());
339 this.listener = listener;
340 }
341
342 /**
343 * Constructor.
344 * It is assumed that body is located at zero NED coordinates (latitude = 0,
345 * longitude = 0, and height = 0) with provided orientation.
346 * This constructor assumes that time is current time instant.
347 *
348 * @param nedC coordinate transformation from body to local navigation
349 * (NED) coordinates. This contains orientation respect the
350 * horizon at current body location.
351 * @param listener listener to handle events raised by this estimator.
352 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
353 * transformation is not
354 * from body to local
355 * navigation coordinates.
356 * @throws IOException if initialization of
357 * world magnetic model
358 * fails.
359 */
360 public BodyMagneticFluxDensityBiasEstimator(
361 final CoordinateTransformation nedC, final BodyMagneticFluxDensityBiasEstimatorListener listener)
362 throws InvalidSourceAndDestinationFrameTypeException, IOException {
363 this(nedC, new Date());
364 this.listener = listener;
365 }
366
367 /**
368 * Constructor.
369 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
370 * pitch = 0, yaw = 0) respect the horizon at provided body location.
371 * For Android devices this means that the device is flat on a horizontal surface
372 * with the screen facing down.
373 * This constructor assumes that time is current time instant.
374 *
375 * @param latitude latitude expressed in radians (rad).
376 * @param longitude longitude expressed in radians (rad).
377 * @param height height expressed in meters (m).
378 * @param listener listener to handle events raised by this estimator.
379 * @throws IOException if initialization of world magnetic model fails.
380 */
381 public BodyMagneticFluxDensityBiasEstimator(
382 final double latitude, final double longitude, final double height,
383 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
384 this(latitude, longitude, height, new Date());
385 this.listener = listener;
386 }
387
388 /**
389 * Constructor.
390 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
391 * pitch = 0, yaw = 0) respect the horizon at provided body location.
392 * For Android devices this means that the device is flat on a horizontal surface
393 * with the screen facing down.
394 * This constructor assumes that time is current time instant.
395 *
396 * @param latitude latitude.
397 * @param longitude longitude.
398 * @param height height expressed in meters (m).
399 * @param listener listener to handle events raised by this estimator.
400 * @throws IOException if initialization of world magnetic model fails.
401 */
402 public BodyMagneticFluxDensityBiasEstimator(
403 final Angle latitude, final Angle longitude, final double height,
404 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
405 this(latitude, longitude, height, new Date());
406 this.listener = listener;
407 }
408
409 /**
410 * Constructor.
411 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
412 * pitch = 0, yaw = 0) respect the horizon at provided body location.
413 * For Android devices this means that the device is flat on a horizontal surface
414 * with the screen facing down.
415 * This constructor assumes that time is current time instant.
416 *
417 * @param latitude latitude.
418 * @param longitude longitude.
419 * @param height height.
420 * @param listener listener to handle events raised by this estimator.
421 * @throws IOException if initialization of world magnetic model fails.
422 */
423 public BodyMagneticFluxDensityBiasEstimator(
424 final Angle latitude, final Angle longitude, final Distance height,
425 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
426 this(latitude, longitude, height, new Date());
427 this.listener = listener;
428 }
429
430 /**
431 * Constructor.
432 * This constructor assumes that time is current time instant.
433 *
434 * @param position body position expressed in NED coordinates.
435 * @param nedC coordinate transformation from body to local navigation
436 * (NED) coordinates. This contains orientation respect the
437 * horizon at current body location.
438 * @param listener listener to handle events raised by this estimator.
439 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
440 * transformation is not
441 * from body to local
442 * navigation coordinates.
443 * @throws IOException if initialization of
444 * world magnetic model
445 * fails.
446 */
447 public BodyMagneticFluxDensityBiasEstimator(
448 final NEDPosition position, final CoordinateTransformation nedC,
449 final BodyMagneticFluxDensityBiasEstimatorListener listener)
450 throws InvalidSourceAndDestinationFrameTypeException, IOException {
451 this(position, nedC, new Date());
452 this.listener = listener;
453 }
454
455 /**
456 * Constructor.
457 *
458 * @param position body position expressed in ECEF coordinates.
459 * @param nedC coordinate transformation from body to local navigation
460 * (NED) coordinates. This contains orientation respect the
461 * horizon at current body location.
462 * @param listener listener to handle events raised by this estimator.
463 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
464 * transformation is not
465 * from body to local
466 * navigation coordinates.
467 * @throws IOException if initialization of
468 * world magnetic model
469 * fails.
470 */
471 public BodyMagneticFluxDensityBiasEstimator(
472 final ECEFPosition position, final CoordinateTransformation nedC,
473 final BodyMagneticFluxDensityBiasEstimatorListener listener)
474 throws InvalidSourceAndDestinationFrameTypeException, IOException {
475 this(position, nedC, new Date());
476 this.listener = listener;
477 }
478
479 /**
480 * Constructor.
481 * It is assumed that body is located at zero NED coordinates (latitude = 0,
482 * longitude = 0 and height = 0) and with zero Euler angles representing rotation
483 * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
484 * device is flat on a horizontal surface with the screen facing down.
485 *
486 * @param year time expressed as decimal year.
487 * @throws IOException if initialization of world magnetic model fails.
488 */
489 public BodyMagneticFluxDensityBiasEstimator(final double year) throws IOException {
490 this(year, (WorldMagneticModel) null);
491 }
492
493 /**
494 * Constructor.
495 * It is assumed that body is located at zero NED coordinates (latitude = 0,
496 * longitude = 0, and height = 0) with provided orientation.
497 *
498 * @param nedC coordinate transformation from body to local navigation
499 * (NED) coordinates. This contains orientation respect the horizon
500 * at current body location.
501 * @param year time expressed as decimal year.
502 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
503 * transformation is not
504 * from body to local
505 * navigation coordinates.
506 * @throws IOException if initialization of
507 * world magnetic model
508 * fails.
509 */
510 public BodyMagneticFluxDensityBiasEstimator(final CoordinateTransformation nedC, final double year)
511 throws InvalidSourceAndDestinationFrameTypeException, IOException {
512 this(nedC, year, (WorldMagneticModel) null);
513 }
514
515 /**
516 * Constructor.
517 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
518 * pitch = 0, yaw = 0) respect the horizon at provided body location.
519 * For Android devices this means that the device is flat on a horizontal surface
520 * with the screen facing down.
521 *
522 * @param latitude latitude expressed in radians (rad).
523 * @param longitude longitude expressed in radians (rad).
524 * @param height height expressed in meters (m).
525 * @param year time expressed as decimal year.
526 * @throws IOException if initialization of world magnetic model fails.
527 */
528 public BodyMagneticFluxDensityBiasEstimator(
529 final double latitude, final double longitude, final double height, final double year) throws IOException {
530 this(latitude, longitude, height, year, (WorldMagneticModel) null);
531 }
532
533 /**
534 * Constructor.
535 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
536 * pitch = 0, yaw = 0) respect the horizon at provided body location.
537 * For Android devices this means that the device is flat on a horizontal surface
538 * with the screen facing down.
539 *
540 * @param latitude latitude.
541 * @param longitude longitude.
542 * @param height height expressed in meters (m).
543 * @param year time expressed as decimal year.
544 * @throws IOException if initialization of world magnetic model fails.
545 */
546 public BodyMagneticFluxDensityBiasEstimator(
547 final Angle latitude, final Angle longitude, final double height, final double year) throws IOException {
548 this(latitude, longitude, height, year, (WorldMagneticModel) null);
549 }
550
551 /**
552 * Constructor.
553 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
554 * pitch = 0, yaw = 0) respect the horizon at provided body location.
555 * For Android devices this means that the device is flat on a horizontal surface
556 * with the screen facing down.
557 *
558 * @param latitude latitude.
559 * @param longitude longitude.
560 * @param height height.
561 * @param year time expressed as decimal year.
562 * @throws IOException if initialization of world magnetic model fails.
563 */
564 public BodyMagneticFluxDensityBiasEstimator(
565 final Angle latitude, final Angle longitude, final Distance height, final double year) throws IOException {
566 this(latitude, longitude, height, year, (WorldMagneticModel) null);
567 }
568
569 /**
570 * Constructor.
571 *
572 * @param position body position expressed in NED coordinates.
573 * @param nedC coordinate transformation from body to local navigation
574 * (NED) coordinates. This contains orientation respect the
575 * horizon at current body location.
576 * @param year time expressed as decimal year.
577 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
578 * transformation is not
579 * from body to local
580 * navigation coordinates.
581 * @throws IOException if initialization of
582 * world magnetic model
583 * fails.
584 */
585 public BodyMagneticFluxDensityBiasEstimator(
586 final NEDPosition position, final CoordinateTransformation nedC, final double year)
587 throws InvalidSourceAndDestinationFrameTypeException, IOException {
588 this(position, nedC, year, (WorldMagneticModel) null);
589 }
590
591 /**
592 * Constructor.
593 *
594 * @param position body position expressed in ECEF coordinates.
595 * @param nedC coordinate transformation from body to local navigation
596 * (NED) coordinates. This contains orientation respect the
597 * horizon at current body location.
598 * @param year time expressed as decimal year.
599 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
600 * transformation is not
601 * from body to local
602 * navigation coordinates.
603 * @throws IOException if initialization of
604 * world magnetic model
605 * fails.
606 */
607 public BodyMagneticFluxDensityBiasEstimator(
608 final ECEFPosition position, final CoordinateTransformation nedC, final double year)
609 throws InvalidSourceAndDestinationFrameTypeException, IOException {
610 this(position, nedC, year, (WorldMagneticModel) null);
611 }
612
613 /**
614 * Constructor.
615 * It is assumed that body is located at zero NED coordinates (latitude = 0,
616 * longitude = 0 and height = 0) and with zero Euler angles representing rotation
617 * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
618 * device is flat on a horizontal surface with the screen facing down.
619 *
620 * @param year time expressed as decimal year.
621 * @param listener listener to handle events raised by this estimator.
622 * @throws IOException if initialization of world magnetic model fails.
623 */
624 public BodyMagneticFluxDensityBiasEstimator(
625 final double year, final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
626 this(year);
627 this.listener = listener;
628 }
629
630 /**
631 * Constructor.
632 * It is assumed that body is located at zero NED coordinates (latitude = 0,
633 * longitude = 0, and height = 0) with provided orientation.
634 *
635 * @param nedC coordinate transformation from body to local navigation
636 * (NED) coordinates. This contains orientation respect the
637 * horizon at current body location.
638 * @param year time expressed as decimal year.
639 * @param listener listener to handle events raised by this estimator.
640 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
641 * transformation is not
642 * from body to local
643 * navigation coordinates.
644 * @throws IOException if initialization of
645 * world magnetic model
646 * fails.
647 */
648 public BodyMagneticFluxDensityBiasEstimator(
649 final CoordinateTransformation nedC, final double year,
650 final BodyMagneticFluxDensityBiasEstimatorListener listener)
651 throws InvalidSourceAndDestinationFrameTypeException, IOException {
652 this(nedC, year);
653 this.listener = listener;
654 }
655
656 /**
657 * Constructor.
658 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
659 * pitch = 0, yaw = 0) respect the horizon at provided body location.
660 * For Android devices this means that the device is flat on a horizontal surface
661 * with the screen facing down.
662 *
663 * @param latitude latitude expressed in radians (rad).
664 * @param longitude longitude expressed in radians (rad).
665 * @param height height expressed in meters (m).
666 * @param year time expressed as decimal year.
667 * @param listener listener to handle events raised by this estimator.
668 * @throws IOException if initialization of world magnetic model fails.
669 */
670 public BodyMagneticFluxDensityBiasEstimator(
671 final double latitude, final double longitude, final double height, final double year,
672 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
673 this(latitude, longitude, height, year);
674 this.listener = listener;
675 }
676
677 /**
678 * Constructor.
679 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
680 * pitch = 0, yaw = 0) respect the horizon at provided body location.
681 * For Android devices this means that the device is flat on a horizontal surface
682 * with the screen facing down.
683 *
684 * @param latitude latitude.
685 * @param longitude longitude.
686 * @param height height expressed in meters (m).
687 * @param year time expressed as decimal year.
688 * @param listener listener to handle events raised by this estimator.
689 * @throws IOException if initialization of world magnetic model fails.
690 */
691 public BodyMagneticFluxDensityBiasEstimator(
692 final Angle latitude, final Angle longitude, final double height, final double year,
693 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
694 this(latitude, longitude, height, year);
695 this.listener = listener;
696 }
697
698 /**
699 * Constructor.
700 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
701 * pitch = 0, yaw = 0) respect the horizon at provided body location.
702 * For Android devices this means that the device is flat on a horizontal surface
703 * with the screen facing down.
704 *
705 * @param latitude latitude.
706 * @param longitude longitude.
707 * @param height height.
708 * @param year time expressed as decimal year.
709 * @param listener listener to handle events raised by this estimator.
710 * @throws IOException if initialization of world magnetic model fails.
711 */
712 public BodyMagneticFluxDensityBiasEstimator(
713 final Angle latitude, final Angle longitude, final Distance height,
714 final double year, final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
715 this(latitude, longitude, height, year);
716 this.listener = listener;
717 }
718
719 /**
720 * Constructor.
721 *
722 * @param position body position expressed in NED coordinates.
723 * @param nedC coordinate transformation from body to local navigation
724 * (NED) coordinates. This contains orientation respect the
725 * horizon at current body location.
726 * @param year time expressed as decimal year.
727 * @param listener listener to handle events raised by this estimator.
728 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
729 * transformation is not
730 * from body to local
731 * navigation coordinates.
732 * @throws IOException if initialization of
733 * world magnetic model
734 * fails.
735 */
736 public BodyMagneticFluxDensityBiasEstimator(
737 final NEDPosition position, final CoordinateTransformation nedC, final double year,
738 final BodyMagneticFluxDensityBiasEstimatorListener listener)
739 throws InvalidSourceAndDestinationFrameTypeException, IOException {
740 this(position, nedC, year);
741 this.listener = listener;
742 }
743
744 /**
745 * Constructor.
746 *
747 * @param position body position expressed in ECEF coordinates.
748 * @param nedC coordinate transformation from body to local navigation
749 * (NED) coordinates. This contains orientation respect the
750 * horizon at current body location.
751 * @param year time expressed as decimal year.
752 * @param listener listener to handle events raised by this estimator.
753 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
754 * transformation is not
755 * from body to local
756 * navigation coordinates.
757 * @throws IOException if initialization of
758 * world magnetic model
759 * fails.
760 */
761 public BodyMagneticFluxDensityBiasEstimator(
762 final ECEFPosition position, final CoordinateTransformation nedC,
763 final double year, final BodyMagneticFluxDensityBiasEstimatorListener listener)
764 throws InvalidSourceAndDestinationFrameTypeException, IOException {
765 this(position, nedC, year);
766 this.listener = listener;
767 }
768
769 /**
770 * Constructor.
771 * It is assumed that body is located at zero NED coordinates (latitude = 0,
772 * longitude = 0 and height = 0) and with zero Euler angles representing rotation
773 * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
774 * device is flat on a horizontal surface with the screen facing down.
775 *
776 * @param date a time instance to be converted.
777 * @throws IOException if initialization of world magnetic model fails.
778 */
779 public BodyMagneticFluxDensityBiasEstimator(final Date date) throws IOException {
780 this(convertTime(date));
781 }
782
783 /**
784 * Constructor.
785 * It is assumed that body is located at zero NED coordinates (latitude = 0,
786 * longitude = 0, and height = 0) with provided orientation.
787 *
788 * @param nedC coordinate transformation from body to local navigation
789 * (NED) coordinates. This contains orientation respect the horizon
790 * at current body location.
791 * @param date a time instance to be converted.
792 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
793 * transformation is not
794 * from body to local
795 * navigation coordinates.
796 * @throws IOException if initialization of
797 * world magnetic model
798 * fails.
799 */
800 public BodyMagneticFluxDensityBiasEstimator(final CoordinateTransformation nedC, final Date date)
801 throws InvalidSourceAndDestinationFrameTypeException, IOException {
802 this(nedC, convertTime(date));
803 }
804
805 /**
806 * Constructor.
807 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
808 * pitch = 0, yaw = 0) respect the horizon at provided body location.
809 * For Android devices this means that the device is flat on a horizontal surface
810 * with the screen facing down.
811 *
812 * @param latitude latitude expressed in radians (rad).
813 * @param longitude longitude expressed in radians (rad).
814 * @param height height expressed in meters (m).
815 * @param date a time instance to be converted.
816 * @throws IOException if initialization of world magnetic model fails.
817 */
818 public BodyMagneticFluxDensityBiasEstimator(
819 final double latitude, final double longitude, final double height, final Date date) throws IOException {
820 this(latitude, longitude, height, convertTime(date));
821 }
822
823 /**
824 * Constructor.
825 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
826 * pitch = 0, yaw = 0) respect the horizon at provided body location.
827 * For Android devices this means that the device is flat on a horizontal surface
828 * with the screen facing down.
829 *
830 * @param latitude latitude.
831 * @param longitude longitude.
832 * @param height height expressed in meters (m).
833 * @param date a time instance to be converted.
834 * @throws IOException if initialization of world magnetic model fails.
835 */
836 public BodyMagneticFluxDensityBiasEstimator(
837 final Angle latitude, final Angle longitude, final double height, final Date date) throws IOException {
838 this(latitude, longitude, height, convertTime(date));
839 }
840
841 /**
842 * Constructor.
843 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
844 * pitch = 0, yaw = 0) respect the horizon at provided body location.
845 * For Android devices this means that the device is flat on a horizontal surface
846 * with the screen facing down.
847 *
848 * @param latitude latitude.
849 * @param longitude longitude.
850 * @param height height.
851 * @param date a time instance to be converted.
852 * @throws IOException if initialization of world magnetic model fails.
853 */
854 public BodyMagneticFluxDensityBiasEstimator(
855 final Angle latitude, final Angle longitude, final Distance height, final Date date) throws IOException {
856 this(latitude, longitude, height, convertTime(date));
857 }
858
859 /**
860 * Constructor.
861 *
862 * @param position body position expressed in NED coordinates.
863 * @param nedC coordinate transformation from body to local navigation
864 * (NED) coordinates. This contains orientation respect the
865 * horizon at current body location.
866 * @param date a time instance to be converted.
867 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
868 * transformation is not
869 * from body to local
870 * navigation coordinates.
871 * @throws IOException if initialization of
872 * world magnetic model
873 * fails.
874 */
875 public BodyMagneticFluxDensityBiasEstimator(
876 final NEDPosition position, final CoordinateTransformation nedC, final Date date)
877 throws InvalidSourceAndDestinationFrameTypeException, IOException {
878 this(position, nedC, convertTime(date));
879 }
880
881 /**
882 * Constructor.
883 *
884 * @param position body position expressed in ECEF coordinates.
885 * @param nedC coordinate transformation from body to local navigation
886 * (NED) coordinates. This contains orientation respect the
887 * horizon at current body location.
888 * @param date a time instance to be converted.
889 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
890 * transformation is not
891 * from body to local
892 * navigation coordinates.
893 * @throws IOException if initialization of
894 * world magnetic model
895 * fails.
896 */
897 public BodyMagneticFluxDensityBiasEstimator(
898 final ECEFPosition position, final CoordinateTransformation nedC, final Date date)
899 throws InvalidSourceAndDestinationFrameTypeException, IOException {
900 this(position, nedC, convertTime(date));
901 }
902
903 /**
904 * Constructor.
905 * It is assumed that body is located at zero NED coordinates (latitude = 0,
906 * longitude = 0 and height = 0) and with zero Euler angles representing rotation
907 * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
908 * device is flat on a horizontal surface with the screen facing down.
909 *
910 * @param date a time instance to be converted.
911 * @param listener listener to handle events raised by this estimator.
912 * @throws IOException if initialization of world magnetic model fails.
913 */
914 public BodyMagneticFluxDensityBiasEstimator(
915 final Date date, final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
916 this(convertTime(date), listener);
917 }
918
919 /**
920 * Constructor.
921 * It is assumed that body is located at zero NED coordinates (latitude = 0,
922 * longitude = 0, and height = 0) with provided orientation.
923 *
924 * @param nedC coordinate transformation from body to local navigation
925 * (NED) coordinates. This contains orientation respect the
926 * horizon at current body location.
927 * @param date a time instance to be converted.
928 * @param listener listener to handle events raised by this estimator.
929 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
930 * transformation is not
931 * from body to local
932 * navigation coordinates.
933 * @throws IOException if initialization of
934 * world magnetic model
935 * fails.
936 */
937 public BodyMagneticFluxDensityBiasEstimator(
938 final CoordinateTransformation nedC, final Date date,
939 final BodyMagneticFluxDensityBiasEstimatorListener listener)
940 throws InvalidSourceAndDestinationFrameTypeException, IOException {
941 this(nedC, convertTime(date), listener);
942 }
943
944 /**
945 * Constructor.
946 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
947 * pitch = 0, yaw = 0) respect the horizon at provided body location.
948 * For Android devices this means that the device is flat on a horizontal surface
949 * with the screen facing down.
950 *
951 * @param latitude latitude expressed in radians (rad).
952 * @param longitude longitude expressed in radians (rad).
953 * @param height height expressed in meters (m).
954 * @param date a time instance to be converted.
955 * @param listener listener to handle events raised by this estimator.
956 * @throws IOException if initialization of world magnetic model fails.
957 */
958 public BodyMagneticFluxDensityBiasEstimator(
959 final double latitude, final double longitude, final double height, final Date date,
960 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
961 this(latitude, longitude, height, convertTime(date), listener);
962 }
963
964 /**
965 * Constructor.
966 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
967 * pitch = 0, yaw = 0) respect the horizon at provided body location.
968 * For Android devices this means that the device is flat on a horizontal surface
969 * with the screen facing down.
970 *
971 * @param latitude latitude.
972 * @param longitude longitude.
973 * @param height height expressed in meters (m).
974 * @param date a time instance to be converted.
975 * @param listener listener to handle events raised by this estimator.
976 * @throws IOException if initialization of world magnetic model fails.
977 */
978 public BodyMagneticFluxDensityBiasEstimator(
979 final Angle latitude, final Angle longitude, final double height, final Date date,
980 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
981 this(latitude, longitude, height, convertTime(date), listener);
982 }
983
984 /**
985 * Constructor.
986 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
987 * pitch = 0, yaw = 0) respect the horizon at provided body location.
988 * For Android devices this means that the device is flat on a horizontal surface
989 * with the screen facing down.
990 *
991 * @param latitude latitude.
992 * @param longitude longitude.
993 * @param height height.
994 * @param date a time instance to be converted.
995 * @param listener listener to handle events raised by this estimator.
996 * @throws IOException if initialization of world magnetic model fails.
997 */
998 public BodyMagneticFluxDensityBiasEstimator(
999 final Angle latitude, final Angle longitude, final Distance height, final Date date,
1000 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
1001 this(latitude, longitude, height, convertTime(date), listener);
1002 }
1003
1004 /**
1005 * Constructor.
1006 *
1007 * @param position body position expressed in NED coordinates.
1008 * @param nedC coordinate transformation from body to local navigation
1009 * (NED) coordinates. This contains orientation respect the
1010 * horizon at current body location.
1011 * @param date a time instance to be converted.
1012 * @param listener listener to handle events raised by this estimator.
1013 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1014 * transformation is not
1015 * from body to local
1016 * navigation coordinates.
1017 * @throws IOException if initialization of
1018 * world magnetic model
1019 * fails.
1020 */
1021 public BodyMagneticFluxDensityBiasEstimator(
1022 final NEDPosition position, final CoordinateTransformation nedC, final Date date,
1023 final BodyMagneticFluxDensityBiasEstimatorListener listener)
1024 throws InvalidSourceAndDestinationFrameTypeException, IOException {
1025 this(position, nedC, convertTime(date), listener);
1026 }
1027
1028 /**
1029 * Constructor.
1030 *
1031 * @param position body position expressed in ECEF coordinates.
1032 * @param nedC coordinate transformation from body to local navigation
1033 * (NED) coordinates. This contains orientation respect the
1034 * horizon at current body location.
1035 * @param date a time instance to be converted.
1036 * @param listener listener to handle events raised by this estimator.
1037 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1038 * transformation is not
1039 * from body to local
1040 * navigation coordinates.
1041 * @throws IOException if initialization of
1042 * world magnetic model
1043 * fails.
1044 */
1045 public BodyMagneticFluxDensityBiasEstimator(
1046 final ECEFPosition position, final CoordinateTransformation nedC, final Date date,
1047 final BodyMagneticFluxDensityBiasEstimatorListener listener)
1048 throws InvalidSourceAndDestinationFrameTypeException, IOException {
1049 this(position, nedC, convertTime(date), listener);
1050 }
1051
1052 /**
1053 * Constructor.
1054 * It is assumed that body is located at zero NED coordinates (latitude = 0,
1055 * longitude = 0 and height = 0) and with zero Euler angles representing rotation
1056 * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
1057 * device is flat on a horizontal surface with the screen facing down.
1058 *
1059 * @param year time expressed as decimal year.
1060 * @param magneticModel world magnetic model of Earth or null if default
1061 * model is used.
1062 * @throws IOException if initialization of world magnetic model fails.
1063 */
1064 public BodyMagneticFluxDensityBiasEstimator(
1065 final double year, final WorldMagneticModel magneticModel) throws IOException {
1066 frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(new NEDFrame());
1067 this.year = year;
1068 this.magneticModel = magneticModel;
1069 initialize();
1070 }
1071
1072 /**
1073 * Constructor.
1074 * It is assumed that body is located at zero NED coordinates (latitude = 0,
1075 * longitude = 0, and height = 0) with provided orientation.
1076 *
1077 * @param nedC coordinate transformation from body to local navigation
1078 * (NED) coordinates. This contains orientation respect the horizon
1079 * at current body location.
1080 * @param year time expressed as decimal year.
1081 * @param magneticModel world magnetic model of Earth or null if default
1082 * model is used.
1083 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1084 * transformation is not
1085 * from body to local
1086 * navigation coordinates.
1087 * @throws IOException if initialization of
1088 * world magnetic model fails.
1089 */
1090 public BodyMagneticFluxDensityBiasEstimator(
1091 final CoordinateTransformation nedC, final double year, final WorldMagneticModel magneticModel)
1092 throws InvalidSourceAndDestinationFrameTypeException, IOException {
1093 frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(new NEDFrame(nedC));
1094 this.year = year;
1095 this.magneticModel = magneticModel;
1096 initialize();
1097 }
1098
1099 /**
1100 * Constructor.
1101 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
1102 * pitch = 0, yaw = 0) respect the horizon at provided body location.
1103 * For Android devices this means that the device is flat on a horizontal surface
1104 * with the screen facing down.
1105 *
1106 * @param latitude latitude expressed in radians (rad).
1107 * @param longitude longitude expressed in radians (rad).
1108 * @param height height expressed in meters (m).
1109 * @param year time expressed as decimal year.
1110 * @param magneticModel world magnetic model of Earth or null if default
1111 * model is used.
1112 * @throws IOException if initialization of world magnetic model fails.
1113 */
1114 public BodyMagneticFluxDensityBiasEstimator(
1115 final double latitude, final double longitude, final double height,
1116 final double year, final WorldMagneticModel magneticModel) throws IOException {
1117 frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(new NEDFrame(latitude, longitude, height));
1118 this.year = year;
1119 this.magneticModel = magneticModel;
1120 initialize();
1121 }
1122
1123 /**
1124 * Constructor.
1125 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
1126 * pitch = 0, yaw = 0) respect the horizon at provided body location.
1127 * For Android devices this means that the device is flat on a horizontal surface
1128 * with the screen facing down.
1129 *
1130 * @param latitude latitude.
1131 * @param longitude longitude.
1132 * @param height height expressed in meters (m).
1133 * @param year time expressed as decimal year.
1134 * @param magneticModel world magnetic model of Earth or null if default
1135 * model is used.
1136 * @throws IOException if initialization of world magnetic model fails.
1137 */
1138 public BodyMagneticFluxDensityBiasEstimator(
1139 final Angle latitude, final Angle longitude, final double height,
1140 final double year, final WorldMagneticModel magneticModel) throws IOException {
1141 frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(new NEDFrame(latitude, longitude, height));
1142 this.year = year;
1143 this.magneticModel = magneticModel;
1144 initialize();
1145 }
1146
1147 /**
1148 * Constructor.
1149 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
1150 * pitch = 0, yaw = 0) respect the horizon at provided body location.
1151 * For Android devices this means that the device is flat on a horizontal surface
1152 * with the screen facing down.
1153 *
1154 * @param latitude latitude.
1155 * @param longitude longitude.
1156 * @param height height.
1157 * @param year time expressed as decimal year.
1158 * @param magneticModel world magnetic model of Earth or null if default
1159 * model is used.
1160 * @throws IOException if initialization of world magnetic model fails.
1161 */
1162 public BodyMagneticFluxDensityBiasEstimator(
1163 final Angle latitude, final Angle longitude, final Distance height,
1164 final double year, final WorldMagneticModel magneticModel) throws IOException {
1165 frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(new NEDFrame(latitude, longitude, height));
1166 this.year = year;
1167 this.magneticModel = magneticModel;
1168 initialize();
1169 }
1170
1171 /**
1172 * Constructor.
1173 *
1174 * @param position body position expressed in NED coordinates.
1175 * @param nedC coordinate transformation from body to local navigation
1176 * (NED) coordinates. This contains orientation respect the
1177 * horizon at current body location.
1178 * @param year time expressed as decimal year.
1179 * @param magneticModel world magnetic model of Earth or null if default
1180 * model is used.
1181 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1182 * transformation is not
1183 * from body to local
1184 * navigation coordinates.
1185 * @throws IOException if initialization of
1186 * world magnetic model
1187 * fails.
1188 */
1189 public BodyMagneticFluxDensityBiasEstimator(
1190 final NEDPosition position, final CoordinateTransformation nedC,
1191 final double year, final WorldMagneticModel magneticModel)
1192 throws InvalidSourceAndDestinationFrameTypeException, IOException {
1193 frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(new NEDFrame(position, nedC));
1194 this.year = year;
1195 this.magneticModel = magneticModel;
1196 initialize();
1197 }
1198
1199 /**
1200 * Constructor.
1201 *
1202 * @param position body position expressed in ECEF coordinates.
1203 * @param nedC coordinate transformation from body to local navigation
1204 * (NED) coordinates. This contains orientation respect the
1205 * horizon at current body location.
1206 * @param year time expressed as decimal year.
1207 * @param magneticModel world magnetic model of Earth or null if default
1208 * model is used.
1209 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1210 * transformation is not
1211 * from body to local
1212 * navigation coordinates.
1213 * @throws IOException if initialization of
1214 * world magnetic model
1215 * fails.
1216 */
1217 public BodyMagneticFluxDensityBiasEstimator(
1218 final ECEFPosition position, final CoordinateTransformation nedC,
1219 final double year, final WorldMagneticModel magneticModel)
1220 throws InvalidSourceAndDestinationFrameTypeException, IOException {
1221 frame = new ECEFFrame(position);
1222 final var nedFrame = ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(frame);
1223 nedFrame.setCoordinateTransformation(nedC);
1224 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
1225 this.year = year;
1226 this.magneticModel = magneticModel;
1227 initialize();
1228 }
1229
1230 /**
1231 * Constructor.
1232 * It is assumed that body is located at zero NED coordinates (latitude = 0,
1233 * longitude = 0 and height = 0) and with zero Euler angles representing rotation
1234 * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
1235 * device is flat on a horizontal surface with the screen facing down.
1236 *
1237 * @param year time expressed as decimal year.
1238 * @param listener listener to handle events raised by this estimator.
1239 * @param magneticModel world magnetic model of Earth or null if default
1240 * model is used.
1241 * @throws IOException if initialization of world magnetic model fails.
1242 */
1243 public BodyMagneticFluxDensityBiasEstimator(
1244 final double year, final WorldMagneticModel magneticModel,
1245 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
1246 this(year, magneticModel);
1247 this.listener = listener;
1248 }
1249
1250 /**
1251 * Constructor.
1252 * It is assumed that body is located at zero NED coordinates (latitude = 0,
1253 * longitude = 0, and height = 0) with provided orientation.
1254 *
1255 * @param nedC coordinate transformation from body to local navigation
1256 * (NED) coordinates. This contains orientation respect the
1257 * horizon at current body location.
1258 * @param year time expressed as decimal year.
1259 * @param magneticModel world magnetic model of Earth or null if default
1260 * model is used.
1261 * @param listener listener to handle events raised by this estimator.
1262 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1263 * transformation is not
1264 * from body to local
1265 * navigation coordinates.
1266 * @throws IOException if initialization of
1267 * world magnetic model
1268 * fails.
1269 */
1270 public BodyMagneticFluxDensityBiasEstimator(
1271 final CoordinateTransformation nedC, final double year, final WorldMagneticModel magneticModel,
1272 final BodyMagneticFluxDensityBiasEstimatorListener listener)
1273 throws InvalidSourceAndDestinationFrameTypeException, IOException {
1274 this(nedC, year, magneticModel);
1275 this.listener = listener;
1276 }
1277
1278 /**
1279 * Constructor.
1280 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
1281 * pitch = 0, yaw = 0) respect the horizon at provided body location.
1282 * For Android devices this means that the device is flat on a horizontal surface
1283 * with the screen facing down.
1284 *
1285 * @param latitude latitude expressed in radians (rad).
1286 * @param longitude longitude expressed in radians (rad).
1287 * @param height height expressed in meters (m).
1288 * @param year time expressed as decimal year.
1289 * @param magneticModel world magnetic model of Earth or null if default
1290 * model is used.
1291 * @param listener listener to handle events raised by this estimator.
1292 * @throws IOException if initialization of world magnetic model fails.
1293 */
1294 public BodyMagneticFluxDensityBiasEstimator(
1295 final double latitude, final double longitude, final double height,
1296 final double year, final WorldMagneticModel magneticModel,
1297 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
1298 this(latitude, longitude, height, year, magneticModel);
1299 this.listener = listener;
1300 }
1301
1302 /**
1303 * Constructor.
1304 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
1305 * pitch = 0, yaw = 0) respect the horizon at provided body location.
1306 * For Android devices this means that the device is flat on a horizontal surface
1307 * with the screen facing down.
1308 *
1309 * @param latitude latitude.
1310 * @param longitude longitude.
1311 * @param height height expressed in meters (m).
1312 * @param year time expressed as decimal year.
1313 * @param magneticModel world magnetic model of Earth or null if default
1314 * model is used.
1315 * @param listener listener to handle events raised by this estimator.
1316 * @throws IOException if initialization of world magnetic model fails.
1317 */
1318 public BodyMagneticFluxDensityBiasEstimator(
1319 final Angle latitude, final Angle longitude, final double height, final double year,
1320 final WorldMagneticModel magneticModel,
1321 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
1322 this(latitude, longitude, height, year, magneticModel);
1323 this.listener = listener;
1324 }
1325
1326 /**
1327 * Constructor.
1328 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
1329 * pitch = 0, yaw = 0) respect the horizon at provided body location.
1330 * For Android devices this means that the device is flat on a horizontal surface
1331 * with the screen facing down.
1332 *
1333 * @param latitude latitude.
1334 * @param longitude longitude.
1335 * @param height height.
1336 * @param year time expressed as decimal year.
1337 * @param magneticModel world magnetic model of Earth or null if default
1338 * model is used.
1339 * @param listener listener to handle events raised by this estimator.
1340 * @throws IOException if initialization of world magnetic model fails.
1341 */
1342 public BodyMagneticFluxDensityBiasEstimator(
1343 final Angle latitude, final Angle longitude, final Distance height,
1344 final double year, final WorldMagneticModel magneticModel,
1345 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
1346 this(latitude, longitude, height, year, magneticModel);
1347 this.listener = listener;
1348 }
1349
1350 /**
1351 * Constructor.
1352 *
1353 * @param position body position expressed in NED coordinates.
1354 * @param nedC coordinate transformation from body to local navigation
1355 * (NED) coordinates. This contains orientation respect the
1356 * horizon at current body location.
1357 * @param year time expressed as decimal year.
1358 * @param magneticModel world magnetic model of Earth or null if default
1359 * model is used.
1360 * @param listener listener to handle events raised by this estimator.
1361 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1362 * transformation is not
1363 * from body to local
1364 * navigation coordinates.
1365 * @throws IOException if initialization of
1366 * world magnetic model
1367 * fails.
1368 */
1369 public BodyMagneticFluxDensityBiasEstimator(
1370 final NEDPosition position, final CoordinateTransformation nedC, final double year,
1371 final WorldMagneticModel magneticModel, final BodyMagneticFluxDensityBiasEstimatorListener listener)
1372 throws InvalidSourceAndDestinationFrameTypeException, IOException {
1373 this(position, nedC, year, magneticModel);
1374 this.listener = listener;
1375 }
1376
1377 /**
1378 * Constructor.
1379 *
1380 * @param position body position expressed in ECEF coordinates.
1381 * @param nedC coordinate transformation from body to local navigation
1382 * (NED) coordinates. This contains orientation respect the
1383 * horizon at current body location.
1384 * @param year time expressed as decimal year.
1385 * @param magneticModel world magnetic model of Earth or null if default
1386 * model is used.
1387 * @param listener listener to handle events raised by this estimator.
1388 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1389 * transformation is not
1390 * from body to local
1391 * navigation coordinates.
1392 * @throws IOException if initialization of
1393 * world magnetic model
1394 * fails.
1395 */
1396 public BodyMagneticFluxDensityBiasEstimator(
1397 final ECEFPosition position, final CoordinateTransformation nedC,
1398 final double year, final WorldMagneticModel magneticModel,
1399 final BodyMagneticFluxDensityBiasEstimatorListener listener)
1400 throws InvalidSourceAndDestinationFrameTypeException, IOException {
1401 this(position, nedC, year, magneticModel);
1402 this.listener = listener;
1403 }
1404
1405 /**
1406 * Constructor.
1407 * It is assumed that body is located at zero NED coordinates (latitude = 0,
1408 * longitude = 0 and height = 0) and with zero Euler angles representing rotation
1409 * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
1410 * device is flat on a horizontal surface with the screen facing down.
1411 *
1412 * @param date a time instance to be converted.
1413 * @param magneticModel world magnetic model of Earth or null if default
1414 * model is used.
1415 * @throws IOException if initialization of world magnetic model fails.
1416 */
1417 public BodyMagneticFluxDensityBiasEstimator(
1418 final Date date, final WorldMagneticModel magneticModel) throws IOException {
1419 this(convertTime(date), magneticModel);
1420 }
1421
1422 /**
1423 * Constructor.
1424 * It is assumed that body is located at zero NED coordinates (latitude = 0,
1425 * longitude = 0, and height = 0) with provided orientation.
1426 *
1427 * @param nedC coordinate transformation from body to local navigation
1428 * (NED) coordinates. This contains orientation respect the horizon
1429 * at current body location.
1430 * @param date a time instance to be converted.
1431 * @param magneticModel world magnetic model of Earth or null if default
1432 * model is used.
1433 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1434 * transformation is not
1435 * from body to local
1436 * navigation coordinates.
1437 * @throws IOException if initialization of
1438 * world magnetic model
1439 * fails.
1440 */
1441 public BodyMagneticFluxDensityBiasEstimator(
1442 final CoordinateTransformation nedC, final Date date, final WorldMagneticModel magneticModel)
1443 throws InvalidSourceAndDestinationFrameTypeException, IOException {
1444 this(nedC, convertTime(date), magneticModel);
1445 }
1446
1447 /**
1448 * Constructor.
1449 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
1450 * pitch = 0, yaw = 0) respect the horizon at provided body location.
1451 * For Android devices this means that the device is flat on a horizontal surface
1452 * with the screen facing down.
1453 *
1454 * @param latitude latitude expressed in radians (rad).
1455 * @param longitude longitude expressed in radians (rad).
1456 * @param height height expressed in meters (m).
1457 * @param date a time instance to be converted.
1458 * @param magneticModel world magnetic model of Earth or null if default
1459 * model is used.
1460 * @throws IOException if initialization of world magnetic model fails.
1461 */
1462 public BodyMagneticFluxDensityBiasEstimator(
1463 final double latitude, final double longitude, final double height,
1464 final Date date, final WorldMagneticModel magneticModel) throws IOException {
1465 this(latitude, longitude, height, convertTime(date), magneticModel);
1466 }
1467
1468 /**
1469 * Constructor.
1470 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
1471 * pitch = 0, yaw = 0) respect the horizon at provided body location.
1472 * For Android devices this means that the device is flat on a horizontal surface
1473 * with the screen facing down.
1474 *
1475 * @param latitude latitude.
1476 * @param longitude longitude.
1477 * @param height height expressed in meters (m).
1478 * @param date a time instance to be converted.
1479 * @param magneticModel world magnetic model of Earth or null if default
1480 * model is used.
1481 * @throws IOException if initialization of world magnetic model fails.
1482 */
1483 public BodyMagneticFluxDensityBiasEstimator(
1484 final Angle latitude, final Angle longitude, final double height,
1485 final Date date, final WorldMagneticModel magneticModel) throws IOException {
1486 this(latitude, longitude, height, convertTime(date), magneticModel);
1487 }
1488
1489 /**
1490 * Constructor.
1491 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
1492 * pitch = 0, yaw = 0) respect the horizon at provided body location.
1493 * For Android devices this means that the device is flat on a horizontal surface
1494 * with the screen facing down.
1495 *
1496 * @param latitude latitude.
1497 * @param longitude longitude.
1498 * @param height height.
1499 * @param date a time instance to be converted.
1500 * @param magneticModel world magnetic model of Earth or null if default
1501 * model is used.
1502 * @throws IOException if initialization of world magnetic model fails.
1503 */
1504 public BodyMagneticFluxDensityBiasEstimator(
1505 final Angle latitude, final Angle longitude, final Distance height,
1506 final Date date, final WorldMagneticModel magneticModel) throws IOException {
1507 this(latitude, longitude, height, convertTime(date), magneticModel);
1508 }
1509
1510 /**
1511 * Constructor.
1512 *
1513 * @param position body position expressed in NED coordinates.
1514 * @param nedC coordinate transformation from body to local navigation
1515 * (NED) coordinates. This contains orientation respect the
1516 * horizon at current body location.
1517 * @param date a time instance to be converted.
1518 * @param magneticModel world magnetic model of Earth or null if default
1519 * model is used.
1520 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1521 * transformation is not
1522 * from body to local
1523 * navigation coordinates.
1524 * @throws IOException if initialization of
1525 * world magnetic model
1526 * fails.
1527 */
1528 public BodyMagneticFluxDensityBiasEstimator(
1529 final NEDPosition position, final CoordinateTransformation nedC, final Date date,
1530 final WorldMagneticModel magneticModel) throws InvalidSourceAndDestinationFrameTypeException, IOException {
1531 this(position, nedC, convertTime(date), magneticModel);
1532 }
1533
1534 /**
1535 * Constructor.
1536 *
1537 * @param position body position expressed in ECEF coordinates.
1538 * @param nedC coordinate transformation from body to local navigation
1539 * (NED) coordinates. This contains orientation respect the
1540 * horizon at current body location.
1541 * @param date a time instance to be converted.
1542 * @param magneticModel world magnetic model of Earth or null if default
1543 * model is used.
1544 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1545 * transformation is not
1546 * from body to local
1547 * navigation coordinates.
1548 * @throws IOException if initialization of
1549 * world magnetic model
1550 * fails.
1551 */
1552 public BodyMagneticFluxDensityBiasEstimator(
1553 final ECEFPosition position, final CoordinateTransformation nedC, final Date date,
1554 final WorldMagneticModel magneticModel) throws InvalidSourceAndDestinationFrameTypeException, IOException {
1555 this(position, nedC, convertTime(date), magneticModel);
1556 }
1557
1558 /**
1559 * Constructor.
1560 * It is assumed that body is located at zero NED coordinates (latitude = 0,
1561 * longitude = 0 and height = 0) and with zero Euler angles representing rotation
1562 * (roll = 0, pith = 0, yaw = 0), which for Android devices it means that the
1563 * device is flat on a horizontal surface with the screen facing down.
1564 *
1565 * @param date a time instance to be converted.
1566 * @param magneticModel world magnetic model of Earth or null if default
1567 * model is used.
1568 * @param listener listener to handle events raised by this estimator.
1569 * @throws IOException if initialization of world magnetic model fails.
1570 */
1571 public BodyMagneticFluxDensityBiasEstimator(
1572 final Date date, final WorldMagneticModel magneticModel,
1573 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
1574 this(convertTime(date), magneticModel, listener);
1575 }
1576
1577 /**
1578 * Constructor.
1579 * It is assumed that body is located at zero NED coordinates (latitude = 0,
1580 * longitude = 0, and height = 0) with provided orientation.
1581 *
1582 * @param nedC coordinate transformation from body to local navigation
1583 * (NED) coordinates. This contains orientation respect the
1584 * horizon at current body location.
1585 * @param date a time instance to be converted.
1586 * @param magneticModel world magnetic model of Earth or null if default
1587 * model is used.
1588 * @param listener listener to handle events raised by this estimator.
1589 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1590 * transformation is not
1591 * from body to local
1592 * navigation coordinates.
1593 * @throws IOException if initialization of
1594 * world magnetic model
1595 * fails.
1596 */
1597 public BodyMagneticFluxDensityBiasEstimator(
1598 final CoordinateTransformation nedC, final Date date, final WorldMagneticModel magneticModel,
1599 final BodyMagneticFluxDensityBiasEstimatorListener listener)
1600 throws InvalidSourceAndDestinationFrameTypeException, IOException {
1601 this(nedC, convertTime(date), magneticModel, listener);
1602 }
1603
1604 /**
1605 * Constructor.
1606 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
1607 * pitch = 0, yaw = 0) respect the horizon at provided body location.
1608 * For Android devices this means that the device is flat on a horizontal surface
1609 * with the screen facing down.
1610 *
1611 * @param latitude latitude expressed in radians (rad).
1612 * @param longitude longitude expressed in radians (rad).
1613 * @param height height expressed in meters (m).
1614 * @param date a time instance to be converted.
1615 * @param magneticModel world magnetic model of Earth or null if default
1616 * model is used.
1617 * @param listener listener to handle events raised by this estimator.
1618 * @throws IOException if initialization of world magnetic model fails.
1619 */
1620 public BodyMagneticFluxDensityBiasEstimator(
1621 final double latitude, final double longitude, final double height,
1622 final Date date, final WorldMagneticModel magneticModel,
1623 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws IOException {
1624 this(latitude, longitude, height, convertTime(date), magneticModel, listener);
1625 }
1626
1627 /**
1628 * Constructor.
1629 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
1630 * pitch = 0, yaw = 0) respect the horizon at provided body location.
1631 * For Android devices this means that the device is flat on a horizontal surface
1632 * with the screen facing down.
1633 *
1634 * @param latitude latitude.
1635 * @param longitude longitude.
1636 * @param height height expressed in meters (m).
1637 * @param date a time instance to be converted.
1638 * @param magneticModel world magnetic model of Earth or null if default
1639 * model is used.
1640 * @param listener listener to handle events raised by this estimator.
1641 * @throws IOException if initialization of world magnetic model fails.
1642 */
1643 public BodyMagneticFluxDensityBiasEstimator(
1644 final Angle latitude, final Angle longitude, final double height, final Date date,
1645 final WorldMagneticModel magneticModel, final BodyMagneticFluxDensityBiasEstimatorListener listener)
1646 throws IOException {
1647 this(latitude, longitude, height, convertTime(date), magneticModel, listener);
1648 }
1649
1650 /**
1651 * Constructor.
1652 * It is assumed that body has zero Euler angles representing rotation (roll = 0,
1653 * pitch = 0, yaw = 0) respect the horizon at provided body location.
1654 * For Android devices this means that the device is flat on a horizontal surface
1655 * with the screen facing down.
1656 *
1657 * @param latitude latitude.
1658 * @param longitude longitude.
1659 * @param height height.
1660 * @param date a time instance to be converted.
1661 * @param magneticModel world magnetic model of Earth or null if default
1662 * model is used.
1663 * @param listener listener to handle events raised by this estimator.
1664 * @throws IOException if initialization of world magnetic model fails.
1665 */
1666 public BodyMagneticFluxDensityBiasEstimator(
1667 final Angle latitude, final Angle longitude, final Distance height, final Date date,
1668 final WorldMagneticModel magneticModel, final BodyMagneticFluxDensityBiasEstimatorListener listener)
1669 throws IOException {
1670 this(latitude, longitude, height, convertTime(date), magneticModel, listener);
1671 }
1672
1673 /**
1674 * Constructor.
1675 *
1676 * @param position body position expressed in NED coordinates.
1677 * @param nedC coordinate transformation from body to local navigation
1678 * (NED) coordinates. This contains orientation respect the
1679 * horizon at current body location.
1680 * @param date a time instance to be converted.
1681 * @param magneticModel world magnetic model of Earth or null if default
1682 * model is used.
1683 * @param listener listener to handle events raised by this estimator.
1684 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1685 * transformation is not
1686 * from body to local
1687 * navigation coordinates.
1688 * @throws IOException if initialization of
1689 * world magnetic model
1690 * fails.
1691 */
1692 public BodyMagneticFluxDensityBiasEstimator(
1693 final NEDPosition position, final CoordinateTransformation nedC,
1694 final Date date, final WorldMagneticModel magneticModel,
1695 final BodyMagneticFluxDensityBiasEstimatorListener listener)
1696 throws InvalidSourceAndDestinationFrameTypeException, IOException {
1697 this(position, nedC, convertTime(date), magneticModel, listener);
1698 }
1699
1700 /**
1701 * Constructor.
1702 *
1703 * @param position body position expressed in ECEF coordinates.
1704 * @param nedC coordinate transformation from body to local navigation
1705 * (NED) coordinates. This contains orientation respect the
1706 * horizon at current body location.
1707 * @param date a time instance to be converted.
1708 * @param magneticModel world magnetic model of Earth or null if default
1709 * model is used.
1710 * @param listener listener to handle events raised by this estimator.
1711 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1712 * transformation is not
1713 * from body to local
1714 * navigation coordinates.
1715 * @throws IOException if initialization of
1716 * world magnetic model
1717 * fails.
1718 */
1719 public BodyMagneticFluxDensityBiasEstimator(
1720 final ECEFPosition position, final CoordinateTransformation nedC,
1721 final Date date, final WorldMagneticModel magneticModel,
1722 final BodyMagneticFluxDensityBiasEstimatorListener listener)
1723 throws InvalidSourceAndDestinationFrameTypeException, IOException {
1724 this(position, nedC, convertTime(date), magneticModel, listener);
1725 }
1726
1727
1728 /**
1729 * Gets time interval between body kinematics (IMU acceleration + gyroscope)
1730 * samples expressed in seconds (s).
1731 *
1732 * @return time interval between body kinematics samples.
1733 */
1734 public double getTimeInterval() {
1735 return timeInterval;
1736 }
1737
1738 /**
1739 * Sets time interval between body kinematics (IMU acceleration + gyroscope)
1740 * samples expressed in seconds (s).
1741 *
1742 * @param timeInterval time interval between body kinematics samples.
1743 * @throws LockedException if estimator is currently running.
1744 */
1745 public void setTimeInterval(final double timeInterval) throws LockedException {
1746 if (running) {
1747 throw new LockedException();
1748 }
1749
1750 if (timeInterval < 0.0) {
1751 throw new IllegalArgumentException();
1752 }
1753
1754 this.timeInterval = timeInterval;
1755 }
1756
1757 /**
1758 * Gets time interval between body kinematics (IMU acceleration + gyroscope)
1759 * samples.
1760 *
1761 * @return time interval between body kinematics samples.
1762 */
1763 public Time getTimeIntervalAsTime() {
1764 return new Time(timeInterval, TimeUnit.SECOND);
1765 }
1766
1767 /**
1768 * Gets time interval between body kinematics (IMU acceleration + gyroscope)
1769 * samples.
1770 *
1771 * @param result instance where time interval will be stored.
1772 */
1773 public void getTimeIntervalAsTime(final Time result) {
1774 result.setValue(timeInterval);
1775 result.setUnit(TimeUnit.SECOND);
1776 }
1777
1778 /**
1779 * Sets time interval between body kinematics (IMU acceleration + gyroscope)
1780 * samples.
1781 *
1782 * @param timeInterval time interval between body kinematics samples.
1783 * @throws LockedException if estimator is currently running.
1784 */
1785 public void setTimeInterval(final Time timeInterval) throws LockedException {
1786 setTimeInterval(convertTime(timeInterval));
1787 }
1788
1789 /**
1790 * Gets current body position expressed in ECEF coordinates.
1791 *
1792 * @return current body position expressed in ECEF coordinates.
1793 */
1794 public ECEFPosition getEcefPosition() {
1795 return frame.getECEFPosition();
1796 }
1797
1798 /**
1799 * Gets current body position expressed in ECEF coordinates.
1800 *
1801 * @param result instance where current body position will be stored.
1802 */
1803 public void getEcefPosition(final ECEFPosition result) {
1804 frame.getECEFPosition(result);
1805 }
1806
1807 /**
1808 * Sets current body position expressed in ECEF coordinates.
1809 *
1810 * @param position current body position to be set.
1811 * @throws LockedException if estimator is currently running.
1812 */
1813 public void setEcefPosition(final ECEFPosition position) throws LockedException {
1814 if (running) {
1815 throw new LockedException();
1816 }
1817
1818 frame.setPosition(position);
1819
1820 rebuildExpectedBodyMagneticFluxDensity();
1821 }
1822
1823 /**
1824 * Sets current body position expressed in ECEF coordinates.
1825 *
1826 * @param x x position resolved around ECEF axes and expressed in meters (m).
1827 * @param y y position resolved around ECEF axes and expressed in meters (m).
1828 * @param z z position resolved around ECEF axes and expressed in meters (m).
1829 * @throws LockedException if estimator is currently running.
1830 */
1831 public void setEcefPosition(final double x, final double y, final double z) throws LockedException {
1832 if (running) {
1833 throw new LockedException();
1834 }
1835
1836 frame.setCoordinates(x, y, z);
1837
1838 rebuildExpectedBodyMagneticFluxDensity();
1839 }
1840
1841 /**
1842 * Sets current body position expressed in ECEF coordinates.
1843 *
1844 * @param x x position resolved around ECEF axes.
1845 * @param y y position resolved around ECEF axes.
1846 * @param z z position resolved around ECEF axes.
1847 * @throws LockedException if estimator is currently running.
1848 */
1849 public void setEcefPosition(
1850 final Distance x, final Distance y, final Distance z) throws LockedException {
1851 if (running) {
1852 throw new LockedException();
1853 }
1854
1855 frame.setPositionCoordinates(x, y, z);
1856
1857 rebuildExpectedBodyMagneticFluxDensity();
1858 }
1859
1860 /**
1861 * Sets current body position expressed in ECEF coordinates.
1862 *
1863 * @param position position resolved around ECEF axes and expressed in meters (m).
1864 * @throws LockedException if estimator is currently running.
1865 */
1866 public void setEcefPosition(final Point3D position) throws LockedException {
1867 if (running) {
1868 throw new LockedException();
1869 }
1870
1871 frame.setPosition(position);
1872
1873 rebuildExpectedBodyMagneticFluxDensity();
1874 }
1875
1876 /**
1877 * Gets ECEF frame containing current body position and orientation expressed
1878 * in ECEF coordinates. Frame also contains body velocity, but it is always
1879 * assumed to be zero during calibration.
1880 *
1881 * @return ECEF frame containing current body position and orientation resolved
1882 * around ECEF axes.
1883 */
1884 public ECEFFrame getEcefFrame() {
1885 return new ECEFFrame(frame);
1886 }
1887
1888 /**
1889 * Gets ECEF frame containing current body position and orientation expressed
1890 * in ECEF coordinates. Frame also contains body velocity, but it is always
1891 * assumed to be zero during calibration.
1892 *
1893 * @param result instance where ECEF frame containing current body position and
1894 * orientation resolved around ECEF axes will be stored.
1895 */
1896 public void getEcefFrame(final ECEFFrame result) {
1897 frame.copyTo(result);
1898 }
1899
1900 /**
1901 * Gets NED frame containing current body position and orientation expressed
1902 * in NED coordinates. Frame also contains body velocity, but it is always
1903 * assumed to be zero during calibration.
1904 *
1905 * @return NED frame containing current body position and orientation resolved
1906 * around NED axes.
1907 */
1908 public NEDFrame getNedFrame() {
1909 return ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(frame);
1910 }
1911
1912 /**
1913 * Gets NED frame containing current body position and orientation expressed
1914 * in NED coordinates. Frame also contains body velocity, but it is always
1915 * assumed to be zero during calibration.
1916 *
1917 * @param result instance where NED frame containing current body position and
1918 * orientation resolved around NED axes will be stored.
1919 */
1920 public void getNedFrame(final NEDFrame result) {
1921 ECEFtoNEDFrameConverter.convertECEFtoNED(frame, result);
1922 }
1923
1924 /**
1925 * Gets current body position expressed in NED coordinates.
1926 *
1927 * @return current body position expressed in NED coordinates.
1928 */
1929 public NEDPosition getNedPosition() {
1930 return getNedFrame().getPosition();
1931 }
1932
1933 /**
1934 * Gets current body position expressed in NED coordinates.
1935 *
1936 * @param result instance where current body position will be stored.
1937 */
1938 public void getNedPosition(final NEDPosition result) {
1939 getNedFrame().getPosition(result);
1940 }
1941
1942 /**
1943 * Sets current body position expressed in NED coordinates.
1944 *
1945 * @param position current body position to be set.
1946 * @throws LockedException if estimator is currently running.
1947 */
1948 public void setNedPosition(final NEDPosition position) throws LockedException {
1949 if (running) {
1950 throw new LockedException();
1951 }
1952
1953 final var nedFrame = getNedFrame();
1954 nedFrame.setPosition(position);
1955 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
1956
1957 rebuildExpectedBodyMagneticFluxDensity();
1958 }
1959
1960 /**
1961 * Sets current body position expressed in NED coordinates.
1962 *
1963 * @param latitude latitude NED coordinate expressed in radians (rad).
1964 * @param longitude longitude NED coordinate expressed in radians (rad).
1965 * @param height height NED coordinate expressed in meters (m).
1966 * @throws LockedException if estimator is currently running.
1967 */
1968 public void setNedPosition(final double latitude, final double longitude, final double height)
1969 throws LockedException {
1970 if (running) {
1971 throw new LockedException();
1972 }
1973
1974 final var nedFrame = getNedFrame();
1975 nedFrame.setPosition(latitude, longitude, height);
1976 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
1977
1978 rebuildExpectedBodyMagneticFluxDensity();
1979 }
1980
1981 /**
1982 * Sets current body position expressed in NED coordinates.
1983 *
1984 * @param latitude latitude NED coordinate.
1985 * @param longitude longitude NED coordinate.
1986 * @param height height NED coordinate expressed in meters (m).
1987 * @throws LockedException if estimator is currently running.
1988 */
1989 public void setNedPosition(
1990 final Angle latitude, final Angle longitude, final double height) throws LockedException {
1991 if (running) {
1992 throw new LockedException();
1993 }
1994
1995 final var nedFrame = getNedFrame();
1996 nedFrame.setPosition(latitude, longitude, height);
1997 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
1998
1999 rebuildExpectedBodyMagneticFluxDensity();
2000 }
2001
2002 /**
2003 * Sets current body position expressed in NED coordinates.
2004 *
2005 * @param latitude latitude NED coordinate.
2006 * @param longitude longitude NED coordinate.
2007 * @param height height NED coordinate.
2008 * @throws LockedException if estimator is currently running.
2009 */
2010 public void setNedPosition(
2011 final Angle latitude, final Angle longitude, final Distance height) throws LockedException {
2012 if (running) {
2013 throw new LockedException();
2014 }
2015
2016 final var nedFrame = getNedFrame();
2017 nedFrame.setPosition(latitude, longitude, height);
2018 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2019
2020 rebuildExpectedBodyMagneticFluxDensity();
2021 }
2022
2023 /**
2024 * Gets current body orientation as a transformation from body to ECEF coordinates.
2025 * Notice that returned orientation refers to ECEF Earth axes, which means that
2026 * orientation is not relative to the ground or horizon at current body position.
2027 * Typically it is more convenient to use {@link #getNedC()} to obtain orientation
2028 * relative to the ground or horizon at current body position. For instance, on
2029 * Android devices a NED orientation with Euler angles (roll = 0, pitch = 0,
2030 * yaw = 0) means that the device is laying flat on a horizontal surface with the
2031 * screen facing down towards the ground.
2032 *
2033 * @return current body orientation resolved on ECEF axes.
2034 */
2035 public CoordinateTransformation getEcefC() {
2036 return frame.getCoordinateTransformation();
2037 }
2038
2039 /**
2040 * Gets current body orientation as a transformation from body to ECEF coordinates.
2041 * Notice that returned orientation refers to ECEF Earth axes, which means that
2042 * orientation is not relative to the ground or horizon at current body position.
2043 * Typically it is more convenient to use {@link #getNedC()} to obtain orientation
2044 * relative to the ground or horizon at current body position. For instance, on
2045 * Android devices a NED orientation with Euler angles (roll = 0, pitch = 0,
2046 * yaw = 0) means that the device is laying flat on a horizontal surface with the
2047 * screen facing down towards the ground.
2048 *
2049 * @param result instance where current body orientation resolved on ECEF axes
2050 * will be stored.
2051 */
2052 public void getEcefC(final CoordinateTransformation result) {
2053 frame.getCoordinateTransformation(result);
2054 }
2055
2056 /**
2057 * Sets current body orientation as a transformation from body to ECEF coordinates.
2058 * Notice that ECEF orientation refers to ECEF Earth axes, which means that
2059 * orientation is not relative to the ground or horizon at current body position.
2060 * Typically it is more convenient to use
2061 * {@link #setNedC(CoordinateTransformation)} to specify orientation relative to
2062 * the ground or horizon at current body position.
2063 * For instance, on Android devices a NED orientation with Euler angles (roll = 0,
2064 * pitch = 0, yaw = 0) means that the device is laying flat on a horizontal surface
2065 * with the screen facing down towards the ground.
2066 *
2067 * @param ecefC body orientation resolved on ECEF axes to be set.
2068 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2069 * transformation is not from
2070 * body to ECEF coordinates.
2071 * @throws LockedException if estimator is currently
2072 * running.
2073 */
2074 public void setEcefC(final CoordinateTransformation ecefC)
2075 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2076 if (running) {
2077 throw new LockedException();
2078 }
2079
2080 frame.setCoordinateTransformation(ecefC);
2081
2082 rebuildExpectedBodyMagneticFluxDensity();
2083 }
2084
2085 /**
2086 * Gets current body orientation as a transformation from body to NED coordinates.
2087 * Notice that returned orientation refers to current local position. This means
2088 * that two equal NED orientations will transform into different ECEF orientations
2089 * if the body is located at different positions.
2090 * As a reference, on Android devices a NED orientation with Euler angles
2091 * (roll = 0, pitch = 0, yaw = 0) means that the device is laying flat on a
2092 * horizontal surface with the screen facing down towards the ground.
2093 *
2094 * @return current body orientation resolved on NED axes.
2095 */
2096 public CoordinateTransformation getNedC() {
2097 return getNedFrame().getCoordinateTransformation();
2098 }
2099
2100 /**
2101 * Gets current body orientation as a transformation from body to NED coordinates.
2102 * Notice that returned orientation refers to current local position. This means
2103 * that two equal NED orientations will transform into different ECEF orientations
2104 * if the body is located at different positions.
2105 * As a reference, on Android devices a NED orientation with Euler angles
2106 * (roll = 0, pitch = 0, yaw = 0) means that the device is laying flat on a
2107 * horizontal surface with the screen facing down towards the ground.
2108 *
2109 * @param result instance where current body orientation resolved on NED axes
2110 * will be stored.
2111 */
2112 public void getNedC(final CoordinateTransformation result) {
2113 getNedFrame().getCoordinateTransformation(result);
2114 }
2115
2116 /**
2117 * Sets current body orientation as a transformation from body to NED coordinates.
2118 * Notice that provided orientation refers to current local position. This means
2119 * that two equal NED orientations will transform into different ECEF orientations
2120 * if the body is located at different positions.
2121 * As a reference, on Android devices a NED orientation with Euler angles
2122 * (roll = 0, pitch = 0, yaw = 0) means that the device is laying flat on a
2123 * horizontal surface with the screen facing down towards the ground.
2124 *
2125 * @param nedC orientation resolved on NED axes to be set.
2126 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2127 * transformation is not from
2128 * body to NED coordinates.
2129 * @throws LockedException if estimator is currently
2130 * running.
2131 */
2132 public void setNedC(final CoordinateTransformation nedC)
2133 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2134 if (running) {
2135 throw new LockedException();
2136 }
2137
2138 final var nedFrame = getNedFrame();
2139 nedFrame.setCoordinateTransformation(nedC);
2140 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2141
2142 rebuildExpectedBodyMagneticFluxDensity();
2143 }
2144
2145 /**
2146 * Sets position and orientation both expressed on NED coordinates.
2147 *
2148 * @param nedPosition position expressed on NED coordinates.
2149 * @param nedC body to NED coordinate transformation indicating
2150 * body orientation.
2151 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2152 * transformation is not from
2153 * body to NED coordinates.
2154 * @throws LockedException if estimator is currently
2155 * running.
2156 * @see #setNedPosition(NEDPosition)
2157 * @see #setNedC(CoordinateTransformation)
2158 */
2159 public void setNedPositionAndNedOrientation(
2160 final NEDPosition nedPosition, final CoordinateTransformation nedC)
2161 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2162 if (running) {
2163 throw new LockedException();
2164 }
2165
2166 final var nedFrame = getNedFrame();
2167 nedFrame.setPosition(nedPosition);
2168 nedFrame.setCoordinateTransformation(nedC);
2169 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2170
2171 rebuildExpectedBodyMagneticFluxDensity();
2172 }
2173
2174 /**
2175 * Sets position and orientation both expressed on NED coordinates.
2176 *
2177 * @param latitude latitude expressed in radians (rad).
2178 * @param longitude longitude expressed in radians (rad).
2179 * @param height height expressed in meters (m).
2180 * @param nedC body to NED coordinate transformation indicating
2181 * body orientation.
2182 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2183 * transformation is not from
2184 * body to NED coordinates.
2185 * @throws LockedException if estimator is currently
2186 * running.
2187 * @see #setNedPosition(double, double, double)
2188 * @see #setNedC(CoordinateTransformation)
2189 */
2190 public void setNedPositionAndNedOrientation(
2191 final double latitude, final double longitude, final double height, final CoordinateTransformation nedC)
2192 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2193 if (running) {
2194 throw new LockedException();
2195 }
2196
2197 final var nedFrame = getNedFrame();
2198 nedFrame.setPosition(latitude, longitude, height);
2199 nedFrame.setCoordinateTransformation(nedC);
2200 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2201
2202 rebuildExpectedBodyMagneticFluxDensity();
2203 }
2204
2205 /**
2206 * Sets position and orientation both expressed on NED coordinates.
2207 *
2208 * @param latitude latitude.
2209 * @param longitude longitude.
2210 * @param height height expressed in meters (m).
2211 * @param nedC body to NED coordinate transformation indicating
2212 * body orientation.
2213 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2214 * transformation is not from
2215 * body to NED coordinates.
2216 * @throws LockedException if estimator is currently
2217 * running.
2218 * @see #setNedPosition(Angle, Angle, double)
2219 * @see #setNedC(CoordinateTransformation)
2220 */
2221 public void setNedPositionAndNedOrientation(
2222 final Angle latitude, final Angle longitude, final double height, final CoordinateTransformation nedC)
2223 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2224 if (running) {
2225 throw new LockedException();
2226 }
2227
2228 final var nedFrame = getNedFrame();
2229 nedFrame.setPosition(latitude, longitude, height);
2230 nedFrame.setCoordinateTransformation(nedC);
2231 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2232
2233 rebuildExpectedBodyMagneticFluxDensity();
2234 }
2235
2236 /**
2237 * Sets position and orientation both expressed on NED coordinates.
2238 *
2239 * @param latitude latitude.
2240 * @param longitude longitude.
2241 * @param height height.
2242 * @param nedC body to NED coordinate transformation indicating
2243 * body orientation.
2244 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2245 * transformation is not from
2246 * body to NED coordinates.
2247 * @throws LockedException if estimator is currently
2248 * running.
2249 * @see #setNedPosition(Angle, Angle, Distance)
2250 * @see #setNedC(CoordinateTransformation)
2251 */
2252 public void setNedPositionAndNedOrientation(
2253 final Angle latitude, final Angle longitude, final Distance height, final CoordinateTransformation nedC)
2254 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2255 if (running) {
2256 throw new LockedException();
2257 }
2258
2259 final var nedFrame = getNedFrame();
2260 nedFrame.setPosition(latitude, longitude, height);
2261 nedFrame.setCoordinateTransformation(nedC);
2262 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2263
2264 rebuildExpectedBodyMagneticFluxDensity();
2265 }
2266
2267 /**
2268 * Sets position and orientation both expressed on ECEF coordinates.
2269 *
2270 * @param ecefPosition position expressed on ECEF coordinates.
2271 * @param ecefC body to ECEF coordinate transformation indicating body
2272 * orientation.
2273 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2274 * transformation is not from
2275 * body to ECEF coordinates.
2276 * @throws LockedException if estimator is currently
2277 * running.
2278 * @see #setEcefPosition(ECEFPosition)
2279 * @see #setEcefC(CoordinateTransformation)
2280 */
2281 public void setEcefPositionAndEcefOrientation(
2282 final ECEFPosition ecefPosition, final CoordinateTransformation ecefC)
2283 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2284 if (running) {
2285 throw new LockedException();
2286 }
2287
2288 frame.setPosition(ecefPosition);
2289 frame.setCoordinateTransformation(ecefC);
2290
2291 rebuildExpectedBodyMagneticFluxDensity();
2292 }
2293
2294 /**
2295 * Sets position and orientation both expressed on ECEF coordinates.
2296 *
2297 * @param x x coordinate of ECEF position expressed in meters (m).
2298 * @param y y coordinate of ECEF position expressed in meters (m).
2299 * @param z z coordinate of ECEF position expressed in meters (m).
2300 * @param ecefC body to ECEF coordinate transformation indicating body
2301 * orientation.
2302 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2303 * transformation is not from
2304 * body to ECEF coordinates.
2305 * @throws LockedException if estimator is currently
2306 * running.
2307 * @see #setEcefPosition(double, double, double)
2308 * @see #setEcefC(CoordinateTransformation)
2309 */
2310 public void setEcefPositionAndEcefOrientation(
2311 final double x, final double y, final double z, final CoordinateTransformation ecefC)
2312 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2313 if (running) {
2314 throw new LockedException();
2315 }
2316
2317 frame.setCoordinates(x, y, z);
2318 frame.setCoordinateTransformation(ecefC);
2319
2320 rebuildExpectedBodyMagneticFluxDensity();
2321 }
2322
2323 /**
2324 * Sets position and orientation both expressed on ECEF coordinates.
2325 *
2326 * @param x x coordinate of ECEF position.
2327 * @param y y coordinate of ECEF position.
2328 * @param z z coordinate of ECEF position.
2329 * @param ecefC body to ECEF coordinate transformation indicating body
2330 * orientation.
2331 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2332 * transformation is not from
2333 * body to ECEF coordinates.
2334 * @throws LockedException if estimator is currently
2335 * running.
2336 * @see #setEcefPosition(Distance, Distance, Distance)
2337 * @see #setEcefC(CoordinateTransformation)
2338 */
2339 public void setEcefPositionAndEcefOrientation(
2340 final Distance x, final Distance y, final Distance z, final CoordinateTransformation ecefC)
2341 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2342 if (running) {
2343 throw new LockedException();
2344 }
2345
2346 frame.setPositionCoordinates(x, y, z);
2347 frame.setCoordinateTransformation(ecefC);
2348
2349 rebuildExpectedBodyMagneticFluxDensity();
2350 }
2351
2352 /**
2353 * Sets position and orientation both expressed on ECEF coordinates.
2354 *
2355 * @param position position resolved around ECEF axes and expressed in meters (m).
2356 * @param ecefC body to ECEF coordinate transformation indicating body
2357 * orientation.
2358 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2359 * transformation is not from
2360 * body to ECEF coordinates.
2361 * @throws LockedException if estimator is currently
2362 * running.
2363 * @see #setEcefPosition(Point3D)
2364 * @see #setEcefC(CoordinateTransformation)
2365 */
2366 public void setEcefPositionAndEcefOrientation(
2367 final Point3D position, final CoordinateTransformation ecefC)
2368 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2369 if (running) {
2370 throw new LockedException();
2371 }
2372
2373 frame.setPosition(position);
2374 frame.setCoordinateTransformation(ecefC);
2375
2376 rebuildExpectedBodyMagneticFluxDensity();
2377 }
2378
2379 /**
2380 * Sets position expressed on NED coordinates and orientation respect to ECEF
2381 * axes.
2382 *
2383 * @param position position expressed on NED coordinates.
2384 * @param ecefC body to ECEF coordinate transformation indicating body
2385 * orientation.
2386 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2387 * transformation is not from
2388 * body to ECEF coordinates.
2389 * @throws LockedException if estimator is currently
2390 * running.
2391 * @see #setNedPosition(NEDPosition)
2392 * @see #setEcefC(CoordinateTransformation)
2393 */
2394 public void setNedPositionAndEcefOrientation(
2395 final NEDPosition position, final CoordinateTransformation ecefC)
2396 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2397 if (running) {
2398 throw new LockedException();
2399 }
2400
2401 final var nedFrame = getNedFrame();
2402 nedFrame.setPosition(position);
2403 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2404 frame.setCoordinateTransformation(ecefC);
2405
2406 rebuildExpectedBodyMagneticFluxDensity();
2407 }
2408
2409 /**
2410 * Sets position expressed on NED coordinates and orientation respect to ECEF
2411 * axes.
2412 *
2413 * @param latitude latitude expressed in radians (rad).
2414 * @param longitude longitude expressed in radians (rad).
2415 * @param height height expressed in meters (m).
2416 * @param ecefC body to ECEF coordinate transformation indicating body
2417 * orientation.
2418 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2419 * transformation is not from
2420 * body to ECEF coordinates.
2421 * @throws LockedException if estimator is currently
2422 * running.
2423 * @see #setNedPosition(double, double, double)
2424 * @see #setEcefC(CoordinateTransformation)
2425 */
2426 public void setNedPositionAndEcefOrientation(
2427 final double latitude, final double longitude, final double height, final CoordinateTransformation ecefC)
2428 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2429 if (running) {
2430 throw new LockedException();
2431 }
2432
2433 final var nedFrame = getNedFrame();
2434 nedFrame.setPosition(latitude, longitude, height);
2435 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2436 frame.setCoordinateTransformation(ecefC);
2437
2438 rebuildExpectedBodyMagneticFluxDensity();
2439 }
2440
2441 /**
2442 * Sets position expressed on NED coordinates and orientation respect to ECEF
2443 * axes.
2444 *
2445 * @param latitude latitude.
2446 * @param longitude longitude.
2447 * @param height height expressed in meters (m).
2448 * @param ecefC body to ECEF coordinate transformation indicating body
2449 * orientation.
2450 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2451 * transformation is not from
2452 * body to ECEF coordinates.
2453 * @throws LockedException if estimator is currently
2454 * running.
2455 * @see #setNedPosition(Angle, Angle, double)
2456 * @see #setEcefC(CoordinateTransformation)
2457 */
2458 public void setNedPositionAndEcefOrientation(
2459 final Angle latitude, final Angle longitude, final double height,
2460 final CoordinateTransformation ecefC)
2461 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2462 if (running) {
2463 throw new LockedException();
2464 }
2465
2466 final var nedFrame = getNedFrame();
2467 nedFrame.setPosition(latitude, longitude, height);
2468 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2469 frame.setCoordinateTransformation(ecefC);
2470
2471 rebuildExpectedBodyMagneticFluxDensity();
2472 }
2473
2474 /**
2475 * Sets position expressed on NED coordinates and orientation respect to ECEF
2476 * axes.
2477 *
2478 * @param latitude latitude.
2479 * @param longitude longitude.
2480 * @param height height.
2481 * @param ecefC body to ECEF coordinate transformation indicating body
2482 * orientation.
2483 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2484 * transformation is not from
2485 * body to ECEF coordinates.
2486 * @throws LockedException if estimator is currently
2487 * running.
2488 * @see #setNedPosition(Angle, Angle, Distance)
2489 * @see #setEcefC(CoordinateTransformation)
2490 */
2491 public void setNedPositionAndEcefOrientation(
2492 final Angle latitude, final Angle longitude, final Distance height,
2493 final CoordinateTransformation ecefC)
2494 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2495 if (running) {
2496 throw new LockedException();
2497 }
2498
2499 final var nedFrame = getNedFrame();
2500 nedFrame.setPosition(latitude, longitude, height);
2501 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2502 frame.setCoordinateTransformation(ecefC);
2503
2504 rebuildExpectedBodyMagneticFluxDensity();
2505 }
2506
2507 /**
2508 * Sets position expressed on ECEF coordinates and orientation respect to
2509 * NED axes.
2510 * In order to preserve provided orientation, first position is set and
2511 * then orientation is applied.
2512 *
2513 * @param ecefPosition position expressed on ECEF coordinates.
2514 * @param nedC body to NED coordinate transformation indicating body
2515 * orientation.
2516 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2517 * transformation is not from
2518 * body to NED coordinates.
2519 * @throws LockedException if estimator is currently
2520 * running.
2521 * @see #setEcefPosition(ECEFPosition)
2522 * @see #setNedC(CoordinateTransformation)
2523 */
2524 public void setEcefPositionAndNedOrientation(
2525 final ECEFPosition ecefPosition, final CoordinateTransformation nedC)
2526 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2527 if (running) {
2528 throw new LockedException();
2529 }
2530
2531 frame.setPosition(ecefPosition);
2532
2533 final var nedFrame = getNedFrame();
2534 nedFrame.setCoordinateTransformation(nedC);
2535 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2536
2537 rebuildExpectedBodyMagneticFluxDensity();
2538 }
2539
2540 /**
2541 * Sets position expressed on ECEF coordinates and orientation respect to
2542 * NED axes.
2543 * In order to preserve provided orientation, first position is set and
2544 * then orientation is applied.
2545 *
2546 * @param x x coordinate of ECEF position expressed in meters (m).
2547 * @param y y coordinate of ECEF position expressed in meters (m).
2548 * @param z z coordinate of ECEF position expressed in meters (m).
2549 * @param nedC body to NED coordinate transformation indicating body
2550 * orientation.
2551 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2552 * transformation is not from
2553 * body to NED coordinates.
2554 * @throws LockedException if estimator is currently
2555 * running.
2556 * @see #setEcefPosition(double, double, double)
2557 * @see #setNedC(CoordinateTransformation)
2558 */
2559 public void setEcefPositionAndNedOrientation(
2560 final double x, final double y, final double z,
2561 final CoordinateTransformation nedC) throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2562 if (running) {
2563 throw new LockedException();
2564 }
2565
2566 frame.setCoordinates(x, y, z);
2567
2568 final var nedFrame = getNedFrame();
2569 nedFrame.setCoordinateTransformation(nedC);
2570 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2571
2572 rebuildExpectedBodyMagneticFluxDensity();
2573 }
2574
2575 /**
2576 * Sets position expressed on ECEF coordinates and orientation respect to
2577 * NED axes.
2578 * In order to preserve provided orientation, first position is set and
2579 * then orientation is applied.
2580 *
2581 * @param x x coordinate of ECEF position.
2582 * @param y y coordinate of ECEF position.
2583 * @param z z coordinate of ECEF position.
2584 * @param nedC body to NED coordinate transformation indicating body
2585 * orientation.
2586 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2587 * transformation is not from
2588 * body to NED coordinates.
2589 * @throws LockedException if estimator is currently
2590 * running.
2591 * @see #setEcefPosition(Distance, Distance, Distance)
2592 * @see #setNedC(CoordinateTransformation)
2593 */
2594 public void setEcefPositionAndNedOrientation(
2595 final Distance x, final Distance y, final Distance z,
2596 final CoordinateTransformation nedC) throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2597 if (running) {
2598 throw new LockedException();
2599 }
2600
2601 frame.setPositionCoordinates(x, y, z);
2602
2603 final var nedFrame = getNedFrame();
2604 nedFrame.setCoordinateTransformation(nedC);
2605 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2606
2607 rebuildExpectedBodyMagneticFluxDensity();
2608 }
2609
2610 /**
2611 * Sets position expressed on ECEF coordinates and orientation respect to
2612 * NED axes.
2613 * In order to preserve provided orientation, first position is set and
2614 * then orientation is applied.
2615 *
2616 * @param position position resolved around ECEF axes and expressed in meters (m).
2617 * @param nedC body to NED coordinate transformation indicating body
2618 * orientation.
2619 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
2620 * transformation is not from
2621 * body to NED coordinates.
2622 * @throws LockedException if estimator is currently
2623 * running.
2624 * @see #setEcefPosition(Point3D)
2625 * @see #setNedC(CoordinateTransformation)
2626 */
2627 public void setEcefPositionAndNedOrientation(
2628 final Point3D position, final CoordinateTransformation nedC)
2629 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
2630 if (running) {
2631 throw new LockedException();
2632 }
2633
2634 frame.setPosition(position);
2635
2636 final var nedFrame = getNedFrame();
2637 nedFrame.setCoordinateTransformation(nedC);
2638 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
2639
2640 rebuildExpectedBodyMagneticFluxDensity();
2641 }
2642
2643 /**
2644 * Gets year expressed in decimal format.
2645 *
2646 * @return year expressed in decimal format.
2647 */
2648 public double getYear() {
2649 return year;
2650 }
2651
2652 /**
2653 * Sets year expressed in decimal format.
2654 *
2655 * @param year year expressed in decimal format.
2656 * @throws LockedException if estimator is running.
2657 */
2658 public void setYear(final double year) throws LockedException {
2659 if (running) {
2660 throw new LockedException();
2661 }
2662
2663 this.year = year;
2664
2665 rebuildExpectedBodyMagneticFluxDensity();
2666 }
2667
2668 /**
2669 * Sets decimal year from provided date instance.
2670 *
2671 * @param date a date instance containing a timestamp.
2672 * @throws LockedException if estimator is running.
2673 */
2674 public void setTime(final Date date) throws LockedException {
2675 if (running) {
2676 throw new LockedException();
2677 }
2678
2679 year = convertTime(date);
2680
2681 rebuildExpectedBodyMagneticFluxDensity();
2682 }
2683
2684 /**
2685 * Sets decimal year from provided calendar instance.
2686 *
2687 * @param calendar a calendar instance containing a timestamp.
2688 * @throws LockedException if estimator is running.
2689 */
2690 public void setTime(final GregorianCalendar calendar) throws LockedException {
2691 if (running) {
2692 throw new LockedException();
2693 }
2694
2695 year = convertTime(calendar);
2696
2697 rebuildExpectedBodyMagneticFluxDensity();
2698 }
2699
2700 /**
2701 * Gets Earth's magnetic model.
2702 *
2703 * @return Earth's magnetic model or null if not provided.
2704 */
2705 public WorldMagneticModel getMagneticModel() {
2706 return magneticModel;
2707 }
2708
2709 /**
2710 * Sets Earth's magnetic model.
2711 * If not provided a default model will be loaded internally.
2712 *
2713 * @param magneticModel Earth's magnetic model to be set.
2714 * @throws LockedException if calibrator is currently running.
2715 * @throws IOException if initialization of world magnetic model fails.
2716 */
2717 public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException, IOException {
2718 if (running) {
2719 throw new LockedException();
2720 }
2721 this.magneticModel = magneticModel;
2722 initialize();
2723 }
2724
2725 /**
2726 * Gets listener to handle events raised by this estimator.
2727 *
2728 * @return listener to handle events raised by this estimator.
2729 */
2730 public BodyMagneticFluxDensityBiasEstimatorListener getListener() {
2731 return listener;
2732 }
2733
2734 /**
2735 * Sets listener to handle events raised by this estimator.
2736 *
2737 * @param listener listener to handle events raised by this estimator.
2738 * @throws LockedException if this estimator is running.
2739 */
2740 public void setListener(
2741 final BodyMagneticFluxDensityBiasEstimatorListener listener) throws LockedException {
2742 if (running) {
2743 throw new LockedException();
2744 }
2745
2746 this.listener = listener;
2747 }
2748
2749 /**
2750 * Gets last provided body magnetic flux density values or null if not
2751 * available.
2752 *
2753 * @return last provided body magnetic flux density values or null.
2754 */
2755 public BodyMagneticFluxDensity getLastBodyMagneticFluxDensity() {
2756 return lastBodyMagneticFluxDensity != null ? new BodyMagneticFluxDensity(lastBodyMagneticFluxDensity) : null;
2757 }
2758
2759 /**
2760 * Gets last provided body magnetic flux density values.
2761 *
2762 * @param result instance where last provided body magnetic flux density will
2763 * be stored.
2764 * @return true if result instance was updated, false otherwise.
2765 */
2766 public boolean getLastBodyMagneticFluxDensity(final BodyMagneticFluxDensity result) {
2767 if (lastBodyMagneticFluxDensity != null) {
2768 lastBodyMagneticFluxDensity.copyTo(result);
2769 return true;
2770 } else {
2771 return false;
2772 }
2773 }
2774
2775 /**
2776 * Gets estimated bias of x coordinate of body magnetic flux density
2777 * expressed in Teslas (T). Notice that bias is equivalent to hard iron
2778 * component on a magnetometer calibrator.
2779 *
2780 * @return bias of x coordinate of body magnetic flux density.
2781 */
2782 public double getBiasX() {
2783 return biasX;
2784 }
2785
2786 /**
2787 * Gets estimated bias of x coordinate of body magnetic flux density.
2788 * Notice that bias is equivalent to hard iron component on a magnetometer
2789 * calibrator.
2790 *
2791 * @return bias of x coordinate of body magnetic flux density.
2792 */
2793 public MagneticFluxDensity getBiasXAsMagneticFluxDensity() {
2794 return new MagneticFluxDensity(biasX, MagneticFluxDensityUnit.TESLA);
2795 }
2796
2797 /**
2798 * Gets estimated bias of x coordinate of body magnetic flux density.
2799 * Notice that bias is equivalent to hard iron component on a magnetometer
2800 * calibrator.
2801 *
2802 * @param result instance where bias of x coordinate of body magnetic flux
2803 * density will be stored.
2804 */
2805 public void getBiasXAsMagneticFluxDensity(final MagneticFluxDensity result) {
2806 result.setValue(biasX);
2807 result.setUnit(MagneticFluxDensityUnit.TESLA);
2808 }
2809
2810 /**
2811 * Gets estimated bias of y coordinate of body magnetic flux density
2812 * expressed in Teslas (T). Notice that bias is equivalent to hard iron
2813 * component on a magnetometer calibrator.
2814 *
2815 * @return bias of y coordinate of body magnetic flux density.
2816 */
2817 public double getBiasY() {
2818 return biasY;
2819 }
2820
2821 /**
2822 * Gets estimated bias of y coordinate of body magnetic flux density.
2823 * Notice that bias is equivalent to hard iron component on a
2824 * magnetometer calibrator.
2825 *
2826 * @return bias of y coordinate of body magnetic flux density.
2827 */
2828 public MagneticFluxDensity getBiasYAsMagneticFluxDensity() {
2829 return new MagneticFluxDensity(biasY, MagneticFluxDensityUnit.TESLA);
2830 }
2831
2832 /**
2833 * Gets estimated bias of y coordinate of body magnetic flux density.
2834 * Notice that bias is equivalent to hard iron component on a
2835 * magnetometer calibrator.
2836 *
2837 * @param result instance where bias of y coordinate of body magnetic flux
2838 * density will be stored.
2839 */
2840 public void getBiasYAsMagneticFluxDensity(final MagneticFluxDensity result) {
2841 result.setValue(biasY);
2842 result.setUnit(MagneticFluxDensityUnit.TESLA);
2843 }
2844
2845 /**
2846 * Gets estimated bias of z coordinate of body magnetic flux density
2847 * expressed in Teslas (T). Notice that bias is equivalent to hard iron
2848 * component on a magnetometer calibrator.
2849 *
2850 * @return bias of z coordinate of body magnetic flux density.
2851 */
2852 public double getBiasZ() {
2853 return biasZ;
2854 }
2855
2856 /**
2857 * Gets estimated bias of z coordinate of body magnetic flux density.
2858 * Notice that bias is equivalent to hard iron component on a magnetometer
2859 * calibrator.
2860 *
2861 * @return bias of z coordinate of body magnetic flux density.
2862 */
2863 public MagneticFluxDensity getBiasZAsMagneticFluxDensity() {
2864 return new MagneticFluxDensity(biasZ, MagneticFluxDensityUnit.TESLA);
2865 }
2866
2867 /**
2868 * Gets estimated bias of z coordinate of body magnetic flux density.
2869 * Notice that bias is equivalent to hard iron component on a magnetometer
2870 * calibrator.
2871 *
2872 * @param result instance where bias of z coordinate of body magnetic flux
2873 * density will be stored.
2874 */
2875 public void getBiasZAsMagneticFluxDensity(final MagneticFluxDensity result) {
2876 result.setValue(biasZ);
2877 result.setUnit(MagneticFluxDensityUnit.TESLA);
2878 }
2879
2880 /**
2881 * Gets estimated bias of body magnetic flux density.
2882 *
2883 * @return estimated bias of magnetic flux density.
2884 */
2885 public MagneticFluxDensityTriad getBiasTriad() {
2886 return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, biasX, biasY, biasZ);
2887 }
2888
2889 /**
2890 * Gets estimated bias of body magnetic flux density.
2891 *
2892 * @param result instance where bias of body magnetic flux density will
2893 * be stored.
2894 */
2895 public void getBiasTriad(final MagneticFluxDensityTriad result) {
2896 result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, MagneticFluxDensityUnit.TESLA);
2897 }
2898
2899 /**
2900 * Gets estimated variance of x coordinate of body magnetic flux density
2901 * expressed in squared Teslas (T^2).
2902 *
2903 * @return estimated variance of x coordinate of body magnetic flux density.
2904 */
2905 public double getVarianceX() {
2906 return varianceX;
2907 }
2908
2909 /**
2910 * Gets estimated variance of y coordinate of body magnetic flux density
2911 * expressed in squared Teslas (T^2).
2912 *
2913 * @return estimated variance of y coordinate of body magnetic flux density.
2914 */
2915 public double getVarianceY() {
2916 return varianceY;
2917 }
2918
2919 /**
2920 * Gets estimated variance of z coordinate of body magnetic flux density
2921 * expressed in squared Teslas (T^2).
2922 *
2923 * @return estimated variance of z coordinate of body magnetic flux density.
2924 */
2925 public double getVarianceZ() {
2926 return varianceZ;
2927 }
2928
2929 /**
2930 * Gets estimated standard deviation of x coordinate of body magnetic flux
2931 * density expressed in Teslas (T).
2932 *
2933 * @return estimated standard deviation of x coordinate of body magnetic
2934 * flux density.
2935 */
2936 public double getStandardDeviationX() {
2937 return Math.sqrt(varianceX);
2938 }
2939
2940 /**
2941 * Gets estimated standard deviation of x coordinate of body magnetic flux
2942 * density.
2943 *
2944 * @return estimated standard deviation of x coordinate of body magnetic
2945 * flux density.
2946 */
2947 public MagneticFluxDensity getStandardDeviationXAsMagneticFluxDensity() {
2948 return new MagneticFluxDensity(getStandardDeviationX(), MagneticFluxDensityUnit.TESLA);
2949 }
2950
2951 /**
2952 * Gets estimated standard deviation of x coordinate of body magnetic flux
2953 * density.
2954 *
2955 * @param result instance where estimated standard deviation of x coordinate
2956 * of body magnetic flux density will be stored.
2957 */
2958 public void getStandardDeviationXAsMagneticFluxDensity(final MagneticFluxDensity result) {
2959 result.setValue(getStandardDeviationX());
2960 result.setUnit(MagneticFluxDensityUnit.TESLA);
2961 }
2962
2963 /**
2964 * Gets estimated standard deviation of y coordinate of body magnetic flux
2965 * density expressed in Teslas (T).
2966 *
2967 * @return estimated standard deviation of y coordinate of body magnetic
2968 * flux density.
2969 */
2970 public double getStandardDeviationY() {
2971 return Math.sqrt(varianceY);
2972 }
2973
2974 /**
2975 * Gets estimated standard deviation of y coordinate of body magnetic flux
2976 * density.
2977 *
2978 * @return estimated standard deviation of y coordinate of body magnetic
2979 * flux density.
2980 */
2981 public MagneticFluxDensity getStandardDeviationYAsMagneticFluxDensity() {
2982 return new MagneticFluxDensity(getStandardDeviationY(), MagneticFluxDensityUnit.TESLA);
2983 }
2984
2985 /**
2986 * Gets estimated standard deviation of y coordinate of body magnetic flux
2987 * density.
2988 *
2989 * @param result instance where estimated standard deviation of y coordinate
2990 * of body magnetic flux density will be stored.
2991 */
2992 public void getStandardDeviationYAsMagneticFluxDensity(final MagneticFluxDensity result) {
2993 result.setValue(getStandardDeviationY());
2994 result.setUnit(MagneticFluxDensityUnit.TESLA);
2995 }
2996
2997 /**
2998 * Gets estimated standard deviation of z coordinate of body magnetic flux
2999 * density expressed in Teslas (T).
3000 *
3001 * @return estimated standard deviation of z coordinate of body magnetic
3002 * flux density.
3003 */
3004 public double getStandardDeviationZ() {
3005 return Math.sqrt(varianceZ);
3006 }
3007
3008 /**
3009 * Gets estimated standard deviation of z coordinate of body magnetic flux
3010 * density.
3011 *
3012 * @return estimated standard deviation of z coordinate of body magnetic
3013 * flux density.
3014 */
3015 public MagneticFluxDensity getStandardDeviationZAsMagneticFluxDensity() {
3016 return new MagneticFluxDensity(getStandardDeviationZ(), MagneticFluxDensityUnit.TESLA);
3017 }
3018
3019 /**
3020 * Gets estimated standard deviation of z coordinate of body magnetic flux
3021 * density.
3022 *
3023 * @param result instance where estimated standard deviation of z coordinate
3024 * of body magnetic flux density will be stored.
3025 */
3026 public void getStandardDeviationZAsMagneticFluxDensity(final MagneticFluxDensity result) {
3027 result.setValue(getStandardDeviationZ());
3028 result.setUnit(MagneticFluxDensityUnit.TESLA);
3029 }
3030
3031 /**
3032 * Gets estimated standard deviation of body magnetic flux density.
3033 *
3034 * @return estimated standard deviation of body magnetic flux density.
3035 */
3036 public MagneticFluxDensityTriad getStandardDeviationTriad() {
3037 return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
3038 getStandardDeviationX(), getStandardDeviationY(), getStandardDeviationZ());
3039 }
3040
3041 /**
3042 * Gets estimated standard deviation of body magnetic flux density.
3043 *
3044 * @param result instance where estimated standard deviation of body magnetic
3045 * flux density will be stored.
3046 */
3047 public void getStandardDeviationTriad(final MagneticFluxDensityTriad result) {
3048 result.setValueCoordinatesAndUnit(getStandardDeviationX(), getStandardDeviationY(), getStandardDeviationZ(),
3049 MagneticFluxDensityUnit.TESLA);
3050 }
3051
3052 /**
3053 * Gets average of estimated standard deviation of body magnetic flux density
3054 * expressed in Teslas (T).
3055 *
3056 * @return average of estimated standard deviation of body magnetic flux
3057 * density.
3058 */
3059 public double getAverageStandardDeviation() {
3060 return (getStandardDeviationX() + getStandardDeviationY() + getStandardDeviationZ()) / 3.0;
3061 }
3062
3063 /**
3064 * Gets average of estimated standard deviation of body magnetic flux density.
3065 *
3066 * @return average of estimated standard deviation of body magnetic flux
3067 * density.
3068 */
3069 public MagneticFluxDensity getAverageStandardDeviationAsMagneticFluxDensity() {
3070 return new MagneticFluxDensity(getAverageStandardDeviation(), MagneticFluxDensityUnit.TESLA);
3071 }
3072
3073 /**
3074 * Gets average of estimated standard deviation of body magnetic flux density.
3075 *
3076 * @param result instance where average of estimated standard deviation of
3077 * body magnetic flux density will be stored.
3078 */
3079 public void getAverageStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
3080 result.setValue(getAverageStandardDeviation());
3081 result.setUnit(MagneticFluxDensityUnit.TESLA);
3082 }
3083
3084 /**
3085 * Gets magnetometer noise PSD (Power Spectral Density) on x axis expressed
3086 * in (T^2 * s).
3087 *
3088 * @return magnetometer noise PSD on x axis.
3089 */
3090 public double getPsdX() {
3091 return varianceX * timeInterval;
3092 }
3093
3094 /**
3095 * Gets magnetometer noise PSD (Power Spectral Density) on y axis expressed
3096 * in (T^2 * s).
3097 *
3098 * @return magnetometer noise PSD on y axis.
3099 */
3100 public double getPsdY() {
3101 return varianceY * timeInterval;
3102 }
3103
3104 /**
3105 * Gets magnetometer noise PSD (Power Spectral Density) on z axis expressed
3106 * in (T^2 * s).
3107 *
3108 * @return magnetometer noise PSD on z axis.
3109 */
3110 public double getPsdZ() {
3111 return varianceZ * timeInterval;
3112 }
3113
3114 /**
3115 * Gets magnetometer noise root PSD (Power Spectral Density) on x axis
3116 * expressed in (T * s^0.5).
3117 *
3118 * @return magnetometer noise root PSD on x axis.
3119 */
3120 public double getRootPsdX() {
3121 return Math.sqrt(getPsdX());
3122 }
3123
3124 /**
3125 * Gets magnetometer noise root PSD (Power Spectral Density) on y axis
3126 * expressed in (T * s^0.5).
3127 *
3128 * @return magnetometer noise root PSD on y axis.
3129 */
3130 public double getRootPsdY() {
3131 return Math.sqrt(getPsdY());
3132 }
3133
3134 /**
3135 * Gets magnetometer noise root PSD (Power Spectral Density) on z axis
3136 * expressed in (T * s^0.5).
3137 *
3138 * @return magnetometer noise root PSD on z axis.
3139 */
3140 public double getRootPsdZ() {
3141 return Math.sqrt(getPsdZ());
3142 }
3143
3144 /**
3145 * Gets average magnetometer noise PSD (Power Spectral Density) among
3146 * x,y,z components expressed as (T^2 * s).
3147 *
3148 * @return average magnetometer noise PSD.
3149 */
3150 public double getAvgPsd() {
3151 return (getPsdX() + getPsdY() + getPsdZ()) / 3.0;
3152 }
3153
3154 /**
3155 * Gets magnetometer root noise root PSD (Power Spectral Density) which is
3156 * the norm of root PSD components expressed as (T * s^0.5).
3157 *
3158 * @return average magnetometer noise root PSD.
3159 */
3160 public double getRootPsd() {
3161 return Math.sqrt(getPsdX() + getPsdY() + getPsdZ());
3162 }
3163
3164 /**
3165 * Gets number of samples that have been processed so far.
3166 *
3167 * @return number of samples that have been processed so far.
3168 */
3169 public int getNumberOfProcessedSamples() {
3170 return numberOfProcessedSamples;
3171 }
3172
3173 /**
3174 * Indicates whether estimator is currently running or not.
3175 *
3176 * @return true if estimator is running, false otherwise.
3177 */
3178 public boolean isRunning() {
3179 return running;
3180 }
3181
3182 /**
3183 * Gets theoretically expected body magnetic flux density for provided instant,
3184 * body position and orientation, assuming that body remains at the same
3185 * position (zero velocity).
3186 * When body remains static, sensed magnetic flux density will remain constant
3187 * for a few minutes respect to provided time instant.
3188 *
3189 * @return expected body magnetic flux density.
3190 */
3191 public BodyMagneticFluxDensity getExpectedBodyMagneticFluxDensity() {
3192 return new BodyMagneticFluxDensity(expectedBodyMagneticFluxDensity);
3193 }
3194
3195 /**
3196 * Gets theoretically expected body magnetic flux density for provided instant,
3197 * body position and orientation, assuming that body remains at the same
3198 * position (zero velocity).
3199 * When body remains static, sensed magnetic flux density will remain constant
3200 * for a few minutes respect to provided time instant.
3201 *
3202 * @param result instance where expected body magnetic flux density will be
3203 * stored.
3204 */
3205 public void getExpectedBodyMagneticFluxDensity(final BodyMagneticFluxDensity result) {
3206 expectedBodyMagneticFluxDensity.copyTo(result);
3207 }
3208
3209 /**
3210 * Adds a sample of body magnetic flux density. If estimator is already
3211 *
3212 * @param bodyMagneticFluxDensity body magnetic flux density to be added
3213 * and processed.
3214 * @throws LockedException if estimator is currently running.
3215 */
3216 public void addBodyMagneticFluxDensity(final BodyMagneticFluxDensity bodyMagneticFluxDensity)
3217 throws LockedException {
3218
3219 if (running) {
3220 throw new LockedException();
3221 }
3222
3223 running = true;
3224
3225 if (lastBodyMagneticFluxDensity == null && listener != null) {
3226 listener.onStart(this);
3227 }
3228
3229 final var bx = bodyMagneticFluxDensity.getBx();
3230 final var by = bodyMagneticFluxDensity.getBy();
3231 final var bz = bodyMagneticFluxDensity.getBz();
3232
3233 final var expectedBx = expectedBodyMagneticFluxDensity.getBx();
3234 final var expectedBy = expectedBodyMagneticFluxDensity.getBy();
3235 final var expectedBz = expectedBodyMagneticFluxDensity.getBz();
3236
3237 final var diffBx = bx - expectedBx;
3238 final var diffBy = by - expectedBy;
3239 final var diffBz = bz - expectedBz;
3240
3241 // compute biases
3242 final var tmp = (double) numberOfProcessedSamples / (double) numberOfProcessedSamplesPlusOne;
3243 biasX = biasX * tmp + diffBx / numberOfProcessedSamplesPlusOne;
3244 biasY = biasY * tmp + diffBy / numberOfProcessedSamplesPlusOne;
3245 biasZ = biasZ * tmp + diffBz / numberOfProcessedSamplesPlusOne;
3246
3247 // compute variances
3248 final var diffBiasX = diffBx - biasX;
3249 final var diffBiasY = diffBy - biasY;
3250 final var diffBiasZ = diffBz - biasZ;
3251
3252 final var diffBiasX2 = diffBiasX * diffBiasX;
3253 final var diffBiasY2 = diffBiasY * diffBiasY;
3254 final var diffBiasZ2 = diffBiasZ * diffBiasZ;
3255
3256 varianceX = varianceX * tmp + diffBiasX2 / numberOfProcessedSamplesPlusOne;
3257 varianceY = varianceY * tmp + diffBiasY2 / numberOfProcessedSamplesPlusOne;
3258 varianceZ = varianceZ * tmp + diffBiasZ2 / numberOfProcessedSamplesPlusOne;
3259
3260 lastBodyMagneticFluxDensity = bodyMagneticFluxDensity;
3261
3262 numberOfProcessedSamples++;
3263 numberOfProcessedSamplesPlusOne++;
3264
3265 if (listener != null) {
3266 listener.onBodyMagneticFluxDensityAdded(this);
3267 }
3268
3269 running = false;
3270 }
3271
3272 /**
3273 * Resets current estimator.
3274 *
3275 * @return true if estimator was successfully reset, false if no reset
3276 * was needed.
3277 * @throws LockedException if estimator is currently running.
3278 */
3279 public boolean reset() throws LockedException {
3280 if (running) {
3281 throw new LockedException();
3282 }
3283
3284 if (numberOfProcessedSamples == 0) {
3285 return false;
3286 }
3287
3288 running = true;
3289 lastBodyMagneticFluxDensity = null;
3290 biasX = 0.0;
3291 biasY = 0.0;
3292 biasZ = 0.0;
3293 varianceX = 0.0;
3294 varianceY = 0.0;
3295 varianceZ = 0.0;
3296 numberOfProcessedSamples = 0;
3297 numberOfProcessedSamplesPlusOne = 1;
3298
3299 if (listener != null) {
3300 listener.onReset(this);
3301 }
3302
3303 running = false;
3304
3305 return true;
3306 }
3307
3308 /**
3309 * Converts a time instant contained ina date object to a
3310 * decimal year.
3311 *
3312 * @param date a time instance to be converted.
3313 * @return converted value expressed in decimal years.
3314 */
3315 public static double convertTime(final Date date) {
3316 final var calendar = new GregorianCalendar();
3317 calendar.setTime(date);
3318 return convertTime(calendar);
3319 }
3320
3321 /**
3322 * Converts a time instant contained in a gregorian calendar to a
3323 * decimal year.
3324 *
3325 * @param calendar calendar containing a specific instant to be
3326 * converted.
3327 * @return converted value expressed in decimal years.
3328 */
3329 public static double convertTime(final GregorianCalendar calendar) {
3330 return WMMEarthMagneticFluxDensityEstimator.convertTime(calendar);
3331 }
3332
3333 /**
3334 * Converts provided time instance to seconds.
3335 *
3336 * @param time instance to be converted.
3337 * @return obtained conversion in seconds.
3338 */
3339 private static double convertTime(final Time time) {
3340 return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
3341 }
3342
3343 /**
3344 * Initializes world magnetic model and estimates expected body magnetic flux
3345 * density.
3346 *
3347 * @throws IOException if world magnetic model loading fails.
3348 */
3349 private void initialize() throws IOException {
3350 if (magneticModel != null) {
3351 wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
3352 } else {
3353 wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
3354 }
3355
3356 rebuildExpectedBodyMagneticFluxDensity();
3357 }
3358
3359 /**
3360 * Rebuilds expected body magnetic flux density based on current instant,
3361 * location and body orientation.
3362 */
3363 private void rebuildExpectedBodyMagneticFluxDensity() {
3364 final var nedFrame = ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(frame);
3365
3366 final var latitude = nedFrame.getLatitude();
3367 final var longitude = nedFrame.getLongitude();
3368 final var height = nedFrame.getHeight();
3369
3370 final var cbn = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
3371 final var cnb = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
3372 nedFrame.getCoordinateTransformation(cbn);
3373 cbn.inverse(cnb);
3374
3375 final var earthB = wmmEstimator.estimate(latitude, longitude, height, year);
3376
3377 // estimate expected body magnetic flux density taking into
3378 // account body attitude (inverse of frame orientation) and
3379 // estimated Earth magnetic flux density
3380 if (expectedBodyMagneticFluxDensity == null) {
3381 expectedBodyMagneticFluxDensity = new BodyMagneticFluxDensity();
3382 }
3383 BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedBodyMagneticFluxDensity);
3384 }
3385 }