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