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.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20 import com.irurueta.navigation.inertial.BodyKinematics;
21 import com.irurueta.units.Time;
22 import com.irurueta.units.TimeConverter;
23 import com.irurueta.units.TimeUnit;
24
25 import java.util.ArrayList;
26 import java.util.Collection;
27 import java.util.Random;
28
29 /**
30 * Generates body kinematic instances from true body kinematic values taking into
31 * account provided IMU errors for a calibrated IMU.
32 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multisensor
33 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
34 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/IMU_model.m">
35 * https://github.com/ymjdz/MATLAB-Codes/blob/master/IMU_model.m
36 * </a>
37 */
38 @SuppressWarnings("DuplicatedCode")
39 public class BodyKinematicsGenerator {
40
41 /**
42 * Prevents instantiation of utility class.
43 */
44 private BodyKinematicsGenerator() {
45 }
46
47 /**
48 * Generates uncalibrated body kinematics instances containing a certain level
49 * of noise for provided ground-truth body kinematics and IMU errors.
50 * This method ignores IMU quantization levels.
51 *
52 * @param timeInterval time interval between epochs.
53 * @param trueKinematics collection of ground-truth kinematics.
54 * @param errors IMU errors containing calibration data.
55 * @param random a random number generator to generate noise.
56 * @return a new collection containing generated uncalibrated kinematics
57 * for each provided ground-truth one.
58 */
59 public static Collection<BodyKinematics> generate(
60 final Time timeInterval, final Collection<BodyKinematics> trueKinematics, final IMUErrors errors,
61 final Random random) {
62 return generate(convertTime(timeInterval), trueKinematics, errors, random);
63 }
64
65 /**
66 * Generates uncalibrated body kinematics instances containing a certain level
67 * of noise for provided ground-truth body kinematics and IMU errors.
68 * This method ignores IMU quantization levels.
69 *
70 * @param timeInterval time interval between epochs.
71 * @param trueKinematics collection of ground-truth kinematics.
72 * @param errors IMU errors containing calibration data.
73 * @param random a random number generator to generate noise.
74 * @param result collection where generated uncalibrated kinematics
75 * for each provided ground-truth one will be stored.
76 */
77 public static void generate(
78 final Time timeInterval, final Collection<BodyKinematics> trueKinematics, final IMUErrors errors,
79 final Random random, final Collection<BodyKinematics> result) {
80 generate(convertTime(timeInterval), trueKinematics, errors, random, result);
81 }
82
83 /**
84 * Generates uncalibrated body kinematics instances containing a certain level
85 * of noise for provided ground-truth body kinematics and IMU errors.
86 * This method ignores IMU quantization levels.
87 *
88 * @param timeInterval time interval between epochs expressed in seconds (s).
89 * @param trueKinematics collection of ground-truth kinematics.
90 * @param errors IMU errors containing calibration data.
91 * @param random a random number generator to generate noise.
92 * @return a new collection containing generated uncalibrated kinematics
93 * for each provided ground-truth one.
94 */
95 public static Collection<BodyKinematics> generate(
96 final double timeInterval, final Collection<BodyKinematics> trueKinematics, final IMUErrors errors,
97 final Random random) {
98 final var result = new ArrayList<BodyKinematics>();
99 generate(timeInterval, trueKinematics, errors, random, result);
100 return result;
101 }
102
103 /**
104 * Generates uncalibrated body kinematics instances containing a certain level
105 * of noise for provided ground-truth body kinematics and IMU errors.
106 * This method ignores IMU quantization levels.
107 *
108 * @param timeInterval time interval between epochs expressed in seconds (s).
109 * @param trueKinematics collection of ground-truth kinematics.
110 * @param errors IMU errors containing calibration data.
111 * @param random a random number generator to generate noise.
112 * @param result collection where generated uncalibrated kinematics
113 * for each provided ground-truth one will be stored.
114 */
115 public static void generate(
116 final double timeInterval, final Collection<BodyKinematics> trueKinematics,
117 final IMUErrors errors, final Random random, final Collection<BodyKinematics> result) {
118 try {
119 final var trueFibb = new Matrix(BodyKinematics.COMPONENTS, 1);
120 final var ma = errors.getAccelerometerScaleFactorAndCrossCouplingErrors();
121 final var ba = errors.getAccelerometerBiasesAsMatrix();
122 final var trueOmegaIbb = new Matrix(BodyKinematics.COMPONENTS, 1);
123 final var mg = errors.getGyroScaleFactorAndCrossCouplingErrors();
124 final var bg = errors.getGyroBiasesAsMatrix();
125 final var gg = errors.getGyroGDependentBiases();
126 final var identity = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
127 final var tmp33 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
128 final var tmp31a = new Matrix(BodyKinematics.COMPONENTS, 1);
129 final var tmp31b = new Matrix(BodyKinematics.COMPONENTS, 1);
130
131 for (final var k : trueKinematics) {
132 final var r = new BodyKinematics();
133
134 internalGenerate(timeInterval, k, errors, random, null, r, null,
135 trueFibb, ma, ba, trueOmegaIbb, mg, bg, gg, identity, tmp33, tmp31a, tmp31b);
136
137 result.add(r);
138 }
139
140 } catch (final WrongSizeException ignore) {
141 // never happens
142 }
143
144 }
145
146 /**
147 * Generates an uncalibrated body kinematics instance containing a certain level
148 * of noise for provided ground-truth body kinematics and IMU errors.
149 *
150 * @param timeInterval time interval between epochs expressed in seconds (s).
151 * @param trueKinematics ground-truth kinematics.
152 * @param errors IMU errors containing calibration data.
153 * @param random a random number generator to generate noise.
154 * @return uncalibrated body kinematics.
155 */
156 public static BodyKinematics generate(
157 final Time timeInterval, final BodyKinematics trueKinematics, final IMUErrors errors, final Random random) {
158 return generate(convertTime(timeInterval), trueKinematics, errors, random);
159 }
160
161 /**
162 * Generates an uncalibrated body kinematics instance containing a certain level
163 * of noise for provided ground-truth body kinematics and IMU errors.
164 *
165 * @param timeInterval time interval between epochs.
166 * @param trueKinematics ground-truth kinematics.
167 * @param errors IMU errors containing calibration data.
168 * @param random a random number generator to generate noise.
169 * @param result instance where uncalibrated body kinematics will be stored.
170 */
171 public static void generate(
172 final Time timeInterval, final BodyKinematics trueKinematics, final IMUErrors errors, final Random random,
173 final BodyKinematics result) {
174 generate(convertTime(timeInterval), trueKinematics, errors, random, result);
175 }
176
177 /**
178 * Generates an uncalibrated body kinematics instance containing a certain level
179 * of noise for provided ground-truth body kinematics and IMU errors.
180 *
181 * @param timeInterval time interval between epochs expressed in seconds (s).
182 * @param trueKinematics ground-truth kinematics.
183 * @param errors IMU errors containing calibration data.
184 * @param random a random number generator to generate noise.
185 * @return uncalibrated body kinematics.
186 */
187 public static BodyKinematics generate(
188 final double timeInterval, final BodyKinematics trueKinematics, final IMUErrors errors,
189 final Random random) {
190 final var result = new BodyKinematics();
191 generate(timeInterval, trueKinematics, errors, random, result);
192 return result;
193 }
194
195 /**
196 * Generates an uncalibrated body kinematics instance containing a certain level
197 * of noise for provided ground-truth body kinematics and IMU errors.
198 *
199 * @param timeInterval time interval between epochs expressed in seconds (s).
200 * @param trueKinematics ground-truth kinematics.
201 * @param errors IMU errors containing calibration data.
202 * @param random a random number generator to generate noise.
203 * @param result instance where uncalibrated body kinematics will be stored.
204 */
205 public static void generate(
206 final double timeInterval, final BodyKinematics trueKinematics, final IMUErrors errors, final Random random,
207 final BodyKinematics result) {
208 generate(timeInterval, trueKinematics, errors, random, null, result,
209 null);
210 }
211
212 /**
213 * Generates an uncalibrated body kinematics instance containing a certain level
214 * of noise for provided ground-truth body kinematics and IMU errors.
215 *
216 * @param timeInterval time interval between epochs.
217 * @param trueKinematics ground-truth kinematics.
218 * @param errors IMU errors containing calibration data.
219 * @param random a random number generator to generate noise.
220 * @param oldQuantizationResiduals previous quantization residuals from previous
221 * executions. Optional. If provided, must have
222 * length 6, if not provided quantization levels
223 * are ignored.
224 * @param quantizationResiduals generated quantization residuals. Optional.
225 * If provided, must have length 6.
226 * @return generated uncalibrated body kinematics.
227 * @throws IllegalArgumentException if either oldQuantizationResiduals or
228 * quantizationResiduals are not length 6.
229 */
230 public static BodyKinematics generate(
231 final Time timeInterval, final BodyKinematics trueKinematics, final IMUErrors errors, final Random random,
232 final double[] oldQuantizationResiduals, final double[] quantizationResiduals) {
233 return generate(convertTime(timeInterval), trueKinematics, errors, random, oldQuantizationResiduals,
234 quantizationResiduals);
235 }
236
237 /**
238 * Generates an uncalibrated body kinematics instance containing a certain level
239 * of noise for provided ground-truth body kinematics and IMU errors.
240 *
241 * @param timeInterval time interval between epochs.
242 * @param trueKinematics ground-truth kinematics.
243 * @param errors IMU errors containing calibration data.
244 * @param random a random number generator to generate noise.
245 * @param oldQuantizationResiduals previous quantization residuals from previous
246 * executions. Optional. If provided, must have
247 * length 6, if not provided quantization levels
248 * are ignored.
249 * @param result instance where uncalibrated body kinematics will be stored.
250 * @param quantizationResiduals generated quantization residuals. Optional.
251 * If provided, must have length 6.
252 * @throws IllegalArgumentException if either oldQuantizationResiduals or
253 * quantizationResiduals are not length 6.
254 */
255 public static void generate(
256 final Time timeInterval, final BodyKinematics trueKinematics, final IMUErrors errors, final Random random,
257 final double[] oldQuantizationResiduals, final BodyKinematics result,
258 final double[] quantizationResiduals) {
259 generate(convertTime(timeInterval), trueKinematics, errors, random, oldQuantizationResiduals, result,
260 quantizationResiduals);
261 }
262
263 /**
264 * Generates an uncalibrated body kinematics instance containing a certain level
265 * of noise for provided ground-truth body kinematics and IMU errors.
266 *
267 * @param timeInterval time interval between epochs expressed in seconds (s).
268 * @param trueKinematics ground-truth kinematics.
269 * @param errors IMU errors containing calibration data.
270 * @param random a random number generator to generate noise.
271 * @param oldQuantizationResiduals previous quantization residuals from previous
272 * executions. Optional. If provided, must have
273 * length 6, if not provided quantization levels
274 * are ignored.
275 * @param quantizationResiduals generated quantization residuals. Optional.
276 * If provided, must have length 6.
277 * @return generated uncalibrated body kinematics.
278 * @throws IllegalArgumentException if either oldQuantizationResiduals or
279 * quantizationResiduals are not length 6.
280 */
281 public static BodyKinematics generate(
282 final double timeInterval, final BodyKinematics trueKinematics, final IMUErrors errors, final Random random,
283 final double[] oldQuantizationResiduals, final double[] quantizationResiduals) {
284 final var result = new BodyKinematics();
285 generate(timeInterval, trueKinematics, errors, random, oldQuantizationResiduals, result, quantizationResiduals);
286 return result;
287 }
288
289 /**
290 * Generates an uncalibrated body kinematics instance containing a certain level
291 * of noise for provided ground-truth body kinematics and IMU errors.
292 *
293 * @param timeInterval time interval between epochs expressed in seconds (s).
294 * @param trueKinematics ground-truth kinematics.
295 * @param errors IMU errors containing calibration data.
296 * @param random a random number generator to generate noise.
297 * @param oldQuantizationResiduals previous quantization residuals from previous
298 * executions. Optional. If provided, must have
299 * length 6, if not provided quantization levels
300 * are ignored.
301 * @param result instance where uncalibrated body kinematics will be stored.
302 * @param quantizationResiduals generated quantization residuals. Optional.
303 * If provided, must have length 6.
304 * @throws IllegalArgumentException if either oldQuantizationResiduals or
305 * quantizationResiduals are not length 6.
306 */
307 public static void generate(
308 final double timeInterval, final BodyKinematics trueKinematics, final IMUErrors errors, final Random random,
309 final double[] oldQuantizationResiduals, final BodyKinematics result,
310 final double[] quantizationResiduals) {
311 try {
312 final var trueFibb = new Matrix(BodyKinematics.COMPONENTS, 1);
313 final var ma = errors.getAccelerometerScaleFactorAndCrossCouplingErrors();
314 final var ba = errors.getAccelerometerBiasesAsMatrix();
315 final var trueOmegaIbb = new Matrix(BodyKinematics.COMPONENTS, 1);
316 final var mg = errors.getGyroScaleFactorAndCrossCouplingErrors();
317 final var bg = errors.getGyroBiasesAsMatrix();
318 final var gg = errors.getGyroGDependentBiases();
319 final var identity = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
320 final var tmp33 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
321 final var tmp31a = new Matrix(BodyKinematics.COMPONENTS, 1);
322 final var tmp31b = new Matrix(BodyKinematics.COMPONENTS, 1);
323 internalGenerate(timeInterval, trueKinematics, errors, random, oldQuantizationResiduals, result,
324 quantizationResiduals, trueFibb, ma, ba, trueOmegaIbb, mg, bg, gg, identity, tmp33, tmp31a, tmp31b);
325 } catch (final WrongSizeException ignore) {
326 // never happens
327 }
328 }
329
330 /**
331 * Internally generates an uncalibrated body kinematics instance containing a certain
332 * level of noise for provided ground-truth body kinematics and IMU errors.
333 *
334 * @param timeInterval time interval between epochs expressed in seconds (s).
335 * @param trueKinematics ground-truth kinematics.
336 * @param errors IMU errors containing calibration data.
337 * @param random a random number generator to generate noise.
338 * @param oldQuantizationResiduals previous quantization residuals from previous
339 * executions. Optional. If provided, must have
340 * length 6, if not provided quantization levels
341 * are ignored.
342 * @param result instance where uncalibrated body kinematics will be stored.
343 * @param quantizationResiduals generated quantization residuals. Optional.
344 * If provided, must have length 6.
345 * @param trueFibb 3x1 matrix to be reused for specific force storage.
346 * @param ma 3x3 matrix to be reused for accelerometer scaling and
347 * cross coupling errors.
348 * @param ba 3x1 matrix to be reused for accelerometer biases.
349 * @param trueOmegaibb 3x1 matrix to be reused for angular rates storage.
350 * @param mg 3x3 matrix to be reused for gyro scaling and cross
351 * coupling errors.
352 * @param bg 3x1 matrix to be reused for gyro biases.
353 * @param gg 3x3 matrix to be reused for gyro dependant cross
354 * coupling errors.
355 * @param identity 3x3 identity matrix to be reused.
356 * @param tmp33 3x3 matrix to be reused.
357 * @param tmp31a 3x1 matrix to be reused.
358 * @param tmp31b 3x1 matrix to be reused.
359 * @throws WrongSizeException if any of provided matrices has invalid size.
360 * @throws IllegalArgumentException if either oldQuantizationResiduals or
361 * quantizationResiduals are not length 6.
362 */
363 private static void internalGenerate(
364 final double timeInterval, final BodyKinematics trueKinematics, final IMUErrors errors, final Random random,
365 final double[] oldQuantizationResiduals, final BodyKinematics result, final double[] quantizationResiduals,
366 final Matrix trueFibb, final Matrix ma, final Matrix ba, final Matrix trueOmegaibb, final Matrix mg,
367 final Matrix bg, final Matrix gg, final Matrix identity, final Matrix tmp33, final Matrix tmp31a,
368 final Matrix tmp31b) throws WrongSizeException {
369
370 final var comp2 = 2 * BodyKinematics.COMPONENTS;
371 if (oldQuantizationResiduals != null && oldQuantizationResiduals.length != comp2) {
372 throw new IllegalArgumentException();
373 }
374 if (quantizationResiduals != null && quantizationResiduals.length != comp2) {
375 throw new IllegalArgumentException();
376 }
377
378 final double accelNoiseX;
379 final double accelNoiseY;
380 final double accelNoiseZ;
381 final double gyroNoiseX;
382 final double gyroNoiseY;
383 final double gyroNoiseZ;
384 if (timeInterval > 0.0) {
385 final var sqrtTimeInterval = Math.sqrt(timeInterval);
386
387 final var accelNoiseRootPSD = errors.getAccelerometerNoiseRootPSD();
388 final var accelStd = accelNoiseRootPSD / sqrtTimeInterval;
389
390 accelNoiseX = random.nextGaussian() * accelStd;
391 accelNoiseY = random.nextGaussian() * accelStd;
392 accelNoiseZ = random.nextGaussian() * accelStd;
393
394 final var gyroNoiseRootPSD = errors.getGyroNoiseRootPSD();
395 final var gyroStd = gyroNoiseRootPSD / sqrtTimeInterval;
396
397 gyroNoiseX = random.nextGaussian() * gyroStd;
398 gyroNoiseY = random.nextGaussian() * gyroStd;
399 gyroNoiseZ = random.nextGaussian() * gyroStd;
400 } else {
401 accelNoiseX = 0.0;
402 accelNoiseY = 0.0;
403 accelNoiseZ = 0.0;
404
405 gyroNoiseX = 0.0;
406 gyroNoiseY = 0.0;
407 gyroNoiseZ = 0.0;
408 }
409
410 // Calculate accelerometer and gyro outputs using (4.16) and (4.17)
411 trueKinematics.asSpecificForceMatrix(trueFibb);
412 trueKinematics.asAngularRateMatrix(trueOmegaibb);
413
414 identity.add(ma, tmp33);
415 tmp33.multiply(trueFibb, tmp31a);
416 tmp31a.add(ba);
417
418 final var uqFibbX = tmp31a.getElementAtIndex(0) + accelNoiseX;
419 final var uqFibbY = tmp31a.getElementAtIndex(1) + accelNoiseY;
420 final var uqFibbZ = tmp31a.getElementAtIndex(2) + accelNoiseZ;
421
422 identity.add(mg, tmp33);
423 tmp33.multiply(trueOmegaibb, tmp31a);
424 tmp31a.add(bg);
425
426 gg.multiply(trueFibb, tmp31b);
427 tmp31a.add(tmp31b);
428
429 final var uqOmegaIbbX = tmp31a.getElementAtIndex(0) + gyroNoiseX;
430 final var uqOmegaIbbY = tmp31a.getElementAtIndex(1) + gyroNoiseY;
431 final var uqOmegaIbbZ = tmp31a.getElementAtIndex(2) + gyroNoiseZ;
432
433 // Quantize accelerometer outputs
434 if (errors.getAccelerometerQuantizationLevel() > 0.0 && oldQuantizationResiduals != null) {
435 final var accelQuantLevel = errors.getAccelerometerQuantizationLevel();
436 final var fx = accelQuantLevel * Math.round((uqFibbX + oldQuantizationResiduals[0]) / accelQuantLevel);
437 final var fy = accelQuantLevel * Math.round((uqFibbY + oldQuantizationResiduals[1]) / accelQuantLevel);
438 final var fz = accelQuantLevel * Math.round((uqFibbZ + oldQuantizationResiduals[2]) / accelQuantLevel);
439
440 result.setSpecificForceCoordinates(fx, fy, fz);
441
442 if (quantizationResiduals != null) {
443 quantizationResiduals[0] = uqFibbX + oldQuantizationResiduals[0] - fx;
444 quantizationResiduals[1] = uqFibbY + oldQuantizationResiduals[1] - fy;
445 quantizationResiduals[2] = uqFibbZ + oldQuantizationResiduals[2] - fz;
446 }
447 } else {
448 result.setSpecificForceCoordinates(uqFibbX, uqFibbY, uqFibbZ);
449
450 if (quantizationResiduals != null) {
451 quantizationResiduals[0] = 0.0;
452 quantizationResiduals[1] = 0.0;
453 quantizationResiduals[2] = 0.0;
454 }
455 }
456
457 // Quantize gyro outputs
458 if (errors.getGyroQuantizationLevel() > 0.0 && oldQuantizationResiduals != null) {
459 final var gyroQuantLevel = errors.getGyroQuantizationLevel();
460 final var omegaX = gyroQuantLevel * Math.round((uqOmegaIbbX + oldQuantizationResiduals[3])
461 / gyroQuantLevel);
462 final var omegaY = gyroQuantLevel * Math.round((uqOmegaIbbY + oldQuantizationResiduals[4])
463 / gyroQuantLevel);
464 final var omegaZ = gyroQuantLevel * Math.round((uqOmegaIbbZ + oldQuantizationResiduals[5])
465 / gyroQuantLevel);
466
467 result.setAngularRateCoordinates(omegaX, omegaY, omegaZ);
468
469 if (quantizationResiduals != null) {
470 quantizationResiduals[3] = uqOmegaIbbX + oldQuantizationResiduals[3] - omegaX;
471 quantizationResiduals[4] = uqOmegaIbbY + oldQuantizationResiduals[4] - omegaY;
472 quantizationResiduals[5] = uqOmegaIbbZ + oldQuantizationResiduals[5] - omegaZ;
473 }
474 } else {
475 result.setAngularRateCoordinates(uqOmegaIbbX, uqOmegaIbbY, uqOmegaIbbZ);
476
477 if (quantizationResiduals != null) {
478 quantizationResiduals[3] = 0.0;
479 quantizationResiduals[4] = 0.0;
480 quantizationResiduals[5] = 0.0;
481 }
482 }
483 }
484
485 /**
486 * Converts provided time instance to seconds.
487 *
488 * @param time time instance to be converted.
489 * @return converted value.
490 */
491 private static double convertTime(final Time time) {
492 return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
493 }
494 }