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.attitudes;
18  
19  import org.hipparchus.CalculusFieldElement;
20  import org.hipparchus.Field;
21  import org.hipparchus.geometry.euclidean.threed.Rotation;
22  import org.hipparchus.geometry.euclidean.threed.RotationConvention;
23  import org.hipparchus.geometry.euclidean.threed.Vector3D;
24  import org.hipparchus.util.Binary64Field;
25  import org.hipparchus.util.FastMath;
26  import org.junit.jupiter.api.AfterEach;
27  import org.junit.jupiter.api.Assertions;
28  import org.junit.jupiter.api.BeforeEach;
29  import org.junit.jupiter.api.Test;
30  import org.orekit.Utils;
31  import org.orekit.bodies.CelestialBodyFactory;
32  import org.orekit.bodies.OneAxisEllipsoid;
33  import org.orekit.errors.OrekitException;
34  import org.orekit.frames.Frame;
35  import org.orekit.frames.FramesFactory;
36  import org.orekit.orbits.*;
37  import org.orekit.propagation.FieldSpacecraftState;
38  import org.orekit.propagation.Propagator;
39  import org.orekit.propagation.SpacecraftState;
40  import org.orekit.propagation.analytical.KeplerianPropagator;
41  import org.orekit.time.AbsoluteDate;
42  import org.orekit.time.DateComponents;
43  import org.orekit.time.FieldAbsoluteDate;
44  import org.orekit.time.TimeComponents;
45  import org.orekit.time.TimeInterpolator;
46  import org.orekit.time.TimeScalesFactory;
47  import org.orekit.utils.AngularCoordinates;
48  import org.orekit.utils.CartesianDerivativesFilter;
49  import org.orekit.utils.ExtendedPositionProvider;
50  import org.orekit.utils.IERSConventions;
51  import org.orekit.utils.PVCoordinates;
52  import org.orekit.utils.TimeStampedFieldPVCoordinates;
53  import org.orekit.utils.TimeStampedPVCoordinates;
54  import org.orekit.utils.TimeStampedPVCoordinatesHermiteInterpolator;
55  
56  import java.util.ArrayList;
57  import java.util.List;
58  
59  
60  class YawSteeringTest {
61  
62      // Computation date
63      private AbsoluteDate date;
64  
65      // Reference frame = ITRF
66      private Frame itrf;
67  
68      // Satellite position
69      CircularOrbit circOrbit;
70  
71      // Earth shape
72      OneAxisEllipsoid earthShape;
73  
74      @Test
75      void testTarget() {
76  
77          //  Attitude laws
78          // **************
79          // Target pointing attitude provider without yaw compensation
80          NadirPointing nadirLaw = new NadirPointing(circOrbit.getFrame(), earthShape);
81  
82          // Target pointing attitude provider with yaw compensation
83          YawSteering yawCompensLaw =
84              new YawSteering(circOrbit.getFrame(), nadirLaw, CelestialBodyFactory.getSun(), Vector3D.MINUS_I);
85  
86          //  Check observed ground point
87          // *****************************
88          TimeStampedPVCoordinates noYawObserved = nadirLaw.getTargetPV(circOrbit, date, itrf);
89  
90          // with yaw compensation
91          TimeStampedPVCoordinates yawObserved = yawCompensLaw.getTargetPV(circOrbit, date, itrf);
92  
93          // Check difference
94          PVCoordinates observedDiff = new PVCoordinates(yawObserved, noYawObserved);
95  
96          Assertions.assertEquals(0.0, observedDiff.getPosition().getNorm(), Utils.epsilonTest);
97          Assertions.assertEquals(0.0, observedDiff.getVelocity().getNorm(), Utils.epsilonTest);
98          Assertions.assertEquals(0.0, observedDiff.getAcceleration().getNorm(), Utils.epsilonTest);
99          Assertions.assertSame(nadirLaw, yawCompensLaw.getUnderlyingAttitudeProvider());
100 
101     }
102 
103     @Test
104     void testSunAligned() {
105 
106         //  Attitude laws
107         // **************
108         // Target pointing attitude provider over satellite nadir at date, without yaw compensation
109         NadirPointing nadirLaw = new NadirPointing(circOrbit.getFrame(), earthShape);
110 
111         // Target pointing attitude provider with yaw compensation
112         ExtendedPositionProvider sun = CelestialBodyFactory.getSun();
113         YawSteering yawCompensLaw = new YawSteering(circOrbit.getFrame(), nadirLaw, sun, Vector3D.MINUS_I);
114 
115         // Get sun direction in satellite frame
116         Rotation rotYaw = yawCompensLaw.getAttitude(circOrbit, date, circOrbit.getFrame()).getRotation();
117         Vector3D sunEME2000 = sun.getPosition(date, FramesFactory.getEME2000());
118         Vector3D sunSat = rotYaw.applyTo(sunEME2000);
119 
120         // Check sun is in (X, Z) plane
121         Assertions.assertEquals(0.0, FastMath.sin(sunSat.getAlpha()), 1.0e-7);
122 
123     }
124 
125     @Test
126     void testCompensAxis() {
127 
128         //  Attitude laws
129         // **************
130         // Target pointing attitude provider over satellite nadir at date, without yaw compensation
131         NadirPointing nadirLaw = new NadirPointing(circOrbit.getFrame(), earthShape);
132 
133         // Target pointing attitude provider with yaw compensation
134         YawSteering yawCompensLaw =
135             new YawSteering(circOrbit.getFrame(), nadirLaw, CelestialBodyFactory.getSun(), Vector3D.MINUS_I);
136 
137         // Get attitude rotations from non yaw compensated / yaw compensated laws
138         Rotation rotNoYaw = nadirLaw.getAttitude(circOrbit, date, circOrbit.getFrame()).getRotation();
139         Rotation rotYaw = yawCompensLaw.getAttitude(circOrbit, date, circOrbit.getFrame()).getRotation();
140 
141         checkField(Binary64Field.getInstance(), yawCompensLaw, circOrbit, circOrbit.getDate(), circOrbit.getFrame());
142 
143         // Compose rotations composition
144         Rotation compoRot = rotYaw.compose(rotNoYaw.revert(), RotationConvention.VECTOR_OPERATOR);
145         Vector3D yawAxis = compoRot.getAxis(RotationConvention.VECTOR_OPERATOR);
146 
147         // Check axis
148         Assertions.assertEquals(0., yawAxis.getX(), Utils.epsilonTest);
149         Assertions.assertEquals(0., yawAxis.getY(), Utils.epsilonTest);
150         Assertions.assertEquals(1., yawAxis.getZ(), Utils.epsilonTest);
151 
152     }
153 
154     /** Test the derivatives of the sliding target
155      */
156     @Test
157     void testSlidingDerivatives() {
158 
159         GroundPointing law = new YawSteering(circOrbit.getFrame(),
160                                              new NadirPointing(circOrbit.getFrame(), earthShape),
161                                              CelestialBodyFactory.getSun(),
162                                              Vector3D.MINUS_I);
163 
164         List<TimeStampedPVCoordinates> sample = new ArrayList<TimeStampedPVCoordinates>();
165         for (double dt = -0.1; dt < 0.1; dt += 0.05) {
166             Orbit o = circOrbit.shiftedBy(dt);
167             sample.add(law.getTargetPV(o, o.getDate(), o.getFrame()));
168         }
169 
170         // create interpolator
171         final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
172                 new TimeStampedPVCoordinatesHermiteInterpolator(sample.size(), CartesianDerivativesFilter.USE_P);
173 
174         TimeStampedPVCoordinates reference = interpolator.interpolate(circOrbit.getDate(), sample);
175 
176         TimeStampedPVCoordinates target =
177                 law.getTargetPV(circOrbit, circOrbit.getDate(), circOrbit.getFrame());
178 
179         Assertions.assertEquals(0.0,
180                             Vector3D.distance(reference.getPosition(),     target.getPosition()),
181                             1.0e-15 * reference.getPosition().getNorm());
182         Assertions.assertEquals(0.0,
183                             Vector3D.distance(reference.getVelocity(),     target.getVelocity()),
184                             4.0e-11 * reference.getVelocity().getNorm());
185         Assertions.assertEquals(0.0,
186                             Vector3D.distance(reference.getAcceleration(), target.getAcceleration()),
187                             8.0e-6 * reference.getAcceleration().getNorm());
188 
189     }
190 
191     @Test
192     void testSpin() {
193 
194         NadirPointing nadirLaw = new NadirPointing(circOrbit.getFrame(), earthShape);
195 
196         // Target pointing attitude provider with yaw compensation
197         AttitudeProvider law = new YawSteering(circOrbit.getFrame(), nadirLaw, CelestialBodyFactory.getSun(), Vector3D.MINUS_I);
198 
199         KeplerianOrbit orbit =
200             new KeplerianOrbit(7178000.0, 1.e-4, FastMath.toRadians(50.),
201                               FastMath.toRadians(10.), FastMath.toRadians(20.),
202                               FastMath.toRadians(30.), PositionAngleType.MEAN,
203                               FramesFactory.getEME2000(),
204                               date.shiftedBy(-300.0),
205                               3.986004415e14);
206 
207         Propagator propagator = new KeplerianPropagator(orbit, law);
208 
209         double h = 0.01;
210         SpacecraftState sMinus = propagator.propagate(date.shiftedBy(-h));
211         SpacecraftState s0     = propagator.propagate(date);
212         SpacecraftState sPlus  = propagator.propagate(date.shiftedBy(h));
213 
214         // check spin is consistent with attitude evolution
215         double errorAngleMinus     = Rotation.distance(sMinus.shiftedBy(h).getAttitude().getRotation(),
216                                                        s0.getAttitude().getRotation());
217         double evolutionAngleMinus = Rotation.distance(sMinus.getAttitude().getRotation(),
218                                                        s0.getAttitude().getRotation());
219         Assertions.assertEquals(0.0, errorAngleMinus, 1.0e-9 * evolutionAngleMinus);
220         double errorAnglePlus      = Rotation.distance(s0.getAttitude().getRotation(),
221                                                        sPlus.shiftedBy(-h).getAttitude().getRotation());
222         double evolutionAnglePlus  = Rotation.distance(s0.getAttitude().getRotation(),
223                                                        sPlus.getAttitude().getRotation());
224         Assertions.assertEquals(0.0, errorAnglePlus, 1.0e-9 * evolutionAnglePlus);
225 
226         Vector3D spin0 = s0.getAttitude().getSpin();
227         Vector3D reference = AngularCoordinates.estimateRate(sMinus.getAttitude().getRotation(),
228                                                              sPlus.getAttitude().getRotation(),
229                                                              2 * h);
230         Assertions.assertTrue(spin0.getNorm() > 1.0e-3);
231         Assertions.assertEquals(0.0, spin0.subtract(reference).getNorm(), 5.0e-13);
232 
233     }
234 
235     private <T extends CalculusFieldElement<T>> void checkField(final Field<T> field, final GroundPointing provider,
236                                                             final Orbit orbit, final AbsoluteDate date,
237                                                             final Frame frame)
238         {
239 
240         final Attitude attitudeD = provider.getAttitude(orbit, date, frame);
241         final FieldOrbit<T> orbitF = new FieldSpacecraftState<>(field, new SpacecraftState(orbit)).getOrbit();
242         final FieldAbsoluteDate<T> dateF = new FieldAbsoluteDate<>(field, date);
243         final FieldAttitude<T> attitudeF = provider.getAttitude(orbitF, dateF, frame);
244         Assertions.assertEquals(0.0, Rotation.distance(attitudeD.getRotation(), attitudeF.getRotation().toRotation()), 1.0e-15);
245         Assertions.assertEquals(0.0, Vector3D.distance(attitudeD.getSpin(), attitudeF.getSpin().toVector3D()), 4.0e-14);
246         Assertions.assertEquals(0.0, Vector3D.distance(attitudeD.getRotationAcceleration(), attitudeF.getRotationAcceleration().toVector3D()), 3.0e-12);
247 
248         final TimeStampedPVCoordinates         pvD = provider.getTargetPV(orbit, date, frame);
249         final TimeStampedFieldPVCoordinates<T> pvF = provider.getTargetPV(orbitF, dateF, frame);
250         Assertions.assertEquals(0.0, Vector3D.distance(pvD.getPosition(),     pvF.getPosition().toVector3D()),     2.0e-9);
251         Assertions.assertEquals(0.0, Vector3D.distance(pvD.getVelocity(),     pvF.getVelocity().toVector3D()),     7.0e-8);
252         Assertions.assertEquals(0.0, Vector3D.distance(pvD.getAcceleration(), pvF.getAcceleration().toVector3D()), 4.0e-5);
253 
254     }
255 
256     @BeforeEach
257     public void setUp() {
258         try {
259             Utils.setDataRoot("regular-data");
260 
261             // Computation date
262             date = new AbsoluteDate(new DateComponents(1970, 04, 07),
263                                     TimeComponents.H00,
264                                     TimeScalesFactory.getUTC());
265 
266             // Body mu
267             final double mu = 3.9860047e14;
268 
269             // Reference frame = ITRF
270             itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
271 
272             //  Satellite position
273             circOrbit =
274                 new CircularOrbit(7178000.0, 0.5e-4, -0.5e-4, FastMath.toRadians(50.), FastMath.toRadians(270.),
275                                        FastMath.toRadians(5.300), PositionAngleType.MEAN,
276                                        FramesFactory.getEME2000(), date, mu);
277 
278             // Elliptic earth shape
279             earthShape =
280                 new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, itrf);
281 
282         } catch (OrekitException oe) {
283             Assertions.fail(oe.getMessage());
284         }
285 
286     }
287 
288     @AfterEach
289     public void tearDown() {
290         date = null;
291         itrf = null;
292         circOrbit = null;
293         earthShape = null;
294     }
295 
296 }
297