EstimatedEarthFrameProvider.java

/* Copyright 2002-2018 CS Systèmes d'Information
 * Licensed to CS Systèmes d'Information (CS) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * CS licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.orekit.estimation.measurements;

import java.io.Serializable;
import java.util.Map;

import org.hipparchus.RealFieldElement;
import org.hipparchus.analysis.differentiation.DSFactory;
import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.geometry.euclidean.threed.FieldRotation;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Rotation;
import org.hipparchus.geometry.euclidean.threed.RotationConvention;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.util.FastMath;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitExceptionWrapper;
import org.orekit.errors.OrekitInternalError;
import org.orekit.errors.OrekitMessages;
import org.orekit.frames.FieldTransform;
import org.orekit.frames.Transform;
import org.orekit.frames.TransformProvider;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.time.UT1Scale;
import org.orekit.utils.IERSConventions;
import org.orekit.utils.ParameterDriver;

/** Class modeling an Earth frame whose Earth Orientation Parameters can be estimated.
 * <p>
 * This class adds parameters for an additional polar motion
 * and an additional prime meridian orientation on top of an underlying regular Earth
 * frame like {@link org.orekit.frames.FramesFactory#getITRF(IERSConventions, boolean) ITRF}.
 * The polar motion and prime meridian orientation are applied <em>after</em> regular Earth
 * orientation parameters, so the value of the estimated parameters will be correction to EOP,
 * they will not be the complete EOP values by themselves. Basically, this means that for
 * Earth, the following transforms are applied in order, between inertial frame and this frame:
 * </p>
 * <ol>
 *   <li>precession/nutation, as theoretical model plus celestial pole EOP parameters</li>
 *   <li>body rotation, as theoretical model plus prime meridian EOP parameters</li>
 *   <li>polar motion, which is only from EOP parameters (no theoretical models)</li>
 *   <li>additional body rotation, controlled by {@link #getPrimeMeridianOffsetDriver()} and {@link #getPrimeMeridianDriftDriver()}</li>
 *   <li>additional polar motion, controlled by {@link #getPolarOffsetXDriver()}, {@link #getPolarDriftXDriver()},
 *   {@link #getPolarOffsetYDriver()} and {@link #getPolarDriftYDriver()}</li>
 * </ol>
 * @author Luc Maisonobe
 * @since 9.1
 */
public class EstimatedEarthFrameProvider implements TransformProvider {

    /** Earth Angular Velocity, in rad/s, from TIRF model. */
    public static final double EARTH_ANGULAR_VELOCITY = 7.292115146706979e-5;

    /** Serializable UID. */
    private static final long serialVersionUID = 20170922L;

    /** Angular scaling factor.
     * <p>
     * We use a power of 2 to avoid numeric noise introduction
     * in the multiplications/divisions sequences.
     * </p>
     */
    private static final double ANGULAR_SCALE = FastMath.scalb(1.0, -22);

    /** Underlying raw UT1. */
    private final UT1Scale baseUT1;

    /** Estimated UT1. */
    private final transient UT1Scale estimatedUT1;

    /** Driver for prime meridian offset. */
    private final transient ParameterDriver primeMeridianOffsetDriver;

    /** Driver for prime meridian drift. */
    private final transient ParameterDriver primeMeridianDriftDriver;

    /** Driver for pole offset along X. */
    private final transient ParameterDriver polarOffsetXDriver;

    /** Driver for pole drift along X. */
    private final transient ParameterDriver polarDriftXDriver;

    /** Driver for pole offset along Y. */
    private final transient ParameterDriver polarOffsetYDriver;

    /** Driver for pole drift along Y. */
    private final transient ParameterDriver polarDriftYDriver;

