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
18 package org.orekit.propagation.relative.yamanakaankersen;
19
20 import org.hipparchus.geometry.euclidean.threed.Vector3D;
21 import org.hipparchus.linear.MatrixUtils;
22 import org.hipparchus.linear.RealMatrix;
23 import org.hipparchus.linear.RealVector;
24 import org.hipparchus.util.FastMath;
25 import org.hipparchus.util.SinCos;
26 import org.orekit.utils.PVCoordinates;
27 import org.orekit.utils.TimeStampedPVCoordinates;
28
29 /**
30 * This class stores the 2 matrices (in plane: xz, out of plane: y, in Local LVLH CCSDS frame) of the Yamanaka Ankersen
31 * equations. Matrices in LVLH CCSDS Frame.
32 *
33 * @author Romain Cuvillon
34 * @since 14.0
35 */
36 public class YamanakaAnkersenMatrices {
37
38 /**
39 * Time since epoch used in computing the matrices in seconds.
40 */
41 private final double timeSinceEpoch;
42
43 /**
44 * True anomaly of the target spacecraft.
45 */
46
47 private final double targetTheta;
48
49 /**
50 * Matrix to compute in-plane (x-z) components of position and velocity from initial in-plane components.
51 */
52
53 private final RealMatrix inPlaneMatrix;
54
55 /**
56 * Matrix to compute out of plane (y) components of position and velocity from initial out of plane components.
57 */
58
59 private final RealMatrix outPlaneMatrix;
60
61 /**
62 * Constructor for the state transition matrices given by the Yamanaka-Ankersen equations.
63 *
64 * @param timeSinceEpoch duration in seconds since epoch
65 * @param targetTheta true anomaly of the target
66 * @param inPlaneMatrix in-plane transition matrix
67 * @param outPlaneMatrix out-plane transition matrix
68 */
69 public YamanakaAnkersenMatrices(final double timeSinceEpoch, final double targetTheta,
70 final RealMatrix inPlaneMatrix, final RealMatrix outPlaneMatrix) {
71 this.timeSinceEpoch = timeSinceEpoch;
72 this.targetTheta = targetTheta;
73 this.inPlaneMatrix = inPlaneMatrix;
74 this.outPlaneMatrix = outPlaneMatrix;
75 }
76
77 /**
78 * Get the true anomaly of the target.
79 *
80 * @return true anomaly of the target
81 */
82 public double getTargetTrueAnomaly() {
83 return targetTheta;
84 }
85
86 /**
87 * Get the in-plane matrix at current time.
88 *
89 * @return In-plane matrix at current time
90 */
91 public RealMatrix getInPlaneMatrix() {
92 return inPlaneMatrix;
93 }
94
95 /**
96 * Get the out of plane matrix at current time.
97 *
98 * @return Out of plane matrix at current time
99 */
100 public RealMatrix getOutPlaneMatrix() {
101 return outPlaneMatrix;
102 }
103
104 /**
105 * Transforms the input initial PVT expressed in the target's LVLH CCSDS LOF to the PVT at the time and true anomaly
106 * associated encoded in the Yamanaka-Ankersen matrices contained in the object. First transform LOF PV coordinates
107 * to ~ coordinates system as presented in the Yamanaka Ankersen paper. Compute actualized coordinates in the ~
108 * system Retransform from transformed frame to LOF to get desired PVCoordinates in LOF.
109 *
110 * @param pvt input initial PVT expressed in the target's LVLH CCSDS LOF
111 * @param initialTrueAnomaly true anomaly of the target Spacecraft at initial time
112 * @param trueAnomaly true anomaly of the target Spacecraft at final time
113 * @param eccentricity eccentricity of the target Orbit
114 * @param sma semi-major axis of the target Orbit
115 * @param mu mu of the target orbit
116 * @return PVT at the time encoded in the Yamanaka-Ankersen matrices, expressed in the target's LVLH CCSDS LOF.
117 */
118 public TimeStampedPVCoordinates transform(final TimeStampedPVCoordinates pvt, final double initialTrueAnomaly,
119 final double trueAnomaly, final double eccentricity, final double sma,
120 final double mu) {
121 return new TimeStampedPVCoordinates(pvt.getDate().shiftedBy(timeSinceEpoch),
122 transform(new PVCoordinates(pvt.getPosition(), pvt.getVelocity()),
123 initialTrueAnomaly, trueAnomaly, eccentricity, sma, mu));
124 }
125
126 /**
127 * Transforms the input initial PVT expressed in the target's LVLH CCSDS LOF to the PVT at the time and true anomaly
128 * associated encoded in the Yamanaka-Ankersen matrices contained in the object. First transform LOF PV coordinates
129 * to ~ coordinates system as presented in the Yamanaka Ankersen paper. Compute actualized coordinates in the ~
130 * system Retransform from transformed frame to LOF to get desired PVCoordinates in LOF.
131 *
132 * @param pv input initial PV expressed in the target's LVLH CCSDS LOF.
133 * @param initialTrueAnomaly true anomaly of the target Spacecraft at initial time.
134 * @param trueAnomaly true anomaly of the target Spacecraft at final time.
135 * @param eccentricity eccentricity of the target Orbit.
136 * @param sma semi-major axis of the target Orbit.
137 * @param mu mu of the target orbit.
138 * @return PVT at the time encoded in the Yamanaka-Ankersen matrices, expressed in the target's LVLH CCSDS LOF.
139 */
140 public PVCoordinates transform(final PVCoordinates pv, final double initialTrueAnomaly, final double trueAnomaly,
141 final double eccentricity, final double sma, final double mu) {
142 // Compute constant k2
143 final double p = sma * (1 - eccentricity * eccentricity);
144 final double k2 = FastMath.sqrt(mu / (p * p * p));
145
146 // Pre-compute quantities
147 final SinCos scNu0 = FastMath.sinCos(initialTrueAnomaly);
148 final double rho0 = 1 + eccentricity * scNu0.cos();
149 final SinCos scNu = FastMath.sinCos(trueAnomaly);
150 final double rho = 1 + eccentricity * scNu.cos();
151
152 // Convert Vector3D to RealMatrix
153 final RealMatrix pos0 = MatrixUtils.createColumnRealMatrix(pv.getPosition().toArray());
154 final RealMatrix vel0 = MatrixUtils.createColumnRealMatrix(pv.getVelocity().toArray());
155
156 // Transform coordinates in the ~ (tilde) coordinate system
157 final RealMatrix pos_tilde0 = pos0.scalarMultiply(rho0);
158 final RealMatrix vel_tilde0 = pos0.scalarMultiply(-eccentricity * scNu0.sin())
159 .add(vel0.scalarMultiply(1 / (k2 * rho0)));
160
161 // Reform the vectors [x,z,vx,vz] and [y,vy]
162 final double[] xz_coord0 = {
163 pos_tilde0.getEntry(0, 0), pos_tilde0.getEntry(2, 0), vel_tilde0.getEntry(0, 0),
164 vel_tilde0.getEntry(2, 0)
165 };
166 final double[] y_coord0 = {pos_tilde0.getEntry(1, 0), vel_tilde0.getEntry(1, 0)};
167
168 // compute at time t and theta
169 final RealVector xz_coord = inPlaneMatrix.operate(MatrixUtils.createRealVector(xz_coord0));
170 final RealVector y_coord = outPlaneMatrix.operate(MatrixUtils.createRealVector(y_coord0));
171
172 // Reshape the vectors
173 final RealVector pos_tilde = MatrixUtils.createRealVector(
174 new double[] {xz_coord.getEntry(0), y_coord.getEntry(0), xz_coord.getEntry(1)});
175 final RealVector vel_tilde = MatrixUtils.createRealVector(
176 new double[] {xz_coord.getEntry(2), y_coord.getEntry(1), xz_coord.getEntry(3)});
177
178 // Compute final vectors in LOF
179 final RealVector pos = pos_tilde.mapDivide(rho);
180 final RealVector vel = pos_tilde.mapMultiply(k2 * eccentricity * scNu.sin())
181 .add(vel_tilde.mapMultiply(k2 * rho));
182 final Vector3D pos1 = new Vector3D(pos.toArray());
183 final Vector3D vel1 = new Vector3D(vel.toArray());
184
185 // Return transformed PV
186 return new PVCoordinates(pos1, vel1);
187 }
188 }
189