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

Example 11 with RealMatrix

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

the class HolmesFeatherstoneAttractionModel method accelerationWrtState.

/**
 * Compute acceleration derivatives with respect to state parameters.
 * <p>
 * From a theoretical point of view, this method computes the same values
 * as {@link #acceleration(FieldSpacecraftState, RealFieldElement[])} in the
 * specific case of {@link DerivativeStructure} with respect to state, so
 * it is less general. However, it is *much* faster in this important case.
 * <p>
 * <p>
 * The derivatives should be computed with respect to position. The input
 * parameters already take into account the free parameters (6 or 7 depending
 * on derivation with respect to mass being considered or not) and order
 * (always 1). Free parameters at indices 0, 1 and 2 correspond to derivatives
 * with respect to position. Free parameters at indices 3, 4 and 5 correspond
 * to derivatives with respect to velocity (these derivatives will remain zero
 * as acceleration due to gravity does not depend on velocity). Free parameter
 * at index 6 (if present) corresponds to to derivatives with respect to mass
 * (this derivative will remain zero as acceleration due to gravity does not
 * depend on mass).
 * </p>
 * @param date current date
 * @param frame inertial reference frame for state (both orbit and attitude)
 * @param position position of spacecraft in inertial frame
 * @param mu central attraction coefficient to use
 * @return acceleration with all derivatives specified by the input parameters
 * own derivatives
 * @exception OrekitException if derivatives cannot be computed
 * @since 6.0
 */
private FieldVector3D<DerivativeStructure> accelerationWrtState(final AbsoluteDate date, final Frame frame, final FieldVector3D<DerivativeStructure> position, final DerivativeStructure mu) throws OrekitException {
    // get the position in body frame
    final Transform fromBodyFrame = bodyFrame.getTransformTo(frame, date);
    final Transform toBodyFrame = fromBodyFrame.getInverse();
    final Vector3D positionBody = toBodyFrame.transformPosition(position.toVector3D());
    // compute gradient and Hessian
    final GradientHessian gh = gradientHessian(date, positionBody, mu.getReal());
    // gradient of the non-central part of the gravity field
    final double[] gInertial = fromBodyFrame.transformVector(new Vector3D(gh.getGradient())).toArray();
    // Hessian of the non-central part of the gravity field
    final RealMatrix hBody = new Array2DRowRealMatrix(gh.getHessian(), false);
    final RealMatrix rot = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix());
    final RealMatrix hInertial = rot.transpose().multiply(hBody).multiply(rot);
    // distribute all partial derivatives in a compact acceleration vector
    final double[] derivatives = new double[1 + position.getX().getFreeParameters()];
    final DerivativeStructure[] accDer = new DerivativeStructure[3];
    for (int i = 0; i < 3; ++i) {
        // first element is value of acceleration (i.e. gradient of field)
        derivatives[0] = gInertial[i];
        // next three elements are one row of the Jacobian of acceleration (i.e. Hessian of field)
        derivatives[1] = hInertial.getEntry(i, 0);
        derivatives[2] = hInertial.getEntry(i, 1);
        derivatives[3] = hInertial.getEntry(i, 2);
        // next element is derivative with respect to parameter mu
        if (derivatives.length > 4 && isVariable(mu, 3)) {
            derivatives[4] = gInertial[i] / mu.getReal();
        }
        accDer[i] = position.getX().getFactory().build(derivatives);
    }
    return new FieldVector3D<>(accDer);
}

Example 12 with RealMatrix

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

the class JacobiansMapper method setInitialJacobians.

/**
 * Set the Jacobian with respect to state into a one-dimensional additional state array.
 * <p>
 * This method converts the Jacobians to Cartesian parameters and put the converted data
 * in the one-dimensional {@code p} array.
 * </p>
 * @param state spacecraft state
 * @param dY1dY0 Jacobian of current state at time t₁
 * with respect to state at some previous time t₀
 * @param dY1dP Jacobian of current state at time t₁
 * with respect to parameters (may be null if there are no parameters)
 * @param p placeholder where to put the one-dimensional additional state
 * @see #getStateJacobian(SpacecraftState, double[][])
 */