    /** Build an estimated Earth frame.
     * <p>
     * The initial values for the pole and prime meridian parametric linear models
     * ({@link #getPrimeMeridianOffsetDriver()}, {@link #getPrimeMeridianDriftDriver()},
     * {@link #getPolarOffsetXDriver()}, {@link #getPolarDriftXDriver()},
     * {@link #getPolarOffsetXDriver()}, {@link #getPolarDriftXDriver()}) are set to 0.
     * </p>
     * @param baseUT1 underlying base UT1
     * @exception OrekitException if scales are too close to zero (never happens)
     * @since 9.1
     */
    public EstimatedEarthFrameProvider(final UT1Scale baseUT1)
        throws OrekitException {

        this.primeMeridianOffsetDriver = new ParameterDriver("prime-meridian-offset",
                                                             0.0, ANGULAR_SCALE,
                                                            -FastMath.PI, FastMath.PI);

        this.primeMeridianDriftDriver = new ParameterDriver("prime-meridian-drift",
                                                            0.0, ANGULAR_SCALE,
                                                            Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);

        this.polarOffsetXDriver = new ParameterDriver("polar-offset-X",
                                                      0.0, ANGULAR_SCALE,
                                                      -FastMath.PI, FastMath.PI);

        this.polarDriftXDriver = new ParameterDriver("polar-drift-X",
                                                     0.0, ANGULAR_SCALE,
                                                     Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);

        this.polarOffsetYDriver = new ParameterDriver("polar-offset-Y",
                                                      0.0, ANGULAR_SCALE,
                                                      -FastMath.PI, FastMath.PI);

        this.polarDriftYDriver = new ParameterDriver("polar-drift-Y",
                                                     0.0, ANGULAR_SCALE,
                                                     Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);

        this.baseUT1      = baseUT1;
        this.estimatedUT1 = new EstimatedUT1Scale();

    }

    /** Get a driver allowing to add a prime meridian rotation.
     * <p>
     * The parameter is an angle in radians. In order to convert this
     * value to a DUT1 in seconds, the value must be divided by
     * {@link #EARTH_ANGULAR_VELOCITY} (nominal Angular Velocity of Earth).
     * </p>
     * @return driver for prime meridian rotation
     */
    public ParameterDriver getPrimeMeridianOffsetDriver() {
        return primeMeridianOffsetDriver;
    }

    /** Get a driver allowing to add a prime meridian rotation rate.
     * <p>
     * The parameter is an angle rate in radians per second. In order to convert this
     * value to a LOD in seconds, the value must be multiplied by -86400 and divided by
     * {@link #EARTH_ANGULAR_VELOCITY} (nominal Angular Velocity of Earth).
     * </p>
     * @return driver for prime meridian rotation rate
     */
    public ParameterDriver getPrimeMeridianDriftDriver() {
        return primeMeridianDriftDriver;
    }

    /** Get a driver allowing to add a polar offset along X.
     * <p>
     * The parameter is an angle in radians
     * </p>
     * @return driver for polar offset along X
     */
    public ParameterDriver getPolarOffsetXDriver() {
        return polarOffsetXDriver;
    }

    /** Get a driver allowing to add a polar drift along X.
     * <p>
     * The parameter is an angle rate in radians per second
     * </p>
     * @return driver for polar drift along X
     */
    public ParameterDriver getPolarDriftXDriver() {
        return polarDriftXDriver;
    }

    /** Get a driver allowing to add a polar offset along Y.
     * <p>
     * The parameter is an angle in radians
     * </p>
     * @return driver for polar offset along Y
     */
    public ParameterDriver getPolarOffsetYDriver() {
        return polarOffsetYDriver;
    }

    /** Get a driver allowing to add a polar drift along Y.
     * <p>
     * The parameter is an angle rate in radians per second
     * </p>
     * @return driver for polar drift along Y
     */
    public ParameterDriver getPolarDriftYDriver() {
        return polarDriftYDriver;
    }

    /** Get the estimated UT1 time scale.
     * @return estimated UT1 time scale
     */
    public UT1Scale getEstimatedUT1() {
        return estimatedUT1;
    }

    /** {@inheritDoc} */
    @Override
    public Transform getTransform(final AbsoluteDate date)
        throws OrekitException {

        // take parametric prime meridian shift into account
        final double theta    = linearModel(date, primeMeridianOffsetDriver, primeMeridianDriftDriver);
        final double thetaDot = parametricModel(primeMeridianDriftDriver);
        final Transform meridianShift =
                        new Transform(date,
                                      new Rotation(Vector3D.PLUS_K, theta, RotationConvention.FRAME_TRANSFORM),
                                      new Vector3D(0, 0, thetaDot));

        // take parametric pole shift into account
        final double xpNeg     = -linearModel(date, polarOffsetXDriver, polarDriftXDriver);
        final double ypNeg     = -linearModel(date, polarOffsetYDriver, polarDriftYDriver);
        final double xpNegDot  = -parametricModel(polarDriftXDriver);
        final double ypNegDot  = -parametricModel(polarDriftYDriver);
        final Transform poleShift =
                        new Transform(date,
                                      new Transform(date,
                                                    new Rotation(Vector3D.PLUS_J, xpNeg, RotationConvention.FRAME_TRANSFORM),
                                                    new Vector3D(0.0, xpNegDot, 0.0)),
                                      new Transform(date,
                                                    new Rotation(Vector3D.PLUS_I, ypNeg, RotationConvention.FRAME_TRANSFORM),
                                                    new Vector3D(ypNegDot, 0.0, 0.0)));

        return new Transform(date, meridianShift, poleShift);

    }

