1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.attitudes;
18
19 import java.util.ArrayList;
20 import java.util.List;
21
22 import org.hipparchus.Field;
23 import org.hipparchus.CalculusFieldElement;
24 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.geometry.euclidean.threed.Vector3D;
27 import org.hipparchus.util.Decimal64Field;
28 import org.hipparchus.util.FastMath;
29 import org.junit.Assert;
30 import org.junit.Test;
31 import org.orekit.Utils;
32 import org.orekit.bodies.OneAxisEllipsoid;
33 import org.orekit.frames.FramesFactory;
34 import org.orekit.orbits.FieldCircularOrbit;
35 import org.orekit.propagation.analytical.FieldEcksteinHechlerPropagator;
36 import org.orekit.time.FieldAbsoluteDate;
37 import org.orekit.utils.Constants;
38 import org.orekit.utils.FieldAngularCoordinates;
39 import org.orekit.utils.FieldPVCoordinates;
40 import org.orekit.utils.IERSConventions;
41
42 public class FieldAttitudeTest {
43
44 @Test
45 public void testShift() {
46 doTestShift(Decimal64Field.getInstance());
47 }
48
49 @Test
50 public void testSpin() {
51 doTestSpin(Decimal64Field.getInstance());
52 }
53
54 @Test
55 public void testInterpolation() {
56 doTestInterpolation(Decimal64Field.getInstance());
57 }
58
59 private <T extends CalculusFieldElement<T>> void doTestShift(final Field<T> field){
60 T zero = field.getZero();
61 T one = field.getOne();
62 T rate = one.multiply(2 * FastMath.PI / (12 * 60));
63 FieldAttitude<T> attitude = new FieldAttitude<>(new FieldAbsoluteDate<>(field), FramesFactory.getEME2000(),
64 new FieldRotation<>(one, zero, zero, zero, false),
65 new FieldVector3D<>(rate, new FieldVector3D<>(zero, zero, one)), new FieldVector3D<>(zero, zero, zero));
66 Assert.assertEquals(rate.getReal(), attitude.getSpin().getNorm().getReal(), 1.0e-10);
67 double dt_R = 10.0;
68 T dt = zero.add(dt_R);
69
70 T alpha = rate.multiply(dt);
71 FieldAttitude<T> shifted = attitude.shiftedBy(dt);
72 Assert.assertEquals(rate.getReal(), shifted.getSpin().getNorm().getReal(), 1.0e-10);
73 Assert.assertEquals(alpha.getReal(), FieldRotation.distance(attitude.getRotation(), shifted.getRotation()).getReal(), 1.0e-10);
74
75 FieldVector3D<T> xSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I);
76 Assert.assertEquals(0.0, xSat.subtract(new FieldVector3D<>(alpha.cos(), alpha.sin(), zero)).getNorm().getReal(), 1.0e-10);
77 FieldVector3D<T> ySat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J);
78 Assert.assertEquals(0.0, ySat.subtract(new FieldVector3D<>(alpha.sin().multiply(-1), alpha.cos(), zero)).getNorm().getReal(), 1.0e-10);
79 FieldVector3D<T> zSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K);
80 Assert.assertEquals(0.0, zSat.subtract(Vector3D.PLUS_K).getNorm().getReal(), 1.0e-10);
81
82 }
83
84
85 private <T extends CalculusFieldElement<T>> void doTestSpin(final Field<T> field) {
86 T zero = field.getZero();
87 T rate = zero.add(2 * FastMath.PI / (12 * 60));
88 FieldAttitude<T> attitude = new FieldAttitude<>(new FieldAbsoluteDate<>(field), FramesFactory.getEME2000(),
89 new FieldRotation<>(zero.add(0.48), zero.add(0.64), zero.add(0.36), zero.add(0.48), false),
90 new FieldVector3D<>(rate, FieldVector3D.getPlusK(field)), FieldVector3D.getZero(field));
91 Assert.assertEquals(rate.getReal(), attitude.getSpin().getNorm().getReal(), 1.0e-10);
92 T dt = zero.add(10.0);
93 FieldAttitude<T> shifted = attitude.shiftedBy(dt);
94 Assert.assertEquals(rate.getReal(), shifted.getSpin().getNorm().getReal(), 1.0e-10);
95
96 FieldVector3D<T> shiftedX = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I);
97 FieldVector3D<T> shiftedY = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J);
98 FieldVector3D<T> shiftedZ = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K);
99 FieldVector3D<T> originalX = attitude.getRotation().applyInverseTo(Vector3D.PLUS_I);
100 FieldVector3D<T> originalY = attitude.getRotation().applyInverseTo(Vector3D.PLUS_J);
101 FieldVector3D<T> originalZ = attitude.getRotation().applyInverseTo(Vector3D.PLUS_K);
102 Assert.assertEquals( FastMath.cos(rate.getReal() * dt.getReal()), FieldVector3D.dotProduct(shiftedX, originalX).getReal(), 1.0e-10);
103 Assert.assertEquals( FastMath.sin(rate.getReal() * dt.getReal()), FieldVector3D.dotProduct(shiftedX, originalY).getReal(), 1.0e-10);
104 Assert.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedX, originalZ).getReal(), 1.0e-10);
105 Assert.assertEquals(-FastMath.sin(rate.getReal() * dt.getReal()), FieldVector3D.dotProduct(shiftedY, originalX).getReal(), 1.0e-10);
106 Assert.assertEquals( FastMath.cos(rate.getReal() * dt.getReal()), FieldVector3D.dotProduct(shiftedY, originalY).getReal(), 1.0e-10);
107 Assert.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedY, originalZ).getReal(), 1.0e-10);
108 Assert.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedZ, originalX).getReal(), 1.0e-10);
109 Assert.assertEquals( 0.0, FieldVector3D.dotProduct(shiftedZ, originalY).getReal(), 1.0e-10);
110 Assert.assertEquals( 1.0, FieldVector3D.dotProduct(shiftedZ, originalZ).getReal(), 1.0e-10);
111
112 FieldVector3D<T> forward = FieldAngularCoordinates.estimateRate(attitude.getRotation(), shifted.getRotation(), dt);
113 Assert.assertEquals(0.0, forward.subtract(attitude.getSpin()).getNorm().getReal(), 1.0e-10);
114
115 FieldVector3D<T> reversed = FieldAngularCoordinates.estimateRate(shifted.getRotation(), attitude.getRotation(), dt);
116 Assert.assertEquals(0.0, reversed.add(attitude.getSpin()).getNorm().getReal(), 1.0e-10);
117
118 }
119
120 private <T extends CalculusFieldElement<T>> void doTestInterpolation(final Field<T> field) {
121
122 T zero = field.getZero();
123
124 Utils.setDataRoot("regular-data");
125 final double ehMu = 3.9860047e14;
126 final double ae = 6.378137e6;
127 final double c20 = -1.08263e-3;
128 final double c30 = 2.54e-6;
129 final double c40 = 1.62e-6;
130 final double c50 = 2.3e-7;
131 final double c60 = -5.5e-7;
132
133 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field).shiftedBy(584.);
134 final FieldVector3D<T> position = new FieldVector3D<>(zero.add(3220103.), zero.add(69623.), zero.add(6449822.));
135 final FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(6414.7), zero.add(-2006.), zero.add(-3180.));
136 final FieldCircularOrbit<T> initialOrbit = new FieldCircularOrbit<>(new FieldPVCoordinates<>(position, velocity),
137 FramesFactory.getEME2000(), date, zero.add(ehMu));
138
139 FieldEcksteinHechlerPropagator<T> propagator =
140 new FieldEcksteinHechlerPropagator<>(initialOrbit, ae, zero.add(ehMu), c20, c30, c40, c50, c60);
141 OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
142 Constants.WGS84_EARTH_FLATTENING,
143 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
144 propagator.setAttitudeProvider(new BodyCenterPointing(initialOrbit.getFrame(), earth));
145 FieldAttitude<T> initialAttitude = propagator.propagate(initialOrbit.getDate()).getAttitude();
146
147
148
149 List<FieldAttitude<T>> sample = new ArrayList<FieldAttitude<T>>();
150 for (double dt = 0; dt < 251.0; dt += 60.0) {
151 sample.add(propagator.propagate(date.shiftedBy(dt)).getAttitude());
152 }
153
154
155 double maxShiftAngleError = 0;
156 double maxInterpolationAngleError = 0;
157 double maxShiftRateError = 0 ;
158 double maxInterpolationRateError = 0;
159 for (double dt_R = 0; dt_R < 240.0; dt_R += 1.0) {
160 T dt = zero.add(dt_R);
161 FieldAbsoluteDate<T> t = initialOrbit.getDate().shiftedBy(dt);
162 FieldAttitude<T> propagated = propagator.propagate(t).getAttitude();
163 T shiftAngleError = FieldRotation.distance(propagated.getRotation(),
164 initialAttitude.shiftedBy(dt).getRotation());
165 T interpolationAngleError = FieldRotation.distance(propagated.getRotation(),
166 initialAttitude.interpolate(t, sample).getRotation());
167 T shiftRateError = FieldVector3D.distance(propagated.getSpin(),
168 initialAttitude.shiftedBy(dt).getSpin());
169 T interpolationRateError = FieldVector3D.distance(propagated.getSpin(),
170 initialAttitude.interpolate(t, sample).getSpin());
171 maxShiftAngleError = FastMath.max(maxShiftAngleError, shiftAngleError.getReal());
172 maxInterpolationAngleError = FastMath.max(maxInterpolationAngleError, interpolationAngleError.getReal());
173 maxShiftRateError = FastMath.max(maxShiftRateError, shiftRateError.getReal());
174 maxInterpolationRateError = FastMath.max(maxInterpolationRateError, interpolationRateError.getReal());
175
176 }
177 Assert.assertTrue(maxShiftAngleError > 6.0e-6);
178 Assert.assertTrue(maxInterpolationAngleError < 6.0e-15);
179 Assert.assertTrue(maxShiftRateError > 7.0e-8);
180 Assert.assertTrue(maxInterpolationRateError < 2.0e-16);
181
182
183 maxShiftAngleError = 0;
184 maxInterpolationAngleError = 0;
185 maxShiftRateError = 0;
186 maxInterpolationRateError = 0;
187 for (double dt_R = 250.0; dt_R < 300.0; dt_R += 1.0) {
188 T dt = zero.add(dt_R);
189 FieldAbsoluteDate<T> t = initialOrbit.getDate().shiftedBy(dt);
190 FieldAttitude<T> propagated = propagator.propagate(t).getAttitude();
191 T shiftAngleError = FieldRotation.distance(propagated.getRotation(),
192 initialAttitude.shiftedBy(dt).getRotation());
193 T interpolationAngleError = FieldRotation.distance(propagated.getRotation(),
194 initialAttitude.interpolate(t, sample).getRotation());
195 T shiftRateError = FieldVector3D.distance(propagated.getSpin(),
196 initialAttitude.shiftedBy(dt).getSpin());
197 T interpolationRateError = FieldVector3D.distance(propagated.getSpin(),
198 initialAttitude.interpolate(t, sample).getSpin());
199 maxShiftAngleError = FastMath.max(maxShiftAngleError, shiftAngleError.getReal());
200 maxInterpolationAngleError = FastMath.max(maxInterpolationAngleError, interpolationAngleError.getReal());
201 maxShiftRateError = FastMath.max(maxShiftRateError, shiftRateError.getReal());
202 maxInterpolationRateError = FastMath.max(maxInterpolationRateError, interpolationRateError.getReal());
203 }
204 Assert.assertTrue(maxShiftAngleError > 1.0e-5);
205 Assert.assertTrue(maxInterpolationAngleError < 8.0e-13);
206 Assert.assertTrue(maxShiftRateError > 1.0e-7);
207 Assert.assertTrue(maxInterpolationRateError < 6.0e-14);
208
209 }
210
211 }
212