void setInitialJacobians(final SpacecraftState state, final double[][] dY1dY0, final double[][] dY1dP, final double[] p) {
    // set up a converter between state parameters and Cartesian parameters
    final RealMatrix dY1dC1 = new Array2DRowRealMatrix(getdYdC(state), false);
    final DecompositionSolver solver = new QRDecomposition(dY1dC1).getSolver();
    // convert the provided state Jacobian to Cartesian parameters
    final RealMatrix dC1dY0 = solver.solve(new Array2DRowRealMatrix(dY1dY0, false));
    // map the converted state Jacobian to one-dimensional array
    int index = 0;
    for (int i = 0; i < STATE_DIMENSION; ++i) {
        for (int j = 0; j < STATE_DIMENSION; ++j) {
            p[index++] = dC1dY0.getEntry(i, j);
        }
    }
    if (parameters.getNbParams() != 0) {
        // convert the provided state Jacobian to Cartesian parameters
        final RealMatrix dC1dP = solver.solve(new Array2DRowRealMatrix(dY1dP, false));
        // map the converted parameters Jacobian to one-dimensional array
        for (int i = 0; i < STATE_DIMENSION; ++i) {
            for (int j = 0; j < parameters.getNbParams(); ++j) {
                p[index++] = dC1dP.getEntry(i, j);
            }
        }
    }
}

Example 13 with RealMatrix

use of org.hipparchus.linear.RealMatrix 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 14 with RealMatrix

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

the class TransformTest method testLinear.

@Test
public void testLinear() {
    RandomGenerator random = new Well19937a(0x14f6411217b148d8l);
    for (int n = 0; n < 100; ++n) {
        Transform t = randomTransform(random);
        // build an equivalent linear transform by extracting raw translation/rotation
        RealMatrix linearA = MatrixUtils.createRealMatrix(3, 4);
        linearA.setSubMatrix(t.getRotation().getMatrix(), 0, 0);
        Vector3D rt = t.getRotation().applyTo(t.getTranslation());
        linearA.setEntry(0, 3, rt.getX());
        linearA.setEntry(1, 3, rt.getY());
        linearA.setEntry(2, 3, rt.getZ());
        // build an equivalent linear transform by observing transformed points
        RealMatrix linearB = MatrixUtils.createRealMatrix(3, 4);
        Vector3D p0 = t.transformPosition(Vector3D.ZERO);
        Vector3D pI = t.transformPosition(Vector3D.PLUS_I).subtract(p0);
        Vector3D pJ = t.transformPosition(Vector3D.PLUS_J).subtract(p0);
        Vector3D pK = t.transformPosition(Vector3D.PLUS_K).subtract(p0);
        linearB.setColumn(0, new double[] { pI.getX(), pI.getY(), pI.getZ() });
        linearB.setColumn(1, new double[] { pJ.getX(), pJ.getY(), pJ.getZ() });
        linearB.setColumn(2, new double[] { pK.getX(), pK.getY(), pK.getZ() });
        linearB.setColumn(3, new double[] { p0.getX(), p0.getY(), p0.getZ() });
        // both linear transforms should be equal
        Assert.assertEquals(0.0, linearB.subtract(linearA).getNorm(), 1.0e-15 * linearA.getNorm());
        for (int i = 0; i < 100; ++i) {
            Vector3D p = randomVector(1.0e3, random);
            Vector3D q = t.transformPosition(p);
            double[] qA = linearA.operate(new double[] { p.getX(), p.getY(), p.getZ(), 1.0 });
            Assert.assertEquals(q.getX(), qA[0], 1.0e-13 * p.getNorm());
            Assert.assertEquals(q.getY(), qA[1], 1.0e-13 * p.getNorm());
            Assert.assertEquals(q.getZ(), qA[2], 1.0e-13 * p.getNorm());
            double[] qB = linearB.operate(new double[] { p.getX(), p.getY(), p.getZ(), 1.0 });
            Assert.assertEquals(q.getX(), qB[0], 1.0e-10 * p.getNorm());
            Assert.assertEquals(q.getY(), qB[1], 1.0e-10 * p.getNorm());
            Assert.assertEquals(q.getZ(), qB[2], 1.0e-10 * p.getNorm());
        }
    }
}