    /** {@inheritDoc} */
    @Override
    public <T extends RealFieldElement<T>> FieldTransform<T> getTransform(final FieldAbsoluteDate<T> date)
        throws OrekitException {

        final T zero = date.getField().getZero();

        // prime meridian shift parameters
        final T theta    = linearModel(date, primeMeridianOffsetDriver, primeMeridianDriftDriver);
        final T thetaDot = zero.add(parametricModel(primeMeridianDriftDriver));

        // pole shift parameters
        final T xpNeg    = linearModel(date, polarOffsetXDriver, polarDriftXDriver).negate();
        final T ypNeg    = linearModel(date, polarOffsetYDriver, polarDriftYDriver).negate();
        final T xpNegDot = zero.subtract(parametricModel(polarDriftXDriver));
        final T ypNegDot = zero.subtract(parametricModel(polarDriftYDriver));

        return getTransform(date, theta, thetaDot, xpNeg, xpNegDot, ypNeg, ypNegDot);

    }

    /** Get the transform with derivatives.
     * @param date date of the transform
     * @param factory factory for the derivatives
     * @param indices indices of the estimated parameters in derivatives computations
     * @return computed transform with derivatives
     * @exception OrekitException if some frame transforms cannot be computed
     */
    public FieldTransform<DerivativeStructure> getTransform(final FieldAbsoluteDate<DerivativeStructure> date,
                                                            final DSFactory factory,
                                                            final Map<String, Integer> indices)
        throws OrekitException {

        // prime meridian shift parameters
        final DerivativeStructure theta    = linearModel(factory, date,
                                                         primeMeridianOffsetDriver, primeMeridianDriftDriver,
                                                         indices);
        final DerivativeStructure thetaDot = parametricModel(factory, primeMeridianDriftDriver, indices);

        // pole shift parameters
        final DerivativeStructure xpNeg    = linearModel(factory, date,
                                                         polarOffsetXDriver, polarDriftXDriver, indices).negate();
        final DerivativeStructure ypNeg    = linearModel(factory, date,
                                                         polarOffsetYDriver, polarDriftYDriver, indices).negate();
        final DerivativeStructure xpNegDot = parametricModel(factory, polarDriftXDriver, indices).negate();
        final DerivativeStructure ypNegDot = parametricModel(factory, polarDriftYDriver, indices).negate();

        return getTransform(date, theta, thetaDot, xpNeg, xpNegDot, ypNeg, ypNegDot);

    }

    /** Get the transform with derivatives.
     * @param date date of the transform
     * @param theta angle of the prime meridian
     * @param thetaDot angular rate of the prime meridian
     * @param xpNeg opposite of the angle of the pole motion along X
     * @param xpNegDot opposite of the angular rate of the pole motion along X
     * @param ypNeg opposite of the angle of the pole motion along Y
     * @param ypNegDot opposite of the angular rate of the pole motion along Y
     * @param <T> type of the field elements
     * @return computed transform with derivatives
     * @exception OrekitException if some frame transforms cannot be computed
     */
    private <T extends RealFieldElement<T>> FieldTransform<T> getTransform(final FieldAbsoluteDate<T> date,
                                                                           final T theta, final T thetaDot,
                                                                           final T xpNeg, final T xpNegDot,
                                                                           final T ypNeg, final T ypNegDot)
        throws OrekitException {

        final T                zero  = date.getField().getZero();
        final FieldVector3D<T> plusI = FieldVector3D.getPlusI(date.getField());
        final FieldVector3D<T> plusJ = FieldVector3D.getPlusJ(date.getField());
        final FieldVector3D<T> plusK = FieldVector3D.getPlusK(date.getField());

        // take parametric prime meridian shift into account
        final FieldTransform<T> meridianShift =
                        new FieldTransform<>(date,
                                             new FieldRotation<>(plusK, theta, RotationConvention.FRAME_TRANSFORM),
                                             new FieldVector3D<>(zero, zero, thetaDot));

        // take parametric pole shift into account
        final FieldTransform<T> poleShift =
                        new FieldTransform<>(date,
                                      new FieldTransform<>(date,
                                                           new FieldRotation<>(plusJ, xpNeg, RotationConvention.FRAME_TRANSFORM),
                                                           new FieldVector3D<>(zero, xpNegDot, zero)),
                                      new FieldTransform<>(date,
                                                           new FieldRotation<>(plusI, ypNeg, RotationConvention.FRAME_TRANSFORM),
                                                           new FieldVector3D<>(ypNegDot, zero, zero)));

        return new FieldTransform<>(date, meridianShift, poleShift);

    }

