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 INSLooselyCoupledKalmanConfig 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 * Position measurement noise SD (Standard Deviation) per axis expressed in
66 * meters (m).
67 */
68 private double positionNoiseSD;
69
70 /**
71 * Velocity measurement noise SD (Standard Deviation) per axis expressed in
72 * meters per second (m/s).
73 */
74 private double velocityNoiseSD;
75
76 /**
77 * Constructor.
78 */
79 public INSLooselyCoupledKalmanConfig() {
80 }
81
82 /**
83 * Constructor.
84 *
85 * @param gyroNoisePSD gyro noise PSD (Power Spectral Density) expressed in
86 * squared radians per second (rad^2/s).
87 * @param accelerometerNoisePSD accelerometer noise PSD (Power Spectral Density)
88 * expressed in (m^2 * s^-3).
89 * @param accelerometerBiasPSD accelerometer bias random walk PSD (Power Spectral
90 * Density) expressed in (m^2 * s^-5).
91 * @param gyroBiasPSD gyro bias random walk PSD (Power Spectral Density)
92 * expressed in (rad^2 * s^-3).
93 * @param positionNoiseSD position measurement noise SD (Standard Deviation)
94 * per axis expressed in meters (m).
95 * @param velocityNoiseSD velocity measurement noise SD (Standard Deviation)
96 * per axis expressed in meters per second (m/s).
97 */
98 public INSLooselyCoupledKalmanConfig(
99 final double gyroNoisePSD, final double accelerometerNoisePSD, final double accelerometerBiasPSD,
100 final double gyroBiasPSD, final double positionNoiseSD, final double velocityNoiseSD) {
101 setValues(gyroNoisePSD, accelerometerNoisePSD, accelerometerBiasPSD, gyroBiasPSD, positionNoiseSD,
102 velocityNoiseSD);
103 }
104
105 /**
106 * Constructor.
107 *
108 * @param gyroNoisePSD gyro noise PSD (Power Spectral Density) expressed in
109 * squared radians per second (rad^2/s).
110 * @param accelerometerNoisePSD accelerometer noise PSD (Power Spectral Density)
111 * expressed in (m^2 * s^-3).
112 * @param accelerometerBiasPSD accelerometer bias random walk PSD (Power Spectral
113 * Density) expressed in (m^2 * s^-5).
114 * @param gyroBiasPSD gyro bias random walk PSD (Power Spectral Density)
115 * expressed in (rad^2 * s^-3).
116 * @param positionNoiseSD position measurement noise SD (Standard Deviation)
117 * per axis.
118 * @param velocityNoiseSD velocity measurement noise SD (Standard Deviation)
119 * per axis.
120 */
121 public INSLooselyCoupledKalmanConfig(
122 final double gyroNoisePSD, final double accelerometerNoisePSD, final double accelerometerBiasPSD,
123 final double gyroBiasPSD, final Distance positionNoiseSD, final Speed velocityNoiseSD) {
124 setValues(gyroNoisePSD, accelerometerNoisePSD, accelerometerBiasPSD, gyroBiasPSD, positionNoiseSD,
125 velocityNoiseSD);
126 }
127
128 /**
129 * Copy constructor.
130 *
131 * @param input input instance to copy data from.
132 */
133 public INSLooselyCoupledKalmanConfig(final INSLooselyCoupledKalmanConfig input) {
134 copyFrom(input);
135 }
136
137 /**
138 * Gets gyro noise PSD (Power Spectral Density) expressed in squared radians per
139 * second (rad^2/s).
140 *
141 * @return gyro noise PSD.
142 */
143 public double getGyroNoisePSD() {
144 return gyroNoisePSD;
145 }
146
147 /**
148 * Sets gyro noise PSD (Power Spectral Density) expressed in squared radians per
149 * second (rad^2/s).
150 *
151 * @param gyroNoisePSD gyro noise PSD.
152 */
153 public void setGyroNoisePSD(final double gyroNoisePSD) {
154 this.gyroNoisePSD = gyroNoisePSD;
155 }
156
157 /**
158 * Gets accelerometer noise PSD (Power Spectral Density) expressed in (m^2 * s^-3).
159 *
160 * @return accelerometer noise PSD.
161 */
162 public double getAccelerometerNoisePSD() {
163 return accelerometerNoisePSD;
164 }
165
166 /**
167 * Sets accelerometer noise PSD (Power Spectral Density) expressed in (m^2 * s^-3).
168 *
169 * @param accelerometerNoisePSD accelerometer noise PSD.
170 */
171 public void setAccelerometerNoisePSD(final double accelerometerNoisePSD) {
172 this.accelerometerNoisePSD = accelerometerNoisePSD;
173 }
174
175 /**
176 * Gets accelerometer bias random walk PSD (Power Spectral Density) expressed
177 * in (m^2 * s^-5).
178 *
179 * @return accelerometer bias random walk PSD.
180 */
181 public double getAccelerometerBiasPSD() {
182 return accelerometerBiasPSD;
183 }
184
185 /**
186 * Sets accelerometer bias random walk PSD (Power Spectral Density) expressed
187 * in (m^2 * s^-5).
188 *
189 * @param accelerometerBiasPSD accelerometer bias random walk PSD.
190 */
191 public void setAccelerometerBiasPSD(final double accelerometerBiasPSD) {
192 this.accelerometerBiasPSD = accelerometerBiasPSD;
193 }
194
195 /**
196 * Gets gyro bias random walk PSD (Power Spectral Density) expressed in
197 * (rad^2 * s^-3).
198 *
199 * @return gyro bias random walk PSD.
200 */
201 public double getGyroBiasPSD() {
202 return gyroBiasPSD;
203 }
204
205 /**
206 * Sets gyro bias random walk PSD (Power Spectral Density) expressed in
207 * (rad^2 * s^-3).
208 *
209 * @param gyroBiasPSD gyro bias random walk PSD.
210 */
211 public void setGyroBiasPSD(final double gyroBiasPSD) {
212 this.gyroBiasPSD = gyroBiasPSD;
213 }
214
215 /**
216 * Gets position measurement noise SD (Standard Deviation) per axis expressed
217 * in meters (m).
218 *
219 * @return position measurement noise SD.
220 */
221 public double getPositionNoiseSD() {
222 return positionNoiseSD;
223 }
224
225 /**
226 * Sets position measurement noise SD (Standard Deviation) per axis expressed
227 * in meters (m).
228 *
229 * @param positionNoiseSD position measurement noise SD.
230 */
231 public void setPositionNoiseSD(final double positionNoiseSD) {
232 this.positionNoiseSD = positionNoiseSD;
233 }
234
235 /**
236 * Gets velocity measurement noise SD (Standard Deviation) per axis expressed in
237 * meters per second (m/s).
238 *
239 * @return velocity measurement noise SD.
240 */
241 public double getVelocityNoiseSD() {
242 return velocityNoiseSD;
243 }
244
245 /**
246 * Sets velocity measurement noise SD (Standard Deviation) per axis expressed in
247 * meters per second (m/s).
248 *
249 * @param velocityNoiseSD velocity measurement noise SD.
250 */
251 public void setVelocityNoiseSD(final double velocityNoiseSD) {
252 this.velocityNoiseSD = velocityNoiseSD;
253 }
254
255 /**
256 * Sets configuration parameters.
257 *
258 * @param gyroNoisePSD gyro noise PSD (Power Spectral Density) expressed in
259 * squared radians per second (rad^2/s).
260 * @param accelerometerNoisePSD accelerometer noise PSD (Power Spectral Density)
261 * expressed in (m^2 * s^-3).
262 * @param accelerometerBiasPSD accelerometer bias random walk PSD (Power Spectral
263 * Density) expressed in (m^2 * s^-5).
264 * @param gyroBiasPSD gyro bias random walk PSD (Power Spectral Density)
265 * expressed in (rad^2 * s^-3).
266 * @param positionNoiseSD position measurement noise SD (Standard Deviation)
267 * per axis expressed in meters (m).
268 * @param velocityNoiseSD velocity measurement noise SD (Standard Deviation)
269 * per axis expressed in meters per second (m/s).
270 */
271 public void setValues(
272 final double gyroNoisePSD, final double accelerometerNoisePSD, final double accelerometerBiasPSD,
273 final double gyroBiasPSD, final double positionNoiseSD, final double velocityNoiseSD) {
274 this.gyroNoisePSD = gyroNoisePSD;
275 this.accelerometerNoisePSD = accelerometerNoisePSD;
276 this.accelerometerBiasPSD = accelerometerBiasPSD;
277 this.gyroBiasPSD = gyroBiasPSD;
278 this.positionNoiseSD = positionNoiseSD;
279 this.velocityNoiseSD = velocityNoiseSD;
280 }
281
282 /**
283 * Gets position measurement noise SD (Standard Deviation) per axis.
284 *
285 * @param result instance where position measurement noise SD will be stored.
286 */
287 public void getPositionNoiseSDAsDistance(final Distance result) {
288 result.setValue(positionNoiseSD);
289 result.setUnit(DistanceUnit.METER);
290 }
291
292 /**
293 * Gets position measurement noise SD (Standard Deviation) per axis.
294 *
295 * @return position measurement noise SD.
296 */
297 public Distance getPositionNoiseSDAsDistance() {
298 return new Distance(positionNoiseSD, DistanceUnit.METER);
299 }
300
301 /**
302 * Sets position measurement noise SD (Standard Deviation) per axis.
303 *
304 * @param positionNoiseSD position measurement noise SD.
305 */
306 public void setPositionNoiseSD(final Distance positionNoiseSD) {
307 this.positionNoiseSD = DistanceConverter.convert(positionNoiseSD.getValue().doubleValue(),
308 positionNoiseSD.getUnit(), DistanceUnit.METER);
309 }
310
311 /**
312 * Gets velocity measurement noise SD (Standard Deviation) per axis.
313 *
314 * @param result instance where velocity measurement noise SD will be stored.
315 */
316 public void getVelocityNoiseSDAsSpeed(final Speed result) {
317 result.setValue(velocityNoiseSD);
318 result.setUnit(SpeedUnit.METERS_PER_SECOND);
319 }
320
321 /**
322 * Gets velocity measurement noise SD (Standard Deviation) per axis.
323 *
324 * @return velocity measurement noise SD per axis.
325 */
326 public Speed getVelocityNoiseSDAsSpeed() {
327 return new Speed(velocityNoiseSD, SpeedUnit.METERS_PER_SECOND);
328 }
329
330 /**
331 * Sets velocity measurement noise SD (Standard Deviation) per axis.
332 *
333 * @param velocityNoiseSD velocity measurement noise SD per axis.
334 */
335 public void setVelocityNoiseSD(final Speed velocityNoiseSD) {
336 this.velocityNoiseSD = SpeedConverter.convert(velocityNoiseSD.getValue().doubleValue(),
337 velocityNoiseSD.getUnit(), SpeedUnit.METERS_PER_SECOND);
338 }
339
340 /**
341 * Sets configuration parameters.
342 *
343 * @param gyroNoisePSD gyro noise PSD (Power Spectral Density) expressed in
344 * squared radians per second (rad^2/s).
345 * @param accelerometerNoisePSD accelerometer noise PSD (Power Spectral Density)
346 * expressed in (m^2 * s^-3).
347 * @param accelerometerBiasPSD accelerometer bias random walk PSD (Power Spectral
348 * Density) expressed in (m^2 * s^-5).
349 * @param gyroBiasPSD gyro bias random walk PSD (Power Spectral Density)
350 * expressed in (rad^2 * s^-3).
351 * @param positionNoiseSD position measurement noise SD (Standard Deviation)
352 * per axis.
353 * @param velocityNoiseSD velocity measurement noise SD (Standard Deviation)
354 * per axis.
355 */
356 public void setValues(
357 final double gyroNoisePSD, final double accelerometerNoisePSD, final double accelerometerBiasPSD,
358 final double gyroBiasPSD, final Distance positionNoiseSD, final Speed velocityNoiseSD) {
359 setValues(gyroNoisePSD, accelerometerNoisePSD, accelerometerBiasPSD, gyroBiasPSD,
360 DistanceConverter.convert(positionNoiseSD.getValue().doubleValue(), positionNoiseSD.getUnit(),
361 DistanceUnit.METER),
362 SpeedConverter.convert(velocityNoiseSD.getValue().doubleValue(), velocityNoiseSD.getUnit(),
363 SpeedUnit.METERS_PER_SECOND));
364 }
365
366 /**
367 * Copies this instance data into provided instance.
368 *
369 * @param output destination instance where data will be copied to.
370 */
371 public void copyTo(final INSLooselyCoupledKalmanConfig output) {
372 output.gyroNoisePSD = gyroNoisePSD;
373 output.accelerometerNoisePSD = accelerometerNoisePSD;
374 output.accelerometerBiasPSD = accelerometerBiasPSD;
375 output.gyroBiasPSD = gyroBiasPSD;
376 output.positionNoiseSD = positionNoiseSD;
377 output.velocityNoiseSD = velocityNoiseSD;
378 }
379
380 /**
381 * Copies data of provided instance into this instance.
382 *
383 * @param input instance to copy data from.
384 */
385 public void copyFrom(final INSLooselyCoupledKalmanConfig input) {
386 gyroNoisePSD = input.gyroNoisePSD;
387 accelerometerNoisePSD = input.accelerometerNoisePSD;
388 accelerometerBiasPSD = input.accelerometerBiasPSD;
389 gyroBiasPSD = input.gyroBiasPSD;
390 positionNoiseSD = input.positionNoiseSD;
391 velocityNoiseSD = input.velocityNoiseSD;
392 }
393
394 /**
395 * Computes and returns hash code for this instance. Hash codes are almost unique
396 * values that are useful for fast classification and storage of objects in collections.
397 *
398 * @return Hash code.
399 */
400 @Override
401 public int hashCode() {
402 return Objects.hash(gyroNoisePSD, accelerometerNoisePSD, accelerometerBiasPSD, gyroBiasPSD,
403 positionNoiseSD, velocityNoiseSD);
404 }
405
406 /**
407 * Checks if provided object is a INSLooselyCoupledKalmanConfig having exactly
408 * the same contents as this instance.
409 *
410 * @param obj Object to be compared.
411 * @return true if both objects are considered to be equal, false otherwise.
412 */
413 @Override
414 public boolean equals(final Object obj) {
415 if (this == obj) {
416 return true;
417 }
418 if (obj == null || getClass() != obj.getClass()) {
419 return false;
420 }
421 final var other = (INSLooselyCoupledKalmanConfig) obj;
422 return equals(other);
423 }
424
425 /**
426 * Checks if provided instance has exactly the same contents as this instance.
427 *
428 * @param other instance to be compared.
429 * @return true if both instances are considered to be equal, false otherwise.
430 */
431 public boolean equals(final INSLooselyCoupledKalmanConfig other) {
432 return equals(other, 0.0);
433 }
434
435 /**
436 * Checks if provided instance has contents similar to this instance up to provided
437 * threshold value.
438 *
439 * @param other instance to be compared.
440 * @param threshold maximum difference allowed for values.
441 * @return true if both instances are considered to be equal (up to provided threshold),
442 * false otherwise.
443 */
444 public boolean equals(final INSLooselyCoupledKalmanConfig other, final double threshold) {
445 if (other == null) {
446 return false;
447 }
448
449 return Math.abs(gyroNoisePSD - other.gyroNoisePSD) <= threshold
450 && Math.abs(accelerometerNoisePSD - other.accelerometerNoisePSD) <= threshold
451 && Math.abs(accelerometerBiasPSD - other.accelerometerBiasPSD) <= threshold
452 && Math.abs(gyroBiasPSD - other.gyroBiasPSD) <= threshold
453 && Math.abs(positionNoiseSD - other.positionNoiseSD) <= threshold
454 && Math.abs(velocityNoiseSD - other.velocityNoiseSD) <= threshold;
455 }
456
457 /**
458 * Makes a copy of this instance.
459 *
460 * @return a copy of this instance.
461 * @throws CloneNotSupportedException if clone fails for some reason.
462 */
463 @Override
464 protected Object clone() throws CloneNotSupportedException {
465 final var result = (INSLooselyCoupledKalmanConfig) super.clone();
466 copyTo(result);
467 return result;
468 }
469 }