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