1 /* Copyright 2002-2026 CS GROUP
2 * Licensed to CS GROUP (CS) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * CS licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.orekit.propagation.relative.yamanakaankersen;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
21 import org.hipparchus.linear.FieldMatrix;
22 import org.hipparchus.linear.FieldVector;
23 import org.hipparchus.linear.MatrixUtils;
24 import org.hipparchus.util.FastMath;
25 import org.hipparchus.util.FieldSinCos;
26 import org.hipparchus.util.MathArrays;
27 import org.orekit.utils.FieldPVCoordinates;
28 import org.orekit.utils.TimeStampedFieldPVCoordinates;
29
30 /**
31 * This class stores the 2 matrices (in plane: xz, out of plane: y, in local LVLH CCSDS frame) of the Yamanaka Ankersen
32 * equations. Matrices in LVLH CCSDS Frame.
33 *
34 * @param <T> Any scalar field.
35 * @author Romain Cuvillon
36 * @since 14.0
37 */
38 public class FieldYamanakaAnkersenMatrices<T extends CalculusFieldElement<T>> {
39
40 /**
41 * Time since epoch used in computing the matrices in seconds.
42 */
43 private final T timeSinceEpoch;
44
45 /**
46 * True anomaly of the target spacecraft.
47 */
48
49 private final T targetTheta;
50
51 /**
52 * Matrix to compute in-plane (x-z) components of position and velocity from initial in-plane components.
53 */
54
55 private final FieldMatrix<T> inPlaneMatrix;
56
57 /**
58 * Matrix to compute out of plane (y) components of position and velocity from initial out of plane components.
59 */
60
61 private final FieldMatrix<T> outPlaneMatrix;
62
63 /**
64 * Constructor for the state transition matrices given by the Yamanaka-Ankersen equations.
65 *
66 * @param timeSinceEpoch duration in seconds since epoch
67 * @param targetTheta current true anomaly of the target
68 * @param inPlaneMatrix in-plane transition matrix
69 * @param outPlaneMatrix out-plane transition matrix
70 */
71 public FieldYamanakaAnkersenMatrices(final T timeSinceEpoch, final T targetTheta,
72 final FieldMatrix<T> inPlaneMatrix, final FieldMatrix<T> outPlaneMatrix) {
73 this.timeSinceEpoch = timeSinceEpoch;
74 this.targetTheta = targetTheta;
75 this.inPlaneMatrix = inPlaneMatrix;
76 this.outPlaneMatrix = outPlaneMatrix;
77 }
78
79 /**
80 * Get the true anomaly of the target.
81 *
82 * @return true anomaly of the target
83 */
84 public T getTargetTrueAnomaly() {
85 return targetTheta;
86 }
87
88 /**
89 * Get the in-plane matrix at current time.
90 *
91 * @return In-plane matrix at current time
92 */
93 public FieldMatrix<T> getInPlaneMatrix() {
94 return inPlaneMatrix;
95 }
96
97 /**
98 * Get the out of plane matrix at current time.
99 *
100 * @return out of plane matrix at current time
101 */
102 public FieldMatrix<T> getOutPlaneMatrix() {
103 return outPlaneMatrix;
104 }
105
106 /**
107 * Transforms the input initial PVT expressed in the target's LVLH CCSDS LOF to the PVT at the time and true anomaly
108 * associated encoded in the Yamanaka-Ankersen matrices contained in the object. First transform LOF PV coordinates
109 * to ~ coordinates system as presented in the Yamanaka Ankersen paper. Compute actualized coordinates in the ~
110 * system Retransform from transformed frame to LOF to get desired PVCoordinates in LOF.
111 *
112 * @param pvt input initial PVT expressed in the target's LVLH CCSDS LOF
113 * @param initialTrueAnomaly true anomaly of the target Spacecraft at initial time
114 * @param trueAnomaly true anomaly of the target Spacecraft at final time
115 * @param eccentricity eccentricity of the target Orbit
116 * @param sma semi-major axis of the target Orbit
117 * @param mu mu of the target orbit
118 * @return PVT at the time encoded in the Yamanaka-Ankersen matrices, expressed in the target's LVLH CCSDS LOF
119 */
120 public TimeStampedFieldPVCoordinates<T> transform(final TimeStampedFieldPVCoordinates<T> pvt,
121 final T initialTrueAnomaly, final T trueAnomaly,
122 final T eccentricity, final T sma, final T mu) {
123 return new TimeStampedFieldPVCoordinates<>(pvt.getDate().shiftedBy(timeSinceEpoch),
124 transform(new FieldPVCoordinates<>(pvt.getPosition(),
125 pvt.getVelocity()),
126 initialTrueAnomaly, trueAnomaly, eccentricity, sma, mu));
127 }
128
129 /**
130 * Transforms the input initial PVT expressed in the target's LVLH CCSDS LOF to the PVT at the time and true anomaly
131 * associated encoded in the Yamanaka-Ankersen matrices contained in the object. First transform LOF PV coordinates
132 * to ~ coordinates system as presented in the Yamanaka Ankersen paper. Compute actualized coordinates in the ~
133 * system Retransform from transformed frame to LOF to get desired PVCoordinates in LOF.
134 *
135 * @param pv input initial PV expressed in the target's LVLH CCSDS LOF
136 * @param initialTrueAnomaly true anomaly of the target Spacecraft at initial time
137 * @param trueAnomaly true anomaly of the target Spacecraft at final time
138 * @param eccentricity eccentricity of the target Orbit
139 * @param sma semi-major axis of the target Orbit
140 * @param mu mu of the target orbit
141 * @return PVT at the time encoded in the Yamanaka-Ankersen matrices, expressed in the target's LVLH CCSDS LOF
142 */
143 public FieldPVCoordinates<T> transform(final FieldPVCoordinates<T> pv, final T initialTrueAnomaly,
144 final T trueAnomaly, final T eccentricity, final T sma, final T mu) {
145
146 // Compute constant k2
147 final T p = sma.multiply(sma.getField().getOne().subtract(eccentricity.pow(2)));
148 final T k2 = FastMath.sqrt(mu.divide(p.pow(3)));
149
150 // Pre-compute quantities
151 final FieldSinCos<T> scNu0 = initialTrueAnomaly.sinCos();
152 final T rho0 = eccentricity.multiply(scNu0.cos()).add(1);
153 final FieldSinCos<T> scNu = trueAnomaly.sinCos();
154 final T rho = eccentricity.multiply(scNu.cos()).add(1);
155
156 // Convert Vector3D to RealMatrix
157 final FieldMatrix<T> pos0 = MatrixUtils.createColumnFieldMatrix(pv.getPosition().toArray());
158 final FieldMatrix<T> vel0 = MatrixUtils.createColumnFieldMatrix(pv.getVelocity().toArray());
159
160 // Transform coordinates in the ~ (tilde) coordinate system
161 final FieldMatrix<T> pos_tilde0 = pos0.scalarMultiply(rho0);
162 final FieldMatrix<T> vel_tilde0 =
163 pos0.scalarMultiply(eccentricity.multiply(-1.).multiply(scNu0.sin()))
164 .add(vel0.scalarMultiply(sma.getField().getOne().divide(k2.multiply(rho0))));
165
166 final T[] xz_coord0 = MathArrays.buildArray(trueAnomaly.getField(), 4);
167 xz_coord0[0] = pos_tilde0.getEntry(0, 0);
168 xz_coord0[1] = pos_tilde0.getEntry(2, 0);
169 xz_coord0[2] = vel_tilde0.getEntry(0, 0);
170 xz_coord0[3] = vel_tilde0.getEntry(2, 0);
171 final T[] y_coord0 = MathArrays.buildArray(trueAnomaly.getField(), 2);
172 y_coord0[0] = pos_tilde0.getEntry(1, 0);
173 y_coord0[1] = vel_tilde0.getEntry(1, 0);
174
175 // compute at time t and theta
176 final FieldVector<T> xz_coord = inPlaneMatrix.operate(MatrixUtils.createFieldVector(xz_coord0));
177 final FieldVector<T> y_coord = outPlaneMatrix.operate(MatrixUtils.createFieldVector(y_coord0));
178
179 // Reshape the vectors
180 final T[] array_pos = MathArrays.buildArray(trueAnomaly.getField(), 3);
181 array_pos[0] = xz_coord.getEntry(0);
182 array_pos[1] = y_coord.getEntry(0);
183 array_pos[2] = xz_coord.getEntry(1);
184
185 final T[] array_vel = MathArrays.buildArray(trueAnomaly.getField(), 3);
186 array_vel[0] = xz_coord.getEntry(2);
187 array_vel[1] = y_coord.getEntry(1);
188 array_vel[2] = xz_coord.getEntry(3);
189
190 final FieldVector<T> pos_tilde = MatrixUtils.createFieldVector(array_pos);
191 final FieldVector<T> vel_tilde = MatrixUtils.createFieldVector(array_vel);
192
193 final FieldVector<T> pos = pos_tilde.mapDivide(rho);
194 final FieldVector<T> vel = pos_tilde.mapMultiply(k2.multiply(eccentricity).multiply(scNu.sin()))
195 .add(vel_tilde.mapMultiply(k2.multiply(rho)));
196
197 final FieldVector3D<T> pos1 = new FieldVector3D<>(pos.toArray());
198 final FieldVector3D<T> vel1 = new FieldVector3D<>(vel.toArray());
199
200 // Return transformed PV
201 return new FieldPVCoordinates<>(pos1, vel1);
202 }
203 }