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.inertial.BodyKinematics;
19 import com.irurueta.units.Acceleration;
20 import com.irurueta.units.AccelerationConverter;
21 import com.irurueta.units.AccelerationUnit;
22 import com.irurueta.units.AngularSpeed;
23 import com.irurueta.units.AngularSpeedConverter;
24 import com.irurueta.units.AngularSpeedUnit;
25 import com.irurueta.units.Time;
26
27 import java.io.Serial;
28 import java.util.Objects;
29
30 /**
31 * Contains a body kinematics measurement (accelerometer + gyroscope) along with the
32 * corresponding timestamp when measure was made and standard deviations of measured
33 * specific force and angular rates.
34 * Notice that timestamp does not need to be absolute.
35 * Usually timestamps are used in sequences of measurements of body kinematics, where
36 * the first measurement can have any timestamp value (e.g. zero), and hence the subsequent
37 * measurements will have timestamps relative to the first one.
38 */
39 public class StandardDeviationTimedBodyKinematics extends TimedBodyKinematics {
40
41 /**
42 * Serialization version. This is used to ensure compatibility of deserialization of permanently stored serialized
43 * instances.
44 */
45 @Serial
46 private static final long serialVersionUID = 0L;
47
48 /**
49 * Standard deviation of measured specific force expressed in meters per squared
50 * second (m/s^2).
51 */
52 private double specificForceStandardDeviation;
53
54 /**
55 * Standard deviation of measured angular rate expressed in radians per second (rad/s).
56 */
57 private double angularRateStandardDeviation;
58
59 /**
60 * Constructor.
61 */
62 public StandardDeviationTimedBodyKinematics() {
63 super();
64 }
65
66 /**
67 * Constructor.
68 *
69 * @param kinematics current body kinematics measurement.
70 */
71 public StandardDeviationTimedBodyKinematics(final BodyKinematics kinematics) {
72 super(kinematics);
73 }
74
75 /**
76 * Constructor.
77 *
78 * @param timestampSeconds timestamp value expressed in seconds.
79 */
80 public StandardDeviationTimedBodyKinematics(final double timestampSeconds) {
81 super(timestampSeconds);
82 }
83
84 /**
85 * Constructor.
86 *
87 * @param timestamp timestamp value.
88 */
89 public StandardDeviationTimedBodyKinematics(final Time timestamp) {
90 super(timestamp);
91 }
92
93 /**
94 * Constructor.
95 *
96 * @param kinematics current body kinematics measurement.
97 * @param timestampSeconds timestamp value expressed in seconds.
98 */
99 public StandardDeviationTimedBodyKinematics(final BodyKinematics kinematics, final double timestampSeconds) {
100 super(kinematics, timestampSeconds);
101 }
102
103 /**
104 * Constructor.
105 *
106 * @param kinematics current body kinematics measurement.
107 * @param timestamp timestamp value.
108 */
109 public StandardDeviationTimedBodyKinematics(final BodyKinematics kinematics, final Time timestamp) {
110 super(kinematics, timestamp);
111 }
112
113 /**
114 * Constructor.
115 *
116 * @param specificForceStandardDeviation standard deviation of measured specific
117 * force expressed in meters per squared
118 * second (m/s^2).
119 * @param angularRateStandardDeviation standard deviation of measured angular rate
120 * expressed in radians per second (rad/s).
121 * @throws IllegalArgumentException if either specific force standard deviation or
122 * angular rate standard deviation is negative.
123 */
124 public StandardDeviationTimedBodyKinematics(
125 final double specificForceStandardDeviation, final double angularRateStandardDeviation) {
126 super();
127 setSpecificForceStandardDeviation(specificForceStandardDeviation);
128 setAngularRateStandardDeviation(angularRateStandardDeviation);
129 }
130
131 /**
132 * Constructor.
133 *
134 * @param kinematics current body kinematics measurement.
135 * @param specificForceStandardDeviation standard deviation of measured specific
136 * force expressed in meters per squared
137 * second (m/s^2).
138 * @param angularRateStandardDeviation standard deviation of measured angular rate
139 * expressed in radians per second (rad/s).
140 * @throws IllegalArgumentException if either specific force standard deviation or
141 * angular rate standard deviation is negative.
142 */
143 public StandardDeviationTimedBodyKinematics(
144 final BodyKinematics kinematics, final double specificForceStandardDeviation,
145 final double angularRateStandardDeviation) {
146 super(kinematics);
147 setSpecificForceStandardDeviation(specificForceStandardDeviation);
148 setAngularRateStandardDeviation(angularRateStandardDeviation);
149 }
150
151 /**
152 * Constructor.
153 *
154 * @param timestampSeconds timestamp value expressed in seconds.
155 * @param specificForceStandardDeviation standard deviation of measured specific
156 * force expressed in meters per squared
157 * second (m/s^2).
158 * @param angularRateStandardDeviation standard deviation of measured angular rate
159 * expressed in radians per second (rad/s).
160 * @throws IllegalArgumentException if either specific force standard deviation or
161 * angular rate standard deviation is negative.
162 */
163 public StandardDeviationTimedBodyKinematics(
164 final double timestampSeconds, final double specificForceStandardDeviation,
165 final double angularRateStandardDeviation) {
166 super(timestampSeconds);
167 setSpecificForceStandardDeviation(specificForceStandardDeviation);
168 setAngularRateStandardDeviation(angularRateStandardDeviation);
169 }
170
171 /**
172 * Constructor.
173 *
174 * @param timestamp timestamp value.
175 * @param specificForceStandardDeviation standard deviation of measured specific
176 * force expressed in meters per squared
177 * second (m/s^2).
178 * @param angularRateStandardDeviation standard deviation of measured angular rate
179 * expressed in radians per second (rad/s).
180 * @throws IllegalArgumentException if either specific force standard deviation or
181 * angular rate standard deviation is negative.
182 */
183 public StandardDeviationTimedBodyKinematics(
184 final Time timestamp, final double specificForceStandardDeviation,
185 final double angularRateStandardDeviation) {
186 super(timestamp);
187 setSpecificForceStandardDeviation(specificForceStandardDeviation);
188 setAngularRateStandardDeviation(angularRateStandardDeviation);
189 }
190
191 /**
192 * Constructor.
193 *
194 * @param kinematics current body kinematics measurement.
195 * @param timestampSeconds timestamp value expressed in seconds.
196 * @param specificForceStandardDeviation standard deviation of measured specific
197 * force expressed in meters per squared
198 * second (m/s^2).
199 * @param angularRateStandardDeviation standard deviation of measured angular rate
200 * expressed in radians per second (rad/s).
201 * @throws IllegalArgumentException if either specific force standard deviation or
202 * angular rate standard deviation is negative.
203 */
204 public StandardDeviationTimedBodyKinematics(
205 final BodyKinematics kinematics, final double timestampSeconds, final double specificForceStandardDeviation,
206 final double angularRateStandardDeviation) {
207 super(kinematics, timestampSeconds);
208 setSpecificForceStandardDeviation(specificForceStandardDeviation);
209 setAngularRateStandardDeviation(angularRateStandardDeviation);
210 }
211
212 /**
213 * Constructor.
214 *
215 * @param kinematics current body kinematics measurement.
216 * @param timestamp timestamp value.
217 * @param specificForceStandardDeviation standard deviation of measured specific
218 * force expressed in meters per squared
219 * second (m/s^2).
220 * @param angularRateStandardDeviation standard deviation of measured angular rate
221 * expressed in radians per second (rad/s).
222 * @throws IllegalArgumentException if either specific force standard deviation or
223 * angular rate standard deviation is negative.
224 */
225 public StandardDeviationTimedBodyKinematics(
226 final BodyKinematics kinematics, final Time timestamp, final double specificForceStandardDeviation,
227 final double angularRateStandardDeviation) {
228 super(kinematics, timestamp);
229 setSpecificForceStandardDeviation(specificForceStandardDeviation);
230 setAngularRateStandardDeviation(angularRateStandardDeviation);
231 }
232
233 /**
234 * Constructor.
235 *
236 * @param specificForceStandardDeviation standard deviation of measured specific
237 * force.
238 * @param angularRateStandardDeviation standard deviation of measured angular
239 * rate.
240 * @throws IllegalArgumentException if either specific force standard deviation or
241 * angular rate standard deviation is negative.
242 */
243 public StandardDeviationTimedBodyKinematics(
244 final Acceleration specificForceStandardDeviation, final AngularSpeed angularRateStandardDeviation) {
245 this(convertAcceleration(specificForceStandardDeviation), convertAngularSpeed(angularRateStandardDeviation));
246 }
247
248 /**
249 * Constructor.
250 *
251 * @param kinematics current body kinematics measurement.
252 * @param specificForceStandardDeviation standard deviation of measured specific
253 * force.
254 * @param angularRateStandardDeviation standard deviation of measured angular
255 * rate.
256 * @throws IllegalArgumentException if either specific force standard deviation or
257 * angular rate standard deviation is negative.
258 */
259 public StandardDeviationTimedBodyKinematics(
260 final BodyKinematics kinematics, final Acceleration specificForceStandardDeviation,
261 final AngularSpeed angularRateStandardDeviation) {
262 this(kinematics, convertAcceleration(specificForceStandardDeviation),
263 convertAngularSpeed(angularRateStandardDeviation));
264 }
265
266 /**
267 * Constructor.
268 *
269 * @param timestampSeconds timestamp value expressed in seconds.
270 * @param specificForceStandardDeviation standard deviation of measured specific
271 * force.
272 * @param angularRateStandardDeviation standard deviation of measured angular
273 * rate.
274 * @throws IllegalArgumentException if either specific force standard deviation or
275 * angular rate standard deviation is negative.
276 */
277 public StandardDeviationTimedBodyKinematics(
278 final double timestampSeconds, final Acceleration specificForceStandardDeviation,
279 final AngularSpeed angularRateStandardDeviation) {
280 this(timestampSeconds, convertAcceleration(specificForceStandardDeviation),
281 convertAngularSpeed(angularRateStandardDeviation));
282 }
283
284 /**
285 * Constructor.
286 *
287 * @param timestamp timestamp value.
288 * @param specificForceStandardDeviation standard deviation of measured specific
289 * force.
290 * @param angularRateStandardDeviation standard deviation of measured angular
291 * rate.
292 * @throws IllegalArgumentException if either specific force standard deviation or
293 * angular rate standard deviation is negative.
294 */
295 public StandardDeviationTimedBodyKinematics(
296 final Time timestamp, final Acceleration specificForceStandardDeviation,
297 final AngularSpeed angularRateStandardDeviation) {
298 this(timestamp, convertAcceleration(specificForceStandardDeviation),
299 convertAngularSpeed(angularRateStandardDeviation));
300 }
301
302 /**
303 * Constructor.
304 *
305 * @param kinematics current body kinematics measurement.
306 * @param timestampSeconds timestamp value expressed in seconds.
307 * @param specificForceStandardDeviation standard deviation of measured specific
308 * force.
309 * @param angularRateStandardDeviation standard deviation of measured angular
310 * rate.
311 * @throws IllegalArgumentException if either specific force standard deviation or
312 * angular rate standard deviation is negative.
313 */
314 public StandardDeviationTimedBodyKinematics(
315 final BodyKinematics kinematics, final double timestampSeconds,
316 final Acceleration specificForceStandardDeviation, final AngularSpeed angularRateStandardDeviation) {
317 this(kinematics, timestampSeconds, convertAcceleration(specificForceStandardDeviation),
318 convertAngularSpeed(angularRateStandardDeviation));
319 }
320
321 /**
322 * Constructor.
323 *
324 * @param kinematics current body kinematics measurement.
325 * @param timestamp timestamp value.
326 * @param specificForceStandardDeviation standard deviation of measured specific
327 * force.
328 * @param angularRateStandardDeviation standard deviation of measured angular
329 * rate.
330 * @throws IllegalArgumentException if either specific force standard deviation or
331 * angular rate standard deviation is negative.
332 */
333 public StandardDeviationTimedBodyKinematics(
334 final BodyKinematics kinematics, final Time timestamp,
335 final Acceleration specificForceStandardDeviation, final AngularSpeed angularRateStandardDeviation) {
336 this(kinematics, timestamp, convertAcceleration(specificForceStandardDeviation),
337 convertAngularSpeed(angularRateStandardDeviation));
338 }
339
340 /**
341 * Constructor.
342 *
343 * @param input instance to copy data from.
344 */
345 public StandardDeviationTimedBodyKinematics(final StandardDeviationTimedBodyKinematics input) {
346 copyFrom(input);
347 }
348
349 /**
350 * Gets standard deviation of measured specific force expressed in meters per squared
351 * second (m/s^2).
352 *
353 * @return standard deviation of measured specific force.
354 */
355 public double getSpecificForceStandardDeviation() {
356 return specificForceStandardDeviation;
357 }
358
359 /**
360 * Sets standard deviation of measured specific force expressed in meters per squared
361 * second (m/s^2).
362 *
363 * @param specificForceStandardDeviation standard deviation of measured specific force.
364 * @throws IllegalArgumentException if provided value is negative.
365 */
366 public void setSpecificForceStandardDeviation(final double specificForceStandardDeviation) {
367 if (specificForceStandardDeviation < 0.0) {
368 throw new IllegalArgumentException();
369 }
370
371 this.specificForceStandardDeviation = specificForceStandardDeviation;
372 }
373
374 /**
375 * Gets standard deviation of measured specific force.
376 *
377 * @return standard deviation of measured specific force.
378 */
379 public Acceleration getSpecificForceStandardDeviationAsAcceleration() {
380 return new Acceleration(specificForceStandardDeviation, AccelerationUnit.METERS_PER_SQUARED_SECOND);
381 }
382
383 /**
384 * Gets standard deviation of measured specific force.
385 *
386 * @param result instance where standard deviation of measured specific force will be
387 * stored.
388 */
389 public void getSpecificForceStandardDeviationAsAcceleration(final Acceleration result) {
390 result.setValue(specificForceStandardDeviation);
391 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
392 }
393
394 /**
395 * Sets standard deviation of measured specific force.
396 *
397 * @param specificForceStandardDeviation standard deviation of measured specific force.
398 * @throws IllegalArgumentException if provided value is negative.
399 */
400 public void setSpecificForceStandardDeviation(final Acceleration specificForceStandardDeviation) {
401 setSpecificForceStandardDeviation(convertAcceleration(specificForceStandardDeviation));
402 }
403
404 /**
405 * Gets standard deviation of measured angular rate expressed in radians per second (rad/s).
406 *
407 * @return standard deviation of measured angular rate.
408 */
409 public double getAngularRateStandardDeviation() {
410 return angularRateStandardDeviation;
411 }
412
413 /**
414 * Sets standard deviation of measured angular rate expressed in radians per second (rad/s).
415 *
416 * @param angularRateStandardDeviation standard deviation of measured angular rate.
417 * @throws IllegalArgumentException if provided value is negative.
418 */
419 public void setAngularRateStandardDeviation(final double angularRateStandardDeviation) {
420 if (angularRateStandardDeviation < 0.0) {
421 throw new IllegalArgumentException();
422 }
423
424 this.angularRateStandardDeviation = angularRateStandardDeviation;
425 }
426
427 /**
428 * Gets standard deviation of measured angular rate.
429 *
430 * @return standard deviation of measured angular rate.
431 */
432 public AngularSpeed getAngularRateStandardDeviationAsAngularSpeed() {
433 return new AngularSpeed(angularRateStandardDeviation, AngularSpeedUnit.RADIANS_PER_SECOND);
434 }
435
436 /**
437 * Gets standard deviation of measured angular rate.
438 *
439 * @param result instance where standard deviation of measured angular rate will be
440 * stored.
441 */
442 public void getAngularRateStandardDeviationAsAngularSpeed(final AngularSpeed result) {
443 result.setValue(angularRateStandardDeviation);
444 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
445 }
446
447 /**
448 * Sets standard deviation of measured angular rate.
449 *
450 * @param angularRateStandardDeviation standard deviation of measured angular rate.
451 * @throws IllegalArgumentException if provided value is negative.
452 */
453 public void setAngularRateStandardDeviation(final AngularSpeed angularRateStandardDeviation) {
454 setAngularRateStandardDeviation(convertAngularSpeed(angularRateStandardDeviation));
455 }
456
457 /**
458 * Copies data of provided instance into this instance.
459 *
460 * @param input instance to copy data from.
461 */
462 public void copyFrom(final StandardDeviationTimedBodyKinematics input) {
463 super.copyFrom(input);
464 specificForceStandardDeviation = input.specificForceStandardDeviation;
465 angularRateStandardDeviation = input.angularRateStandardDeviation;
466 }
467
468 @Override
469 public void copyFrom(final TimedBodyKinematics input) {
470 copyFrom((StandardDeviationTimedBodyKinematics) input);
471 }
472
473 /**
474 * Computes and returns hash code for this instance. Hash codes are almost unique
475 * values that are useful for fast classification and storage of objects in collections.
476 *
477 * @return Hash code.
478 */
479 @Override
480 public int hashCode() {
481 return Objects.hash(super.hashCode(), specificForceStandardDeviation, angularRateStandardDeviation);
482 }
483
484 /**
485 * Checks if provided instance has exactly the same contents as this instance.
486 *
487 * @param other instance to be compared.
488 * @return true if both instances are considered to be equal, false otherwise.
489 */
490 public boolean equals(final StandardDeviationTimedBodyKinematics other) {
491 return equals(other, 0.0);
492 }
493
494 /**
495 * Checks if provided instance has contents similar to this instance up to provided
496 * threshold value.
497 *
498 * @param other instance to be compared.
499 * @param threshold maximum allowed difference between kinematics and standard deviation
500 * values.
501 * @return true if both instances are considered to be equal (up to provided
502 * threshold), false otherwise.
503 */
504 public boolean equals(final StandardDeviationTimedBodyKinematics other, final double threshold) {
505 if (other == null) {
506 return false;
507 }
508
509 return super.equals(other, threshold)
510 && Math.abs(specificForceStandardDeviation - other.specificForceStandardDeviation) <= threshold
511 && Math.abs(angularRateStandardDeviation - other.angularRateStandardDeviation) <= threshold;
512 }
513
514 /**
515 * Makes a copy of this instance.
516 *
517 * @return a copy of this instance.
518 * @throws CloneNotSupportedException if clone fails for some reason.
519 */
520 @Override
521 protected Object clone() throws CloneNotSupportedException {
522 final var result = (StandardDeviationTimedBodyKinematics) super.clone();
523 copyTo(result);
524 return result;
525 }
526
527 /**
528 * Checks if provided object is a StandardDeviationTimedBodyKinematics instance having exactly the
529 * same contents as this instance.
530 *
531 * @param obj object to be compared.
532 * @return true if both objects are considered to be equal, false otherwise.
533 */
534 @Override
535 public boolean equals(final Object obj) {
536 if (this == obj) {
537 return true;
538 }
539 if (obj == null || getClass() != obj.getClass()) {
540 return false;
541 }
542 final var other = (StandardDeviationTimedBodyKinematics) obj;
543 return equals(other);
544 }
545
546 /**
547 * Converts provided acceleration to meters per squared second (m/s^2).
548 *
549 * @param acceleration instance to be converted.
550 * @return converted value.
551 */
552 private static double convertAcceleration(final Acceleration acceleration) {
553 return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
554 AccelerationUnit.METERS_PER_SQUARED_SECOND);
555 }
556
557 /**
558 * Converts provided angular speed to radians per second (rad/s).
559 *
560 * @param angularSpeed instance to be converted.
561 * @return converted value.
562 */
563 private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
564 return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit(),
565 AngularSpeedUnit.RADIANS_PER_SECOND);
566 }
567 }