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.attitudes;
18  
19  
20  import java.util.ArrayList;
21  import java.util.List;
22  
23  import org.hipparchus.Field;
24  import org.hipparchus.CalculusFieldElement;
25  import org.hipparchus.geometry.euclidean.threed.Rotation;
26  import org.hipparchus.geometry.euclidean.threed.RotationConvention;
27  import org.hipparchus.geometry.euclidean.threed.Vector3D;
28  import org.hipparchus.util.Decimal64Field;
29  import org.hipparchus.util.FastMath;
30  import org.junit.After;
31  import org.junit.Assert;
32  import org.junit.Before;
33  import org.junit.Test;
34  import org.orekit.Utils;
35  import org.orekit.bodies.GeodeticPoint;
36  import org.orekit.bodies.OneAxisEllipsoid;
37  import org.orekit.errors.OrekitException;
38  import org.orekit.frames.Frame;
39  import org.orekit.frames.FramesFactory;
40  import org.orekit.orbits.CircularOrbit;
41  import org.orekit.orbits.FieldOrbit;
42  import org.orekit.orbits.KeplerianOrbit;
43  import org.orekit.orbits.Orbit;
44  import org.orekit.orbits.PositionAngle;
45  import org.orekit.propagation.FieldSpacecraftState;
46  import org.orekit.propagation.Propagator;
47  import org.orekit.propagation.SpacecraftState;
48  import org.orekit.propagation.analytical.KeplerianPropagator;
49  import org.orekit.time.AbsoluteDate;
50  import org.orekit.time.DateComponents;
51  import org.orekit.time.FieldAbsoluteDate;
52  import org.orekit.time.TimeComponents;
53  import org.orekit.time.TimeScalesFactory;
54  import org.orekit.utils.AngularCoordinates;
55  import org.orekit.utils.CartesianDerivativesFilter;
56  import org.orekit.utils.IERSConventions;
57  import org.orekit.utils.TimeStampedFieldPVCoordinates;
58  import org.orekit.utils.TimeStampedPVCoordinates;
59  
60  
61  public class NadirPointingTest {
62  
63      // Computation date
64      private AbsoluteDate date;
65  
66      // Body mu
67      private double mu;
68  
69      // Reference frame = ITRF
70      private Frame itrf;
71  
72      /** Test in the case of a spheric earth : nadir pointing shall be
73       * the same as earth center pointing
74       */
75      @Test
76      public void testSphericEarth() {
77  
78          // Spheric earth shape
79          OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 0., itrf);
80  
81          // Create nadir pointing attitude provider
82          NadirPointing nadirAttitudeLaw = new NadirPointing(FramesFactory.getEME2000(), earthShape);
83  
84          // Create earth center pointing attitude provider
85          BodyCenterPointing earthCenterAttitudeLaw =
86                  new BodyCenterPointing(FramesFactory.getEME2000(), earthShape);
87  
88          // Create satellite position as circular parameters
89          CircularOrbit circ =
90              new CircularOrbit(7178000.0, 0.5e-4, -0.5e-4, FastMath.toRadians(50.), FastMath.toRadians(270.),
91                                     FastMath.toRadians(5.300), PositionAngle.MEAN,
92                                     FramesFactory.getEME2000(), date, mu);
93  
94          // Get nadir attitude
95          Rotation rotNadir = nadirAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
96  
97          // Get earth center attitude
98          Rotation rotCenter = earthCenterAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
99  
100         // For a spheric earth, earth center pointing attitude and nadir pointing attitude
101         // shall be the same, i.e the composition of inverse earth pointing rotation
102         // with nadir pointing rotation shall be identity.
103         Rotation rotCompo = rotCenter.composeInverse(rotNadir, RotationConvention.VECTOR_OPERATOR);
104         double angle = rotCompo.getAngle();
105         Assert.assertEquals(angle, 0.0, Utils.epsilonAngle);
106 
107     }
108 
109     /** Test in the case of an elliptic earth : nadir pointing shall be :
110      *   - the same as earth center pointing in case of equatorial or polar position
111      *   - different from earth center pointing in any other case
112      */
113     @Test
114     public void testNonSphericEarth() {
115 
116         // Elliptic earth shape
117         OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, itrf);
118 
119         // Create nadir pointing attitude provider
120         NadirPointing nadirAttitudeLaw = new NadirPointing(FramesFactory.getEME2000(), earthShape);
121 
122         // Create earth center pointing attitude provider
123         BodyCenterPointing earthCenterAttitudeLaw =
124                 new BodyCenterPointing(FramesFactory.getEME2000(), earthShape);
125 
126         //  Satellite on equatorial position
127         // **********************************
128         KeplerianOrbit kep =
129             new KeplerianOrbit(7178000.0, 1.e-8, FastMath.toRadians(50.), 0., 0.,
130                                     0., PositionAngle.TRUE, FramesFactory.getEME2000(), date, mu);
131 
132         // Get nadir attitude
133         Rotation rotNadir = nadirAttitudeLaw.getAttitude(kep, date, kep.getFrame()).getRotation();
134 
135         checkField(Decimal64Field.getInstance(), nadirAttitudeLaw, kep, kep.getDate(), kep.getFrame());
136 
137         // Get earth center attitude
138         Rotation rotCenter = earthCenterAttitudeLaw.getAttitude(kep, date, kep.getFrame()).getRotation();
139 
140         // For a satellite at equatorial position, earth center pointing attitude and nadir pointing
141         // attitude shall be the same, i.e the composition of inverse earth pointing rotation
142         // with nadir pointing rotation shall be identity.
143         Rotation rotCompo = rotCenter.composeInverse(rotNadir, RotationConvention.VECTOR_OPERATOR);
144         double angle = rotCompo.getAngle();
145         Assert.assertEquals(0.0, angle, 5.e-6);
146 
147         //  Satellite on polar position
148         // *****************************
149         CircularOrbit circ =
150             new CircularOrbit(7178000.0, 1.e-5, 0., FastMath.toRadians(90.), 0.,
151                                    FastMath.toRadians(90.), PositionAngle.TRUE,
152                                    FramesFactory.getEME2000(), date, mu);
153 
154        // Get nadir attitude
155         rotNadir = nadirAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
156 
157         // Get earth center attitude
158         rotCenter = earthCenterAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
159 
160         // For a satellite at polar position, earth center pointing attitude and nadir pointing
161         // attitude shall be the same, i.e the composition of inverse earth pointing rotation
162         // with nadir pointing rotation shall be identity.
163         rotCompo = rotCenter.composeInverse(rotNadir, RotationConvention.VECTOR_OPERATOR);
164         angle = rotCompo.getAngle();
165         Assert.assertEquals(angle, 0.0, 5.e-6);
166 
167         //  Satellite on any position
168         // ***************************
169         circ =
170             new CircularOrbit(7178000.0, 1.e-5, 0., FastMath.toRadians(50.), 0.,
171                                    FastMath.toRadians(90.), PositionAngle.TRUE,
172                                    FramesFactory.getEME2000(), date, mu);
173 
174         // Get nadir attitude
175         rotNadir = nadirAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
176 
177         // Get earth center attitude
178         rotCenter = earthCenterAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
179 
180         // For a satellite at any position, earth center pointing attitude and nadir pointing
181         // and nadir pointing attitude shall not be the same, i.e the composition of inverse earth
182         // pointing rotation with nadir pointing rotation shall be different from identity.
183         rotCompo = rotCenter.composeInverse(rotNadir, RotationConvention.VECTOR_OPERATOR);
184         angle = rotCompo.getAngle();
185         Assert.assertEquals(angle, FastMath.toRadians(0.16797386586252272), Utils.epsilonAngle);
186     }
187 
188     /** Vertical test : check that Z satellite axis is collinear to local vertical axis,
189         which direction is : (cos(lon)*cos(lat), sin(lon)*cos(lat), sin(lat)),
190         where lon et lat stand for observed point coordinates
191         (i.e satellite ones, since they are the same by construction,
192         but that's what is to test.
193      */
194     @Test
195     public void testVertical() {
196 
197         // Elliptic earth shape
198         OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, itrf);
199 
200         // Create earth center pointing attitude provider
201         NadirPointing nadirAttitudeLaw = new NadirPointing(FramesFactory.getEME2000(), earthShape);
202 
203         //  Satellite on any position
204         CircularOrbit circ =
205             new CircularOrbit(7178000.0, 1.e-5, 0., FastMath.toRadians(50.), 0.,
206                                    FastMath.toRadians(90.), PositionAngle.TRUE,
207                                    FramesFactory.getEME2000(), date, mu);
208 
209         //  Vertical test
210         // ***************
211         // Get observed ground point position/velocity
212         TimeStampedPVCoordinates pvTargetItrf = nadirAttitudeLaw.getTargetPV(circ, date, itrf);
213 
214         // Convert to geodetic coordinates
215         GeodeticPoint geoTarget = earthShape.transform(pvTargetItrf.getPosition(), itrf, date);
216 
217         // Compute local vertical axis
218         double xVert = FastMath.cos(geoTarget.getLongitude())*FastMath.cos(geoTarget.getLatitude());
219         double yVert = FastMath.sin(geoTarget.getLongitude())*FastMath.cos(geoTarget.getLatitude());
220         double zVert = FastMath.sin(geoTarget.getLatitude());
221         Vector3D targetVertical = new Vector3D(xVert, yVert, zVert);
222 
223         // Get attitude rotation state
224         Rotation rotSatEME2000 = nadirAttitudeLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
225 
226         // Get satellite Z axis in EME2000 frame
227         Vector3D zSatEME2000 = rotSatEME2000.applyInverseTo(Vector3D.PLUS_K);
228         Vector3D zSatItrf = FramesFactory.getEME2000().getTransformTo(itrf, date).transformVector(zSatEME2000);
229 
230         // Check that satellite Z axis is collinear to local vertical axis
231         double angle= Vector3D.angle(zSatItrf, targetVertical);
232         Assert.assertEquals(0.0, FastMath.sin(angle), Utils.epsilonTest);
233 
234     }
235 
236     /** Test the derivatives of the sliding target
237      */
238     @Test
239     public void testSlidingDerivatives() {
240 
241         // Elliptic earth shape
242         OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, itrf);
243 
244         // Create earth center pointing attitude provider
245         NadirPointing nadirAttitudeLaw = new NadirPointing(FramesFactory.getEME2000(), earthShape);
246 
247         //  Satellite on any position
248         CircularOrbit circ =
249             new CircularOrbit(7178000.0, 1.e-5, 0., FastMath.toRadians(50.), 0.,
250                                    FastMath.toRadians(90.), PositionAngle.TRUE,
251                                    FramesFactory.getEME2000(), date, mu);
252 
253         List<TimeStampedPVCoordinates> sample = new ArrayList<TimeStampedPVCoordinates>();
254         for (double dt = -0.1; dt < 0.1; dt += 0.05) {
255             Orbit o = circ.shiftedBy(dt);
256             sample.add(nadirAttitudeLaw.getTargetPV(o, o.getDate(), o.getFrame()));
257         }
258         TimeStampedPVCoordinates reference =
259                 TimeStampedPVCoordinates.interpolate(circ.getDate(),
260                                                      CartesianDerivativesFilter.USE_P, sample);
261 
262         TimeStampedPVCoordinates target =
263                 nadirAttitudeLaw.getTargetPV(circ, circ.getDate(), circ.getFrame());
264 
265         Assert.assertEquals(0.0,
266                             Vector3D.distance(reference.getPosition(),     target.getPosition()),
267                             1.0e-15 * reference.getPosition().getNorm());
268         Assert.assertEquals(0.0,
269                             Vector3D.distance(reference.getVelocity(),     target.getVelocity()),
270                             3.0e-11 * reference.getVelocity().getNorm());
271         Assert.assertEquals(0.0,
272                             Vector3D.distance(reference.getAcceleration(), target.getAcceleration()),
273                             1.3e-5 * reference.getAcceleration().getNorm());
274 
275     }
276 
277     @Test
278     public void testSpin() {
279 
280         // Elliptic earth shape
281         OneAxisEllipsoid earthShape = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, itrf);
282 
283         // Create earth center pointing attitude provider
284         NadirPointing law = new NadirPointing(FramesFactory.getEME2000(), earthShape);
285 
286         //  Satellite on any position
287         KeplerianOrbit orbit =
288             new KeplerianOrbit(7178000.0, 1.e-4, FastMath.toRadians(50.),
289                               FastMath.toRadians(10.), FastMath.toRadians(20.),
290                               FastMath.toRadians(30.), PositionAngle.MEAN,
291                               FramesFactory.getEME2000(), date, mu);
292 
293         Propagator propagator = new KeplerianPropagator(orbit, law, mu, 2500.0);
294 
295         double h = 0.1;
296         SpacecraftState sMinus = propagator.propagate(date.shiftedBy(-h));
297         SpacecraftState s0     = propagator.propagate(date);
298         SpacecraftState sPlus  = propagator.propagate(date.shiftedBy(h));
299 
300         // check spin is consistent with attitude evolution
301         double errorAngleMinus     = Rotation.distance(sMinus.shiftedBy(h).getAttitude().getRotation(),
302                                                        s0.getAttitude().getRotation());
303         double evolutionAngleMinus = Rotation.distance(sMinus.getAttitude().getRotation(),
304                                                        s0.getAttitude().getRotation());
305         Assert.assertEquals(0.0, errorAngleMinus, 5.3e-9 * evolutionAngleMinus);
306         double errorAnglePlus      = Rotation.distance(s0.getAttitude().getRotation(),
307                                                        sPlus.shiftedBy(-h).getAttitude().getRotation());
308         double evolutionAnglePlus  = Rotation.distance(s0.getAttitude().getRotation(),
309                                                        sPlus.getAttitude().getRotation());
310         Assert.assertEquals(0.0, errorAnglePlus, 8.1e-9 * evolutionAnglePlus);
311 
312         Vector3D spin0 = s0.getAttitude().getSpin();
313         Rotation rM = sMinus.getAttitude().getRotation();
314         Rotation rP = sPlus.getAttitude().getRotation();
315         Vector3D reference = AngularCoordinates.estimateRate(rM, rP, 2 * h);
316         Assert.assertTrue(Rotation.distance(rM, rP) > 2.0e-4);
317         Assert.assertEquals(0.0, spin0.subtract(reference).getNorm(), 2.0e-6);
318 
319     }
320 
321     private <T extends CalculusFieldElement<T>> void checkField(final Field<T> field, final GroundPointing provider,
322                                                             final Orbit orbit, final AbsoluteDate date,
323                                                             final Frame frame)
324         {
325 
326         final Attitude attitudeD = provider.getAttitude(orbit, date, frame);
327         final FieldOrbit<T> orbitF = new FieldSpacecraftState<>(field, new SpacecraftState(orbit)).getOrbit();
328         final FieldAbsoluteDate<T> dateF = new FieldAbsoluteDate<>(field, date);
329         final FieldAttitude<T> attitudeF = provider.getAttitude(orbitF, dateF, frame);
330         Assert.assertEquals(0.0, Rotation.distance(attitudeD.getRotation(), attitudeF.getRotation().toRotation()), 1.0e-15);
331         Assert.assertEquals(0.0, Vector3D.distance(attitudeD.getSpin(), attitudeF.getSpin().toVector3D()), 2.0e-14);
332         Assert.assertEquals(0.0, Vector3D.distance(attitudeD.getRotationAcceleration(), attitudeF.getRotationAcceleration().toVector3D()), 4.0e-12);
333 
334         final TimeStampedPVCoordinates         pvD = provider.getTargetPV(orbit, date, frame);
335         final TimeStampedFieldPVCoordinates<T> pvF = provider.getTargetPV(orbitF, dateF, frame);
336         Assert.assertEquals(0.0, Vector3D.distance(pvD.getPosition(),     pvF.getPosition().toVector3D()),     1.0e-15);
337         Assert.assertEquals(0.0, Vector3D.distance(pvD.getVelocity(),     pvF.getVelocity().toVector3D()),     2.0e-8);
338         Assert.assertEquals(0.0, Vector3D.distance(pvD.getAcceleration(), pvF.getAcceleration().toVector3D()), 3.0e-5);
339 
340     }
341 
342     @Before
343     public void setUp() {
344         try {
345 
346             Utils.setDataRoot("regular-data");
347 
348             // Computation date
349             date = new AbsoluteDate(new DateComponents(2008, 04, 07),
350                                     TimeComponents.H00,
351                                     TimeScalesFactory.getUTC());
352 
353             // Body mu
354             mu = 3.9860047e14;
355 
356             // Reference frame = ITRF
357             itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
358 
359         } catch (OrekitException oe) {
360             Assert.fail(oe.getMessage());
361         }
362 
363     }
364 
365     @After
366     public void tearDown() {
367         date = null;
368         itrf = null;
369     }
370 
371 }
372