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

Example 1 with RealVector

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

the class JacobianPropagatorConverter method getModel.

/**
 * {@inheritDoc}
 */
protected MultivariateJacobianFunction getModel() {
    return new MultivariateJacobianFunction() {

        /**
         * {@inheritDoc}
         */
        public Pair<RealVector, RealMatrix> value(final RealVector point) throws IllegalArgumentException, OrekitExceptionWrapper {
            try {
                final RealVector value = new ArrayRealVector(getTargetSize());
                final RealMatrix jacobian = MatrixUtils.createRealMatrix(getTargetSize(), point.getDimension());
                final NumericalPropagator prop = builder.buildPropagator(point.toArray());
                final int stateSize = isOnlyPosition() ? 3 : 6;
                final ParameterDriversList orbitalParameters = builder.getOrbitalParametersDrivers();
                final PartialDerivativesEquations pde = new PartialDerivativesEquations("pde", prop);
                final ParameterDriversList propagationParameters = pde.getSelectedParameters();
                prop.setInitialState(pde.setInitialJacobians(prop.getInitialState()));
                final JacobiansMapper mapper = pde.getMapper();
                final List<SpacecraftState> sample = getSample();
                for (int i = 0; i < sample.size(); ++i) {
                    final int row = i * stateSize;
                    if (prop.getInitialState().getDate().equals(sample.get(i).getDate())) {
                        // use initial state and Jacobians
                        fillRows(value, jacobian, row, prop.getInitialState(), stateSize, orbitalParameters, propagationParameters, mapper);
                    } else {
                        // use a date detector to pick up state and Jacobians
                        prop.addEventDetector(new DateDetector(sample.get(i).getDate()).withHandler(new EventHandler<DateDetector>() {

                            /**
                             * {@inheritDoc}
                             */
                            @Override
                            public Action eventOccurred(final SpacecraftState state, final DateDetector detector, final boolean increasing) throws OrekitException {
                                fillRows(value, jacobian, row, state, stateSize, orbitalParameters, propagationParameters, mapper);
                                return row + stateSize >= getTargetSize() ? Action.STOP : Action.CONTINUE;
                            }
                        }));
                    }
                }
                prop.propagate(sample.get(sample.size() - 1).getDate().shiftedBy(10.0));
                return new Pair<RealVector, RealMatrix>(value, jacobian);
            } catch (OrekitException ex) {
                throw new OrekitExceptionWrapper(ex);
            }
        }
    };
}

Example 2 with RealVector

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

the class Model method unNormalizeStateVector.

/**
 * Un-normalize a state vector.
 * A state vector S is of size M = nbOrb + nbPropag + nbMeas
 * For each parameter i the normalized value of the state vector is:
 * Sn[i] = S[i] / scale[i]
 * @param normalizedStateVector The normalized state vector in input
 * @return the "physical" state vector
 */
private RealVector unNormalizeStateVector(final RealVector normalizedStateVector) {
    // Initialize output matrix
    final int nbParams = normalizedStateVector.getDimension();
    final RealVector physicalStateVector = new ArrayRealVector(nbParams);
    // Retrieve the scaling factors
    final double[] scale = getParametersScale();
    // Normalize the state matrix
    for (int i = 0; i < nbParams; ++i) {
        physicalStateVector.setEntry(i, normalizedStateVector.getEntry(i) * scale[i]);
    }
    return physicalStateVector;
}

Example 3 with RealVector

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

the class Model method updateParameters.

/**
 * Update the estimated parameters after the correction phase of the filter.
 * The min/max allowed values are handled by the parameter themselves.
 * @throws OrekitException if setting the normalized values failed
 */
private void updateParameters() throws OrekitException {
    final RealVector correctedState = correctedEstimate.getState();
    int i = 0;
    for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
        // let the parameter handle min/max clipping
        driver.setNormalizedValue(correctedState.getEntry(i));
        correctedState.setEntry(i++, driver.getNormalizedValue());
    }
    for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
        // let the parameter handle min/max clipping
        driver.setNormalizedValue(correctedState.getEntry(i));
        correctedState.setEntry(i++, driver.getNormalizedValue());
    }
    for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
        // let the parameter handle min/max clipping
        driver.setNormalizedValue(correctedState.getEntry(i));
        correctedState.setEntry(i++, driver.getNormalizedValue());
    }
}

Example 4 with RealVector

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

the class AngularCoordinates method inverseCrossProducts.

/**
 * Find a vector from two known cross products.
 * <p>
 * We want to find Ω such that: Ω ⨯ v₁ = c₁ and Ω ⨯ v₂ = c₂
 * </p>
 * <p>
 * The first equation (Ω ⨯ v₁ = c₁) will always be fulfilled exactly,
 * and the second one will be fulfilled if possible.
 * </p>
 * @param v1 vector forming the first known cross product
 * @param c1 know vector for cross product Ω ⨯ v₁
 * @param v2 vector forming the second known cross product
 * @param c2 know vector for cross product Ω ⨯ v₂
 * @param tolerance relative tolerance factor used to check singularities
 * @return vector Ω such that: Ω ⨯ v₁ = c₁ and Ω ⨯ v₂ = c₂
 * @exception MathIllegalArgumentException if vectors are inconsistent and
 * no solution can be found
 */
