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.estimation.leastsquares;
18  
19  import static org.junit.Assert.assertEquals;
20  
21  import java.util.ArrayList;
22  import java.util.List;
23  import java.util.Map;
24  
25  import org.hipparchus.geometry.euclidean.threed.Vector3D;
26  import org.hipparchus.linear.ArrayRealVector;
27  import org.hipparchus.linear.RealMatrix;
28  import org.hipparchus.linear.RealVector;
29  import org.hipparchus.util.Incrementor;
30  import org.hipparchus.util.Pair;
31  import org.junit.Assert;
32  import org.junit.Test;
33  import org.orekit.estimation.DSSTContext;
34  import org.orekit.estimation.DSSTEstimationTestUtils;
35  import org.orekit.estimation.DSSTForce;
36  import org.orekit.estimation.measurements.EstimatedMeasurement;
37  import org.orekit.estimation.measurements.ObservedMeasurement;
38  import org.orekit.estimation.measurements.PVMeasurementCreator;
39  import org.orekit.orbits.Orbit;
40  import org.orekit.propagation.PropagationType;
41  import org.orekit.propagation.Propagator;
42  import org.orekit.propagation.SpacecraftState;
43  import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
44  import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
45  import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
46  import org.orekit.utils.ParameterDriver;
47  import org.orekit.utils.ParameterDriversList;
48  
49  public class DSSTBatchLSModelTest {
50  
51      @Test
52      public void testPerfectValue() {
53  
54          final DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
55  
56          final DSSTPropagatorBuilder propagatorBuilder =
57                          context.createBuilder(true, 1.0e-6, 60.0, 0.001);
58          final DSSTPropagatorBuilder[] builders = { propagatorBuilder };
59  
60          // create perfect PV measurements
61          final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit,
62                                                                                 propagatorBuilder);
63  
64          final List<ObservedMeasurement<?>> measurements =
65                          DSSTEstimationTestUtils.createMeasurements(propagator,
66                                                                 new PVMeasurementCreator(),
67                                                                 0.0, 1.0, 300.0);
68          final ParameterDriversList estimatedMeasurementsParameters = new ParameterDriversList();
69          for (ObservedMeasurement<?> measurement : measurements) {
70              for (final ParameterDriver driver : measurement.getParametersDrivers()) {
71                  if (driver.isSelected()) {
72                      estimatedMeasurementsParameters.add(driver);
73                  }
74              }
75          }
76  
77          // create model
78          final ModelObserver modelObserver = new ModelObserver() {
79              /** {@inheritDoc} */
80              @Override
81              public void modelCalled(final Orbit[] newOrbits,
82                                      final Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> newEvaluations) {
83                  Assert.assertEquals(1, newOrbits.length);
84                  Assert.assertEquals(0,
85                                      context.initialOrbit.getDate().durationFrom(newOrbits[0].getDate()),
86                                      1.0e-15);
87                  Assert.assertEquals(0,
88                                      Vector3D.distance(context.initialOrbit.getPVCoordinates().getPosition(),
89                                                        newOrbits[0].getPVCoordinates().getPosition()),
90                                      1.0e-15);
91                  Assert.assertEquals(measurements.size(), newEvaluations.size());
92              }
93          };
94          final DSSTBatchLSModel model = new DSSTBatchLSModel(builders, measurements, estimatedMeasurementsParameters, modelObserver, PropagationType.MEAN, PropagationType.MEAN);
95          model.setIterationsCounter(new Incrementor(100));
96          model.setEvaluationsCounter(new Incrementor(100));
97          
98          // Test forward propagation flag to true
99          assertEquals(true, model.isForwardPropagation());
100 
101         // evaluate model on perfect start point
102         final double[] normalizedProp = propagatorBuilder.getSelectedNormalizedParameters();
103         final double[] normalized = new double[normalizedProp.length + estimatedMeasurementsParameters.getNbParams()];
104         System.arraycopy(normalizedProp, 0, normalized, 0, normalizedProp.length);
105         int i = normalizedProp.length;
106         for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
107             normalized[i++] = driver.getNormalizedValue();
108         }
109         Pair<RealVector, RealMatrix> value = model.value(new ArrayRealVector(normalized));
110         int index = 0;
111         for (ObservedMeasurement<?> measurement : measurements) {
112             for (int k = 0; k < measurement.getDimension(); ++k) {
113                 // the value is already a weighted residual
114                 Assert.assertEquals(0.0, value.getFirst().getEntry(index++), 6.3e-8);
115             }
116         }
117         Assert.assertEquals(index, value.getFirst().getDimension());
118 
119     }
120     
121     @Test
122     public void testBackwardPropagation() {
123 
124         final DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
125 
126         final DSSTPropagatorBuilder propagatorBuilder =
127                         context.createBuilder(true, 1.0e-6, 60.0, 0.001);
128         final DSSTPropagatorBuilder[] builders = { propagatorBuilder };
129 
130         // create perfect PV measurements
131         final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit,
132                                                                            propagatorBuilder);
133         final List<ObservedMeasurement<?>> measurements =
134                         DSSTEstimationTestUtils.createMeasurements(propagator,
135                                                                new PVMeasurementCreator(),
136                                                                0.0, -1.0, 300.0);
137         final ParameterDriversList estimatedMeasurementsParameters = new ParameterDriversList();
138         for (ObservedMeasurement<?> measurement : measurements) {
139             for (final ParameterDriver driver : measurement.getParametersDrivers()) {
140                 if (driver.isSelected()) {
141                     estimatedMeasurementsParameters.add(driver);
142                 }
143             }
144         }
145 
146         // create model
147         final ModelObserver modelObserver = new ModelObserver() {
148             /** {@inheritDoc} */
149             @Override
150             public void modelCalled(final Orbit[] newOrbits,
151                                     final Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> newEvaluations) {
152                 // Do nothing here 
153             }
154         };
155         final DSSTBatchLSModel model = new DSSTBatchLSModel(builders, measurements, estimatedMeasurementsParameters, modelObserver, PropagationType.MEAN, PropagationType.MEAN);
156         // Test forward propagation flag to false
157         assertEquals(false, model.isForwardPropagation());
158     }
159 
160     @Test
161     public void testIssue718() {
162 
163         // Context
164         final DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
165 
166         // Force models
167         final DSSTForce zonal = DSSTForce.ZONAL;
168         final List<DSSTForceModel> forces = new ArrayList<>();
169         forces.add(zonal.getForceModel(context));
170 
171         // Create propagator builders
172         final DSSTPropagatorBuilder propagatorBuilderMean =
173                         context.createBuilder(true, 0.01, 600.0, 1.0);
174         final DSSTPropagatorBuilder propagatorBuilderOsc  =
175                         context.createBuilder(PropagationType.OSCULATING, PropagationType.OSCULATING, true, 0.01, 600.0, 1.0, zonal);
176 
177         // Propagators
178         final Propagator propagatorMean = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilderMean);
179         final Propagator propagatorOsc  = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilderOsc);
180 
181         // Measurements
182         final List<ObservedMeasurement<?>> measurements = DSSTEstimationTestUtils.createMeasurements(propagatorMean, new PVMeasurementCreator(),
183                                                                                                      0.0, 1.0, 300.0);
184         // Empty list of measurement parameters
185         final ParameterDriversList estimatedMeasurementsParameters = new ParameterDriversList();
186 
187         // Verify MEAN case
188         final ModelObserver observerMean = new ModelObserver() {
189             /** {@inheritDoc} */
190             @Override
191             public void modelCalled(final Orbit[] newOrbits,
192                                     final Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> newEvaluations) {
193                 // Verify length
194                 Assert.assertEquals(1, newOrbits.length);
195                 // Verify first orbit
196                 Assert.assertEquals(0, context.initialOrbit.getDate().durationFrom(newOrbits[0].getDate()), 1.0e-15);
197                 Assert.assertEquals(0, Vector3D.distance(context.initialOrbit.getPVCoordinates().getPosition(),
198                                                          newOrbits[0].getPVCoordinates().getPosition()), 1.0e-15);
199 
200             }
201         };
202         
203         final DSSTBatchLSModel modelMean = propagatorBuilderMean.buildLSModel(new DSSTPropagatorBuilder[] {propagatorBuilderMean}, measurements, estimatedMeasurementsParameters, observerMean);
204         modelMean.setIterationsCounter(new Incrementor(100));
205         modelMean.setEvaluationsCounter(new Incrementor(100));
206 
207         // Evaluate model (MEAN)
208         modelMean.value(new ArrayRealVector(propagatorBuilderMean.getSelectedNormalizedParameters()));
209 
210         // Verify OSCULATING case
211         final ModelObserver observerOsc = new ModelObserver() {
212             /** {@inheritDoc} */
213             @Override
214             public void modelCalled(final Orbit[] newOrbits,
215                                     final Map<ObservedMeasurement<?>, EstimatedMeasurement<?>> newEvaluations) {
216                 // Compute mean state from osculating propagator
217                 final SpacecraftState meanState = DSSTPropagator.computeMeanState(propagatorOsc.getInitialState(), propagatorOsc.getAttitudeProvider(), forces);
218                 // Verify length
219                 Assert.assertEquals(1, newOrbits.length);
220                 // Verify first orbit
221                 Assert.assertEquals(0, context.initialOrbit.getDate().durationFrom(newOrbits[0].getDate()), 1.0e-15);
222                 Assert.assertEquals(0, Vector3D.distance(meanState.getPVCoordinates().getPosition(),
223                                                          newOrbits[0].getPVCoordinates().getPosition()), 1.0e-15);
224 
225             }
226         };
227 
228         final DSSTBatchLSModel modelOsc = propagatorBuilderOsc.buildLSModel(new DSSTPropagatorBuilder[] {propagatorBuilderOsc}, measurements, estimatedMeasurementsParameters, observerOsc);
229         modelOsc.setIterationsCounter(new Incrementor(100));
230         modelOsc.setEvaluationsCounter(new Incrementor(100));
231 
232         // Evaluate model (OSCULATING)
233         modelOsc.value(new ArrayRealVector(propagatorBuilderOsc.getSelectedNormalizedParameters()));
234 
235     }
236 
237 }