1 /*
2 * Copyright (C) 2021 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.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.geometry.InvalidRotationMatrixException;
21 import com.irurueta.navigation.LockedException;
22 import com.irurueta.navigation.NotReadyException;
23 import com.irurueta.navigation.frames.ECEFFrame;
24 import com.irurueta.navigation.frames.NEDFrame;
25 import com.irurueta.navigation.inertial.BodyKinematics;
26 import com.irurueta.navigation.inertial.INSException;
27 import com.irurueta.navigation.inertial.INSLooselyCoupledKalmanConfig;
28 import com.irurueta.navigation.inertial.INSLooselyCoupledKalmanFilteredEstimator;
29 import com.irurueta.navigation.inertial.INSLooselyCoupledKalmanInitializerConfig;
30 import com.irurueta.navigation.inertial.INSLooselyCoupledKalmanState;
31
32 /**
33 * Estimates accumulated drift in body orientation, position and velocity per
34 * unit of time using an INS Kalman filtered solution.
35 */
36 public class KalmanDriftEstimator extends DriftEstimator {
37
38 /**
39 * Configuration parameters for INS Loosely Coupled Kalman filter obtained
40 * through calibration.
41 */
42 private INSLooselyCoupledKalmanConfig kalmanConfig;
43
44 /**
45 * Configuration parameters determining initial system noise covariance matrix.
46 */
47 private INSLooselyCoupledKalmanInitializerConfig initConfig;
48
49 /**
50 * Internal INS loosely coupled Kalman filtered estimator to estimate
51 * accumulated body position, velocity and orientation.
52 */
53 private INSLooselyCoupledKalmanFilteredEstimator kalmanEstimator;
54
55 /**
56 * Current Kalman filter state.
57 */
58 private INSLooselyCoupledKalmanState state;
59
60 /**
61 * Constructor.
62 */
63 public KalmanDriftEstimator() {
64 super();
65 }
66
67 /**
68 * Constructor.
69 *
70 * @param listener listener to handle events.
71 */
72 public KalmanDriftEstimator(final DriftEstimatorListener listener) {
73 super(listener);
74 }
75
76 /**
77 * Constructor.
78 *
79 * @param kalmanConfig configuration parameters obtained through calibration.
80 * @param initConfig configuration parameters determining initial system
81 * noise covariance matrix.
82 */
83 public KalmanDriftEstimator(
84 final INSLooselyCoupledKalmanConfig kalmanConfig,
85 final INSLooselyCoupledKalmanInitializerConfig initConfig) {
86 this.kalmanConfig = kalmanConfig;
87 this.initConfig = initConfig;
88 }
89
90 /**
91 * Constructor.
92 *
93 * @param kalmanConfig configuration parameters obtained through calibration.
94 * @param initConfig configuration parameters determining initial system
95 * noise covariance matrix.
96 * @param listener listener to handle events.
97 */
98 public KalmanDriftEstimator(
99 final INSLooselyCoupledKalmanConfig kalmanConfig,
100 final INSLooselyCoupledKalmanInitializerConfig initConfig,
101 final DriftEstimatorListener listener) {
102 super(listener);
103 this.kalmanConfig = kalmanConfig;
104 this.initConfig = initConfig;
105 }
106
107 /**
108 * Constructor.
109 *
110 * @param referenceFrame initial frame containing body position, velocity and
111 * orientation expressed in ECEF coordinates.
112 * @param kalmanConfig configuration parameters obtained through calibration.
113 * @param initConfig configuration parameters determining initial system
114 * noise covariance matrix.
115 */
116 public KalmanDriftEstimator(
117 final ECEFFrame referenceFrame,
118 final INSLooselyCoupledKalmanConfig kalmanConfig,
119 final INSLooselyCoupledKalmanInitializerConfig initConfig) {
120 super(referenceFrame);
121 this.kalmanConfig = kalmanConfig;
122 this.initConfig = initConfig;
123 }
124
125 /**
126 * Constructor.
127 *
128 * @param referenceFrame initial frame containing body position, velocity and
129 * orientation expressed in ECEF coordinates.
130 * @param kalmanConfig configuration parameters obtained through calibration.
131 * @param initConfig configuration parameters determining initial system
132 * noise covariance matrix.
133 * @param listener listener to handle events.
134 */
135 public KalmanDriftEstimator(
136 final ECEFFrame referenceFrame,
137 final INSLooselyCoupledKalmanConfig kalmanConfig,
138 final INSLooselyCoupledKalmanInitializerConfig initConfig,
139 final DriftEstimatorListener listener) {
140 super(referenceFrame, listener);
141 this.kalmanConfig = kalmanConfig;
142 this.initConfig = initConfig;
143 }
144
145 /**
146 * Constructor.
147 *
148 * @param referenceFrame initial frame containing body position, velocity and
149 * orientation expressed in NED coordinates.
150 * @param kalmanConfig configuration parameters obtained through calibration.
151 * @param initConfig configuration parameters determining initial system
152 * noise covariance matrix.
153 */
154 public KalmanDriftEstimator(
155 final NEDFrame referenceFrame,
156 final INSLooselyCoupledKalmanConfig kalmanConfig,
157 final INSLooselyCoupledKalmanInitializerConfig initConfig) {
158 super(referenceFrame);
159 this.kalmanConfig = kalmanConfig;
160 this.initConfig = initConfig;
161 }
162
163 /**
164 * Constructor.
165 *
166 * @param referenceFrame initial frame containing body position, velocity and
167 * orientation expressed in NED coordinates.
168 * @param kalmanConfig configuration parameters obtained through calibration.
169 * @param initConfig configuration parameters determining initial system
170 * noise covariance matrix.
171 * @param listener listener to handle events.
172 */
173 public KalmanDriftEstimator(
174 final NEDFrame referenceFrame,
175 final INSLooselyCoupledKalmanConfig kalmanConfig,
176 final INSLooselyCoupledKalmanInitializerConfig initConfig,
177 final DriftEstimatorListener listener) {
178 super(referenceFrame, listener);
179 this.kalmanConfig = kalmanConfig;
180 this.initConfig = initConfig;
181 }
182
183 /**
184 * Constructor.
185 *
186 * @param ba acceleration bias to be set.
187 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
188 * @param bg angular speed bias to be set.
189 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
190 * @param kalmanConfig configuration parameters obtained through calibration.
191 * @param initConfig configuration parameters determining initial system
192 * noise covariance matrix.
193 * @throws AlgebraException if provided cross-coupling matrices cannot
194 * be inverted.
195 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
196 */
197 public KalmanDriftEstimator(
198 final AccelerationTriad ba,
199 final Matrix ma,
200 final AngularSpeedTriad bg,
201 final Matrix mg,
202 final INSLooselyCoupledKalmanConfig kalmanConfig,
203 final INSLooselyCoupledKalmanInitializerConfig initConfig)
204 throws AlgebraException {
205 super(ba, ma, bg, mg);
206 this.kalmanConfig = kalmanConfig;
207 this.initConfig = initConfig;
208 }
209
210 /**
211 * Constructor.
212 *
213 * @param ba acceleration bias to be set.
214 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
215 * @param bg angular speed bias to be set.
216 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
217 * @param kalmanConfig configuration parameters obtained through calibration.
218 * @param initConfig configuration parameters determining initial system
219 * noise covariance matrix.
220 * @param listener listener to handle events.
221 * @throws AlgebraException if provided cross-coupling matrices cannot
222 * be inverted.
223 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
224 */
225 public KalmanDriftEstimator(
226 final AccelerationTriad ba,
227 final Matrix ma,
228 final AngularSpeedTriad bg,
229 final Matrix mg,
230 final INSLooselyCoupledKalmanConfig kalmanConfig,
231 final INSLooselyCoupledKalmanInitializerConfig initConfig,
232 final DriftEstimatorListener listener) throws AlgebraException {
233 super(ba, ma, bg, mg, listener);
234 this.kalmanConfig = kalmanConfig;
235 this.initConfig = initConfig;
236 }
237
238 /**
239 * Constructor.
240 *
241 * @param ba acceleration bias to be set.
242 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
243 * @param bg angular speed bias to be set.
244 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
245 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
246 * @param kalmanConfig configuration parameters obtained through calibration.
247 * @param initConfig configuration parameters determining initial system
248 * noise covariance matrix.
249 * @throws AlgebraException if provided cross-coupling matrices cannot
250 * be inverted.
251 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
252 */
253 public KalmanDriftEstimator(
254 final AccelerationTriad ba,
255 final Matrix ma,
256 final AngularSpeedTriad bg,
257 final Matrix mg,
258 final Matrix gg,
259 final INSLooselyCoupledKalmanConfig kalmanConfig,
260 final INSLooselyCoupledKalmanInitializerConfig initConfig)
261 throws AlgebraException {
262 super(ba, ma, bg, mg, gg);
263 this.kalmanConfig = kalmanConfig;
264 this.initConfig = initConfig;
265 }
266
267 /**
268 * Constructor.
269 *
270 * @param ba acceleration bias to be set.
271 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
272 * @param bg angular speed bias to be set.
273 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
274 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
275 * @param kalmanConfig configuration parameters obtained through calibration.
276 * @param initConfig configuration parameters determining initial system
277 * noise covariance matrix.
278 * @param listener listener to handle events.
279 * @throws AlgebraException if provided cross-coupling matrices cannot
280 * be inverted.
281 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
282 */
283 public KalmanDriftEstimator(
284 final AccelerationTriad ba,
285 final Matrix ma,
286 final AngularSpeedTriad bg,
287 final Matrix mg,
288 final Matrix gg,
289 final INSLooselyCoupledKalmanConfig kalmanConfig,
290 final INSLooselyCoupledKalmanInitializerConfig initConfig,
291 final DriftEstimatorListener listener) throws AlgebraException {
292 super(ba, ma, bg, mg, gg, listener);
293 this.kalmanConfig = kalmanConfig;
294 this.initConfig = initConfig;
295 }
296
297 /**
298 * Constructor.
299 *
300 * @param ba acceleration bias to be set expressed in meters per squared second
301 * (m/s`2). Must be 3x1.
302 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
303 * @param bg angular speed bias to be set expressed in radians per second
304 * (rad/s). Must be 3x1.
305 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
306 * @param kalmanConfig configuration parameters obtained through calibration.
307 * @param initConfig configuration parameters determining initial system
308 * noise covariance matrix.
309 * @throws AlgebraException if provided cross-coupling matrices cannot
310 * be inverted.
311 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
312 */
313 public KalmanDriftEstimator(
314 final Matrix ba,
315 final Matrix ma,
316 final Matrix bg,
317 final Matrix mg,
318 final INSLooselyCoupledKalmanConfig kalmanConfig,
319 final INSLooselyCoupledKalmanInitializerConfig initConfig)
320 throws AlgebraException {
321 super(ba, ma, bg, mg);
322 this.kalmanConfig = kalmanConfig;
323 this.initConfig = initConfig;
324 }
325
326 /**
327 * Constructor.
328 *
329 * @param ba acceleration bias to be set expressed in meters per squared second
330 * (m/s`2). Must be 3x1.
331 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
332 * @param bg angular speed bias to be set expressed in radians per second
333 * (rad/s). Must be 3x1.
334 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
335 * @param kalmanConfig configuration parameters obtained through calibration.
336 * @param initConfig configuration parameters determining initial system
337 * noise covariance matrix.
338 * @param listener listener to handle events.
339 * @throws AlgebraException if provided cross-coupling matrices cannot
340 * be inverted.
341 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
342 */
343 public KalmanDriftEstimator(
344 final Matrix ba,
345 final Matrix ma,
346 final Matrix bg,
347 final Matrix mg,
348 final INSLooselyCoupledKalmanConfig kalmanConfig,
349 final INSLooselyCoupledKalmanInitializerConfig initConfig,
350 final DriftEstimatorListener listener) throws AlgebraException {
351 super(ba, ma, bg, mg, listener);
352 this.kalmanConfig = kalmanConfig;
353 this.initConfig = initConfig;
354 }
355
356 /**
357 * Constructor.
358 *
359 * @param ba acceleration bias to be set expressed in meters per squared second
360 * (m/s`2). Must be 3x1.
361 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
362 * @param bg angular speed bias to be set expressed in radians per second
363 * (rad/s). Must be 3x1.
364 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
365 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
366 * @param kalmanConfig configuration parameters obtained through calibration.
367 * @param initConfig configuration parameters determining initial system
368 * noise covariance matrix.
369 * @throws AlgebraException if provided cross-coupling matrices cannot
370 * be inverted.
371 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
372 */
373 public KalmanDriftEstimator(
374 final Matrix ba,
375 final Matrix ma,
376 final Matrix bg,
377 final Matrix mg,
378 final Matrix gg,
379 final INSLooselyCoupledKalmanConfig kalmanConfig,
380 final INSLooselyCoupledKalmanInitializerConfig initConfig)
381 throws AlgebraException {
382 super(ba, ma, bg, mg, gg);
383 this.kalmanConfig = kalmanConfig;
384 this.initConfig = initConfig;
385 }
386
387 /**
388 * Constructor.
389 *
390 * @param ba acceleration bias to be set expressed in meters per squared second
391 * (m/s`2). Must be 3x1.
392 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
393 * @param bg angular speed bias to be set expressed in radians per second
394 * (rad/s). Must be 3x1.
395 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
396 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
397 * @param kalmanConfig configuration parameters obtained through calibration.
398 * @param initConfig configuration parameters determining initial system
399 * noise covariance matrix.
400 * @param listener listener to handle events.
401 * @throws AlgebraException if provided cross-coupling matrices cannot
402 * be inverted.
403 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
404 */
405 public KalmanDriftEstimator(
406 final Matrix ba,
407 final Matrix ma,
408 final Matrix bg,
409 final Matrix mg,
410 final Matrix gg,
411 final INSLooselyCoupledKalmanConfig kalmanConfig,
412 final INSLooselyCoupledKalmanInitializerConfig initConfig,
413 final DriftEstimatorListener listener) throws AlgebraException {
414 super(ba, ma, bg, mg, gg, listener);
415 this.kalmanConfig = kalmanConfig;
416 this.initConfig = initConfig;
417 }
418
419 /**
420 * Constructor.
421 *
422 * @param referenceFrame initial frame containing body position, velocity and
423 * orientation expressed in ECEF coordinates.
424 * @param ba acceleration bias to be set.
425 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
426 * @param bg angular speed bias to be set.
427 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
428 * @param kalmanConfig configuration parameters obtained through calibration.
429 * @param initConfig configuration parameters determining initial system
430 * noise covariance matrix.
431 * @throws AlgebraException if provided cross-coupling matrices cannot
432 * be inverted.
433 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
434 */
435 public KalmanDriftEstimator(
436 final ECEFFrame referenceFrame,
437 final AccelerationTriad ba,
438 final Matrix ma,
439 final AngularSpeedTriad bg,
440 final Matrix mg,
441 final INSLooselyCoupledKalmanConfig kalmanConfig,
442 final INSLooselyCoupledKalmanInitializerConfig initConfig)
443 throws AlgebraException {
444 super(referenceFrame, ba, ma, bg, mg);
445 this.kalmanConfig = kalmanConfig;
446 this.initConfig = initConfig;
447 }
448
449 /**
450 * Constructor.
451 *
452 * @param referenceFrame initial frame containing body position, velocity and
453 * orientation expressed in ECEF coordinates.
454 * @param ba acceleration bias to be set.
455 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
456 * @param bg angular speed bias to be set.
457 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
458 * @param kalmanConfig configuration parameters obtained through calibration.
459 * @param initConfig configuration parameters determining initial system
460 * noise covariance matrix.
461 * @param listener listener to handle events.
462 * @throws AlgebraException if provided cross-coupling matrices cannot
463 * be inverted.
464 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
465 */
466 public KalmanDriftEstimator(
467 final ECEFFrame referenceFrame,
468 final AccelerationTriad ba,
469 final Matrix ma,
470 final AngularSpeedTriad bg,
471 final Matrix mg,
472 final INSLooselyCoupledKalmanConfig kalmanConfig,
473 final INSLooselyCoupledKalmanInitializerConfig initConfig,
474 final DriftEstimatorListener listener) throws AlgebraException {
475 super(referenceFrame, ba, ma, bg, mg, listener);
476 this.kalmanConfig = kalmanConfig;
477 this.initConfig = initConfig;
478 }
479
480 /**
481 * Constructor.
482 *
483 * @param referenceFrame initial frame containing body position, velocity and
484 * orientation expressed in ECEF coordinates.
485 * @param ba acceleration bias to be set.
486 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
487 * @param bg angular speed bias to be set.
488 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
489 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
490 * @param kalmanConfig configuration parameters obtained through calibration.
491 * @param initConfig configuration parameters determining initial system
492 * noise covariance matrix.
493 * @throws AlgebraException if provided cross-coupling matrices cannot
494 * be inverted.
495 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
496 */
497 public KalmanDriftEstimator(
498 final ECEFFrame referenceFrame,
499 final AccelerationTriad ba,
500 final Matrix ma,
501 final AngularSpeedTriad bg,
502 final Matrix mg,
503 final Matrix gg,
504 final INSLooselyCoupledKalmanConfig kalmanConfig,
505 final INSLooselyCoupledKalmanInitializerConfig initConfig)
506 throws AlgebraException {
507 super(referenceFrame, ba, ma, bg, mg, gg);
508 this.kalmanConfig = kalmanConfig;
509 this.initConfig = initConfig;
510 }
511
512 /**
513 * Constructor.
514 *
515 * @param referenceFrame initial frame containing body position, velocity and
516 * orientation expressed in ECEF coordinates.
517 * @param ba acceleration bias to be set.
518 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
519 * @param bg angular speed bias to be set.
520 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
521 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
522 * @param kalmanConfig configuration parameters obtained through calibration.
523 * @param initConfig configuration parameters determining initial system
524 * noise covariance matrix.
525 * @param listener listener to handle events.
526 * @throws AlgebraException if provided cross-coupling matrices cannot
527 * be inverted.
528 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
529 */
530 public KalmanDriftEstimator(
531 final ECEFFrame referenceFrame,
532 final AccelerationTriad ba,
533 final Matrix ma,
534 final AngularSpeedTriad bg,
535 final Matrix mg,
536 final Matrix gg,
537 final INSLooselyCoupledKalmanConfig kalmanConfig,
538 final INSLooselyCoupledKalmanInitializerConfig initConfig,
539 final DriftEstimatorListener listener) throws AlgebraException {
540 super(referenceFrame, ba, ma, bg, mg, gg, listener);
541 this.kalmanConfig = kalmanConfig;
542 this.initConfig = initConfig;
543 }
544
545 /**
546 * Constructor.
547 *
548 * @param referenceFrame initial frame containing body position, velocity and
549 * orientation expressed in ECEF coordinates.
550 * @param ba acceleration bias to be set expressed in meters per squared second
551 * (m/s`2). Must be 3x1.
552 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
553 * @param bg angular speed bias to be set expressed in radians per second
554 * (rad/s). Must be 3x1.
555 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
556 * @param kalmanConfig configuration parameters obtained through calibration.
557 * @param initConfig configuration parameters determining initial system
558 * noise covariance matrix.
559 * @throws AlgebraException if provided cross-coupling matrices cannot
560 * be inverted.
561 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
562 */
563 public KalmanDriftEstimator(
564 final ECEFFrame referenceFrame,
565 final Matrix ba,
566 final Matrix ma,
567 final Matrix bg,
568 final Matrix mg,
569 final INSLooselyCoupledKalmanConfig kalmanConfig,
570 final INSLooselyCoupledKalmanInitializerConfig initConfig)
571 throws AlgebraException {
572 super(referenceFrame, ba, ma, bg, mg);
573 this.kalmanConfig = kalmanConfig;
574 this.initConfig = initConfig;
575 }
576
577 /**
578 * Constructor.
579 *
580 * @param referenceFrame initial frame containing body position, velocity and
581 * orientation expressed in ECEF coordinates.
582 * @param ba acceleration bias to be set expressed in meters per squared second
583 * (m/s`2). Must be 3x1.
584 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
585 * @param bg angular speed bias to be set expressed in radians per second
586 * (rad/s). Must be 3x1.
587 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
588 * @param kalmanConfig configuration parameters obtained through calibration.
589 * @param initConfig configuration parameters determining initial system
590 * noise covariance matrix.
591 * @param listener listener to handle events.
592 * @throws AlgebraException if provided cross-coupling matrices cannot
593 * be inverted.
594 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
595 */
596 public KalmanDriftEstimator(
597 final ECEFFrame referenceFrame,
598 final Matrix ba,
599 final Matrix ma,
600 final Matrix bg,
601 final Matrix mg,
602 final INSLooselyCoupledKalmanConfig kalmanConfig,
603 final INSLooselyCoupledKalmanInitializerConfig initConfig,
604 final DriftEstimatorListener listener) throws AlgebraException {
605 super(referenceFrame, ba, ma, bg, mg, listener);
606 this.kalmanConfig = kalmanConfig;
607 this.initConfig = initConfig;
608 }
609
610 /**
611 * Constructor.
612 *
613 * @param referenceFrame initial frame containing body position, velocity and
614 * orientation expressed in ECEF coordinates.
615 * @param ba acceleration bias to be set expressed in meters per squared second
616 * (m/s`2). Must be 3x1.
617 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
618 * @param bg angular speed bias to be set expressed in radians per second
619 * (rad/s). Must be 3x1.
620 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
621 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
622 * @param kalmanConfig configuration parameters obtained through calibration.
623 * @param initConfig configuration parameters determining initial system
624 * noise covariance matrix.
625 * @throws AlgebraException if provided cross-coupling matrices cannot
626 * be inverted.
627 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
628 */
629 public KalmanDriftEstimator(
630 final ECEFFrame referenceFrame,
631 final Matrix ba,
632 final Matrix ma,
633 final Matrix bg,
634 final Matrix mg,
635 final Matrix gg,
636 final INSLooselyCoupledKalmanConfig kalmanConfig,
637 final INSLooselyCoupledKalmanInitializerConfig initConfig)
638 throws AlgebraException {
639 super(referenceFrame, ba, ma, bg, mg, gg);
640 this.kalmanConfig = kalmanConfig;
641 this.initConfig = initConfig;
642 }
643
644 /**
645 * Constructor.
646 *
647 * @param referenceFrame initial frame containing body position, velocity and
648 * orientation expressed in ECEF coordinates.
649 * @param ba acceleration bias to be set expressed in meters per squared second
650 * (m/s`2). Must be 3x1.
651 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
652 * @param bg angular speed bias to be set expressed in radians per second
653 * (rad/s). Must be 3x1.
654 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
655 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
656 * @param kalmanConfig configuration parameters obtained through calibration.
657 * @param initConfig configuration parameters determining initial system
658 * noise covariance matrix.
659 * @param listener listener to handle events.
660 * @throws AlgebraException if provided cross-coupling matrices cannot
661 * be inverted.
662 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
663 */
664 public KalmanDriftEstimator(
665 final ECEFFrame referenceFrame,
666 final Matrix ba,
667 final Matrix ma,
668 final Matrix bg,
669 final Matrix mg,
670 final Matrix gg,
671 final INSLooselyCoupledKalmanConfig kalmanConfig,
672 final INSLooselyCoupledKalmanInitializerConfig initConfig,
673 final DriftEstimatorListener listener) throws AlgebraException {
674 super(referenceFrame, ba, ma, bg, mg, gg, listener);
675 this.kalmanConfig = kalmanConfig;
676 this.initConfig = initConfig;
677 }
678
679 /**
680 * Constructor.
681 *
682 * @param referenceFrame initial frame containing body position, velocity and
683 * orientation expressed in NED coordinates.
684 * @param ba acceleration bias to be set.
685 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
686 * @param bg angular speed bias to be set.
687 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
688 * @param kalmanConfig configuration parameters obtained through calibration.
689 * @param initConfig configuration parameters determining initial system
690 * noise covariance matrix.
691 * @throws AlgebraException if provided cross-coupling matrices cannot
692 * be inverted.
693 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
694 */
695 public KalmanDriftEstimator(
696 final NEDFrame referenceFrame,
697 final AccelerationTriad ba,
698 final Matrix ma,
699 final AngularSpeedTriad bg,
700 final Matrix mg,
701 final INSLooselyCoupledKalmanConfig kalmanConfig,
702 final INSLooselyCoupledKalmanInitializerConfig initConfig)
703 throws AlgebraException {
704 super(referenceFrame, ba, ma, bg, mg);
705 this.kalmanConfig = kalmanConfig;
706 this.initConfig = initConfig;
707 }
708
709 /**
710 * Constructor.
711 *
712 * @param referenceFrame initial frame containing body position, velocity and
713 * orientation expressed in NED coordinates.
714 * @param ba acceleration bias to be set.
715 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
716 * @param bg angular speed bias to be set.
717 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
718 * @param kalmanConfig configuration parameters obtained through calibration.
719 * @param initConfig configuration parameters determining initial system
720 * noise covariance matrix.
721 * @param listener listener to handle events.
722 * @throws AlgebraException if provided cross-coupling matrices cannot
723 * be inverted.
724 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
725 */
726 public KalmanDriftEstimator(
727 final NEDFrame referenceFrame,
728 final AccelerationTriad ba,
729 final Matrix ma,
730 final AngularSpeedTriad bg,
731 final Matrix mg,
732 final INSLooselyCoupledKalmanConfig kalmanConfig,
733 final INSLooselyCoupledKalmanInitializerConfig initConfig,
734 final DriftEstimatorListener listener) throws AlgebraException {
735 super(referenceFrame, ba, ma, bg, mg, listener);
736 this.kalmanConfig = kalmanConfig;
737 this.initConfig = initConfig;
738 }
739
740 /**
741 * Constructor.
742 *
743 * @param referenceFrame initial frame containing body position, velocity and
744 * orientation expressed in NED coordinates.
745 * @param ba acceleration bias to be set.
746 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
747 * @param bg angular speed bias to be set.
748 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
749 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
750 * @param kalmanConfig configuration parameters obtained through calibration.
751 * @param initConfig configuration parameters determining initial system
752 * noise covariance matrix.
753 * @throws AlgebraException if provided cross-coupling matrices cannot
754 * be inverted.
755 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
756 */
757 public KalmanDriftEstimator(
758 final NEDFrame referenceFrame,
759 final AccelerationTriad ba,
760 final Matrix ma,
761 final AngularSpeedTriad bg,
762 final Matrix mg,
763 final Matrix gg,
764 final INSLooselyCoupledKalmanConfig kalmanConfig,
765 final INSLooselyCoupledKalmanInitializerConfig initConfig)
766 throws AlgebraException {
767 super(referenceFrame, ba, ma, bg, mg, gg);
768 this.kalmanConfig = kalmanConfig;
769 this.initConfig = initConfig;
770 }
771
772 /**
773 * Constructor.
774 *
775 * @param referenceFrame initial frame containing body position, velocity and
776 * orientation expressed in NED coordinates.
777 * @param ba acceleration bias to be set.
778 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
779 * @param bg angular speed bias to be set.
780 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
781 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
782 * @param kalmanConfig configuration parameters obtained through calibration.
783 * @param initConfig configuration parameters determining initial system
784 * noise covariance matrix.
785 * @param listener listener to handle events.
786 * @throws AlgebraException if provided cross-coupling matrices cannot
787 * be inverted.
788 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
789 */
790 public KalmanDriftEstimator(
791 final NEDFrame referenceFrame,
792 final AccelerationTriad ba,
793 final Matrix ma,
794 final AngularSpeedTriad bg,
795 final Matrix mg,
796 final Matrix gg,
797 final INSLooselyCoupledKalmanConfig kalmanConfig,
798 final INSLooselyCoupledKalmanInitializerConfig initConfig,
799 final DriftEstimatorListener listener) throws AlgebraException {
800 super(referenceFrame, ba, ma, bg, mg, gg, listener);
801 this.kalmanConfig = kalmanConfig;
802 this.initConfig = initConfig;
803 }
804
805 /**
806 * Constructor.
807 *
808 * @param referenceFrame initial frame containing body position, velocity and
809 * orientation expressed in NED coordinates.
810 * @param ba acceleration bias to be set expressed in meters per squared second
811 * (m/s`2). Must be 3x1.
812 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
813 * @param bg angular speed bias to be set expressed in radians per second
814 * (rad/s). Must be 3x1.
815 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
816 * @param kalmanConfig configuration parameters obtained through calibration.
817 * @param initConfig configuration parameters determining initial system
818 * noise covariance matrix.
819 * @throws AlgebraException if provided cross-coupling matrices cannot
820 * be inverted.
821 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
822 */
823 public KalmanDriftEstimator(
824 final NEDFrame referenceFrame,
825 final Matrix ba,
826 final Matrix ma,
827 final Matrix bg,
828 final Matrix mg,
829 final INSLooselyCoupledKalmanConfig kalmanConfig,
830 final INSLooselyCoupledKalmanInitializerConfig initConfig)
831 throws AlgebraException {
832 super(referenceFrame, ba, ma, bg, mg);
833 this.kalmanConfig = kalmanConfig;
834 this.initConfig = initConfig;
835 }
836
837 /**
838 * Constructor.
839 *
840 * @param referenceFrame initial frame containing body position, velocity and
841 * orientation expressed in NED coordinates.
842 * @param ba acceleration bias to be set expressed in meters per squared second
843 * (m/s`2). Must be 3x1.
844 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
845 * @param bg angular speed bias to be set expressed in radians per second
846 * (rad/s). Must be 3x1.
847 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
848 * @param kalmanConfig configuration parameters obtained through calibration.
849 * @param initConfig configuration parameters determining initial system
850 * noise covariance matrix.
851 * @param listener listener to handle events.
852 * @throws AlgebraException if provided cross-coupling matrices cannot
853 * be inverted.
854 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
855 */
856 public KalmanDriftEstimator(
857 final NEDFrame referenceFrame,
858 final Matrix ba,
859 final Matrix ma,
860 final Matrix bg,
861 final Matrix mg,
862 final INSLooselyCoupledKalmanConfig kalmanConfig,
863 final INSLooselyCoupledKalmanInitializerConfig initConfig,
864 final DriftEstimatorListener listener) throws AlgebraException {
865 super(referenceFrame, ba, ma, bg, mg, listener);
866 this.kalmanConfig = kalmanConfig;
867 this.initConfig = initConfig;
868 }
869
870 /**
871 * Constructor.
872 *
873 * @param referenceFrame initial frame containing body position, velocity and
874 * orientation expressed in NED coordinates.
875 * @param ba acceleration bias to be set expressed in meters per squared second
876 * (m/s`2). Must be 3x1.
877 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
878 * @param bg angular speed bias to be set expressed in radians per second
879 * (rad/s). Must be 3x1.
880 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
881 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
882 * @param kalmanConfig configuration parameters obtained through calibration.
883 * @param initConfig configuration parameters determining initial system
884 * noise covariance matrix.
885 * @throws AlgebraException if provided cross-coupling matrices cannot
886 * be inverted.
887 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
888 */
889 public KalmanDriftEstimator(
890 final NEDFrame referenceFrame,
891 final Matrix ba,
892 final Matrix ma,
893 final Matrix bg,
894 final Matrix mg,
895 final Matrix gg,
896 final INSLooselyCoupledKalmanConfig kalmanConfig,
897 final INSLooselyCoupledKalmanInitializerConfig initConfig)
898 throws AlgebraException {
899 super(referenceFrame, ba, ma, bg, mg, gg);
900 this.kalmanConfig = kalmanConfig;
901 this.initConfig = initConfig;
902 }
903
904 /**
905 * Constructor.
906 *
907 * @param referenceFrame initial frame containing body position, velocity and
908 * orientation expressed in ECEF coordinates.
909 * @param ba acceleration bias to be set expressed in meters per squared second
910 * (m/s`2). Must be 3x1.
911 * @param ma acceleration cross-coupling errors matrix. Must be 3x3.
912 * @param bg angular speed bias to be set expressed in radians per second
913 * (rad/s). Must be 3x1.
914 * @param mg angular speed cross-coupling errors matrix. Must be 3x3.
915 * @param gg angular speed g-dependent cross-biases matrix. Must be 3x3.
916 * @param kalmanConfig configuration parameters obtained through calibration.
917 * @param initConfig configuration parameters determining initial system
918 * noise covariance matrix.
919 * @param listener listener to handle events.
920 * @throws AlgebraException if provided cross-coupling matrices cannot
921 * be inverted.
922 * @throws IllegalArgumentException if any of the provided matrices are not 3x3.
923 */
924 public KalmanDriftEstimator(
925 final NEDFrame referenceFrame,
926 final Matrix ba,
927 final Matrix ma,
928 final Matrix bg,
929 final Matrix mg,
930 final Matrix gg,
931 final INSLooselyCoupledKalmanConfig kalmanConfig,
932 final INSLooselyCoupledKalmanInitializerConfig initConfig,
933 final DriftEstimatorListener listener) throws AlgebraException {
934 super(referenceFrame, ba, ma, bg, mg, gg, listener);
935 this.kalmanConfig = kalmanConfig;
936 this.initConfig = initConfig;
937 }
938
939 /**
940 * Gets configuration parameters for INS Loosely Coupled Kalman filter obtained
941 * through calibration.
942 *
943 * @return configuration parameters for Kalman filter or null.
944 */
945 public INSLooselyCoupledKalmanConfig getKalmanConfig() {
946 return kalmanConfig;
947 }
948
949 /**
950 * Sets configuration parameters for INS Loosely Coupled Kalman filter obtained
951 * through calibration.
952 *
953 * @param kalmanConfig configuration parameters for Kalman filter.
954 * @throws LockedException if estimator is already running.
955 */
956 public void setKalmanConfig(final INSLooselyCoupledKalmanConfig kalmanConfig) throws LockedException {
957 if (running) {
958 throw new LockedException();
959 }
960
961 this.kalmanConfig = kalmanConfig;
962 }
963
964 /**
965 * Gets configuration parameters determining initial system noise covariance
966 * matrix.
967 *
968 * @return configuration parameters determining initial system noise covariance
969 * matrix or null.
970 */
971 public INSLooselyCoupledKalmanInitializerConfig getInitConfig() {
972 return initConfig;
973 }
974
975 /**
976 * Sets configuration parameters determining initial system noise covariance
977 * matrix.
978 *
979 * @param initConfig configuration parameters determining initial system noise
980 * covariance matrix.
981 * @throws LockedException if estimator is already running.
982 */
983 public void setInitConfig(final INSLooselyCoupledKalmanInitializerConfig initConfig) throws LockedException {
984 if (running) {
985 throw new LockedException();
986 }
987
988 this.initConfig = initConfig;
989 }
990
991 /**
992 * Indicates if estimator is ready to start processing additional kinematics
993 * measurements.
994 *
995 * @return true if ready, false otherwise.
996 */
997 @Override
998 public boolean isReady() {
999 return super.isReady() && kalmanConfig != null && initConfig != null;
1000 }
1001
1002 /**
1003 * Adds a sample of measured body kinematics (accelerometer and gyroscope readings)
1004 * obtained from an IMU, fixes their values and uses fixed values to estimate
1005 * current drift and their average values.
1006 *
1007 * @param kinematics measured body kinematics.
1008 * @throws LockedException if estimator is currently running.
1009 * @throws NotReadyException if estimator is not ready.
1010 * @throws DriftEstimationException if estimation fails for some reason.
1011 */
1012 @Override
1013 public void addBodyKinematics(final BodyKinematics kinematics) throws LockedException, NotReadyException,
1014 DriftEstimationException {
1015 if (isRunning()) {
1016 throw new LockedException();
1017 }
1018
1019 if (!isReady()) {
1020 throw new NotReadyException();
1021 }
1022
1023 try {
1024 running = true;
1025
1026 if (numberOfProcessedSamples == 0) {
1027 if (listener != null) {
1028 listener.onStart(this);
1029 }
1030
1031 initialize();
1032 }
1033
1034 if (fixKinematics) {
1035 // fix kinematics and update kalman filter
1036 fixer.fix(kinematics, fixedKinematics);
1037 } else {
1038 // only update kalman filter
1039 fixedKinematics.copyFrom(kinematics);
1040 }
1041
1042 final var timestamp = getElapsedTimeSeconds();
1043 kalmanEstimator.update(fixedKinematics, timestamp);
1044
1045 if (state == null) {
1046 state = kalmanEstimator.getState();
1047 } else {
1048 kalmanEstimator.getState(state);
1049 }
1050
1051 // estimate drift values
1052 computeCurrentPositionDrift();
1053 computeCurrentVelocityDrift();
1054 computeCurrentOrientationDrift();
1055
1056 numberOfProcessedSamples++;
1057
1058 if (listener != null) {
1059 listener.onBodyKinematicsAdded(this, kinematics, fixedKinematics);
1060 }
1061 } catch (final AlgebraException | InvalidRotationMatrixException | INSException e) {
1062 throw new DriftEstimationException(e);
1063 } finally {
1064 running = false;
1065 }
1066 }
1067
1068 /**
1069 * Resets this estimator to its initial state.
1070 *
1071 * @throws LockedException if estimator is currently running.
1072 */
1073 @Override
1074 public void reset() throws LockedException {
1075 if (isRunning()) {
1076 throw new LockedException();
1077 }
1078
1079 kalmanEstimator = null;
1080 state = null;
1081 running = false;
1082
1083 super.reset();
1084 }
1085
1086 /**
1087 * Gets state of internal Kalman filter containing current body position,
1088 * orientation and velocity after last provided body kinematics measurement.
1089 *
1090 * @return state of internal Kalman filter.
1091 */
1092 public INSLooselyCoupledKalmanState getState() {
1093 return kalmanEstimator != null ? kalmanEstimator.getState() : null;
1094 }
1095
1096 /**
1097 * Gets state of internal Kalman filter containing current body position,
1098 * orientation and velocity after last provided body kinematics measurement.
1099 *
1100 * @param result instance where the result will be stored.
1101 * @return true if the internal Kalman filter state is available and the result is
1102 * updated, false otherwise.
1103 */
1104 public boolean getState(final INSLooselyCoupledKalmanState result) {
1105 if (kalmanEstimator != null) {
1106 return kalmanEstimator.getState(result);
1107 } else {
1108 return false;
1109 }
1110 }
1111
1112 /**
1113 * Initializes internal frame and Kalman estimator when the first body kinematics
1114 * measurement is provided.
1115 *
1116 * @throws InvalidRotationMatrixException if orientation cannot be determined for
1117 * numerical reasons.
1118 */
1119 private void initialize() throws InvalidRotationMatrixException {
1120 final var c = referenceFrame.getCoordinateTransformation();
1121 c.asRotation(refQ);
1122 refQ.inverse(invRefQ);
1123
1124 frame.copyFrom(referenceFrame);
1125
1126 kalmanEstimator = new INSLooselyCoupledKalmanFilteredEstimator(kalmanConfig, initConfig, frame);
1127 }
1128
1129 /**
1130 * Computes current position drift.
1131 */
1132 @Override
1133 protected void computeCurrentPositionDrift() {
1134 final var initX = referenceFrame.getX();
1135 final var initY = referenceFrame.getY();
1136 final var initZ = referenceFrame.getZ();
1137
1138 final var currentX = state.getX();
1139 final var currentY = state.getY();
1140 final var currentZ = state.getZ();
1141
1142 final var diffX = currentX - initX;
1143 final var diffY = currentY - initY;
1144 final var diffZ = currentZ - initZ;
1145
1146 currentPositionDrift.setCoordinates(diffX, diffY, diffZ);
1147
1148 currentPositionDriftMeters = currentPositionDrift.getNorm();
1149 }
1150
1151 /**
1152 * Computes current velocity drift.
1153 */
1154 @Override
1155 protected void computeCurrentVelocityDrift() {
1156 final var initVx = referenceFrame.getVx();
1157 final var initVy = referenceFrame.getVy();
1158 final var initVz = referenceFrame.getVz();
1159
1160 final var currentVx = state.getVx();
1161 final var currentVy = state.getVy();
1162 final var currentVz = state.getVz();
1163
1164 final var diffVx = currentVx - initVx;
1165 final var diffVy = currentVy - initVy;
1166 final var diffVz = currentVz - initVz;
1167
1168 currentVelocityDrift.setCoordinates(diffVx, diffVy, diffVz);
1169
1170 currentVelocityDriftMetersPerSecond = currentVelocityDrift.getNorm();
1171 }
1172
1173 /**
1174 * Computes current orientation drift.
1175 *
1176 * @throws InvalidRotationMatrixException if rotation cannot be accurately
1177 * estimated.
1178 */
1179 @Override
1180 protected void computeCurrentOrientationDrift() throws InvalidRotationMatrixException {
1181 currentC = state.getBodyToEcefCoordinateTransformationMatrix();
1182
1183 q.fromMatrix(currentC);
1184 q.combine(invRefQ);
1185
1186 currentOrientationDriftRadians = q.getRotationAngle();
1187 }
1188 }