JacobianPropagatorConverter.java

/* Copyright 2002-2019 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.propagation.conversion;

import java.util.List;

import org.hipparchus.analysis.MultivariateVectorFunction;
import org.hipparchus.linear.ArrayRealVector;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.hipparchus.optim.nonlinear.vector.leastsquares.MultivariateJacobianFunction;
import org.hipparchus.util.Pair;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
import org.orekit.orbits.OrbitType;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.events.DateDetector;
import org.orekit.propagation.events.handlers.EventHandler;
import org.orekit.propagation.numerical.JacobiansMapper;
import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.propagation.numerical.PartialDerivativesEquations;
import org.orekit.utils.PVCoordinates;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.ParameterDriversList;

/** Propagator converter using the real Jacobian.
 * @author Pascal Parraud
 * @since 6.0
 */
public class JacobianPropagatorConverter extends AbstractPropagatorConverter {

    /** Numerical propagator builder. */
    private final NumericalPropagatorBuilder builder;

    /** Simple constructor.
     * @param builder builder for adapted propagator, it <em>must</em>
     * be configured to generate {@link OrbitType#CARTESIAN} states
     * @param threshold absolute threshold for optimization algorithm
     * @param maxIterations maximum number of iterations for fitting
     */
    public JacobianPropagatorConverter(final NumericalPropagatorBuilder builder,
                                       final double threshold,
                                       final int maxIterations) {
        super(builder, threshold, maxIterations);
        if (builder.getOrbitType() != OrbitType.CARTESIAN) {
            throw new OrekitException(OrekitMessages.ORBIT_TYPE_NOT_ALLOWED,
                                      builder.getOrbitType(), OrbitType.CARTESIAN);
        }
        this.builder = builder;
    }

    /** {@inheritDoc} */
    protected MultivariateVectorFunction getObjectiveFunction() {
        return new MultivariateVectorFunction() {

            /** {@inheritDoc} */
            public double[] value(final double[] arg)
                throws IllegalArgumentException, OrekitException {

                final double[] value = new double[getTargetSize()];

                final NumericalPropagator prop = builder.buildPropagator(arg);

                final int stateSize = isOnlyPosition() ? 3 : 6;
                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
                        fillRows(value, row, prop.getInitialState());
                    } else {
                        // use a date detector to pick up states
                        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) {
                                fillRows(value, row, state);
                                return row + stateSize >= getTargetSize() ? Action.STOP : Action.CONTINUE;
                            }
                        }));
                    }
                }

                prop.propagate(sample.get(sample.size() - 1).getDate().shiftedBy(10.0));

                return value;
            }
        };
    }

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

            /** {@inheritDoc} */
            public Pair<RealVector, RealMatrix> value(final RealVector point)
                throws IllegalArgumentException, OrekitException {

                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) {
                                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);
            }
        };
    }

    /** Fill up a few value rows (either 6 or 3 depending on velocities used or not).
     * @param value values array
     * @param row first row index
     * @param state spacecraft state
     */
    private void fillRows(final double[] value, final int row, final SpacecraftState state) {
        final PVCoordinates pv = state.getPVCoordinates(getFrame());
        value[row    ] = pv.getPosition().getX();
        value[row + 1] = pv.getPosition().getY();
        value[row + 2] = pv.getPosition().getZ();
        if (!isOnlyPosition()) {
            value[row + 3] = pv.getVelocity().getX();
            value[row + 4] = pv.getVelocity().getY();
            value[row + 5] = pv.getVelocity().getZ();
        }
    }

    /** Fill up a few Jacobian rows (either 6 or 3 depending on velocities used or not).
     * @param value values vector
     * @param jacobian Jacobian matrix
     * @param row first row index
     * @param state spacecraft state
     * @param stateSize state size
     * @param orbitalParameters drivers for the orbital parameters
     * @param propagationParameters drivers for the propagation model parameters
     * @param mapper state mapper
     */
    private void fillRows(final RealVector value, final RealMatrix jacobian, final int row,
                          final SpacecraftState state, final int stateSize,
                          final ParameterDriversList orbitalParameters,
                          final ParameterDriversList propagationParameters,
                          final JacobiansMapper mapper) {

        // value part
        final PVCoordinates pv = state.getPVCoordinates(getFrame());
        value.setEntry(row,     pv.getPosition().getX());
        value.setEntry(row + 1, pv.getPosition().getY());
        value.setEntry(row + 2, pv.getPosition().getZ());
        if (!isOnlyPosition()) {
            value.setEntry(row + 3, pv.getVelocity().getX());
            value.setEntry(row + 4, pv.getVelocity().getY());
            value.setEntry(row + 5, pv.getVelocity().getZ());
        }

        // Jacobian part
        final double[][] dYdY0 = new double[JacobiansMapper.STATE_DIMENSION][JacobiansMapper.STATE_DIMENSION];
        final double[][] dYdP  = new double[JacobiansMapper.STATE_DIMENSION][mapper.getParameters()];
        mapper.getStateJacobian(state, dYdY0);
        mapper.getParametersJacobian(state, dYdP);
        for (int k = 0; k < stateSize; k++) {
            int index = 0;
            for (int j = 0; j < orbitalParameters.getNbParams(); ++j) {
                final ParameterDriver driver = orbitalParameters.getDrivers().get(j);
                if (driver.isSelected()) {
                    jacobian.setEntry(row + k, index++, dYdY0[k][j] * driver.getScale());
                }
            }
            for (int j = 0; j < propagationParameters.getNbParams(); ++j) {
                final ParameterDriver driver = propagationParameters.getDrivers().get(j);
                jacobian.setEntry(row + k, index++, dYdP[k][j] * driver.getScale());
            }
        }

    }

}