private static Vector3D inverseCrossProducts(final Vector3D v1, final Vector3D c1, final Vector3D v2, final Vector3D c2, final double tolerance) throws MathIllegalArgumentException {
    final double v12 = v1.getNormSq();
    final double v1n = FastMath.sqrt(v12);
    final double v22 = v2.getNormSq();
    final double v2n = FastMath.sqrt(v22);
    final double threshold = tolerance * FastMath.max(v1n, v2n);
    Vector3D omega;
    try {
        // create the over-determined linear system representing the two cross products
        final RealMatrix m = MatrixUtils.createRealMatrix(6, 3);
        m.setEntry(0, 1, v1.getZ());
        m.setEntry(0, 2, -v1.getY());
        m.setEntry(1, 0, -v1.getZ());
        m.setEntry(1, 2, v1.getX());
        m.setEntry(2, 0, v1.getY());
        m.setEntry(2, 1, -v1.getX());
        m.setEntry(3, 1, v2.getZ());
        m.setEntry(3, 2, -v2.getY());
        m.setEntry(4, 0, -v2.getZ());
        m.setEntry(4, 2, v2.getX());
        m.setEntry(5, 0, v2.getY());
        m.setEntry(5, 1, -v2.getX());
        final RealVector rhs = MatrixUtils.createRealVector(new double[] { c1.getX(), c1.getY(), c1.getZ(), c2.getX(), c2.getY(), c2.getZ() });
        // find the best solution we can
        final DecompositionSolver solver = new QRDecomposition(m, threshold).getSolver();
        final RealVector v = solver.solve(rhs);
        omega = new Vector3D(v.getEntry(0), v.getEntry(1), v.getEntry(2));
    } catch (MathIllegalArgumentException miae) {
        if (miae.getSpecifier() == LocalizedCoreFormats.SINGULAR_MATRIX) {
            // handle some special cases for which we can compute a solution
            final double c12 = c1.getNormSq();
            final double c1n = FastMath.sqrt(c12);
            final double c22 = c2.getNormSq();
            final double c2n = FastMath.sqrt(c22);
            if (c1n <= threshold && c2n <= threshold) {
                // simple special case, velocities are cancelled
                return Vector3D.ZERO;
            } else if (v1n <= threshold && c1n >= threshold) {
                // this is inconsistent, if v₁ is zero, c₁ must be 0 too
                throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, c1n, 0, true);
            } else if (v2n <= threshold && c2n >= threshold) {
                // this is inconsistent, if v₂ is zero, c₂ must be 0 too
                throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, c2n, 0, true);
            } else if (Vector3D.crossProduct(v1, v2).getNorm() <= threshold && v12 > threshold) {
                // simple special case, v₂ is redundant with v₁, we just ignore it
                // use the simplest Ω: orthogonal to both v₁ and c₁
                omega = new Vector3D(1.0 / v12, Vector3D.crossProduct(v1, c1));
            } else {
                throw miae;
            }
        } else {
            throw miae;
        }
    }
    // check results
    final double d1 = Vector3D.distance(Vector3D.crossProduct(omega, v1), c1);
    if (d1 > threshold) {
        throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, d1, 0, true);
    }
    final double d2 = Vector3D.distance(Vector3D.crossProduct(omega, v2), c2);
    if (d2 > threshold) {
        throw new MathIllegalArgumentException(LocalizedCoreFormats.NUMBER_TOO_LARGE, d2, 0, true);
    }
    return omega;
}

Example 5 with RealVector

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

the class ModelTest method testPerfectValue.

@Test
public void testPerfectValue() throws OrekitException {
    final Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.0e-6, 60.0, 0.001);
    final NumericalPropagatorBuilder[] builders = { propagatorBuilder };
    // create perfect PV measurements
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new PVMeasurementCreator(), 0.0, 1.0, 300.0);
    final ParameterDriversList estimatedMeasurementsParameters = new ParameterDriversList();
    for (ObservedMeasurement<?> measurement : measurements) {
        for (final ParameterDriver driver : measurement.getParametersDrivers()) {
            if (driver.isSelected()) {
                estimatedMeasurementsParameters.add(driver);
            }
        }
    }
    // create model
    final ModelObserver modelObserver = new ModelObserver() {

        /**
         * {@inheritDoc}
         */
        @Override
        public void modelCalled(final Orbit[] newOrbits, final Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> newEvaluations) {
            Assert.assertEquals(1, newOrbits.length);
            Assert.assertEquals(0, context.initialOrbit.getDate().durationFrom(newOrbits[0].getDate()), 1.0e-15);
            Assert.assertEquals(0, Vector3D.distance(context.initialOrbit.getPVCoordinates().getPosition(), newOrbits[0].getPVCoordinates().getPosition()), 1.0e-15);
            Assert.assertEquals(measurements.size(), newEvaluations.size());
        }
    };
    final Model model = new Model(builders, measurements, estimatedMeasurementsParameters, modelObserver);
    model.setIterationsCounter(new Incrementor(100));
    model.setEvaluationsCounter(new Incrementor(100));
    // Test forward propagation flag to true
    assertEquals(true, model.isForwardPropagation());
    // evaluate model on perfect start point
    final double[] normalizedProp = propagatorBuilder.getSelectedNormalizedParameters();
    final double[] normalized = new double[normalizedProp.length + estimatedMeasurementsParameters.getNbParams()];
    System.arraycopy(normalizedProp, 0, normalized, 0, normalizedProp.length);
    int i = normalizedProp.length;
    for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
        normalized[i++] = driver.getNormalizedValue();
    }
    Pair<RealVector, RealMatrix> value = model.value(new ArrayRealVector(normalized));
    int index = 0;
    for (ObservedMeasurement<?> measurement : measurements) {
        for (int k = 0; k < measurement.getDimension(); ++k) {
            // the value is already a weighted residual
            Assert.assertEquals(0.0, value.getFirst().getEntry(index++), 1.6e-7);
        }
    }
    Assert.assertEquals(index, value.getFirst().getDimension());
}