INSLooselyCoupledKalmanInitializerConfigCreator.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;

import com.irurueta.navigation.NotReadyException;
import com.irurueta.navigation.inertial.INSLooselyCoupledKalmanInitializerConfig;

/**
 * Utility class to create {@link INSLooselyCoupledKalmanInitializerConfig} by combining
 * different sources of estimated data.
 * Sources of data can be any accelerometer calibrator implementing
 * {@link AccelerometerBiasUncertaintySource}, or any gyroscope calibrator implementing
 * {@link GyroscopeBiasUncertaintySource}.
 */
public class INSLooselyCoupledKalmanInitializerConfigCreator {

    /**
     * A source of estimated accelerometer bias uncertainty.
     */
    private AccelerometerBiasUncertaintySource accelerometerBiasUncertaintySource;

    /**
     * A source of estimated gyroscope bias uncertainty.
     */
    private GyroscopeBiasUncertaintySource gyroscopeBiasUncertaintySource;

    /**
     * A source of attitude uncertainty.
     */
    private AttitudeUncertaintySource attitudeUncertaintySource;

    /**
     * A source of velocity uncertainty.
     */
    private VelocityUncertaintySource velocityUncertaintySource;

    /**
     * A source of position uncertainty.
     */
    private PositionUncertaintySource positionUncertaintySource;

    /**
     * Constructor.
     */
    public INSLooselyCoupledKalmanInitializerConfigCreator() {
    }

    /**
     * Constructor.
     *
     * @param accelerometerBiasUncertaintySource a source of estimated accelerometer bias uncertainty.
     * @param gyroscopeBiasUncertaintySource     a source of estimated gyroscope bias uncertainty.
     * @param attitudeUncertaintySource          a source of attitude uncertainty.
     * @param velocityUncertaintySource          a source of velocity uncertainty.
     * @param positionUncertaintySource          a source of position uncertainty.
     */
    public INSLooselyCoupledKalmanInitializerConfigCreator(
            final AccelerometerBiasUncertaintySource accelerometerBiasUncertaintySource,
            final GyroscopeBiasUncertaintySource gyroscopeBiasUncertaintySource,
            final AttitudeUncertaintySource attitudeUncertaintySource,
            final VelocityUncertaintySource velocityUncertaintySource,
            final PositionUncertaintySource positionUncertaintySource) {
        this.accelerometerBiasUncertaintySource = accelerometerBiasUncertaintySource;
        this.gyroscopeBiasUncertaintySource = gyroscopeBiasUncertaintySource;
        this.attitudeUncertaintySource = attitudeUncertaintySource;
        this.velocityUncertaintySource = velocityUncertaintySource;
        this.positionUncertaintySource = positionUncertaintySource;
    }

    /**
     * Constructor.
     *
     * @param accelerometerBiasUncertaintySource a source of estimated accelerometer bias uncertainty.
     * @param gyroscopeBiasUncertaintySource     a source of estimated gyroscope bias uncertainty.
     * @param randomWalkEstimator                a random walk estimator.
     */
    public INSLooselyCoupledKalmanInitializerConfigCreator(
            final AccelerometerBiasUncertaintySource accelerometerBiasUncertaintySource,
            final GyroscopeBiasUncertaintySource gyroscopeBiasUncertaintySource,
            final RandomWalkEstimator randomWalkEstimator) {
        this(accelerometerBiasUncertaintySource, gyroscopeBiasUncertaintySource, randomWalkEstimator,
                randomWalkEstimator, randomWalkEstimator);
    }

    /**
     * Gets the source of estimated accelerometer bias uncertainty.
     *
     * @return source of estimated accelerometer bias uncertainty.
     */
    public AccelerometerBiasUncertaintySource getAccelerometerBiasUncertaintySource() {
        return accelerometerBiasUncertaintySource;
    }

    /**
     * Sets source of estimated accelerometer bias uncertainty.
     *
     * @param accelerometerBiasUncertaintySource source of estimated accelerometer bias uncertainty.
     */
    public void setAccelerometerBiasUncertaintySource(
            final AccelerometerBiasUncertaintySource accelerometerBiasUncertaintySource) {
        this.accelerometerBiasUncertaintySource = accelerometerBiasUncertaintySource;
    }

