Examples with RealMatrix org.hipparchus.linear.RealMatrix used on opensource projects

Example 36 with RealMatrix

use of org.hipparchus.linear.RealMatrix in project Orekit by CS-SI.

the class Model method fetchEvaluatedMeasurement.

/**
 * Fetch a measurement that was evaluated during propagation.
 * @param index index of the measurement first component
 * @param evaluation measurement evaluation
 * @exception OrekitException if Jacobians cannot be computed
 */
void fetchEvaluatedMeasurement(final int index, final EstimatedMeasurement<?> evaluation) throws OrekitException {
    // States and observed measurement
    final SpacecraftState[] evaluationStates = evaluation.getStates();
    final ObservedMeasurement<?> observedMeasurement = evaluation.getObservedMeasurement();
    evaluations.put(observedMeasurement, evaluation);
    if (evaluation.getStatus() == EstimatedMeasurement.Status.REJECTED) {
        return;
    }
    // compute weighted residuals
    final double[] evaluated = evaluation.getEstimatedValue();
    final double[] observed = observedMeasurement.getObservedValue();
    final double[] sigma = observedMeasurement.getTheoreticalStandardDeviation();
    final double[] weight = evaluation.getObservedMeasurement().getBaseWeight();
    for (int i = 0; i < evaluated.length; ++i) {
        value.setEntry(index + i, weight[i] * (evaluated[i] - observed[i]) / sigma[i]);
    }
    for (int k = 0; k < evaluationStates.length; ++k) {
        final int p = observedMeasurement.getPropagatorsIndices().get(k);
        // partial derivatives of the current Cartesian coordinates with respect to current orbital state
        final double[][] aCY = new double[6][6];
        final Orbit currentOrbit = evaluationStates[k].getOrbit();
        currentOrbit.getJacobianWrtParameters(builders[p].getPositionAngle(), aCY);
        final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);
        // Jacobian of the measurement with respect to current orbital state
        final RealMatrix dMdC = new Array2DRowRealMatrix(evaluation.getStateDerivatives(k), false);
        final RealMatrix dMdY = dMdC.multiply(dCdY);
        // Jacobian of the measurement with respect to initial orbital state
        final double[][] aYY0 = new double[6][6];
        mappers[p].getStateJacobian(evaluationStates[k], aYY0);
        final RealMatrix dYdY0 = new Array2DRowRealMatrix(aYY0, false);
        final RealMatrix dMdY0 = dMdY.multiply(dYdY0);
        for (int i = 0; i < dMdY0.getRowDimension(); ++i) {
            int jOrb = orbitsStartColumns[p];
            for (int j = 0; j < dMdY0.getColumnDimension(); ++j) {
                final ParameterDriver driver = builders[p].getOrbitalParametersDrivers().getDrivers().get(j);
                if (driver.isSelected()) {
                    jacobian.setEntry(index + i, jOrb++, weight[i] * dMdY0.getEntry(i, j) / sigma[i] * driver.getScale());
                }
            }
        }
        // Jacobian of the measurement with respect to propagation parameters
        final ParameterDriversList selectedPropagationDrivers = getSelectedPropagationDriversForBuilder(p);
        final int nbParams = selectedPropagationDrivers.getNbParams();
        if (nbParams > 0) {
            final double[][] aYPp = new double[6][nbParams];
            mappers[p].getParametersJacobian(evaluationStates[k], aYPp);
            final RealMatrix dYdPp = new Array2DRowRealMatrix(aYPp, false);
            final RealMatrix dMdPp = dMdY.multiply(dYdPp);
            for (int i = 0; i < dMdPp.getRowDimension(); ++i) {
                for (int j = 0; j < nbParams; ++j) {
                    final ParameterDriver delegating = selectedPropagationDrivers.getDrivers().get(j);
                    jacobian.addToEntry(index + i, propagationParameterColumns.get(delegating.getName()), weight[i] * dMdPp.getEntry(i, j) / sigma[i] * delegating.getScale());
                }
            }
        }
    }
    // Jacobian of the measurement with respect to measurements parameters
    for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
        if (driver.isSelected()) {
            final double[] aMPm = evaluation.getParameterDerivatives(driver);
            for (int i = 0; i < aMPm.length; ++i) {
                jacobian.setEntry(index + i, measurementParameterColumns.get(driver.getName()), weight[i] * aMPm[i] / sigma[i] * driver.getScale());
            }
        }
    }
}

