1 /*
2 * Copyright (C) 2016 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.ar.slam;
17
18 import com.irurueta.algebra.ArrayUtils;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.WrongSizeException;
21 import com.irurueta.geometry.Quaternion;
22 import com.irurueta.numerical.signal.processing.KalmanFilter;
23 import com.irurueta.numerical.signal.processing.SignalProcessingException;
24 import com.irurueta.statistics.InvalidCovarianceMatrixException;
25 import com.irurueta.statistics.MultivariateNormalDist;
26
27 import java.io.Serializable;
28
29 /**
30 * Estimates position, velocity, acceleration, orientation and angular speed
31 * using data from accelerometer, gyroscope and magnetic field.
32 * This implementation of BaseSlamEstimator is an improvement respect
33 * SlamEstimator to be able to take into account absolute orientation respect
34 * Earth frame instead of just having a relative orientation respect to the
35 * start time of the estimator.
36 */
37 @SuppressWarnings("DuplicatedCode")
38 public class AbsoluteOrientationSlamEstimator extends
39 AbsoluteOrientationBaseSlamEstimator<AbsoluteOrientationSlamCalibrationData> implements Serializable {
40
41 /**
42 * Internal state array length.
43 */
44 protected static final int STATE_LENGTH = 16;
45
46 /**
47 * Length of control array (changes in acceleration and angular speed).
48 */
49 protected static final int CONTROL_LENGTH = 13;
50
51 /**
52 * Length of position measurement, to correct any possible deviations of the
53 * system after doing multiple predictions.
54 */
55 private static final int MEASUREMENT_LENGTH = 3;
56
57 /**
58 * Contains device status containing the following values: position-x,
59 * position-y, position-z, quaternion-a, quaternion-b, quaternion-c,
60 * quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z,
61 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z,
62 * angular-velocity-x, angular-velocity-y, angular-velocity-z.
63 */
64 private final double[] x;
65
66 /**
67 * Control signals containing the following values:
68 * quaternion-change-a, quaternion-change-b, quaternion-change-c,
69 * quaternion-change-d, linear-velocity-change-x, linear-velocity-change-y,
70 * linear-velocity-change-z, linear-acceleration-change-x,
71 * linear-acceleration-change-y, linear-acceleration-change-z,
72 * angular-velocity-change-x, angular-velocity-change-y,
73 * angular-velocity-change-z.
74 */
75 private final double[] u;
76
77 /**
78 * Jacobian respect x state during prediction (16x16).
79 */
80 private Matrix jacobianPredictionX;
81
82 /**
83 * Jacobian respect u control during state prediction (16x13).
84 */
85 private Matrix jacobianPredictionU;
86
87 /**
88 * Column matrix containing mU values to be passed as control values during
89 * Kalman filter prediction.
90 */
91 private Matrix control;
92
93 /**
94 * Kalman's filter to remove effects of noise.
95 */
96 private KalmanFilter kalmanFilter;
97
98 /**
99 * Matrix of size 3x16 relating system status with obtained measures.
100 * [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
101 * [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
102 * [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0]
103 */
104 private Matrix measurementMatrix;
105
106 /**
107 * Measurement data for the Kalman filter in a column matrix.
108 * Contains data in the following order:
109 * [accelerationX]
110 * [accelerationY]
111 * [accelerationZ]
112 * [angularSpeedX]
113 * [angularSpeedY]
114 * [angularSpeedZ]
115 */
116 private Matrix measurement;
117
118 /**
119 * Last sample of absolute orientation.
120 */
121 private final Quaternion lastOrientation = new Quaternion();
122
123 /**
124 * Variation of orientation respect to last sample.
125 */
126 private final Quaternion deltaOrientation = new Quaternion();
127
128 /**
129 * Current state orientation.
130 */
131 private final Quaternion stateOrientation = new Quaternion();
132
133 /**
134 * Last sample of linear acceleration along x-axis.
135 */
136 private double lastAccelerationX;
137
138 /**
139 * Last sample of linear acceleration along y-axis.
140 */
141 private double lastAccelerationY;
142
143 /**
144 * Last sample of linear acceleration along z-axis.
145 */
146 private double lastAccelerationZ;
147
148 /**
149 * Last sample of angular speed along x-axis.
150 */
151 private double lastAngularSpeedX;
152
153 /**
154 * Last sample of angular speed along y-axis.
155 */
156 private double lastAngularSpeedY;
157
158 /**
159 * Last sample of angular speed along z-axis.
160 */
161 private double lastAngularSpeedZ;
162
163 /**
164 * Last timestamp of a full sample expressed in nanoseconds since the epoch
165 * time.
166 */
167 private long lastTimestampNanos = -1;
168
169 /**
170 * Indicates whether a prediction has been made to initialize the internal
171 * Kalman filter. Corrections can only be requested to Kalman filter once
172 * a prediction has been made. Attempts to request corrections before having
173 * a prediction will be ignored.
174 */
175 private boolean predictionAvailable;
176
177 /**
178 * Constructor.
179 */
180 public AbsoluteOrientationSlamEstimator() {
181 super();
182 x = new double[STATE_LENGTH];
183 // initial value of quaternion.
184 x[3] = 1.0;
185 u = new double[CONTROL_LENGTH];
186 try {
187 jacobianPredictionX = new Matrix(STATE_LENGTH, STATE_LENGTH);
188 jacobianPredictionU = new Matrix(STATE_LENGTH, CONTROL_LENGTH);
189 control = new Matrix(CONTROL_LENGTH, 1);
190 measurement = new Matrix(MEASUREMENT_LENGTH, 1);
191 measurementMatrix = Matrix.identity(MEASUREMENT_LENGTH, STATE_LENGTH);
192
193 } catch (final WrongSizeException ignore) {
194 // never thrown
195 }
196
197 try {
198 kalmanFilter = new KalmanFilter(STATE_LENGTH, MEASUREMENT_LENGTH, CONTROL_LENGTH);
199 // setup matrix relating position measures with internal status.
200 kalmanFilter.setMeasurementMatrix(measurementMatrix);
201 } catch (final SignalProcessingException ignore) {
202 // never thrown
203 }
204
205 try {
206 // set initial Kalman filter state (state pre and pro must be two
207 // different instances!)
208 kalmanFilter.getStatePre().fromArray(x);
209 kalmanFilter.getStatePost().fromArray(x);
210
211 } catch (final WrongSizeException ignore) {
212 // never thrown
213 }
214 }
215
216 /**
217 * Gets covariance matrix of state variables (position, velocity, acceleration, orientation and
218 * angular speed).
219 * Diagonal elements of matrix returned by this method are in the following order:
220 * position-x, position-y, position-z, quaternion-a, quaternion-b, quaternion-c,
221 * quaternion-d, linear-velocity-x, linear-velocity-y, linear-velocity-z,
222 * linear-acceleration-x, linear-acceleration-y, linear-acceleration-z,
223 * angular-velocity-x, angular-velocity-y, angular-velocity-z
224 * Off-diagonal elements correspond to cross-correlation values of diagonal ones.
225 *
226 * @return covariance matrix of state variables.
227 */
228 @Override
229 public Matrix getStateCovariance() {
230 return kalmanFilter.getStatePre();
231 }
232
233
234 /**
235 * Updates covariance matrix of position measures.
236 * If null is provided, covariance matrix is not updated.
237 *
238 * @param positionCovariance new position covariance determining position
239 * accuracy or null if last available covariance does not need to be
240 * updated.
241 * @throws IllegalArgumentException if provided covariance matrix is not
242 * 3x3.
243 */
244 @Override
245 public void setPositionCovarianceMatrix(final Matrix positionCovariance) {
246 if (positionCovariance != null) {
247 kalmanFilter.setMeasurementNoiseCov(positionCovariance);
248 }
249 }
250
251 /**
252 * Gets current covariance matrix of position measures determining current
253 * accuracy of provided position measures.
254 *
255 * @return covariance matrix of position measures.
256 */
257 @Override
258 public Matrix getPositionCovarianceMatrix() {
259 return kalmanFilter.getMeasurementNoiseCov();
260 }
261
262 /**
263 * Corrects system state with provided position measure using current
264 * position accuracy.
265 *
266 * @param positionX new position along x-axis expressed in meters (m).
267 * @param positionY new position along y-axis expressed in meters (m).
268 * @param positionZ new position along z-axis expressed in meters (m).
269 */
270 @Override
271 public void correctWithPositionMeasure(final double positionX, final double positionY, final double positionZ) {
272 if (!predictionAvailable) {
273 return;
274 }
275
276 if (listener != null) {
277 listener.onCorrectWithPositionMeasure(this);
278 }
279
280 try {
281 measurement.setElementAtIndex(0, positionX);
282 measurement.setElementAtIndex(1, positionY);
283 measurement.setElementAtIndex(2, positionZ);
284
285 updateCorrectedState(kalmanFilter.correct(measurement));
286
287 // copy corrected state to predicted state
288 kalmanFilter.getStatePost().copyTo(kalmanFilter.getStatePre());
289 kalmanFilter.getErrorCovPost().copyTo(kalmanFilter.getErrorCovPre());
290
291 } catch (final SignalProcessingException e) {
292 error = true;
293 }
294
295 if (listener != null) {
296 listener.onCorrectedWithPositionMeasure(this);
297 }
298 }
299
300 /**
301 * Creates an instance of a calibrator to be used with this SLAM estimator.
302 *
303 * @return a calibrator.
304 */
305 public static AbsoluteOrientationSlamCalibrator createCalibrator() {
306 return new AbsoluteOrientationSlamCalibrator();
307 }
308
309 /**
310 * Processes a full sample (accelerometer + gyroscope) to update system
311 * state.
312 */
313 @Override
314 protected void processFullSample() {
315 if (listener != null) {
316 listener.onFullSampleReceived(this);
317 }
318
319 final var timestamp = getMostRecentTimestampNanos();
320 if (lastTimestampNanos < 0) {
321 // first time receiving control data, we just set linear acceleration
322 // and angular speed into system state
323 lastAccelerationX = stateAccelerationX = x[10] = accumulatedAccelerationSampleX;
324 lastAccelerationY = stateAccelerationY = x[11] = accumulatedAccelerationSampleY;
325 lastAccelerationZ = stateAccelerationZ = x[12] = accumulatedAccelerationSampleZ;
326 lastAngularSpeedX = stateAngularSpeedX = x[13] = accumulatedAngularSpeedSampleX;
327 lastAngularSpeedY = stateAngularSpeedY = x[14] = accumulatedAngularSpeedSampleY;
328 lastAngularSpeedZ = stateAngularSpeedZ = x[15] = accumulatedAngularSpeedSampleZ;
329
330 try {
331 kalmanFilter.getStatePre().fromArray(x);
332 kalmanFilter.getStatePost().fromArray(x);
333 } catch (final WrongSizeException ignore) { /* never thrown */ }
334
335 lastTimestampNanos = timestamp;
336
337 if (listener != null) {
338 listener.onFullSampleProcessed(this);
339 }
340
341 return;
342 }
343
344 accumulatedOrientation.normalize();
345
346 lastOrientation.inverse(deltaOrientation);
347 deltaOrientation.combine(accumulatedOrientation);
348 deltaOrientation.normalize();
349
350 final var deltaAccelerationX = accumulatedAccelerationSampleX - lastAccelerationX;
351 final var deltaAccelerationY = accumulatedAccelerationSampleY - lastAccelerationY;
352 final var deltaAccelerationZ = accumulatedAccelerationSampleZ - lastAccelerationZ;
353 final var deltaAngularSpeedX = accumulatedAngularSpeedSampleX - lastAngularSpeedX;
354 final var deltaAngularSpeedY = accumulatedAngularSpeedSampleY - lastAngularSpeedY;
355 final var deltaAngularSpeedZ = accumulatedAngularSpeedSampleZ - lastAngularSpeedZ;
356 final var deltaTimestamp = (timestamp - lastTimestampNanos) * NANOS_TO_SECONDS;
357
358 // when a full sample is received, we update the data model
359 u[0] = deltaOrientation.getA();
360 u[1] = deltaOrientation.getB();
361 u[2] = deltaOrientation.getC();
362 u[3] = deltaOrientation.getD();
363 // change in linear speed
364 u[4] = u[5] = u[6] = 0.0;
365 u[7] = deltaAccelerationX;
366 u[8] = deltaAccelerationY;
367 u[9] = deltaAccelerationZ;
368 u[10] = deltaAngularSpeedX;
369 u[11] = deltaAngularSpeedY;
370 u[12] = deltaAngularSpeedZ;
371
372 if (calibrationData != null && calibrationData.getControlMean() != null) {
373 // if calibrator is available, remove mean to correct possible biases
374 ArrayUtils.subtract(u, calibrationData.getControlMean(), u);
375 }
376
377 StatePredictor.predictWithRotationAdjustment(x, u, deltaTimestamp, x, jacobianPredictionX, jacobianPredictionU);
378
379 // update Kalman filter transition matrix taking into account current
380 // state
381 kalmanFilter.setTransitionMatrix(jacobianPredictionX);
382
383 // update control matrix from control vector jacobian
384 kalmanFilter.setControlMatrix(jacobianPredictionU);
385
386 if (calibrationData != null && calibrationData.getControlMean() != null
387 && calibrationData.getControlCovariance() != null) {
388 // if calibrator is available, propagate covariance to set process
389 // covariance matrix
390 if (normalDist == null) {
391 normalDist = new MultivariateNormalDist(STATE_LENGTH);
392 }
393
394 try {
395 calibrationData.propagateWithControlJacobian(jacobianPredictionU, normalDist);
396 // update kalman filter process noise
397 final var processNoise = kalmanFilter.getProcessNoiseCov();
398
399 // copy normal dist covariance into processNoise
400 normalDist.getCovariance(processNoise);
401 } catch (final InvalidCovarianceMatrixException e) {
402 // ignore
403 }
404 }
405
406 try {
407 // also predict the state using Kalman filter with current control
408 // data
409 control.fromArray(u, true);
410 updatePredictedState(kalmanFilter.predict(control));
411
412 // copy predicted state to corrected state
413 kalmanFilter.getStatePre().copyTo(kalmanFilter.getStatePost());
414 kalmanFilter.getErrorCovPre().copyTo(kalmanFilter.getErrorCovPost());
415
416 predictionAvailable = true;
417 } catch (final Exception e) {
418 error = true;
419 }
420
421 lastOrientation.fromRotation(stateOrientation);
422 lastAccelerationX = stateAccelerationX;
423 lastAccelerationY = stateAccelerationY;
424 lastAccelerationZ = stateAccelerationZ;
425 lastAngularSpeedX = stateAngularSpeedX;
426 lastAngularSpeedY = stateAngularSpeedY;
427 lastAngularSpeedZ = stateAngularSpeedZ;
428 lastTimestampNanos = timestamp;
429
430 if (listener != null) {
431 listener.onFullSampleProcessed(this);
432 }
433 }
434
435 /**
436 * Resets position, linear velocity, linear acceleration, orientation and
437 * angular speed to provided values.
438 * This method implementation also resets Kalman filter state.
439 *
440 * @param statePositionX position along x-axis expressed in meters (m).
441 * @param statePositionY position along y-axis expressed in meters (m).
442 * @param statePositionZ position along z-axis expressed in meters (m).
443 * @param stateVelocityX linear velocity along x-axis expressed in meters
444 * per second (m/s).
445 * @param stateVelocityY linear velocity along y-axis expressed in meters
446 * per second (m/s).
447 * @param stateVelocityZ linear velocity along z-axis expressed in meters
448 * per second (m/s).
449 * @param stateAccelerationX linear acceleration along x-axis expressed in
450 * meters per squared second (m/s^2).
451 * @param stateAccelerationY linear acceleration along y-axis expressed in
452 * meters per squared second (m/s^2).
453 * @param stateAccelerationZ linear acceleration along z-axis expressed in
454 * meters per squared second (m/s^2).
455 * @param stateQuaternionA A value of orientation quaternion.
456 * @param stateQuaternionB B value of orientation quaternion.
457 * @param stateQuaternionC C value of orientation quaternion.
458 * @param stateQuaternionD D value of orientation quaternion.
459 * @param stateAngularSpeedX angular speed along x-axis expressed in radians
460 * per second (rad/s).
461 * @param stateAngularSpeedY angular speed along y-axis expressed in radians
462 * per second (rad/s).
463 * @param stateAngularSpeedZ angular speed along z-axis expressed in radians
464 * per second (rad/s).
465 */
466 @Override
467 protected void reset(
468 final double statePositionX, final double statePositionY, final double statePositionZ,
469 final double stateVelocityX, final double stateVelocityY, final double stateVelocityZ,
470 final double stateAccelerationX, final double stateAccelerationY,
471 final double stateAccelerationZ, final double stateQuaternionA, final double stateQuaternionB,
472 final double stateQuaternionC, final double stateQuaternionD,
473 final double stateAngularSpeedX, final double stateAngularSpeedY,
474 final double stateAngularSpeedZ) {
475 super.reset(statePositionX, statePositionY, statePositionZ,
476 stateVelocityX, stateVelocityY, stateVelocityZ,
477 stateAccelerationX, stateAccelerationY, stateAccelerationZ,
478 stateQuaternionA, stateQuaternionB, stateQuaternionC, stateQuaternionD,
479 stateAngularSpeedX, stateAngularSpeedY, stateAngularSpeedZ);
480 //noinspection ConstantValue
481 if (stateOrientation != null) {
482 stateOrientation.setA(stateQuaternionA);
483 stateOrientation.setB(stateQuaternionB);
484 stateOrientation.setC(stateQuaternionC);
485 stateOrientation.setD(stateQuaternionD);
486 }
487
488 //noinspection ConstantValue
489 if (lastOrientation != null) {
490 lastOrientation.setA(1.0);
491 lastOrientation.setB(0.0);
492 lastOrientation.setC(0.0);
493 lastOrientation.setD(0.0);
494 }
495
496 if (x != null) {
497 // position
498 x[0] = statePositionX;
499 x[1] = statePositionY;
500 x[2] = statePositionZ;
501
502 // quaternion
503 x[3] = stateQuaternionA;
504 x[4] = stateQuaternionB;
505 x[5] = stateQuaternionC;
506 x[6] = stateQuaternionD;
507
508 // velocity
509 x[7] = stateVelocityX;
510 x[8] = stateVelocityY;
511 x[9] = stateVelocityZ;
512
513 // linear acceleration
514 x[10] = stateAccelerationX;
515 x[11] = stateAccelerationY;
516 x[12] = stateAccelerationZ;
517
518 // angular speed
519 x[13] = stateAngularSpeedX;
520 x[14] = stateAngularSpeedY;
521 x[15] = stateAngularSpeedZ;
522
523 try {
524 // set initial Kalman filter state (state pre and pro must be two
525 // different instances!)
526 kalmanFilter.getStatePre().fromArray(x);
527 kalmanFilter.getStatePost().fromArray(x);
528 } catch (final WrongSizeException ignore) {
529 // never thrown
530 }
531 }
532
533 error = false;
534 lastTimestampNanos = -1;
535 predictionAvailable = false;
536 }
537
538 /**
539 * Updates state data of the device by using state matrix obtained after
540 * prediction from Kalman filter.
541 * to ensure that state follows proper values (specially on quaternions),
542 * we keep x values, which have been predicted using the state predictor,
543 * which uses analytical values.
544 * We then updated x using latest Kalman filter state for next iteration
545 * on state predictor.
546 *
547 * @param state state matrix obtained from Kalman filter.
548 */
549 private void updatePredictedState(final Matrix state) {
550 // position
551 statePositionX = x[0];
552 x[0] = state.getElementAtIndex(0);
553 statePositionY = x[1];
554 x[1] = state.getElementAtIndex(1);
555 statePositionZ = x[2];
556 x[2] = state.getElementAtIndex(2);
557
558 // quaternion state predictor is more reliable than Kalman filter, for
559 // that reason we ignore predicted quaternion values on Kalman filter and
560 // simply keep predicted ones. Besides, typically gyroscope samples are
561 // much more reliable than accelerometer ones. For that reason state
562 // elements corresponding to quaternion (3 to 6) are not copied into mX
563 // array.
564 stateQuaternionA = x[3];
565 stateQuaternionB = x[4];
566 stateQuaternionC = x[5];
567 stateQuaternionD = x[6];
568
569 stateOrientation.setA(stateQuaternionA);
570 stateOrientation.setB(stateQuaternionB);
571 stateOrientation.setC(stateQuaternionC);
572 stateOrientation.setD(stateQuaternionD);
573
574 // velocity
575 stateVelocityX = x[7];
576 x[7] = state.getElementAtIndex(7);
577 stateVelocityY = x[8];
578 x[8] = state.getElementAtIndex(8);
579 stateVelocityZ = x[9];
580 x[9] = state.getElementAtIndex(9);
581
582 // linear acceleration
583 stateAccelerationX = x[10];
584 x[10] = state.getElementAtIndex(10);
585 stateAccelerationY = x[11];
586 x[11] = state.getElementAtIndex(11);
587 stateAccelerationZ = x[12];
588 x[12] = state.getElementAtIndex(12);
589
590 // angular velocity
591 stateAngularSpeedX = x[13];
592 x[13] = state.getElementAtIndex(13);
593 stateAngularSpeedY = x[14];
594 x[14] = state.getElementAtIndex(14);
595 stateAngularSpeedZ = x[15];
596 x[15] = state.getElementAtIndex(15);
597 }
598
599 /**
600 * Updates state data of the device by using state matrix obtained from
601 * Kalman filter after correction.
602 *
603 * @param state state matrix obtained from Kalman filter.
604 */
605 private void updateCorrectedState(final Matrix state) {
606 // position
607 statePositionX = x[0] = state.getElementAtIndex(0);
608 statePositionY = x[1] = state.getElementAtIndex(1);
609 statePositionZ = x[2] = state.getElementAtIndex(2);
610
611 // quaternion
612 stateQuaternionA = x[3] = state.getElementAtIndex(3);
613 stateQuaternionB = x[4] = state.getElementAtIndex(4);
614 stateQuaternionC = x[5] = state.getElementAtIndex(5);
615 stateQuaternionD = x[6] = state.getElementAtIndex(6);
616
617 stateOrientation.setA(stateQuaternionA);
618 stateOrientation.setB(stateQuaternionB);
619 stateOrientation.setC(stateQuaternionC);
620 stateOrientation.setD(stateQuaternionD);
621 stateOrientation.normalize();
622
623 // velocity
624 stateVelocityX = x[7] = state.getElementAtIndex(7);
625 stateVelocityY = x[8] = state.getElementAtIndex(8);
626 stateVelocityZ = x[9] = state.getElementAtIndex(9);
627
628 // linear acceleration
629 stateAccelerationX = x[10] = state.getElementAtIndex(10);
630 stateAccelerationY = x[11] = state.getElementAtIndex(11);
631 stateAccelerationZ = x[12] = state.getElementAtIndex(12);
632
633 // angular velocity
634 stateAngularSpeedX = x[13] = state.getElementAtIndex(13);
635 stateAngularSpeedY = x[14] = state.getElementAtIndex(14);
636 stateAngularSpeedZ = x[15] = state.getElementAtIndex(15);
637 }
638 }