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.sequential;
18  
19  import java.util.ArrayList;
20  import java.util.List;
21  
22  import org.hipparchus.geometry.euclidean.threed.Vector3D;
23  import org.hipparchus.linear.MatrixUtils;
24  import org.hipparchus.linear.RealMatrix;
25  import org.junit.jupiter.api.Assertions;
26  import org.junit.jupiter.api.Test;
27  import org.orekit.attitudes.LofOffset;
28  import org.orekit.errors.OrekitException;
29  import org.orekit.errors.OrekitMessages;
30  import org.orekit.estimation.EstimationTestUtils;
31  import org.orekit.estimation.TLEContext;
32  import org.orekit.estimation.TLEEstimationTestUtils;
33  import org.orekit.estimation.measurements.ObservedMeasurement;
34  import org.orekit.estimation.measurements.PVMeasurementCreator;
35  import org.orekit.estimation.measurements.Range;
36  import org.orekit.estimation.measurements.RangeRateMeasurementCreator;
37  import org.orekit.estimation.measurements.TwoWayRangeMeasurementCreator;
38  import org.orekit.estimation.measurements.modifiers.PhaseCentersRangeModifier;
39  import org.orekit.frames.LOFType;
40  import org.orekit.gnss.antenna.FrequencyPattern;
41  import org.orekit.orbits.Orbit;
42  import org.orekit.orbits.PositionAngleType;
43  import org.orekit.propagation.Propagator;
44  import org.orekit.propagation.analytical.tle.TLEPropagator;
45  import org.orekit.propagation.conversion.TLEPropagatorBuilder;
46  import org.orekit.time.AbsoluteDate;
47  import org.orekit.utils.ParameterDriver;
48  import org.orekit.utils.ParameterDriversList;
49  
50  public class TLEKalmanEstimatorTest {
51  
52      @Test
53      public void testMissingPropagatorBuilder() {
54          try {
55              new KalmanEstimatorBuilder().
56              build();
57              Assertions.fail("an exception should have been thrown");
58          } catch (OrekitException oe) {
59              Assertions.assertEquals(OrekitMessages.NO_PROPAGATOR_CONFIGURED, oe.getSpecifier());
60          }
61      }
62  
63      /**
64       * Perfect PV measurements with a perfect start
65       * Keplerian formalism
66       */
67      @Test
68      public void testPV() {
69  
70          // Create context
71          TLEContext context = TLEEstimationTestUtils.eccentricContext("regular-data:potential:tides");
72  
73          // Create initial orbit and propagator builder
74          final PositionAngleType positionAngleType = PositionAngleType.MEAN;
75          final double        dP            = 1.;
76          final TLEPropagatorBuilder propagatorBuilder = context.createBuilder(dP);
77  
78          // Create perfect PV measurements
79          final Orbit initialOrbit = TLEPropagator.selectExtrapolator(context.initialTLE).getInitialState().getOrbit();
80          final Propagator propagator = TLEEstimationTestUtils.createPropagator(initialOrbit,
81                                                                             propagatorBuilder);
82          final List<ObservedMeasurement<?>> measurements =
83                          TLEEstimationTestUtils.createMeasurements(propagator,
84                                                                 new PVMeasurementCreator(),
85                                                                 0.0, 3.0, 300.0);
86          // Reference propagator for estimation performances
87          final Propagator referencePropagator = propagatorBuilder.buildPropagator();
88  
89          // Reference position/velocity at last measurement date
90          final Orbit refOrbit = referencePropagator.
91                          propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
92  
93          // Covariance matrix initialization
94          final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
95              1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5
96          });
97  
98          // Process noise matrix
99          RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double [] {
100             1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8
101         });
102 
103 
104         // Build the Kalman filter
105         final KalmanEstimator kalman = new KalmanEstimatorBuilder().
106                         addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
107                         estimatedMeasurementsParameters(new ParameterDriversList(), null).
108                         build();
109 
110         // Filter the measurements and check the results
111         final double   expectedDeltaPos  = 0.;
112         final double   posEps            = 9.61e-2; // With numerical propagator: 5.80e-8;
113         final double   expectedDeltaVel  = 0.;
114         final double   velEps            = 7.86e-5; // With numerical propagator: 2.28e-11;
115         TLEEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
116                                            refOrbit, positionAngleType,
117                                            expectedDeltaPos, posEps,
118                                            expectedDeltaVel, velEps);
119     }
120 
121     /**
122      * Perfect range measurements with a biased start
123      * Keplerian formalism
124      */
125     @Test
126     public void testRange() {
127 
128         // Create context
129         TLEContext context = TLEEstimationTestUtils.eccentricContext("regular-data:potential:tides");
130 
131         // Create initial orbit and propagator builder
132         final PositionAngleType positionAngleType = PositionAngleType.MEAN;
133         final double        dP            = 1.;
134         final TLEPropagatorBuilder propagatorBuilder =
135                         context.createBuilder(dP);
136 
137         // Create perfect range measurements
138         Orbit initialOrbit = TLEPropagator.selectExtrapolator(context.initialTLE).getInitialState().getOrbit();
139         final Propagator propagator = TLEEstimationTestUtils.createPropagator(initialOrbit,
140                                                                            propagatorBuilder);
141         final List<ObservedMeasurement<?>> measurements =
142                         TLEEstimationTestUtils.createMeasurements(propagator,
143                                                                new TwoWayRangeMeasurementCreator(context),
144                                                                1.0, 4.0, 60.0);
145 
146         // Reference propagator for estimation performances
147         final Propagator referencePropagator = propagatorBuilder.buildPropagator();
148 
149         // Reference position/velocity at last measurement date
150         final Orbit refOrbit = referencePropagator.
151                         propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
152 
153         // Change X position of 10m as in the batch test
154         ParameterDriver xDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
155         xDriver.setValue(xDriver.getValue() + 10.0);
156         xDriver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
157 
158         // Cartesian covariance matrix initialization
159         // 100m on position / 1e-2m/s on velocity
160         final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
161             100., 100., 100., 1e-2, 1e-2, 1e-2
162         });
163 
164         // Process noise matrix is set to 0 here
165         RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
166 
167         // Build the Kalman filter
168         final KalmanEstimator kalman = new KalmanEstimatorBuilder().
169                         addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(cartesianP, Q)).
170                         estimatedMeasurementsParameters(new ParameterDriversList(), null).
171                         build();
172 
173         // Filter the measurements and check the results
174         final double   expectedDeltaPos  = 0.;
175         final double   posEps            = 0.32; // With numerical propagator: 1.77e-4;
176         final double   expectedDeltaVel  = 0.;
177         final double   velEps            = 7.45e-5; // With numerical propagator: 7.93e-8;
178         TLEEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
179                                            refOrbit, positionAngleType,
180                                            expectedDeltaPos, posEps,
181                                            expectedDeltaVel, velEps);
182     }
183 
184     /**
185      * Perfect range measurements with a biased start and an on-board antenna range offset
186      * Keplerian formalism
187      */
188     @Test
189     public void testRangeWithOnBoardAntennaOffset() {
190 
191         // Create context
192         TLEContext context = TLEEstimationTestUtils.eccentricContext("regular-data:potential:tides");
193 
194         // Create initial orbit and propagator builder
195         final PositionAngleType positionAngleType = PositionAngleType.MEAN;
196         final double        dP            = 1.;
197         final TLEPropagatorBuilder propagatorBuilder =
198                         context.createBuilder(dP);
199         propagatorBuilder.setAttitudeProvider(new LofOffset(propagatorBuilder.getFrame(), LOFType.LVLH));
200 
201         // Antenna phase center definition
202         final Vector3D antennaPhaseCenter = new Vector3D(-1.2, 2.3, -0.7);
203 
204         // Create perfect range measurements with antenna offset
205         Orbit initialOrbit = TLEPropagator.selectExtrapolator(context.initialTLE).getInitialState().getOrbit();
206         final Propagator propagator = EstimationTestUtils.createPropagator(initialOrbit,
207                                                                            propagatorBuilder);
208         final List<ObservedMeasurement<?>> measurements =
209                         TLEEstimationTestUtils.createMeasurements(propagator,
210                                                                new TwoWayRangeMeasurementCreator(context,
211                                                                                                  Vector3D.ZERO, null,
212                                                                                                  antennaPhaseCenter, null,
213                                                                                                  0),
214                                                                1.0, 3.0, 300.0);
215 
216         // Add antenna offset to the measurements
217         final PhaseCentersRangeModifier obaModifier = new PhaseCentersRangeModifier(FrequencyPattern.ZERO_CORRECTION,
218                                                                                     new FrequencyPattern(antennaPhaseCenter,
219                                                                                                          null));
220         for (final ObservedMeasurement<?> range : measurements) {
221             ((Range) range).addModifier(obaModifier);
222         }
223 
224         // Reference propagator for estimation performances
225         final Propagator referencePropagator = propagatorBuilder.buildPropagator();
226 
227         // Reference position/velocity at last measurement date
228         final Orbit refOrbit = referencePropagator.
229                         propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
230 
231         // Change X position of 10m as in the batch test
232         ParameterDriver xDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
233         xDriver.setValue(xDriver.getValue() + 10.0);
234         xDriver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
235 
236         // Cartesian covariance matrix initialization
237         // 100m on position / 1e-2m/s on velocity
238         final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
239             100., 100., 100., 1e-2, 1e-2, 1e-2
240         });
241 
242         // Process noise matrix is set to 0 here
243         RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
244 
245         // Build the Kalman filter
246         final KalmanEstimator kalman = new KalmanEstimatorBuilder().
247                         addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(cartesianP, Q)).
248                         estimatedMeasurementsParameters(new ParameterDriversList(), null).
249                         build();
250 
251         // Filter the measurements and check the results
252         final double   expectedDeltaPos  = 0.;
253         final double   posEps            = 0.69; // With numerical propagator: 4.57e-3;
254         final double   expectedDeltaVel  = 0.;
255         final double   velEps            = 2.69e-4; // With numerical propagator: 7.29e-6;
256         TLEEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
257                                            refOrbit, positionAngleType,
258                                            expectedDeltaPos, posEps,
259                                            expectedDeltaVel, velEps);
260     }
261 
262     /**
263      * Perfect range and range rate measurements with a perfect start
264      */
265     @Test
266     public void testRangeAndRangeRate() {
267 
268         // Create context
269         TLEContext context = TLEEstimationTestUtils.eccentricContext("regular-data:potential:tides");
270 
271         // Create initial orbit and propagator builder
272         final PositionAngleType positionAngleType = PositionAngleType.MEAN;
273         final double        dP            = 1.;
274         final TLEPropagatorBuilder propagatorBuilder =
275                         context.createBuilder(dP);
276 
277         // Create perfect range & range rate measurements
278         Orbit initialOrbit = TLEPropagator.selectExtrapolator(context.initialTLE).getInitialState().getOrbit();
279         final Propagator propagator = TLEEstimationTestUtils.createPropagator(initialOrbit,
280                                                                            propagatorBuilder);
281         final List<ObservedMeasurement<?>> measurementsRange =
282                         TLEEstimationTestUtils.createMeasurements(propagator,
283                                                                new TwoWayRangeMeasurementCreator(context),
284                                                                1.0, 3.0, 300.0);
285 
286         final List<ObservedMeasurement<?>> measurementsRangeRate =
287                         TLEEstimationTestUtils.createMeasurements(propagator,
288                                                                new RangeRateMeasurementCreator(context, false, 0.0),
289                                                                1.0, 3.0, 300.0);
290 
291         // Concatenate measurements
292         final List<ObservedMeasurement<?>> measurements = new ArrayList<>();
293         measurements.addAll(measurementsRange);
294         measurements.addAll(measurementsRangeRate);
295 
296         // Reference propagator for estimation performances
297         final Propagator referencePropagator = propagatorBuilder.buildPropagator();
298 
299         // Reference position/velocity at last measurement date
300         final Orbit refOrbit = referencePropagator.
301                         propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
302 
303         // Change X position of 10m as in the batch test
304         ParameterDriver xDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
305         xDriver.setValue(xDriver.getValue() + 10.0);
306         xDriver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
307 
308         // Cartesian covariance matrix initialization
309         // 100m on position / 1e-2m/s on velocity
310         final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
311             100., 100., 100., 1e-2, 1e-2, 1e-2
312         });
313 
314         // Process noise matrix is set to 0 here
315         RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
316 
317         // Build the Kalman filter
318         final KalmanEstimator kalman = new KalmanEstimatorBuilder().
319                         addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(cartesianP, Q)).
320                         build();
321 
322         // Filter the measurements and check the results
323         final double   expectedDeltaPos  = 0.;
324         final double   posEps            = 0.45; // With numerical propagator: 1.2e-6;
325         final double   expectedDeltaVel  = 0.;
326         final double   velEps            = 1.86e-4; // With numerical propagator: 4.2e-10;
327         TLEEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
328                                            refOrbit, positionAngleType,
329                                            expectedDeltaPos, posEps,
330                                            expectedDeltaVel, velEps);
331     }
332 
333     /**
334      * Test of a wrapped exception in a Kalman observer
335      */
336     @Test
337     public void testWrappedException() {
338 
339         // Create context
340         TLEContext context = TLEEstimationTestUtils.eccentricContext("regular-data:potential:tides");
341 
342         // Create initial orbit and propagator builder
343         final PositionAngleType positionAngleType = PositionAngleType.TRUE;
344         final double        dP            = 1.;
345         final TLEPropagatorBuilder propagatorBuilder =
346                         context.createBuilder(dP);
347 
348         // Create perfect range measurements
349         Orbit initialOrbit = TLEPropagator.selectExtrapolator(context.initialTLE).getInitialState().getOrbit();
350         final Propagator propagator = TLEEstimationTestUtils.createPropagator(initialOrbit,
351                                                                            propagatorBuilder);
352         final List<ObservedMeasurement<?>> measurements =
353                         TLEEstimationTestUtils.createMeasurements(propagator,
354                                                                new TwoWayRangeMeasurementCreator(context),
355                                                                1.0, 3.0, 300.0);
356         // Build the Kalman filter
357         final KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
358         kalmanBuilder.addPropagationConfiguration(propagatorBuilder,
359                                                   new ConstantProcessNoise(MatrixUtils.createRealMatrix(6, 6)));
360         kalmanBuilder.estimatedMeasurementsParameters(new ParameterDriversList(), null);
361         final KalmanEstimator kalman = kalmanBuilder.build();
362         kalman.setObserver(estimation -> {
363                 throw new DummyException();
364             });
365 
366 
367         try {
368             // Filter the measurements and expect an exception to occur
369             TLEEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
370                                                initialOrbit, positionAngleType,
371                                                0., 0.,
372                                                0., 0.);
373         } catch (DummyException de) {
374             // expected
375         }
376 
377     }
378 
379     private static class DummyException extends OrekitException {
380         private static final long serialVersionUID = 1L;
381         public DummyException() {
382             super(OrekitMessages.INTERNAL_ERROR);
383         }
384     }
385 
386 }