Example 37 with RealMatrix

use of org.hipparchus.linear.RealMatrix in project Orekit by CS-SI.

the class BatchLSEstimator method getPhysicalCovariances.

/**
 * Get the covariances matrix in space flight dynamics physical units.
 * <p>
 * This method retrieve the {@link
 * org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem.Evaluation#getCovariances(double)
 * covariances} from the [@link {@link #getOptimum() optimum} and applies the scaling factors
 * to it in order to convert it from raw normalized values back to physical values.
 * </p>
 * @param threshold threshold to identify matrix singularity
 * @return covariances matrix in space flight dynamics physical units
 * @exception OrekitException if the covariance matrix cannot be computed (singular problem).
 * @since 9.1
 */
public RealMatrix getPhysicalCovariances(final double threshold) throws OrekitException {
    final RealMatrix covariances;
    try {
        // get the normalized matrix
        covariances = optimum.getCovariances(threshold).copy();
    } catch (MathIllegalArgumentException miae) {
        // the problem is singular
        throw new OrekitException(miae);
    }
    // retrieve the scaling factors
    final double[] scale = new double[covariances.getRowDimension()];
    int index = 0;
    for (final ParameterDriver driver : getOrbitalParametersDrivers(true).getDrivers()) {
        scale[index++] = driver.getScale();
    }
    for (final ParameterDriver driver : getPropagatorParametersDrivers(true).getDrivers()) {
        scale[index++] = driver.getScale();
    }
    for (final ParameterDriver driver : getMeasurementsParametersDrivers(true).getDrivers()) {
        scale[index++] = driver.getScale();
    }
    // unnormalize the matrix, to retrieve physical covariances
    for (int i = 0; i < covariances.getRowDimension(); ++i) {
        for (int j = 0; j < covariances.getColumnDimension(); ++j) {
            covariances.setEntry(i, j, scale[i] * scale[j] * covariances.getEntry(i, j));
        }
    }
    return covariances;
}

Example 38 with RealMatrix

use of org.hipparchus.linear.RealMatrix in project Orekit by CS-SI.

the class Model method normalizeCovarianceMatrix.

/**
 * Normalize a covariance matrix.
 * The covariance P is an MxM matrix where M = nbOrb + nbPropag + nbMeas
 * For each element [i,j] of P the corresponding normalized value is:
 * Pn[i,j] = P[i,j] / (scale[i]*scale[j])
 * @param physicalCovarianceMatrix The "physical" covariance matrix in input
 * @return the normalized covariance matrix
 */
private RealMatrix normalizeCovarianceMatrix(final RealMatrix physicalCovarianceMatrix) {
    // Initialize output matrix
    final int nbParams = physicalCovarianceMatrix.getRowDimension();
    final RealMatrix normalizedCovarianceMatrix = MatrixUtils.createRealMatrix(nbParams, nbParams);
    // Retrieve the scaling factors
    final double[] scale = getParametersScale();
    // Normalize the state matrix
    for (int i = 0; i < nbParams; ++i) {
        for (int j = 0; j < nbParams; ++j) {
            normalizedCovarianceMatrix.setEntry(i, j, physicalCovarianceMatrix.getEntry(i, j) / (scale[i] * scale[j]));
        }
    }
    return normalizedCovarianceMatrix;
}

Example 39 with RealMatrix

use of org.hipparchus.linear.RealMatrix in project Orekit by CS-SI.

the class Model method unNormalizeCovarianceMatrix.

