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