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