1   /* Copyright 2002-2022 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.utils;
18  
19  import java.util.ArrayList;
20  import java.util.List;
21  
22  import org.hipparchus.geometry.euclidean.threed.Vector3D;
23  import org.hipparchus.ode.ODEIntegrator;
24  import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
25  import org.hipparchus.ode.nonstiff.DormandPrince54Integrator;
26  import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
27  import org.junit.Assert;
28  import org.junit.Before;
29  import org.junit.Test;
30  import org.orekit.Utils;
31  import org.orekit.attitudes.Attitude;
32  import org.orekit.attitudes.AttitudeProvider;
33  import org.orekit.bodies.CelestialBody;
34  import org.orekit.bodies.CelestialBodyFactory;
35  import org.orekit.errors.OrekitException;
36  import org.orekit.forces.ForceModel;
37  import org.orekit.forces.gravity.NewtonianAttraction;
38  import org.orekit.forces.gravity.ThirdBodyAttraction;
39  import org.orekit.forces.gravity.ThirdBodyAttractionEpoch;
40  import org.orekit.frames.Frame;
41  import org.orekit.frames.FramesFactory;
42  import org.orekit.propagation.SpacecraftState;
43  import org.orekit.propagation.numerical.EpochDerivativesEquations;
44  import org.orekit.propagation.numerical.NumericalPropagator;
45  import org.orekit.time.AbsoluteDate;
46  import org.orekit.time.TimeScalesFactory;
47  
48  public class MultipleShooterTest {
49  
50      private static final double eps = 1.0e-6;
51  
52      /** arbitrary date */
53      private static final AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
54      /** arbitrary inertial frame */
55      private static final Frame eci = FramesFactory.getGCRF();
56  
57      /** unused propagator */
58      private NumericalPropagator propagator;
59      /** mock force model */
60      private ForceModel forceModel;
61      /** arbitrary PV */
62      private PVCoordinates pv;
63      /** arbitrary state */
64      private SpacecraftState state;
65      /** subject under test */
66      private EpochDerivativesEquations pde;
67  
68      @Before
69      public void setUp() {
70          Utils.setDataRoot("regular-data");
71          propagator = new NumericalPropagator(new DormandPrince54Integrator(1, 500, 0.001, 0.001));
72          forceModel = new ThirdBodyAttractionEpoch(CelestialBodyFactory.getSun());
73          propagator.addForceModel(forceModel);
74          propagator.addForceModel(new NewtonianAttraction(Constants.WGS84_EARTH_MU));
75          pde = new EpochDerivativesEquations("ede", propagator);
76          Vector3D p = new Vector3D(7378137, 0, 0);
77          Vector3D v = new Vector3D(0, 7500, 0);
78          pv = new PVCoordinates(p, v);
79          state = new SpacecraftState(new AbsolutePVCoordinates(eci,date,pv))
80                  .addAdditionalState("ede", new double[2 * 3 * 6]);
81          pde.setInitialJacobians(state);
82      }
83  
84      @Test
85      public void testMultipleShooting() {
86  
87          // Time settings
88          // -------------
89          final AbsoluteDate initialDate =
90                          new AbsoluteDate(2000, 01, 01, 0, 0, 00.000,
91                                           TimeScalesFactory.getUTC());
92          final double arcDuration = 10000;
93         
94          final PVCoordinates firstGuess = new PVCoordinates(new Vector3D(1.25E10, 1.450E11, -7.5E9),
95                                                             new Vector3D(-30000.0, 2500.0, -3500.0));
96  
97          // Integration parameters
98          // ---------------------------
99          // Adaptive stepsize boundaries
100         final double minStep = 1;
101         final double maxstep = 30000;
102 
103         // Integrator tolerances
104         final double positionTolerance = 1000;
105         final double velocityTolerance = 1;
106         final double massTolerance = 1.0e-6;
107         final double[] vecAbsoluteTolerances = {positionTolerance, positionTolerance, positionTolerance, velocityTolerance, velocityTolerance, velocityTolerance, massTolerance };
108         final double[] vecRelativeTolerances =
109                         new double[vecAbsoluteTolerances.length];
110 
111         // Integrator definition
112         final AdaptiveStepsizeIntegrator integrator =
113                         new DormandPrince853Integrator(minStep, maxstep,
114                                                        vecAbsoluteTolerances,
115                                                        vecRelativeTolerances);
116 
117         // Load Celestial bodies
118         // ---------------------
119         final CelestialBody   sun     = CelestialBodyFactory.getSun();
120         final CelestialBody   earth    = CelestialBodyFactory.getEarthMoonBarycenter();
121         final Frame primaryFrame = sun.getInertiallyOrientedFrame();
122 
123         // Trajectory definition
124         // ---------------------
125         final int narcs = 5;
126         final List<SpacecraftState> correctedList = new ArrayList<SpacecraftState>(narcs + 1);
127         final AbsolutePVCoordinates firstGuessAPV = new AbsolutePVCoordinates(primaryFrame, initialDate, firstGuess);
128         List<SpacecraftState> firstGuessList2 = generatePatchPointsEphemeris(sun, earth, firstGuessAPV, arcDuration, narcs, integrator);
129         final List<NumericalPropagator> propagatorList  = initializePropagators(sun, earth, integrator, narcs);
130         final List<EpochDerivativesEquations> epochEquations = addDerivativesProviders(propagatorList);
131 
132         for (int i = 0; i < narcs + 1; i++) {
133             final SpacecraftState sp = firstGuessList2.get(i);
134             correctedList.add(new SpacecraftState(sp.getAbsPVA(), sp.getAttitude()));
135         }
136 
137         // Perturbation on a patch point
138         // -----------------------------
139 
140         final int nP = 1; // Perturbed patch point
141         final Vector3D deltaP = new Vector3D(-50000,1000,0);
142         final Vector3D deltaV = new Vector3D(0.1,0,1.0);
143         final double deltaEpoch = 1000;
144 
145         final SpacecraftState firstGuessSP = correctedList.get(nP);
146         final AttitudeProvider attPro = propagatorList.get(nP).getAttitudeProvider();
147 
148         // Small change of the a patch point
149         final Vector3D newPos = firstGuessSP.getAbsPVA().getPosition().add(deltaP); 
150         final Vector3D newVel = firstGuessSP.getAbsPVA().getVelocity().add(deltaV);
151         final AbsoluteDate newDate = firstGuessSP.getDate().shiftedBy(deltaEpoch);
152         AbsolutePVCoordinates absPva = new AbsolutePVCoordinates(firstGuessSP.getFrame(), newDate, newPos, newVel);
153         final Attitude attitude = attPro.getAttitude(absPva, newDate, absPva.getFrame());
154         SpacecraftState newSP = new SpacecraftState(absPva , attitude);
155         correctedList.set(nP, newSP);
156 
157         final double tolerance = 1.0;
158 
159         MultipleShooter multipleShooting = new MultipleShooter(correctedList, propagatorList, epochEquations, arcDuration, tolerance, 10);
160         multipleShooting.setPatchPointComponentFreedom(1, 0, false);
161         multipleShooting.setPatchPointComponentFreedom(1, 1, false);
162         multipleShooting.setPatchPointComponentFreedom(1, 2, false);
163         multipleShooting.setPatchPointComponentFreedom(1, 3, false);
164         multipleShooting.setPatchPointComponentFreedom(narcs + 1, 0, false);
165         multipleShooting.setPatchPointComponentFreedom(narcs + 1, 1, false);
166         multipleShooting.setPatchPointComponentFreedom(narcs + 1, 2, false);
167         multipleShooting.setEpochFreedom(1, false);
168 
169         multipleShooting.compute();
170 
171         // Verify
172         Assert.assertEquals(0.0,      Vector3D.distance(firstGuessList2.get(0).getAbsPVA().getPosition(), correctedList.get(0).getAbsPVA().getPosition()), eps);
173         Assert.assertEquals(0.0,      Vector3D.distance(firstGuessList2.get(0).getAbsPVA().getVelocity(), correctedList.get(0).getAbsPVA().getVelocity()), eps);
174         Assert.assertEquals(0.005230, Vector3D.distance(firstGuessList2.get(1).getAbsPVA().getPosition(), correctedList.get(1).getAbsPVA().getPosition()), eps);
175         Assert.assertEquals(0.0,      Vector3D.distance(firstGuessList2.get(1).getAbsPVA().getVelocity(), correctedList.get(1).getAbsPVA().getVelocity()), eps);
176         Assert.assertEquals(0.009869, Vector3D.distance(firstGuessList2.get(2).getAbsPVA().getPosition(), correctedList.get(2).getAbsPVA().getPosition()), eps);
177         Assert.assertEquals(0.0,      Vector3D.distance(firstGuessList2.get(2).getAbsPVA().getVelocity(), correctedList.get(2).getAbsPVA().getVelocity()), eps);
178         Assert.assertEquals(0.006641, Vector3D.distance(firstGuessList2.get(3).getAbsPVA().getPosition(), correctedList.get(3).getAbsPVA().getPosition()), eps);
179         Assert.assertEquals(0.0,      Vector3D.distance(firstGuessList2.get(3).getAbsPVA().getVelocity(), correctedList.get(3).getAbsPVA().getVelocity()), eps);
180         Assert.assertEquals(0.003216, Vector3D.distance(firstGuessList2.get(4).getAbsPVA().getPosition(), correctedList.get(4).getAbsPVA().getPosition()), eps);
181         Assert.assertEquals(0.0,      Vector3D.distance(firstGuessList2.get(4).getAbsPVA().getVelocity(), correctedList.get(4).getAbsPVA().getVelocity()), eps);
182         Assert.assertEquals(0.0,      Vector3D.distance(firstGuessList2.get(5).getAbsPVA().getPosition(), correctedList.get(5).getAbsPVA().getPosition()), eps);
183         Assert.assertEquals(0.0,      Vector3D.distance(firstGuessList2.get(5).getAbsPVA().getVelocity(), correctedList.get(5).getAbsPVA().getVelocity()), eps);
184 
185     }
186 
187     @Test
188     public void testMultipleShootingWithEstimatedParameters() {
189 
190         // Time settings
191         // -------------
192         final AbsoluteDate initialDate =
193                         new AbsoluteDate(2000, 01, 01, 0, 0, 00.000,
194                                          TimeScalesFactory.getUTC());
195         final double arcDuration = 10000;
196        
197         final PVCoordinates firstGuess = new PVCoordinates(new Vector3D(1.25E10, 1.450E11, -7.5E9),
198                                                            new Vector3D(-30000.0, 2500.0, -3500.0));
199 
200         // Integration parameters
201         // ---------------------------
202         // Adaptive stepsize boundaries
203         final double minStep = 1;
204         final double maxstep = 30000;
205 
206         // Integrator tolerances
207         final double positionTolerance = 1000;
208         final double velocityTolerance = 1;
209         final double massTolerance = 1.0e-6;
210         final double[] vecAbsoluteTolerances = {positionTolerance, positionTolerance, positionTolerance, velocityTolerance, velocityTolerance, velocityTolerance, massTolerance };
211         final double[] vecRelativeTolerances =
212                         new double[vecAbsoluteTolerances.length];
213 
214         // Integrator definition
215         final AdaptiveStepsizeIntegrator integrator =
216                         new DormandPrince853Integrator(minStep, maxstep,
217                                                        vecAbsoluteTolerances,
218                                                        vecRelativeTolerances);
219 
220         // Load Celestial bodies
221         // ---------------------
222         final CelestialBody   sun     = CelestialBodyFactory.getSun();
223         final CelestialBody   earth    = CelestialBodyFactory.getEarthMoonBarycenter();
224         final Frame primaryFrame = sun.getInertiallyOrientedFrame();
225 
226         // Trajectory definition
227         // ---------------------
228         final int narcs = 5;
229         final List<SpacecraftState> correctedList = new ArrayList<SpacecraftState>(narcs + 1);
230         final AbsolutePVCoordinates firstGuessAPV = new AbsolutePVCoordinates(primaryFrame, initialDate, firstGuess);
231         List<SpacecraftState> firstGuessList2 = generatePatchPointsEphemeris(sun, earth, firstGuessAPV, arcDuration, narcs, integrator);
232         final List<NumericalPropagator> propagatorList  = initializePropagatorsWithEstimated(sun, earth, integrator, narcs);
233         final List<EpochDerivativesEquations> epochEquations = addDerivativesProviders(propagatorList);
234 
235         for (int i = 0; i < narcs + 1; i++) {
236             final SpacecraftState sp = firstGuessList2.get(i);
237             correctedList.add(new SpacecraftState(sp.getAbsPVA(), sp.getAttitude()));
238         }
239 
240         // Perturbation on a patch point
241         // -----------------------------
242 
243         final int nP = 1; // Perturbated patch point
244         final Vector3D deltaP = new Vector3D(-50000,1000,0);
245         final Vector3D deltaV = new Vector3D(0.1,0,1.0);
246         final double deltaEpoch = 1000;
247 
248         final SpacecraftState firstGuessSP = correctedList.get(nP);
249         final AttitudeProvider attPro = propagatorList.get(nP).getAttitudeProvider();
250 
251         // Small change of the a patch point
252         final Vector3D newPos = firstGuessSP.getAbsPVA().getPosition().add(deltaP); 
253         final Vector3D newVel = firstGuessSP.getAbsPVA().getVelocity().add(deltaV);
254         final AbsoluteDate newDate = firstGuessSP.getDate().shiftedBy(deltaEpoch);
255         AbsolutePVCoordinates absPva = new AbsolutePVCoordinates(firstGuessSP.getFrame(), newDate, newPos, newVel);
256         final Attitude attitude = attPro.getAttitude(absPva, newDate, absPva.getFrame());
257         SpacecraftState newSP = new SpacecraftState(absPva , attitude);
258         correctedList.set(1, newSP);
259 
260         final double tolerance = 1.0;
261 
262         MultipleShooter multipleShooting = new MultipleShooter(correctedList, propagatorList, epochEquations, arcDuration, tolerance, 10);
263         multipleShooting.setPatchPointComponentFreedom(1, 0, false);
264         multipleShooting.setPatchPointComponentFreedom(1, 1, false);
265         multipleShooting.setPatchPointComponentFreedom(1, 2, false);
266         multipleShooting.setPatchPointComponentFreedom(1, 3, false);
267         multipleShooting.setPatchPointComponentFreedom(narcs + 1, 0, false);
268         multipleShooting.setPatchPointComponentFreedom(narcs + 1, 1, false);
269         multipleShooting.setPatchPointComponentFreedom(narcs + 1, 2, false);
270         multipleShooting.setEpochFreedom(1, false);
271 
272         multipleShooting.compute();
273 
274         // Verify
275         Assert.assertEquals(0.0,      Vector3D.distance(firstGuessList2.get(0).getAbsPVA().getPosition(), correctedList.get(0).getAbsPVA().getPosition()), eps);
276         Assert.assertEquals(0.0,      Vector3D.distance(firstGuessList2.get(0).getAbsPVA().getVelocity(), correctedList.get(0).getAbsPVA().getVelocity()), eps);
277         Assert.assertEquals(0.000108, Vector3D.distance(firstGuessList2.get(1).getAbsPVA().getPosition(), correctedList.get(1).getAbsPVA().getPosition()), eps);
278         Assert.assertEquals(0.0,      Vector3D.distance(firstGuessList2.get(1).getAbsPVA().getVelocity(), correctedList.get(1).getAbsPVA().getVelocity()), eps);
279         Assert.assertEquals(0.000308, Vector3D.distance(firstGuessList2.get(2).getAbsPVA().getPosition(), correctedList.get(2).getAbsPVA().getPosition()), eps);
280         Assert.assertEquals(0.0,      Vector3D.distance(firstGuessList2.get(2).getAbsPVA().getVelocity(), correctedList.get(2).getAbsPVA().getVelocity()), eps);
281     }
282 
283     @Test(expected=OrekitException.class)
284     public void testNotInitialized() {
285         new EpochDerivativesEquations("partials", propagator).getMapper();
286     }
287 
288     @Test(expected=OrekitException.class)
289     public void testTooSmallDimension() {
290         final EpochDerivativesEquations partials = new EpochDerivativesEquations("partials", propagator);
291         partials.setInitialJacobians(state, new double[5][6], new double[6][2]);
292     }
293 
294     @Test(expected=OrekitException.class)
295     public void testTooLargeDimension() {
296         final EpochDerivativesEquations partials = new EpochDerivativesEquations("partials", propagator);
297         partials.setInitialJacobians(state, new double[8][6], new double[6][2]);
298     }
299 
300     @Test(expected=OrekitException.class)
301     public void testMismatchedDimensions() {
302         final EpochDerivativesEquations partials = new EpochDerivativesEquations("partials", propagator);
303         partials.setInitialJacobians(state, new double[6][6], new double[7][2]);
304     }
305 
306     private static List<SpacecraftState> generatePatchPointsEphemeris(final CelestialBody primary, final CelestialBody secondary,
307                                                                       final AbsolutePVCoordinates initialAPV, final double arcDuration,
308                                                                       final int narcs, final ODEIntegrator integrator) {
309 
310         List<SpacecraftState> firstGuessList = new ArrayList<SpacecraftState>(narcs+1);
311 
312         final double integrationTime = arcDuration;
313 
314         // Creating the initial spacecraftstate that will be given to the propagator
315 
316         final SpacecraftState initialState2 = new SpacecraftState(initialAPV);
317 
318         firstGuessList.add(initialState2);
319 
320         NumericalPropagator propagator = new NumericalPropagator(integrator);
321 
322         propagator.addForceModel(new NewtonianAttraction(primary.getGM()));
323         propagator.addForceModel(new ThirdBodyAttraction(secondary));
324         propagator.setOrbitType(null);
325 
326 
327         propagator.setInitialState(initialState2);
328 
329         AbsoluteDate currentDate = initialAPV.getDate();
330         for(int i = 0; i < narcs; i++) {
331 
332             currentDate = currentDate.shiftedBy(integrationTime);
333             final SpacecraftState sp = propagator.propagate(currentDate);
334 
335             firstGuessList.add(sp);
336         }
337         return firstGuessList;
338     }
339 
340     private static List<NumericalPropagator> initializePropagators(final CelestialBody primary, final CelestialBody secondary, final ODEIntegrator integrator,
341                                                                    final int propagationNumber) {
342         final List<NumericalPropagator> propagatorList = new ArrayList<NumericalPropagator>(propagationNumber);
343 
344         // Definition of the propagator
345         for(int i = 0; i < propagationNumber; i++) {
346 
347             NumericalPropagator propagator = new NumericalPropagator(integrator);
348 
349             propagator.addForceModel(new NewtonianAttraction(primary.getGM()));
350             propagator.addForceModel(new ThirdBodyAttractionEpoch(secondary));
351 
352             propagator.setOrbitType(null);
353             propagatorList.add(propagator);
354         }        
355         return propagatorList;
356     }
357 
358     private static List<NumericalPropagator> initializePropagatorsWithEstimated(final CelestialBody primary, final CelestialBody secondary, final ODEIntegrator integrator,
359                                                                                 final int propagationNumber) {
360         final List<NumericalPropagator> propagatorList = new ArrayList<NumericalPropagator>(propagationNumber);
361 
362         // Definition of the propagator
363         for(int i = 0; i < propagationNumber; i++) {
364 
365             NumericalPropagator propagator = new NumericalPropagator(integrator);
366 
367             propagator.addForceModel(new NewtonianAttraction(primary.getGM()));
368             final ForceModel model = new ThirdBodyAttractionEpoch(secondary);
369             model.getParametersDrivers().get(0).setSelected(true);
370             propagator.addForceModel(model);
371 
372             propagator.setOrbitType(null);
373             propagatorList.add(propagator);
374         }        
375         return propagatorList;
376     }
377 
378     private static List<EpochDerivativesEquations> addDerivativesProviders(List<NumericalPropagator> propagatorList){
379         final int narcs = propagatorList.size();
380         final List<EpochDerivativesEquations> additionalEquations = new ArrayList<>(narcs) ;
381         for(int i = 0; i < narcs; i++) {
382             additionalEquations.add(new EpochDerivativesEquations("derivatives", propagatorList.get(i)));
383         }
384         return additionalEquations;
385 
386     }
387 
388 }