Examples with WeightedObservedPoint org.hipparchus.fitting.WeightedObservedPoint used on opensource projects

Example 1 with WeightedObservedPoint

use of org.hipparchus.fitting.WeightedObservedPoint in project Orekit by CS-SI.

the class BodyCenterPointingTest method testQDot.

@Test
public void testQDot() throws OrekitException {
    Utils.setDataRoot("regular-data");
    final double ehMu = 3.9860047e14;
    final double ae = 6.378137e6;
    final double c20 = -1.08263e-3;
    final double c30 = 2.54e-6;
    final double c40 = 1.62e-6;
    final double c50 = 2.3e-7;
    final double c60 = -5.5e-7;
    final AbsoluteDate date = AbsoluteDate.J2000_EPOCH.shiftedBy(584.);
    final Vector3D position = new Vector3D(3220103., 69623., 6449822.);
    final Vector3D velocity = new Vector3D(6414.7, -2006., -3180.);
    final CircularOrbit initialOrbit = new CircularOrbit(new PVCoordinates(position, velocity), FramesFactory.getEME2000(), date, ehMu);
    EcksteinHechlerPropagator propagator = new EcksteinHechlerPropagator(initialOrbit, ae, ehMu, c20, c30, c40, c50, c60);
    propagator.setAttitudeProvider(earthCenterAttitudeLaw);
    List<WeightedObservedPoint> w0 = new ArrayList<WeightedObservedPoint>();
    List<WeightedObservedPoint> w1 = new ArrayList<WeightedObservedPoint>();
    List<WeightedObservedPoint> w2 = new ArrayList<WeightedObservedPoint>();
    List<WeightedObservedPoint> w3 = new ArrayList<WeightedObservedPoint>();
    for (double dt = -1; dt < 1; dt += 0.01) {
        Rotation rP = propagator.propagate(date.shiftedBy(dt)).getAttitude().getRotation();
        w0.add(new WeightedObservedPoint(1, dt, rP.getQ0()));
        w1.add(new WeightedObservedPoint(1, dt, rP.getQ1()));
        w2.add(new WeightedObservedPoint(1, dt, rP.getQ2()));
        w3.add(new WeightedObservedPoint(1, dt, rP.getQ3()));
    }
    double q0DotRef = PolynomialCurveFitter.create(2).fit(w0)[1];
    double q1DotRef = PolynomialCurveFitter.create(2).fit(w1)[1];
    double q2DotRef = PolynomialCurveFitter.create(2).fit(w2)[1];
    double q3DotRef = PolynomialCurveFitter.create(2).fit(w3)[1];
    Attitude a0 = propagator.propagate(date).getAttitude();
    double q0 = a0.getRotation().getQ0();
    double q1 = a0.getRotation().getQ1();
    double q2 = a0.getRotation().getQ2();
    double q3 = a0.getRotation().getQ3();
    double oX = a0.getSpin().getX();
    double oY = a0.getSpin().getY();
    double oZ = a0.getSpin().getZ();
    // first time-derivatives of the quaternion
    double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
    double q1Dot = 0.5 * MathArrays.linearCombination(q0, oX, -q3, oY, q2, oZ);
    double q2Dot = 0.5 * MathArrays.linearCombination(q3, oX, q0, oY, -q1, oZ);
    double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX, q1, oY, q0, oZ);
    Assert.assertEquals(q0DotRef, q0Dot, 5.0e-9);
    Assert.assertEquals(q1DotRef, q1Dot, 5.0e-9);
    Assert.assertEquals(q2DotRef, q2Dot, 5.0e-9);
    Assert.assertEquals(q3DotRef, q3Dot, 5.0e-9);
}

Example 2 with WeightedObservedPoint

use of org.hipparchus.fitting.WeightedObservedPoint in project Orekit by CS-SI.

the class BodyCenterPointingTest method doTestQDot.

