List of usage examples for org.apache.commons.math3.geometry.euclidean.threed FieldRotation FieldRotation
public FieldRotation(final T q0, final T q1, final T q2, final T q3, final boolean needsNormalization)
From source file:org.orekit.forces.gravity.CunninghamAttractionModelTest.java
@Test public void testStateJacobian() throws OrekitException { Utils.setDataRoot("regular-data:potential/grgs-format"); GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true)); // initialization AbsoluteDate date = new AbsoluteDate(new DateComponents(2000, 07, 01), new TimeComponents(13, 59, 27.816), TimeScalesFactory.getUTC()); double i = FastMath.toRadians(98.7); double omega = FastMath.toRadians(93.0); double OMEGA = FastMath.toRadians(15.0 * 22.5); Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i, omega, OMEGA, 0, PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu); OrbitType integrationType = OrbitType.CARTESIAN; double[][] tolerances = NumericalPropagator.tolerances(0.01, orbit, integrationType); propagator = new NumericalPropagator( new DormandPrince853Integrator(1.0e-3, 120, tolerances[0], tolerances[1])); propagator.setOrbitType(integrationType); CunninghamAttractionModel cuModel = new CunninghamAttractionModel(itrf2008, GravityFieldFactory.getUnnormalizedProvider(50, 50)); Assert.assertEquals(TideSystem.UNKNOWN, cuModel.getTideSystem()); propagator.addForceModel(cuModel);/* w w w .j a v a 2 s .co m*/ SpacecraftState state0 = new SpacecraftState(orbit); propagator.setInitialState(state0); try { DerivativeStructure one = new DerivativeStructure(7, 1, 1.0); cuModel.accelerationDerivatives(state0.getDate(), state0.getFrame(), new FieldVector3D<DerivativeStructure>(one, state0.getPVCoordinates().getPosition()), new FieldVector3D<DerivativeStructure>(one, state0.getPVCoordinates().getVelocity()), new FieldRotation<DerivativeStructure>(one.multiply(state0.getAttitude().getRotation().getQ0()), one.multiply(state0.getAttitude().getRotation().getQ1()), one.multiply(state0.getAttitude().getRotation().getQ2()), one.multiply(state0.getAttitude().getRotation().getQ3()), false), one.multiply(state0.getMass())); Assert.fail("an exception should have been thrown"); } catch (OrekitException oe) { Assert.assertEquals(OrekitMessages.STEPS_NOT_INITIALIZED_FOR_FINITE_DIFFERENCES, oe.getSpecifier()); } cuModel.setSteps(1.0, 1.0e10); checkStateJacobian(propagator, state0, date.shiftedBy(3.5 * 3600.0), 40000, tolerances[0], 2.0e-7); }
From source file:org.orekit.forces.gravity.DrozinerAttractionModelTest.java
@Test public void testStateJacobian() throws OrekitException { Utils.setDataRoot("regular-data:potential/grgs-format"); GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true)); // initialization AbsoluteDate date = new AbsoluteDate(new DateComponents(2000, 07, 01), new TimeComponents(13, 59, 27.816), TimeScalesFactory.getUTC()); double i = FastMath.toRadians(98.7); double omega = FastMath.toRadians(93.0); double OMEGA = FastMath.toRadians(15.0 * 22.5); Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i, omega, OMEGA, 0, PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu); OrbitType integrationType = OrbitType.CARTESIAN; double[][] tolerances = NumericalPropagator.tolerances(0.01, orbit, integrationType); propagator = new NumericalPropagator( new DormandPrince853Integrator(1.0e-3, 120, tolerances[0], tolerances[1])); propagator.setOrbitType(integrationType); DrozinerAttractionModel drModel = new DrozinerAttractionModel(itrf2008, GravityFieldFactory.getUnnormalizedProvider(50, 50)); Assert.assertEquals(TideSystem.UNKNOWN, drModel.getTideSystem()); propagator.addForceModel(drModel);//from w w w . ja v a 2 s.c o m SpacecraftState state0 = new SpacecraftState(orbit); propagator.setInitialState(state0); try { DerivativeStructure one = new DerivativeStructure(7, 1, 1.0); drModel.accelerationDerivatives(state0.getDate(), state0.getFrame(), new FieldVector3D<DerivativeStructure>(one, state0.getPVCoordinates().getPosition()), new FieldVector3D<DerivativeStructure>(one, state0.getPVCoordinates().getVelocity()), new FieldRotation<DerivativeStructure>(one.multiply(state0.getAttitude().getRotation().getQ0()), one.multiply(state0.getAttitude().getRotation().getQ1()), one.multiply(state0.getAttitude().getRotation().getQ2()), one.multiply(state0.getAttitude().getRotation().getQ3()), false), one.multiply(state0.getMass())); Assert.fail("an exception should have been thrown"); } catch (OrekitException oe) { Assert.assertEquals(OrekitMessages.STEPS_NOT_INITIALIZED_FOR_FINITE_DIFFERENCES, oe.getSpecifier()); } drModel.setSteps(1.0, 1.0e10); checkStateJacobian(propagator, state0, date.shiftedBy(3.5 * 3600.0), 40000, tolerances[0], 2.0e-7); }
From source file:org.orekit.propagation.numerical.PartialDerivativesEquations.java
/** {@inheritDoc} */ public double[] computeDerivatives(final SpacecraftState s, final double[] pDot) throws OrekitException { final int dim = 3; // Lazy initialization if (dirty) {// w w w . j a v a 2 s . c o m // if step has not been set by user, set a default value if (Double.isNaN(hPos)) { hPos = FastMath.sqrt(Precision.EPSILON) * s.getPVCoordinates().getPosition().getNorm(); } // set up derivatives providers derivativesProviders.clear(); derivativesProviders.addAll(propagator.getForceModels()); derivativesProviders.add(propagator.getNewtonianAttractionForceModel()); // check all parameters are handled by at least one Jacobian provider for (final ParameterConfiguration param : selectedParameters) { final String parameterName = param.getParameterName(); boolean found = false; for (final ForceModel provider : derivativesProviders) { if (provider.isSupported(parameterName)) { param.setProvider(provider); found = true; } } if (!found) { // build the list of supported parameters, avoiding duplication final SortedSet<String> set = new TreeSet<String>(); for (final ForceModel provider : derivativesProviders) { for (final String forceModelParameter : provider.getParametersNames()) { set.add(forceModelParameter); } } final StringBuilder builder = new StringBuilder(); for (final String forceModelParameter : set) { if (builder.length() > 0) { builder.append(", "); } builder.append(forceModelParameter); } throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME, parameterName, builder.toString()); } } // check the numbers of parameters and matrix size agree if (selectedParameters.size() != paramDim) { throw new OrekitException(OrekitMessages.INITIAL_MATRIX_AND_PARAMETERS_NUMBER_MISMATCH, paramDim, selectedParameters.size()); } dAccdParam = new double[dim]; dAccdPos = new double[dim][dim]; dAccdVel = new double[dim][dim]; dAccdM = (stateDim > 6) ? new double[dim] : null; dirty = false; } // initialize acceleration Jacobians to zero for (final double[] row : dAccdPos) { Arrays.fill(row, 0.0); } for (final double[] row : dAccdVel) { Arrays.fill(row, 0.0); } if (dAccdM != null) { Arrays.fill(dAccdM, 0.0); } // prepare derivation variables, 3 for position, 3 for velocity and optionally 1 for mass final int nbVars = (dAccdM == null) ? 6 : 7; // position corresponds three free parameters final Vector3D position = s.getPVCoordinates().getPosition(); final FieldVector3D<DerivativeStructure> dsP = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(nbVars, 1, 0, position.getX()), new DerivativeStructure(nbVars, 1, 1, position.getY()), new DerivativeStructure(nbVars, 1, 2, position.getZ())); // velocity corresponds three free parameters final Vector3D velocity = s.getPVCoordinates().getVelocity(); final FieldVector3D<DerivativeStructure> dsV = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(nbVars, 1, 3, velocity.getX()), new DerivativeStructure(nbVars, 1, 4, velocity.getY()), new DerivativeStructure(nbVars, 1, 5, velocity.getZ())); // mass corresponds either to a constant or to one free parameter final DerivativeStructure dsM = (dAccdM == null) ? new DerivativeStructure(nbVars, 1, s.getMass()) : new DerivativeStructure(nbVars, 1, 6, s.getMass()); // we should compute attitude partial derivatives with respect to position/velocity // see issue #200 final Rotation rotation = s.getAttitude().getRotation(); final FieldRotation<DerivativeStructure> dsR = new FieldRotation<DerivativeStructure>( new DerivativeStructure(nbVars, 1, rotation.getQ0()), new DerivativeStructure(nbVars, 1, rotation.getQ1()), new DerivativeStructure(nbVars, 1, rotation.getQ2()), new DerivativeStructure(nbVars, 1, rotation.getQ3()), false); // compute acceleration Jacobians for (final ForceModel derivativesProvider : derivativesProviders) { final FieldVector3D<DerivativeStructure> acceleration = derivativesProvider .accelerationDerivatives(s.getDate(), s.getFrame(), dsP, dsV, dsR, dsM); addToRow(acceleration.getX(), 0); addToRow(acceleration.getY(), 1); addToRow(acceleration.getZ(), 2); } // the variational equations of the complete state Jacobian matrix have the // following form for 7x7, i.e. when mass partial derivatives are also considered // (when mass is not considered, only the A, B, D and E matrices are used along // with their derivatives): // [ | | ] [ | | ] [ | | ] // [ Adot | Bdot | Cdot ] [ dVel/dPos = 0 | dVel/dVel = Id | dVel/dm = 0 ] [ A | B | C ] // [ | | ] [ | | ] [ | | ] // --------+--------+--- ---- ------------------+------------------+---------------- -----+-----+----- // [ | | ] [ | | ] [ | | ] // [ Ddot | Edot | Fdot ] = [ dAcc/dPos | dAcc/dVel | dAcc/dm ] * [ D | E | F ] // [ | | ] [ | | ] [ | | ] // --------+--------+--- ---- ------------------+------------------+---------------- -----+-----+----- // [ Gdot | Hdot | Idot ] [ dmDot/dPos = 0 | dmDot/dVel = 0 | dmDot/dm = 0 ] [ G | H | I ] // The A, B, D and E sub-matrices and their derivatives (Adot ...) are 3x3 matrices, // the C and F sub-matrices and their derivatives (Cdot ...) are 3x1 matrices, // the G and H sub-matrices and their derivatives (Gdot ...) are 1x3 matrices, // the I sub-matrix and its derivative (Idot) is a 1x1 matrix. // The expanded multiplication above can be rewritten to take into account // the fixed values found in the sub-matrices in the left factor. This leads to: // [ Adot ] = [ D ] // [ Bdot ] = [ E ] // [ Cdot ] = [ F ] // [ Ddot ] = [ dAcc/dPos ] * [ A ] + [ dAcc/dVel ] * [ D ] + [ dAcc/dm ] * [ G ] // [ Edot ] = [ dAcc/dPos ] * [ B ] + [ dAcc/dVel ] * [ E ] + [ dAcc/dm ] * [ H ] // [ Fdot ] = [ dAcc/dPos ] * [ C ] + [ dAcc/dVel ] * [ F ] + [ dAcc/dm ] * [ I ] // [ Gdot ] = [ 0 ] // [ Hdot ] = [ 0 ] // [ Idot ] = [ 0 ] // The following loops compute these expressions taking care of the mapping of the // (A, B, ... I) matrices into the single dimension array p and of the mapping of the // (Adot, Bdot, ... Idot) matrices into the single dimension array pDot. // copy D, E and F into Adot, Bdot and Cdot final double[] p = s.getAdditionalState(getName()); System.arraycopy(p, dim * stateDim, pDot, 0, dim * stateDim); // compute Ddot, Edot and Fdot for (int i = 0; i < dim; ++i) { final double[] dAdPi = dAccdPos[i]; final double[] dAdVi = dAccdVel[i]; for (int j = 0; j < stateDim; ++j) { pDot[(dim + i) * stateDim + j] = dAdPi[0] * p[j] + dAdPi[1] * p[j + stateDim] + dAdPi[2] * p[j + 2 * stateDim] + dAdVi[0] * p[j + 3 * stateDim] + dAdVi[1] * p[j + 4 * stateDim] + dAdVi[2] * p[j + 5 * stateDim] + ((dAccdM == null) ? 0.0 : dAccdM[i] * p[j + 6 * stateDim]); } } if (dAccdM != null) { // set Gdot, Hdot and Idot to 0 Arrays.fill(pDot, 6 * stateDim, 7 * stateDim, 0.0); } for (int k = 0; k < paramDim; ++k) { // compute the acceleration gradient with respect to current parameter final ParameterConfiguration param = selectedParameters.get(k); final ForceModel provider = param.getProvider(); final FieldVector3D<DerivativeStructure> accDer = provider.accelerationDerivatives(s, param.getParameterName()); dAccdParam[0] = accDer.getX().getPartialDerivative(1); dAccdParam[1] = accDer.getY().getPartialDerivative(1); dAccdParam[2] = accDer.getZ().getPartialDerivative(1); // the variational equations of the parameters Jacobian matrix are computed // one column at a time, they have the following form: // [ ] [ | | ] [ ] [ ] // [ Jdot ] [ dVel/dPos = 0 | dVel/dVel = Id | dVel/dm = 0 ] [ J ] [ dVel/dParam = 0 ] // [ ] [ | | ] [ ] [ ] // -------- ------------------+------------------+---------------- ----- -------------------- // [ ] [ | | ] [ ] [ ] // [ Kdot ] = [ dAcc/dPos | dAcc/dVel | dAcc/dm ] * [ K ] + [ dAcc/dParam ] // [ ] [ | | ] [ ] [ ] // -------- ------------------+------------------+---------------- ----- -------------------- // [ Ldot ] [ dmDot/dPos = 0 | dmDot/dVel = 0 | dmDot/dm = 0 ] [ L ] [ dmDot/dParam = 0 ] // The J and K sub-columns and their derivatives (Jdot ...) are 3 elements columns, // the L sub-colums and its derivative (Ldot) are 1 elements columns. // The expanded multiplication and addition above can be rewritten to take into // account the fixed values found in the sub-matrices in the left factor. This leads to: // [ Jdot ] = [ K ] // [ Kdot ] = [ dAcc/dPos ] * [ J ] + [ dAcc/dVel ] * [ K ] + [ dAcc/dm ] * [ L ] + [ dAcc/dParam ] // [ Ldot ] = [ 0 ] // The following loops compute these expressions taking care of the mapping of the // (J, K, L) columns into the single dimension array p and of the mapping of the // (Jdot, Kdot, Ldot) columns into the single dimension array pDot. // copy K into Jdot final int columnTop = stateDim * stateDim + k; pDot[columnTop] = p[columnTop + 3 * paramDim]; pDot[columnTop + paramDim] = p[columnTop + 4 * paramDim]; pDot[columnTop + 2 * paramDim] = p[columnTop + 5 * paramDim]; // compute Kdot for (int i = 0; i < dim; ++i) { final double[] dAdPi = dAccdPos[i]; final double[] dAdVi = dAccdVel[i]; pDot[columnTop + (dim + i) * paramDim] = dAccdParam[i] + dAdPi[0] * p[columnTop] + dAdPi[1] * p[columnTop + paramDim] + dAdPi[2] * p[columnTop + 2 * paramDim] + dAdVi[0] * p[columnTop + 3 * paramDim] + dAdVi[1] * p[columnTop + 4 * paramDim] + dAdVi[2] * p[columnTop + 5 * paramDim] + ((dAccdM == null) ? 0.0 : dAccdM[i] * p[columnTop + 6 * paramDim]); } if (dAccdM != null) { // set Ldot to 0 pDot[columnTop + 6 * paramDim] = 0; } } // these equations have no effect of the main state itself return null; }
From source file:org.orekit.utils.AngularCoordinates.java
/** Transform the instance to a {@link FieldRotation}<{@link DerivativeStructure}>. * <p>/* ww w.j a v a 2 s. c om*/ * The {@link DerivativeStructure} coordinates correspond to time-derivatives up * to the user-specified order. * </p> * @param order derivation order for the vector components * @return rotation with time-derivatives embedded within the coordinates * @exception OrekitException if the user specified order is too large */ public FieldRotation<DerivativeStructure> toDerivativeStructureRotation(final int order) throws OrekitException { // quaternion components final double q0 = rotation.getQ0(); final double q1 = rotation.getQ1(); final double q2 = rotation.getQ2(); final double q3 = rotation.getQ3(); // first time-derivatives of the quaternion final double oX = rotationRate.getX(); final double oY = rotationRate.getY(); final double oZ = rotationRate.getZ(); final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ); final double q1Dot = 0.5 * MathArrays.linearCombination(q0, oX, -q3, oY, q2, oZ); final double q2Dot = 0.5 * MathArrays.linearCombination(q3, oX, q0, oY, -q1, oZ); final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX, q1, oY, q0, oZ); // second time-derivatives of the quaternion final double oXDot = rotationAcceleration.getX(); final double oYDot = rotationAcceleration.getY(); final double oZDot = rotationAcceleration.getZ(); final double q0DotDot = -0.5 * MathArrays.linearCombination( new double[] { q1, q2, q3, q1Dot, q2Dot, q3Dot }, new double[] { oXDot, oYDot, oZDot, oX, oY, oZ }); final double q1DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q2, -q3, q0Dot, q2Dot, -q3Dot }, new double[] { oXDot, oZDot, oYDot, oX, oZ, oY }); final double q2DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q3, -q1, q0Dot, q3Dot, -q1Dot }, new double[] { oYDot, oXDot, oZDot, oY, oX, oZ }); final double q3DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q1, -q2, q0Dot, q1Dot, -q2Dot }, new double[] { oZDot, oYDot, oXDot, oZ, oY, oX }); final DerivativeStructure q0DS; final DerivativeStructure q1DS; final DerivativeStructure q2DS; final DerivativeStructure q3DS; switch (order) { case 0: q0DS = new DerivativeStructure(1, 0, q0); q1DS = new DerivativeStructure(1, 0, q1); q2DS = new DerivativeStructure(1, 0, q2); q3DS = new DerivativeStructure(1, 0, q3); break; case 1: q0DS = new DerivativeStructure(1, 1, q0, q0Dot); q1DS = new DerivativeStructure(1, 1, q1, q1Dot); q2DS = new DerivativeStructure(1, 1, q2, q2Dot); q3DS = new DerivativeStructure(1, 1, q3, q3Dot); break; case 2: q0DS = new DerivativeStructure(1, 2, q0, q0Dot, q0DotDot); q1DS = new DerivativeStructure(1, 2, q1, q1Dot, q1DotDot); q2DS = new DerivativeStructure(1, 2, q2, q2Dot, q2DotDot); q3DS = new DerivativeStructure(1, 2, q3, q3Dot, q3DotDot); break; default: throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order); } return new FieldRotation<DerivativeStructure>(q0DS, q1DS, q2DS, q3DS, false); }
From source file:org.orekit.utils.FieldAngularCoordinates.java
/** Get a time-shifted state. * <p>//w w w . j a v a 2s .c om * The state can be slightly shifted to close dates. This shift is based on * a simple quadratic model. It is <em>not</em> intended as a replacement for * proper attitude propagation but should be sufficient for either small * time shifts or coarse accuracy. * </p> * @param dt time shift in seconds * @return a new state, shifted with respect to the instance (which is immutable) */ public FieldAngularCoordinates<T> shiftedBy(final double dt) { // the shiftedBy method is based on a local approximation. // It considers separately the contribution of the constant // rotation, the linear contribution or the rate and the // quadratic contribution of the acceleration. The rate // and acceleration contributions are small rotations as long // as the time shift is small, which is the crux of the algorithm. // Small rotations are almost commutative, so we append these small // contributions one after the other, as if they really occurred // successively, despite this is not what really happens. // compute the linear contribution first, ignoring acceleration // BEWARE: there is really a minus sign here, because if // the target frame rotates in one direction, the vectors in the origin // frame seem to rotate in the opposite direction final T rate = rotationRate.getNorm(); final T zero = rate.getField().getZero(); final T one = rate.getField().getOne(); final FieldRotation<T> rateContribution = (rate.getReal() == 0.0) ? new FieldRotation<T>(one, zero, zero, zero, false) : new FieldRotation<T>(rotationRate, rate.multiply(-dt)); // append rotation and rate contribution final FieldAngularCoordinates<T> linearPart = new FieldAngularCoordinates<T>( rateContribution.applyTo(rotation), rotationRate); final T acc = rotationAcceleration.getNorm(); if (acc.getReal() == 0.0) { // no acceleration, the linear part is sufficient return linearPart; } // compute the quadratic contribution, ignoring initial rotation and rotation rate // BEWARE: there is really a minus sign here, because if // the target frame rotates in one direction, the vectors in the origin // frame seem to rotate in the opposite direction final FieldAngularCoordinates<T> quadraticContribution = new FieldAngularCoordinates<T>( new FieldRotation<T>(rotationAcceleration, acc.multiply(-0.5 * dt * dt)), new FieldVector3D<T>(dt, rotationAcceleration), rotationAcceleration); // the quadratic contribution is a small rotation: // its initial angle and angular rate are both zero. // small rotations are almost commutative, so we append the small // quadratic part after the linear part as a simple offset return quadraticContribution.addOffset(linearPart); }
From source file:org.orekit.utils.FieldAngularCoordinatesTest.java
private FieldRotation<DerivativeStructure> createRotation(double q0, double q1, double q2, double q3, boolean needsNormalization) { return new FieldRotation<DerivativeStructure>(new DerivativeStructure(4, 1, 0, q0), new DerivativeStructure(4, 1, 1, q1), new DerivativeStructure(4, 1, 2, q2), new DerivativeStructure(4, 1, 3, q3), needsNormalization); }
From source file:org.orekit.utils.TimeStampedFieldAngularCoordinates.java
/** Interpolate angular coordinates. * <p>/*from w w w.j a v a 2s . c o m*/ * The interpolated instance is created by polynomial Hermite interpolation * on Rodrigues vector ensuring FieldRotation<T> rate remains the exact derivative of FieldRotation<T>. * </p> * <p> * This method is based on Sergei Tanygin's paper <a * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues * vector as described in Malcolm D. Shuster's paper <a * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A * Survey of Attitude Representations</a>. This change avoids the singularity at . * There is still a singularity at 2, which is handled by slightly offsetting all FieldRotation<T>s * when this singularity is detected. * </p> * <p> * Note that even if first time derivatives (FieldRotation<T> rates) * from sample can be ignored, the interpolated instance always includes * interpolated derivatives. This feature can be used explicitly to * compute these derivatives when it would be too complex to compute them * from an analytical formula: just compute a few sample points from the * explicit formula and set the derivatives to zero in these sample points, * then use interpolation to add derivatives consistent with the FieldRotation<T>s. * </p> * @param date interpolation date * @param filter filter for derivatives from the sample to use in interpolation * @param sample sample points on which interpolation should be done * @param <T> the type of the field elements * @return a new position-velocity, interpolated at specified date * @exception OrekitException if the number of point is too small for interpolating */ @SuppressWarnings("unchecked") public static <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> interpolate( final AbsoluteDate date, final AngularDerivativesFilter filter, final Collection<TimeStampedFieldAngularCoordinates<T>> sample) throws OrekitException { // get field properties final Field<T> field = sample.iterator().next().getRotation().getQ0().getField(); final T zero = field.getZero(); final T one = field.getOne(); // set up safety elements for 2 singularity avoidance final double epsilon = 2 * FastMath.PI / sample.size(); final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4)); // set up a linear model canceling mean rotation rate final FieldVector3D<T> meanRate; if (filter != AngularDerivativesFilter.USE_R) { FieldVector3D<T> sum = new FieldVector3D<T>(zero, zero, zero); for (final TimeStampedFieldAngularCoordinates<T> datedAC : sample) { sum = sum.add(datedAC.getRotationRate()); } meanRate = new FieldVector3D<T>(1.0 / sample.size(), sum); } else { if (sample.size() < 2) { throw new OrekitException(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION, sample.size()); } FieldVector3D<T> sum = new FieldVector3D<T>(zero, zero, zero); TimeStampedFieldAngularCoordinates<T> previous = null; for (final TimeStampedFieldAngularCoordinates<T> datedAC : sample) { if (previous != null) { sum = sum.add(estimateRate(previous.getRotation(), datedAC.getRotation(), datedAC.date.durationFrom(previous.getDate()))); } previous = datedAC; } meanRate = new FieldVector3D<T>(1.0 / (sample.size() - 1), sum); } TimeStampedFieldAngularCoordinates<T> offset = new TimeStampedFieldAngularCoordinates<T>(date, new FieldRotation<T>(one, zero, zero, zero, false), meanRate, new FieldVector3D<T>(zero, zero, zero)); boolean restart = true; for (int i = 0; restart && i < sample.size() + 2; ++i) { // offset adaptation parameters restart = false; // set up an interpolator taking derivatives into account final FieldHermiteInterpolator<T> interpolator = new FieldHermiteInterpolator<T>(); // add sample points final double[] previous = new double[] { 1.0, 0.0, 0.0, 0.0 }; for (final TimeStampedFieldAngularCoordinates<T> ac : sample) { // remove linear offset from the current coordinates final T dt = zero.add(ac.date.durationFrom(date)); final TimeStampedFieldAngularCoordinates<T> fixed = ac .subtractOffset(offset.shiftedBy(dt.getReal())); final T[][] rodrigues = getModifiedRodrigues(fixed, previous, threshold); if (rodrigues == null) { // the sample point is close to a modified Rodrigues vector singularity // we need to change the linear offset model to avoid this restart = true; break; } switch (filter) { case USE_RRA: // populate sample with rotation, rotation rate and acceleration data interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]); break; case USE_RR: // populate sample with rotation and rotation rate data interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]); break; case USE_R: // populate sample with rotation data only interpolator.addSamplePoint(dt, rodrigues[0]); break; default: // this should never happen throw new OrekitInternalError(null); } } if (restart) { // interpolation failed, some intermediate rotation was too close to 2 // we need to offset all rotations to avoid the singularity offset = offset.addOffset(new FieldAngularCoordinates<T>( new FieldRotation<T>(new FieldVector3D<T>(one, zero, zero), zero.add(epsilon)), new FieldVector3D<T>(zero, zero, zero), new FieldVector3D<T>(zero, zero, zero))); } else { // interpolation succeeded with the current offset final T[][] p = interpolator.derivatives(field.getZero(), 2); return createFromModifiedRodrigues(p, offset); } } // this should never happen throw new OrekitInternalError(null); }
From source file:org.orekit.utils.TimeStampedFieldAngularCoordinates.java
/** Convert a modified Rodrigues vector and derivatives to angular coordinates. * @param r modified Rodrigues vector (with first derivatives) * @param offset linear offset model to add (its date must be consistent with the modified Rodrigues vector) * @param <T> the type of the field elements * @return angular coordinates// w w w.ja va2s . co m */ private static <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> createFromModifiedRodrigues( final T[][] r, final TimeStampedFieldAngularCoordinates<T> offset) { // rotation final T rSquared = r[0][0].multiply(r[0][0]).add(r[0][1].multiply(r[0][1])).add(r[0][2].multiply(r[0][2])); final T oPQ0 = rSquared.add(1).reciprocal().multiply(2); final T q0 = oPQ0.subtract(1); final T q1 = oPQ0.multiply(r[0][0]); final T q2 = oPQ0.multiply(r[0][1]); final T q3 = oPQ0.multiply(r[0][2]); // rotation rate final T oPQ02 = oPQ0.multiply(oPQ0); final T q0Dot = oPQ02.negate() .multiply(q0.linearCombination(r[0][0], r[1][0], r[0][1], r[1][1], r[0][2], r[1][2])); final T q1Dot = oPQ0.multiply(r[1][0]).add(r[0][0].multiply(q0Dot)); final T q2Dot = oPQ0.multiply(r[1][1]).add(r[0][1].multiply(q0Dot)); final T q3Dot = oPQ0.multiply(r[1][2]).add(r[0][2].multiply(q0Dot)); final T oX = q1.linearCombination(q1.negate(), q0Dot, q0, q1Dot, q3, q2Dot, q2.negate(), q3Dot).multiply(2); final T oY = q2.linearCombination(q2.negate(), q0Dot, q3.negate(), q1Dot, q0, q2Dot, q1, q3Dot).multiply(2); final T oZ = q3.linearCombination(q3.negate(), q0Dot, q2, q1Dot, q1.negate(), q2Dot, q0, q3Dot).multiply(2); // rotation acceleration final T q0DotDot = q0.getField().getOne().subtract(q0).divide(oPQ0).multiply(q0Dot).multiply(q0Dot) .subtract( oPQ02.multiply(q0.linearCombination(r[0][0], r[2][0], r[0][1], r[2][1], r[0][2], r[2][2]))) .subtract(q1Dot.multiply(q1Dot).add(q2Dot.multiply(q2Dot)).add(q3Dot.multiply(q3Dot))); final T q1DotDot = q1.linearCombination(oPQ0, r[2][0], r[1][0].multiply(2), q0Dot, r[0][0], q0DotDot); final T q2DotDot = q2.linearCombination(oPQ0, r[2][1], r[1][1].multiply(2), q0Dot, r[0][1], q0DotDot); final T q3DotDot = q3.linearCombination(oPQ0, r[2][2], r[1][2].multiply(2), q0Dot, r[0][2], q0DotDot); final T oXDot = q1 .linearCombination(q1.negate(), q0DotDot, q0, q1DotDot, q3, q2DotDot, q2.negate(), q3DotDot) .multiply(2); final T oYDot = q2 .linearCombination(q2.negate(), q0DotDot, q3.negate(), q1DotDot, q0, q2DotDot, q1, q3DotDot) .multiply(2); final T oZDot = q3 .linearCombination(q3.negate(), q0DotDot, q2, q1DotDot, q1.negate(), q2DotDot, q0, q3DotDot) .multiply(2); return new TimeStampedFieldAngularCoordinates<T>(offset.getDate(), new FieldRotation<T>(q0, q1, q2, q3, false), new FieldVector3D<T>(oX, oY, oZ), new FieldVector3D<T>(oXDot, oYDot, oZDot)).addOffset(offset); }