    /** Evaluate a parametric linear model.
     * @param date current date
     * @param offsetDriver driver for the offset parameter
     * @param driftDriver driver for the drift parameter
     * @return current value of the linear model
     * @exception OrekitException if reference date has not been set for the
     * offset driver
     */
    private double linearModel(final AbsoluteDate date,
                               final ParameterDriver offsetDriver, final ParameterDriver driftDriver)
        throws OrekitException {
        if (offsetDriver.getReferenceDate() == null) {
            throw new OrekitException(OrekitMessages.NO_REFERENCE_DATE_FOR_PARAMETER,
                                      offsetDriver.getName());
        }
        final double dt     = date.durationFrom(offsetDriver.getReferenceDate());
        final double offset = parametricModel(offsetDriver);
        final double drift  = parametricModel(driftDriver);
        return dt * drift + offset;
    }

    /** Evaluate a parametric linear model.
     * @param date current date
     * @param offsetDriver driver for the offset parameter
     * @param driftDriver driver for the drift parameter
     * @return current value of the linear model
     * @exception OrekitException if reference date has not been set for the
     * offset driver
     * @param <T> type of the filed elements
     */
    private <T extends RealFieldElement<T>> T linearModel(final FieldAbsoluteDate<T> date,
                                                          final ParameterDriver offsetDriver,
                                                          final ParameterDriver driftDriver)
        throws OrekitException {
        if (offsetDriver.getReferenceDate() == null) {
            throw new OrekitException(OrekitMessages.NO_REFERENCE_DATE_FOR_PARAMETER,
                                      offsetDriver.getName());
        }
        final T dt          = date.durationFrom(offsetDriver.getReferenceDate());
        final double offset = parametricModel(offsetDriver);
        final double drift  = parametricModel(driftDriver);
        return dt.multiply(drift).add(offset);
    }

    /** Evaluate a parametric linear model.
     * @param factory factory for the derivatives
     * @param date current date
     * @param offsetDriver driver for the offset parameter
     * @param driftDriver driver for the drift parameter
     * @param indices indices of the estimated parameters in derivatives computations
     * @return current value of the linear model
     * @exception OrekitException if reference date has not been set for the
     * offset driver
     */
    private DerivativeStructure linearModel(final DSFactory factory, final FieldAbsoluteDate<DerivativeStructure> date,
                                            final ParameterDriver offsetDriver, final ParameterDriver driftDriver,
                                            final Map<String, Integer> indices)
        throws OrekitException {
        if (offsetDriver.getReferenceDate() == null) {
            throw new OrekitException(OrekitMessages.NO_REFERENCE_DATE_FOR_PARAMETER,
                                      offsetDriver.getName());
        }
        final DerivativeStructure dt     = date.durationFrom(offsetDriver.getReferenceDate());
        final DerivativeStructure offset = parametricModel(factory, offsetDriver, indices);
        final DerivativeStructure drift  = parametricModel(factory, driftDriver, indices);
        return dt.multiply(drift).add(offset);
    }

    /** Evaluate a parametric model.
     * @param driver driver managing the parameter
     * @return value of the parametric model
     */
    private double parametricModel(final ParameterDriver driver) {
        return driver.getValue();
    }

    /** Evaluate a parametric model.
     * @param factory factory for the derivatives
     * @param driver driver managing the parameter
     * @param indices indices of the estimated parameters in derivatives computations
     * @return value of the parametric model
     */
    private DerivativeStructure parametricModel(final DSFactory factory, final ParameterDriver driver,
                                                final Map<String, Integer> indices) {
        final Integer index = indices.get(driver.getName());
        return (index == null) ?
               factory.constant(driver.getValue()) :
               factory.variable(index, driver.getValue());
    }

