1 /*
2 * Copyright (C) 2019 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;
17
18 import com.irurueta.units.*;
19
20 import java.io.Serial;
21 import java.io.Serializable;
22 import java.util.Objects;
23
24 /**
25 * Contains configuration parameters to determine the system noise covariance matrix
26 * when initializing INS/GNS Loosely Coupled Kalman filter.
27 */
28 public class INSLooselyCoupledKalmanInitializerConfig implements Serializable, Cloneable {
29
30 /**
31 * Serialization version. This is used to ensure compatibility of deserialization of permanently stored serialized
32 * instances.
33 */
34 @Serial
35 private static final long serialVersionUID = 0L;
36
37 /**
38 * Initial attitude uncertainty per axis expressed in radians (rad).
39 */
40 private double initialAttitudeUncertainty;
41
42 /**
43 * Initial velocity uncertainty per axis expressed in meters per second (m/s).
44 */
45 private double initialVelocityUncertainty;
46
47 /**
48 * Initial position uncertainty per axis expressed in meters (m).
49 */
50 private double initialPositionUncertainty;
51
52 /**
53 * Initial acceleration bias uncertainty expressed in meters per squared second (m/s^2).
54 */
55 private double initialAccelerationBiasUncertainty;
56
57 /**
58 * Initial gyroscope bias uncertainty expressed in radians per second (rad/s).
59 */
60 private double initialGyroscopeBiasUncertainty;
61
62 /**
63 * Constructor.
64 */
65 public INSLooselyCoupledKalmanInitializerConfig() {
66 }
67
68 /**
69 * Constructor.
70 *
71 * @param initialAttitudeUncertainty initial attitude uncertainty per axis
72 * expressed in radians (rad).
73 * @param initialVelocityUncertainty initial velocity uncertainty per axis
74 * expressed in meters per second (m/s).
75 * @param initialPositionUncertainty initial position uncertainty per axis
76 * expressed in meters (m).
77 * @param initialAccelerationBiasUncertainty initial acceleration bias uncertainty
78 * expressed in meters per squared second (m/s^2).
79 * @param initialGyroscopeBiasUncertainty initial gyroscope bias uncertainty
80 * expressed in radians per second (rad/s).
81 */
82 public INSLooselyCoupledKalmanInitializerConfig(
83 final double initialAttitudeUncertainty, final double initialVelocityUncertainty,
84 final double initialPositionUncertainty, final double initialAccelerationBiasUncertainty,
85 final double initialGyroscopeBiasUncertainty) {
86 setValues(initialAttitudeUncertainty, initialVelocityUncertainty, initialPositionUncertainty,
87 initialAccelerationBiasUncertainty, initialGyroscopeBiasUncertainty);
88 }
89
90 /**
91 * Constructor.
92 *
93 * @param initialAttitudeUncertainty initial attitude uncertainty per axis.
94 * @param initialVelocityUncertainty initial velocity uncertainty per axis.
95 * @param initialPositionUncertainty initial position uncertainty per axis.
96 * @param initialAccelerationBiasUncertainty initial acceleration bias uncertainty.
97 * @param initialGyroscopeBiasUncertainty initial gyroscope bias uncertainty.
98 */
99 public INSLooselyCoupledKalmanInitializerConfig(
100 final Angle initialAttitudeUncertainty, final Speed initialVelocityUncertainty,
101 final Distance initialPositionUncertainty, final Acceleration initialAccelerationBiasUncertainty,
102 final AngularSpeed initialGyroscopeBiasUncertainty) {
103 setValues(initialAttitudeUncertainty, initialVelocityUncertainty, initialPositionUncertainty,
104 initialAccelerationBiasUncertainty, initialGyroscopeBiasUncertainty);
105 }
106
107 /**
108 * Copy constructor.
109 *
110 * @param input input instance to copy data from.
111 */
112 public INSLooselyCoupledKalmanInitializerConfig(final INSLooselyCoupledKalmanInitializerConfig input) {
113 copyFrom(input);
114 }
115
116 /**
117 * Gets initial attitude uncertainty per axis expressed in radians (rad).
118 *
119 * @return initial attitude uncertainty per axis expressed in radians (rad).
120 */
121 public double getInitialAttitudeUncertainty() {
122 return initialAttitudeUncertainty;
123 }
124
125 /**
126 * Sets initial attitude uncertainty per axis expressed in radians (rad).
127 *
128 * @param initialAttitudeUncertainty initial attitude uncertainty per axis expressed
129 * in radians (rad).
130 */
131 public void setInitialAttitudeUncertainty(final double initialAttitudeUncertainty) {
132 this.initialAttitudeUncertainty = initialAttitudeUncertainty;
133 }
134
135 /**
136 * Gets initial attitude uncertainty per axis.
137 *
138 * @param result instance where initial attitude uncertainty per axis will be stored.
139 */
140 public void getInitialAttitudeUncertaintyAngle(final Angle result) {
141 result.setValue(initialAttitudeUncertainty);
142 result.setUnit(AngleUnit.RADIANS);
143 }
144
145 /**
146 * Gets initial attitude uncertainty per axis.
147 *
148 * @return initial attitude uncertainty per axis.
149 */
150 public Angle getInitialAttitudeUncertaintyAngle() {
151 return new Angle(initialAttitudeUncertainty, AngleUnit.RADIANS);
152 }
153
154 /**
155 * Sets initial attitude uncertainty per axis.
156 *
157 * @param initialAttitudeUncertainty initial attitude uncertainty per axis.
158 */
159 public void setInitialAttitudeUncertainty(final Angle initialAttitudeUncertainty) {
160 this.initialAttitudeUncertainty = AngleConverter.convert(initialAttitudeUncertainty.getValue().doubleValue(),
161 initialAttitudeUncertainty.getUnit(), AngleUnit.RADIANS);
162 }
163
164 /**
165 * Gets initial velocity uncertainty per axis expressed in meters per second (m/s).
166 *
167 * @return initial velocity uncertainty per axis expressed in meters per second (m/s).
168 */
169 public double getInitialVelocityUncertainty() {
170 return initialVelocityUncertainty;
171 }
172
173 /**
174 * Sets initial velocity uncertainty per axis expressed in meters per second (m/s).
175 *
176 * @param initialVelocityUncertainty initial velocity uncertainty per axis expressed
177 * in meters per second (m/s).
178 */
179 public void setInitialVelocityUncertainty(final double initialVelocityUncertainty) {
180 this.initialVelocityUncertainty = initialVelocityUncertainty;
181 }
182
183 /**
184 * Gets initial velocity uncertainty per axis.
185 *
186 * @param result instance where initial attitude uncertainty per axis will be stored.
187 */
188 public void getInitialVelocityUncertaintySpeed(final Speed result) {
189 result.setValue(initialVelocityUncertainty);
190 result.setUnit(SpeedUnit.METERS_PER_SECOND);
191 }
192
193 /**
194 * Gets initial velocity uncertainty per axis.
195 *
196 * @return initial velocity uncertainty per axis.
197 */
198 public Speed getInitialVelocityUncertaintySpeed() {
199 return new Speed(initialVelocityUncertainty, SpeedUnit.METERS_PER_SECOND);
200 }
201
202 /**
203 * Sets initial velocity uncertainty per axis.
204 *
205 * @param initialVelocityUncertainty initial velocity uncertainty per axis.
206 */
207 public void setInitialVelocityUncertainty(final Speed initialVelocityUncertainty) {
208 this.initialVelocityUncertainty = SpeedConverter.convert(initialVelocityUncertainty.getValue().doubleValue(),
209 initialVelocityUncertainty.getUnit(), SpeedUnit.METERS_PER_SECOND);
210 }
211
212 /**
213 * Gets initial position uncertainty per axis expressed in meters (m)
214 *
215 * @return initial position uncertainty per axis expressed in meters (m).
216 */
217 public double getInitialPositionUncertainty() {
218 return initialPositionUncertainty;
219 }
220
221 /**
222 * Sets initial position uncertainty per axis expressed in meters (m)
223 *
224 * @param initialPositionUncertainty initial position uncertainty per axis expressed
225 * in meters (m).
226 */
227 public void setInitialPositionUncertainty(final double initialPositionUncertainty) {
228 this.initialPositionUncertainty = initialPositionUncertainty;
229 }
230
231 /**
232 * Gets initial position uncertainty per axis.
233 *
234 * @param result instance where initial position uncertainty per axis will be stored.
235 */
236 public void getInitialPositionUncertaintyDistance(final Distance result) {
237 result.setValue(initialPositionUncertainty);
238 result.setUnit(DistanceUnit.METER);
239 }
240
241 /**
242 * Gets initial position uncertainty per axis.
243 *
244 * @return initial position uncertainty per axis.
245 */
246 public Distance getInitialPositionUncertaintyDistance() {
247 return new Distance(initialPositionUncertainty, DistanceUnit.METER);
248 }
249
250 /**
251 * Sets initial position uncertainty per axis.
252 *
253 * @param initialPositionUncertainty initial position uncertainty per axis.
254 */
255 public void setInitialPositionUncertainty(final Distance initialPositionUncertainty) {
256 this.initialPositionUncertainty = DistanceConverter.convert(initialPositionUncertainty.getValue().doubleValue(),
257 initialPositionUncertainty.getUnit(), DistanceUnit.METER);
258 }
259
260 /**
261 * Gets initial acceleration bias uncertainty expressed in meters per squared second (m/s^2).
262 *
263 * @return initial acceleration bias uncertainty expressed in meters per squared second (m/s^2).
264 */
265 public double getInitialAccelerationBiasUncertainty() {
266 return initialAccelerationBiasUncertainty;
267 }
268
269 /**
270 * Sets initial acceleration bias uncertainty expressed in meters per squared second (m/s^2).
271 *
272 * @param initialAccelerationBiasUncertainty initial acceleration bias uncertainty expressed in
273 * meters per squared second (m/s^2).
274 */
275 public void setInitialAccelerationBiasUncertainty(final double initialAccelerationBiasUncertainty) {
276 this.initialAccelerationBiasUncertainty = initialAccelerationBiasUncertainty;
277 }
278
279 /**
280 * Gets initial acceleration bias uncertainty.
281 *
282 * @param result instance where initial acceleration bias uncertainty will be stored.
283 */
284 public void getInitialAccelerationBiasUncertaintyAcceleration(final Acceleration result) {
285 result.setValue(initialAccelerationBiasUncertainty);
286 result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
287 }
288
289 /**
290 * Gets initial acceleration bias uncertainty.
291 *
292 * @return initial acceleration bias uncertainty.
293 */
294 public Acceleration getInitialAccelerationBiasUncertaintyAcceleration() {
295 return new Acceleration(initialAccelerationBiasUncertainty, AccelerationUnit.METERS_PER_SQUARED_SECOND);
296 }
297
298 /**
299 * Sets initial acceleration bias uncertainty.
300 *
301 * @param initialAccelerationUncertainty initial acceleration bias uncertainty.
302 */
303 public void setInitialAccelerationBiasUncertainty(final Acceleration initialAccelerationUncertainty) {
304 initialAccelerationBiasUncertainty = AccelerationConverter.convert(
305 initialAccelerationUncertainty.getValue().doubleValue(), initialAccelerationUncertainty.getUnit(),
306 AccelerationUnit.METERS_PER_SQUARED_SECOND);
307 }
308
309 /**
310 * Gets initial gyroscope bias uncertainty expressed in radians per second (rad/s).
311 *
312 * @return initial gyroscope bias uncertainty expressed in radians per second (rad/s).
313 */
314 public double getInitialGyroscopeBiasUncertainty() {
315 return initialGyroscopeBiasUncertainty;
316 }
317
318 /**
319 * Sets initial gyroscope bias uncertainty expressed in radians per second (rad/s).
320 *
321 * @param initialGyroscopeBiasUncertainty initial gyroscope bias uncertainty expressed
322 * in radians per second (rad/s).
323 */
324 public void setInitialGyroscopeBiasUncertainty(final double initialGyroscopeBiasUncertainty) {
325 this.initialGyroscopeBiasUncertainty = initialGyroscopeBiasUncertainty;
326 }
327
328 /**
329 * Gets initial gyroscope bias uncertainty.
330 *
331 * @param result instance where initial gyroscope bias uncertainty will be stored.
332 */
333 public void getInitialGyroscopeBiasUncertaintyAngularSpeed(final AngularSpeed result) {
334 result.setValue(initialGyroscopeBiasUncertainty);
335 result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
336 }
337
338 /**
339 * Gets initial gyroscope bias uncertainty.
340 *
341 * @return initial gyroscope bias uncertainty.
342 */
343 public AngularSpeed getInitialGyroscopeBiasUncertaintyAngularSpeed() {
344 return new AngularSpeed(initialGyroscopeBiasUncertainty, AngularSpeedUnit.RADIANS_PER_SECOND);
345 }
346
347 /**
348 * Sets initial gyroscope bias uncertainty.
349 *
350 * @param initialGyroscopeBiasUncertainty initial gyroscope bias uncertainty.
351 */
352 public void setInitialGyroscopeBiasUncertainty(final AngularSpeed initialGyroscopeBiasUncertainty) {
353 this.initialGyroscopeBiasUncertainty = AngularSpeedConverter.convert(
354 initialGyroscopeBiasUncertainty.getValue().doubleValue(), initialGyroscopeBiasUncertainty.getUnit(),
355 AngularSpeedUnit.RADIANS_PER_SECOND);
356 }
357
358 /**
359 * Sets configuration parameters.
360 *
361 * @param initialAttitudeUncertainty initial attitude uncertainty per axis
362 * expressed in radians (rad).
363 * @param initialVelocityUncertainty initial velocity uncertainty per axis
364 * expressed in meters per second (m/s).
365 * @param initialPositionUncertainty initial position uncertainty per axis
366 * expressed in meters (m).
367 * @param initialAccelerationBiasUncertainty initial acceleration bias uncertainty
368 * expressed in meters per squared second (m/s^2).
369 * @param initialGyroscopeBiasUncertainty initial gyroscope bias uncertainty
370 * expressed in radians per second (rad/s).
371 */
372 public void setValues(
373 final double initialAttitudeUncertainty, final double initialVelocityUncertainty,
374 final double initialPositionUncertainty, final double initialAccelerationBiasUncertainty,
375 final double initialGyroscopeBiasUncertainty) {
376 this.initialAttitudeUncertainty = initialAttitudeUncertainty;
377 this.initialVelocityUncertainty = initialVelocityUncertainty;
378 this.initialPositionUncertainty = initialPositionUncertainty;
379 this.initialAccelerationBiasUncertainty = initialAccelerationBiasUncertainty;
380 this.initialGyroscopeBiasUncertainty = initialGyroscopeBiasUncertainty;
381 }
382
383 /**
384 * Sets configuration parameters.
385 *
386 * @param initialAttitudeUncertainty initial attitude uncertainty per axis.
387 * @param initialVelocityUncertainty initial velocity uncertainty per axis.
388 * @param initialPositionUncertainty initial position uncertainty per axis.
389 * @param initialAccelerationBiasUncertainty initial acceleration bias uncertainty.
390 * @param initialGyroscopeBiasUncertainty initial gyroscope bias uncertainty.
391 */
392 public void setValues(
393 final Angle initialAttitudeUncertainty, final Speed initialVelocityUncertainty,
394 final Distance initialPositionUncertainty, final Acceleration initialAccelerationBiasUncertainty,
395 final AngularSpeed initialGyroscopeBiasUncertainty) {
396 setInitialAttitudeUncertainty(initialAttitudeUncertainty);
397 setInitialVelocityUncertainty(initialVelocityUncertainty);
398 setInitialPositionUncertainty(initialPositionUncertainty);
399 setInitialAccelerationBiasUncertainty(initialAccelerationBiasUncertainty);
400 setInitialGyroscopeBiasUncertainty(initialGyroscopeBiasUncertainty);
401 }
402
403 /**
404 * Copies this instance data into provided instance.
405 *
406 * @param output destination instance where data will be copied to.
407 */
408 public void copyTo(final INSLooselyCoupledKalmanInitializerConfig output) {
409 output.initialAttitudeUncertainty = initialAttitudeUncertainty;
410 output.initialVelocityUncertainty = initialVelocityUncertainty;
411 output.initialPositionUncertainty = initialPositionUncertainty;
412 output.initialAccelerationBiasUncertainty = initialAccelerationBiasUncertainty;
413 output.initialGyroscopeBiasUncertainty = initialGyroscopeBiasUncertainty;
414 }
415
416 /**
417 * Copies data of provided instance into this instance.
418 *
419 * @param input instance to copy data from.
420 */
421 public void copyFrom(final INSLooselyCoupledKalmanInitializerConfig input) {
422 initialAttitudeUncertainty = input.initialAttitudeUncertainty;
423 initialVelocityUncertainty = input.initialVelocityUncertainty;
424 initialPositionUncertainty = input.initialPositionUncertainty;
425 initialAccelerationBiasUncertainty = input.initialAccelerationBiasUncertainty;
426 initialGyroscopeBiasUncertainty = input.initialGyroscopeBiasUncertainty;
427 }
428
429 /**
430 * Computes and returns hash code for this instance. Hash codes are almost unique
431 * values that are useful for fast classification and storage of objects in collections.
432 *
433 * @return Hash code.
434 */
435 @Override
436 public int hashCode() {
437 return Objects.hash(initialAttitudeUncertainty, initialVelocityUncertainty, initialPositionUncertainty,
438 initialAccelerationBiasUncertainty, initialGyroscopeBiasUncertainty);
439 }
440
441 /**
442 * Checks if provided instance has exactly the same contents as this instance.
443 *
444 * @param obj instance to be compared.
445 * @return true if both instances are considered to be equal, false otherwise.
446 */
447 @Override
448 public boolean equals(final Object obj) {
449 if (this == obj) {
450 return true;
451 }
452 if (obj == null || getClass() != obj.getClass()) {
453 return false;
454 }
455
456 final var other = (INSLooselyCoupledKalmanInitializerConfig) obj;
457 return equals(other);
458 }
459
460 /**
461 * Checks if provided instance has exactly the same contents as this instance.
462 *
463 * @param other instance to be compared.
464 * @return true if both instances are considered to be equal, false otherwise.
465 */
466 public boolean equals(final INSLooselyCoupledKalmanInitializerConfig other) {
467 return equals(other, 0.0);
468 }
469
470 /**
471 * Checks if provided instance has contents similar to this instance up to provided
472 * threshold value.
473 *
474 * @param other instance to be compared.
475 * @param threshold maximum difference allowed for values.
476 * @return true if both instances are considered to be equal (up to provided threshold),
477 * false otherwise.
478 */
479 public boolean equals(final INSLooselyCoupledKalmanInitializerConfig other, final double threshold) {
480 if (other == null) {
481 return false;
482 }
483
484 return Math.abs(initialAttitudeUncertainty - other.initialAttitudeUncertainty) <= threshold
485 && Math.abs(initialVelocityUncertainty - other.initialVelocityUncertainty) <= threshold
486 && Math.abs(initialPositionUncertainty - other.initialPositionUncertainty) <= threshold
487 && Math.abs(initialAccelerationBiasUncertainty - other.initialAccelerationBiasUncertainty) <= threshold
488 && Math.abs(initialGyroscopeBiasUncertainty - other.initialGyroscopeBiasUncertainty) <= threshold;
489 }
490
491 /**
492 * Makes a copy of this instance.
493 *
494 * @return a copy of this instance.
495 * @throws CloneNotSupportedException if clone fails for some reason.
496 */
497 @Override
498 protected Object clone() throws CloneNotSupportedException {
499 final var result = (INSLooselyCoupledKalmanInitializerConfig) super.clone();
500 copyTo(result);
501 return result;
502 }
503 }