1   /* Copyright 2002-2024 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.measurements;
18  
19  import org.hipparchus.analysis.UnivariateFunction;
20  import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
21  import org.hipparchus.analysis.solvers.UnivariateSolver;
22  import org.hipparchus.geometry.euclidean.threed.Vector3D;
23  import org.hipparchus.util.FastMath;
24  import org.orekit.estimation.StationDataProvider;
25  import org.orekit.frames.Frame;
26  import org.orekit.frames.Transform;
27  import org.orekit.propagation.SpacecraftState;
28  import org.orekit.time.AbsoluteDate;
29  import org.orekit.utils.Constants;
30  import org.orekit.utils.ParameterDriver;
31  import org.orekit.utils.TrackingCoordinates;
32  
33  import java.util.Arrays;
34  
35  public class AngularAzElMeasurementCreator extends MeasurementCreator {
36  
37      private final StationDataProvider context;
38      private final ObservableSatellite satellite;
39      private final double azBias;
40      private final double elBias;
41  
42      public AngularAzElMeasurementCreator(final StationDataProvider context) {
43          this(context, 0., 0.);
44      }
45  
46      public AngularAzElMeasurementCreator(final StationDataProvider context, final double azBias, final double elBias) {
47          this.context   = context;
48          this.satellite = new ObservableSatellite(0);
49          this.azBias    = azBias;
50          this.elBias    = elBias;
51      }
52  
53      public void init(SpacecraftState s0, AbsoluteDate t, double step) {
54          for (final GroundStation station : context.getStations()) {
55              for (ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
56                                                          station.getEastOffsetDriver(),
57                                                          station.getNorthOffsetDriver(),
58                                                          station.getZenithOffsetDriver(),
59                                                          station.getPrimeMeridianOffsetDriver(),
60                                                          station.getPrimeMeridianDriftDriver(),
61                                                          station.getPolarOffsetXDriver(),
62                                                          station.getPolarDriftXDriver(),
63                                                          station.getPolarOffsetYDriver(),
64                                                          station.getPolarDriftYDriver())) {
65                  if (driver.getReferenceDate() == null) {
66                      driver.setReferenceDate(s0.getDate());
67                  }
68              }
69  
70          }
71      }
72  
73      public void handleStep(final SpacecraftState currentState) {
74          for (final GroundStation station : context.getStations()) {
75  
76              final AbsoluteDate     date      = currentState.getDate();
77              final Frame            inertial  = currentState.getFrame();
78              final Vector3D         position  = currentState.getPosition();
79  
80              if (station.getBaseFrame().getTrackingCoordinates(position, inertial, date).getElevation() > FastMath.toRadians(30.0)) {
81                  final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
82  
83                  final double downLinkDelay  = solver.solve(1000, new UnivariateFunction() {
84                      public double value(final double x) {
85                          final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(x), false);
86                          final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
87                          return d - x * Constants.SPEED_OF_LIGHT;
88                      }
89                  }, -1.0, 1.0);
90  
91                  // Satellite position at signal departure
92                  final Vector3D satelliteAtDeparture = currentState.shiftedBy(-downLinkDelay).getPosition();
93  
94                  // Initialize measurement
95                  final double[] angular = new double[2];
96                  final double[] sigma = {1.0, 1.0};
97                  final double[] baseweight = {10.0, 10.0};
98  
99                  // Compute measurement
100                 final TrackingCoordinates tc =
101                                 station.getBaseFrame().getTrackingCoordinates(satelliteAtDeparture,
102                                                                               currentState.getFrame(),
103                                                                               currentState.getDate());
104                 angular[0] = tc.getAzimuth()   + azBias;
105                 angular[1] = tc.getElevation() + elBias;
106 
107                 addMeasurement(new AngularAzEl(station, date, angular, sigma, baseweight, satellite));
108             }
109 
110         }
111     }
112 
113 }