private <T extends RealFieldElement<T>> void doTestQDot(final Field<T> field) throws OrekitException {
    final double ehMu = 3.9860047e14;
    final double ae = 6.378137e6;
    final double c20 = -1.08263e-3;
    final double c30 = 2.54e-6;
    final double c40 = 1.62e-6;
    final double c50 = 2.3e-7;
    final double c60 = -5.5e-7;
    // Satellite position as circular parameters
    T zero = field.getZero();
    final T a = zero.add(7178000.0);
    final T e = zero.add(7e-5);
    final T i = zero.add(FastMath.toRadians(50.));
    final T pa = zero.add(FastMath.toRadians(45.));
    final T raan = zero.add(FastMath.toRadians(270.));
    final T m = zero.add(FastMath.toRadians(5.3 - 270));
    // Computation date
    FieldAbsoluteDate<T> date_comp = new FieldAbsoluteDate<>(field, new DateComponents(2008, 04, 07), TimeComponents.H00, TimeScalesFactory.getUTC());
    // Orbit
    FieldKeplerianOrbit<T> circ = new FieldKeplerianOrbit<>(a, e, i, pa, raan, m, PositionAngle.MEAN, FramesFactory.getEME2000(), date_comp, ehMu);
    // WGS84 Earth model
    OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, Constants.WGS84_EARTH_FLATTENING, FramesFactory.getITRF(IERSConventions.IERS_2010, true));
    // Earth center pointing attitude provider
    BodyCenterPointing earthCenterAttitudeLaw = new BodyCenterPointing(circ.getFrame(), earth);
    final FieldAbsoluteDate<T> date = FieldAbsoluteDate.getJ2000Epoch(field).shiftedBy(584.);
    final FieldVector3D<T> position = new FieldVector3D<>(zero.add(3220103.), zero.add(69623.), zero.add(6449822.));
    final FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(6414.7), zero.add(-2006.), zero.add(-3180.));
    final FieldCircularOrbit<T> initialOrbit = new FieldCircularOrbit<>(new FieldPVCoordinates<>(position, velocity), FramesFactory.getEME2000(), date, ehMu);
    FieldEcksteinHechlerPropagator<T> propagator = new FieldEcksteinHechlerPropagator<>(initialOrbit, ae, ehMu, c20, c30, c40, c50, c60);
    propagator.setAttitudeProvider(earthCenterAttitudeLaw);
    List<WeightedObservedPoint> w0 = new ArrayList<WeightedObservedPoint>();
    List<WeightedObservedPoint> w1 = new ArrayList<WeightedObservedPoint>();
    List<WeightedObservedPoint> w2 = new ArrayList<WeightedObservedPoint>();
    List<WeightedObservedPoint> w3 = new ArrayList<WeightedObservedPoint>();
    for (double dt = -1; dt < 1; dt += 0.01) {
        FieldRotation<T> rP = propagator.propagate(date.shiftedBy(dt)).getAttitude().getRotation();
        w0.add(new WeightedObservedPoint(1, dt, rP.getQ0().getReal()));
        w1.add(new WeightedObservedPoint(1, dt, rP.getQ1().getReal()));
        w2.add(new WeightedObservedPoint(1, dt, rP.getQ2().getReal()));
        w3.add(new WeightedObservedPoint(1, dt, rP.getQ3().getReal()));
    }
    double q0DotRef = PolynomialCurveFitter.create(2).fit(w0)[1];
    double q1DotRef = PolynomialCurveFitter.create(2).fit(w1)[1];
    double q2DotRef = PolynomialCurveFitter.create(2).fit(w2)[1];
    double q3DotRef = PolynomialCurveFitter.create(2).fit(w3)[1];
    FieldAttitude<T> a0 = propagator.propagate(date).getAttitude();
    T q0 = a0.getRotation().getQ0();
    T q1 = a0.getRotation().getQ1();
    T q2 = a0.getRotation().getQ2();
    T q3 = a0.getRotation().getQ3();
    T oX = a0.getSpin().getX();
    T oY = a0.getSpin().getY();
    T oZ = a0.getSpin().getZ();
    // first time-derivatives of the quaternion
    double q0Dot = 0.5 * MathArrays.linearCombination(-q1.getReal(), oX.getReal(), -q2.getReal(), oY.getReal(), -q3.getReal(), oZ.getReal());
    double q1Dot = 0.5 * MathArrays.linearCombination(q0.getReal(), oX.getReal(), -q3.getReal(), oY.getReal(), q2.getReal(), oZ.getReal());
    double q2Dot = 0.5 * MathArrays.linearCombination(q3.getReal(), oX.getReal(), q0.getReal(), oY.getReal(), -q1.getReal(), oZ.getReal());
    double q3Dot = 0.5 * MathArrays.linearCombination(-q2.getReal(), oX.getReal(), q1.getReal(), oY.getReal(), q0.getReal(), oZ.getReal());
    Assert.assertEquals(q0DotRef, q0Dot, 5.0e-9);
    Assert.assertEquals(q1DotRef, q1Dot, 5.0e-9);
    Assert.assertEquals(q2DotRef, q2Dot, 5.0e-9);
    Assert.assertEquals(q3DotRef, q3Dot, 5.0e-9);
}

Example 3 with WeightedObservedPoint

use of org.hipparchus.fitting.WeightedObservedPoint in project Orekit by CS-SI.

the class SecularAndHarmonic method fit.

/**
 * Fit parameters.
 * @see #getFittedParameters()
 */
public void fit() {
    final AbstractCurveFitter fitter = new AbstractCurveFitter() {

        /**
         * {@inheritDoc}
         */
        @Override
        protected LeastSquaresProblem getProblem(final Collection<WeightedObservedPoint> observations) {
            // Prepare least-squares problem.
            final int len = observations.size();
            final double[] target = new double[len];
            final double[] weights = new double[len];
            int i = 0;
            for (final WeightedObservedPoint obs : observations) {
                target[i] = obs.getY();
                weights[i] = obs.getWeight();
                ++i;
            }
            final AbstractCurveFitter.TheoreticalValuesFunction model = new AbstractCurveFitter.TheoreticalValuesFunction(new LocalParametricFunction(), observations);
            // build a new least squares problem set up to fit a secular and harmonic curve to the observed points
            return new LeastSquaresBuilder().maxEvaluations(Integer.MAX_VALUE).maxIterations(Integer.MAX_VALUE).start(fitted).target(target).weight(new DiagonalMatrix(weights)).model(model.getModelFunction(), model.getModelFunctionJacobian()).build();
        }
    };
    fitted = fitter.fit(observedPoints);
}