1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.forces;
18
19 import org.hipparchus.analysis.differentiation.DerivativeStructure;
20 import org.hipparchus.analysis.differentiation.Gradient;
21 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
22 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
23 import org.hipparchus.geometry.euclidean.threed.Vector3D;
24 import org.hipparchus.util.Precision;
25 import org.junit.jupiter.api.Assertions;
26 import org.orekit.attitudes.AttitudeProvider;
27 import org.orekit.frames.Frame;
28 import org.orekit.propagation.FieldSpacecraftState;
29 import org.orekit.propagation.SpacecraftState;
30 import org.orekit.time.AbsoluteDate;
31
32
33 public abstract class AbstractLegacyForceModelTest extends AbstractForceModelTest {
34
35 protected abstract FieldVector3D<DerivativeStructure> accelerationDerivatives(ForceModel forceModel,
36 final AbsoluteDate date, final Frame frame,
37 final FieldVector3D<DerivativeStructure> position,
38 final FieldVector3D<DerivativeStructure> velocity,
39 final FieldRotation<DerivativeStructure> rotation,
40 final DerivativeStructure mass);
41
42 protected abstract FieldVector3D<Gradient> accelerationDerivativesGradient(ForceModel forceModel,
43 final AbsoluteDate date, final Frame frame,
44 final FieldVector3D<Gradient> position,
45 final FieldVector3D<Gradient> velocity,
46 final FieldRotation<Gradient> rotation,
47 final Gradient mass);
48
49 protected void checkStateJacobianVs80Implementation(final SpacecraftState state, final ForceModel forceModel,
50 final AttitudeProvider attitudeProvider,
51 final double checkTolerance, final boolean print)
52 {
53 FieldSpacecraftState<DerivativeStructure> fState = toDS(state, attitudeProvider);
54 FieldVector3D<DerivativeStructure> dsNew = forceModel.acceleration(fState,
55 forceModel.getParameters(fState.getDate().getField()));
56 FieldVector3D<DerivativeStructure> dsOld = accelerationDerivatives(forceModel, fState.getDate().toAbsoluteDate(),
57 fState.getFrame(),
58 fState.getPVCoordinates().getPosition(),
59 fState.getPVCoordinates().getVelocity(),
60 fState.getAttitude().getRotation(),
61 fState.getMass());
62 Vector3D dFdPXRef = new Vector3D(dsOld.getX().getPartialDerivative(1, 0, 0, 0, 0, 0),
63 dsOld.getY().getPartialDerivative(1, 0, 0, 0, 0, 0),
64 dsOld.getZ().getPartialDerivative(1, 0, 0, 0, 0, 0));
65 Vector3D dFdPXRes = new Vector3D(dsNew.getX().getPartialDerivative(1, 0, 0, 0, 0, 0),
66 dsNew.getY().getPartialDerivative(1, 0, 0, 0, 0, 0),
67 dsNew.getZ().getPartialDerivative(1, 0, 0, 0, 0, 0));
68 Vector3D dFdPYRef = new Vector3D(dsOld.getX().getPartialDerivative(0, 1, 0, 0, 0, 0),
69 dsOld.getY().getPartialDerivative(0, 1, 0, 0, 0, 0),
70 dsOld.getZ().getPartialDerivative(0, 1, 0, 0, 0, 0));
71 Vector3D dFdPYRes = new Vector3D(dsNew.getX().getPartialDerivative(0, 1, 0, 0, 0, 0),
72 dsNew.getY().getPartialDerivative(0, 1, 0, 0, 0, 0),
73 dsNew.getZ().getPartialDerivative(0, 1, 0, 0, 0, 0));
74 Vector3D dFdPZRef = new Vector3D(dsOld.getX().getPartialDerivative(0, 0, 1, 0, 0, 0),
75 dsOld.getY().getPartialDerivative(0, 0, 1, 0, 0, 0),
76 dsOld.getZ().getPartialDerivative(0, 0, 1, 0, 0, 0));
77 Vector3D dFdPZRes = new Vector3D(dsNew.getX().getPartialDerivative(0, 0, 1, 0, 0, 0),
78 dsNew.getY().getPartialDerivative(0, 0, 1, 0, 0, 0),
79 dsNew.getZ().getPartialDerivative(0, 0, 1, 0, 0, 0));
80 Vector3D dFdVXRef = new Vector3D(dsOld.getX().getPartialDerivative(0, 0, 0, 1, 0, 0),
81 dsOld.getY().getPartialDerivative(0, 0, 0, 1, 0, 0),
82 dsOld.getZ().getPartialDerivative(0, 0, 0, 1, 0, 0));
83 Vector3D dFdVXRes = new Vector3D(dsNew.getX().getPartialDerivative(0, 0, 0, 1, 0, 0),
84 dsNew.getY().getPartialDerivative(0, 0, 0, 1, 0, 0),
85 dsNew.getZ().getPartialDerivative(0, 0, 0, 1, 0, 0));
86 Vector3D dFdVYRef = new Vector3D(dsOld.getX().getPartialDerivative(0, 0, 0, 0, 1, 0),
87 dsOld.getY().getPartialDerivative(0, 0, 0, 0, 1, 0),
88 dsOld.getZ().getPartialDerivative(0, 0, 0, 0, 1, 0));
89 Vector3D dFdVYRes = new Vector3D(dsNew.getX().getPartialDerivative(0, 0, 0, 0, 1, 0),
90 dsNew.getY().getPartialDerivative(0, 0, 0, 0, 1, 0),
91 dsNew.getZ().getPartialDerivative(0, 0, 0, 0, 1, 0));
92 Vector3D dFdVZRef = new Vector3D(dsOld.getX().getPartialDerivative(0, 0, 0, 0, 0, 1),
93 dsOld.getY().getPartialDerivative(0, 0, 0, 0, 0, 1),
94 dsOld.getZ().getPartialDerivative(0, 0, 0, 0, 0, 1));
95 Vector3D dFdVZRes = new Vector3D(dsNew.getX().getPartialDerivative(0, 0, 0, 0, 0, 1),
96 dsNew.getY().getPartialDerivative(0, 0, 0, 0, 0, 1),
97 dsNew.getZ().getPartialDerivative(0, 0, 0, 0, 0, 1));
98 if (print) {
99 System.out.println("dF/dPX ref norm: " + dFdPXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPXRef, dFdPXRes) + ", rel error: " + (Vector3D.distance(dFdPXRef, dFdPXRes) / dFdPXRef.getNorm()));
100 System.out.println("dF/dPY ref norm: " + dFdPYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPYRef, dFdPYRes) + ", rel error: " + (Vector3D.distance(dFdPYRef, dFdPYRes) / dFdPYRef.getNorm()));
101 System.out.println("dF/dPZ ref norm: " + dFdPZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPZRef, dFdPZRes) + ", rel error: " + (Vector3D.distance(dFdPZRef, dFdPZRes) / dFdPZRef.getNorm()));
102 System.out.println("dF/dVX ref norm: " + dFdVXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVXRef, dFdVXRes) + ", rel error: " + (Vector3D.distance(dFdVXRef, dFdVXRes) / dFdVXRef.getNorm()));
103 System.out.println("dF/dVY ref norm: " + dFdVYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVYRef, dFdVYRes) + ", rel error: " + (Vector3D.distance(dFdVYRef, dFdVYRes) / dFdVYRef.getNorm()));
104 System.out.println("dF/dVZ ref norm: " + dFdVZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVZRef, dFdVZRes) + ", rel error: " + (Vector3D.distance(dFdVZRef, dFdVZRes) / dFdVZRef.getNorm()));
105 }
106 checkdFdP(dFdPXRef, dFdPXRes, checkTolerance);
107 checkdFdP(dFdPYRef, dFdPYRes, checkTolerance);
108 checkdFdP(dFdPZRef, dFdPZRes, checkTolerance);
109 checkdFdP(dFdVXRef, dFdVXRes, checkTolerance);
110 checkdFdP(dFdVYRef, dFdVYRes, checkTolerance);
111 checkdFdP(dFdVZRef, dFdVZRes, checkTolerance);
112
113 }
114
115 protected void checkStateJacobianVs80ImplementationGradient(final SpacecraftState state, final ForceModel forceModel,
116 final AttitudeProvider attitudeProvider,
117 final double checkTolerance, final boolean print)
118 {
119 FieldSpacecraftState<Gradient> fState = toGradient(state, attitudeProvider);
120 FieldVector3D<Gradient> gNew = forceModel.acceleration(fState,
121 forceModel.getParameters(fState.getDate().getField()));
122 FieldVector3D<Gradient> gOld = accelerationDerivativesGradient(forceModel, fState.getDate().toAbsoluteDate(),
123 fState.getFrame(),
124 fState.getPVCoordinates().getPosition(),
125 fState.getPVCoordinates().getVelocity(),
126 fState.getAttitude().getRotation(),
127 fState.getMass());
128 Vector3D dFdPXRef = new Vector3D(gOld.getX().getPartialDerivative(0),
129 gOld.getY().getPartialDerivative(0),
130 gOld.getZ().getPartialDerivative(0));
131 Vector3D dFdPXRes = new Vector3D(gNew.getX().getPartialDerivative(0),
132 gNew.getY().getPartialDerivative(0),
133 gNew.getZ().getPartialDerivative(0));
134 Vector3D dFdPYRef = new Vector3D(gOld.getX().getPartialDerivative(1),
135 gOld.getY().getPartialDerivative(1),
136 gOld.getZ().getPartialDerivative(1));
137 Vector3D dFdPYRes = new Vector3D(gNew.getX().getPartialDerivative(1),
138 gNew.getY().getPartialDerivative(1),
139 gNew.getZ().getPartialDerivative(1));
140 Vector3D dFdPZRef = new Vector3D(gOld.getX().getPartialDerivative(2),
141 gOld.getY().getPartialDerivative(2),
142 gOld.getZ().getPartialDerivative(2));
143 Vector3D dFdPZRes = new Vector3D(gNew.getX().getPartialDerivative(2),
144 gNew.getY().getPartialDerivative(2),
145 gNew.getZ().getPartialDerivative(2));
146 Vector3D dFdVXRef = new Vector3D(gOld.getX().getPartialDerivative(3),
147 gOld.getY().getPartialDerivative(3),
148 gOld.getZ().getPartialDerivative(3));
149 Vector3D dFdVXRes = new Vector3D(gNew.getX().getPartialDerivative(3),
150 gNew.getY().getPartialDerivative(3),
151 gNew.getZ().getPartialDerivative(3));
152 Vector3D dFdVYRef = new Vector3D(gOld.getX().getPartialDerivative(4),
153 gOld.getY().getPartialDerivative(4),
154 gOld.getZ().getPartialDerivative(4));
155 Vector3D dFdVYRes = new Vector3D(gNew.getX().getPartialDerivative(4),
156 gNew.getY().getPartialDerivative(4),
157 gNew.getZ().getPartialDerivative(4));
158 Vector3D dFdVZRef = new Vector3D(gOld.getX().getPartialDerivative(5),
159 gOld.getY().getPartialDerivative(5),
160 gOld.getZ().getPartialDerivative(5));
161 Vector3D dFdVZRes = new Vector3D(gNew.getX().getPartialDerivative(5),
162 gNew.getY().getPartialDerivative(5),
163 gNew.getZ().getPartialDerivative(5));
164 if (print) {
165 System.out.println("dF/dPX ref norm: " + dFdPXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPXRef, dFdPXRes) + ", rel error: " + (Vector3D.distance(dFdPXRef, dFdPXRes) / dFdPXRef.getNorm()));
166 System.out.println("dF/dPY ref norm: " + dFdPYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPYRef, dFdPYRes) + ", rel error: " + (Vector3D.distance(dFdPYRef, dFdPYRes) / dFdPYRef.getNorm()));
167 System.out.println("dF/dPZ ref norm: " + dFdPZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPZRef, dFdPZRes) + ", rel error: " + (Vector3D.distance(dFdPZRef, dFdPZRes) / dFdPZRef.getNorm()));
168 System.out.println("dF/dVX ref norm: " + dFdVXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVXRef, dFdVXRes) + ", rel error: " + (Vector3D.distance(dFdVXRef, dFdVXRes) / dFdVXRef.getNorm()));
169 System.out.println("dF/dVY ref norm: " + dFdVYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVYRef, dFdVYRes) + ", rel error: " + (Vector3D.distance(dFdVYRef, dFdVYRes) / dFdVYRef.getNorm()));
170 System.out.println("dF/dVZ ref norm: " + dFdVZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVZRef, dFdVZRes) + ", rel error: " + (Vector3D.distance(dFdVZRef, dFdVZRes) / dFdVZRef.getNorm()));
171 }
172 checkdFdP(dFdPXRef, dFdPXRes, checkTolerance);
173 checkdFdP(dFdPYRef, dFdPYRes, checkTolerance);
174 checkdFdP(dFdPZRef, dFdPZRes, checkTolerance);
175 checkdFdP(dFdVXRef, dFdVXRes, checkTolerance);
176 checkdFdP(dFdVYRef, dFdVYRes, checkTolerance);
177 checkdFdP(dFdVZRef, dFdVZRes, checkTolerance);
178
179 }
180
181 private void checkdFdP(final Vector3D reference, final Vector3D result, final double checkTolerance) {
182 if (reference.getNorm() == 0) {
183
184
185 Assertions.assertEquals(0, result.getNorm(), Precision.SAFE_MIN);
186 } else {
187 Assertions.assertEquals(0, Vector3D.distance(reference, result), checkTolerance * reference.getNorm());
188 }
189 }
190
191 }
192
193