1 /*
2 * Copyright (C) 2021 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;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.geometry.Point3D;
21 import com.irurueta.navigation.LockedException;
22 import com.irurueta.navigation.NotReadyException;
23 import com.irurueta.navigation.frames.CoordinateTransformation;
24 import com.irurueta.navigation.frames.ECEFFrame;
25 import com.irurueta.navigation.frames.ECEFPosition;
26 import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
27 import com.irurueta.navigation.frames.NEDFrame;
28 import com.irurueta.navigation.frames.NEDPosition;
29 import com.irurueta.navigation.inertial.BodyKinematics;
30 import com.irurueta.navigation.inertial.INSLooselyCoupledKalmanConfig;
31 import com.irurueta.navigation.inertial.calibration.bias.BodyKinematicsBiasEstimator;
32 import com.irurueta.units.Acceleration;
33 import com.irurueta.units.Angle;
34 import com.irurueta.units.AngleUnit;
35 import com.irurueta.units.AngularSpeed;
36 import com.irurueta.units.Distance;
37 import com.irurueta.units.DistanceUnit;
38 import com.irurueta.units.Speed;
39 import com.irurueta.units.SpeedUnit;
40 import com.irurueta.units.Time;
41 import com.irurueta.units.TimeUnit;
42
43 /**
44 * Estimates random walk data by running this estimator while the body of IMU remains static
45 * at a given position and orientation when IMU has already been calibrated.
46 * Internally, this estimator accumulates samples for a given "drift period" to
47 * estimate accumulated drift values of position, velocity and attitude, and then
48 * repeats the process several times to estimate the mean and variance of such values.
49 * The duration of the drift period is application-dependent. It should be set
50 * to a value more or less similar to the amount of time the device won't be able
51 * to set a position by some other system (e.g., GPS, Wi-Fi, cameras, etc...)
52 */
53 public class RandomWalkEstimator implements AccelerometerBiasRandomWalkSource,
54 GyroscopeBiasRandomWalkSource, PositionUncertaintySource,
55 VelocityUncertaintySource, AttitudeUncertaintySource,
56 PositionNoiseStandardDeviationSource, VelocityNoiseStandardDeviationSource {
57
58 /**
59 * Number of samples to be used by default on each drift period.
60 */
61 public static final int DEFAULT_DRIFT_PERIOD_SAMPLES = 150;
62
63 /**
64 * Listener to handle events raised by this estimator.
65 */
66 private RandomWalkEstimatorListener listener;
67
68 /**
69 * Fixes body kinematics measurements using accelerometer and gyroscope
70 * calibration data to fix measurements.
71 */
72 private final BodyKinematicsFixer fixer = new BodyKinematicsFixer();
73
74 /**
75 * Estimates bias for fixed body kinematics measurements to determine further
76 * bias variations while the IMU body remains static.
77 */
78 private final BodyKinematicsBiasEstimator biasEstimator = new BodyKinematicsBiasEstimator();
79
80 /**
81 * Estimates amount of position, velocity and orientation drift.
82 */
83 private final DriftEstimator driftEstimator = new DriftEstimator();
84
85 /**
86 * Instance containing the last fixed body kinematics to be reused.
87 */
88 private final BodyKinematics fixedKinematics = new BodyKinematics();
89
90 /**
91 * Indicates whether measured kinematics must be fixed or not.
92 * When enabled, provided calibration data is used; otherwise it is
93 * ignored.
94 * By default, this is enabled.
95 */
96 private boolean fixKinematics = true;
97
98 /**
99 * Number of samples to be used on each drift period.
100 */
101 private int driftPeriodSamples = DEFAULT_DRIFT_PERIOD_SAMPLES;
102
103 /**
104 * Indicates whether this estimator is running or not.
105 */
106 private boolean running;
107
108 /**
109 * Number of drift periods that have been processed.
110 */
111 private int numberOfProcessedDriftPeriods;
112
113 /**
114 * Accelerometer bias random walk PSD (Power Spectral Density) expressed
115 * in (m^2 * s^-5).
116 */
117 private double accelerometerBiasPSD;
118
119 /**
120 * Gyro bias random walk PSD (Power Spectral Density) expressed in (rad^2 * s^-3).
121 */
122 private double gyroBiasPSD;
123
124 /**
125 * Average position drift expressed in meters (m).
126 * This gives a sign of position accuracy.
127 */
128 private double avgPositionDrift;
129
130 /**
131 * Average velocity drift expressed in meters per second (m/s).
132 */
133 private double avgVelocityDrift;
134
135 /**
136 * Average attitude drift expressed in radians (rad).
137 */
138 private double avgAttitudeDrift;
139
140 /**
141 * Position variance expressed in square meters (m^2).
142 */
143 private double varPositionDrift;
144
145 /**
146 * Velocity variance expressed in (m^2/s^2).
147 */
148 private double varVelocityDrift;
149
150 /**
151 * Attitude variance expressed in squared radians (rad^2).
152 */
153 private double varAttitudeDrift;
154
155 /**
156 * Contains acceleration triad to be reused for bias norm estimation.
157 * This is reused for efficiency.
158 */
159 private final AccelerationTriad accelerationTriad = new AccelerationTriad();
160
161 /**
162 * Contains angular speed triad to be reused for bias norm estimation.
163 * This is reused for efficiency.
164 */
165 private final AngularSpeedTriad angularSpeedTriad = new AngularSpeedTriad();
166
167 /**
168 * Constructor.
169 */
170 public RandomWalkEstimator() {
171 }
172
173 /**
174 * Constructor.
175 *
176 * @param listener listener to handle events.
177 */
178 public RandomWalkEstimator(final RandomWalkEstimatorListener listener) {
179 this.listener = listener;
180 }
181
182 /**
183 * Constructor.
184 *
185 * @param ba acceleration bias to be set.
186 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
187 * @param bg angular speed bias to be set.
188 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
189 * @throws AlgebraException if provided cross-coupling matrices cannot
190 * be inverted.
191 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
192 */
193 public RandomWalkEstimator(
194 final AccelerationTriad ba,
195 final Matrix ma,
196 final AngularSpeedTriad bg,
197 final Matrix mg) throws AlgebraException {
198 try {
199 setAccelerationBias(ba);
200 setAccelerationCrossCouplingErrors(ma);
201 setAngularSpeedBias(bg);
202 setAngularSpeedCrossCouplingErrors(mg);
203 } catch (final LockedException ignore) {
204 // never happens
205 }
206 }
207
208 /**
209 * Constructor.
210 *
211 * @param ba acceleration bias to be set.
212 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
213 * @param bg angular speed bias to be set.
214 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
215 * @param listener listener to handle events.
216 * @throws AlgebraException if provided cross-coupling matrices cannot
217 * be inverted.
218 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
219 */
220 public RandomWalkEstimator(
221 final AccelerationTriad ba,
222 final Matrix ma,
223 final AngularSpeedTriad bg,
224 final Matrix mg,
225 final RandomWalkEstimatorListener listener)
226 throws AlgebraException {
227 this(ba, ma, bg, mg);
228 this.listener = listener;
229 }
230
231 /**
232 * Constructor.
233 *
234 * @param ba acceleration bias to be set.
235 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
236 * @param bg angular speed bias to be set.
237 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
238 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
239 * @throws AlgebraException if provided cross-coupling matrices cannot
240 * be inverted.
241 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
242 */
243 public RandomWalkEstimator(
244 final AccelerationTriad ba,
245 final Matrix ma,
246 final AngularSpeedTriad bg,
247 final Matrix mg,
248 final Matrix gg) throws AlgebraException {
249 this(ba, ma, bg, mg);
250 try {
251 setAngularSpeedGDependantCrossBias(gg);
252 } catch (final LockedException ignore) {
253 // never happens
254 }
255 }
256
257 /**
258 * Constructor.
259 *
260 * @param ba acceleration bias to be set.
261 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
262 * @param bg angular speed bias to be set.
263 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
264 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
265 * @param listener listener to handle events.
266 * @throws AlgebraException if provided cross-coupling matrices cannot
267 * be inverted.
268 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
269 */
270 public RandomWalkEstimator(
271 final AccelerationTriad ba,
272 final Matrix ma,
273 final AngularSpeedTriad bg,
274 final Matrix mg,
275 final Matrix gg,
276 final RandomWalkEstimatorListener listener) throws AlgebraException {
277 this(ba, ma, bg, mg, gg);
278 this.listener = listener;
279 }
280
281 /**
282 * Constructor.
283 *
284 * @param ba acceleration bias to be set expressed in meters per squared second
285 * (m/s`2). Must be 3x1.
286 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
287 * @param bg angular speed bias to be set expressed in radians per second
288 * (rad/s). Must be 3x1.
289 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
290 * @throws AlgebraException if provided cross-coupling matrices cannot
291 * be inverted.
292 * @throws IllegalArgumentException if any of the provided matrices do not have a proper
293 * size.
294 */
295 public RandomWalkEstimator(
296 final Matrix ba,
297 final Matrix ma,
298 final Matrix bg,
299 final Matrix mg) throws AlgebraException {
300 try {
301 setAccelerationBias(ba);
302 setAccelerationCrossCouplingErrors(ma);
303 setAngularSpeedBias(bg);
304 setAngularSpeedCrossCouplingErrors(mg);
305 } catch (final LockedException ignore) {
306 // never happens
307 }
308 }
309
310 /**
311 * Constructor.
312 *
313 * @param ba acceleration bias to be set expressed in meters per squared second
314 * (m/s`2). Must be 3x1.
315 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
316 * @param bg angular speed bias to be set expressed in radians per second
317 * (rad/s). Must be 3x1.
318 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
319 * @param listener listener to handle events.
320 * @throws AlgebraException if provided cross-coupling matrices cannot
321 * be inverted.
322 * @throws IllegalArgumentException if any of the provided matrices do not have a proper
323 * size.
324 */
325 public RandomWalkEstimator(
326 final Matrix ba,
327 final Matrix ma,
328 final Matrix bg,
329 final Matrix mg,
330 final RandomWalkEstimatorListener listener) throws AlgebraException {
331 this(ba, ma, bg, mg);
332 this.listener = listener;
333 }
334
335 /**
336 * Constructor.
337 *
338 * @param ba acceleration bias to be set expressed in meters per squared second
339 * (m/s`2). Must be 3x1.
340 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
341 * @param bg angular speed bias to be set expressed in radians per second
342 * (rad/s). Must be 3x1.
343 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
344 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
345 * @throws AlgebraException if provided cross-coupling matrices cannot
346 * be inverted.
347 * @throws IllegalArgumentException if any of the provided matrices do not have a proper
348 * size.
349 */
350 public RandomWalkEstimator(
351 final Matrix ba,
352 final Matrix ma,
353 final Matrix bg,
354 final Matrix mg,
355 final Matrix gg) throws AlgebraException {
356 this(ba, ma, bg, mg);
357 try {
358 setAngularSpeedGDependantCrossBias(gg);
359 } catch (final LockedException ignore) {
360 // never happens
361 }
362 }
363
364 /**
365 * Constructor.
366 *
367 * @param ba acceleration bias to be set expressed in meters per squared second
368 * (m/s`2). Must be 3x1.
369 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
370 * @param bg angular speed bias to be set expressed in radians per second
371 * (rad/s). Must be 3x1.
372 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
373 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
374 * @param listener listener to handle events.
375 * @throws AlgebraException if provided cross-coupling matrices cannot
376 * be inverted.
377 * @throws IllegalArgumentException if any of the provided matrices do not have a proper
378 * size.
379 */
380 public RandomWalkEstimator(
381 final Matrix ba,
382 final Matrix ma,
383 final Matrix bg,
384 final Matrix mg,
385 final Matrix gg,
386 final RandomWalkEstimatorListener listener) throws AlgebraException {
387 this(ba, ma, bg, mg, gg);
388 this.listener = listener;
389 }
390
391 /**
392 * Constructor.
393 *
394 * @param nedPosition position expressed on NED coordinates.
395 * @param nedC body to NED coordinate transformation indicating
396 * body orientation.
397 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
398 * transformation is not from
399 * body to NED coordinates.
400 */
401 public RandomWalkEstimator(
402 final NEDPosition nedPosition,
403 final CoordinateTransformation nedC) throws InvalidSourceAndDestinationFrameTypeException {
404 try {
405 setNedPositionAndNedOrientation(nedPosition, nedC);
406 } catch (final LockedException ignore) {
407 // never happens
408 }
409 }
410
411 /**
412 * Constructor.
413 *
414 * @param nedPosition position expressed on NED coordinates.
415 * @param nedC body to NED coordinate transformation indicating
416 * body orientation.
417 * @param listener listener to handle events.
418 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
419 * transformation is not from
420 * body to NED coordinates.
421 */
422 public RandomWalkEstimator(
423 final NEDPosition nedPosition,
424 final CoordinateTransformation nedC,
425 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException {
426 this(nedPosition, nedC);
427 this.listener = listener;
428 }
429
430 /**
431 * Constructor.
432 *
433 * @param nedPosition position expressed on NED coordinates.
434 * @param nedC body to NED coordinate transformation indicating
435 * body orientation.
436 * @param ba acceleration bias to be set.
437 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
438 * @param bg angular speed bias to be set.
439 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
440 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
441 * transformation is not from
442 * body to NED coordinates.
443 * @throws AlgebraException if provided cross-coupling matrices cannot
444 * be inverted.
445 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
446 */
447 public RandomWalkEstimator(
448 final NEDPosition nedPosition,
449 final CoordinateTransformation nedC,
450 final AccelerationTriad ba,
451 final Matrix ma,
452 final AngularSpeedTriad bg,
453 final Matrix mg) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
454 this(nedPosition, nedC);
455 try {
456 setAccelerationBias(ba);
457 setAccelerationCrossCouplingErrors(ma);
458 setAngularSpeedBias(bg);
459 setAngularSpeedCrossCouplingErrors(mg);
460 } catch (final LockedException ignore) {
461 // never happens
462 }
463 }
464
465 /**
466 * Constructor.
467 *
468 * @param nedPosition position expressed on NED coordinates.
469 * @param nedC body to NED coordinate transformation indicating
470 * body orientation.
471 * @param ba acceleration bias to be set.
472 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
473 * @param bg angular speed bias to be set.
474 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
475 * @param listener listener to handle events.
476 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
477 * transformation is not from
478 * body to NED coordinates.
479 * @throws AlgebraException if provided cross-coupling matrices cannot
480 * be inverted.
481 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
482 */
483 public RandomWalkEstimator(
484 final NEDPosition nedPosition,
485 final CoordinateTransformation nedC,
486 final AccelerationTriad ba,
487 final Matrix ma,
488 final AngularSpeedTriad bg,
489 final Matrix mg,
490 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
491 AlgebraException {
492 this(nedPosition, nedC, ba, ma, bg, mg);
493 this.listener = listener;
494 }
495
496 /**
497 * Constructor.
498 *
499 * @param nedPosition position expressed on NED coordinates.
500 * @param nedC body to NED coordinate transformation indicating
501 * body orientation.
502 * @param ba acceleration bias to be set.
503 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
504 * @param bg angular speed bias to be set.
505 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
506 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
507 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
508 * transformation is not from
509 * body to NED coordinates.
510 * @throws AlgebraException if provided cross-coupling matrices cannot
511 * be inverted.
512 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
513 */
514 public RandomWalkEstimator(
515 final NEDPosition nedPosition,
516 final CoordinateTransformation nedC,
517 final AccelerationTriad ba,
518 final Matrix ma,
519 final AngularSpeedTriad bg,
520 final Matrix mg,
521 final Matrix gg) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
522 this(nedPosition, nedC);
523 try {
524 setAccelerationBias(ba);
525 setAccelerationCrossCouplingErrors(ma);
526 setAngularSpeedBias(bg);
527 setAngularSpeedCrossCouplingErrors(mg);
528 setAngularSpeedGDependantCrossBias(gg);
529 } catch (final LockedException ignore) {
530 // never happens
531 }
532 }
533
534 /**
535 * Constructor.
536 *
537 * @param nedPosition position expressed on NED coordinates.
538 * @param nedC body to NED coordinate transformation indicating
539 * body orientation.
540 * @param ba acceleration bias to be set.
541 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
542 * @param bg angular speed bias to be set.
543 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
544 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
545 * @param listener listener to handle events.
546 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
547 * transformation is not from
548 * body to NED coordinates.
549 * @throws AlgebraException if provided cross-coupling matrices cannot
550 * be inverted.
551 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
552 */
553 public RandomWalkEstimator(
554 final NEDPosition nedPosition,
555 final CoordinateTransformation nedC,
556 final AccelerationTriad ba,
557 final Matrix ma,
558 final AngularSpeedTriad bg,
559 final Matrix mg,
560 final Matrix gg,
561 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
562 AlgebraException {
563 this(nedPosition, nedC, ba, ma, bg, mg, gg);
564 this.listener = listener;
565 }
566
567 /**
568 * Constructor.
569 *
570 * @param nedPosition position expressed on NED coordinates.
571 * @param nedC body to NED coordinate transformation indicating
572 * body orientation.
573 * @param ba acceleration bias to be set expressed in meters per squared second
574 * (m/s`2). Must be 3x1.
575 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
576 * @param bg angular speed bias to be set expressed in radians per second
577 * (rad/s). Must be 3x1.
578 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
579 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
580 * transformation is not from
581 * body to NED coordinates.
582 * @throws AlgebraException if provided cross-coupling matrices cannot
583 * be inverted.
584 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
585 */
586 public RandomWalkEstimator(
587 final NEDPosition nedPosition,
588 final CoordinateTransformation nedC,
589 final Matrix ba,
590 final Matrix ma,
591 final Matrix bg,
592 final Matrix mg) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
593 this(nedPosition, nedC);
594 try {
595 setAccelerationBias(ba);
596 setAccelerationCrossCouplingErrors(ma);
597 setAngularSpeedBias(bg);
598 setAngularSpeedCrossCouplingErrors(mg);
599 } catch (final LockedException ignore) {
600 // never happens
601 }
602 }
603
604 /**
605 * Constructor.
606 *
607 * @param nedPosition position expressed on NED coordinates.
608 * @param nedC body to NED coordinate transformation indicating
609 * body orientation.
610 * @param ba acceleration bias to be set expressed in meters per squared second
611 * (m/s`2). Must be 3x1.
612 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
613 * @param bg angular speed bias to be set expressed in radians per second
614 * (rad/s). Must be 3x1.
615 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
616 * @param listener listener to handle events.
617 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
618 * transformation is not from
619 * body to NED coordinates.
620 * @throws AlgebraException if provided cross-coupling matrices cannot
621 * be inverted.
622 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
623 */
624 public RandomWalkEstimator(
625 final NEDPosition nedPosition,
626 final CoordinateTransformation nedC,
627 final Matrix ba,
628 final Matrix ma,
629 final Matrix bg,
630 final Matrix mg,
631 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
632 AlgebraException {
633 this(nedPosition, nedC, ba, ma, bg, mg);
634 this.listener = listener;
635 }
636
637 /**
638 * Constructor.
639 *
640 * @param nedPosition position expressed on NED coordinates.
641 * @param nedC body to NED coordinate transformation indicating
642 * body orientation.
643 * @param ba acceleration bias to be set expressed in meters per squared second
644 * (m/s`2). Must be 3x1.
645 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
646 * @param bg angular speed bias to be set expressed in radians per second
647 * (rad/s). Must be 3x1.
648 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
649 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
650 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
651 * transformation is not from
652 * body to NED coordinates.
653 * @throws AlgebraException if provided cross-coupling matrices cannot
654 * be inverted.
655 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
656 */
657 public RandomWalkEstimator(
658 final NEDPosition nedPosition,
659 final CoordinateTransformation nedC,
660 final Matrix ba,
661 final Matrix ma,
662 final Matrix bg,
663 final Matrix mg,
664 final Matrix gg) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
665 this(nedPosition, nedC, ba, ma, bg, mg);
666 try {
667 setAngularSpeedGDependantCrossBias(gg);
668 } catch (final LockedException ignore) {
669 // never happens
670 }
671 }
672
673 /**
674 * Constructor.
675 *
676 * @param nedPosition position expressed on NED coordinates.
677 * @param nedC body to NED coordinate transformation indicating
678 * body orientation.
679 * @param ba acceleration bias to be set expressed in meters per squared second
680 * (m/s`2). Must be 3x1.
681 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
682 * @param bg angular speed bias to be set expressed in radians per second
683 * (rad/s). Must be 3x1.
684 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
685 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
686 * @param listener listener to handle events.
687 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
688 * transformation is not from
689 * body to NED coordinates.
690 * @throws AlgebraException if provided cross-coupling matrices cannot
691 * be inverted.
692 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
693 */
694 public RandomWalkEstimator(
695 final NEDPosition nedPosition,
696 final CoordinateTransformation nedC,
697 final Matrix ba,
698 final Matrix ma,
699 final Matrix bg,
700 final Matrix mg,
701 final Matrix gg,
702 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
703 AlgebraException {
704 this(nedPosition, nedC, ba, ma, bg, mg, gg);
705 this.listener = listener;
706 }
707
708 /**
709 * Constructor.
710 *
711 * @param ecefPosition position expressed on ECEF coordinates.
712 * @param nedC body to NED coordinate transformation indicating body
713 * orientation.
714 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
715 * transformation is not from
716 * body to NED coordinates.
717 */
718 public RandomWalkEstimator(
719 final ECEFPosition ecefPosition,
720 final CoordinateTransformation nedC) throws InvalidSourceAndDestinationFrameTypeException {
721 try {
722 setEcefPositionAndNedOrientation(ecefPosition, nedC);
723 } catch (final LockedException ignore) {
724 // never happens
725 }
726 }
727
728 /**
729 * Constructor.
730 *
731 * @param ecefPosition position expressed on ECEF coordinates.
732 * @param nedC body to NED coordinate transformation indicating body
733 * orientation.
734 * @param listener listener to handle events.
735 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
736 * transformation is not from
737 * body to NED coordinates.
738 */
739 public RandomWalkEstimator(
740 final ECEFPosition ecefPosition,
741 final CoordinateTransformation nedC,
742 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException {
743 this(ecefPosition, nedC);
744 this.listener = listener;
745 }
746
747 /**
748 * Constructor.
749 *
750 * @param ecefPosition position expressed on ECEF coordinates.
751 * @param nedC body to NED coordinate transformation indicating body
752 * orientation.
753 * @param ba acceleration bias to be set.
754 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
755 * @param bg angular speed bias to be set.
756 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
757 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
758 * transformation is not from
759 * body to NED coordinates.
760 * @throws AlgebraException if provided cross-coupling matrices cannot
761 * be inverted.
762 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
763 */
764 public RandomWalkEstimator(
765 final ECEFPosition ecefPosition,
766 final CoordinateTransformation nedC,
767 final AccelerationTriad ba,
768 final Matrix ma,
769 final AngularSpeedTriad bg,
770 final Matrix mg) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
771 this(ecefPosition, nedC);
772 try {
773 setAccelerationBias(ba);
774 setAccelerationCrossCouplingErrors(ma);
775 setAngularSpeedBias(bg);
776 setAngularSpeedCrossCouplingErrors(mg);
777 } catch (final LockedException ignore) {
778 // never happens
779 }
780 }
781
782 /**
783 * Constructor.
784 *
785 * @param ecefPosition position expressed on ECEF coordinates.
786 * @param nedC body to NED coordinate transformation indicating body
787 * orientation.
788 * @param ba acceleration bias to be set.
789 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
790 * @param bg angular speed bias to be set.
791 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
792 * @param listener listener to handle events.
793 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
794 * transformation is not from
795 * body to NED coordinates.
796 * @throws AlgebraException if provided cross-coupling matrices cannot
797 * be inverted.
798 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
799 */
800 public RandomWalkEstimator(
801 final ECEFPosition ecefPosition,
802 final CoordinateTransformation nedC,
803 final AccelerationTriad ba,
804 final Matrix ma,
805 final AngularSpeedTriad bg,
806 final Matrix mg,
807 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
808 AlgebraException {
809 this(ecefPosition, nedC, ba, ma, bg, mg);
810 this.listener = listener;
811 }
812
813 /**
814 * Constructor.
815 *
816 * @param ecefPosition position expressed on ECEF coordinates.
817 * @param nedC body to NED coordinate transformation indicating body
818 * orientation.
819 * @param ba acceleration bias to be set.
820 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
821 * @param bg angular speed bias to be set.
822 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
823 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
824 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
825 * transformation is not from
826 * body to NED coordinates.
827 * @throws AlgebraException if provided cross-coupling matrices cannot
828 * be inverted.
829 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
830 */
831 public RandomWalkEstimator(
832 final ECEFPosition ecefPosition,
833 final CoordinateTransformation nedC,
834 final AccelerationTriad ba,
835 final Matrix ma,
836 final AngularSpeedTriad bg,
837 final Matrix mg,
838 final Matrix gg) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
839 this(ecefPosition, nedC);
840 try {
841 setAccelerationBias(ba);
842 setAccelerationCrossCouplingErrors(ma);
843 setAngularSpeedBias(bg);
844 setAngularSpeedCrossCouplingErrors(mg);
845 setAngularSpeedGDependantCrossBias(gg);
846 } catch (final LockedException ignore) {
847 // never happens
848 }
849 }
850
851 /**
852 * Constructor.
853 *
854 * @param ecefPosition position expressed on ECEF coordinates.
855 * @param nedC body to NED coordinate transformation indicating body
856 * orientation.
857 * @param ba acceleration bias to be set.
858 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
859 * @param bg angular speed bias to be set.
860 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
861 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
862 * @param listener listener to handle events.
863 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
864 * transformation is not from
865 * body to NED coordinates.
866 * @throws AlgebraException if provided cross-coupling matrices cannot
867 * be inverted.
868 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
869 */
870 public RandomWalkEstimator(
871 final ECEFPosition ecefPosition,
872 final CoordinateTransformation nedC,
873 final AccelerationTriad ba,
874 final Matrix ma,
875 final AngularSpeedTriad bg,
876 final Matrix mg,
877 final Matrix gg,
878 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
879 AlgebraException {
880 this(ecefPosition, nedC, ba, ma, bg, mg, gg);
881 this.listener = listener;
882 }
883
884 /**
885 * Constructor.
886 *
887 * @param ecefPosition position expressed on ECEF coordinates.
888 * @param nedC body to NED coordinate transformation indicating body
889 * orientation.
890 * @param ba acceleration bias to be set.
891 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
892 * @param bg angular speed bias to be set.
893 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
894 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
895 * transformation is not from
896 * body to NED coordinates.
897 * @throws AlgebraException if provided cross-coupling matrices cannot
898 * be inverted.
899 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
900 */
901 public RandomWalkEstimator(
902 final ECEFPosition ecefPosition,
903 final CoordinateTransformation nedC,
904 final Matrix ba,
905 final Matrix ma,
906 final Matrix bg,
907 final Matrix mg) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
908 this(ecefPosition, nedC);
909 try {
910 setAccelerationBias(ba);
911 setAccelerationCrossCouplingErrors(ma);
912 setAngularSpeedBias(bg);
913 setAngularSpeedCrossCouplingErrors(mg);
914 } catch (final LockedException ignore) {
915 // never happens
916 }
917 }
918
919 /**
920 * Constructor.
921 *
922 * @param ecefPosition position expressed on ECEF coordinates.
923 * @param nedC body to NED coordinate transformation indicating body
924 * orientation.
925 * @param ba acceleration bias to be set.
926 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
927 * @param bg angular speed bias to be set.
928 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
929 * @param listener listener to handle events.
930 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
931 * transformation is not from
932 * body to NED coordinates.
933 * @throws AlgebraException if provided cross-coupling matrices cannot
934 * be inverted.
935 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
936 */
937 public RandomWalkEstimator(
938 final ECEFPosition ecefPosition,
939 final CoordinateTransformation nedC,
940 final Matrix ba,
941 final Matrix ma,
942 final Matrix bg,
943 final Matrix mg,
944 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
945 AlgebraException {
946 this(ecefPosition, nedC, ba, ma, bg, mg);
947 this.listener = listener;
948 }
949
950 /**
951 * Constructor.
952 *
953 * @param ecefPosition position expressed on ECEF coordinates.
954 * @param nedC body to NED coordinate transformation indicating body
955 * orientation.
956 * @param ba acceleration bias to be set.
957 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
958 * @param bg angular speed bias to be set.
959 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
960 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
961 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
962 * transformation is not from
963 * body to NED coordinates.
964 * @throws AlgebraException if provided cross-coupling matrices cannot
965 * be inverted.
966 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
967 */
968 public RandomWalkEstimator(
969 final ECEFPosition ecefPosition,
970 final CoordinateTransformation nedC,
971 final Matrix ba,
972 final Matrix ma,
973 final Matrix bg,
974 final Matrix mg,
975 final Matrix gg) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
976 this(ecefPosition, nedC, ba, ma, bg, mg);
977 try {
978 setAngularSpeedGDependantCrossBias(gg);
979 } catch (final LockedException ignore) {
980 // never happens
981 }
982 }
983
984 /**
985 * Constructor.
986 *
987 * @param ecefPosition position expressed on ECEF coordinates.
988 * @param nedC body to NED coordinate transformation indicating body
989 * orientation.
990 * @param ba acceleration bias to be set.
991 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
992 * @param bg angular speed bias to be set.
993 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
994 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
995 * @param listener listener to handle events.
996 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
997 * transformation is not from
998 * body to NED coordinates.
999 * @throws AlgebraException if provided cross-coupling matrices cannot
1000 * be inverted.
1001 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1002 */
1003 public RandomWalkEstimator(
1004 final ECEFPosition ecefPosition,
1005 final CoordinateTransformation nedC,
1006 final Matrix ba,
1007 final Matrix ma,
1008 final Matrix bg,
1009 final Matrix mg,
1010 final Matrix gg,
1011 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
1012 AlgebraException {
1013 this(ecefPosition, nedC, ba, ma, bg, mg, gg);
1014 this.listener = listener;
1015 }
1016
1017 /**
1018 * Constructor.
1019 *
1020 * @param nedPosition position expressed on NED coordinates.
1021 * @param nedC body to NED coordinate transformation indicating
1022 * body orientation.
1023 * @param ba acceleration bias to be set.
1024 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1025 * @param bg angular speed bias to be set.
1026 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1027 * @param timeInterval time interval between body kinematics samples expressed
1028 * in seconds (s).
1029 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1030 * transformation is not from
1031 * body to NED coordinates.
1032 * @throws AlgebraException if provided cross-coupling matrices cannot
1033 * be inverted.
1034 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1035 */
1036 public RandomWalkEstimator(
1037 final NEDPosition nedPosition,
1038 final CoordinateTransformation nedC,
1039 final AccelerationTriad ba,
1040 final Matrix ma,
1041 final AngularSpeedTriad bg,
1042 final Matrix mg,
1043 final double timeInterval) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
1044 this(nedPosition, nedC, ba, ma, bg, mg);
1045 try {
1046 setTimeInterval(timeInterval);
1047 } catch (final LockedException ignore) {
1048 // never happens
1049 }
1050 }
1051
1052 /**
1053 * Constructor.
1054 *
1055 * @param nedPosition position expressed on NED coordinates.
1056 * @param nedC body to NED coordinate transformation indicating
1057 * body orientation.
1058 * @param ba acceleration bias to be set.
1059 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1060 * @param bg angular speed bias to be set.
1061 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1062 * @param timeInterval time interval between body kinematics samples expressed
1063 * in seconds (s).
1064 * @param listener listener to handle events.
1065 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1066 * transformation is not from
1067 * body to NED coordinates.
1068 * @throws AlgebraException if provided cross-coupling matrices cannot
1069 * be inverted.
1070 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1071 */
1072 public RandomWalkEstimator(
1073 final NEDPosition nedPosition,
1074 final CoordinateTransformation nedC,
1075 final AccelerationTriad ba,
1076 final Matrix ma,
1077 final AngularSpeedTriad bg,
1078 final Matrix mg,
1079 final double timeInterval,
1080 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
1081 AlgebraException {
1082 this(nedPosition, nedC, ba, ma, bg, mg, timeInterval);
1083 this.listener = listener;
1084 }
1085
1086 /**
1087 * Constructor.
1088 *
1089 * @param nedPosition position expressed on NED coordinates.
1090 * @param nedC body to NED coordinate transformation indicating
1091 * body orientation.
1092 * @param ba acceleration bias to be set.
1093 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1094 * @param bg angular speed bias to be set.
1095 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1096 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
1097 * @param timeInterval time interval between body kinematics samples expressed
1098 * in seconds (s).
1099 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1100 * transformation is not from
1101 * body to NED coordinates.
1102 * @throws AlgebraException if provided cross-coupling matrices cannot
1103 * be inverted.
1104 * @throws IllegalArgumentException if provided, matrices are not 3x3.
1105 */
1106 public RandomWalkEstimator(
1107 final NEDPosition nedPosition,
1108 final CoordinateTransformation nedC,
1109 final AccelerationTriad ba,
1110 final Matrix ma,
1111 final AngularSpeedTriad bg,
1112 final Matrix mg,
1113 final Matrix gg,
1114 final double timeInterval) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
1115 this(nedPosition, nedC, ba, ma, bg, mg, gg);
1116 try {
1117 setTimeInterval(timeInterval);
1118 } catch (final LockedException ignore) {
1119 // never happens
1120 }
1121 }
1122
1123 /**
1124 * Constructor.
1125 *
1126 * @param nedPosition position expressed on NED coordinates.
1127 * @param nedC body to NED coordinate transformation indicating
1128 * body orientation.
1129 * @param ba acceleration bias to be set.
1130 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1131 * @param bg angular speed bias to be set.
1132 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1133 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
1134 * @param timeInterval time interval between body kinematics samples expressed
1135 * in seconds (s).
1136 * @param listener listener to handle events.
1137 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1138 * transformation is not from
1139 * body to NED coordinates.
1140 * @throws AlgebraException if provided cross-coupling matrices cannot
1141 * be inverted.
1142 * @throws IllegalArgumentException if provided, matrices are not 3x3.
1143 */
1144 public RandomWalkEstimator(
1145 final NEDPosition nedPosition,
1146 final CoordinateTransformation nedC,
1147 final AccelerationTriad ba,
1148 final Matrix ma,
1149 final AngularSpeedTriad bg,
1150 final Matrix mg,
1151 final Matrix gg,
1152 final double timeInterval,
1153 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
1154 AlgebraException {
1155 this(nedPosition, nedC, ba, ma, bg, mg, gg, timeInterval);
1156 this.listener = listener;
1157 }
1158
1159 /**
1160 * Constructor.
1161 *
1162 * @param nedPosition position expressed on NED coordinates.
1163 * @param nedC body to NED coordinate transformation indicating
1164 * body orientation.
1165 * @param ba acceleration bias to be set expressed in meters per squared second
1166 * (m/s`2). Must be 3x1.
1167 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1168 * @param bg angular speed bias to be set expressed in radians per second
1169 * (rad/s). Must be 3x1.
1170 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1171 * @param timeInterval time interval between body kinematics samples expressed
1172 * in seconds (s).
1173 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1174 * transformation is not from
1175 * body to NED coordinates.
1176 * @throws AlgebraException if provided cross-coupling matrices cannot
1177 * be inverted.
1178 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1179 */
1180 public RandomWalkEstimator(
1181 final NEDPosition nedPosition,
1182 final CoordinateTransformation nedC,
1183 final Matrix ba,
1184 final Matrix ma,
1185 final Matrix bg,
1186 final Matrix mg,
1187 final double timeInterval) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
1188 this(nedPosition, nedC, ba, ma, bg, mg);
1189 try {
1190 setTimeInterval(timeInterval);
1191 } catch (final LockedException ignore) {
1192 // never happens
1193 }
1194 }
1195
1196 /**
1197 * Constructor.
1198 *
1199 * @param nedPosition position expressed on NED coordinates.
1200 * @param nedC body to NED coordinate transformation indicating
1201 * body orientation.
1202 * @param ba acceleration bias to be set expressed in meters per squared second
1203 * (m/s`2). Must be 3x1.
1204 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1205 * @param bg angular speed bias to be set expressed in radians per second
1206 * (rad/s). Must be 3x1.
1207 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1208 * @param timeInterval time interval between body kinematics samples expressed
1209 * in seconds (s).
1210 * @param listener listener to handle events.
1211 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1212 * transformation is not from
1213 * body to NED coordinates.
1214 * @throws AlgebraException if provided cross-coupling matrices cannot
1215 * be inverted.
1216 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1217 */
1218 public RandomWalkEstimator(
1219 final NEDPosition nedPosition,
1220 final CoordinateTransformation nedC,
1221 final Matrix ba,
1222 final Matrix ma,
1223 final Matrix bg,
1224 final Matrix mg,
1225 final double timeInterval,
1226 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
1227 AlgebraException {
1228 this(nedPosition, nedC, ba, ma, bg, mg, timeInterval);
1229 this.listener = listener;
1230 }
1231
1232 /**
1233 * Constructor.
1234 *
1235 * @param nedPosition position expressed on NED coordinates.
1236 * @param nedC body to NED coordinate transformation indicating
1237 * body orientation.
1238 * @param ba acceleration bias to be set expressed in meters per squared second
1239 * (m/s`2). Must be 3x1.
1240 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1241 * @param bg angular speed bias to be set expressed in radians per second
1242 * (rad/s). Must be 3x1.
1243 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1244 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
1245 * @param timeInterval time interval between body kinematics samples expressed
1246 * in seconds (s).
1247 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1248 * transformation is not from
1249 * body to NED coordinates.
1250 * @throws AlgebraException if provided cross-coupling matrices cannot
1251 * be inverted.
1252 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1253 */
1254 public RandomWalkEstimator(
1255 final NEDPosition nedPosition,
1256 final CoordinateTransformation nedC,
1257 final Matrix ba,
1258 final Matrix ma,
1259 final Matrix bg,
1260 final Matrix mg,
1261 final Matrix gg,
1262 final double timeInterval) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
1263 this(nedPosition, nedC, ba, ma, bg, mg, gg);
1264 try {
1265 setTimeInterval(timeInterval);
1266 } catch (final LockedException ignore) {
1267 // never happens
1268 }
1269 }
1270
1271 /**
1272 * Constructor.
1273 *
1274 * @param nedPosition position expressed on NED coordinates.
1275 * @param nedC body to NED coordinate transformation indicating
1276 * body orientation.
1277 * @param ba acceleration bias to be set expressed in meters per squared second
1278 * (m/s`2). Must be 3x1.
1279 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1280 * @param bg angular speed bias to be set expressed in radians per second
1281 * (rad/s). Must be 3x1.
1282 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1283 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
1284 * @param timeInterval time interval between body kinematics samples expressed
1285 * in seconds (s).
1286 * @param listener listener to handle events.
1287 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1288 * transformation is not from
1289 * body to NED coordinates.
1290 * @throws AlgebraException if provided cross-coupling matrices cannot
1291 * be inverted.
1292 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1293 */
1294 public RandomWalkEstimator(
1295 final NEDPosition nedPosition,
1296 final CoordinateTransformation nedC,
1297 final Matrix ba,
1298 final Matrix ma,
1299 final Matrix bg,
1300 final Matrix mg,
1301 final Matrix gg,
1302 final double timeInterval,
1303 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
1304 AlgebraException {
1305 this(nedPosition, nedC, ba, ma, bg, mg, gg, timeInterval);
1306 this.listener = listener;
1307 }
1308
1309 /**
1310 * Constructor.
1311 *
1312 * @param ecefPosition position expressed on ECEF coordinates.
1313 * @param nedC body to NED coordinate transformation indicating body
1314 * orientation.
1315 * @param ba acceleration bias to be set.
1316 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1317 * @param bg angular speed bias to be set.
1318 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1319 * @param timeInterval time interval between body kinematics samples expressed
1320 * in seconds (s).
1321 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1322 * transformation is not from
1323 * body to NED coordinates.
1324 * @throws AlgebraException if provided cross-coupling matrices cannot
1325 * be inverted.
1326 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1327 */
1328 public RandomWalkEstimator(
1329 final ECEFPosition ecefPosition,
1330 final CoordinateTransformation nedC,
1331 final AccelerationTriad ba,
1332 final Matrix ma,
1333 final AngularSpeedTriad bg,
1334 final Matrix mg,
1335 final double timeInterval) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
1336 this(ecefPosition, nedC, ba, ma, bg, mg);
1337 try {
1338 setTimeInterval(timeInterval);
1339 } catch (final LockedException ignore) {
1340 // never happens
1341 }
1342 }
1343
1344 /**
1345 * Constructor.
1346 *
1347 * @param ecefPosition position expressed on ECEF coordinates.
1348 * @param nedC body to NED coordinate transformation indicating body
1349 * orientation.
1350 * @param ba acceleration bias to be set.
1351 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1352 * @param bg angular speed bias to be set.
1353 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1354 * @param timeInterval time interval between body kinematics samples expressed
1355 * in seconds (s).
1356 * @param listener listener to handle events.
1357 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1358 * transformation is not from
1359 * body to NED coordinates.
1360 * @throws AlgebraException if provided cross-coupling matrices cannot
1361 * be inverted.
1362 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1363 */
1364 public RandomWalkEstimator(
1365 final ECEFPosition ecefPosition,
1366 final CoordinateTransformation nedC,
1367 final AccelerationTriad ba,
1368 final Matrix ma,
1369 final AngularSpeedTriad bg,
1370 final Matrix mg,
1371 final double timeInterval,
1372 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
1373 AlgebraException {
1374 this(ecefPosition, nedC, ba, ma, bg, mg, timeInterval);
1375 this.listener = listener;
1376 }
1377
1378 /**
1379 * Constructor.
1380 *
1381 * @param ecefPosition position expressed on ECEF coordinates.
1382 * @param nedC body to NED coordinate transformation indicating body
1383 * orientation.
1384 * @param ba acceleration bias to be set.
1385 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1386 * @param bg angular speed bias to be set.
1387 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1388 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
1389 * @param timeInterval time interval between body kinematics samples expressed
1390 * in seconds (s).
1391 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1392 * transformation is not from
1393 * body to NED coordinates.
1394 * @throws AlgebraException if provided cross-coupling matrices cannot
1395 * be inverted.
1396 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1397 */
1398 public RandomWalkEstimator(
1399 final ECEFPosition ecefPosition,
1400 final CoordinateTransformation nedC,
1401 final AccelerationTriad ba,
1402 final Matrix ma,
1403 final AngularSpeedTriad bg,
1404 final Matrix mg,
1405 final Matrix gg,
1406 final double timeInterval) throws InvalidSourceAndDestinationFrameTypeException,
1407 AlgebraException {
1408 this(ecefPosition, nedC, ba, ma, bg, mg, gg);
1409 try {
1410 setTimeInterval(timeInterval);
1411 } catch (final LockedException ignore) {
1412 // never happens
1413 }
1414 }
1415
1416 /**
1417 * Constructor.
1418 *
1419 * @param ecefPosition position expressed on ECEF coordinates.
1420 * @param nedC body to NED coordinate transformation indicating body
1421 * orientation.
1422 * @param ba acceleration bias to be set.
1423 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1424 * @param bg angular speed bias to be set.
1425 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1426 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
1427 * @param timeInterval time interval between body kinematics samples expressed
1428 * in seconds (s).
1429 * @param listener listener to handle events.
1430 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1431 * transformation is not from
1432 * body to NED coordinates.
1433 * @throws AlgebraException if provided cross-coupling matrices cannot
1434 * be inverted.
1435 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1436 */
1437 public RandomWalkEstimator(
1438 final ECEFPosition ecefPosition,
1439 final CoordinateTransformation nedC,
1440 final AccelerationTriad ba,
1441 final Matrix ma,
1442 final AngularSpeedTriad bg,
1443 final Matrix mg,
1444 final Matrix gg,
1445 final double timeInterval,
1446 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
1447 AlgebraException {
1448 this(ecefPosition, nedC, ba, ma, bg, mg, gg, timeInterval);
1449 this.listener = listener;
1450 }
1451
1452 /**
1453 * Constructor.
1454 *
1455 * @param ecefPosition position expressed on ECEF coordinates.
1456 * @param nedC body to NED coordinate transformation indicating body
1457 * orientation.
1458 * @param ba acceleration bias to be set.
1459 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1460 * @param bg angular speed bias to be set.
1461 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1462 * @param timeInterval time interval between body kinematics samples expressed
1463 * in seconds (s).
1464 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1465 * transformation is not from
1466 * body to NED coordinates.
1467 * @throws AlgebraException if provided cross-coupling matrices cannot
1468 * be inverted.
1469 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1470 */
1471 public RandomWalkEstimator(
1472 final ECEFPosition ecefPosition,
1473 final CoordinateTransformation nedC,
1474 final Matrix ba,
1475 final Matrix ma,
1476 final Matrix bg,
1477 final Matrix mg,
1478 final double timeInterval) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
1479 this(ecefPosition, nedC, ba, ma, bg, mg);
1480 try {
1481 setTimeInterval(timeInterval);
1482 } catch (final LockedException ignore) {
1483 // never happens
1484 }
1485 }
1486
1487 /**
1488 * Constructor.
1489 *
1490 * @param ecefPosition position expressed on ECEF coordinates.
1491 * @param nedC body to NED coordinate transformation indicating body
1492 * orientation.
1493 * @param ba acceleration bias to be set.
1494 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1495 * @param bg angular speed bias to be set.
1496 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1497 * @param timeInterval time interval between body kinematics samples expressed
1498 * in seconds (s).
1499 * @param listener listener to handle events.
1500 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1501 * transformation is not from
1502 * body to NED coordinates.
1503 * @throws AlgebraException if provided cross-coupling matrices cannot
1504 * be inverted.
1505 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1506 */
1507 public RandomWalkEstimator(
1508 final ECEFPosition ecefPosition,
1509 final CoordinateTransformation nedC,
1510 final Matrix ba,
1511 final Matrix ma,
1512 final Matrix bg,
1513 final Matrix mg,
1514 final double timeInterval,
1515 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
1516 AlgebraException {
1517 this(ecefPosition, nedC, ba, ma, bg, mg, timeInterval);
1518 this.listener = listener;
1519 }
1520
1521 /**
1522 * Constructor.
1523 *
1524 * @param ecefPosition position expressed on ECEF coordinates.
1525 * @param nedC body to NED coordinate transformation indicating body
1526 * orientation.
1527 * @param ba acceleration bias to be set.
1528 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1529 * @param bg angular speed bias to be set.
1530 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1531 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
1532 * @param timeInterval time interval between body kinematics samples expressed
1533 * in seconds (s).
1534 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1535 * transformation is not from
1536 * body to NED coordinates.
1537 * @throws AlgebraException if provided cross-coupling matrices cannot
1538 * be inverted.
1539 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1540 */
1541 public RandomWalkEstimator(
1542 final ECEFPosition ecefPosition,
1543 final CoordinateTransformation nedC,
1544 final Matrix ba,
1545 final Matrix ma,
1546 final Matrix bg,
1547 final Matrix mg,
1548 final Matrix gg,
1549 final double timeInterval) throws InvalidSourceAndDestinationFrameTypeException, AlgebraException {
1550 this(ecefPosition, nedC, ba, ma, bg, mg, gg);
1551 try {
1552 setTimeInterval(timeInterval);
1553 } catch (final LockedException ignore) {
1554 // never happens
1555 }
1556 }
1557
1558 /**
1559 * Constructor.
1560 *
1561 * @param ecefPosition position expressed on ECEF coordinates.
1562 * @param nedC body to NED coordinate transformation indicating body
1563 * orientation.
1564 * @param ba acceleration bias to be set.
1565 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
1566 * @param bg angular speed bias to be set.
1567 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
1568 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
1569 * @param timeInterval time interval between body kinematics samples expressed
1570 * in seconds (s).
1571 * @param listener listener to handle events.
1572 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
1573 * transformation is not from
1574 * body to NED coordinates.
1575 * @throws AlgebraException if provided cross-coupling matrices cannot
1576 * be inverted.
1577 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
1578 */
1579 public RandomWalkEstimator(
1580 final ECEFPosition ecefPosition,
1581 final CoordinateTransformation nedC,
1582 final Matrix ba,
1583 final Matrix ma,
1584 final Matrix bg,
1585 final Matrix mg,
1586 final Matrix gg,
1587 final double timeInterval,
1588 final RandomWalkEstimatorListener listener) throws InvalidSourceAndDestinationFrameTypeException,
1589 AlgebraException {
1590 this(ecefPosition, nedC, ba, ma, bg, mg, gg, timeInterval);
1591 this.listener = listener;
1592 }
1593
1594 /**
1595 * Gets listener to handle events raised by this estimator.
1596 *
1597 * @return listener to handle events raised by this estimator.
1598 */
1599 public RandomWalkEstimatorListener getListener() {
1600 return listener;
1601 }
1602
1603 /**
1604 * Sets listener to handle events raised by this estimator.
1605 *
1606 * @param listener listener to handle events raised by this estimator.
1607 * @throws LockedException if estimator is running.
1608 */
1609 public void setListener(final RandomWalkEstimatorListener listener) throws LockedException {
1610 if (running) {
1611 throw new LockedException();
1612 }
1613
1614 this.listener = listener;
1615 }
1616
1617 /**
1618 * Gets acceleration bias values expressed in meters per squared second (m/s^2).
1619 *
1620 * @return bias values expressed in meters per squared second.
1621 */
1622 public Matrix getAccelerationBias() {
1623 return fixer.getAccelerationBias();
1624 }
1625
1626 /**
1627 * Gets acceleration bias values expressed in meters per squared second (m/s^2).
1628 *
1629 * @param result instance where the result will be stored.
1630 */
1631 public void getAccelerationBias(final Matrix result) {
1632 fixer.getAccelerationBias(result);
1633 }
1634
1635 /**
1636 * Sets acceleration bias values expressed in meters per squared second (m/s^2).
1637 *
1638 * @param bias bias values expressed in meters per squared second.
1639 * Must be 3x1.
1640 * @throws LockedException if estimator is running.
1641 * @throws IllegalArgumentException if matrix is not 3x1.
1642 */
1643 public void setAccelerationBias(final Matrix bias) throws LockedException {
1644 if (running) {
1645 throw new LockedException();
1646 }
1647
1648 fixer.setAccelerationBias(bias);
1649 driftEstimator.setAccelerationBias(bias);
1650 }
1651
1652 /**
1653 * Gets acceleration bias values expressed in meters per squared second (m/s^2).
1654 *
1655 * @return bias values expressed in meters per squared second.
1656 */
1657 public double[] getAccelerationBiasArray() {
1658 return fixer.getAccelerationBiasArray();
1659 }
1660
1661 /**
1662 * Gets acceleration bias values expressed in meters per squared second (m/s^2).
1663 *
1664 * @param result instance where result data will be stored.
1665 * @throws IllegalArgumentException if provided array does not have length 3.
1666 */
1667 public void getAccelerationBiasArray(final double[] result) {
1668 fixer.getAccelerationBiasArray(result);
1669 }
1670
1671 /**
1672 * Sets acceleration bias values expressed in meters per squared second (m/s^2).
1673 *
1674 * @param bias bias values expressed in meters per squared second (m/s^2).
1675 * Must have length 3.
1676 * @throws IllegalArgumentException if provided array does not have length 3.
1677 * @throws LockedException if estimator is running.
1678 */
1679 public void setAccelerationBias(final double[] bias) throws LockedException {
1680 if (running) {
1681 throw new LockedException();
1682 }
1683
1684 fixer.setAccelerationBias(bias);
1685 driftEstimator.setAccelerationBias(bias);
1686 }
1687
1688 /**
1689 * Gets acceleration bias.
1690 *
1691 * @return acceleration bias.
1692 */
1693 public AccelerationTriad getAccelerationBiasAsTriad() {
1694 return fixer.getAccelerationBiasAsTriad();
1695 }
1696
1697 /**
1698 * Gets acceleration bias.
1699 *
1700 * @param result instance where the result will be stored.
1701 */
1702 public void getAccelerationBiasAsTriad(final AccelerationTriad result) {
1703 fixer.getAccelerationBiasAsTriad(result);
1704 }
1705
1706 /**
1707 * Sets acceleration bias.
1708 *
1709 * @param bias acceleration bias to be set.
1710 * @throws LockedException if estimator is running.
1711 */
1712 public void setAccelerationBias(final AccelerationTriad bias) throws LockedException {
1713 if (running) {
1714 throw new LockedException();
1715 }
1716
1717 fixer.setAccelerationBias(bias);
1718 driftEstimator.setAccelerationBias(bias);
1719 }
1720
1721 /**
1722 * Gets acceleration x-coordinate of bias expressed in meters per squared
1723 * second (m/s^2).
1724 *
1725 * @return x-coordinate of bias expressed in meters per squared second (m/s^2).
1726 */
1727 public double getAccelerationBiasX() {
1728 return fixer.getAccelerationBiasX();
1729 }
1730
1731 /**
1732 * Sets acceleration x-coordinate of bias expressed in meters per squared
1733 * second (m/s^2).
1734 *
1735 * @param biasX x-coordinate of bias expressed in meters per squared second
1736 * (m/s^2).
1737 * @throws LockedException if estimator is running.
1738 */
1739 public void setAccelerationBiasX(final double biasX) throws LockedException {
1740 if (running) {
1741 throw new LockedException();
1742 }
1743
1744 fixer.setAccelerationBiasX(biasX);
1745 driftEstimator.setAccelerationBiasX(biasX);
1746 }
1747
1748 /**
1749 * Gets acceleration y-coordinate of bias expressed in meters per squared
1750 * second (m/s^2).
1751 *
1752 * @return y-coordinate of bias expressed in meters per squared second (m/s^2).
1753 */
1754 public double getAccelerationBiasY() {
1755 return fixer.getAccelerationBiasY();
1756 }
1757
1758 /**
1759 * Sets acceleration y-coordinate of bias expressed in meters per squared
1760 * second (m/s^2).
1761 *
1762 * @param biasY y-coordinate of bias expressed in meters per squared second
1763 * (m/s^2).
1764 * @throws LockedException if estimator is running.
1765 */
1766 public void setAccelerationBiasY(final double biasY) throws LockedException {
1767 if (running) {
1768 throw new LockedException();
1769 }
1770
1771 fixer.setAccelerationBiasY(biasY);
1772 driftEstimator.setAccelerationBiasY(biasY);
1773 }
1774
1775 /**
1776 * Gets acceleration z-coordinate of bias expressed in meters per squared
1777 * second (m/s^2).
1778 *
1779 * @return z-coordinate of bias expressed in meters per squared second (m/s^2).
1780 */
1781 public double getAccelerationBiasZ() {
1782 return fixer.getAccelerationBiasZ();
1783 }
1784
1785 /**
1786 * Sets acceleration z-coordinate of bias expressed in meters per squared
1787 * second (m/s^2).
1788 *
1789 * @param biasZ z-coordinate of bias expressed in meters per squared second (m/s^2).
1790 * @throws LockedException if estimator is running.
1791 */
1792 public void setAccelerationBiasZ(final double biasZ) throws LockedException {
1793 if (running) {
1794 throw new LockedException();
1795 }
1796
1797 fixer.setAccelerationBiasZ(biasZ);
1798 driftEstimator.setAccelerationBiasZ(biasZ);
1799 }
1800
1801 /**
1802 * Sets acceleration coordinates of bias expressed in meters per squared
1803 * second (m/s^2).
1804 *
1805 * @param biasX x-coordinate of bias.
1806 * @param biasY y-coordinate of bias.
1807 * @param biasZ z-coordinate of bias.
1808 * @throws LockedException if estimator is running.
1809 */
1810 public void setAccelerationBias(
1811 final double biasX, final double biasY, final double biasZ) throws LockedException {
1812 if (running) {
1813 throw new LockedException();
1814 }
1815
1816 fixer.setAccelerationBias(biasX, biasY, biasZ);
1817 driftEstimator.setAccelerationBias(biasX, biasY, biasZ);
1818 }
1819
1820 /**
1821 * Gets acceleration x-coordinate of bias.
1822 *
1823 * @return acceleration x-coordinate of bias.
1824 */
1825 public Acceleration getAccelerationBiasXAsAcceleration() {
1826 return fixer.getAccelerationBiasXAsAcceleration();
1827 }
1828
1829 /**
1830 * Gets acceleration x-coordinate of bias.
1831 *
1832 * @param result instance where the result will be stored.
1833 */
1834 public void getAccelerationBiasXAsAcceleration(final Acceleration result) {
1835 fixer.getAccelerationBiasXAsAcceleration(result);
1836 }
1837
1838 /**
1839 * Sets acceleration x-coordinate of bias.
1840 *
1841 * @param biasX acceleration x-coordinate of bias.
1842 * @throws LockedException if estimator is running.
1843 */
1844 public void setAccelerationBiasX(final Acceleration biasX) throws LockedException {
1845 if (running) {
1846 throw new LockedException();
1847 }
1848
1849 fixer.setAccelerationBiasX(biasX);
1850 driftEstimator.setAccelerationBiasX(biasX);
1851 }
1852
1853 /**
1854 * Gets acceleration y-coordinate of bias.
1855 *
1856 * @return acceleration y-coordinate of bias.
1857 */
1858 public Acceleration getAccelerationBiasYAsAcceleration() {
1859 return fixer.getAccelerationBiasYAsAcceleration();
1860 }
1861
1862 /**
1863 * Gets acceleration y-coordinate of bias.
1864 *
1865 * @param result instance where the result will be stored.
1866 */
1867 public void getAccelerationBiasYAsAcceleration(final Acceleration result) {
1868 fixer.getAccelerationBiasYAsAcceleration(result);
1869 }
1870
1871 /**
1872 * Sets acceleration y-coordinate of bias.
1873 *
1874 * @param biasY acceleration y-coordinate of bias.
1875 * @throws LockedException if estimator is running.
1876 */
1877 public void setAccelerationBiasY(final Acceleration biasY) throws LockedException {
1878 if (running) {
1879 throw new LockedException();
1880 }
1881
1882 fixer.setAccelerationBiasY(biasY);
1883 driftEstimator.setAccelerationBiasY(biasY);
1884 }
1885
1886 /**
1887 * Gets acceleration z-coordinate of bias.
1888 *
1889 * @return acceleration z-coordinate of bias.
1890 */
1891 public Acceleration getAccelerationBiasZAsAcceleration() {
1892 return fixer.getAccelerationBiasZAsAcceleration();
1893 }
1894
1895 /**
1896 * Gets acceleration z-coordinate of bias.
1897 *
1898 * @param result instance where the result will be stored.
1899 */
1900 public void getAccelerationBiasZAsAcceleration(final Acceleration result) {
1901 fixer.getAccelerationBiasZAsAcceleration(result);
1902 }
1903
1904 /**
1905 * Sets acceleration z-coordinate of bias.
1906 *
1907 * @param biasZ z-coordinate of bias.
1908 * @throws LockedException if estimator is running.
1909 */
1910 public void setAccelerationBiasZ(final Acceleration biasZ) throws LockedException {
1911 if (running) {
1912 throw new LockedException();
1913 }
1914
1915 fixer.setAccelerationBiasZ(biasZ);
1916 driftEstimator.setAccelerationBiasZ(biasZ);
1917 }
1918
1919 /**
1920 * Sets acceleration coordinates of bias.
1921 *
1922 * @param biasX x-coordinate of bias.
1923 * @param biasY y-coordinate of bias.
1924 * @param biasZ z-coordinate of bias.
1925 * @throws LockedException if estimator is running.
1926 */
1927 public void setAccelerationBias(
1928 final Acceleration biasX,
1929 final Acceleration biasY,
1930 final Acceleration biasZ) throws LockedException {
1931 if (running) {
1932 throw new LockedException();
1933 }
1934
1935 fixer.setAccelerationBias(biasX, biasY, biasZ);
1936 driftEstimator.setAccelerationBias(biasX, biasY, biasZ);
1937 }
1938
1939 /**
1940 * Gets acceleration cross-coupling errors matrix.
1941 *
1942 * @return acceleration cross-coupling errors matrix.
1943 */
1944 public Matrix getAccelerationCrossCouplingErrors() {
1945 return fixer.getAccelerationCrossCouplingErrors();
1946 }
1947
1948 /**
1949 * Gets acceleration cross-coupling errors matrix.
1950 *
1951 * @param result instance where the result will be stored.
1952 */
1953 public void getAccelerationCrossCouplingErrors(final Matrix result) {
1954 fixer.getAccelerationCrossCouplingErrors(result);
1955 }
1956
1957 /**
1958 * Sets acceleration cross-coupling errors matrix.
1959 *
1960 * @param crossCouplingErrors acceleration cross-coupling errors matrix.
1961 * Must be 3x3.
1962 * @throws LockedException if estimator is running.
1963 * @throws AlgebraException if matrix cannot be inverted.
1964 * @throws IllegalArgumentException if matrix is not 3x3.
1965 */
1966 public void setAccelerationCrossCouplingErrors(final Matrix crossCouplingErrors) throws AlgebraException,
1967 LockedException {
1968 if (running) {
1969 throw new LockedException();
1970 }
1971
1972 fixer.setAccelerationCrossCouplingErrors(crossCouplingErrors);
1973 driftEstimator.setAccelerationCrossCouplingErrors(crossCouplingErrors);
1974 }
1975
1976 /**
1977 * Gets acceleration x scaling factor.
1978 *
1979 * @return x scaling factor.
1980 */
1981 public double getAccelerationSx() {
1982 return fixer.getAccelerationSx();
1983 }
1984
1985 /**
1986 * Sets acceleration x scaling factor.
1987 *
1988 * @param sx x scaling factor.
1989 * @throws LockedException if estimator is running.
1990 * @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
1991 */
1992 public void setAccelerationSx(final double sx) throws LockedException, AlgebraException {
1993 if (running) {
1994 throw new LockedException();
1995 }
1996
1997 fixer.setAccelerationSx(sx);
1998 driftEstimator.setAccelerationSx(sx);
1999 }
2000
2001 /**
2002 * Gets acceleration y scaling factor.
2003 *
2004 * @return y scaling factor.
2005 */
2006 public double getAccelerationSy() {
2007 return fixer.getAccelerationSy();
2008 }
2009
2010 /**
2011 * Sets acceleration y scaling factor.
2012 *
2013 * @param sy y scaling factor.
2014 * @throws LockedException if estimator is running.
2015 * @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
2016 */
2017 public void setAccelerationSy(final double sy) throws LockedException, AlgebraException {
2018 if (running) {
2019 throw new LockedException();
2020 }
2021
2022 fixer.setAccelerationSy(sy);
2023 driftEstimator.setAccelerationSy(sy);
2024 }
2025
2026 /**
2027 * Gets acceleration z scaling factor.
2028 *
2029 * @return z scaling factor.
2030 */
2031 public double getAccelerationSz() {
2032 return fixer.getAccelerationSz();
2033 }
2034
2035 /**
2036 * Sets acceleration z scaling factor.
2037 *
2038 * @param sz z scaling factor.
2039 * @throws LockedException if estimator is running.
2040 * @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
2041 */
2042 public void setAccelerationSz(final double sz) throws LockedException, AlgebraException {
2043 if (running) {
2044 throw new LockedException();
2045 }
2046
2047 fixer.setAccelerationSz(sz);
2048 driftEstimator.setAccelerationSz(sz);
2049 }
2050
2051 /**
2052 * Gets acceleration x-y cross-coupling error.
2053 *
2054 * @return acceleration x-y cross-coupling error.
2055 */
2056 public double getAccelerationMxy() {
2057 return fixer.getAccelerationMxy();
2058 }
2059
2060 /**
2061 * Sets acceleration x-y cross-coupling error.
2062 *
2063 * @param mxy acceleration x-y cross-coupling error.
2064 * @throws LockedException if estimator is running.
2065 * @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
2066 */
2067 public void setAccelerationMxy(final double mxy) throws LockedException, AlgebraException {
2068 if (running) {
2069 throw new LockedException();
2070 }
2071
2072 fixer.setAccelerationMxy(mxy);
2073 driftEstimator.setAccelerationMxy(mxy);
2074 }
2075
2076 /**
2077 * Gets acceleration x-z cross-coupling error.
2078 *
2079 * @return acceleration x-z cross-coupling error.
2080 */
2081 public double getAccelerationMxz() {
2082 return fixer.getAccelerationMxz();
2083 }
2084
2085 /**
2086 * Sets acceleration x-z cross-coupling error.
2087 *
2088 * @param mxz acceleration x-z cross-coupling error.
2089 * @throws LockedException if estimator is running.
2090 * @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
2091 */
2092 public void setAccelerationMxz(final double mxz) throws LockedException, AlgebraException {
2093 if (running) {
2094 throw new LockedException();
2095 }
2096
2097 fixer.setAccelerationMxz(mxz);
2098 driftEstimator.setAccelerationMxz(mxz);
2099 }
2100
2101 /**
2102 * Gets acceleration y-x cross-coupling error.
2103 *
2104 * @return acceleration y-x cross-coupling error.
2105 */
2106 public double getAccelerationMyx() {
2107 return fixer.getAccelerationMyx();
2108 }
2109
2110 /**
2111 * Sets acceleration y-x cross-coupling error.
2112 *
2113 * @param myx acceleration y-x cross-coupling error.
2114 * @throws LockedException if estimator is running.
2115 * @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
2116 */
2117 public void setAccelerationMyx(final double myx) throws LockedException, AlgebraException {
2118 if (running) {
2119 throw new LockedException();
2120 }
2121
2122 fixer.setAccelerationMyx(myx);
2123 driftEstimator.setAccelerationMyx(myx);
2124 }
2125
2126 /**
2127 * Gets acceleration y-z cross-coupling error.
2128 *
2129 * @return y-z cross coupling error.
2130 */
2131 public double getAccelerationMyz() {
2132 return fixer.getAccelerationMyz();
2133 }
2134
2135 /**
2136 * Sets acceleration y-z cross-coupling error.
2137 *
2138 * @param myz y-z cross coupling error.
2139 * @throws LockedException if estimator is running.
2140 * @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
2141 */
2142 public void setAccelerationMyz(final double myz) throws LockedException, AlgebraException {
2143 if (running) {
2144 throw new LockedException();
2145 }
2146
2147 fixer.setAccelerationMyz(myz);
2148 driftEstimator.setAccelerationMyz(myz);
2149 }
2150
2151 /**
2152 * Gets acceleration z-x cross-coupling error.
2153 *
2154 * @return acceleration z-x cross-coupling error.
2155 */
2156 public double getAccelerationMzx() {
2157 return fixer.getAccelerationMzx();
2158 }
2159
2160 /**
2161 * Sets acceleration z-x cross-coupling error.
2162 *
2163 * @param mzx acceleration z-x cross coupling error.
2164 * @throws LockedException if estimator is running.
2165 * @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
2166 */
2167 public void setAccelerationMzx(final double mzx) throws LockedException, AlgebraException {
2168 if (running) {
2169 throw new LockedException();
2170 }
2171
2172 fixer.setAccelerationMzx(mzx);
2173 driftEstimator.setAccelerationMzx(mzx);
2174 }
2175
2176 /**
2177 * Gets acceleration z-y cross-coupling error.
2178 *
2179 * @return acceleration z-y cross-coupling error.
2180 */
2181 public double getAccelerationMzy() {
2182 return fixer.getAccelerationMzy();
2183 }
2184
2185 /**
2186 * Sets acceleration z-y cross-coupling error.
2187 *
2188 * @param mzy acceleration z-y cross coupling error.
2189 * @throws LockedException if estimator is running.
2190 * @throws AlgebraException if provided value makes acceleration cross-coupling matrix non-invertible.
2191 */
2192 public void setAccelerationMzy(final double mzy) throws LockedException, AlgebraException {
2193 if (running) {
2194 throw new LockedException();
2195 }
2196
2197 fixer.setAccelerationMzy(mzy);
2198 driftEstimator.setAccelerationMzy(mzy);
2199 }
2200
2201 /**
2202 * Sets acceleration scaling factors.
2203 *
2204 * @param sx x scaling factor.
2205 * @param sy y scaling factor.
2206 * @param sz z scaling factor.
2207 * @throws LockedException if estimator is running.
2208 * @throws AlgebraException if provided values make acceleration cross-coupling matrix non-invertible.
2209 */
2210 public void setAccelerationScalingFactors(final double sx, final double sy, final double sz) throws LockedException,
2211 AlgebraException {
2212 if (running) {
2213 throw new LockedException();
2214 }
2215
2216 fixer.setAccelerationScalingFactors(sx, sy, sz);
2217 driftEstimator.setAccelerationScalingFactors(sx, sy, sz);
2218 }
2219
2220 /**
2221 * Sets acceleration cross-coupling errors.
2222 *
2223 * @param mxy x-y cross coupling error.
2224 * @param mxz x-z cross coupling error.
2225 * @param myx y-x cross coupling error.
2226 * @param myz y-z cross coupling error.
2227 * @param mzx z-x cross coupling error.
2228 * @param mzy z-y cross coupling error.
2229 * @throws LockedException if estimator is running.
2230 * @throws AlgebraException if provided values make acceleration cross-coupling matrix non-invertible.
2231 */
2232 public void setAccelerationCrossCouplingErrors(
2233 final double mxy, final double mxz,
2234 final double myx, final double myz,
2235 final double mzx, final double mzy) throws LockedException, AlgebraException {
2236 if (running) {
2237 throw new LockedException();
2238 }
2239
2240 fixer.setAccelerationCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
2241 driftEstimator.setAccelerationCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
2242 }
2243
2244 /**
2245 * Sets acceleration scaling factors and cross-coupling errors.
2246 *
2247 * @param sx x scaling factor.
2248 * @param sy y scaling factor.
2249 * @param sz z scaling factor.
2250 * @param mxy x-y cross coupling error.
2251 * @param mxz x-z cross coupling error.
2252 * @param myx y-x cross coupling error.
2253 * @param myz y-z cross coupling error.
2254 * @param mzx z-x cross coupling error.
2255 * @param mzy z-y cross coupling error.
2256 * @throws LockedException if estimator is running.
2257 * @throws AlgebraException if provided values make acceleration cross-coupling matrix non-invertible.
2258 */
2259 public void setAccelerationScalingFactorsAndCrossCouplingErrors(
2260 final double sx, final double sy, final double sz,
2261 final double mxy, final double mxz,
2262 final double myx, final double myz,
2263 final double mzx, final double mzy) throws LockedException, AlgebraException {
2264 if (running) {
2265 throw new LockedException();
2266 }
2267
2268 fixer.setAccelerationScalingFactorsAndCrossCouplingErrors(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
2269 driftEstimator.setAccelerationScalingFactorsAndCrossCouplingErrors(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
2270 }
2271
2272 /**
2273 * Gets angular speed bias values expressed in radians per second (rad/s).
2274 *
2275 * @return angular speed bias values expressed in radians per second.
2276 */
2277 public Matrix getAngularSpeedBias() {
2278 return fixer.getAngularSpeedBias();
2279 }
2280
2281 /**
2282 * Gets angular speed bias values expressed in radians per second (rad/s).
2283 *
2284 * @param result instance where the result will be stored.
2285 */
2286 public void getAngularSpeedBias(final Matrix result) {
2287 fixer.getAngularSpeedBias(result);
2288 }
2289
2290 /**
2291 * Sets angular speed bias values expressed in radians per second (rad/s).
2292 *
2293 * @param bias bias values expressed in radians per second. Must be 3x1.
2294 * @throws IllegalArgumentException if matrix is not 3x1.
2295 * @throws LockedException if estimator is running.
2296 */
2297 public void setAngularSpeedBias(final Matrix bias) throws LockedException {
2298 if (running) {
2299 throw new LockedException();
2300 }
2301
2302 fixer.setAngularSpeedBias(bias);
2303 driftEstimator.setAngularSpeedBias(bias);
2304 }
2305
2306 /**
2307 * Gets angular speed bias values expressed in radians per second (rad/s).
2308 *
2309 * @return bias values expressed in radians per second.
2310 */
2311 public double[] getAngularSpeedBiasArray() {
2312 return fixer.getAngularSpeedBiasArray();
2313 }
2314
2315 /**
2316 * Gets angular speed bias values expressed in radians per second (rad/s).
2317 *
2318 * @param result instance where result data will be stored.
2319 * @throws IllegalArgumentException if provided array does not have length 3.
2320 */
2321 public void getAngularSpeedBiasArray(final double[] result) {
2322 fixer.getAngularSpeedBiasArray(result);
2323 }
2324
2325 /**
2326 * Sets angular speed bias values expressed in radians per second (rad/s).
2327 *
2328 * @param bias bias values expressed in radians per second (rad/s). Must
2329 * have length 3.
2330 * @throws IllegalArgumentException if provided array does not have length 3.
2331 * @throws LockedException if estimator is running.
2332 */
2333 public void setAngularSpeedBias(final double[] bias) throws LockedException {
2334 if (running) {
2335 throw new LockedException();
2336 }
2337
2338 fixer.setAngularSpeedBias(bias);
2339 driftEstimator.setAngularSpeedBias(bias);
2340 }
2341
2342 /**
2343 * Gets angular speed bias.
2344 *
2345 * @return angular speed bias.
2346 */
2347 public AngularSpeedTriad getAngularSpeedBiasAsTriad() {
2348 return fixer.getAngularSpeedBiasAsTriad();
2349 }
2350
2351 /**
2352 * Gets angular speed bias.
2353 *
2354 * @param result instance where the result will be stored.
2355 */
2356 public void getAngularSpeedBiasAsTriad(final AngularSpeedTriad result) {
2357 fixer.getAngularSpeedBiasAsTriad(result);
2358 }
2359
2360 /**
2361 * Sets angular speed bias.
2362 *
2363 * @param bias angular speed bias to be set.
2364 * @throws LockedException if estimator is running.
2365 */
2366 public void setAngularSpeedBias(final AngularSpeedTriad bias) throws LockedException {
2367 if (running) {
2368 throw new LockedException();
2369 }
2370
2371 fixer.setAngularSpeedBias(bias);
2372 driftEstimator.setAngularSpeedBias(bias);
2373 }
2374
2375 /**
2376 * Gets angular speed x-coordinate of bias expressed in radians per second
2377 * (rad/s).
2378 *
2379 * @return x-coordinate of bias expressed in radians per second (rad/s).
2380 */
2381 public double getAngularSpeedBiasX() {
2382 return fixer.getAngularSpeedBiasX();
2383 }
2384
2385 /**
2386 * Sets angular speed x-coordinate of bias expressed in radians per second
2387 * (rad/s).
2388 *
2389 * @param biasX x-coordinate of bias expressed in radians per second (rad/s).
2390 * @throws LockedException if estimator is running.
2391 */
2392 public void setAngularSpeedBiasX(final double biasX) throws LockedException {
2393 if (running) {
2394 throw new LockedException();
2395 }
2396
2397 fixer.setAngularSpeedBiasX(biasX);
2398 driftEstimator.setAngularSpeedBiasX(biasX);
2399 }
2400
2401 /**
2402 * Gets angular speed y-coordinate of bias expressed in radians per second
2403 * (rad/s).
2404 *
2405 * @return y-coordinate of bias expressed in radians per second (rad/s).
2406 */
2407 public double getAngularSpeedBiasY() {
2408 return fixer.getAngularSpeedBiasY();
2409 }
2410
2411 /**
2412 * Sets angular speed y-coordinate of bias expressed in radians per second
2413 * (rad/s).
2414 *
2415 * @param biasY y-coordinate of bias expressed in radians per second (rad/s).
2416 * @throws LockedException if estimator is running.
2417 */
2418 public void setAngularSpeedBiasY(final double biasY) throws LockedException {
2419 if (running) {
2420 throw new LockedException();
2421 }
2422
2423 fixer.setAngularSpeedBiasY(biasY);
2424 driftEstimator.setAngularSpeedBiasY(biasY);
2425 }
2426
2427 /**
2428 * Gets angular speed z-coordinate of bias expressed in radians per second
2429 * (rad/s).
2430 *
2431 * @return z-coordinate of bias expressed in radians per second (rad/s).
2432 */
2433 public double getAngularSpeedBiasZ() {
2434 return fixer.getAngularSpeedBiasZ();
2435 }
2436
2437 /**
2438 * Sets angular speed z-coordinate of bias expressed in radians per second
2439 * (rad/s).
2440 *
2441 * @param biasZ z-coordinate of bias expressed in radians per second (rad/s).
2442 * @throws LockedException if estimator is running.
2443 */
2444 public void setAngularSpeedBiasZ(final double biasZ) throws LockedException {
2445 if (running) {
2446 throw new LockedException();
2447 }
2448
2449 fixer.setAngularSpeedBiasZ(biasZ);
2450 driftEstimator.setAngularSpeedBiasZ(biasZ);
2451 }
2452
2453 /**
2454 * Sets angular speed coordinates of bias expressed in radians per second
2455 * (rad/s).
2456 *
2457 * @param biasX x-coordinate of bias.
2458 * @param biasY y-coordinate of bias.
2459 * @param biasZ z-coordinate of bias.
2460 * @throws LockedException if estimator is running.
2461 */
2462 public void setAngularSpeedBias(final double biasX, final double biasY, final double biasZ) throws LockedException {
2463 if (running) {
2464 throw new LockedException();
2465 }
2466
2467 fixer.setAngularSpeedBias(biasX, biasY, biasZ);
2468 driftEstimator.setAngularSpeedBias(biasX, biasY, biasZ);
2469 }
2470
2471 /**
2472 * Gets angular speed x-coordinate of bias.
2473 *
2474 * @return x-coordinate of bias.
2475 */
2476 public AngularSpeed getAngularSpeedBiasXAsAngularSpeed() {
2477 return fixer.getAngularSpeedBiasXAsAngularSpeed();
2478 }
2479
2480 /**
2481 * Gets angular speed x-coordinate of bias.
2482 *
2483 * @param result instance where the result will be stored.
2484 */
2485 public void getAngularSpeedBiasXAsAngularSpeed(final AngularSpeed result) {
2486 fixer.getAngularSpeedBiasXAsAngularSpeed(result);
2487 }
2488
2489 /**
2490 * Sets angular speed x-coordinate of bias.
2491 *
2492 * @param biasX x-coordinate of bias.
2493 * @throws LockedException if estimator is running.
2494 */
2495 public void setAngularSpeedBiasX(final AngularSpeed biasX) throws LockedException {
2496 if (running) {
2497 throw new LockedException();
2498 }
2499
2500 fixer.setAngularSpeedBiasX(biasX);
2501 driftEstimator.setAngularSpeedBiasX(biasX);
2502 }
2503
2504 /**
2505 * Gets angular speed y-coordinate of bias.
2506 *
2507 * @return y-coordinate of bias.
2508 */
2509 public AngularSpeed getAngularSpeedBiasYAsAngularSpeed() {
2510 return fixer.getAngularSpeedBiasYAsAngularSpeed();
2511 }
2512
2513 /**
2514 * Gets angular speed y-coordinate of bias.
2515 *
2516 * @param result instance where the result will be stored.
2517 */
2518 public void getAngularSpeedBiasYAsAngularSpeed(final AngularSpeed result) {
2519 fixer.getAngularSpeedBiasYAsAngularSpeed(result);
2520 }
2521
2522 /**
2523 * Sets angular speed y-coordinate of bias.
2524 *
2525 * @param biasY y-coordinate of bias.
2526 * @throws LockedException if estimator is running.
2527 */
2528 public void setAngularSpeedBiasY(final AngularSpeed biasY) throws LockedException {
2529 if (running) {
2530 throw new LockedException();
2531 }
2532
2533 fixer.setAngularSpeedBiasY(biasY);
2534 driftEstimator.setAngularSpeedBiasY(biasY);
2535 }
2536
2537 /**
2538 * Gets angular speed z-coordinate of bias.
2539 *
2540 * @return z-coordinate of bias.
2541 */
2542 public AngularSpeed getAngularSpeedBiasZAsAngularSpeed() {
2543 return fixer.getAngularSpeedBiasZAsAngularSpeed();
2544 }
2545
2546 /**
2547 * Gets angular speed z-coordinate of bias.
2548 *
2549 * @param result instance where the result will be stored.
2550 */
2551 public void getAngularSpeedBiasZAsAngularSpeed(final AngularSpeed result) {
2552 fixer.getAngularSpeedBiasZAsAngularSpeed(result);
2553 }
2554
2555 /**
2556 * Sets angular speed z-coordinate of bias.
2557 *
2558 * @param biasZ z-coordinate of bias.
2559 * @throws LockedException if estimator is running.
2560 */
2561 public void setAngularSpeedBiasZ(final AngularSpeed biasZ) throws LockedException {
2562 if (running) {
2563 throw new LockedException();
2564 }
2565
2566 fixer.setAngularSpeedBiasZ(biasZ);
2567 driftEstimator.setAngularSpeedBiasZ(biasZ);
2568 }
2569
2570 /**
2571 * Sets angular speed coordinates of bias.
2572 *
2573 * @param biasX x-coordinate of bias.
2574 * @param biasY y-coordinate of bias.
2575 * @param biasZ z-coordinate of bias.
2576 * @throws LockedException if estimator is running.
2577 */
2578 public void setAngularSpeedBias(
2579 final AngularSpeed biasX,
2580 final AngularSpeed biasY,
2581 final AngularSpeed biasZ) throws LockedException {
2582 if (running) {
2583 throw new LockedException();
2584 }
2585
2586 fixer.setAngularSpeedBias(biasX, biasY, biasZ);
2587 driftEstimator.setAngularSpeedBias(biasX, biasY, biasZ);
2588 }
2589
2590 /**
2591 * Gets angular speed cross-coupling errors matrix.
2592 *
2593 * @return cross coupling errors matrix.
2594 */
2595 public Matrix getAngularSpeedCrossCouplingErrors() {
2596 return fixer.getAngularSpeedCrossCouplingErrors();
2597 }
2598
2599 /**
2600 * Gets angular speed cross-coupling errors matrix.
2601 *
2602 * @param result instance where the result will be stored.
2603 */
2604 public void getAngularSpeedCrossCouplingErrors(final Matrix result) {
2605 fixer.getAngularSpeedCrossCouplingErrors(result);
2606 }
2607
2608 /**
2609 * Sets angular speed cross-coupling errors matrix.
2610 *
2611 * @param crossCouplingErrors cross-coupling errors matrix. Must be 3x3.
2612 * @throws AlgebraException if matrix cannot be inverted.
2613 * @throws IllegalArgumentException if matrix is not 3x3.
2614 * @throws LockedException if estimator is running.
2615 */
2616 public void setAngularSpeedCrossCouplingErrors(final Matrix crossCouplingErrors) throws AlgebraException,
2617 LockedException {
2618 if (running) {
2619 throw new LockedException();
2620 }
2621
2622 fixer.setAngularSpeedCrossCouplingErrors(crossCouplingErrors);
2623 driftEstimator.setAngularSpeedCrossCouplingErrors(crossCouplingErrors);
2624 }
2625
2626 /**
2627 * Gets angular speed x scaling factor.
2628 *
2629 * @return x scaling factor.
2630 */
2631 public double getAngularSpeedSx() {
2632 return fixer.getAngularSpeedSx();
2633 }
2634
2635 /**
2636 * Sets angular speed x scaling factor.
2637 *
2638 * @param sx x scaling factor.
2639 * @throws LockedException if estimator is running.
2640 * @throws AlgebraException if provided value makes angular speed-cross coupling matrix non-invertible.
2641 */
2642 public void setAngularSpeedSx(final double sx) throws LockedException, AlgebraException {
2643 if (running) {
2644 throw new LockedException();
2645 }
2646
2647 fixer.setAngularSpeedSx(sx);
2648 driftEstimator.setAngularSpeedSx(sx);
2649 }
2650
2651 /**
2652 * Gets angular speed y scaling factor.
2653 *
2654 * @return y scaling factor.
2655 */
2656 public double getAngularSpeedSy() {
2657 return fixer.getAngularSpeedSy();
2658 }
2659
2660 /**
2661 * Sets angular speed y-scaling factor.
2662 *
2663 * @param sy y scaling factor.
2664 * @throws LockedException if estimator is running.
2665 * @throws AlgebraException if provided value makes angular speed
2666 * cross-coupling matrix non-invertible.
2667 */
2668 public void setAngularSpeedSy(final double sy) throws LockedException, AlgebraException {
2669 if (running) {
2670 throw new LockedException();
2671 }
2672
2673 fixer.setAngularSpeedSy(sy);
2674 driftEstimator.setAngularSpeedSy(sy);
2675 }
2676
2677 /**
2678 * Gets angular speed z scaling factor.
2679 *
2680 * @return z scaling factor.
2681 */
2682 public double getAngularSpeedSz() {
2683 return fixer.getAngularSpeedSz();
2684 }
2685
2686 /**
2687 * Sets angular speed z scaling factor.
2688 *
2689 * @param sz z scaling factor.
2690 * @throws LockedException if estimator is running.
2691 * @throws AlgebraException if provided value makes angular speed
2692 * cross-coupling matrix non-invertible.
2693 */
2694 public void setAngularSpeedSz(final double sz) throws LockedException, AlgebraException {
2695 if (running) {
2696 throw new LockedException();
2697 }
2698
2699 fixer.setAngularSpeedSz(sz);
2700 driftEstimator.setAngularSpeedSz(sz);
2701 }
2702
2703 /**
2704 * Gets angular speed x-y cross-coupling error.
2705 *
2706 * @return x-y cross coupling error.
2707 */
2708 public double getAngularSpeedMxy() {
2709 return fixer.getAngularSpeedMxy();
2710 }
2711
2712 /**
2713 * Sets angular speed x-y cross-coupling error.
2714 *
2715 * @param mxy x-y cross coupling error.
2716 * @throws LockedException if estimator is running.
2717 * @throws AlgebraException if provided value makes angular speed
2718 * cross-coupling matrix non-invertible.
2719 */
2720 public void setAngularSpeedMxy(final double mxy) throws LockedException, AlgebraException {
2721 if (running) {
2722 throw new LockedException();
2723 }
2724
2725 fixer.setAngularSpeedMxy(mxy);
2726 driftEstimator.setAngularSpeedMxy(mxy);
2727 }
2728
2729 /**
2730 * Gets angular speed x-z cross-coupling error.
2731 *
2732 * @return x-z cross coupling error.
2733 */
2734 public double getAngularSpeedMxz() {
2735 return fixer.getAngularSpeedMxz();
2736 }
2737
2738 /**
2739 * Sets angular speed x-z cross-coupling error.
2740 *
2741 * @param mxz x-z cross coupling error.
2742 * @throws LockedException if estimator is running.
2743 * @throws AlgebraException if provided value makes angular speed
2744 * cross-coupling matrix non-invertible.
2745 */
2746 public void setAngularSpeedMxz(final double mxz) throws LockedException, AlgebraException {
2747 if (running) {
2748 throw new LockedException();
2749 }
2750
2751 fixer.setAngularSpeedMxz(mxz);
2752 driftEstimator.setAngularSpeedMxz(mxz);
2753 }
2754
2755 /**
2756 * Gets angular speed y-x cross-coupling error.
2757 *
2758 * @return y-x cross coupling error.
2759 */
2760 public double getAngularSpeedMyx() {
2761 return fixer.getAngularSpeedMyx();
2762 }
2763
2764 /**
2765 * Sets angular speed y-x cross-coupling error.
2766 *
2767 * @param myx y-x cross coupling error.
2768 * @throws LockedException if estimator is running.
2769 * @throws AlgebraException if provided value makes angular speed
2770 * cross-coupling matrix non-invertible.
2771 */
2772 public void setAngularSpeedMyx(final double myx) throws LockedException, AlgebraException {
2773 if (running) {
2774 throw new LockedException();
2775 }
2776
2777 fixer.setAngularSpeedMyx(myx);
2778 driftEstimator.setAngularSpeedMyx(myx);
2779 }
2780
2781 /**
2782 * Gets angular speed y-z cross-coupling error.
2783 *
2784 * @return y-z cross coupling error.
2785 */
2786 public double getAngularSpeedMyz() {
2787 return fixer.getAngularSpeedMyz();
2788 }
2789
2790 /**
2791 * Sets angular speed y-z cross-coupling error.
2792 *
2793 * @param myz y-z cross coupling error.
2794 * @throws LockedException if estimator is running.
2795 * @throws AlgebraException if provided value makes angular speed
2796 * cross-coupling matrix non-invertible.
2797 */
2798 public void setAngularSpeedMyz(final double myz) throws LockedException, AlgebraException {
2799 if (running) {
2800 throw new LockedException();
2801 }
2802
2803 fixer.setAngularSpeedMyz(myz);
2804 driftEstimator.setAngularSpeedMyz(myz);
2805 }
2806
2807 /**
2808 * Gets angular speed z-x cross-coupling error.
2809 *
2810 * @return z-x cross coupling error.
2811 */
2812 public double getAngularSpeedMzx() {
2813 return fixer.getAngularSpeedMzx();
2814 }
2815
2816 /**
2817 * Sets angular speed z-x cross-coupling error.
2818 *
2819 * @param mzx z-x cross coupling error.
2820 * @throws LockedException if estimator is running.
2821 * @throws AlgebraException if provided value makes angular speed
2822 * cross-coupling matrix non-invertible.
2823 */
2824 public void setAngularSpeedMzx(final double mzx) throws LockedException, AlgebraException {
2825 if (running) {
2826 throw new LockedException();
2827 }
2828
2829 fixer.setAngularSpeedMzx(mzx);
2830 driftEstimator.setAngularSpeedMzx(mzx);
2831 }
2832
2833 /**
2834 * Gets angular speed z-y cross-coupling error.
2835 *
2836 * @return z-y cross coupling error.
2837 */
2838 public double getAngularSpeedMzy() {
2839 return fixer.getAngularSpeedMzy();
2840 }
2841
2842 /**
2843 * Sets angular speed z-y cross-coupling error.
2844 *
2845 * @param mzy z-y cross coupling error.
2846 * @throws LockedException if estimator is running.
2847 * @throws AlgebraException if provided value makes angular speed
2848 * cross-coupling matrix non-invertible.
2849 */
2850 public void setAngularSpeedMzy(final double mzy) throws LockedException, AlgebraException {
2851 if (running) {
2852 throw new LockedException();
2853 }
2854
2855 fixer.setAngularSpeedMzy(mzy);
2856 driftEstimator.setAngularSpeedMzy(mzy);
2857 }
2858
2859 /**
2860 * Sets angular speed scaling factors.
2861 *
2862 * @param sx x scaling factor.
2863 * @param sy y scaling factor.
2864 * @param sz z scaling factor.
2865 * @throws LockedException if estimator is running.
2866 * @throws AlgebraException if provided values make angular speed
2867 * cross-coupling matrix non-invertible.
2868 */
2869 public void setAngularSpeedScalingFactors(final double sx, final double sy, final double sz) throws LockedException,
2870 AlgebraException {
2871 if (running) {
2872 throw new LockedException();
2873 }
2874
2875 fixer.setAngularSpeedScalingFactors(sx, sy, sz);
2876 driftEstimator.setAngularSpeedScalingFactors(sx, sy, sz);
2877 }
2878
2879 /**
2880 * Sets angular speed cross-coupling errors.
2881 *
2882 * @param mxy x-y cross coupling error.
2883 * @param mxz x-z cross coupling error.
2884 * @param myx y-x cross coupling error.
2885 * @param myz y-z cross coupling error.
2886 * @param mzx z-x cross coupling error.
2887 * @param mzy z-y cross coupling error.
2888 * @throws LockedException if estimator is running.
2889 * @throws AlgebraException if provided values make angular speed
2890 * cross-coupling matrix non-invertible.
2891 */
2892 public void setAngularSpeedCrossCouplingErrors(
2893 final double mxy, final double mxz,
2894 final double myx, final double myz,
2895 final double mzx, final double mzy) throws LockedException, AlgebraException {
2896 if (running) {
2897 throw new LockedException();
2898 }
2899
2900 fixer.setAngularSpeedCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
2901 driftEstimator.setAngularSpeedCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
2902 }
2903
2904 /**
2905 * Sets angular speed scaling factors and cross-coupling errors.
2906 *
2907 * @param sx x scaling factor.
2908 * @param sy y scaling factor.
2909 * @param sz z scaling factor.
2910 * @param mxy x-y cross coupling error.
2911 * @param mxz x-z cross coupling error.
2912 * @param myx y-x cross coupling error.
2913 * @param myz y-z cross coupling error.
2914 * @param mzx z-x cross coupling error.
2915 * @param mzy z-y cross coupling error.
2916 * @throws LockedException if estimator is running.
2917 * @throws AlgebraException if provided values make angular speed
2918 * cross-coupling matrix non-invertible.
2919 */
2920 public void setAngularSpeedScalingFactorsAndCrossCouplingErrors(
2921 final double sx, final double sy, final double sz,
2922 final double mxy, final double mxz,
2923 final double myx, final double myz,
2924 final double mzx, final double mzy) throws LockedException, AlgebraException {
2925 if (running) {
2926 throw new LockedException();
2927 }
2928
2929 fixer.setAngularSpeedScalingFactorsAndCrossCouplingErrors(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
2930 driftEstimator.setAngularSpeedScalingFactorsAndCrossCouplingErrors(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
2931 }
2932
2933 /**
2934 * Gets angular speed g-dependant cross biases matrix.
2935 *
2936 * @return g-dependant cross biases matrix.
2937 */
2938 public Matrix getAngularSpeedGDependantCrossBias() {
2939 return fixer.getAngularSpeedGDependantCrossBias();
2940 }
2941
2942 /**
2943 * Gets angular speed g-dependant cross biases matrix.
2944 *
2945 * @param result instance where the result will be stored.
2946 */
2947 public void getAngularSpeedGDependantCrossBias(final Matrix result) {
2948 fixer.getAngularSpeedGDependantCrossBias(result);
2949 }
2950
2951 /**
2952 * Sets angular speed g-dependant cross biases matrix.
2953 *
2954 * @param gDependantCrossBias g-dependant cross biases matrix.
2955 * @throws IllegalArgumentException if provided, matrix is not 3x3.
2956 * @throws LockedException if estimator is running.
2957 */
2958 public void setAngularSpeedGDependantCrossBias(final Matrix gDependantCrossBias) throws LockedException {
2959 if (running) {
2960 throw new LockedException();
2961 }
2962
2963 fixer.setAngularSpeedGDependantCrossBias(gDependantCrossBias);
2964 driftEstimator.setAngularSpeedGDependantCrossBias(gDependantCrossBias);
2965 }
2966
2967 /**
2968 * Gets the time interval between body kinematics (IMU acceleration and gyroscope)
2969 * samples expressed in seconds (s).
2970 *
2971 * @return time interval between body kinematics samples.
2972 */
2973 public double getTimeInterval() {
2974 return biasEstimator.getTimeInterval();
2975 }
2976
2977 /**
2978 * Sets a time interval between body kinematics (IMU acceleration and gyroscope)
2979 * samples expressed in seconds (s).
2980 *
2981 * @param timeInterval time interval between body kinematics samples.
2982 * @throws LockedException if estimator is currently running.
2983 */
2984 public void setTimeInterval(final double timeInterval) throws LockedException {
2985 if (running) {
2986 throw new LockedException();
2987 }
2988
2989 biasEstimator.setTimeInterval(timeInterval);
2990 driftEstimator.setTimeInterval(timeInterval);
2991 }
2992
2993 /**
2994 * Gets time interval between body kinematics (IMU acceleration and gyroscope)
2995 * samples.
2996 *
2997 * @return time interval between body kinematics samples.
2998 */
2999 public Time getTimeIntervalAsTime() {
3000 return biasEstimator.getTimeIntervalAsTime();
3001 }
3002
3003 /**
3004 * Gets time interval between body kinematics (IMU acceleration and gyroscope)
3005 * samples.
3006 *
3007 * @param result instance where the time interval will be stored.
3008 */
3009 public void getTimeIntervalAsTime(final Time result) {
3010 biasEstimator.getTimeIntervalAsTime(result);
3011 }
3012
3013 /**
3014 * Sets time interval between body kinematics (IMU acceleration and gyroscope)
3015 * samples.
3016 *
3017 * @param timeInterval time interval between body kinematics samples.
3018 * @throws LockedException if estimator is currently running.
3019 */
3020 public void setTimeInterval(final Time timeInterval) throws LockedException {
3021 if (running) {
3022 throw new LockedException();
3023 }
3024
3025 biasEstimator.setTimeInterval(timeInterval);
3026 driftEstimator.setTimeInterval(timeInterval);
3027 }
3028
3029 /**
3030 * Gets current body position expressed in ECEF coordinates.
3031 *
3032 * @return current body position expressed in ECEF coordinates.
3033 */
3034 public ECEFPosition getEcefPosition() {
3035 return biasEstimator.getEcefPosition();
3036 }
3037
3038 /**
3039 * Gets current body position expressed in ECEF coordinates.
3040 *
3041 * @param result instance where current body position will be stored.
3042 */
3043 public void getEcefPosition(final ECEFPosition result) {
3044 biasEstimator.getEcefPosition(result);
3045 }
3046
3047 /**
3048 * Sets current body position expressed in ECEF coordinates.
3049 *
3050 * @param position current body position to be set.
3051 * @throws LockedException if estimator is currently running.
3052 */
3053 public void setEcefPosition(final ECEFPosition position) throws LockedException {
3054 if (running) {
3055 throw new LockedException();
3056 }
3057
3058 biasEstimator.setEcefPosition(position);
3059 driftEstimator.setReferenceEcefPosition(position);
3060 }
3061
3062 /**
3063 * Sets current body position expressed in ECEF coordinates.
3064 *
3065 * @param x x position resolved around ECEF axes and expressed in meters (m).
3066 * @param y y position resolved around ECEF axes and expressed in meters (m).
3067 * @param z z position resolved around ECEF axes and expressed in meters (m).
3068 * @throws LockedException if estimator is currently running.
3069 */
3070 public void setEcefPosition(final double x, final double y, final double z) throws LockedException {
3071 if (running) {
3072 throw new LockedException();
3073 }
3074
3075 biasEstimator.setEcefPosition(x, y, z);
3076 driftEstimator.setReferenceEcefPosition(biasEstimator.getEcefPosition());
3077 }
3078
3079 /**
3080 * Sets current body position expressed in ECEF coordinates.
3081 *
3082 * @param x x position resolved around ECEF axes.
3083 * @param y y position resolved around ECEF axes.
3084 * @param z z position resolved around ECEF axes.
3085 * @throws LockedException if estimator is currently running.
3086 */
3087 public void setEcefPosition(final Distance x, final Distance y, final Distance z) throws LockedException {
3088 if (running) {
3089 throw new LockedException();
3090 }
3091
3092 biasEstimator.setEcefPosition(x, y, z);
3093 driftEstimator.setReferenceEcefPosition(biasEstimator.getEcefPosition());
3094 }
3095
3096 /**
3097 * Sets current body position expressed in ECEF coordinates.
3098 *
3099 * @param position position resolved around ECEF axes and expressed in meters (m).
3100 * @throws LockedException if estimator is currently running.
3101 */
3102 public void setEcefPosition(final Point3D position) throws LockedException {
3103 if (running) {
3104 throw new LockedException();
3105 }
3106
3107 biasEstimator.setEcefPosition(position);
3108 driftEstimator.setReferenceEcefPosition(biasEstimator.getEcefPosition());
3109 }
3110
3111 /**
3112 * Gets ECEF frame containing current body position and orientation expressed
3113 * in ECEF coordinates. Frame also contains body velocity, but it is always
3114 * assumed to be zero during calibration.
3115 *
3116 * @return ECEF frame containing current body position and orientation resolved
3117 * around ECEF axes.
3118 */
3119 public ECEFFrame getEcefFrame() {
3120 return biasEstimator.getEcefFrame();
3121 }
3122
3123 /**
3124 * Gets ECEF frame containing current body position and orientation expressed
3125 * in ECEF coordinates. Frame also contains body velocity, but it is always
3126 * assumed to be zero during calibration.
3127 *
3128 * @param result instance where ECEF frame containing current body position and
3129 * orientation resolved around ECEF axes will be stored.
3130 */
3131 public void getEcefFrame(final ECEFFrame result) {
3132 biasEstimator.getEcefFrame(result);
3133 }
3134
3135 /**
3136 * Gets NED frame containing current body position and orientation expressed
3137 * in NED coordinates. Frame also contains body velocity, but it is always
3138 * assumed to be zero during calibration.
3139 *
3140 * @return NED frame containing current body position and orientation resolved
3141 * around NED axes.
3142 */
3143 public NEDFrame getNedFrame() {
3144 return biasEstimator.getNedFrame();
3145 }
3146
3147 /**
3148 * Gets NED frame containing current body position and orientation expressed
3149 * in NED coordinates. Frame also contains body velocity, but it is always
3150 * assumed to be zero during calibration.
3151 *
3152 * @param result instance where NED frame containing current body position and
3153 * orientation resolved around NED axes will be stored.
3154 */
3155 public void getNedFrame(final NEDFrame result) {
3156 biasEstimator.getNedFrame(result);
3157 }
3158
3159 /**
3160 * Gets current body position expressed in NED coordinates.
3161 *
3162 * @return current body position expressed in NED coordinates.
3163 */
3164 public NEDPosition getNedPosition() {
3165 return biasEstimator.getNedPosition();
3166 }
3167
3168 /**
3169 * Gets current body position expressed in NED coordinates.
3170 *
3171 * @param result instance where current body position will be stored.
3172 */
3173 public void getNedPosition(final NEDPosition result) {
3174 biasEstimator.getNedPosition(result);
3175 }
3176
3177 /**
3178 * Sets current body position expressed in NED coordinates.
3179 *
3180 * @param position current body position to be set.
3181 * @throws LockedException if estimator is currently running.
3182 */
3183 public void setNedPosition(final NEDPosition position) throws LockedException {
3184 if (running) {
3185 throw new LockedException();
3186 }
3187
3188 biasEstimator.setNedPosition(position);
3189 driftEstimator.setReferenceNedPosition(position);
3190 }
3191
3192 /**
3193 * Sets current body position expressed in NED coordinates.
3194 *
3195 * @param latitude latitude NED coordinate expressed in radians (rad).
3196 * @param longitude longitude NED coordinate expressed in radians (rad).
3197 * @param height height NED coordinate expressed in meters (m).
3198 * @throws LockedException if estimator is currently running.
3199 */
3200 public void setNedPosition(final double latitude, final double longitude, final double height)
3201 throws LockedException {
3202 if (running) {
3203 throw new LockedException();
3204 }
3205
3206 biasEstimator.setNedPosition(latitude, longitude, height);
3207 driftEstimator.setReferenceEcefPosition(biasEstimator.getEcefPosition());
3208 }
3209
3210 /**
3211 * Sets current body position expressed in NED coordinates.
3212 *
3213 * @param latitude latitude NED coordinate.
3214 * @param longitude longitude NED coordinate.
3215 * @param height height NED coordinate expressed in meters (m).
3216 * @throws LockedException if estimator is currently running.
3217 */
3218 public void setNedPosition(final Angle latitude, final Angle longitude, final double height)
3219 throws LockedException {
3220 if (running) {
3221 throw new LockedException();
3222 }
3223
3224 biasEstimator.setNedPosition(latitude, longitude, height);
3225 driftEstimator.setReferenceEcefPosition(biasEstimator.getEcefPosition());
3226 }
3227
3228 /**
3229 * Sets current body position expressed in NED coordinates.
3230 *
3231 * @param latitude latitude NED coordinate.
3232 * @param longitude longitude NED coordinate.
3233 * @param height height NED coordinate.
3234 * @throws LockedException if estimator is currently running.
3235 */
3236 public void setNedPosition(final Angle latitude, final Angle longitude, final Distance height)
3237 throws LockedException {
3238 if (running) {
3239 throw new LockedException();
3240 }
3241
3242 biasEstimator.setNedPosition(latitude, longitude, height);
3243 driftEstimator.setReferenceEcefPosition(biasEstimator.getEcefPosition());
3244 }
3245
3246 /**
3247 * Gets current body orientation as a transformation from body to ECEF coordinates.
3248 * Notice that returned orientation refers to ECEF Earth axes, which means that
3249 * orientation is not relative to the ground or horizon at current body position.
3250 * Typically, it is more convenient to use {@link #getNedC()} to get orientation
3251 * relative to the ground or horizon at current body position. For instance, on
3252 * Android devices a NED orientation with Euler angles (roll = 0, pitch = 0,
3253 * yaw = 0) means that the device is laying flat on a horizontal surface with the
3254 * screen facing down towards the ground.
3255 *
3256 * @return current body orientation resolved on ECEF axes.
3257 */
3258 public CoordinateTransformation getEcefC() {
3259 return biasEstimator.getEcefC();
3260 }
3261
3262 /**
3263 * Gets current body orientation as a transformation from body to ECEF coordinates.
3264 * Notice that returned orientation refers to ECEF Earth axes, which means that
3265 * orientation is not relative to the ground or horizon at current body position.
3266 * Typically, it is more convenient to use {@link #getNedC()} to get orientation
3267 * relative to the ground or horizon at current body position. For instance, on
3268 * Android devices a NED orientation with Euler angles (roll = 0, pitch = 0,
3269 * yaw = 0) means that the device is laying flat on a horizontal surface with the
3270 * screen facing down towards the ground.
3271 *
3272 * @param result instance where current body orientation resolved on ECEF axes
3273 * will be stored.
3274 */
3275 public void getEcefC(final CoordinateTransformation result) {
3276 biasEstimator.getEcefC(result);
3277 }
3278
3279 /**
3280 * Sets current body orientation as a transformation from body to ECEF coordinates.
3281 * Notice that ECEF orientation refers to ECEF Earth axes, which means that
3282 * orientation is not relative to the ground or horizon at current body position.
3283 * Typically, it is more convenient to use
3284 * {@link #setNedC(CoordinateTransformation)} to specify orientation relative to
3285 * the ground or horizon at current body position.
3286 * For instance, on Android devices a NED orientation with Euler angles (roll = 0,
3287 * pitch = 0, yaw = 0) means that the device is laying flat on a horizontal surface
3288 * with the screen facing down towards the ground.
3289 *
3290 * @param ecefC body orientation resolved on ECEF axes to be set.
3291 * @throws InvalidSourceAndDestinationFrameTypeException if coordinate
3292 * transformation is not from
3293 * body to ECEF coordinates.
3294 * @throws LockedException if estimator is currently
3295 * running.
3296 */
3297 public void setEcefC(final CoordinateTransformation ecefC) throws InvalidSourceAndDestinationFrameTypeException,
3298 LockedException {
3299 if (running) {
3300 throw new LockedException();
3301 }
3302
3303 biasEstimator.setEcefC(ecefC);
3304 driftEstimator.setReferenceEcefCoordinateTransformation(ecefC);
3305 }
3306
3307 /**
3308 * Gets current body orientation as a transformation from body to NED coordinates.
3309 * Notice that returned orientation refers to the current local position. This means
3310 * that two equal NED orientations will transform into different ECEF orientations
3311 * if the body is located at different positions.
3312 * As a reference, on Android devices a NED orientation with Euler angles
3313 * (roll = 0, pitch = 0, yaw = 0) means that the device is laying flat on a
3314 * horizontal surface with the screen facing down towards the ground.
3315 *
3316 * @return current body orientation resolved on NED axes.
3317 */
3318 public CoordinateTransformation getNedC() {
3319 return biasEstimator.getNedC();
3320 }
3321
3322 /**
3323 * Gets current body orientation as a transformation from body to NED coordinates.
3324 * Notice that returned orientation refers to the current local position. This means
3325 * that two equal NED orientations will transform into different ECEF orientations
3326 * if the body is located at different positions.
3327 * As a reference, on Android devices a NED orientation with Euler angles
3328 * (roll = 0, pitch = 0, yaw = 0) means that the device is laying flat on a
3329 * horizontal surface with the screen facing down towards the ground.
3330 *
3331 * @param result instance where current body orientation resolved on NED axes
3332 * will be stored.
3333 */
3334 public void getNedC(final CoordinateTransformation result) {
3335 biasEstimator.getNedC(result);
3336 }
3337
3338 /**
3339 * Sets current body orientation as a transformation from body to NED coordinates.
3340 * Notice that the provided orientation refers to the current local position. This means
3341 * that two equal NED orientations will transform into different ECEF orientations
3342 * if the body is located at different positions.
3343 * As a reference, on Android devices a NED orientation with Euler angles
3344 * (roll = 0, pitch = 0, yaw = 0) means that the device is laying flat on a
3345 * horizontal surface with the screen facing down towards the ground.
3346 *
3347 * @param nedC orientation resolved on NED axes to be set.
3348 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
3349 * transformation is not from
3350 * body to NED coordinates.
3351 * @throws LockedException if estimator is currently
3352 * running.
3353 */
3354 public void setNedC(final CoordinateTransformation nedC) throws InvalidSourceAndDestinationFrameTypeException,
3355 LockedException {
3356 if (running) {
3357 throw new LockedException();
3358 }
3359
3360 biasEstimator.setNedC(nedC);
3361 driftEstimator.setReferenceNedCoordinateTransformation(nedC);
3362 }
3363
3364 /**
3365 * Sets position and orientation both expressed on NED coordinates.
3366 *
3367 * @param nedPosition position expressed on NED coordinates.
3368 * @param nedC body to NED coordinate transformation indicating
3369 * body orientation.
3370 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
3371 * transformation is not from
3372 * body to NED coordinates.
3373 * @throws LockedException if estimator is currently
3374 * running.
3375 * @see #setNedPosition(NEDPosition)
3376 * @see #setNedC(CoordinateTransformation)
3377 */
3378 public void setNedPositionAndNedOrientation(final NEDPosition nedPosition, final CoordinateTransformation nedC)
3379 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3380 if (running) {
3381 throw new LockedException();
3382 }
3383
3384 biasEstimator.setNedPositionAndNedOrientation(nedPosition, nedC);
3385 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3386 }
3387
3388 /**
3389 * Sets position and orientation both expressed on NED coordinates.
3390 *
3391 * @param latitude latitude expressed in radians (rad).
3392 * @param longitude longitude expressed in radians (rad).
3393 * @param height height expressed in meters (m).
3394 * @param nedC body to NED coordinate transformation indicating
3395 * body orientation.
3396 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
3397 * transformation is not from
3398 * body to NED coordinates.
3399 * @throws LockedException if estimator is currently
3400 * running.
3401 * @see #setNedPosition(double, double, double)
3402 * @see #setNedC(CoordinateTransformation)
3403 */
3404 public void setNedPositionAndNedOrientation(
3405 final double latitude, final double longitude, final double height, final CoordinateTransformation nedC)
3406 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3407 if (running) {
3408 throw new LockedException();
3409 }
3410
3411 biasEstimator.setNedPositionAndNedOrientation(latitude, longitude, height, nedC);
3412 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3413 }
3414
3415 /**
3416 * Sets position and orientation both expressed on NED coordinates.
3417 *
3418 * @param latitude latitude.
3419 * @param longitude longitude.
3420 * @param height height expressed in meters (m).
3421 * @param nedC body to NED coordinate transformation indicating
3422 * body orientation.
3423 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
3424 * transformation is not from
3425 * body to NED coordinates.
3426 * @throws LockedException if estimator is currently
3427 * running.
3428 * @see #setNedPosition(Angle, Angle, double)
3429 * @see #setNedC(CoordinateTransformation)
3430 */
3431 public void setNedPositionAndNedOrientation(
3432 final Angle latitude, final Angle longitude, final double height, final CoordinateTransformation nedC)
3433 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3434 if (running) {
3435 throw new LockedException();
3436 }
3437
3438 biasEstimator.setNedPositionAndNedOrientation(latitude, longitude, height, nedC);
3439 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3440 }
3441
3442 /**
3443 * Sets position and orientation both expressed on NED coordinates.
3444 *
3445 * @param latitude latitude.
3446 * @param longitude longitude.
3447 * @param height height.
3448 * @param nedC body to NED coordinate transformation indicating
3449 * body orientation.
3450 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
3451 * transformation is not from
3452 * body to NED coordinates.
3453 * @throws LockedException if estimator is currently
3454 * running.
3455 * @see #setNedPosition(Angle, Angle, Distance)
3456 * @see #setNedC(CoordinateTransformation)
3457 */
3458 public void setNedPositionAndNedOrientation(
3459 final Angle latitude, final Angle longitude, final Distance height, final CoordinateTransformation nedC)
3460 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3461 if (running) {
3462 throw new LockedException();
3463 }
3464
3465 biasEstimator.setNedPositionAndNedOrientation(latitude, longitude, height, nedC);
3466 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3467 }
3468
3469 /**
3470 * Sets position and orientation both expressed on ECEF coordinates.
3471 *
3472 * @param ecefPosition position expressed on ECEF coordinates.
3473 * @param ecefC body to ECEF coordinate transformation indicating body
3474 * orientation.
3475 * @throws InvalidSourceAndDestinationFrameTypeException if coordinate
3476 * transformation is not from
3477 * body to ECEF coordinates.
3478 * @throws LockedException if estimator is currently
3479 * running.
3480 * @see #setEcefPosition(ECEFPosition)
3481 * @see #setEcefC(CoordinateTransformation)
3482 */
3483 public void setEcefPositionAndEcefOrientation(
3484 final ECEFPosition ecefPosition, final CoordinateTransformation ecefC)
3485 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3486 if (running) {
3487 throw new LockedException();
3488 }
3489
3490 biasEstimator.setEcefPositionAndEcefOrientation(ecefPosition, ecefC);
3491 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3492 }
3493
3494 /**
3495 * Sets position and orientation both expressed on ECEF coordinates.
3496 *
3497 * @param x x coordinate of ECEF position expressed in meters (m).
3498 * @param y y coordinate of ECEF position expressed in meters (m).
3499 * @param z z coordinate of ECEF position expressed in meters (m).
3500 * @param ecefC body to ECEF coordinate transformation indicating body
3501 * orientation.
3502 * @throws InvalidSourceAndDestinationFrameTypeException if coordinate
3503 * transformation is not from
3504 * body to ECEF coordinates.
3505 * @throws LockedException if estimator is currently
3506 * running.
3507 * @see #setEcefPosition(double, double, double)
3508 * @see #setEcefC(CoordinateTransformation)
3509 */
3510 public void setEcefPositionAndEcefOrientation(
3511 final double x, final double y, final double z, final CoordinateTransformation ecefC)
3512 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3513 if (running) {
3514 throw new LockedException();
3515 }
3516
3517 biasEstimator.setEcefPositionAndEcefOrientation(x, y, z, ecefC);
3518 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3519 }
3520
3521 /**
3522 * Sets position and orientation both expressed on ECEF coordinates.
3523 *
3524 * @param x x coordinate of ECEF position.
3525 * @param y y coordinate of ECEF position.
3526 * @param z z coordinate of ECEF position.
3527 * @param ecefC body to ECEF coordinate transformation indicating body
3528 * orientation.
3529 * @throws InvalidSourceAndDestinationFrameTypeException if coordinate
3530 * transformation is not from
3531 * body to ECEF coordinates.
3532 * @throws LockedException if estimator is currently
3533 * running.
3534 * @see #setEcefPosition(Distance, Distance, Distance)
3535 * @see #setEcefC(CoordinateTransformation)
3536 */
3537 public void setEcefPositionAndEcefOrientation(
3538 final Distance x, final Distance y, final Distance z, final CoordinateTransformation ecefC)
3539 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3540 if (running) {
3541 throw new LockedException();
3542 }
3543
3544 biasEstimator.setEcefPositionAndEcefOrientation(x, y, z, ecefC);
3545 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3546 }
3547
3548 /**
3549 * Sets position and orientation both expressed on ECEF coordinates.
3550 *
3551 * @param position position resolved around ECEF axes and expressed in meters (m).
3552 * @param ecefC body to ECEF coordinate transformation indicating body
3553 * orientation.
3554 * @throws InvalidSourceAndDestinationFrameTypeException if coordinate
3555 * transformation is not from
3556 * body to ECEF coordinates.
3557 * @throws LockedException if estimator is currently
3558 * running.
3559 * @see #setEcefPosition(Point3D)
3560 * @see #setEcefC(CoordinateTransformation)
3561 */
3562 public void setEcefPositionAndEcefOrientation(final Point3D position, final CoordinateTransformation ecefC)
3563 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3564 if (running) {
3565 throw new LockedException();
3566 }
3567
3568 biasEstimator.setEcefPositionAndEcefOrientation(position, ecefC);
3569 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3570 }
3571
3572 /**
3573 * Sets position expressed on NED coordinates and orientation with respect to ECEF
3574 * axes.
3575 *
3576 * @param position position expressed on NED coordinates.
3577 * @param ecefC body to ECEF coordinate transformation indicating body
3578 * orientation.
3579 * @throws InvalidSourceAndDestinationFrameTypeException if coordinate
3580 * transformation is not from
3581 * body to ECEF coordinates.
3582 * @throws LockedException if estimator is currently
3583 * running.
3584 * @see #setNedPosition(NEDPosition)
3585 * @see #setEcefC(CoordinateTransformation)
3586 */
3587 public void setNedPositionAndEcefOrientation(
3588 final NEDPosition position, final CoordinateTransformation ecefC)
3589 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3590 if (running) {
3591 throw new LockedException();
3592 }
3593
3594 biasEstimator.setNedPositionAndEcefOrientation(position, ecefC);
3595 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3596 }
3597
3598 /**
3599 * Sets position expressed on NED coordinates and orientation with respect to ECEF
3600 * axes.
3601 *
3602 * @param latitude latitude expressed in radians (rad).
3603 * @param longitude longitude expressed in radians (rad).
3604 * @param height height expressed in meters (m).
3605 * @param ecefC body to ECEF coordinate transformation indicating body
3606 * orientation.
3607 * @throws InvalidSourceAndDestinationFrameTypeException if coordinate
3608 * transformation is not from
3609 * body to ECEF coordinates.
3610 * @throws LockedException if estimator is currently
3611 * running.
3612 * @see #setNedPosition(double, double, double)
3613 * @see #setEcefC(CoordinateTransformation)
3614 */
3615 public void setNedPositionAndEcefOrientation(final double latitude, final double longitude, final double height,
3616 final CoordinateTransformation ecefC) throws InvalidSourceAndDestinationFrameTypeException,
3617 LockedException {
3618 if (running) {
3619 throw new LockedException();
3620 }
3621
3622 biasEstimator.setNedPositionAndEcefOrientation(latitude, longitude, height, ecefC);
3623 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3624 }
3625
3626 /**
3627 * Sets position expressed on NED coordinates and orientation with respect to ECEF
3628 * axes.
3629 *
3630 * @param latitude latitude.
3631 * @param longitude longitude.
3632 * @param height height expressed in meters (m).
3633 * @param ecefC body to ECEF coordinate transformation indicating body
3634 * orientation.
3635 * @throws InvalidSourceAndDestinationFrameTypeException if coordinate
3636 * transformation is not from
3637 * body to ECEF coordinates.
3638 * @throws LockedException if estimator is currently
3639 * running.
3640 * @see #setNedPosition(Angle, Angle, double)
3641 * @see #setEcefC(CoordinateTransformation)
3642 */
3643 public void setNedPositionAndEcefOrientation(
3644 final Angle latitude, final Angle longitude, final double height, final CoordinateTransformation ecefC)
3645 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3646 if (running) {
3647 throw new LockedException();
3648 }
3649
3650 biasEstimator.setNedPositionAndEcefOrientation(latitude, longitude, height, ecefC);
3651 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3652 }
3653
3654 /**
3655 * Sets position expressed on NED coordinates and orientation with respect to ECEF
3656 * axes.
3657 *
3658 * @param latitude latitude.
3659 * @param longitude longitude.
3660 * @param height height.
3661 * @param ecefC body to ECEF coordinate transformation indicating body
3662 * orientation.
3663 * @throws InvalidSourceAndDestinationFrameTypeException if coordinate
3664 * transformation is not from
3665 * body to ECEF coordinates.
3666 * @throws LockedException if estimator is currently
3667 * running.
3668 * @see #setNedPosition(Angle, Angle, Distance)
3669 * @see #setEcefC(CoordinateTransformation)
3670 */
3671 public void setNedPositionAndEcefOrientation(
3672 final Angle latitude, final Angle longitude, final Distance height, final CoordinateTransformation ecefC)
3673 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3674 if (running) {
3675 throw new LockedException();
3676 }
3677
3678 biasEstimator.setNedPositionAndEcefOrientation(latitude, longitude, height, ecefC);
3679 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3680 }
3681
3682 /**
3683 * Sets position expressed on ECEF coordinates and orientation with respect to
3684 * NED axes.
3685 * To preserve the provided orientation, the first position is set and
3686 * then orientation is applied.
3687 *
3688 * @param ecefPosition position expressed on ECEF coordinates.
3689 * @param nedC body to NED coordinate transformation indicating body
3690 * orientation.
3691 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
3692 * transformation is not from
3693 * body to NED coordinates.
3694 * @throws LockedException if estimator is currently
3695 * running.
3696 * @see #setEcefPosition(ECEFPosition)
3697 * @see #setNedC(CoordinateTransformation)
3698 */
3699 public void setEcefPositionAndNedOrientation(
3700 final ECEFPosition ecefPosition, final CoordinateTransformation nedC)
3701 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3702 if (running) {
3703 throw new LockedException();
3704 }
3705
3706 biasEstimator.setEcefPositionAndNedOrientation(ecefPosition, nedC);
3707 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3708 }
3709
3710 /**
3711 * Sets position expressed on ECEF coordinates and orientation with respect to
3712 * NED axes.
3713 * To preserve the provided orientation, the first position is set and
3714 * then orientation is applied.
3715 *
3716 * @param x x coordinate of ECEF position expressed in meters (m).
3717 * @param y y coordinate of ECEF position expressed in meters (m).
3718 * @param z z coordinate of ECEF position expressed in meters (m).
3719 * @param nedC body to NED coordinate transformation indicating body
3720 * orientation.
3721 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
3722 * transformation is not from
3723 * body to NED coordinates.
3724 * @throws LockedException if estimator is currently
3725 * running.
3726 * @see #setEcefPosition(double, double, double)
3727 * @see #setNedC(CoordinateTransformation)
3728 */
3729 public void setEcefPositionAndNedOrientation(
3730 final double x, final double y, final double z, final CoordinateTransformation nedC)
3731 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3732 if (running) {
3733 throw new LockedException();
3734 }
3735
3736 biasEstimator.setEcefPositionAndNedOrientation(x, y, z, nedC);
3737 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3738 }
3739
3740 /**
3741 * Sets position expressed on ECEF coordinates and orientation with respect to
3742 * NED axes.
3743 * To preserve the provided orientation, the first position is set and
3744 * then orientation is applied.
3745 *
3746 * @param x x coordinate of ECEF position.
3747 * @param y y coordinate of ECEF position.
3748 * @param z z coordinate of ECEF position.
3749 * @param nedC body to NED coordinate transformation indicating body
3750 * orientation.
3751 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
3752 * transformation is not from
3753 * body to NED coordinates.
3754 * @throws LockedException if estimator is currently
3755 * running.
3756 * @see #setEcefPosition(Distance, Distance, Distance)
3757 * @see #setNedC(CoordinateTransformation)
3758 */
3759 public void setEcefPositionAndNedOrientation(
3760 final Distance x, final Distance y, final Distance z, final CoordinateTransformation nedC)
3761 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3762 if (running) {
3763 throw new LockedException();
3764 }
3765
3766 biasEstimator.setEcefPositionAndNedOrientation(x, y, z, nedC);
3767 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3768 }
3769
3770 /**
3771 * Sets position expressed on ECEF coordinates and orientation with respect to
3772 * NED axes.
3773 * To preserve the provided orientation, the first position is set and
3774 * then orientation is applied.
3775 *
3776 * @param position position resolved around ECEF axes and expressed in meters (m).
3777 * @param nedC body to NED coordinate transformation indicating body
3778 * orientation.
3779 * @throws InvalidSourceAndDestinationFrameTypeException if provided coordinate
3780 * transformation is not from
3781 * body to NED coordinates.
3782 * @throws LockedException if estimator is currently
3783 * running.
3784 * @see #setEcefPosition(Point3D)
3785 * @see #setNedC(CoordinateTransformation)
3786 */
3787 public void setEcefPositionAndNedOrientation(final Point3D position, final CoordinateTransformation nedC)
3788 throws InvalidSourceAndDestinationFrameTypeException, LockedException {
3789 if (running) {
3790 throw new LockedException();
3791 }
3792
3793 biasEstimator.setEcefPositionAndNedOrientation(position, nedC);
3794 driftEstimator.setReferenceFrame(biasEstimator.getEcefFrame());
3795 }
3796
3797 /**
3798 * Gets the number of samples that have been processed so far.
3799 *
3800 * @return number of samples that have been processed so far.
3801 */
3802 public int getNumberOfProcessedSamples() {
3803 return biasEstimator.getNumberOfProcessedSamples();
3804 }
3805
3806 /**
3807 * Gets the number of drift periods that have been processed.
3808 *
3809 * @return number of drift periods that have been processed.
3810 */
3811 public int getNumberOfProcessedDriftPeriods() {
3812 return numberOfProcessedDriftPeriods;
3813 }
3814
3815 /**
3816 * Gets amount of total elapsed time since the first processed measurement expressed
3817 * in seconds (s).
3818 *
3819 * @return amount of total elapsed time.
3820 */
3821 public double getElapsedTimeSeconds() {
3822 return biasEstimator.getElapsedTimeSeconds();
3823 }
3824
3825 /**
3826 * Gets the amount of total elapsed time since the first processed measurement.
3827 *
3828 * @return amount of total elapsed time.
3829 */
3830 public Time getElapsedTime() {
3831 return biasEstimator.getElapsedTime();
3832 }
3833
3834 /**
3835 * Gets the amount of total elapsed time since the first processed measurement.
3836 *
3837 * @param result instance where the result will be stored.
3838 */
3839 public void getElapsedTime(final Time result) {
3840 biasEstimator.getElapsedTime(result);
3841 }
3842
3843 /**
3844 * Indicates whether measured kinematics must be fixed or not.
3845 * When enabled, provided calibration data is used; otherwise it is
3846 * ignored.
3847 * By default, this is enabled.
3848 *
3849 * @return indicates whether measured kinematics must be fixed or not.
3850 */
3851 public boolean isFixKinematicsEnabled() {
3852 return fixKinematics;
3853 }
3854
3855 /**
3856 * Specifies whether measured kinematics must be fixed or not.
3857 * When enabled, provided calibration data is used; otherwise it is
3858 * ignored.
3859 *
3860 * @param fixKinematics true if measured kinematics must be fixed or not.
3861 * @throws LockedException if estimator is currently running.
3862 */
3863 public void setFixKinematicsEnabled(final boolean fixKinematics) throws LockedException {
3864 if (running) {
3865 throw new LockedException();
3866 }
3867 this.fixKinematics = fixKinematics;
3868 driftEstimator.setFixKinematicsEnabled(fixKinematics);
3869 }
3870
3871 /**
3872 * Gets the number of samples to be used on each drift period.
3873 *
3874 * @return number of samples to be used on each drift period.
3875 */
3876 public int getDriftPeriodSamples() {
3877 return driftPeriodSamples;
3878 }
3879
3880 /**
3881 * Sets the number of samples to be used on each drift period.
3882 *
3883 * @param driftPeriodSamples number of samples to be used on each drift period.
3884 * @throws LockedException if estimator is currently running.
3885 * @throws IllegalArgumentException if provided value is negative or zero.
3886 */
3887 public void setDriftPeriodSamples(int driftPeriodSamples) throws LockedException {
3888 if (running) {
3889 throw new LockedException();
3890 }
3891 if (driftPeriodSamples <= 0) {
3892 throw new IllegalArgumentException();
3893 }
3894
3895 this.driftPeriodSamples = driftPeriodSamples;
3896 }
3897
3898 /**
3899 * Gets the duration of each drift period expressed in seconds (s).
3900 *
3901 * @return duration of each drift period.
3902 */
3903 public double getDriftPeriodSeconds() {
3904 return driftPeriodSamples * getTimeInterval();
3905 }
3906
3907 /**
3908 * Gets the duration of each drift period.
3909 *
3910 * @return duration of each drift period.
3911 */
3912 public Time getDriftPeriod() {
3913 return new Time(getDriftPeriodSeconds(), TimeUnit.SECOND);
3914 }
3915
3916 /**
3917 * Gets the duration of each drift period.
3918 *
3919 * @param result instance where the result will be stored.
3920 */
3921 public void getDriftPeriod(final Time result) {
3922 result.setValue(getDriftPeriodSeconds());
3923 result.setUnit(TimeUnit.SECOND);
3924 }
3925
3926 /**
3927 * Indicates whether this estimator is running or not.
3928 *
3929 * @return true if estimator is running, false otherwise.
3930 */
3931 public boolean isRunning() {
3932 return running;
3933 }
3934
3935 /**
3936 * Indicates if estimator is ready to start processing additional kinematics
3937 * measurements.
3938 *
3939 * @return true if ready, false otherwise.
3940 */
3941 public boolean isReady() {
3942 return driftEstimator.isReady();
3943 }
3944
3945 /**
3946 * Adds a sample of measured body kinematics (accelerometer and gyroscope readings)
3947 * obtained from an IMU, fixes their values and uses fixed values to estimate
3948 * any additional existing bias or position and velocity variation while the
3949 * IMU body remains static.
3950 *
3951 * @param kinematics measured body kinematics.
3952 * @throws LockedException if estimator is currently running.
3953 * @throws RandomWalkEstimationException if estimation fails for some reason.
3954 * @throws NotReadyException if estimator is not ready.
3955 */
3956 public void addBodyKinematics(final BodyKinematics kinematics) throws LockedException,
3957 RandomWalkEstimationException, NotReadyException {
3958
3959 if (running) {
3960 throw new LockedException();
3961 }
3962
3963 if (!driftEstimator.isReady()) {
3964 throw new NotReadyException();
3965 }
3966
3967 try {
3968 running = true;
3969
3970 final var numberOfSamples = getNumberOfProcessedSamples();
3971
3972 if (numberOfSamples == 0 && listener != null) {
3973 listener.onStart(this);
3974 }
3975
3976 if (fixKinematics) {
3977 fixer.fix(kinematics, fixedKinematics);
3978 } else {
3979 fixedKinematics.copyFrom(kinematics);
3980 }
3981
3982 biasEstimator.addBodyKinematics(fixedKinematics);
3983
3984 final var timeInterval = getTimeInterval();
3985
3986 // update random walk values)
3987
3988 // Get accumulated mean of bias values for all processed samples.
3989 // If calibration and sensor were perfect, this should be zero
3990 final var elapsedTime = getElapsedTimeSeconds();
3991 biasEstimator.getBiasF(accelerationTriad);
3992 biasEstimator.getBiasAngularRate(angularSpeedTriad);
3993
3994 final var accelerationBiasNorm = accelerationTriad.getNorm();
3995 final var angularSpeedBiasNorm = angularSpeedTriad.getNorm();
3996
3997 // estimate rates of variation PSD's of bias values for all processed
3998 // samples
3999 accelerometerBiasPSD = Math.pow(accelerationBiasNorm / elapsedTime, 2.0) * timeInterval;
4000 gyroBiasPSD = Math.pow(angularSpeedBiasNorm / elapsedTime, 2.0) * timeInterval;
4001
4002 // internally drift estimator will fix kinematics if needed
4003 driftEstimator.addBodyKinematics(kinematics);
4004
4005 if (numberOfSamples % driftPeriodSamples == 0) {
4006 // for each drift period, update mean and variance values
4007 // of drift and reset drift estimator
4008 final var n = numberOfProcessedDriftPeriods + 1.0;
4009 final var tmp = numberOfProcessedDriftPeriods / n;
4010
4011 final var positionDrift = driftEstimator.getCurrentPositionDriftNormMeters();
4012 final var velocityDrift = driftEstimator.getCurrentVelocityDriftNormMetersPerSecond();
4013 final var attitudeDrift = driftEstimator.getCurrentOrientationDriftRadians();
4014
4015 avgPositionDrift = avgPositionDrift * tmp + positionDrift / n;
4016 avgVelocityDrift = avgVelocityDrift * tmp + velocityDrift / n;
4017 avgAttitudeDrift = avgAttitudeDrift * tmp + attitudeDrift / n;
4018
4019 final var diffPositionDrift = positionDrift - avgPositionDrift;
4020 final var diffVelocityDrift = velocityDrift - avgVelocityDrift;
4021 final var diffAttitudeDrift = attitudeDrift - avgAttitudeDrift;
4022
4023 final var diffPositionDrift2 = diffPositionDrift * diffPositionDrift;
4024 final var diffVelocityDrift2 = diffVelocityDrift * diffVelocityDrift;
4025 final var diffAttitudeDrift2 = diffAttitudeDrift * diffAttitudeDrift;
4026
4027 varPositionDrift = varPositionDrift * tmp + diffPositionDrift2 / n;
4028 varVelocityDrift = varVelocityDrift * tmp + diffVelocityDrift2 / n;
4029 varAttitudeDrift = varAttitudeDrift * tmp + diffAttitudeDrift2 / n;
4030
4031 driftEstimator.reset();
4032 numberOfProcessedDriftPeriods++;
4033 }
4034
4035 if (listener != null) {
4036 listener.onBodyKinematicsAdded(this, kinematics, fixedKinematics);
4037 }
4038
4039 } catch (AlgebraException | DriftEstimationException e) {
4040 throw new RandomWalkEstimationException(e);
4041 } finally {
4042 running = false;
4043 }
4044 }
4045
4046 /**
4047 * Resets current estimator.
4048 *
4049 * @return true if estimator was successfully reset, false if no reset was needed.
4050 * @throws LockedException if estimator is currently running.
4051 */
4052 public boolean reset() throws LockedException {
4053 if (running) {
4054 throw new LockedException();
4055 }
4056
4057 running = true;
4058 if (biasEstimator.reset()) {
4059 accelerometerBiasPSD = 0.0;
4060 gyroBiasPSD = 0.0;
4061 numberOfProcessedDriftPeriods = 0;
4062 avgPositionDrift = 0.0;
4063 avgVelocityDrift = 0.0;
4064 avgAttitudeDrift = 0.0;
4065 varPositionDrift = 0.0;
4066 varVelocityDrift = 0.0;
4067 varAttitudeDrift = 0.0;
4068
4069 if (listener != null) {
4070 listener.onReset(this);
4071 }
4072
4073 running = false;
4074 return true;
4075 } else {
4076 running = false;
4077 return false;
4078 }
4079 }
4080
4081 /**
4082 * Gets estimated accelerometer bias random walk PSD (Power Spectral Density)
4083 * expressed in (m^2 * s^-5).
4084 * This can be used by {@link INSLooselyCoupledKalmanConfig}.
4085 *
4086 * @return accelerometer bias random walk PSD.
4087 */
4088 @Override
4089 public double getAccelerometerBiasPSD() {
4090 return accelerometerBiasPSD;
4091 }
4092
4093 /**
4094 * Gets estimated gyro bias random walk PSD (Power Spectral Density)
4095 * expressed in (rad^2 * s^-3).
4096 * This can be used by {@link INSLooselyCoupledKalmanConfig}.
4097 *
4098 * @return gyroscope bias random walk PSD.
4099 */
4100 @Override
4101 public double getGyroBiasPSD() {
4102 return gyroBiasPSD;
4103 }
4104
4105 /**
4106 * Gets estimated position noise variance expressed in square meters (m^2).
4107 * This can be used by {@link INSLooselyCoupledKalmanConfig}.
4108 *
4109 * @return position variance.
4110 */
4111 public double getPositionNoiseVariance() {
4112 return varPositionDrift;
4113 }
4114
4115 /**
4116 * Gets estimated velocity noise variance expressed in (m^2/s^2).
4117 * This can be used by {@link INSLooselyCoupledKalmanConfig}.
4118 *
4119 * @return velocity variance.
4120 */
4121 public double getVelocityNoiseVariance() {
4122 return varVelocityDrift;
4123 }
4124
4125 /**
4126 * Gets estimated attitude noise variance expressed in squared radians (rad^2).
4127 *
4128 * @return attitude variance.
4129 */
4130 public double getAttitudeNoiseVariance() {
4131 return varAttitudeDrift;
4132 }
4133
4134 /**
4135 * Gets standard deviation of position noise expressed in meters (m).
4136 * This can be used by {@link INSLooselyCoupledKalmanConfig}.
4137 *
4138 * @return standard deviation of position noise.
4139 */
4140 @Override
4141 public double getPositionNoiseStandardDeviation() {
4142 return Math.sqrt(varPositionDrift);
4143 }
4144
4145 /**
4146 * Gets standard deviation of position noise.
4147 * This can be used by {@link INSLooselyCoupledKalmanConfig}.
4148 *
4149 * @return standard deviation of position noise.
4150 */
4151 public Distance getPositionNoiseStandardDeviationAsDistance() {
4152 return new Distance(getPositionNoiseStandardDeviation(), DistanceUnit.METER);
4153 }
4154
4155 /**
4156 * Gets standard deviation of position noise.
4157 * This can be used by {@link INSLooselyCoupledKalmanConfig}.
4158 *
4159 * @param result instance where the result will be stored.
4160 */
4161 public void getPositionNoiseStandardDeviationAsDistance(final Distance result) {
4162 result.setValue(getPositionNoiseStandardDeviation());
4163 result.setUnit(DistanceUnit.METER);
4164 }
4165
4166 /**
4167 * Gets standard deviation of velocity noise expressed in meters per second
4168 * (m/s).
4169 * This can be used by {@link INSLooselyCoupledKalmanConfig}.
4170 *
4171 * @return standard deviation of velocity noise.
4172 */
4173 @Override
4174 public double getVelocityNoiseStandardDeviation() {
4175 return Math.sqrt(varVelocityDrift);
4176 }
4177
4178 /**
4179 * Gets standard deviation of velocity noise.
4180 * This can be used by {@link INSLooselyCoupledKalmanConfig}.
4181 *
4182 * @return standard deviation of velocity noise.
4183 */
4184 public Speed getVelocityNoiseStandardDeviationAsSpeed() {
4185 return new Speed(getVelocityNoiseStandardDeviation(), SpeedUnit.METERS_PER_SECOND);
4186 }
4187
4188 /**
4189 * Gets standard deviation of velocity noise.
4190 * This can be used by {@link INSLooselyCoupledKalmanConfig}.
4191 *
4192 * @param result instance where the result will be stored.
4193 */
4194 public void getVelocityNoiseStandardDeviationAsSpeed(final Speed result) {
4195 result.setValue(getVelocityNoiseStandardDeviation());
4196 result.setUnit(SpeedUnit.METERS_PER_SECOND);
4197 }
4198
4199 /**
4200 * Gets standard deviation of attitude noise expressed in radians (rad).
4201 *
4202 * @return standard deviation of attitude noise.
4203 */
4204 public double getAttitudeNoiseStandardDeviation() {
4205 return Math.sqrt(varAttitudeDrift);
4206 }
4207
4208 /**
4209 * Gets standard deviation of attitude noise.
4210 *
4211 * @return standard deviation of attitude noise.
4212 */
4213 public Angle getAttitudeNoiseStandardDeviationAsAngle() {
4214 return new Angle(getAttitudeNoiseStandardDeviation(), AngleUnit.RADIANS);
4215 }
4216
4217 /**
4218 * Gets standard deviation of attitude noise.
4219 *
4220 * @param result instance where the result will be stored.
4221 */
4222 public void getAttitudeNoiseStandardDeviationAsAngle(final Angle result) {
4223 result.setValue(getAttitudeNoiseStandardDeviation());
4224 result.setUnit(AngleUnit.RADIANS);
4225 }
4226
4227 /**
4228 * Gets position uncertainty expressed in meters (m).
4229 *
4230 * @return position uncertainty.
4231 */
4232 @Override
4233 public double getPositionUncertainty() {
4234 return avgPositionDrift;
4235 }
4236
4237 /**
4238 * Gets position uncertainty.
4239 *
4240 * @return position uncertainty.
4241 */
4242 public Distance getPositionUncertaintyAsDistance() {
4243 return new Distance(getPositionUncertainty(), DistanceUnit.METER);
4244 }
4245
4246 /**
4247 * Gets position uncertainty.
4248 *
4249 * @param result instance where the result will be stored.
4250 */
4251 public void getPositionUncertaintyAsDistance(final Distance result) {
4252 result.setValue(getPositionUncertainty());
4253 result.setUnit(DistanceUnit.METER);
4254 }
4255
4256 /**
4257 * Gets velocity uncertainty expressed in meters per second (m/s).
4258 *
4259 * @return velocity uncertainty.
4260 */
4261 @Override
4262 public double getVelocityUncertainty() {
4263 return avgVelocityDrift;
4264 }
4265
4266 /**
4267 * Gets velocity uncertainty.
4268 *
4269 * @return velocity uncertainty.
4270 */
4271 public Speed getVelocityUncertaintyAsSpeed() {
4272 return new Speed(getVelocityUncertainty(), SpeedUnit.METERS_PER_SECOND);
4273 }
4274
4275 /**
4276 * Gets velocity uncertainty.
4277 *
4278 * @param result instance where the result will be stored.
4279 */
4280 public void getVelocityUncertaintyAsSpeed(final Speed result) {
4281 result.setValue(getVelocityUncertainty());
4282 result.setUnit(SpeedUnit.METERS_PER_SECOND);
4283 }
4284
4285 /**
4286 * Gets attitude uncertainty expressed in radians (rad).
4287 *
4288 * @return attitude uncertainty.
4289 */
4290 @Override
4291 public double getAttitudeUncertainty() {
4292 return avgAttitudeDrift;
4293 }
4294
4295 /**
4296 * Gets attitude uncertainty.
4297 *
4298 * @return attitude uncertainty.
4299 */
4300 public Angle getAttitudeUncertaintyAsAngle() {
4301 return new Angle(getAttitudeUncertainty(), AngleUnit.RADIANS);
4302 }
4303
4304 /**
4305 * Gets attitude uncertainty.
4306 *
4307 * @param result instance where the result will be stored.
4308 */
4309 public void getAttitudeUncertaintyAsAngle(final Angle result) {
4310 result.setValue(getAttitudeUncertainty());
4311 result.setUnit(AngleUnit.RADIANS);
4312 }
4313
4314 /**
4315 * Gets last added kinematics after being fixed using provided
4316 * calibration data.
4317 * If kinematics fix is disabled, this will be equal to the last
4318 * provided measured kinematics.
4319 *
4320 * @return last fixed body kinematics.
4321 */
4322 public BodyKinematics getFixedKinematics() {
4323 return fixedKinematics;
4324 }
4325
4326 /**
4327 * Gets last added kinematics after being fixed using provided
4328 * calibration data.
4329 * If kinematics fix is disabled, this will be equal to the last
4330 * provided measured kinematics.
4331 *
4332 * @param result last fixed body kinematics.
4333 */
4334 public void getFixedKinematics(final BodyKinematics result) {
4335 result.copyFrom(fixedKinematics);
4336 }
4337 }