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