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