1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.control.indirect.shooting;
18
19 import org.hipparchus.analysis.differentiation.Gradient;
20 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
21 import org.hipparchus.geometry.euclidean.threed.Vector3D;
22 import org.hipparchus.linear.DecompositionSolver;
23 import org.hipparchus.linear.LUDecomposition;
24 import org.hipparchus.linear.MatrixUtils;
25 import org.hipparchus.linear.RealMatrix;
26 import org.hipparchus.linear.RealVector;
27 import org.hipparchus.util.FastMath;
28 import org.orekit.control.indirect.shooting.boundary.CartesianBoundaryConditionChecker;
29 import org.orekit.control.indirect.shooting.boundary.FixedTimeBoundaryOrbits;
30 import org.orekit.control.indirect.shooting.boundary.FixedTimeCartesianBoundaryStates;
31 import org.orekit.control.indirect.shooting.propagation.ShootingPropagationSettings;
32 import org.orekit.propagation.FieldSpacecraftState;
33 import org.orekit.utils.FieldPVCoordinates;
34
35
36
37
38
39
40
41
42 public class NewtonFixedBoundaryCartesianSingleShooting extends AbstractFixedBoundaryCartesianSingleShooting {
43
44
45 private static final double DEFAULT_SINGULARITY_THRESHOLD = 1e-11;
46
47
48 private double singularityThreshold = DEFAULT_SINGULARITY_THRESHOLD;
49
50
51 private double stepFactor = 1.;
52
53
54
55
56
57
58
59 public NewtonFixedBoundaryCartesianSingleShooting(final ShootingPropagationSettings propagationSettings,
60 final FixedTimeCartesianBoundaryStates boundaryConditions,
61 final CartesianBoundaryConditionChecker convergenceChecker) {
62 super(propagationSettings, boundaryConditions, convergenceChecker);
63 }
64
65
66
67
68
69
70
71 public NewtonFixedBoundaryCartesianSingleShooting(final ShootingPropagationSettings propagationSettings,
72 final FixedTimeBoundaryOrbits boundaryConditions,
73 final CartesianBoundaryConditionChecker convergenceChecker) {
74 super(propagationSettings, boundaryConditions, convergenceChecker);
75 }
76
77 @Override
78 public int getMaximumIterationCount() {
79 return getConditionChecker().getMaximumIterationCount();
80 }
81
82
83
84
85
86
87 public void setSingularityThreshold(final double singularityThreshold) {
88 this.singularityThreshold = singularityThreshold;
89 }
90
91
92
93
94
95
96 public double getSingularityThreshold() {
97 return singularityThreshold;
98 }
99
100
101
102
103
104
105 public void setStepFactor(final double stepFactor) {
106 this.stepFactor = FastMath.abs(stepFactor);
107 }
108
109
110 @Override
111 protected double[] updateShootingVariables(final double[] originalShootingVariables,
112 final FieldSpacecraftState<Gradient> fieldTerminalState) {
113
114 final double[] defects = new double[originalShootingVariables.length];
115 final double[][] defectsJacobianData = new double[defects.length][defects.length];
116 final double reciprocalScalePosition = 1. / getScalePositionDefects();
117 final double reciprocalScaleVelocity = 1. / getScaleVelocityDefects();
118 final FieldPVCoordinates<Gradient> terminalPV = fieldTerminalState.getPVCoordinates();
119 final FieldVector3D<Gradient> fieldScaledTerminalPosition = terminalPV.getPosition().scalarMultiply(reciprocalScalePosition);
120 final FieldVector3D<Gradient> fieldScaledTerminalVelocity = terminalPV.getVelocity().scalarMultiply(reciprocalScaleVelocity);
121 final Vector3D terminalScaledPosition = fieldScaledTerminalPosition.toVector3D();
122 final Vector3D terminalScaledVelocity = fieldScaledTerminalVelocity.toVector3D();
123 final Vector3D targetScaledPosition = getTerminalCartesianState().getPosition().scalarMultiply(reciprocalScalePosition);
124 final Vector3D targetScaledVelocity = getTerminalCartesianState().getVelocity().scalarMultiply(reciprocalScaleVelocity);
125 defects[0] = terminalScaledPosition.getX() - targetScaledPosition.getX();
126 defectsJacobianData[0] = fieldScaledTerminalPosition.getX().getGradient();
127 defects[1] = terminalScaledPosition.getY() - targetScaledPosition.getY();
128 defectsJacobianData[1] = fieldScaledTerminalPosition.getY().getGradient();
129 defects[2] = terminalScaledPosition.getZ() - targetScaledPosition.getZ();
130 defectsJacobianData[2] = fieldScaledTerminalPosition.getZ().getGradient();
131 defects[3] = terminalScaledVelocity.getX() - targetScaledVelocity.getX();
132 defectsJacobianData[3] = fieldScaledTerminalVelocity.getX().getGradient();
133 defects[4] = terminalScaledVelocity.getY() - targetScaledVelocity.getY();
134 defectsJacobianData[4] = fieldScaledTerminalVelocity.getY().getGradient();
135 defects[5] = terminalScaledVelocity.getZ() - targetScaledVelocity.getZ();
136 defectsJacobianData[5] = fieldScaledTerminalVelocity.getZ().getGradient();
137 if (originalShootingVariables.length != 6) {
138 final String adjointName = getPropagationSettings().getAdjointDynamicsProvider().getAdjointName();
139 final Gradient terminalMassAdjoint = fieldTerminalState.getAdditionalState(adjointName)[6];
140 defects[6] = terminalMassAdjoint.getValue();
141 defectsJacobianData[6] = terminalMassAdjoint.getGradient();
142 }
143
144 final double[] correction = computeCorrection(defects, defectsJacobianData);
145 final double[] correctedAdjoint = originalShootingVariables.clone();
146 for (int i = 0; i < correction.length; i++) {
147 correctedAdjoint[i] += correction[i];
148 }
149 return correctedAdjoint;
150 }
151
152
153
154
155
156
157
158 private double[] computeCorrection(final double[] defects, final double[][] defectsJacobianData) {
159 final RealMatrix defectsJacobian = MatrixUtils.createRealMatrix(defectsJacobianData);
160 final DecompositionSolver solver = new LUDecomposition(defectsJacobian, singularityThreshold).getSolver();
161 final RealVector negatedDefects = MatrixUtils.createRealVector(defects).mapMultiply(-1);
162 final RealVector solved = solver.solve(negatedDefects);
163 final double[] corrections = new double[solved.getDimension()];
164 for (int i = 0; i < corrections.length; i++) {
165 corrections[i] = solved.getEntry(i) * getScales()[i] * stepFactor;
166 }
167 return corrections;
168 }
169 }