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.frames.converters.ECEFtoNEDFrameConverter;
21 import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
22 import com.irurueta.navigation.inertial.BodyKinematics;
23 import com.irurueta.units.Time;
24 import com.irurueta.units.TimeConverter;
25 import com.irurueta.units.TimeUnit;
26
27 import java.io.Serial;
28 import java.io.Serializable;
29 import java.util.Objects;
30
31 /**
32 * Contains a body kinematics measurement (accelerometer + gyroscope) along with
33 * the corresponding frame (position, orientation and velocity) where the
34 * kinematics measurement was made.
35 */
36 public class FrameBodyKinematics implements Serializable, Cloneable {
37
38 /**
39 * Serialization version. This is used to ensure compatibility of deserialization of permanently stored serialized
40 * instances.
41 */
42 @Serial
43 private static final long serialVersionUID = 0L;
44
45 /**
46 * Current body kinematics measurement. Contains accelerometer and gyroscope measurements.
47 */
48 private BodyKinematics kinematics;
49
50 /**
51 * Contains current body position, velocity (which will typically be zero) and orientation
52 * resolved around ECEF axes.
53 */
54 private ECEFFrame frame;
55
56 /**
57 * Contains body position, velocity (which will typically be zero) and orientation
58 * resolved around ECEF axes of previous IMU measurement.
59 */
60 private ECEFFrame previousFrame;
61
62 /**
63 * Time interval expressed in seconds (s) between IMU measurements used to obtain
64 * current frame and previous frame.
65 */
66 private double timeInterval;
67
68 /**
69 * Constructor.
70 */
71 public FrameBodyKinematics() {
72 }
73
74 /**
75 * Constructor.
76 *
77 * @param kinematics current body kinematics measurement.
78 */
79 public FrameBodyKinematics(final BodyKinematics kinematics) {
80 this.kinematics = kinematics;
81 }
82
83 /**
84 * Constructor.
85 *
86 * @param frame current ECEF frame associated to measurement.
87 */
88 public FrameBodyKinematics(final ECEFFrame frame) {
89 this.frame = frame;
90 }
91
92 /**
93 * Constructor.
94 *
95 * @param frame current NED frame associated to measurement. Internally it will be
96 * converted to its corresponding ECEF frame.
97 */
98 public FrameBodyKinematics(final NEDFrame frame) {
99 setNedFrame(frame);
100 }
101
102 /**
103 * Constructor.
104 *
105 * @param timeInterval time interval expressed in seconds (s) between IMU measurements
106 * used to obtain current frame and previous frame.
107 * @throws IllegalArgumentException if provided value is negative.
108 */
109 public FrameBodyKinematics(final double timeInterval) {
110 setTimeInterval(timeInterval);
111 }
112
113 /**
114 * Constructor.
115 *
116 * @param timeInterval time interval between IMU measurements used to obtain
117 * current frame and previous frame.
118 * @throws IllegalArgumentException if provided value is negative.
119 */
120 public FrameBodyKinematics(final Time timeInterval) {
121 setTimeInterval(timeInterval);
122 }
123
124 /**
125 * Constructor.
126 *
127 * @param frame current ECEF frame associated to measurement.
128 * @param previousFrame previous ECEF frame associated to measurement.
129 */
130 public FrameBodyKinematics(final ECEFFrame frame, final ECEFFrame previousFrame) {
131 this(frame);
132 this.previousFrame = previousFrame;
133 }
134
135 /**
136 * Constructor.
137 *
138 * @param frame current NED frame associated to measurement. Internally it
139 * will be converted to its corresponding ECEF frame.
140 * @param previousFrame previous NED frame associated to measurement. Internally it
141 * will be converted to its corresponding ECEF frame.
142 */
143 public FrameBodyKinematics(final NEDFrame frame, final NEDFrame previousFrame) {
144 this(frame);
145 setPreviousNedFrame(previousFrame);
146 }
147
148 /**
149 * Constructor.
150 *
151 * @param frame current ECEF frame associated to measurement.
152 * @param previousFrame previous ECEF frame associated to measurement.
153 * @param timeInterval time interval expressed in seconds (s) between IMU measurements
154 * used to obtain current frame and previous frame.
155 * @throws IllegalArgumentException if provided value is negative.
156 */
157 public FrameBodyKinematics(final ECEFFrame frame, final ECEFFrame previousFrame, final double timeInterval) {
158 this(frame, previousFrame);
159 setTimeInterval(timeInterval);
160 }
161
162 /**
163 * Constructor.
164 *
165 * @param frame current ECEF frame associated to measurement.
166 * @param previousFrame previous ECEF frame associated to measurement.
167 * @param timeInterval time interval between IMU measurements used to obtain
168 * current frame and previous frame.
169 * @throws IllegalArgumentException if provided value is negative.
170 */
171 public FrameBodyKinematics(final ECEFFrame frame, final ECEFFrame previousFrame, final Time timeInterval) {
172 this(frame, previousFrame);
173 setTimeInterval(timeInterval);
174 }
175
176 /**
177 * Constructor.
178 *
179 * @param frame current NED frame associated to measurement. Internally it
180 * will be converted to its corresponding ECEF frame.
181 * @param previousFrame previous NED frame associated to measurement. Internally it
182 * will be converted to its corresponding ECEF frame.
183 * @param timeInterval time interval expressed in seconds (s) between IMU measurements
184 * used to obtain current frame and previous frame.
185 * @throws IllegalArgumentException if provided value is negative.
186 */
187 public FrameBodyKinematics(final NEDFrame frame, final NEDFrame previousFrame, final double timeInterval) {
188 this(frame, previousFrame);
189 setTimeInterval(timeInterval);
190 }
191
192 /**
193 * Constructor.
194 *
195 * @param frame current NED frame associated to measurement. Internally it
196 * will be converted to its corresponding ECEF frame.
197 * @param previousFrame previous NED frame associated to measurement. Internally it
198 * will be converted to its corresponding ECEF frame.
199 * @param timeInterval time interval between IMU measurements used to obtain
200 * current frame and previous frame.
201 * @throws IllegalArgumentException if provided value is negative.
202 */
203 public FrameBodyKinematics(final NEDFrame frame, final NEDFrame previousFrame, final Time timeInterval) {
204 this(frame, previousFrame);
205 setTimeInterval(timeInterval);
206 }
207
208 /**
209 * Constructor.
210 *
211 * @param kinematics current body kinematics measurement.
212 * @param frame ECEF frame associated to measurement.
213 */
214 public FrameBodyKinematics(final BodyKinematics kinematics, final ECEFFrame frame) {
215 this(kinematics);
216 this.frame = frame;
217 }
218
219 /**
220 * Constructor.
221 *
222 * @param kinematics current body kinematics measurement.
223 * @param frame NED frame associated to measurement. Internally it will be
224 * converted to its corresponding ECEF frame.
225 */
226 public FrameBodyKinematics(final BodyKinematics kinematics, final NEDFrame frame) {
227 this(kinematics);
228 setNedFrame(frame);
229 }
230
231 /**
232 * Constructor.
233 *
234 * @param kinematics current body kinematics measurement.
235 * @param frame current ECEF frame associated to measurement.
236 * @param previousFrame previous ECEF frame associated to measurement.
237 */
238 public FrameBodyKinematics(final BodyKinematics kinematics, final ECEFFrame frame, final ECEFFrame previousFrame) {
239 this(kinematics, frame);
240 this.previousFrame = previousFrame;
241 }
242
243 /**
244 * Constructor.
245 *
246 * @param kinematics current body kinematics measurement.
247 * @param frame current NED frame associated to measurement. Internally it
248 * will be converted to its corresponding ECEF frame.
249 * @param previousFrame previous NED frame associated to measurement. Internally it
250 * will be converted to its corresponding ECEF frame.
251 */
252 public FrameBodyKinematics(final BodyKinematics kinematics, final NEDFrame frame, final NEDFrame previousFrame) {
253 this(kinematics, frame);
254 setPreviousNedFrame(previousFrame);
255 }
256
257 /**
258 * Constructor.
259 *
260 * @param kinematics current body kinematics measurement.
261 * @param frame current ECEF frame associated to measurement.
262 * @param previousFrame previous ECEF frame associated to measurement.
263 * @param timeInterval time interval expressed in seconds (s) between IMU measurements
264 * used to obtain current frame and previous frame.
265 * @throws IllegalArgumentException if provided value is negative.
266 */
267 public FrameBodyKinematics(final BodyKinematics kinematics, final ECEFFrame frame, final ECEFFrame previousFrame,
268 final double timeInterval) {
269 this(kinematics, frame, previousFrame);
270 setTimeInterval(timeInterval);
271 }
272
273 /**
274 * Constructor.
275 *
276 * @param kinematics current body kinematics measurement.
277 * @param frame current ECEF frame associated to measurement.
278 * @param previousFrame previous ECEF frame associated to measurement.
279 * @param timeInterval time interval between IMU measurements used to obtain
280 * current frame and previous frame.
281 * @throws IllegalArgumentException if provided value is negative.
282 */
283 public FrameBodyKinematics(final BodyKinematics kinematics, final ECEFFrame frame, final ECEFFrame previousFrame,
284 final Time timeInterval) {
285 this(kinematics, frame, previousFrame);
286 setTimeInterval(timeInterval);
287 }
288
289 /**
290 * Constructor.
291 *
292 * @param kinematics current body kinematics measurement.
293 * @param frame current NED frame associated to measurement. Internally it
294 * will be converted to its corresponding ECEF frame.
295 * @param previousFrame previous NED frame associated to measurement. Internally it
296 * will be converted to its corresponding ECEF frame.
297 * @param timeInterval time interval expressed in seconds (s) between IMU measurements
298 * used to obtain current frame and previous frame.
299 * @throws IllegalArgumentException if provided value is negative.
300 */
301 public FrameBodyKinematics(final BodyKinematics kinematics, final NEDFrame frame, final NEDFrame previousFrame,
302 final double timeInterval) {
303 this(kinematics, frame, previousFrame);
304 setTimeInterval(timeInterval);
305 }
306
307 /**
308 * Constructor.
309 *
310 * @param kinematics current body kinematics measurement.
311 * @param frame current NED frame associated to measurement. Internally it
312 * will be converted to its corresponding ECEF frame.
313 * @param previousFrame previous NED frame associated to measurement. Internally it
314 * will be converted to its corresponding ECEF frame.
315 * @param timeInterval time interval between IMU measurements used to obtain
316 * current frame and previous frame.
317 * @throws IllegalArgumentException if provided value is negative.
318 */
319 public FrameBodyKinematics(final BodyKinematics kinematics, final NEDFrame frame, final NEDFrame previousFrame,
320 final Time timeInterval) {
321 this(kinematics, frame, previousFrame);
322 setTimeInterval(timeInterval);
323 }
324
325 /**
326 * Constructor.
327 *
328 * @param input instance to copy data from.
329 */
330 public FrameBodyKinematics(final FrameBodyKinematics input) {
331 copyFrom(input);
332 }
333
334 /**
335 * Gets current body kinematics measurement. Contains accelerometer and gyroscope
336 * measurements.
337 *
338 * @return current body kinematics measurement.
339 */
340 public BodyKinematics getKinematics() {
341 return kinematics;
342 }
343
344 /**
345 * Sets current body kinematics measurement. Contains accelerometer and gyroscope
346 * measurements.
347 *
348 * @param kinematics current body kinematics measurement to be set.
349 */
350 public void setKinematics(final BodyKinematics kinematics) {
351 this.kinematics = kinematics;
352 }
353
354 /**
355 * Gets current body position (which will typically remain constant),
356 * velocity (which will typically be zero) and orientation (which usually changes
357 * with each measurement to perform calibration of a single device) resolved
358 * around ECEF axes associated to body kinematics measurement.
359 *
360 * @return current ECEF frame associated to body kinematics measurement or null if
361 * not available.
362 */
363 public ECEFFrame getFrame() {
364 return frame;
365 }
366
367 /**
368 * Sets current body position (which will typically remain constant),
369 * velocity (which will typically be zero) and orientation (which usually changes
370 * with each measurement to perform calibration of a single device) resolved
371 * around ECEF axes associated to body kinematics measurement.
372 *
373 * @param frame current ECEF frame
374 */
375 public void setFrame(final ECEFFrame frame) {
376 this.frame = frame;
377 }
378
379 /**
380 * Gets current body position (which will typically remain constant),
381 * velocity (which will typically be zero) and orientation (which usually changes
382 * with each measurement to perform calibration of a single device) resolved
383 * around NED axes associated to body kinematics measurement.
384 *
385 * @return current NED frame associated to body kinematics measurement or null if
386 * not available.
387 */
388 public NEDFrame getNedFrame() {
389 return frame != null ? ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(frame) : null;
390 }
391
392 /**
393 * Gets current body position (which will typically remain constant),
394 * velocity (which will typically be zero) and orientation (which usually changes
395 * with each measurement to perform calibration of a single device) resolved
396 * around NED axes associated to body kinematics measurement.
397 *
398 * @param result instance where result data will be stored if available.
399 * @return true if result instance was updated, false otherwise.
400 */
401 public boolean getNedFrame(final NEDFrame result) {
402 if (frame != null) {
403 ECEFtoNEDFrameConverter.convertECEFtoNED(frame, result);
404 return true;
405 } else {
406 return false;
407 }
408 }
409
410 /**
411 * Sets current body position (which will typically remain constant),
412 * velocity (which will typically be zero) and orientation (which usually changes
413 * with each measurement to perform calibration of a single device) resolved
414 * around NED axes associated to body kinematics measurement.
415 * <p>
416 * This method will internally store the corresponding ECEF frame to provided
417 * NED frame value.
418 *
419 * @param nedFrame current NED frame associated to body kinematics measurement
420 * to be set.
421 */
422 public void setNedFrame(final NEDFrame nedFrame) {
423 if (nedFrame != null) {
424 if (frame != null) {
425 NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
426 } else {
427 frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
428 }
429 } else {
430 frame = null;
431 }
432 }
433
434 /**
435 * Gets previous body position (which will typically remain constant),
436 * velocity (which will typically be zero) and orientation (which usually changes
437 * with each measurement to perform calibration of a single device) resolved
438 * around ECEF axes associated to previous body kinematics measurement.
439 *
440 * @return previous ECEF frame associated to previous body kinematics measurement or
441 * null if not available.
442 */
443 public ECEFFrame getPreviousFrame() {
444 return previousFrame;
445 }
446
447 /**
448 * Sets previous body position (which will typically remain constant),
449 * velocity (which will typically be zero) and orientation (which usually changes
450 * with each measurement to perform calibration of a single device) resolved
451 * around ECEF axes associated to previous body kinematics measurement.
452 *
453 * @param previousFrame previous ECEF frame associated to previous body kinematics
454 * measurement.
455 */
456 public void setPreviousFrame(final ECEFFrame previousFrame) {
457 this.previousFrame = previousFrame;
458 }
459
460 /**
461 * Gets previous body position (which will typically remain constant),
462 * velocity (which will typically be zero) and orientation (which usually changes
463 * with each measurement to perform calibration of a single device) resolved
464 * around NED axes associated to previous body kinematics measurement.
465 *
466 * @return previous NED frame associated to previous body kinematics measurement or
467 * null if not available.
468 */
469 public NEDFrame getPreviousNedFrame() {
470 return previousFrame != null ? ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(previousFrame) : null;
471 }
472
473 /**
474 * Gets previous body position (which will typically remain constant),
475 * velocity (which will typically be zero) and orientation (which usually changes
476 * with each measurement to perform calibration of a single device) resolved
477 * around NED axes associated to previous body kinematics measurement.
478 *
479 * @param result instance where result data will be stored if available.
480 * @return true if result instance was updated, false otherwise.
481 */
482 public boolean getPreviousNedFrame(final NEDFrame result) {
483 if (previousFrame != null) {
484 ECEFtoNEDFrameConverter.convertECEFtoNED(previousFrame, result);
485 return true;
486 } else {
487 return false;
488 }
489 }
490
491 /**
492 * Sets previous body position (which will typically remain constant),
493 * velocity (which will typically be zero) and orientation (which usually changes
494 * with each measurement to perform calibration of a single device) resolved
495 * around NED axes associated to previous body kinematics measurement.
496 *
497 * @param previousNedFrame previous NED frame associated to body kinematics
498 * measurement to be set.
499 */
500 public void setPreviousNedFrame(final NEDFrame previousNedFrame) {
501 if (previousNedFrame != null) {
502 if (previousFrame != null) {
503 NEDtoECEFFrameConverter.convertNEDtoECEF(previousNedFrame, previousFrame);
504 } else {
505 previousFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(previousNedFrame);
506 }
507 } else {
508 previousFrame = null;
509 }
510 }
511
512 /**
513 * Gets time interval expressed in seconds (s) between IMU measurements used to
514 * obtain current frame and previous frame.
515 *
516 * @return time interval expressed in seconds (s) between IMU measurements.
517 */
518 public double getTimeInterval() {
519 return timeInterval;
520 }
521
522 /**
523 * Sets time interval expressed in seconds (s) between IMU measurements used to
524 * obtain current frame and previous frame.
525 *
526 * @param timeInterval time interval expressed in seconds (s) between IMU
527 * measurements.
528 * @throws IllegalArgumentException if provided value is negative.
529 */
530 public void setTimeInterval(final double timeInterval) {
531 if (timeInterval < 0.0) {
532 throw new IllegalArgumentException();
533 }
534
535 this.timeInterval = timeInterval;
536 }
537
538 /**
539 * Gets time interval between IMU measurements used to obtain current frame and
540 * previous frame.
541 *
542 * @return time interval between IMU measurements.
543 */
544 public Time getTimeIntervalAsTime() {
545 return new Time(timeInterval, TimeUnit.SECOND);
546 }
547
548 /**
549 * Gets time interval between IMU measurements used to obtain current frame and
550 * previous frame.
551 *
552 * @param result instance where result will be stored.
553 */
554 public void getTimeIntervalAsTime(final Time result) {
555 result.setValue(timeInterval);
556 result.setUnit(TimeUnit.SECOND);
557 }
558
559 /**
560 * Sets time interval between IMU measurements used to obtain current frame and
561 * previous frame.
562 *
563 * @param timeInterval time interval between IMU measurements to be set.
564 * @throws IllegalArgumentException if provided value is negative.
565 */
566 public void setTimeInterval(final Time timeInterval) {
567 setTimeInterval(TimeConverter.convert(
568 timeInterval.getValue().doubleValue(), timeInterval.getUnit(), TimeUnit.SECOND));
569 }
570
571 /**
572 * Copies data of provided instance into this instance.
573 *
574 * @param input instance to copy dara from.
575 */
576 public void copyFrom(final FrameBodyKinematics input) {
577 if (input.kinematics != null) {
578 if (kinematics == null) {
579 kinematics = new BodyKinematics(input.kinematics);
580 } else {
581 kinematics.copyFrom(input.kinematics);
582 }
583 } else {
584 kinematics = null;
585 }
586
587 if (input.frame != null) {
588 if (frame == null) {
589 frame = new ECEFFrame(input.frame);
590 } else {
591 frame.copyFrom(input.frame);
592 }
593 } else {
594 frame = null;
595 }
596
597 if (input.previousFrame != null) {
598 if (previousFrame == null) {
599 previousFrame = new ECEFFrame(input.previousFrame);
600 } else {
601 previousFrame.copyFrom(input.previousFrame);
602 }
603 } else {
604 previousFrame = null;
605 }
606
607 timeInterval = input.timeInterval;
608 }
609
610 /**
611 * Copies this instance data into provided instance.
612 *
613 * @param output destination instance where data will be copied to.
614 */
615 public void copyTo(final FrameBodyKinematics output) {
616 output.copyFrom(this);
617 }
618
619 /**
620 * Computes and returns hash code for this instance. Hash codes are almost unique
621 * values that are useful for fast classification and storage of objects in collections.
622 *
623 * @return Hash code.
624 */
625 @Override
626 public int hashCode() {
627 return Objects.hash(kinematics, frame, previousFrame, timeInterval);
628 }
629
630 /**
631 * Checks if provided instance has exactly the same contents as this instance.
632 *
633 * @param other instance to be compared.
634 * @return true if both instances are considered to be equal, false otherwise.
635 */
636 public boolean equals(final FrameBodyKinematics other) {
637 return equals(other, 0.0);
638 }
639
640 /**
641 * Checks if provided instance has contents similar to this instance up to provided
642 * threshold value.
643 *
644 * @param other instance to be compared.
645 * @param threshold maximum allowed difference between kinematics and frame values.
646 * @return true if both instances are considered to be equal (up to provided
647 * threshold), false otherwise.
648 */
649 public boolean equals(final FrameBodyKinematics other, final double threshold) {
650 if (other == null) {
651 return false;
652 }
653
654 return ((other.kinematics == null && kinematics == null)
655 || (kinematics != null && kinematics.equals(other.kinematics, threshold)))
656 && ((other.frame == null && frame == null)
657 || (frame != null && frame.equals(other.frame, threshold)))
658 && ((other.previousFrame == null && previousFrame == null)
659 || (previousFrame != null && previousFrame.equals(other.previousFrame, threshold)))
660 && Math.abs(other.timeInterval - timeInterval) <= threshold;
661 }
662
663 /**
664 * Checks if provided object is a FrameBodyKinematics instance having exactly the same
665 * contents as this instance.
666 *
667 * @param obj object to be compared.
668 * @return true if both objects are considered to be equal, false otherwise.
669 */
670 @Override
671 public boolean equals(final Object obj) {
672 if (this == obj) {
673 return true;
674 }
675 if (obj == null || getClass() != obj.getClass()) {
676 return false;
677 }
678 final var other = (FrameBodyKinematics) obj;
679 return equals(other);
680 }
681
682 /**
683 * Makes a copy of this instance.
684 *
685 * @return a copy of this instance.
686 * @throws CloneNotSupportedException if clone fails for some reason.
687 */
688 @Override
689 protected Object clone() throws CloneNotSupportedException {
690 final var result = (FrameBodyKinematics) super.clone();
691 copyTo(result);
692 return result;
693 }
694 }