1 /*
2 * Copyright (C) 2020 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.navigation.inertial.calibration;
17
18 import com.irurueta.navigation.frames.ECEFFrame;
19 import com.irurueta.navigation.frames.NEDFrame;
20 import com.irurueta.navigation.inertial.BodyKinematics;
21 import com.irurueta.units.Acceleration;
22 import com.irurueta.units.AccelerationConverter;
23 import com.irurueta.units.AccelerationUnit;
24 import com.irurueta.units.AngularSpeed;
25 import com.irurueta.units.AngularSpeedConverter;
26 import com.irurueta.units.AngularSpeedUnit;
27 import com.irurueta.units.Time;
28
29 import java.io.Serial;
30 import java.util.Objects;
31
32 /**
33 * Extension of FrameBodyKinematics containing standard deviations of measured specific force
34 * and angular rates included on measured body kinematics (accelerometer + gyroscope) besides
35 * the corresponding frame (position, orientation and velocity) where the kinematics
36 * measurement was made.
37 */
38 public class StandardDeviationFrameBodyKinematics extends FrameBodyKinematics {
39
40 /**
41 * Serialization version. This is used to ensure compatibility of deserialization of permanently stored serialized
42 * instances.
43 */
44 @Serial
45 private static final long serialVersionUID = 0L;
46
47 /**
48 * Standard deviation of measured specific force expressed in meters per squared
49 * second (m/s^2).
50 */
51 private double specificForceStandardDeviation;
52
53 /**
54 * Standard deviation of measured angular rate expressed in radians per second (rad/s).
55 */
56 private double angularRateStandardDeviation;
57
58 /**
59 * Constructor.
60 */
61 public StandardDeviationFrameBodyKinematics() {
62 }
63
64 /**
65 * Constructor.
66 *
67 * @param kinematics current body kinematics measurement.
68 */
69 public StandardDeviationFrameBodyKinematics(final BodyKinematics kinematics) {
70 super(kinematics);
71 }
72
73 /**
74 * Constructor.
75 *
76 * @param frame current ECEF frame associated to measurement.
77 */
78 public StandardDeviationFrameBodyKinematics(final ECEFFrame frame) {
79 super(frame);
80 }
81
82 /**
83 * Constructor.
84 *
85 * @param frame current NED frame associated to measurement. Internally it will be
86 * converted to its corresponding ECEF frame.
87 */
88 public StandardDeviationFrameBodyKinematics(final NEDFrame frame) {
89 super(frame);
90 }
91
92 /**
93 * Constructor.
94 *
95 * @param timeInterval time interval expressed in seconds (s) between IMU measurements
96 * used to obtain current frame and previous frame.
97 * @throws IllegalArgumentException if provided value is negative.
98 */
99 public StandardDeviationFrameBodyKinematics(final double timeInterval) {
100 super(timeInterval);
101 }
102
103 /**
104 * Constructor.
105 *
106 * @param timeInterval time interval between IMU measurements used to obtain
107 * current frame and previous frame.
108 * @throws IllegalArgumentException if provided value is negative.
109 */
110 public StandardDeviationFrameBodyKinematics(final Time timeInterval) {
111 super(timeInterval);
112 }
113
114 /**
115 * Constructor.
116 *
117 * @param frame current ECEF frame associated to measurement.
118 * @param previousFrame previous ECEF frame associated to measurement.
119 */
120 public StandardDeviationFrameBodyKinematics(final ECEFFrame frame, final ECEFFrame previousFrame) {
121 super(frame, previousFrame);
122 }
123
124 /**
125 * Constructor.
126 *
127 * @param frame current NED frame associated to measurement. Internally it will be
128 * converted to its corresponding ECEF frame.
129 * @param previousFrame previous NED frame associated to measurement. Internally it will be
130 * converted to its corresponding ECEF frame.
131 */
132 public StandardDeviationFrameBodyKinematics(final NEDFrame frame, final NEDFrame previousFrame) {
133 super(frame, previousFrame);
134 }
135
136 /**
137 * Constructor.
138 *
139 * @param frame current ECEF frame associated to measurement.
140 * @param previousFrame previous ECEF frame associated to measurement.
141 * @param timeInterval time interval expressed in seconds (s) between IMU measurements
142 * used to obtain current frame and previous frame.
143 * @throws IllegalArgumentException if provided value is negative.
144 */
145 public StandardDeviationFrameBodyKinematics(
146 final ECEFFrame frame, final ECEFFrame previousFrame, final double timeInterval) {
147 super(frame, previousFrame, timeInterval);
148 }
149
150 /**
151 * Constructor.
152 *
153 * @param frame current ECEF frame associated to measurement.
154 * @param previousFrame previous ECEF frame associated to measurement.
155 * @param timeInterval time interval between IMU measurements used to obtain
156 * current frame and previous frame.
157 * @throws IllegalArgumentException if provided value is negative.
158 */
159 public StandardDeviationFrameBodyKinematics(
160 final ECEFFrame frame, final ECEFFrame previousFrame, final Time timeInterval) {
161 super(frame, previousFrame, timeInterval);
162 }
163
164 /**
165 * Constructor.
166 *
167 * @param frame current NED frame associated to measurement. Internally it
168 * will be converted to its corresponding ECEF frame.
169 * @param previousFrame previous NED frame associated to measurement. Internally it
170 * will be converted to its corresponding ECEF frame.
171 * @param timeInterval time interval expressed in seconds (s) between IMU measurements
172 * used to obtain current frame and previous frame.
173 * @throws IllegalArgumentException if provided value is negative.
174 */
175 public StandardDeviationFrameBodyKinematics(
176 final NEDFrame frame, final NEDFrame previousFrame, final double timeInterval) {
177 super(frame, previousFrame, timeInterval);
178 }
179
180 /**
181 * Constructor.
182 *
183 * @param frame current NED frame associated to measurement. Internally it
184 * will be converted to its corresponding ECEF frame.
185 * @param previousFrame previous NED frame associated to measurement. Internally it
186 * will be converted to its corresponding ECEF frame.
187 * @param timeInterval time interval between IMU measurements used to obtain
188 * current frame and previous frame.
189 * @throws IllegalArgumentException if provided value is negative.
190 */
191 public StandardDeviationFrameBodyKinematics(
192 final NEDFrame frame, final NEDFrame previousFrame, final Time timeInterval) {
193 super(frame, previousFrame, timeInterval);
194 }
195
196 /**
197 * Constructor.
198 *
199 * @param kinematics current body kinematics measurement.
200 * @param frame ECEF frame associated to measurement.
201 */
202 public StandardDeviationFrameBodyKinematics(final BodyKinematics kinematics, final ECEFFrame frame) {
203 super(kinematics, frame);
204 }
205
206 /**
207 * Constructor.
208 *
209 * @param kinematics current body kinematics measurement.
210 * @param frame NED frame associated to measurement. Internally it will be
211 * converted to its corresponding ECEF frame.
212 */
213 public StandardDeviationFrameBodyKinematics(final BodyKinematics kinematics, final NEDFrame frame) {
214 super(kinematics, frame);
215 }
216
217 /**
218 * Constructor.
219 *
220 * @param kinematics current body kinematics measurement.
221 * @param frame current ECEF frame associated to measurement.
222 * @param previousFrame previous ECEF frame associated to measurement.
223 */
224 public StandardDeviationFrameBodyKinematics(
225 final BodyKinematics kinematics, final ECEFFrame frame, final ECEFFrame previousFrame) {
226 super(kinematics, frame, previousFrame);
227 }
228
229 /**
230 * Constructor.
231 *
232 * @param kinematics current body kinematics measurement.
233 * @param frame current NED frame associated to measurement. Internally it
234 * will be converted to its corresponding ECEF frame.
235 * @param previousFrame previous NED frame associated to measurement. Internally it
236 * will be converted to its corresponding ECEF frame.
237 */
238 public StandardDeviationFrameBodyKinematics(
239 final BodyKinematics kinematics, final NEDFrame frame, final NEDFrame previousFrame) {
240 super(kinematics, frame, previousFrame);
241 }
242
243 /**
244 * Constructor.
245 *
246 * @param kinematics current body kinematics measurement.
247 * @param frame current ECEF frame associated to measurement.
248 * @param previousFrame previous ECEF frame associated to measurement.
249 * @param timeInterval time interval expressed in seconds (s) between IMU
250 * measurements used to obtain current frame and previous
251 * frame.
252 * @throws IllegalArgumentException if provided value is negative.
253 */
254 public StandardDeviationFrameBodyKinematics(
255 final BodyKinematics kinematics, final ECEFFrame frame, final ECEFFrame previousFrame,
256 final double timeInterval) {
257 super(kinematics, frame, previousFrame, timeInterval);
258 }
259
260 /**
261 * Constructor.
262 *
263 * @param kinematics current body kinematics measurement.
264 * @param frame current ECEF frame associated to measurement.
265 * @param previousFrame previous ECEF frame associated to measurement.
266 * @param timeInterval time interval between IMU measurements used to obtain
267 * current frame and previous frame.
268 * @throws IllegalArgumentException if provided value is negative.
269 */
270 public StandardDeviationFrameBodyKinematics(
271 final BodyKinematics kinematics, final ECEFFrame frame, final ECEFFrame previousFrame,
272 final Time timeInterval) {
273 super(kinematics, frame, previousFrame, timeInterval);
274 }
275
276 /**
277 * Constructor.
278 *
279 * @param kinematics current body kinematics measurement.
280 * @param frame current NED frame associated to measurement. Internally it
281 * will be converted to its corresponding ECEF frame.
282 * @param previousFrame previous NED frame associated to measurement. Internally it
283 * will be converted to its corresponding ECEF frame.
284 * @param timeInterval time interval expressed in seconds (s) between IMU
285 * measurements used to obtain current frame and previous
286 * frame.
287 * @throws IllegalArgumentException if provided value is negative.
288 */
289 public StandardDeviationFrameBodyKinematics(
290 final BodyKinematics kinematics, final NEDFrame frame, final NEDFrame previousFrame,
291 final double timeInterval) {
292 super(kinematics, frame, previousFrame, timeInterval);
293 }
294
295 /**
296 * Constructor.
297 *
298 * @param kinematics current body kinematics measurement.
299 * @param frame current NED frame associated to measurement. Internally it
300 * will be converted to its corresponding ECEF frame.
301 * @param previousFrame previous NED frame associated to measurement. Internally it
302 * will be converted to its corresponding ECEF frame.
303 * @param timeInterval time interval between IMU measurements used to obtain
304 * current frame and previous frame.
305 * @throws IllegalArgumentException if provided value is negative.
306 */
307 public StandardDeviationFrameBodyKinematics(
308 final BodyKinematics kinematics, final NEDFrame frame, final NEDFrame previousFrame,
309 final Time timeInterval) {
310 super(kinematics, frame, previousFrame, timeInterval);
311 }
312
313 /**
314 * Constructor.
315 *
316 * @param specificForceStandardDeviation standard deviation of measured specific
317 * force expressed in meters per squared
318 * second (m/s^2).
319 * @param angularRateStandardDeviation standard deviation of measured angular rate
320 * expressed in radians per second (rad/s).
321 * @throws IllegalArgumentException if either specific force standard deviation or
322 * angular rate standard deviation is negative.
323 */
324 public StandardDeviationFrameBodyKinematics(
325 final double specificForceStandardDeviation, final double angularRateStandardDeviation) {
326 setSpecificForceStandardDeviation(specificForceStandardDeviation);
327 setAngularRateStandardDeviation(angularRateStandardDeviation);
328 }
329
330 /**
331 * Constructor.
332 *
333 * @param kinematics current body kinematics measurement.
334 * @param specificForceStandardDeviation standard deviation of measured specific
335 * force expressed in meters per squared
336 * second (m/s^2).
337 * @param angularRateStandardDeviation standard deviation of measured angular rate
338 * expressed in radians per second (rad/s).
339 * @throws IllegalArgumentException if either specific force standard deviation or
340 * angular rate standard deviation is negative.
341 */
342 public StandardDeviationFrameBodyKinematics(
343 final BodyKinematics kinematics, final double specificForceStandardDeviation,
344 final double angularRateStandardDeviation) {
345 this(kinematics);
346 setSpecificForceStandardDeviation(specificForceStandardDeviation);
347 setAngularRateStandardDeviation(angularRateStandardDeviation);
348 }
349
350 /**
351 * Constructor.
352 *
353 * @param frame current ECEF frame associated to measurement.
354 * @param specificForceStandardDeviation standard deviation of measured specific
355 * force expressed in meters per squared
356 * second (m/s^2).
357 * @param angularRateStandardDeviation standard deviation of measured angular rate
358 * expressed in radians per second (rad/s).
359 * @throws IllegalArgumentException if either specific force standard deviation or
360 * angular rate standard deviation is negative.
361 */
362 public StandardDeviationFrameBodyKinematics(
363 final ECEFFrame frame, final double specificForceStandardDeviation,
364 final double angularRateStandardDeviation) {
365 this(frame);
366 setSpecificForceStandardDeviation(specificForceStandardDeviation);
367 setAngularRateStandardDeviation(angularRateStandardDeviation);
368 }
369
370 /**
371 * Constructor.
372 *
373 * @param frame current NED frame associated to measurement.
374 * Internally it will be converted to its
375 * corresponding ECEF frame.
376 * @param specificForceStandardDeviation standard deviation of measured specific
377 * force expressed in meters per squared
378 * second (m/s^2).
379 * @param angularRateStandardDeviation standard deviation of measured angular rate
380 * expressed in radians per second (rad/s).
381 * @throws IllegalArgumentException if either specific force standard deviation or
382 * angular rate standard deviation is negative.
383 */
384 public StandardDeviationFrameBodyKinematics(
385 final NEDFrame frame, final double specificForceStandardDeviation,
386 final double angularRateStandardDeviation) {
387 this(frame);
388 setSpecificForceStandardDeviation(specificForceStandardDeviation);
389 setAngularRateStandardDeviation(angularRateStandardDeviation);
390 }
391
392 /**
393 * Constructor.
394 *
395 * @param timeInterval time interval expressed in seconds (s)
396 * between IMU measurements used to obtain
397 * current frame and previous frame.
398 * @param specificForceStandardDeviation standard deviation of measured specific
399 * force expressed in meters per squared
400 * second (m/s^2).
401 * @param angularRateStandardDeviation standard deviation of measured angular rate
402 * expressed in radians per second (rad/s).
403 * @throws IllegalArgumentException if any provided value is negative.
404 */
405 public StandardDeviationFrameBodyKinematics(
406 final double timeInterval, final double specificForceStandardDeviation,
407 final double angularRateStandardDeviation) {
408 this(timeInterval);
409 setSpecificForceStandardDeviation(specificForceStandardDeviation);
410 setAngularRateStandardDeviation(angularRateStandardDeviation);
411 }
412
413 /**
414 * Constructor.
415 *
416 * @param timeInterval time interval between IMU measurements used
417 * to obtain current frame and previous frame.
418 * @param specificForceStandardDeviation standard deviation of measured specific
419 * force expressed in meters per squared
420 * second (m/s^2).
421 * @param angularRateStandardDeviation standard deviation of measured angular rate
422 * expressed in radians per second (rad/s).
423 * @throws IllegalArgumentException if any provided value is negative.
424 */
425 public StandardDeviationFrameBodyKinematics(
426 final Time timeInterval, final double specificForceStandardDeviation,
427 final double angularRateStandardDeviation) {
428 this(timeInterval);
429 setSpecificForceStandardDeviation(specificForceStandardDeviation);
430 setAngularRateStandardDeviation(angularRateStandardDeviation);
431 }
432
433 /**
434 * Constructor.
435 *
436 * @param frame current ECEF frame associated to
437 * measurement.
438 * @param previousFrame previous ECEF frame associated to
439 * measurement.
440 * @param specificForceStandardDeviation standard deviation of measured specific
441 * force expressed in meters per squared
442 * second (m/s^2).
443 * @param angularRateStandardDeviation standard deviation of measured angular rate
444 * expressed in radians per second (rad/s).
445 * @throws IllegalArgumentException if either specific force standard deviation or
446 * angular rate standard deviation is negative.
447 */
448 public StandardDeviationFrameBodyKinematics(
449 final ECEFFrame frame, final ECEFFrame previousFrame, final double specificForceStandardDeviation,
450 final double angularRateStandardDeviation) {
451 this(frame, previousFrame);
452 setSpecificForceStandardDeviation(specificForceStandardDeviation);
453 setAngularRateStandardDeviation(angularRateStandardDeviation);
454 }
455
456 /**
457 * Constructor.
458 *
459 * @param frame current NED frame associated to
460 * measurement. Internally it will be
461 * converted to its corresponding ECEF frame.
462 * @param previousFrame previous NED frame associated to
463 * measurement. Internally it will be
464 * converted to its corresponding ECEF frame.
465 * @param specificForceStandardDeviation standard deviation of measured specific
466 * force expressed in meters per squared
467 * second (m/s^2).
468 * @param angularRateStandardDeviation standard deviation of measured angular rate
469 * expressed in radians per second (rad/s).
470 * @throws IllegalArgumentException if either specific force standard deviation or
471 * angular rate standard deviation is negative.
472 */
473 public StandardDeviationFrameBodyKinematics(
474 final NEDFrame frame, final NEDFrame previousFrame, final double specificForceStandardDeviation,
475 final double angularRateStandardDeviation) {
476 this(frame, previousFrame);
477 setSpecificForceStandardDeviation(specificForceStandardDeviation);
478 setAngularRateStandardDeviation(angularRateStandardDeviation);
479 }
480
481 /**
482 * Constructor.
483 *
484 * @param frame current ECEF frame associated to
485 * measurement.
486 * @param previousFrame previous ECEF frame associated to
487 * measurement.
488 * @param timeInterval time interval expressed in seconds (s)
489 * between IMU measurements used to obtain
490 * current frame and previous frame.
491 * @param specificForceStandardDeviation standard deviation of measured specific
492 * force expressed in meters per squared
493 * second (m/s^2).
494 * @param angularRateStandardDeviation standard deviation of measured angular rate
495 * expressed in radians per second (rad/s).
496 * @throws IllegalArgumentException if either time interval, specific force
497 * standard deviation or angular rate standard
498 * deviation is negative.
499 */
500 public StandardDeviationFrameBodyKinematics(
501 final ECEFFrame frame, final ECEFFrame previousFrame, final double timeInterval,
502 final double specificForceStandardDeviation, final double angularRateStandardDeviation) {
503 this(frame, previousFrame, timeInterval);
504 setSpecificForceStandardDeviation(specificForceStandardDeviation);
505 setAngularRateStandardDeviation(angularRateStandardDeviation);
506 }
507
508 /**
509 * Constructor.
510 *
511 * @param frame current ECEF frame associated to
512 * measurement.
513 * @param previousFrame previous ECEF frame associated to
514 * measurement.
515 * @param timeInterval time interval between IMU measurements
516 * used to obtain current frame and previous
517 * frame.
518 * @param specificForceStandardDeviation standard deviation of measured specific
519 * force expressed in meters per squared
520 * second (m/s^2).
521 * @param angularRateStandardDeviation standard deviation of measured angular rate
522 * expressed in radians per second (rad/s).
523 * @throws IllegalArgumentException if either time interval, specific force
524 * standard deviation or angular rate standard
525 * deviation is negative.
526 */
527 public StandardDeviationFrameBodyKinematics(
528 final ECEFFrame frame, final ECEFFrame previousFrame, final Time timeInterval,
529 final double specificForceStandardDeviation, final double angularRateStandardDeviation) {
530 this(frame, previousFrame, timeInterval);
531 setSpecificForceStandardDeviation(specificForceStandardDeviation);
532 setAngularRateStandardDeviation(angularRateStandardDeviation);
533 }
534
535 /**
536 * Constructor.
537 *
538 * @param frame current NED frame associated to measurement.
539 * Internally it will be converted to its
540 * corresponding ECEF frame.
541 * @param previousFrame previous NED frame associated to
542 * measurement. Internally it will be
543 * converted to its corresponding ECEF frame.
544 * @param timeInterval time interval expressed in seconds (s)
545 * between IMU measurements used to obtain
546 * current frame and previous frame.
547 * @param specificForceStandardDeviation standard deviation of measured specific
548 * force expressed in meters per squared
549 * second (m/s^2).
550 * @param angularRateStandardDeviation standard deviation of measured angular rate
551 * expressed in radians per second (rad/s).
552 * @throws IllegalArgumentException if either time interval, specific force
553 * standard deviation or angular rate standard
554 * deviation is negative.
555 */
556 public StandardDeviationFrameBodyKinematics(
557 final NEDFrame frame, final NEDFrame previousFrame, final double timeInterval,
558 final double specificForceStandardDeviation, final double angularRateStandardDeviation) {
559 this(frame, previousFrame, timeInterval);
560 setSpecificForceStandardDeviation(specificForceStandardDeviation);
561 setAngularRateStandardDeviation(angularRateStandardDeviation);
562 }
563
564 /**
565 * Constructor.
566 *
567 * @param frame current NED frame associated to measurement.
568 * Internally it will be converted to its
569 * corresponding ECEF frame.
570 * @param previousFrame previous NED frame associated to
571 * measurement. Internally it will be
572 * converted to its corresponding ECEF frame.
573 * @param timeInterval time interval between IMU measurements used
574 * to obtain current frame and previous frame.
575 * @param specificForceStandardDeviation standard deviation of measured specific
576 * force expressed in meters per squared
577 * second (m/s^2).
578 * @param angularRateStandardDeviation standard deviation of measured angular rate
579 * expressed in radians per second (rad/s).
580 * @throws IllegalArgumentException if either time interval, specific force
581 * standard deviation or angular rate standard
582 * deviation is negative.
583 */
584 public StandardDeviationFrameBodyKinematics(
585 final NEDFrame frame, final NEDFrame previousFrame, final Time timeInterval,
586 final double specificForceStandardDeviation, final double angularRateStandardDeviation) {
587 this(frame, previousFrame, timeInterval);
588 setSpecificForceStandardDeviation(specificForceStandardDeviation);
589 setAngularRateStandardDeviation(angularRateStandardDeviation);
590 }
591
592 /**
593 * Constructor.
594 *
595 * @param kinematics current body kinematics measurement.
596 * @param frame ECEF frame associated to measurement.
597 * @param specificForceStandardDeviation standard deviation of measured specific
598 * force expressed in meters per squared
599 * second (m/s^2).
600 * @param angularRateStandardDeviation standard deviation of measured angular rate
601 * expressed in radians per second (rad/s).
602 * @throws IllegalArgumentException if either specific force standard deviation or
603 * angular rate standard deviation is negative.
604 */
605 public StandardDeviationFrameBodyKinematics(
606 final BodyKinematics kinematics, final ECEFFrame frame, final double specificForceStandardDeviation,
607 final double angularRateStandardDeviation) {
608 this(kinematics, frame);
609 setSpecificForceStandardDeviation(specificForceStandardDeviation);
610 setAngularRateStandardDeviation(angularRateStandardDeviation);
611 }
612
613 /**
614 * Constructor.
615 *
616 * @param kinematics current body kinematics measurement.
617 * @param frame NED frame associated to measurements.
618 * Internally it will be converted to its
619 * corresponding ECEF frame.
620 * @param specificForceStandardDeviation standard deviation of measured specific
621 * force expressed in meters per squared
622 * second (m/s^2).
623 * @param angularRateStandardDeviation standard deviation of measured angular rate
624 * expressed in radians per second (rad/s).
625 * @throws IllegalArgumentException if either specific force standard deviation or
626 * angular rate standard deviation is negative.
627 */
628 public StandardDeviationFrameBodyKinematics(
629 final BodyKinematics kinematics, final NEDFrame frame, final double specificForceStandardDeviation,
630 final double angularRateStandardDeviation) {
631 this(kinematics, frame);
632 setSpecificForceStandardDeviation(specificForceStandardDeviation);
633 setAngularRateStandardDeviation(angularRateStandardDeviation);
634 }
635
636 /**
637 * Constructor.
638 *
639 * @param kinematics current body kinematics measurement.
640 * @param frame current ECEF frame associated to
641 * measurement.
642 * @param previousFrame previous ECEF frame associated to
643 * measurement.
644 * @param specificForceStandardDeviation standard deviation of measured specific
645 * force expressed in meters per squared
646 * second (m/s^2).
647 * @param angularRateStandardDeviation standard deviation of measured angular rate
648 * expressed in radians per second (rad/s).
649 * @throws IllegalArgumentException if either specific force standard deviation or
650 * angular rate standard deviation is negative.
651 */
652 public StandardDeviationFrameBodyKinematics(
653 final BodyKinematics kinematics, final ECEFFrame frame, final ECEFFrame previousFrame,
654 final double specificForceStandardDeviation, final double angularRateStandardDeviation) {
655 this(kinematics, frame, previousFrame);
656 setSpecificForceStandardDeviation(specificForceStandardDeviation);
657 setAngularRateStandardDeviation(angularRateStandardDeviation);
658 }
659
660 /**
661 * Constructor.
662 *
663 * @param kinematics current body kinematics measurement.
664 * @param frame current NED frame associated to measurement.
665 * Internally it will be converted to its
666 * corresponding ECEF frame.
667 * @param previousFrame previous NED frame associated to measurement.
668 * Internally it will be converted to its
669 * corresponding ECEF frame.
670 * @param specificForceStandardDeviation standard deviation of measured specific
671 * force expressed in meters per squared
672 * second (m/s^2).
673 * @param angularRateStandardDeviation standard deviation of measured angular rate
674 * expressed in radians per second (rad/s).
675 * @throws IllegalArgumentException if either specific force standard deviation or
676 * angular rate standard deviation is negative.
677 */
678 public StandardDeviationFrameBodyKinematics(
679 final BodyKinematics kinematics, final NEDFrame frame, final NEDFrame previousFrame,
680 final double specificForceStandardDeviation, final double angularRateStandardDeviation) {
681 this(kinematics, frame, previousFrame);
682 setSpecificForceStandardDeviation(specificForceStandardDeviation);
683 setAngularRateStandardDeviation(angularRateStandardDeviation);
684 }
685
686 /**
687 * Constructor.
688 *
689 * @param kinematics current body kinematics measurement.
690 * @param frame current ECEF frame associated to
691 * measurement.
692 * @param previousFrame previous ECEF frame associated to
693 * measurement.
694 * @param timeInterval time interval expressed in seconds (s)
695 * between IMU measurements used to obtain
696 * current frame and previous frame.
697 * @param specificForceStandardDeviation standard deviation of measured specific
698 * force expressed in meters per squared
699 * second (m/s^2).
700 * @param angularRateStandardDeviation standard deviation of measured angular rate
701 * expressed in radians per second (rad/s).
702 * @throws IllegalArgumentException if either time interval, specific force
703 * standard deviation or angular rate standard
704 * deviation is negative.
705 */
706 public StandardDeviationFrameBodyKinematics(
707 final BodyKinematics kinematics, final ECEFFrame frame, final ECEFFrame previousFrame,
708 final double timeInterval, final double specificForceStandardDeviation,
709 final double angularRateStandardDeviation) {
710 this(kinematics, frame, previousFrame, timeInterval);
711 setSpecificForceStandardDeviation(specificForceStandardDeviation);
712 setAngularRateStandardDeviation(angularRateStandardDeviation);
713 }
714
715 /**
716 * Constructor.
717 *
718 * @param kinematics current body kinematics measurement.
719 * @param frame current ECEF frame associated to
720 * measurement.
721 * @param previousFrame previous ECEF frame associated to
722 * measurement.
723 * @param timeInterval time interval between IMU measurements used
724 * to obtain current frame and previous frame.
725 * @param specificForceStandardDeviation standard deviation of measured specific
726 * force expressed in meters per squared
727 * second (m/s^2).
728 * @param angularRateStandardDeviation standard deviation of measured angular rate
729 * expressed in radians per second (rad/s).
730 * @throws IllegalArgumentException if either time interval, specific force
731 * standard deviation or angular rate standard
732 * deviation is negative.
733 */
734 public StandardDeviationFrameBodyKinematics(
735 final BodyKinematics kinematics, final ECEFFrame frame, final ECEFFrame previousFrame,
736 final Time timeInterval, final double specificForceStandardDeviation,
737 final double angularRateStandardDeviation) {
738 this(kinematics, frame, previousFrame, timeInterval);
739 setSpecificForceStandardDeviation(specificForceStandardDeviation);
740 setAngularRateStandardDeviation(angularRateStandardDeviation);
741 }
742
743 /**
744 * Constructor.
745 *
746 * @param kinematics current body kinematics measurement.
747 * @param frame current NED frame associated to measurement.
748 * Internally it will be converted to its
749 * corresponding ECEF frame.
750 * @param previousFrame previous NED frame associated to
751 * measurement. Internally it will be
752 * converted to its corresponding ECEF frame.
753 * @param timeInterval time interval expressed in seconds (s)
754 * between IMU measurements used to obtain
755 * current frame and previous frame.
756 * @param specificForceStandardDeviation standard deviation of measured specific
757 * force expressed in meters per squared
758 * second (m/s^2).
759 * @param angularRateStandardDeviation standard deviation of measured angular rate
760 * expressed in radians per second (rad/s).
761 * @throws IllegalArgumentException if either time interval, specific force
762 * standard deviation or angular rate standard
763 * deviation is negative.
764 */
765 public StandardDeviationFrameBodyKinematics(
766 final BodyKinematics kinematics, final NEDFrame frame, final NEDFrame previousFrame,
767 final double timeInterval, final double specificForceStandardDeviation,
768 final double angularRateStandardDeviation) {
769 this(kinematics, frame, previousFrame, timeInterval);
770 setSpecificForceStandardDeviation(specificForceStandardDeviation);
771 setAngularRateStandardDeviation(angularRateStandardDeviation);
772 }
773
774 /**
775 * Constructor.
776 *
777 * @param kinematics current body kinematics measurement.
778 * @param frame current NED frame associated to measurement.
779 * Internally it will be converted to its
780 * corresponding ECEF frame.
781 * @param previousFrame previous NED frame associated to
782 * measurement. Internally it will be
783 * converted to its corresponding ECEF frame.
784 * @param timeInterval time interval between IMU measurements used
785 * to obtain current frame and previous frame.
786 * @param specificForceStandardDeviation standard deviation of measured specific
787 * force expressed in meters per squared
788 * second (m/s^2).
789 * @param angularRateStandardDeviation standard deviation of measured angular rate
790 * expressed in radians per second (rad/s).
791 * @throws IllegalArgumentException if either time interval, specific force
792 * standard deviation or angular rate standard
793 * deviation is negative.
794 */
795 public StandardDeviationFrameBodyKinematics(
796 final BodyKinematics kinematics, final NEDFrame frame, final NEDFrame previousFrame,
797 final Time timeInterval, final double specificForceStandardDeviation,
798 final double angularRateStandardDeviation) {
799 this(kinematics, frame, previousFrame, timeInterval);
800 setSpecificForceStandardDeviation(specificForceStandardDeviation);
801 setAngularRateStandardDeviation(angularRateStandardDeviation);
802 }
803
804 /**
805 * Constructor.
806 *
807 * @param specificForceStandardDeviation standard deviation of measured specific
808 * force.
809 * @param angularRateStandardDeviation standard deviation of measured angular
810 * rate.
811 * @throws IllegalArgumentException if either specific force standard deviation or
812 * angular rate standard deviation is negative.
813 */
814 public StandardDeviationFrameBodyKinematics(
815 final Acceleration specificForceStandardDeviation, final AngularSpeed angularRateStandardDeviation) {
816 this(convertAcceleration(specificForceStandardDeviation),
817 convertAngularSpeed(angularRateStandardDeviation));
818 }
819
820 /**
821 * Constructor.
822 *
823 * @param kinematics current body kinematics measurement.
824 * @param specificForceStandardDeviation standard deviation of measured specific
825 * force.
826 * @param angularRateStandardDeviation standard deviation of measured angular
827 * rate.
828 * @throws IllegalArgumentException if either specific force standard deviation or
829 * angular rate standard deviation is negative.
830 */
831 public StandardDeviationFrameBodyKinematics(
832 final BodyKinematics kinematics, final Acceleration specificForceStandardDeviation,
833 final AngularSpeed angularRateStandardDeviation) {
834 this(kinematics, convertAcceleration(specificForceStandardDeviation),
835 convertAngularSpeed(angularRateStandardDeviation));
836 }
837
838 /**
839 * Constructor.
840 *
841 * @param frame current ECEF frame associated to measurement.
842 * @param specificForceStandardDeviation standard deviation of measured specific
843 * force.
844 * @param angularRateStandardDeviation standard deviation of measured angular
845 * rate.
846 * @throws IllegalArgumentException if either specific force standard deviation or
847 * angular rate standard deviation is negative.
848 */
849 public StandardDeviationFrameBodyKinematics(
850 final ECEFFrame frame, final Acceleration specificForceStandardDeviation,
851 final AngularSpeed angularRateStandardDeviation) {
852 this(frame, convertAcceleration(specificForceStandardDeviation),
853 convertAngularSpeed(angularRateStandardDeviation));
854 }
855
856 /**
857 * Constructor.
858 *
859 * @param frame current NED frame associated to measurement.
860 * Internally it will be converted to its
861 * corresponding ECEF frame.
862 * @param specificForceStandardDeviation standard deviation of measured specific
863 * force.
864 * @param angularRateStandardDeviation standard deviation of measured angular
865 * rate.
866 * @throws IllegalArgumentException if either specific force standard deviation or
867 * angular rate standard deviation is negative.
868 */
869 public StandardDeviationFrameBodyKinematics(
870 final NEDFrame frame, final Acceleration specificForceStandardDeviation,
871 final AngularSpeed angularRateStandardDeviation) {
872 this(frame, convertAcceleration(specificForceStandardDeviation),
873 convertAngularSpeed(angularRateStandardDeviation));
874 }
875
876 /**
877 * Constructor.
878 *
879 * @param timeInterval time interval expressed in seconds (s)
880 * between IMU measurements used to obtain
881 * current frame and previous frame.
882 * @param specificForceStandardDeviation standard deviation of measured specific
883 * force.
884 * @param angularRateStandardDeviation standard deviation of measured angular
885 * rate.
886 * @throws IllegalArgumentException if any provided value is negative.
887 */
888 public StandardDeviationFrameBodyKinematics(
889 final double timeInterval, final Acceleration specificForceStandardDeviation,
890 final AngularSpeed angularRateStandardDeviation) {
891 this(timeInterval, convertAcceleration(specificForceStandardDeviation),
892 convertAngularSpeed(angularRateStandardDeviation));
893 }
894
895 /**
896 * Constructor.
897 *
898 * @param timeInterval time interval between IMU measurements used
899 * to obtain current frame and previous frame.
900 * @param specificForceStandardDeviation standard deviation of measured specific
901 * force.
902 * @param angularRateStandardDeviation standard deviation of measured angular
903 * rate.
904 * @throws IllegalArgumentException if any provided value is negative.
905 */
906 public StandardDeviationFrameBodyKinematics(
907 final Time timeInterval, final Acceleration specificForceStandardDeviation,
908 final AngularSpeed angularRateStandardDeviation) {
909 this(timeInterval, convertAcceleration(specificForceStandardDeviation),
910 convertAngularSpeed(angularRateStandardDeviation));
911 }
912
913 /**
914 * Constructor.
915 *
916 * @param frame current ECEF frame associated to
917 * measurement.
918 * @param previousFrame previous ECEF frame associated to
919 * measurement.
920 * @param specificForceStandardDeviation standard deviation of measured specific
921 * force.
922 * @param angularRateStandardDeviation standard deviation of measured angular
923 * rate.
924 * @throws IllegalArgumentException if either specific force standard deviation or
925 * angular rate standard deviation is negative.
926 */
927 public StandardDeviationFrameBodyKinematics(
928 final ECEFFrame frame, final ECEFFrame previousFrame, final Acceleration specificForceStandardDeviation,
929 final AngularSpeed angularRateStandardDeviation) {
930 this(frame, previousFrame, convertAcceleration(specificForceStandardDeviation),
931 convertAngularSpeed(angularRateStandardDeviation));
932 }
933
934 /**
935 * Constructor.
936 *
937 * @param frame current NED frame associated to
938 * measurement. Internally it will be
939 * converted to its corresponding ECEF frame.
940 * @param previousFrame previous END frame associated to
941 * measurement. Internally it will be
942 * converted to its corresponding ECEF frame.
943 * @param specificForceStandardDeviation standard deviation of measured specific
944 * force.
945 * @param angularRateStandardDeviation standard deviation of measured angular
946 * rate.
947 * @throws IllegalArgumentException if either specific force standard deviation or
948 * angular rate standard deviation is negative.
949 */
950 public StandardDeviationFrameBodyKinematics(
951 final NEDFrame frame, final NEDFrame previousFrame, final Acceleration specificForceStandardDeviation,
952 final AngularSpeed angularRateStandardDeviation) {
953 this(frame, previousFrame, convertAcceleration(specificForceStandardDeviation),
954 convertAngularSpeed(angularRateStandardDeviation));
955 }
956
957 /**
958 * Constructor.
959 *
960 * @param frame current ECEF frame associated to
961 * measurement.
962 * @param previousFrame previous ECEF frame associated to
963 * measurement.
964 * @param timeInterval time interval expressed in seconds (s)
965 * between IMU measurements used to obtain
966 * current frame and previous frame.
967 * @param specificForceStandardDeviation standard deviation of measured specific
968 * force.
969 * @param angularRateStandardDeviation standard deviation of measured angular
970 * rate.
971 * @throws IllegalArgumentException if either time interval, specific force
972 * standard deviation or angular rate standard
973 * deviation is negative.
974 */
975 public StandardDeviationFrameBodyKinematics(
976 final ECEFFrame frame, final ECEFFrame previousFrame, final double timeInterval,
977 final Acceleration specificForceStandardDeviation, final AngularSpeed angularRateStandardDeviation) {
978 this(frame, previousFrame, timeInterval, convertAcceleration(specificForceStandardDeviation),
979 convertAngularSpeed(angularRateStandardDeviation));
980 }
981
982 /**
983 * Constructor.
984 *
985 * @param frame current ECEF frame associated to
986 * measurement.
987 * @param previousFrame previous ECEF frame associated ot
988 * measurement.
989 * @param timeInterval time interval between IMU measurements
990 * used to obtain current frame and previous
991 * frame.
992 * @param specificForceStandardDeviation standard deviation of measured specific
993 * force.
994 * @param angularRateStandardDeviation standard deviation of measured angular
995 * rate.
996 * @throws IllegalArgumentException if either time interval, specific force
997 * standard deviation or angular rate standard
998 * deviation is negative.
999 */
1000 public StandardDeviationFrameBodyKinematics(
1001 final ECEFFrame frame, final ECEFFrame previousFrame, final Time timeInterval,
1002 final Acceleration specificForceStandardDeviation, final AngularSpeed angularRateStandardDeviation) {
1003 this(frame, previousFrame, timeInterval, convertAcceleration(specificForceStandardDeviation),
1004 convertAngularSpeed(angularRateStandardDeviation));
1005 }
1006
1007 /**
1008 * Constructor.
1009 *
1010 * @param frame current NED frame associated to
1011 * measurement. Internally it will be
1012 * converted to its corresponding ECEF frame.
1013 * @param previousFrame previous NED frame associated to
1014 * measurement. Internally it will be
1015 * converted to tis corresponding ECEF frame.
1016 * @param timeInterval time interval expressed in seconds (s)
1017 * between IMU measurements used to obtain
1018 * current frame and previous frame.
1019 * @param specificForceStandardDeviation standard deviation of measured specific
1020 * force.
1021 * @param angularRateStandardDeviation standard deviation of measured angular
1022 * rate.
1023 * @throws IllegalArgumentException if either time interval, specific force
1024 * standard deviation or angular rate standard
1025 * deviation is negative.
1026 */
1027 public StandardDeviationFrameBodyKinematics(
1028 final NEDFrame frame, final NEDFrame previousFrame, final double timeInterval,
1029 final Acceleration specificForceStandardDeviation, final AngularSpeed angularRateStandardDeviation) {
1030 this(frame, previousFrame, timeInterval, convertAcceleration(specificForceStandardDeviation),
1031 convertAngularSpeed(angularRateStandardDeviation));
1032 }
1033
1034 /**
1035 * Constructor.
1036 *
1037 * @param frame current NED frame associated to
1038 * measurement. Internally it will be
1039 * converted to its corresponding ECEF frame.
1040 * @param previousFrame previous NED frame associated to
1041 * measurement. Internally it will be
1042 * converted to its corresponding ECEF frame.
1043 * @param timeInterval time interval between IMU measurements used
1044 * to obtain current frame and previous frame.
1045 * @param specificForceStandardDeviation standard deviation of measured specific
1046 * force.
1047 * @param angularRateStandardDeviation standard deviation of measured angular
1048 * rate.
1049 * @throws IllegalArgumentException if either time interval, specific force
1050 * standard deviation or angular rate standard
1051 * deviation is negative.
1052 */
1053 public StandardDeviationFrameBodyKinematics(
1054 final NEDFrame frame, final NEDFrame previousFrame, final Time timeInterval,
1055 final Acceleration specificForceStandardDeviation, final AngularSpeed angularRateStandardDeviation) {
1056 this(frame, previousFrame, timeInterval, convertAcceleration(specificForceStandardDeviation),
1057 convertAngularSpeed(angularRateStandardDeviation));
1058 }
1059
1060 /**
1061 * Constructor.
1062 *
1063 * @param kinematics current body kinematics measurement.
1064 * @param frame ECEF frame associated to measurement.
1065 * @param specificForceStandardDeviation standard deviation of measured specific
1066 * force.
1067 * @param angularRateStandardDeviation standard deviation of measured angular
1068 * rate.
1069 * @throws IllegalArgumentException if either specific force standard deviation or
1070 * angular rate standard deviation is negative.
1071 */
1072 public StandardDeviationFrameBodyKinematics(
1073 final BodyKinematics kinematics, final ECEFFrame frame, final Acceleration specificForceStandardDeviation,
1074 final AngularSpeed angularRateStandardDeviation) {
1075 this(kinematics, frame, convertAcceleration(specificForceStandardDeviation),
1076 convertAngularSpeed(angularRateStandardDeviation));
1077 }
1078
1079 /**
1080 * Constructor.
1081 *
1082 * @param kinematics current body kinematics measurement.
1083 * @param frame NED frame associated to measurements.
1084 * Internally it will be converted to tis
1085 * corresponding ECEF frame.
1086 * @param specificForceStandardDeviation standard deviation of measured specific
1087 * force.
1088 * @param angularRateStandardDeviation standard deviation of measured angular
1089 * rate.
1090 * @throws IllegalArgumentException if either specific force standard deviation or
1091 * angular rate standard deviation is negative.
1092 */
1093 public StandardDeviationFrameBodyKinematics(
1094 final BodyKinematics kinematics, final NEDFrame frame, final Acceleration specificForceStandardDeviation,
1095 final AngularSpeed angularRateStandardDeviation) {
1096 this(kinematics, frame, convertAcceleration(specificForceStandardDeviation),
1097 convertAngularSpeed(angularRateStandardDeviation));
1098 }
1099
1100 /**
1101 * Constructor.
1102 *
1103 * @param kinematics current body kinematics measurement.
1104 * @param frame current ECEF frame associated to
1105 * measurement.
1106 * @param previousFrame previous ECEF frame associated to
1107 * measurement.
1108 * @param specificForceStandardDeviation standard deviation of measured specific
1109 * force.
1110 * @param angularRateStandardDeviation standard deviation of measured angular
1111 * rate.
1112 * @throws IllegalArgumentException if either specific force standard deviation or
1113 * angular rate standard deviation is negative.
1114 */
1115 public StandardDeviationFrameBodyKinematics(
1116 final BodyKinematics kinematics, final ECEFFrame frame, final ECEFFrame previousFrame,
1117 final Acceleration specificForceStandardDeviation, final AngularSpeed angularRateStandardDeviation) {
1118 this(kinematics, frame, previousFrame, convertAcceleration(specificForceStandardDeviation),
1119 convertAngularSpeed(angularRateStandardDeviation));
1120 }
1121
1122 /**
1123 * Constructor.
1124 *
1125 * @param kinematics current body kinematics measurement.
1126 * @param frame current NED frame associated to
1127 * measurement. Internally it will be
1128 * converted to its corresponding ECEF frame.
1129 * @param previousFrame previous NED frame associated ot
1130 * measurement. Internally it will be converted
1131 * to its corresponding ECEF frame.
1132 * @param specificForceStandardDeviation standard deviation of measured specific
1133 * force.
1134 * @param angularRateStandardDeviation standard deviation of measured angular
1135 * rate.
1136 * @throws IllegalArgumentException if either specific force standard deviation or
1137 * angular rate standard deviation is negative.
1138 */
1139 public StandardDeviationFrameBodyKinematics(
1140 final BodyKinematics kinematics, final NEDFrame frame, final NEDFrame previousFrame,
1141 final Acceleration specificForceStandardDeviation, final AngularSpeed angularRateStandardDeviation) {
1142 this(kinematics, frame, previousFrame, convertAcceleration(specificForceStandardDeviation),
1143 convertAngularSpeed(angularRateStandardDeviation));
1144 }
1145
1146 /**
1147 * Constructor.
1148 *
1149 * @param kinematics current body kinematics measurement.
1150 * @param frame current ECEF frame associated to
1151 * measurement.
1152 * @param previousFrame previous ECEF frame associated to
1153 * measurement.
1154 * @param timeInterval time interval expressed in seconds (s)
1155 * between IMU measurements used to obtain
1156 * current frame and previous frame.
1157 * @param specificForceStandardDeviation standard deviation of measured specific
1158 * force.
1159 * @param angularRateStandardDeviation standard deviation of measured angular
1160 * rate.
1161 * @throws IllegalArgumentException if either time interval, specific force
1162 * standard deviation or angular rate standard
1163 * deviation is negative.
1164 */
1165 public StandardDeviationFrameBodyKinematics(
1166 final BodyKinematics kinematics, final ECEFFrame frame, final ECEFFrame previousFrame,
1167 final double timeInterval, final Acceleration specificForceStandardDeviation,
1168 final AngularSpeed angularRateStandardDeviation) {
1169 this(kinematics, frame, previousFrame, timeInterval, convertAcceleration(specificForceStandardDeviation),
1170 convertAngularSpeed(angularRateStandardDeviation));
1171 }
1172
1173 /**
1174 * Constructor.
1175 *
1176 * @param kinematics current body kinematics measurement.
1177 * @param frame current ECEF frame associated to
1178 * measurement.
1179 * @param previousFrame previous ECEF frame associated to
1180 * measurement.
1181 * @param timeInterval time interval between IMU measurements used
1182 * to obtain current frame and previous frame.
1183 * @param specificForceStandardDeviation standard deviation of measured specific
1184 * force.
1185 * @param angularRateStandardDeviation standard deviation of measured angular
1186 * rate.
1187 * @throws IllegalArgumentException if either time interval, specific force
1188 * standard deviation or angular rate standard
1189 * deviation is negative.
1190 */
1191 public StandardDeviationFrameBodyKinematics(
1192 final BodyKinematics kinematics, final ECEFFrame frame, final ECEFFrame previousFrame,
1193 final Time timeInterval, final Acceleration specificForceStandardDeviation,
1194 final AngularSpeed angularRateStandardDeviation) {
1195 this(kinematics, frame, previousFrame, timeInterval, convertAcceleration(specificForceStandardDeviation),
1196 convertAngularSpeed(angularRateStandardDeviation));
1197 }
1198
1199 /**
1200 * Constructor.
1201 *
1202 * @param kinematics current body kinematics measurement.
1203 * @param frame current NED frame associated to
1204 * measurement. Internally it will be
1205 * converted to its corresponding ECEF frame.
1206 * @param previousFrame previous NED frame associated to
1207 * measurement. Internally it will be
1208 * converted to its corresponding ECEF frame.
1209 * @param timeInterval time interval expressed in seconds (s)
1210 * between IMU measurements used to obtain
1211 * current frame and previous frame.
1212 * @param specificForceStandardDeviation standard deviation of measured specific
1213 * force.
1214 * @param angularRateStandardDeviation standard deviation of measured angular
1215 * rate.
1216 * @throws IllegalArgumentException if either time interval, specific force
1217 * standard deviation or angular rate standard
1218 * deviation is negative.
1219 */
1220 public StandardDeviationFrameBodyKinematics(
1221 final BodyKinematics kinematics, final NEDFrame frame, final NEDFrame previousFrame,
1222 final double timeInterval, final Acceleration specificForceStandardDeviation,
1223 final AngularSpeed angularRateStandardDeviation) {
1224 this(kinematics, frame, previousFrame, timeInterval, convertAcceleration(specificForceStandardDeviation),
1225 convertAngularSpeed(angularRateStandardDeviation));
1226 }
1227
1228 /**
1229 * Constructor.
1230 *
1231 * @param kinematics current body kinematics measurement.
1232 * @param frame current NED frame associated to
1233 * measurement. Internally it will be
1234 * converted to its corresponding ECEF frame.
1235 * @param previousFrame previous NED frame associated to
1236 * measurement. Internally it will be
1237 * converted to its corresponding ECEF frame.
1238 * @param timeInterval time interval between IMU measurements used
1239 * to obtain current frame and previous frame.
1240 * @param specificForceStandardDeviation standard deviation of measured specific
1241 * force.
1242 * @param angularRateStandardDeviation standard deviation of measured angular
1243 * rate.
1244 * @throws IllegalArgumentException if either time interval, specific force
1245 * standard deviation or angular rate standard
1246 * deviation is negative.
1247 */
1248 public StandardDeviationFrameBodyKinematics(
1249 final BodyKinematics kinematics, final NEDFrame frame, final NEDFrame previousFrame,
1250 final Time timeInterval, final Acceleration specificForceStandardDeviation,
1251 final AngularSpeed angularRateStandardDeviation) {
1252 this(kinematics, frame, previousFrame, timeInterval, convertAcceleration(specificForceStandardDeviation),
1253 convertAngularSpeed(angularRateStandardDeviation));
1254 }
1255
1256 /**
1257 * Copy constructor.
1258 *
1259 * @param input instance to copy data from.
1260 */
1261 public StandardDeviationFrameBodyKinematics(final StandardDeviationFrameBodyKinematics input) {
1262 copyFrom(input);
1263 }
1264
1265 /**
1266 * Gets standard deviation of measured specific force expressed in meters per squared
1267 * second (m/s^2).
1268 *
1269 * @return standard deviation of measured specific force.
1270 */
1271 public double getSpecificForceStandardDeviation() {
1272 return specificForceStandardDeviation;
1273 }
1274
1275 /**
1276 * Sets standard deviation of measured specific force expressed in meters per squared
1277 * second (m/s^2).
1278 *
1279 * @param specificForceStandardDeviation standard deviation of measured specific force.
1280 * @throws IllegalArgumentException if provided value is negative.
1281 */
1282 public void setSpecificForceStandardDeviation(final double specificForceStandardDeviation) {
1283 if (specificForceStandardDeviation < 0.0) {
1284 throw new IllegalArgumentException();
1285 }
1286
1287 this.specificForceStandardDeviation = specificForceStandardDeviation;
1288 }
1289
1290 /**
1291 * Gets standard deviation of measured specific force.
1292 *
1293 * @return standard deviation of measured specific force.
1294 */
1295 public Acceleration getSpecificForceStandardDeviationAsAcceleration() {
1296 return new Acceleration(specificForceStandardDeviation, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1297 }
1298
1299 /**
1300 * Gets standard deviation of measured specific force.
1301 *
1302 * @param result instance where standard deviation of measured specific force will be
1303 * stored.
1304 */
1305 public void getSpecificForceStandardDeviationAsAcceleration(final Acceleration result) {
1306 result.setValue(specificForceStandardDeviation);
1307 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
1308 }
1309
1310 /**
1311 * Sets standard deviation of measured specific force.
1312 *
1313 * @param specificForceStandardDeviation standard deviation of measured specific force.
1314 * @throws IllegalArgumentException if provided value is negative.
1315 */
1316 public void setSpecificForceStandardDeviation(final Acceleration specificForceStandardDeviation) {
1317 setSpecificForceStandardDeviation(convertAcceleration(specificForceStandardDeviation));
1318 }
1319
1320 /**
1321 * Gets standard deviation of measured angular rate expressed in radians per second (rad/s).
1322 *
1323 * @return standard deviation of measured angular rate.
1324 */
1325 public double getAngularRateStandardDeviation() {
1326 return angularRateStandardDeviation;
1327 }
1328
1329 /**
1330 * Sets standard deviation of measured angular rate expressed in radians per second (rad/s).
1331 *
1332 * @param angularRateStandardDeviation standard deviation of measured angular rate.
1333 * @throws IllegalArgumentException if provided value is negative.
1334 */
1335 public void setAngularRateStandardDeviation(final double angularRateStandardDeviation) {
1336 if (angularRateStandardDeviation < 0.0) {
1337 throw new IllegalArgumentException();
1338 }
1339
1340 this.angularRateStandardDeviation = angularRateStandardDeviation;
1341 }
1342
1343 /**
1344 * Gets standard deviation of measured angular rate.
1345 *
1346 * @return standard deviation of measured angular rate.
1347 */
1348 public AngularSpeed getAngularRateStandardDeviationAsAngularSpeed() {
1349 return new AngularSpeed(angularRateStandardDeviation, AngularSpeedUnit.RADIANS_PER_SECOND);
1350 }
1351
1352 /**
1353 * Gets standard deviation of measured angular rate.
1354 *
1355 * @param result instance where standard deviation of measured angular rate will be
1356 * stored.
1357 */
1358 public void getAngularRateStandardDeviationAsAngularSpeed(final AngularSpeed result) {
1359 result.setValue(angularRateStandardDeviation);
1360 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
1361 }
1362
1363 /**
1364 * Sets standard deviation of measured angular rate.
1365 *
1366 * @param angularRateStandardDeviation standard deviation of measured angular rate.
1367 * @throws IllegalArgumentException if provided value is negative.
1368 */
1369 public void setAngularRateStandardDeviation(final AngularSpeed angularRateStandardDeviation) {
1370 setAngularRateStandardDeviation(convertAngularSpeed(angularRateStandardDeviation));
1371 }
1372
1373 /**
1374 * Copies data of provided instance into this instance.
1375 *
1376 * @param input instance to copy data from.
1377 */
1378 public void copyFrom(final StandardDeviationFrameBodyKinematics input) {
1379 super.copyFrom(input);
1380
1381 specificForceStandardDeviation = input.specificForceStandardDeviation;
1382 angularRateStandardDeviation = input.angularRateStandardDeviation;
1383 }
1384
1385 /**
1386 * Copies this instance data into provided instance.
1387 *
1388 * @param output destination instance where data will be copied to.
1389 */
1390 public void copyTo(final StandardDeviationFrameBodyKinematics output) {
1391 super.copyTo(output);
1392
1393 output.specificForceStandardDeviation = specificForceStandardDeviation;
1394 output.angularRateStandardDeviation = angularRateStandardDeviation;
1395 }
1396
1397 /**
1398 * Computes and returns hash code for this instance. Hash codes are almost unique
1399 * values that are useful for fast classification and storage of objects in collections.
1400 *
1401 * @return Hash code.
1402 */
1403 @Override
1404 public int hashCode() {
1405 return Objects.hash(super.hashCode(), specificForceStandardDeviation, angularRateStandardDeviation);
1406 }
1407
1408 /**
1409 * Checks if provided instance has exactly the same contents as this instance.
1410 *
1411 * @param other instance to be compared.
1412 * @return true if both instances are considered to be equal, false otherwise.
1413 */
1414 public boolean equals(final StandardDeviationFrameBodyKinematics other) {
1415 return equals(other, 0.0);
1416 }
1417
1418 /**
1419 * Checks if provided instance has contents similar to this instance up to provided
1420 * threshold value.
1421 *
1422 * @param other instance to be compared.
1423 * @param threshold maximum allowed difference between values.
1424 * @return true if both instances are considered to be equal (up to provided
1425 * threshold), false otherwise.
1426 */
1427 public boolean equals(final StandardDeviationFrameBodyKinematics other, final double threshold) {
1428 return super.equals(other, threshold)
1429 && Math.abs(specificForceStandardDeviation - other.specificForceStandardDeviation) <= threshold
1430 && Math.abs(angularRateStandardDeviation - other.angularRateStandardDeviation) <= threshold;
1431 }
1432
1433 /**
1434 * Checks if provided instance has contents similar to this instance up to provided
1435 * threshold value.
1436 *
1437 * @param other instance to be compared.
1438 * @param threshold maximum allowed difference between kinematics and frame values.
1439 * @return true if both instances are considered to be equal (up to provided
1440 * threshold), false otherwise.
1441 */
1442 @Override
1443 public boolean equals(final FrameBodyKinematics other, final double threshold) {
1444 if (this == other) {
1445 return true;
1446 }
1447 if (other == null || getClass() != other.getClass()) {
1448 return false;
1449 }
1450 return super.equals(other, threshold);
1451 }
1452
1453 /**
1454 * Checks if provided object is a StandardDeviationFrameBodyKinematics instance
1455 * having exactly the same contents as this instance.
1456 *
1457 * @param obj object to be compared.
1458 * @return true if both objects are considered to be equal, false otherwise.
1459 */
1460 @Override
1461 public boolean equals(final Object obj) {
1462 if (this == obj) {
1463 return true;
1464 }
1465 if (obj == null || getClass() != obj.getClass()) {
1466 return false;
1467 }
1468 final var other = (StandardDeviationFrameBodyKinematics) obj;
1469 return equals(other);
1470 }
1471
1472 /**
1473 * Makes a copy of this instance.
1474 *
1475 * @return a copy of this instance.
1476 * @throws CloneNotSupportedException if clone fails for some reason.
1477 */
1478 @Override
1479 protected Object clone() throws CloneNotSupportedException {
1480 final var result = (StandardDeviationFrameBodyKinematics) super.clone();
1481 copyTo(result);
1482 return result;
1483 }
1484
1485 /**
1486 * Converts provided acceleration to meters per squared second (m/s^2).
1487 *
1488 * @param acceleration instance to be converted.
1489 * @return converted value.
1490 */
1491 private static double convertAcceleration(final Acceleration acceleration) {
1492 return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
1493 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1494 }
1495
1496 /**
1497 * Converts provided angular speed to radians per second (rad/s).
1498 *
1499 * @param angularSpeed instance to be converted.
1500 * @return converted value.
1501 */
1502 private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
1503 return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit(),
1504 AngularSpeedUnit.RADIANS_PER_SECOND);
1505 }
1506 }