0);
            assertEquals(estimator.getStatePositionY(), 0.0, 0.0);
            assertEquals(estimator.getStatePositionZ(), 0.0, 0.0);
            assertEquals(estimator.getStateVelocityX(), 0.0, 0.0);
            assertEquals(estimator.getStateVelocityY(), 0.0, 0.0);
            assertEquals(estimator.getStateVelocityZ(), 0.0, 0.0);
            assertEquals(estimator.getStateAccelerationX(), 0.0, 0.0);
            assertEquals(estimator.getStateAccelerationY(), 0.0, 0.0);
            assertEquals(estimator.getStateAccelerationZ(), 0.0, 0.0);
            assertEquals(estimator.getStateQuaternionA(), 1.0, 0.0);
            assertEquals(estimator.getStateQuaternionB(), 0.0, 0.0);
            assertEquals(estimator.getStateQuaternionC(), 0.0, 0.0);
            assertEquals(estimator.getStateQuaternionD(), 0.0, 0.0);
            assertEquals(estimator.getStateAngularSpeedX(), 0.0, 0.0);
            assertEquals(estimator.getStateAngularSpeedY(), 0.0, 0.0);
            assertEquals(estimator.getStateAngularSpeedZ(), 0.0, 0.0);
            assertFalse(estimator.hasError());
            assertEquals(fullSampleReceived, 2);
            assertEquals(fullSampleProcessed, 2);
            assertEquals(correctWithPositionMeasure, 0);
            assertEquals(correctedWithPositionMeasure, 0);

            // correct providing a certain position (it is not ignored this time)
            estimator.correctWithPositionMeasure(position, positionCovariance);

            // expected velocities
            final double vx = VELOCITY_GAIN * positionX;
            final double vy = VELOCITY_GAIN * positionY;
            final double vz = VELOCITY_GAIN * positionZ;

            // check correctness
            assertEquals(estimator.getStatePositionX(), positionX,
                    ABSOLUTE_ERROR);
            assertEquals(estimator.getStatePositionY(), positionY,
                    ABSOLUTE_ERROR);
            assertEquals(estimator.getStatePositionZ(), positionZ,
                    ABSOLUTE_ERROR);
            assertEquals(estimator.getStateVelocityX(), vx, ABSOLUTE_ERROR);
            assertEquals(estimator.getStateVelocityY(), vy, ABSOLUTE_ERROR);
            assertEquals(estimator.getStateVelocityZ(), vz, ABSOLUTE_ERROR);
            assertEquals(estimator.getStateAccelerationX(), 0.0, 0.0);
            assertEquals(estimator.getStateAccelerationY(), 0.0, 0.0);
            assertEquals(estimator.getStateAccelerationZ(), 0.0, 0.0);
            final Quaternion stateQuaternion = estimator.getStateQuaternion();
            stateQuaternion.normalize();
            assertEquals(stateQuaternion.getA(), 1.0, 0.0);
            assertEquals(stateQuaternion.getB(), 0.0, 0.0);
            assertEquals(stateQuaternion.getC(), 0.0, 0.0);
            assertEquals(stateQuaternion.getD(), 0.0, 0.0);
            assertEquals(estimator.getStateAngularSpeedX(), 0.0, 0.0);
            assertEquals(estimator.getStateAngularSpeedY(), 0.0, 0.0);
            assertEquals(estimator.getStateAngularSpeedZ(), 0.0, 0.0);
            assertFalse(estimator.hasError());
            assertEquals(correctWithPositionMeasure, 1);
            assertEquals(correctedWithPositionMeasure, 1);

            // Force IllegalArgumentException (wrong covariance matrix size)
            final Matrix wrong = new Matrix(4, 4);
            try {
                estimator.correctWithPositionMeasure(position, wrong);
                fail("IllegalArgumentException expected but not thrown");
            } catch (final IllegalArgumentException ignore) {
            }
        }
    }

    @Test
    public void testGetSetPositionCovarianceMatrix() throws AlgebraException {

        final AbsoluteOrientationSlamEstimator estimator =
                new AbsoluteOrientationSlamEstimator();

        // check initial value
        assertEquals(estimator.getPositionCovarianceMatrix(),
                Matrix.identity(3, 3).multiplyByScalarAndReturnNew(
                        KalmanFilter.DEFAULT_MEASUREMENT_NOISE_VARIANCE));

        // set new value
        final Matrix positionCovariance = Matrix.identity(3, 3).
                multiplyByScalarAndReturnNew(ABSOLUTE_ERROR);

        estimator.setPositionCovarianceMatrix(positionCovariance);

        // check correctness
        assertEquals(estimator.getPositionCovarianceMatrix(),
                positionCovariance);

        // Force IllegalArgumentException
        final Matrix wrong = new Matrix(4, 4);
        try {
            estimator.setPositionCovarianceMatrix(wrong);
            fail("IllegalArgumentException expected but not thrown");
        } catch (final IllegalArgumentException ignore) {
        }
    }

    @Test
    public void testCorrectWithPositionMeasureCoordinatesWithoutCovariance()
            throws AlgebraException {
        for (int t = 0; t < REPEAT_TIMES; t++) {
            reset();

            final AbsoluteOrientationSlamEstimator estimator =
                    new AbsoluteOrientationSlamEstimator();
            estimator.setListener(this);

            final UniformRandomizer randomizer = new UniformRandomizer(new Random());

            final double positionX = randomizer.nextDouble();
            final double positionY = randomizer.nextDouble();
            final double positionZ = randomizer.nextDouble();

            final Matrix positionCovariance = Matrix.identity(3, 3).
                    multiplyByScalarAndReturnNew(ABSOLUTE_ERROR);
            estimator.setPositionCovarianceMatrix(positionCovariance);

            // check initial value
            assertEquals(estimator.getStatePositionX(), 0.0, 0.0);
            assertEquals(estimator.getStatePositionY(), 0.0, 0.0);
            assertEquals(estimator.getStatePositionZ(), 0.0, 0.0);
            assertEquals(estimator.getStateVelocityX(), 0.0, 0.0);
            assertEquals(estimator.getStateVelocityY(), 0.0, 0.0);
            assertEquals(estimator.getStateVelocityZ(), 0.0, 0.0);
            assertEquals(estimator.getStateAccelerationX(), 0.0, 0.0);
            assertEquals(estimator.getStateAccelerationY(), 0.0, 0.0);
            assertEquals(estimator.getStateAccelerationZ(), 0.0, 0.0);
            assertEquals(estimator.getStateQuaternionA(), 1.0, 0.0);
            assertEquals(estimator.getStateQuaternionB(), 0.0, 0.0);
            assertEquals(estimator.getStateQuaternionC(), 0.0, 0.0);
            assertEquals(estimator.getStateQuaternionD(), 0.0, 0.0);
            assertEquals(estimator.getStateAngularSpeedX(), 0.0, 0.0);
            assertEquals(estimator.getStateAngularSpeedY(), 0.0, 0.0);
            assertEquals(estimator.getStateAngularSpeedZ(), 0.0, 0.0);
            assertFalse(estimator.hasError());

            assertFalse(estimator.isAccelerometerSampleReceived());
            assertFalse(estimator.isGyroscopeSampleReceived());
            assertEquals(fullSampleReceived, 0);
            assertEquals(fullSampleProcessed, 0);
            assertEquals(correctWithPositionMeasure, 0);
            assertEquals(correctedWithPositionMeasure, 0);

            // predict
            long timestamp = System.currentTimeMillis() * MILLIS_TO_NANOS;
            estimator.updateAccelerometerSample(timestamp, 0.0f, 0.0f, 0.0f);

            assertTrue(estimator.isAccelerometerSampleReceived());
            assertFalse(estimator.isGyroscopeSampleReceived());
            assertFalse(estimator.isOrientationSampleReceived());
            assertEquals(fullSampleReceived, 0);
            assertEquals(fullSampleProcessed, 0);
            assertEquals(correctWithPositionMeasure, 0);
            assertEquals(correctedWithPositionMeasure, 0);

            estimator.updateGyroscopeSample(timestamp, 0.0f, 0.0f, 0.0f);

            