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