    /**
     * Gets the source of estimated gyroscope bias uncertainty.
     *
     * @return source of estimated gyroscope bias uncertainty.
     */
    public GyroscopeBiasUncertaintySource getGyroscopeBiasUncertaintySource() {
        return gyroscopeBiasUncertaintySource;
    }

    /**
     * Sets source of estimated gyroscope bias uncertainty.
     *
     * @param gyroscopeBiasUncertaintySource source of estimated gyroscope bias uncertainty.
     */
    public void setGyroscopeBiasUncertaintySource(final GyroscopeBiasUncertaintySource gyroscopeBiasUncertaintySource) {
        this.gyroscopeBiasUncertaintySource = gyroscopeBiasUncertaintySource;
    }

    /**
     * Gets source of attitude uncertainty.
     *
     * @return source of attitude uncertainty.
     */
    public AttitudeUncertaintySource getAttitudeUncertaintySource() {
        return attitudeUncertaintySource;
    }

    /**
     * Sets source of attitude uncertainty.
     *
     * @param attitudeUncertaintySource source of attitude uncertainty.
     */
    public void setAttitudeUncertaintySource(final AttitudeUncertaintySource attitudeUncertaintySource) {
        this.attitudeUncertaintySource = attitudeUncertaintySource;
    }

    /**
     * Gets the source of velocity uncertainty.
     *
     * @return source of velocity uncertainty.
     */
    public VelocityUncertaintySource getVelocityUncertaintySource() {
        return velocityUncertaintySource;
    }

    /**
     * Sets source of velocity uncertainty.
     *
     * @param velocityUncertaintySource source of velocity uncertainty.
     */
    public void setVelocityUncertaintySource(final VelocityUncertaintySource velocityUncertaintySource) {
        this.velocityUncertaintySource = velocityUncertaintySource;
    }

    /**
     * Gets the source of position uncertainty.
     *
     * @return source of position uncertainty.
     */
    public PositionUncertaintySource getPositionUncertaintySource() {
        return positionUncertaintySource;
    }

    /**
     * Sets source of position uncertainty.
     *
     * @param positionUncertaintySource source of position uncertainty.
     */
    public void setPositionUncertaintySource(final PositionUncertaintySource positionUncertaintySource) {
        this.positionUncertaintySource = positionUncertaintySource;
    }

    /**
     * Indicates whether all sources have been provided to be able to
     * create a {@link INSLooselyCoupledKalmanInitializerConfig} instance.
     *
     * @return true if the creator is ready, false otherwise.
     */
    public boolean isReady() {
        return accelerometerBiasUncertaintySource != null
                && accelerometerBiasUncertaintySource.getEstimatedBiasStandardDeviationNorm() != null
                && gyroscopeBiasUncertaintySource != null
                && gyroscopeBiasUncertaintySource.getEstimatedBiasStandardDeviationNorm() != null
                && attitudeUncertaintySource != null
                && velocityUncertaintySource != null
                && positionUncertaintySource != null;
    }

    /**
     * Creates a {@link INSLooselyCoupledKalmanInitializerConfig} instance containing estimated
     * parameters during calibration.
     *
     * @return instance containing initial configuration data.
     * @throws NotReadyException if the creator is not ready.
     */
    public INSLooselyCoupledKalmanInitializerConfig create() throws NotReadyException {
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var attitudeUncertainty = attitudeUncertaintySource.getAttitudeUncertainty();
        final var velocityUncertainty = velocityUncertaintySource.getVelocityUncertainty();
        final var positionUncertainty = positionUncertaintySource.getPositionUncertainty();
        final var accelerationBiasUncertainty =
                accelerometerBiasUncertaintySource.getEstimatedBiasStandardDeviationNorm();
        final var gyroBiasUncertainty = gyroscopeBiasUncertaintySource.getEstimatedBiasStandardDeviationNorm();

        return new INSLooselyCoupledKalmanInitializerConfig(attitudeUncertainty, velocityUncertainty,
                positionUncertainty, accelerationBiasUncertainty, gyroBiasUncertainty);
    }
}