ExhaustiveGyroscopeIntervalDetectorThresholdFactorOptimizer.java

/*
 * Copyright (C) 2021 Alberto Irurueta Carro (alberto@irurueta.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package com.irurueta.navigation.inertial.calibration.intervals.thresholdfactor;

import com.irurueta.navigation.LockedException;
import com.irurueta.navigation.NotReadyException;
import com.irurueta.navigation.inertial.calibration.BodyKinematicsSequence;
import com.irurueta.navigation.inertial.calibration.gyroscope.GyroscopeCalibratorMeasurementOrSequenceType;
import com.irurueta.navigation.inertial.calibration.gyroscope.GyroscopeNonLinearCalibrator;

/**
 * Optimizes the threshold factor for interval detection of accelerometer and gyroscope data
 * based on results of calibration.
 * Only gyroscope calibrators based on unknown orientation are supported (in other terms,
 * calibrators must be {@link GyroscopeNonLinearCalibrator} and must support
 * {@link GyroscopeCalibratorMeasurementOrSequenceType#BODY_KINEMATICS_SEQUENCE}).
 * This implementation makes an exhaustive search between minimum and maximum
 * threshold factor values to find the threshold value that produces the
 * minimum Mean Square Error (MSE) for calibration parameters.
 */
public class ExhaustiveGyroscopeIntervalDetectorThresholdFactorOptimizer extends
        GyroscopeIntervalDetectorThresholdFactorOptimizer {

    /**
     * Default step value to make exhaustive search of threshold factor values.
     */
    public static final double DEFAULT_STEP = 1.0;

    /**
     * Step to make exhaustive search of threshold factor values between
     * {@link #getMaxThresholdFactor()} and {@link #getMaxThresholdFactor()}.
     */
    private double thresholdFactorStep = DEFAULT_STEP;

    /**
     * Constructor.
     */
    public ExhaustiveGyroscopeIntervalDetectorThresholdFactorOptimizer() {
        super();
    }

    /**
     * Constructor.
     *
     * @param dataSource instance in charge of retrieving data for this optimizer.
     */
    public ExhaustiveGyroscopeIntervalDetectorThresholdFactorOptimizer(
            final GyroscopeIntervalDetectorThresholdFactorOptimizerDataSource dataSource) {
        super(dataSource);
    }

    /**
     * Constructor.
     *
     * @param calibrator a gyroscope calibrator to be used to optimize its
     *                   Mean Square Error (MSE).
     * @throws IllegalArgumentException if gyroscope calibrator does not use
     *                                  {@link BodyKinematicsSequence} sequences.
     */
    public ExhaustiveGyroscopeIntervalDetectorThresholdFactorOptimizer(final GyroscopeNonLinearCalibrator calibrator) {
        super(calibrator);
    }

    /**
     * Constructor.
     *
     * @param dataSource instance in charge of retrieving data for this optimizer.
     * @param calibrator a gyroscope calibrator to be used to optimize its
     *                   Mean Square Error (MSE).
     * @throws IllegalArgumentException if gyroscope calibrator does not use
     *                                  {@link BodyKinematicsSequence} sequences.
     */
    public ExhaustiveGyroscopeIntervalDetectorThresholdFactorOptimizer(
            final GyroscopeIntervalDetectorThresholdFactorOptimizerDataSource dataSource,
            final GyroscopeNonLinearCalibrator calibrator) {
        super(dataSource, calibrator);
    }

    /**
     * Gets the step to make exhaustive search of threshold values between
     * {@link #getMaxThresholdFactor()} and {@link #getMaxThresholdFactor()}.
     *
     * @return step to make exhaustive search of threshold values.
     */
    public double getThresholdFactorStep() {
        return thresholdFactorStep;
    }

    /**
     * Sets step to make exhaustive search of threshold values between
     * {@link #getMaxThresholdFactor()} and {@link #getMaxThresholdFactor()}.
     *
     * @param thresholdStep step to make exhaustive search of threshold values.
     * @throws LockedException          if optimizer is already running.
     * @throws IllegalArgumentException if provided value is zero or negative.
     */
    public void setThresholdFactorStep(final double thresholdStep) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (thresholdStep <= 0.0) {
            throw new IllegalArgumentException();
        }

        thresholdFactorStep = thresholdStep;
    }

    /**
     * Optimizes the threshold factor for a static interval detector or measurement
     * generator to minimize MSE (Minimum Squared Error) of estimated
     * calibration parameters.
     *
     * @return optimized threshold factor.
     * @throws NotReadyException                                 if this optimizer is not ready to start optimization.
     * @throws LockedException                                   if optimizer is already running.
     * @throws IntervalDetectorThresholdFactorOptimizerException if optimization fails for
     *                                                           some reason.
     */
    @Override
    public double optimize() throws NotReadyException, LockedException,
            IntervalDetectorThresholdFactorOptimizerException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        var hasResult = false;
        minMse = Double.MAX_VALUE;
        try {
            running = true;

            initProgress();
            final var progressStep = (float) (thresholdFactorStep
                    / (thresholdFactorStep + maxThresholdFactor - minThresholdFactor));

            if (listener != null) {
                listener.onOptimizeStart(this);
            }

            for (var thresholdFactor = minThresholdFactor;
                 thresholdFactor <= maxThresholdFactor;
                 thresholdFactor += thresholdFactorStep) {
                try {
                    evaluateForThresholdFactor(thresholdFactor);
                    hasResult = true;
                } catch (final Exception ignore) {
                    // when an error occurs, skip to the next iteration
                }

                progress += progressStep;
                checkAndNotifyProgress();
            }

            if (!hasResult) {
                throw new IntervalDetectorThresholdFactorOptimizerException();
            }

            if (listener != null) {
                listener.onOptimizeEnd(this);
            }

        } finally {
            running = false;
        }

        return optimalThresholdFactor;
    }
}