1   package org.orekit.rugged.adjustment.util;
2   
3   import java.io.File;
4   import java.net.URISyntaxException;
5   import java.util.List;
6   
7   import org.hipparchus.geometry.euclidean.threed.Vector3D;
8   import org.hipparchus.random.GaussianRandomGenerator;
9   import org.hipparchus.random.UncorrelatedRandomVectorGenerator;
10  import org.hipparchus.random.Well19937a;
11  import org.hipparchus.util.FastMath;
12  import org.junit.jupiter.api.Assertions;
13  import org.orekit.bodies.BodyShape;
14  import org.orekit.bodies.GeodeticPoint;
15  import org.orekit.data.DataContext;
16  import org.orekit.data.DirectoryCrawler;
17  import org.orekit.errors.OrekitException;
18  import org.orekit.orbits.Orbit;
19  import org.orekit.rugged.TestUtils;
20  import org.orekit.rugged.adjustment.measurements.Observables;
21  import org.orekit.rugged.adjustment.measurements.SensorToGroundMapping;
22  import org.orekit.rugged.api.AlgorithmId;
23  import org.orekit.rugged.api.BodyRotatingFrameId;
24  import org.orekit.rugged.api.EllipsoidId;
25  import org.orekit.rugged.api.InertialFrameId;
26  import org.orekit.rugged.api.Rugged;
27  import org.orekit.rugged.api.RuggedBuilder;
28  import org.orekit.rugged.linesensor.LineSensor;
29  import org.orekit.rugged.linesensor.SensorPixel;
30  import org.orekit.time.AbsoluteDate;
31  import org.orekit.utils.AngularDerivativesFilter;
32  import org.orekit.utils.CartesianDerivativesFilter;
33  import org.orekit.utils.Constants;
34  import org.orekit.utils.TimeStampedAngularCoordinates;
35  import org.orekit.utils.TimeStampedPVCoordinates;
36  
37  
38  /** Initialization for ground refining (Ground Control Points GCP study) Junit tests.
39   * @author Guylaine Prat
40   */
41  public class InitGroundRefiningTest {
42  
43      /** Pleiades viewing model */
44      private PleiadesViewingModel pleiadesViewingModel;
45  
46      /** Line sensor */
47      private LineSensor lineSensor;
48  
49      /** Rugged's instance */
50      private Rugged rugged;
51  
52      /** Number of parameters to adjust */
53      private int parameterToAdjust;
54      
55      // Part of the name of parameter drivers
56      static final String rollSuffix = "_roll";
57      static final String pitchSuffix = "_pitch";
58      static final String factorName = "factor";
59  
60      // Default values for disruption to apply to roll (deg), pitch (deg) and factor 
61      static final double defaultRollDisruption =  0.004;
62      static final double defaultPitchDisruption = 0.0008;
63      static final double defaultFactorDisruption = 1.000000001;
64      
65      /**
66       * Initialize ground refining tests with default values for disruptions on sensors characteristics
67       */
68      public void initGroundRefiningTest() {
69          
70          initGroundRefiningTest(defaultRollDisruption, defaultPitchDisruption, defaultFactorDisruption);
71      }
72  
73      /** Initialize ground refining tests with disruption on sensors characteristics
74       * @param rollDisruption disruption to apply to roll angle for sensor (deg)
75       * @param pitchDisruption disruption to apply to pitch angle for sensor (deg)
76       * @param factorDisruption disruption to apply to homothety factor for sensor
77       */
78      public void initGroundRefiningTest(double rollDisruption, double pitchDisruption, double factorDisruption) {
79          try {
80              
81              String path = getClass().getClassLoader().getResource("orekit-data").toURI().getPath();
82              DataContext.getDefault().getDataProvidersManager().addProvider(new DirectoryCrawler(new File(path)));
83              
84              // Initialize refining context
85              // ---------------------------
86              final String sensorName = "line";
87              final double rollAngle = -5.0;
88              final String date = "2016-01-01T11:59:50.0";
89              this.pleiadesViewingModel = new PleiadesViewingModel(sensorName, rollAngle, date);
90  
91  
92              PleiadesOrbitModel orbitmodel =  new PleiadesOrbitModel();
93  
94              // Sensor's definition: creation of Pleiades viewing model
95              // --------------------------------------------------------
96              
97              // Create Pleiades Viewing Model
98              final AbsoluteDate minDate =  pleiadesViewingModel.getMinDate();
99              final AbsoluteDate maxDate =  pleiadesViewingModel.getMaxDate();
100             final AbsoluteDate refDate = pleiadesViewingModel.getDatationReference();
101             this.lineSensor =  pleiadesViewingModel.getLineSensor();
102 
103             // ----Satellite position, velocity and attitude: create orbit model
104             BodyShape earth = TestUtils.createEarth();
105             Orbit orbit  = orbitmodel.createOrbit(Constants.EIGEN5C_EARTH_MU, refDate);
106 
107             // ----If no LOF Transform Attitude Provider is Nadir Pointing Yaw Compensation
108             final double [] rollPoly = {0.0,0.0,0.0};
109             final double[] pitchPoly = {0.025,0.0};
110             final double[] yawPoly = {0.0,0.0,0.0};
111             orbitmodel.setLOFTransform(rollPoly, pitchPoly, yawPoly, minDate);
112 
113             // ----Satellite attitude
114             List<TimeStampedAngularCoordinates> satelliteQList = orbitmodel.orbitToQ(orbit, earth, minDate.shiftedBy(-0.0), maxDate.shiftedBy(+0.0), 0.25);
115             final int nbQPoints = 2;
116 
117             // ----Position and velocities
118             List<TimeStampedPVCoordinates> satellitePVListA = orbitmodel.orbitToPV(orbit, earth, minDate.shiftedBy(-0.0), maxDate.shiftedBy(+0.0), 0.25);
119             final int nbPVPoints = 8;
120 
121             // Rugged initialization
122             // ---------------------
123             RuggedBuilder ruggedBuilder = new RuggedBuilder();
124 
125             ruggedBuilder.addLineSensor(lineSensor);
126             ruggedBuilder.setAlgorithm(AlgorithmId.IGNORE_DEM_USE_ELLIPSOID);
127             ruggedBuilder.setEllipsoid(EllipsoidId.WGS84, BodyRotatingFrameId.ITRF);
128             ruggedBuilder.setTimeSpan(minDate,maxDate, 0.001, 5.0);
129             ruggedBuilder.setTrajectory(InertialFrameId.EME2000, satellitePVListA, nbPVPoints,
130                                          CartesianDerivativesFilter.USE_PV, satelliteQList,
131                                          nbQPoints, AngularDerivativesFilter.USE_R);
132             ruggedBuilder.setLightTimeCorrection(false);
133             ruggedBuilder.setAberrationOfLightCorrection(false);
134 
135             ruggedBuilder.setName("Rugged");
136 
137             this.rugged = ruggedBuilder.build();
138 
139             
140             // Select parameters to adjust
141             // ---------------------------
142             RefiningParametersDriver.setSelectedRoll(rugged, sensorName);
143             RefiningParametersDriver.setSelectedPitch(rugged, sensorName);
144 
145             this.parameterToAdjust = 2;
146 
147             // Initialize disruptions:
148             // -----------------------
149             // introduce rotations around instrument axes (roll and pitch angles, scale factor)
150 
151             // apply disruptions on physical model
152             RefiningParametersDriver.applyDisruptionsRoll(rugged, sensorName, FastMath.toRadians(rollDisruption));
153             RefiningParametersDriver.applyDisruptionsPitch(rugged, sensorName, FastMath.toRadians(pitchDisruption));
154             RefiningParametersDriver.applyDisruptionsFactor(rugged, sensorName, factorDisruption);
155             
156             
157         } catch (OrekitException oe) {
158             Assertions.fail(oe.getLocalizedMessage());
159         } catch (URISyntaxException use) {
160             Assertions.fail(use.getLocalizedMessage());
161         }
162     }
163     
164     /**
165      * Get the Rugged instance
166      * @return rugged instance
167      */
168     public Rugged getRugged(){
169         
170         return this.rugged;
171     }
172        
173     /** Generate noisy measurements (sensor to ground mapping)
174      * @param lineSampling line sampling
175      * @param pixelSampling pixel sampling
176      * @param flag to tell if the Rugged name used is the default one (true) or not (false)
177      */
178     public Observables generateNoisyPoints(final int lineSampling, final int pixelSampling, boolean defaultRuggedName) {
179 
180         // Generate reference mapping
181         SensorToGroundMapping groundMapping;
182         if (!defaultRuggedName) {
183             groundMapping = new SensorToGroundMapping(rugged.getName(), lineSensor.getName());
184         } else {
185             // The rugged name used in this test is the same as the dafult one in SensorToGroundMapping
186             groundMapping = new SensorToGroundMapping(lineSensor.getName());
187         }
188         
189         // Observable which contains sensor to ground mapping (one model)
190         Observables observable = new Observables(1);
191         
192         // Estimate latitude and longitude errors (rad)
193         final int pixelMiddle= lineSensor.getNbPixels() / 2;
194         final int lineMiddle = (int) FastMath.floor(pixelMiddle); 
195         
196         final AbsoluteDate dateMiddle = lineSensor.getDate(lineMiddle);
197         final GeodeticPoint gp_pix0 = rugged.directLocation(dateMiddle, lineSensor.getPosition(), lineSensor.getLOS(dateMiddle, pixelMiddle));
198 
199         final AbsoluteDate date1 = lineSensor.getDate(lineMiddle + 1);
200         final GeodeticPoint gp_pix1 = rugged.directLocation(date1, lineSensor.getPosition(), lineSensor.getLOS(date1, pixelMiddle + 1));
201 
202         final double latitudeErr = FastMath.abs(gp_pix0.getLatitude() - gp_pix1.getLatitude());
203         final double longitudeErr = FastMath.abs(gp_pix0.getLongitude() - gp_pix1.getLongitude());
204 
205         // Generation noisy measurements
206         // distribution: gaussian(0), vector dimension: 3 (for latitude, longitude and altitude)
207         // Mean latitude, longitude and altitude
208         final double[] mean = {5.0,5.0,5.0};
209         // Standard deviation latitude, longitude and altitude
210         final double[] std = {0.1,0.1,0.1};
211 
212         final double latErrorMean = mean[0] * latitudeErr;
213         final double lonErrorMean = mean[1] * longitudeErr;
214         final double latErrorStd = std[0] * latitudeErr;
215         final double lonErrorStd = std[1] * longitudeErr;
216 
217         // Gaussian random generator
218         // Build a null mean random uncorrelated vector generator with standard deviation corresponding to the estimated error on ground
219         final double meanGenerator[] =  {latErrorMean, lonErrorMean, mean[2]};
220         final double stdGenerator[] = {latErrorStd, lonErrorStd, std[2]};
221 
222         // seed has been fixed for tests purpose
223         final GaussianRandomGenerator rng = new GaussianRandomGenerator(new Well19937a(0xefac03d9be4d24b9l));
224         final UncorrelatedRandomVectorGenerator rvg = new UncorrelatedRandomVectorGenerator(meanGenerator, stdGenerator, rng);
225 
226         for (double line = 0; line < pleiadesViewingModel.getDimension(); line += lineSampling) {
227             
228             final AbsoluteDate date = lineSensor.getDate(line);
229             for (int pixel = 0; pixel < lineSensor.getNbPixels(); pixel += pixelSampling) {
230 
231                 // Components of generated vector follow (independent) Gaussian distribution
232                 final Vector3D vecRandom = new Vector3D(rvg.nextVector());
233 
234                 final GeodeticPoint gp2 = rugged.directLocation(date, lineSensor.getPosition(),
235                                                                 lineSensor.getLOS(date, pixel));
236 
237                 final GeodeticPoint gpNoisy = new GeodeticPoint(gp2.getLatitude() + vecRandom.getX(),
238                                                                 gp2.getLongitude() + vecRandom.getY(),
239                                                                 gp2.getAltitude() + vecRandom.getZ());
240 
241                 groundMapping.addMapping(new SensorPixel(line, pixel), gpNoisy);
242             }
243         }
244 
245         observable.addGroundMapping(groundMapping);
246         return observable;
247     }
248     
249     /** Get the number of parameters to adjust
250      * @return number of parameters to adjust
251      */
252     public int getParameterToAdjust() {
253         return parameterToAdjust;
254     }
255 
256 }