1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.control.heuristics.lambert;
18
19 import org.hipparchus.geometry.euclidean.threed.Vector3D;
20 import org.hipparchus.linear.RealMatrix;
21 import org.hipparchus.util.FastMath;
22 import org.junit.jupiter.api.BeforeEach;
23 import org.junit.jupiter.api.Test;
24 import org.junit.jupiter.params.ParameterizedTest;
25 import org.junit.jupiter.params.provider.ValueSource;
26 import org.orekit.Utils;
27 import org.orekit.bodies.GeodeticPoint;
28 import org.orekit.bodies.OneAxisEllipsoid;
29 import org.orekit.bodies.TimeStampedGeodeticPoint;
30 import org.orekit.frames.Frame;
31 import org.orekit.frames.FramesFactory;
32 import org.orekit.frames.KinematicTransform;
33 import org.orekit.models.earth.ReferenceEllipsoid;
34 import org.orekit.orbits.CartesianOrbit;
35 import org.orekit.orbits.Orbit;
36 import org.orekit.propagation.analytical.KeplerianPropagator;
37 import org.orekit.time.AbsoluteDate;
38 import org.orekit.utils.Constants;
39 import org.orekit.utils.PVCoordinates;
40 import org.orekit.utils.TimeStampedPVCoordinates;
41
42 import static org.junit.jupiter.api.Assertions.*;
43
44 class LambertSolverTest {
45
46 @Test
47 void testSolveFailure() {
48
49 final LambertSolver solver = new LambertSolver(1.);
50 final AbsoluteDate date = AbsoluteDate.ARBITRARY_EPOCH;
51 final LambertBoundaryConditions boundaryConditions = new LambertBoundaryConditions(date,
52 Vector3D.PLUS_I, date.shiftedBy(1), Vector3D.PLUS_J, FramesFactory.getEME2000());
53
54 final LambertBoundaryVelocities solution = solver.solve(true, 10, boundaryConditions);
55 final RealMatrix jacobian = solver.computeJacobian(true, 10, boundaryConditions);
56
57 assertNull(solution);
58 assertTrue(Double.isNaN(jacobian.getEntry(0,0)));
59 }
60
61 @ParameterizedTest
62 @ValueSource(doubles = {-1e4, 1e5, 1e6, 1e7})
63 void testSolveGeoTimes(final double timeOfFlight) {
64 testSolveGeo(timeOfFlight, 0.1, true);
65 }
66
67 @ParameterizedTest
68 @ValueSource(doubles = {0.1, 0.2, 0.3})
69 void testSolveGeoLongitude(final double longitude) {
70 testSolveGeo(Constants.JULIAN_DAY * 1.5, longitude, true);
71 }
72
73 @ParameterizedTest
74 @ValueSource(booleans = {true, false})
75 void testSolveGeoDirection(final boolean posigrade) {
76 testSolveGeo(1e8, 0.1, posigrade);
77 }
78
79 private void testSolveGeo(final double timeOfFlight, final double longitude2, final boolean posigrade) {
80
81 final AbsoluteDate date = AbsoluteDate.ARBITRARY_EPOCH;
82 final double latitude = 0.;
83 final double altitude = 42157e6 - 6378136.;
84 final OneAxisEllipsoid ellipsoid = ReferenceEllipsoid.getWgs84(FramesFactory.getGTOD(false));
85 final TimeStampedGeodeticPoint point1 = new TimeStampedGeodeticPoint(date, new GeodeticPoint(latitude, 0., altitude));
86 final Frame frame = FramesFactory.getGCRF();
87 final Orbit orbit1 = buildOrbitFromGeodetic(ellipsoid, point1, frame);
88 final TimeStampedGeodeticPoint point2 = new TimeStampedGeodeticPoint(date.shiftedBy(timeOfFlight),
89 new GeodeticPoint(latitude, longitude2, altitude));
90 final Orbit orbit2 = buildOrbitFromGeodetic(ellipsoid, point2, frame);
91 final LambertSolver solver = new LambertSolver(orbit2.getMu());
92 final int nRev = (int) FastMath.floor(timeOfFlight / orbit1.getKeplerianPeriod());
93
94 final LambertBoundaryConditions boundaryConditions = new LambertBoundaryConditions(orbit1.getDate(),
95 orbit1.getPosition(), orbit2.getDate(), orbit2.getPosition(), orbit2.getFrame());
96 final LambertBoundaryVelocities solution = solver.solve(posigrade, nRev, boundaryConditions);
97
98 final Orbit lambertOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(orbit1.getDate(), orbit1.getPosition(), solution.getInitialVelocity()),
99 orbit1.getFrame(), orbit1.getMu());
100 final KeplerianPropagator propagator = new KeplerianPropagator(lambertOrbit);
101 final Orbit propagatedOrbit = propagator.propagateOrbit(orbit2.getDate());
102 assertArrayEquals(orbit2.getPosition().toArray(), propagatedOrbit.getPosition().toArray(), 1.);
103 }
104
105 private static Orbit buildOrbitFromGeodetic(final OneAxisEllipsoid ellipsoid,
106 final TimeStampedGeodeticPoint geodeticPoint,
107 final Frame frame) {
108 final Vector3D position = ellipsoid.transform(geodeticPoint);
109 final KinematicTransform transform = ellipsoid.getBodyFrame().getKinematicTransformTo(frame,
110 geodeticPoint.getDate());
111 final PVCoordinates pvCoordinates = transform.transformOnlyPV(new PVCoordinates(position));
112 return new CartesianOrbit(pvCoordinates, frame, geodeticPoint.getDate(), Constants.EGM96_EARTH_MU);
113 }
114
115 @Test
116 void testSolveJacobian() {
117
118 final AbsoluteDate date = AbsoluteDate.ARBITRARY_EPOCH;
119 final Frame frame = FramesFactory.getGCRF();
120 final double mu = Constants.EGM96_EARTH_MU;
121 final Orbit orbit1 = new CartesianOrbit(new TimeStampedPVCoordinates(date, new Vector3D(7.557249611265823E9, -4.1474095848830185E10, -958845.3227159644),
122 new Vector3D(3024339.1189172766, 551083.3948309845, 96.84004762824829)), frame, mu);
123 final double timeOfFlight = Constants.JULIAN_DAY * 1.5;
124 final Orbit orbit2 = new CartesianOrbit(new TimeStampedPVCoordinates(date.shiftedBy(timeOfFlight),
125 new Vector3D(-1.6651012438926659E10, 3.8729271015852745E10, 650886.2988349741),
126 new Vector3D(-2824183.311624355, -1214211.1159693908, -108.81575040466626)), frame, mu);
127 final LambertSolver solver = new LambertSolver(orbit2.getMu());
128 final int nRev = (int) FastMath.floor(timeOfFlight / orbit1.getKeplerianPeriod());
129 final boolean posigrade = true;
130
131 final LambertBoundaryConditions boundaryConditions = new LambertBoundaryConditions(orbit1.getDate(),
132 orbit1.getPosition(), orbit2.getDate(), orbit2.getPosition(), orbit2.getFrame());
133 final RealMatrix jacobian = solver.computeJacobian(posigrade, nRev, boundaryConditions);
134
135 assertFalse(Double.isNaN(jacobian.getEntry(0, 0)));
136 checkJacobianWithFiniteDifferences(solver, posigrade, nRev, boundaryConditions, jacobian);
137 }
138
139 private static void checkJacobianWithFiniteDifferences(final LambertSolver solver, final boolean posigrade,
140 final int nRev,
141 final LambertBoundaryConditions boundaryConditions,
142 final RealMatrix actualJacobian) {
143 checkPartialDerivativesWrtTimes(solver, posigrade, nRev, boundaryConditions, actualJacobian);
144 final double dV = 1e-1;
145 final double toleranceForVelocity = 1e-7;
146 for (int i = 1; i < 4; i++) {
147 final double[] shift = new double[8];
148 shift[i] = -dV/2.;
149 final LambertBoundaryVelocities solutionBefore = solver.solve(posigrade, nRev, perturbConditions(boundaryConditions, shift));
150 shift[i] = dV/2.;
151 final LambertBoundaryVelocities solutionAfter = solver.solve(posigrade, nRev, perturbConditions(boundaryConditions, shift));
152 checkColumn(solutionBefore, solutionAfter, dV, actualJacobian.getColumn(i), toleranceForVelocity);
153 }
154 for (int i = 5; i < 8; i++) {
155 final double[] shift = new double[8];
156 shift[i] = -dV/2.;
157 final LambertBoundaryVelocities solutionBefore = solver.solve(posigrade, nRev, perturbConditions(boundaryConditions, shift));
158 shift[i] = dV/2.;
159 final LambertBoundaryVelocities solutionAfter = solver.solve(posigrade, nRev, perturbConditions(boundaryConditions, shift));
160 checkColumn(solutionBefore, solutionAfter, dV, actualJacobian.getColumn(i), toleranceForVelocity);
161 }
162 }
163
164 private static void checkPartialDerivativesWrtTimes(final LambertSolver solver, final boolean posigrade,
165 final int nRev,
166 final LambertBoundaryConditions boundaryConditions,
167 final RealMatrix actualJacobian) {
168 final double dt = 1.;
169 final double toleranceForTime = 1e-6;
170 final LambertBoundaryVelocities solution0Before = solver.solve(posigrade, nRev,
171 perturbConditions(boundaryConditions, new double[] {-dt/2., 0., 0., 0., 0., 0., 0., 0.}));
172 final LambertBoundaryVelocities solution0After = solver.solve(posigrade, nRev,
173 perturbConditions(boundaryConditions, new double[] {dt/2., 0., 0., 0., 0., 0., 0., 0.}));
174 checkColumn(solution0Before, solution0After, dt, actualJacobian.getColumn(0), toleranceForTime);
175 final LambertBoundaryVelocities solution7Before = solver.solve(posigrade, nRev,
176 perturbConditions(boundaryConditions, new double[] {0., 0., 0., 0., -dt/2., 0., 0., 0.}));
177 final LambertBoundaryVelocities solution7After = solver.solve(posigrade, nRev,
178 perturbConditions(boundaryConditions, new double[] {0., 0., 0., 0., dt/2.,0., 0., 0.}));
179 checkColumn(solution7Before, solution7After, dt, actualJacobian.getColumn(4), toleranceForTime);
180 }
181
182 private static void checkColumn(final LambertBoundaryVelocities solutionBefore,
183 final LambertBoundaryVelocities solutionAfter,
184 final double dP, final double[] actualColumn, final double tolerance) {
185 final double[] difference = new double[actualColumn.length];
186 final Vector3D differenceInitial = solutionAfter.getInitialVelocity().subtract(solutionBefore.getInitialVelocity());
187 final Vector3D differenceTerminal = solutionAfter.getTerminalVelocity().subtract(solutionBefore.getTerminalVelocity());
188 difference[0] = differenceInitial.getX() / dP;
189 difference[1] = differenceInitial.getY() / dP;
190 difference[2] = differenceInitial.getZ() / dP;
191 difference[3] = differenceTerminal.getX() /dP;
192 difference[4] = differenceTerminal.getY() / dP;
193 difference[5] = differenceTerminal.getZ() / dP;
194 assertArrayEquals(difference, actualColumn, tolerance);
195 }
196
197 private static LambertBoundaryConditions perturbConditions(final LambertBoundaryConditions conditions,
198 final double[] shift) {
199 return new LambertBoundaryConditions(conditions.getInitialDate().shiftedBy(shift[0]),
200 conditions.getInitialPosition().add(new Vector3D(shift[1], shift[2], shift[3])),
201 conditions.getTerminalDate().shiftedBy(shift[4]),
202 conditions.getTerminalPosition().add(new Vector3D(shift[5], shift[6], shift[7])),
203 conditions.getReferenceFrame());
204 }
205
206 @BeforeEach
207 void setUp() {
208 Utils.setDataRoot("regular-data");
209 }
210
211 }