Example 15 with RealMatrix

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

the class Model method getMeasurementMatrix.

/**
 * Get the normalized measurement matrix H.
 * H contains the partial derivatives of the measurement with respect to the state.
 * H is an NxM matrix where N is the size of the measurement vector and M the size of the state vector.
 * @param predictedSpacecraftState the spacecraft state associated with the measurement
 * @return the normalized measurement matrix H
 * @throws OrekitException if Jacobians cannot be computed
 */
private RealMatrix getMeasurementMatrix(final SpacecraftState predictedSpacecraftState) throws OrekitException {
    // Number of parameters
    final int nbOrb = estimatedOrbitalParameters.getNbParams();
    final int nbPropag = estimatedPropagationParameters.getNbParams();
    final int nbMeas = estimatedMeasurementsParameters.getNbParams();
    // Initialize measurement matrix H: N x M
    // N: Number of measurements in current measurement
    // M: State vector size
    final RealMatrix measurementMatrix = MatrixUtils.createRealMatrix(predictedMeasurement.getEstimatedValue().length, nbOrb + nbPropag + nbMeas);
    // Predicted orbit
    final Orbit predictedOrbit = predictedSpacecraftState.getOrbit();
    // Observed measurement characteristics
    final ObservedMeasurement<?> observedMeasurement = predictedMeasurement.getObservedMeasurement();
    final double[] sigma = observedMeasurement.getTheoreticalStandardDeviation();
    // Measurement matrix's columns related to orbital parameters
    // ----------------------------------------------------------
    // Partial derivatives of the current Cartesian coordinates with respect to current orbital state
    final double[][] aCY = new double[6][6];
    // dC/dY
    predictedOrbit.getJacobianWrtParameters(builder.getPositionAngle(), aCY);
    final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);
    // Jacobian of the measurement with respect to current Cartesian coordinates
    final RealMatrix dMdC = new Array2DRowRealMatrix(predictedMeasurement.getStateDerivatives(0), false);
    // Jacobian of the measurement with respect to current orbital state
    final RealMatrix dMdY = dMdC.multiply(dCdY);
    // Fill the normalized measurement matrix's columns related to estimated orbital parameters
    if (nbOrb > 0) {
        for (int i = 0; i < dMdY.getRowDimension(); ++i) {
            int jOrb = 0;
            for (int j = 0; j < dMdY.getColumnDimension(); ++j) {
                final DelegatingDriver delegating = builder.getOrbitalParametersDrivers().getDrivers().get(j);
                if (delegating.isSelected()) {
                    measurementMatrix.setEntry(i, jOrb++, dMdY.getEntry(i, j) / sigma[i] * delegating.getScale());
                }
            }
        }
    }
    // Jacobian of the measurement with respect to propagation parameters
    if (nbPropag > 0) {
        final double[][] aYPp = new double[6][nbPropag];
        jacobiansMapper.getParametersJacobian(predictedSpacecraftState, 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 < nbPropag; ++j) {
                final DelegatingDriver delegating = estimatedPropagationParameters.getDrivers().get(j);
                measurementMatrix.setEntry(i, nbOrb + j, dMdPp.getEntry(i, j) / sigma[i] * delegating.getScale());
            }
        }
    }
    // Jacobian of the measurement with respect to measurement parameters
    if (nbMeas > 0) {
        // Gather the measurement parameters linked to current measurement
        for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
            if (driver.isSelected()) {
                // Derivatives of current measurement w/r to selected measurement parameter
                final double[] aMPm = predictedMeasurement.getParameterDerivatives(driver);
                // Check that the measurement parameter is managed by the filter
                if (measurementParameterColumns.get(driver.getName()) != null) {
                    // Column of the driver in the measurement matrix
                    final int driverColumn = measurementParameterColumns.get(driver.getName());
                    // Fill the corresponding indexes of the measurement matrix
                    for (int i = 0; i < aMPm.length; ++i) {
                        measurementMatrix.setEntry(i, driverColumn, aMPm[i] / sigma[i] * driver.getScale());
                    }
                }
            }
        }
    }
    // Return the normalized measurement matrix
    return measurementMatrix;
}