    /** Replace the instance with a data transfer object for serialization.
     * <p>
     * This intermediate class serializes the files supported names, the ephemeris type
     * and the body name.
     * </p>
     * @return data transfer object that will be serialized
     */
    private Object writeReplace() {
        return new DataTransferObject(baseUT1,
                                      primeMeridianOffsetDriver.getValue(),
                                      primeMeridianDriftDriver.getValue(),
                                      polarOffsetXDriver.getValue(),
                                      polarDriftXDriver.getValue(),
                                      polarOffsetYDriver.getValue(),
                                      polarDriftYDriver.getValue());
    }

    /** Local time scale for estimated UT1. */
    private class EstimatedUT1Scale extends UT1Scale {

        /** Serializable UID. */
        private static final long serialVersionUID = 20170922L;

        /** Simple constructor.
         */
        EstimatedUT1Scale() {
            super(baseUT1.getEOPHistory(), baseUT1.getUTCScale());
        }

        /** {@inheritDoc} */
        @Override
        public <T extends RealFieldElement<T>> T offsetFromTAI(final FieldAbsoluteDate<T> date) {
            try {
                final T dut1 = linearModel(date, primeMeridianOffsetDriver, primeMeridianDriftDriver).divide(EARTH_ANGULAR_VELOCITY);
                return baseUT1.offsetFromTAI(date).add(dut1);
            } catch (OrekitException oe) {
                throw new OrekitExceptionWrapper(oe);
            }
        }

        /** {@inheritDoc} */
        @Override
        public double offsetFromTAI(final AbsoluteDate date) throws OrekitExceptionWrapper {
            try {
                final double dut1 = linearModel(date, primeMeridianOffsetDriver, primeMeridianDriftDriver) / EARTH_ANGULAR_VELOCITY;
                return baseUT1.offsetFromTAI(date) + dut1;
            } catch (OrekitException oe) {
                throw new OrekitExceptionWrapper(oe);
            }
        }

        /** {@inheritDoc} */
        @Override
        public String getName() {
            return baseUT1.getName() + "/estimated";
        }

    }

    /** Internal class used only for serialization. */
    private static class DataTransferObject implements Serializable {

        /** Serializable UID. */
        private static final long serialVersionUID = 20171124L;

        /** Underlying raw UT1. */
        private final UT1Scale baseUT1;

        /** Current prime meridian offset. */
        private final double primeMeridianOffset;

        /** Current prime meridian drift. */
        private final double primeMeridianDrift;

        /** Current pole offset along X. */
        private final double polarOffsetX;

        /** Current pole drift along X. */
        private final double polarDriftX;

        /** Current pole offset along Y. */
        private final double polarOffsetY;

        /** Current pole drift along Y. */
        private final double polarDriftY;

        /** Simple constructor.
         * @param baseUT1 underlying raw UT1
         * @param primeMeridianOffset current prime meridian offset
         * @param primeMeridianDrift current prime meridian drift
         * @param polarOffsetX current pole offset along X
         * @param polarDriftX current pole drift along X
         * @param polarOffsetY current pole offset along Y
         * @param polarDriftY current pole drift along Y
         */
        DataTransferObject(final  UT1Scale baseUT1,
                           final double primeMeridianOffset, final double primeMeridianDrift,
                           final double polarOffsetX,        final double polarDriftX,
                           final double polarOffsetY,        final double polarDriftY) {
            this.baseUT1             = baseUT1;
            this.primeMeridianOffset = primeMeridianOffset;
            this.primeMeridianDrift  = primeMeridianDrift;
            this.polarOffsetX        = polarOffsetX;
            this.polarDriftX         = polarDriftX;
            this.polarOffsetY        = polarOffsetY;
            this.polarDriftY         = polarDriftY;
        }

        /** Replace the deserialized data transfer object with a {@link EstimatedEarthFrameProvider}.
         * @return replacement {@link EstimatedEarthFrameProvider}
         */
        private Object readResolve() {
            try {
                final EstimatedEarthFrameProvider provider = new EstimatedEarthFrameProvider(baseUT1);
                provider.getPrimeMeridianOffsetDriver().setValue(primeMeridianOffset);
                provider.getPrimeMeridianDriftDriver().setValue(primeMeridianDrift);
                provider.getPolarOffsetXDriver().setValue(polarOffsetX);
                provider.getPolarDriftXDriver().setValue(polarDriftX);
                provider.getPolarOffsetYDriver().setValue(polarOffsetY);
                provider.getPolarDriftYDriver().setValue(polarDriftY);
                return provider;
            } catch (OrekitException oe) {
                // this should never happen as values already come from previous drivers
                throw new OrekitInternalError(oe);
            }
        }

    }

}