GNSSKalmanState.java

/*
 * Copyright (C) 2019 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.gnss;

import com.irurueta.algebra.Matrix;

import java.io.Serializable;
import java.util.Objects;

/**
 * Kalman filter state for filtered GNSS estimation.
 */
public class GNSSKalmanState implements Serializable, Cloneable {

    /**
     * Contains estimation of ECEF position and velocity, and estimated clock
     * offset and drift.
     */
    private GNSSEstimation estimation;

    /**
     * Kalman filter error covariance matrix.
     */
    private Matrix covariance;

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

    /**
     * Constructor.
     *
     * @param estimation GNSS estimation containing ECEF position, velocity and clock
     *                   offset and drift.
     * @param covariance Kalman filter error covariance matrix. Must be 8x8.
     * @throws IllegalArgumentException if provided covariance matrix is not 8x8.
     */
    public GNSSKalmanState(final GNSSEstimation estimation, final Matrix covariance) {
        setEstimation(estimation);
        setCovariance(covariance);
    }

    /**
     * Copy constructor.
     *
     * @param input input instance to copy data from.
     */
    public GNSSKalmanState(final GNSSKalmanState input) {
        copyFrom(input);
    }

    /**
     * Gets estimation of ECEF position and velocity, and estimated clock offset
     * and drift.
     * If GNSS estimation data is not available, this method makes no action.
     *
     * @param result instance where result data will be copied to.
     * @return true if result data has been copied, false otherwise.
     */
    public boolean getEstimation(final GNSSEstimation result) {
        if (estimation != null) {
            estimation.copyTo(result);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimation of ECEF position and velocity, and estimated clock offset
     * and drift.
     *
     * @return GNSS estimation.
     */
    public GNSSEstimation getEstimation() {
        return estimation;
    }

    /**
     * Sets estimation of ECEF position and velocity, and estimated clock offset
     * and drift.
     *
     * @param estimation GNSS estimation to be set.
     */
    public void setEstimation(final GNSSEstimation estimation) {
        this.estimation = estimation;
    }

    /**
     * Gets Kalman filter error covariance matrix.
     *
     * @param result instance where result data will be copied to.
     * @return true if result data has been copied, false otherwise.
     */
    public boolean getCovariance(final Matrix result) {
        if (covariance != null) {
            covariance.copyTo(result);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets Kalman filter error covariance matrix.
     *
     * @return Kalman filter error covariance matrix.
     */
    public Matrix getCovariance() {
        return covariance;
    }

    /**
     * Sets Kalman filter error covariance matrix.
     *
     * @param covariance Kalman filter error covariance matrix to be set.
     * @throws IllegalArgumentException if provided covariance matrix is not 8x8.
     */
    public void setCovariance(final Matrix covariance) {
        if (covariance.getRows() != GNSSEstimation.NUM_PARAMETERS
                || covariance.getColumns() != GNSSEstimation.NUM_PARAMETERS) {
            throw new IllegalArgumentException();
        }

        this.covariance = covariance;
    }

    /**
     * Copies this instance data into provided instance.
     *
     * @param output destination instance where data will be copied to.
     */
    public void copyTo(final GNSSKalmanState output) {
        output.copyFrom(this);
    }

    /**
     * Copies data of provided instance into this instance.
     *
     * @param input instance to copy data from.
     */
    public void copyFrom(final GNSSKalmanState input) {
        // copy estimation
        if (input.estimation == null) {
            estimation = null;
        } else {
            if (estimation == null) {
                estimation = new GNSSEstimation();
            }
            estimation.copyFrom(input.estimation);
        }

        // copy covariance
        if (input.covariance == null) {
            covariance = null;
        } else {
            if (covariance == null) {
                covariance = new Matrix(input.covariance);
            } else {
                covariance.copyFrom(input.covariance);
            }
        }
    }

    /**
     * Computes and returns hash code for this instance. Hash codes are almost unique
     * values that are useful for fast classification and storage of objects in collections.
     *
     * @return Hash code.
     */
    @Override
    public int hashCode() {
        return Objects.hash(estimation, covariance);
    }

    /**
     * Checks if provided object is a GNSSKalmanState having exactly the same
     * contents as this instance.
     *
     * @param obj Object to be compared.
     * @return true if both objects are considered to be equal, false otherwise.
     */
    @Override
    public boolean equals(final Object obj) {
        if (this == obj) {
            return true;
        }
        if (obj == null || getClass() != obj.getClass()) {
            return false;
        }
        final var other = (GNSSKalmanState) obj;
        return equals(other);
    }

    /**
     * Checks if provided instance has exactly the same contents as this instance.
     *
     * @param other instance to be compared.
     * @return true if both instances are considered to be equal, false otherwise.
     */
    public boolean equals(final GNSSKalmanState other) {
        return equals(other, 0.0);
    }

    /**
     * Checks if provided instance has contents similar to this instance up to provided
     * threshold value.
     *
     * @param other     instance to be compared.
     * @param threshold maximum difference allowed for values.
     * @return true if both instances are considered to be equal (up to provided threshold),
     * false otherwise.
     */
    public boolean equals(final GNSSKalmanState other, final double threshold) {
        if (other == null) {
            return false;
        }

        return other.estimation != null
                && other.estimation.equals(estimation, threshold)
                && other.covariance != null
                && other.covariance.equals(covariance, threshold);
    }

    /**
     * Makes a copy of this instance.
     *
     * @return a copy of this instance.
     * @throws CloneNotSupportedException if clone fails for some reason.
     */
    @Override
    protected Object clone() throws CloneNotSupportedException {
        final var result = (GNSSKalmanState) super.clone();
        copyTo(result);
        return result;
    }
}