/**
 * Un-normalize a covariance matrix.
 * The covariance P is an MxM matrix where M = nbOrb + nbPropag + nbMeas
 * For each element [i,j] of P the corresponding normalized value is:
 * Pn[i,j] = P[i,j] / (scale[i]*scale[j])
 * @param normalizedCovarianceMatrix The normalized covariance matrix in input
 * @return the "physical" covariance matrix
 */
private RealMatrix unNormalizeCovarianceMatrix(final RealMatrix normalizedCovarianceMatrix) {
    // Initialize output matrix
    final int nbParams = normalizedCovarianceMatrix.getRowDimension();
    final RealMatrix physicalCovarianceMatrix = MatrixUtils.createRealMatrix(nbParams, nbParams);
    // Retrieve the scaling factors
    final double[] scale = getParametersScale();
    // Normalize the state matrix
    for (int i = 0; i < nbParams; ++i) {
        for (int j = 0; j < nbParams; ++j) {
            physicalCovarianceMatrix.setEntry(i, j, normalizedCovarianceMatrix.getEntry(i, j) * (scale[i] * scale[j]));
        }
    }
    return physicalCovarianceMatrix;
}

Example 40 with RealMatrix

use of org.hipparchus.linear.RealMatrix in project Orekit by CS-SI.

the class Model method getEvolution.

/**
 * {@inheritDoc}
 */
@Override
public NonLinearEvolution getEvolution(final double previousTime, final RealVector previousState, final MeasurementDecorator measurement) throws OrekitExceptionWrapper {
    try {
        // Set a reference date for all measurements parameters that lack one (including the not estimated ones)
        final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
        for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
            if (driver.getReferenceDate() == null) {
                driver.setReferenceDate(builder.getInitialOrbitDate());
            }
        }
        ++currentMeasurementNumber;
        currentDate = measurement.getObservedMeasurement().getDate();
        // Note:
        // - N = size of the current measurement
        // Example:
        // * 1 for Range, RangeRate and TurnAroundRange
        // * 2 for Angular (Azimuth/Elevation or Right-ascension/Declination)
        // * 6 for Position/Velocity
        // - M = size of the state vector. N = nbOrb + nbPropag + nbMeas
        // Propagate the reference trajectory to measurement date
        predictedSpacecraftStates[0] = referenceTrajectory.propagate(observedMeasurement.getDate());
        // Predict the state vector (Mx1)
        final RealVector predictedState = predictState(predictedSpacecraftStates[0].getOrbit());
        // Get the error state transition matrix (MxM)
        final RealMatrix stateTransitionMatrix = getErrorStateTransitionMatrix(predictedSpacecraftStates[0]);
        // Predict the measurement based on predicted spacecraft state
        // Compute the innovations (i.e. residuals of the predicted measurement)
        // ------------------------------------------------------------
        // Predicted measurement
        // Note: here the "iteration/evaluation" formalism from the batch LS method
        // is twisted to fit the need of the Kalman filter.
        // The number of "iterations" is actually the number of measurements processed by the filter
        // so far. We use this to be able to apply the OutlierFilter modifiers on the predicted measurement.
        predictedMeasurement = observedMeasurement.estimate(currentMeasurementNumber, currentMeasurementNumber, predictedSpacecraftStates);
        // Normalized measurement matrix (NxM)
        final RealMatrix measurementMatrix = getMeasurementMatrix(predictedSpacecraftStates[0]);
        // compute process noise matrix
        final SpacecraftState previous = correctedSpacecraftStates[0];
        final RealMatrix physicalProcessNoise = processNoiseMatrixProvider.getProcessNoiseMatrix(previous, predictedSpacecraftStates[0]);
        final RealMatrix normalizedProcessNoise = normalizeCovarianceMatrix(physicalProcessNoise);
        return new NonLinearEvolution(measurement.getTime(), predictedState, stateTransitionMatrix, normalizedProcessNoise, measurementMatrix);
    } catch (OrekitException oe) {
        throw new OrekitExceptionWrapper(oe);
    }
}