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
26 import java.io.Serial;
27 import java.io.Serializable;
28 import java.util.Objects;
29
30 /**
31 * Contains a body kinematics measurement (accelerometer + gyroscope) along with
32 * the corresponding standard deviations of measured specific force and angular
33 * rates.
34 */
35 public class StandardDeviationBodyKinematics implements Serializable, Cloneable {
36
37 /**
38 * Serialization version. This is used to ensure compatibility of deserialization of permanently stored serialized
39 * instances.
40 */
41 @Serial
42 private static final long serialVersionUID = 0L;
43
44 /**
45 * Current body kinematics measurement. Contains accelerometer and gyroscope measurements.
46 */
47 private BodyKinematics kinematics;
48
49 /**
50 * Standard deviation of measured specific force expressed in meters per squared
51 * second (m/s^2).
52 */
53 private double specificForceStandardDeviation;
54
55 /**
56 * Standard deviation of measured angular rate expressed in radians per second (rad/s).
57 */
58 private double angularRateStandardDeviation;
59
60 /**
61 * Constructor.
62 */
63 public StandardDeviationBodyKinematics() {
64 }
65
66 /**
67 * Constructor.
68 *
69 * @param kinematics current body kinematics measurement.
70 */
71 public StandardDeviationBodyKinematics(final BodyKinematics kinematics) {
72 this.kinematics = kinematics;
73 }
74
75 /**
76 * Constructor.
77 *
78 * @param specificForceStandardDeviation standard deviation of measured specific
79 * force expressed in meters per squared
80 * second (m/s^2).
81 * @param angularRateStandardDeviation standard deviation of measured angular rate
82 * expressed in radians per second (rad/s).
83 * @throws IllegalArgumentException if either specific force standard deviation or
84 * angular rate standard deviation is negative.
85 */
86 public StandardDeviationBodyKinematics(
87 final double specificForceStandardDeviation, final double angularRateStandardDeviation) {
88 setSpecificForceStandardDeviation(specificForceStandardDeviation);
89 setAngularRateStandardDeviation(angularRateStandardDeviation);
90 }
91
92 /**
93 * Constructor.
94 *
95 * @param kinematics current body kinematics measurement.
96 * @param specificForceStandardDeviation standard deviation of measured specific
97 * force expressed in meters per squared
98 * second (m/s^2).
99 * @param angularRateStandardDeviation standard deviation of measured angular rate
100 * expressed in radians per second (rad/s).
101 * @throws IllegalArgumentException if either specific force standard deviation or
102 * angular rate standard deviation is negative.
103 */
104 public StandardDeviationBodyKinematics(
105 final BodyKinematics kinematics, final double specificForceStandardDeviation,
106 final double angularRateStandardDeviation) {
107 this(kinematics);
108 setSpecificForceStandardDeviation(specificForceStandardDeviation);
109 setAngularRateStandardDeviation(angularRateStandardDeviation);
110 }
111
112 /**
113 * Constructor.
114 *
115 * @param specificForceStandardDeviation standard deviation of measured specific
116 * force.
117 * @param angularRateStandardDeviation standard deviation of measured angular
118 * rate.
119 * @throws IllegalArgumentException if either specific force standard deviation or
120 * angular rate standard deviation is negative.
121 */
122 public StandardDeviationBodyKinematics(
123 final Acceleration specificForceStandardDeviation, final AngularSpeed angularRateStandardDeviation) {
124 this(convertAcceleration(specificForceStandardDeviation),
125 convertAngularSpeed(angularRateStandardDeviation));
126 }
127
128 /**
129 * Constructor.
130 *
131 * @param kinematics current body kinematics measurement.
132 * @param specificForceStandardDeviation standard deviation of measured specific
133 * force.
134 * @param angularRateStandardDeviation standard deviation of measured angular
135 * rate.
136 * @throws IllegalArgumentException if either specific force standard deviation or
137 * angular rate standard deviation is negative.
138 */
139 public StandardDeviationBodyKinematics(
140 final BodyKinematics kinematics, final Acceleration specificForceStandardDeviation,
141 final AngularSpeed angularRateStandardDeviation) {
142 this(kinematics, convertAcceleration(specificForceStandardDeviation),
143 convertAngularSpeed(angularRateStandardDeviation));
144 }
145
146 /**
147 * Constructor.
148 *
149 * @param input instance to copy data from.
150 */
151 public StandardDeviationBodyKinematics(final StandardDeviationBodyKinematics input) {
152 copyFrom(input);
153 }
154
155 /**
156 * Gets current body kinematics measurement. Contains accelerometer and gyroscope
157 * measurements.
158 *
159 * @return current body kinematics measurement.
160 */
161 public BodyKinematics getKinematics() {
162 return kinematics;
163 }
164
165 /**
166 * Sets current body kinematics measurement. Contains accelerometer and gyroscope
167 * measurements.
168 *
169 * @param kinematics current body kinematics measurement to be set.
170 */
171 public void setKinematics(final BodyKinematics kinematics) {
172 this.kinematics = kinematics;
173 }
174
175 /**
176 * Gets standard deviation of measured specific force expressed in meters per squared
177 * second (m/s^2).
178 *
179 * @return standard deviation of measured specific force.
180 */
181 public double getSpecificForceStandardDeviation() {
182 return specificForceStandardDeviation;
183 }
184
185 /**
186 * Sets standard deviation of measured specific force expressed in meters per squared
187 * second (m/s^2).
188 *
189 * @param specificForceStandardDeviation standard deviation of measured specific force.
190 * @throws IllegalArgumentException if provided value is negative.
191 */
192 public void setSpecificForceStandardDeviation(final double specificForceStandardDeviation) {
193 if (specificForceStandardDeviation < 0.0) {
194 throw new IllegalArgumentException();
195 }
196
197 this.specificForceStandardDeviation = specificForceStandardDeviation;
198 }
199
200 /**
201 * Gets standard deviation of measured specific force.
202 *
203 * @return standard deviation of measured specific force.
204 */
205 public Acceleration getSpecificForceStandardDeviationAsAcceleration() {
206 return new Acceleration(specificForceStandardDeviation, AccelerationUnit.METERS_PER_SQUARED_SECOND);
207 }
208
209 /**
210 * Gets standard deviation of measured specific force.
211 *
212 * @param result instance where standard deviation of measured specific force will be
213 * stored.
214 */
215 public void getSpecificForceStandardDeviationAsAcceleration(final Acceleration result) {
216 result.setValue(specificForceStandardDeviation);
217 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
218 }
219
220 /**
221 * Sets standard deviation of measured specific force.
222 *
223 * @param specificForceStandardDeviation standard deviation of measured specific force.
224 * @throws IllegalArgumentException if provided value is negative.
225 */
226 public void setSpecificForceStandardDeviation(final Acceleration specificForceStandardDeviation) {
227 setSpecificForceStandardDeviation(convertAcceleration(specificForceStandardDeviation));
228 }
229
230 /**
231 * Gets standard deviation of measured angular rate expressed in radians per second (rad/s).
232 *
233 * @return standard deviation of measured angular rate.
234 */
235 public double getAngularRateStandardDeviation() {
236 return angularRateStandardDeviation;
237 }
238
239 /**
240 * Sets standard deviation of measured angular rate expressed in radians per second (rad/s).
241 *
242 * @param angularRateStandardDeviation standard deviation of measured angular rate.
243 * @throws IllegalArgumentException if provided value is negative.
244 */
245 public void setAngularRateStandardDeviation(final double angularRateStandardDeviation) {
246 if (angularRateStandardDeviation < 0.0) {
247 throw new IllegalArgumentException();
248 }
249
250 this.angularRateStandardDeviation = angularRateStandardDeviation;
251 }
252
253 /**
254 * Gets standard deviation of measured angular rate.
255 *
256 * @return standard deviation of measured angular rate.
257 */
258 public AngularSpeed getAngularRateStandardDeviationAsAngularSpeed() {
259 return new AngularSpeed(angularRateStandardDeviation, AngularSpeedUnit.RADIANS_PER_SECOND);
260 }
261
262 /**
263 * Gets standard deviation of measured angular rate.
264 *
265 * @param result instance where standard deviation of measured angular rate will be
266 * stored.
267 */
268 public void getAngularRateStandardDeviationAsAngularSpeed(final AngularSpeed result) {
269 result.setValue(angularRateStandardDeviation);
270 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
271 }
272
273 /**
274 * Sets standard deviation of measured angular rate.
275 *
276 * @param angularRateStandardDeviation standard deviation of measured angular rate.
277 * @throws IllegalArgumentException if provided value is negative.
278 */
279 public void setAngularRateStandardDeviation(final AngularSpeed angularRateStandardDeviation) {
280 setAngularRateStandardDeviation(convertAngularSpeed(angularRateStandardDeviation));
281 }
282
283 /**
284 * Copies data of provided instance into this instance.
285 *
286 * @param input instance to copy data from.
287 */
288 public void copyFrom(final StandardDeviationBodyKinematics input) {
289 if (input.kinematics != null) {
290 if (kinematics == null) {
291 kinematics = new BodyKinematics(input.kinematics);
292 } else {
293 kinematics.copyFrom(input.kinematics);
294 }
295 } else {
296 kinematics = null;
297 }
298
299 specificForceStandardDeviation = input.specificForceStandardDeviation;
300 angularRateStandardDeviation = input.angularRateStandardDeviation;
301 }
302
303 /**
304 * Copies this instance data into provided instance.
305 *
306 * @param output destination instance where data will be copied to.
307 */
308 public void copyTo(final StandardDeviationBodyKinematics output) {
309 output.copyFrom(this);
310 }
311
312 /**
313 * Computes and returns hash code for this instance. Hash codes are almost unique
314 * values that are useful for fast classification and storage of objects in collections.
315 *
316 * @return Hash code.
317 */
318 @Override
319 public int hashCode() {
320 return Objects.hash(kinematics, specificForceStandardDeviation, angularRateStandardDeviation);
321 }
322
323 /**
324 * Checks if provided instance has exactly the same contents as this instance.
325 *
326 * @param other instance to be compared.
327 * @return true if both instances are considered to be equal, false otherwise.
328 */
329 public boolean equals(final StandardDeviationBodyKinematics other) {
330 return equals(other, 0.0);
331 }
332
333 /**
334 * Checks if provided instance has contents similar to this instance up to provided
335 * threshold value.
336 *
337 * @param other instance to be compared.
338 * @param threshold maximum allowed difference between kinematics and standard deviation
339 * values.
340 * @return true if both instances are considered to be equal (up to provided
341 * threshold), false otherwise.
342 */
343 public boolean equals(final StandardDeviationBodyKinematics other, final double threshold) {
344 if (other == null) {
345 return false;
346 }
347
348 return ((other.kinematics == null && kinematics == null)
349 || (kinematics != null && kinematics.equals(other.kinematics, threshold)))
350 && Math.abs(specificForceStandardDeviation - other.specificForceStandardDeviation) <= threshold
351 && Math.abs(angularRateStandardDeviation - other.angularRateStandardDeviation) <= threshold;
352 }
353
354 /**
355 * Checks if provided object is a StandardDeviationBodyKinematics instance having exactly the
356 * same contents as this instance.
357 *
358 * @param obj object to be compared.
359 * @return true if both objects are considered to be equal, false otherwise.
360 */
361 @Override
362 public boolean equals(final Object obj) {
363 if (this == obj) {
364 return true;
365 }
366 if (obj == null || getClass() != obj.getClass()) {
367 return false;
368 }
369 final var other = (StandardDeviationBodyKinematics) obj;
370 return equals(other);
371 }
372
373 /**
374 * Makes a copy of this instance.
375 *
376 * @return a copy of this instance.
377 * @throws CloneNotSupportedException if clone fails for some reason.
378 */
379 @Override
380 protected Object clone() throws CloneNotSupportedException {
381 final var result = (StandardDeviationBodyKinematics) super.clone();
382 copyTo(result);
383 return result;
384 }
385
386 /**
387 * Converts provided acceleration to meters per squared second (m/s^2).
388 *
389 * @param acceleration instance to be converted.
390 * @return converted value.
391 */
392 private static double convertAcceleration(final Acceleration acceleration) {
393 return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
394 AccelerationUnit.METERS_PER_SQUARED_SECOND);
395 }
396
397 /**
398 * Converts provided angular speed to radians per second (rad/s).
399 *
400 * @param angularSpeed instance to be converted.
401 * @return converted value.
402 */
403 private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
404 return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit(),
405 AngularSpeedUnit.RADIANS_PER_SECOND);
406 }
407 }