1   /* Copyright 2002-2021 CS GROUP
2    * Licensed to CS GROUP (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.orekit.propagation.analytical.tle;
18  
19  import org.orekit.errors.OrekitException;
20  import org.orekit.errors.OrekitMessages;
21  import org.orekit.propagation.SpacecraftState;
22  import org.orekit.propagation.analytical.AbstractAnalyticalPropagator;
23  import org.orekit.propagation.integration.AdditionalEquations;
24  import org.orekit.utils.ParameterDriver;
25  import org.orekit.utils.ParameterDriversList;
26  
27  /** Set of {@link AdditionalEquations additional equations} computing the partial derivatives
28   * of the state (orbit) with respect to initial state.
29   * <p>
30   * This set of equations are automatically added to an {@link AbstractAnalyticalPropagator analytical propagator}
31   * in order to compute partial derivatives of the orbit along with the orbit itself. This is
32   * useful for example in orbit determination applications.
33   * </p>
34   * <p>
35   * The partial derivatives with respect to initial state are dimension 6 (orbit only).
36   * </p>
37   * @author Bryan Cazabonne
38   * @author Thomas Paulet
39   * @since 11.0
40   */
41  public class TLEPartialDerivativesEquations {
42  
43      /** Propagator computing state evolution. */
44      private final TLEPropagator propagator;
45  
46      /** Selected parameters for Jacobian computation. */
47      private ParameterDriversList selected;
48  
49      /** Name. */
50      private final String name;
51  
52      /** Flag for Jacobian matrices initialization. */
53      private boolean initialized;
54  
55      /** Simple constructor.
56       * <p>
57       * Instance regrouping equations to compute derivatives.
58       * </p>
59       * @param name name of the partial derivatives equations
60       * @param propagator the propagator that will handle the orbit propagation
61       */
62      public TLEPartialDerivativesEquations(final String name,
63                                            final TLEPropagator propagator) {
64          this.name        = name;
65          this.selected    = null;
66          this.propagator  = propagator;
67          this.initialized = false;
68      }
69  
70      /** Get the name of the additional state.
71       * @return name of the additional state
72       */
73      public String getName() {
74          return name;
75      }
76  
77      /** Freeze the selected parameters from the force models.
78       */
79      private void freezeParametersSelection() {
80          if (selected == null) {
81              // create new selected parameter driver list
82              selected = new ParameterDriversList();
83              for (final ParameterDriver driver : propagator.getTLE().getParametersDrivers()) {
84                  if (driver.isSelected()) {
85                      selected.add(driver);
86                  }
87              }
88          }
89      }
90  
91      /** Set the initial value of the Jacobian with respect to state and parameter.
92       * <p>
93       * This method is equivalent to call {@link #setInitialJacobians(SpacecraftState,
94       * double[][], double[][])} with dYdY0 set to the identity matrix and dYdP set
95       * to a zero matrix.
96       * </p>
97       * <p>
98       * The force models parameters for which partial derivatives are desired,
99       * <em>must</em> have been {@link ParameterDriver#setSelected(boolean) selected}
100      * before this method is called, so proper matrices dimensions are used.
101      * </p>
102      * @param s0 initial state
103      * @return state with initial Jacobians added
104      */
105     public SpacecraftState setInitialJacobians(final SpacecraftState s0) {
106         freezeParametersSelection();
107         final int stateDimension = 6;
108         final double[][] dYdY0 = new double[stateDimension][stateDimension];
109         final double[][] dYdP  = new double[stateDimension][selected.getNbParams()];
110         for (int i = 0; i < stateDimension; ++i) {
111             dYdY0[i][i] = 1.0;
112         }
113         return setInitialJacobians(s0, dYdY0, dYdP);
114     }
115 
116     /** Set the initial value of the Jacobian with respect to state and parameter.
117      * <p>
118      * The returned state must be added to the propagator (it is not done
119      * automatically, as the user may need to add more states to it).
120      * </p>
121      * @param s1 current state
122      * @param dY1dY0 Jacobian of current state at time t₁ with respect
123      * to state at some previous time t₀ (must be 6x6)
124      * @param dY1dP Jacobian of current state at time t₁ with respect
125      * to parameters (may be null if no parameters are selected)
126      * @return state with initial Jacobians added
127      */
128     public SpacecraftState setInitialJacobians(final SpacecraftState s1,
129                                                final double[][] dY1dY0, final double[][] dY1dP) {
130 
131         freezeParametersSelection();
132 
133         // Check dimensions
134         final int stateDim = dY1dY0.length;
135         if (stateDim != 6 || stateDim != dY1dY0[0].length) {
136             throw new OrekitException(OrekitMessages.STATE_JACOBIAN_NOT_6X6,
137                                       stateDim, dY1dY0[0].length);
138         }
139         if (dY1dP != null && stateDim != dY1dP.length) {
140             throw new OrekitException(OrekitMessages.STATE_AND_PARAMETERS_JACOBIANS_ROWS_MISMATCH,
141                                       stateDim, dY1dP.length);
142         }
143         if (dY1dP == null && selected.getNbParams() != 0 ||
144             dY1dP != null && selected.getNbParams() != dY1dP[0].length) {
145             throw new OrekitException(new OrekitException(OrekitMessages.INITIAL_MATRIX_AND_PARAMETERS_NUMBER_MISMATCH,
146                                                           dY1dP == null ? 0 : dY1dP[0].length, selected.getNbParams()));
147         }
148 
149         // store the matrices as a single dimension array
150         initialized = true;
151         final TLEJacobiansMapper mapper = getMapper();
152         final double[] p = new double[mapper.getAdditionalStateDimension()];
153         mapper.setInitialJacobians(s1, dY1dY0, dY1dP, p);
154 
155         // set value in propagator
156         return s1.addAdditionalState(name, p);
157 
158     }
159 
160     /** Get a mapper between two-dimensional Jacobians and one-dimensional additional state.
161      * @return a mapper between two-dimensional Jacobians and one-dimensional additional state,
162      * with the same name as the instance
163      * @see #setInitialJacobians(SpacecraftState)
164      * @see #setInitialJacobians(SpacecraftState, double[][], double[][])
165      */
166     public TLEJacobiansMapper getMapper() {
167         if (!initialized) {
168             throw new OrekitException(OrekitMessages.STATE_JACOBIAN_NOT_INITIALIZED);
169         }
170         return new TLEJacobiansMapper(name, selected, propagator);
171     }
172 
173 }