1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.gnss.attitude;
18
19 import org.hipparchus.analysis.UnivariateFunction;
20 import org.hipparchus.analysis.differentiation.DSFactory;
21 import org.hipparchus.analysis.differentiation.DerivativeStructure;
22 import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
23 import org.hipparchus.analysis.solvers.UnivariateSolverUtils;
24 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
25 import org.hipparchus.geometry.euclidean.threed.Vector3D;
26 import org.hipparchus.util.FastMath;
27 import org.hipparchus.util.SinCos;
28 import org.orekit.frames.Frame;
29 import org.orekit.frames.LOFType;
30 import org.orekit.frames.Transform;
31 import org.orekit.time.AbsoluteDate;
32 import org.orekit.time.FieldAbsoluteDate;
33 import org.orekit.time.TimeStamped;
34 import org.orekit.utils.ExtendedPVCoordinatesProvider;
35 import org.orekit.utils.FieldPVCoordinates;
36 import org.orekit.utils.PVCoordinates;
37 import org.orekit.utils.PVCoordinatesProvider;
38 import org.orekit.utils.TimeStampedAngularCoordinates;
39 import org.orekit.utils.TimeStampedPVCoordinates;
40
41
42
43
44
45
46
47
48
49
50
51
52
53 class GNSSAttitudeContext implements TimeStamped {
54
55
56 private static final int ORDER = 2;
57
58
59 private static final DSFactory FACTORY = new DSFactory(1, ORDER);
60
61
62 private static final PVCoordinates PLUS_Y_PV =
63 new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO);
64
65
66 private static final PVCoordinates MINUS_Z_PV =
67 new PVCoordinates(Vector3D.MINUS_K, Vector3D.ZERO, Vector3D.ZERO);
68
69
70 private static final double BETA_SIGN_CHANGE_PROTECTION = FastMath.toRadians(0.07);
71
72
73 private final AbsoluteDate date;
74
75
76 private final FieldAbsoluteDate<DerivativeStructure> dateDS;
77
78
79 private final ExtendedPVCoordinatesProvider sun;
80
81
82 private final PVCoordinatesProvider pvProv;
83
84
85 private final Frame inertialFrame;
86
87
88 private final DerivativeStructure svbCos;
89
90
91 private final boolean morning;
92
93
94 private final DerivativeStructure delta;
95
96
97 private double muRate;
98
99
100 private double cNight;
101
102
103 private double cNoon;
104
105
106 private TurnSpan turnSpan;
107
108
109
110
111
112
113
114
115
116 GNSSAttitudeContext(final AbsoluteDate date,
117 final ExtendedPVCoordinatesProvider sun, final PVCoordinatesProvider pvProv,
118 final Frame inertialFrame, final TurnSpan turnSpan) {
119
120 this.date = date;
121 this.dateDS = addDerivatives(date);
122 this.sun = sun;
123 this.pvProv = pvProv;
124 this.inertialFrame = inertialFrame;
125 final FieldPVCoordinates<DerivativeStructure> sunPVDS = sun.getPVCoordinates(dateDS, inertialFrame);
126
127 final TimeStampedPVCoordinates svPV = pvProv.getPVCoordinates(date, inertialFrame);
128 final FieldPVCoordinates<DerivativeStructure> svPVDS = svPV.toDerivativeStructurePV(FACTORY.getCompiler().getOrder());
129 this.svbCos = FieldVector3D.dotProduct(sunPVDS.getPosition(), svPVDS.getPosition()).
130 divide(sunPVDS.getPosition().getNorm().multiply(svPVDS.getPosition().getNorm()));
131 this.morning = Vector3D.dotProduct(svPV.getVelocity(), sunPVDS.getPosition().toVector3D()) >= 0.0;
132
133 this.muRate = svPV.getAngularVelocity().getNorm();
134
135 this.turnSpan = turnSpan;
136
137 final DerivativeStructure absDelta;
138 if (svbCos.getValue() <= 0) {
139
140 absDelta = inOrbitPlaneAbsoluteAngle(svbCos.acos().negate().add(FastMath.PI));
141 } else {
142
143 absDelta = inOrbitPlaneAbsoluteAngle(svbCos.acos());
144 }
145 delta = FastMath.copySign(absDelta, -absDelta.getPartialDerivative(1));
146
147 }
148
149
150
151
152
153 private AbsoluteDate removeDerivatives(final FieldAbsoluteDate<DerivativeStructure> d) {
154 return d.toAbsoluteDate();
155 }
156
157
158
159
160
161 private FieldAbsoluteDate<DerivativeStructure> addDerivatives(final AbsoluteDate d) {
162 return new FieldAbsoluteDate<>(FACTORY.getDerivativeField(), d).
163 shiftedBy(FACTORY.variable(0, 0.0));
164 }
165
166
167
168
169
170 public TimeStampedAngularCoordinates nominalYaw(final AbsoluteDate d) {
171 final PVCoordinates svPV = pvProv.getPVCoordinates(d, inertialFrame);
172 return new TimeStampedAngularCoordinates(d,
173 svPV.normalize(),
174 PVCoordinates.crossProduct(sun.getPVCoordinates(d, inertialFrame), svPV).normalize(),
175 MINUS_Z_PV,
176 PLUS_Y_PV,
177 1.0e-9);
178 }
179
180
181
182
183
184 public double beta(final AbsoluteDate d) {
185 final TimeStampedPVCoordinates svPV = pvProv.getPVCoordinates(d, inertialFrame);
186 return 0.5 * FastMath.PI - Vector3D.angle(sun.getPVCoordinates(d, inertialFrame).getPosition(), svPV.getMomentum());
187 }
188
189
190
191
192
193 private DerivativeStructure betaDS(final FieldAbsoluteDate<DerivativeStructure> d) {
194 final TimeStampedPVCoordinates svPV = pvProv.getPVCoordinates(removeDerivatives(d), inertialFrame);
195 final FieldPVCoordinates<DerivativeStructure> svPVDS = svPV.toDerivativeStructurePV(FACTORY.getCompiler().getOrder());
196 return FieldVector3D.angle(sun.getPVCoordinates(d, inertialFrame).getPosition(), svPVDS.getMomentum()).
197 negate().
198 add(0.5 * FastMath.PI);
199 }
200
201
202
203
204 public DerivativeStructure betaDS() {
205 return betaDS(dateDS);
206 }
207
208
209 @Override
210 public AbsoluteDate getDate() {
211 return date;
212 }
213
214
215
216
217 public TurnSpan getTurnSpan() {
218 return turnSpan;
219 }
220
221
222
223
224 public double getSVBcos() {
225 return svbCos.getValue();
226 }
227
228
229
230
231
232
233
234
235
236
237
238 public double getSecuredBeta() {
239 final double beta = beta(getDate());
240 return FastMath.abs(beta) < BETA_SIGN_CHANGE_PROTECTION ?
241 beta(turnSpan.getTurnStartDate()) :
242 beta;
243 }
244
245
246
247
248
249
250 public boolean linearModelStillActive(final double linearPhi, final double phiDot) {
251 final double dt0 = turnSpan.getTurnEndDate().durationFrom(date);
252 final UnivariateFunction yawReached = dt -> {
253 final AbsoluteDate t = date.shiftedBy(dt);
254 final Vector3D pSun = sun.getPVCoordinates(t, inertialFrame).getPosition();
255 final PVCoordinates pv = pvProv.getPVCoordinates(t, inertialFrame);
256 final Vector3D pSat = pv.getPosition();
257 final Vector3D targetX = Vector3D.crossProduct(pSat, Vector3D.crossProduct(pSun, pSat)).normalize();
258
259 final double phi = linearPhi + dt * phiDot;
260 final SinCos sc = FastMath.sinCos(phi);
261 final Vector3D pU = pv.getPosition().normalize();
262 final Vector3D mU = pv.getMomentum().normalize();
263 final Vector3D omega = new Vector3D(-phiDot, pU);
264 final Vector3D currentX = new Vector3D(-sc.sin(), mU, -sc.cos(), Vector3D.crossProduct(pU, mU));
265 final Vector3D currentXDot = Vector3D.crossProduct(omega, currentX);
266
267 return Vector3D.dotProduct(targetX, currentXDot);
268 };
269 final double fullTurn = 2 * FastMath.PI / FastMath.abs(phiDot);
270 final double dtMin = FastMath.min(turnSpan.getTurnStartDate().durationFrom(date), dt0 - 60.0);
271 final double dtMax = FastMath.max(dtMin + fullTurn, dt0 + 60.0);
272 double[] bracket = UnivariateSolverUtils.bracket(yawReached, dt0,
273 dtMin, dtMax, 1.0, 2.0, 15);
274 if (yawReached.value(bracket[0]) <= 0.0) {
275
276 bracket = UnivariateSolverUtils.bracket(yawReached, 0.5 * (bracket[0] + bracket[1] + fullTurn),
277 bracket[1], bracket[1] + fullTurn, 1.0, 2.0, 15);
278 }
279 final double dt = new BracketingNthOrderBrentSolver(1.0e-3, 5).
280 solve(100, yawReached, bracket[0], bracket[1]);
281 turnSpan.updateEnd(date.shiftedBy(dt), date);
282
283 return dt > 0.0;
284
285 }
286
287
288
289
290
291
292 public boolean setUpTurnRegion(final double cosNight, final double cosNoon) {
293 this.cNight = cosNight;
294 this.cNoon = cosNoon;
295
296 if (svbCos.getValue() < cNight || svbCos.getValue() > cNoon) {
297
298 return true;
299 } else {
300
301
302 return inTurnTimeRange();
303 }
304
305 }
306
307
308
309
310 public DerivativeStructure getDeltaDS() {
311 return delta;
312 }
313
314
315
316
317 public double getOrbitAngleSinceMidnight() {
318 final double absAngle = inOrbitPlaneAbsoluteAngle(FastMath.PI - FastMath.acos(svbCos.getValue()));
319 return morning ? absAngle : -absAngle;
320 }
321
322
323
324
325 public boolean inSunSide() {
326 return svbCos.getValue() > 0;
327 }
328
329
330
331
332
333
334
335 public double getYawStart(final double sunBeta) {
336 final double halfSpan = 0.5 * turnSpan.getTurnDuration() * muRate;
337 return computePhi(sunBeta, FastMath.copySign(halfSpan, svbCos.getValue()));
338 }
339
340
341
342
343
344
345
346 public double getYawEnd(final double sunBeta) {
347 final double halfSpan = 0.5 * turnSpan.getTurnDuration() * muRate;
348 return computePhi(sunBeta, FastMath.copySign(halfSpan, -svbCos.getValue()));
349 }
350
351
352
353
354
355
356
357 public double yawRate(final double sunBeta) {
358 return (getYawEnd(sunBeta) - getYawStart(sunBeta)) / turnSpan.getTurnDuration();
359 }
360
361
362
363
364 public double getMuRate() {
365 return muRate;
366 }
367
368
369
370
371
372
373
374
375
376
377 private DerivativeStructure inOrbitPlaneAbsoluteAngle(final DerivativeStructure angle) {
378 return FastMath.acos(FastMath.cos(angle).divide(FastMath.cos(beta(getDate()))));
379 }
380
381
382
383
384
385
386
387
388
389
390
391 public double inOrbitPlaneAbsoluteAngle(final double angle) {
392 return FastMath.acos(FastMath.cos(angle) / FastMath.cos(beta(getDate())));
393 }
394
395
396
397
398
399
400
401
402
403 public double computePhi(final double sunBeta, final double inOrbitPlaneAngle) {
404 return FastMath.atan2(-FastMath.tan(sunBeta), FastMath.sin(inOrbitPlaneAngle));
405 }
406
407
408
409
410
411 public void setHalfSpan(final double halfSpan, final double endMargin) {
412
413 final AbsoluteDate start = date.shiftedBy((delta.getValue() - halfSpan) / muRate);
414 final AbsoluteDate end = date.shiftedBy((delta.getValue() + halfSpan) / muRate);
415 final AbsoluteDate estimationDate = getDate();
416
417 if (turnSpan == null) {
418 turnSpan = new TurnSpan(start, end, estimationDate, endMargin);
419 } else {
420 turnSpan.updateStart(start, estimationDate);
421 turnSpan.updateEnd(end, estimationDate);
422 }
423 }
424
425
426
427
428 public boolean inTurnTimeRange() {
429 return turnSpan != null && turnSpan.inTurnTimeRange(getDate());
430 }
431
432
433
434
435 public double timeSinceTurnStart() {
436 return getDate().durationFrom(turnSpan.getTurnStartDate());
437 }
438
439
440
441
442
443
444 public TimeStampedAngularCoordinates turnCorrectedAttitude(final double yaw, final double yawDot) {
445 return turnCorrectedAttitude(FACTORY.build(yaw, yawDot, 0.0));
446 }
447
448
449
450
451
452 public TimeStampedAngularCoordinates turnCorrectedAttitude(final DerivativeStructure yaw) {
453
454
455 final PVCoordinates svPV = pvProv.getPVCoordinates(date, inertialFrame);
456 final Vector3D p = svPV.getPosition();
457 final Vector3D v = svPV.getVelocity();
458 final Vector3D a = svPV.getAcceleration();
459 final double r2 = p.getNormSq();
460 final double r = FastMath.sqrt(r2);
461 final Vector3D keplerianJerk = new Vector3D(-3 * Vector3D.dotProduct(p, v) / r2, a, -a.getNorm() / r, v);
462 final PVCoordinatesdinates">PVCoordinates velocity = new PVCoordinates(v, a, keplerianJerk);
463 final PVCoordinates momentum = PVCoordinates.crossProduct(svPV, velocity);
464
465 final DerivativeStructure c = FastMath.cos(yaw).negate();
466 final DerivativeStructure s = FastMath.sin(yaw).negate();
467 final Vector3D m0 = new Vector3D(s.getValue(), c.getValue(), 0.0);
468 final Vector3D m1 = new Vector3D(s.getPartialDerivative(1), c.getPartialDerivative(1), 0.0);
469 final Vector3D m2 = new Vector3D(s.getPartialDerivative(2), c.getPartialDerivative(2), 0.0);
470 return new TimeStampedAngularCoordinates(date,
471 svPV.normalize(), momentum.normalize(),
472 MINUS_Z_PV, new PVCoordinates(m0, m1, m2),
473 1.0e-9);
474
475 }
476
477
478
479
480 public TimeStampedAngularCoordinates orbitNormalYaw() {
481 final Transform t = LOFType.VVLH.transformFromInertial(date, pvProv.getPVCoordinates(date, inertialFrame));
482 return new TimeStampedAngularCoordinates(date,
483 t.getRotation(),
484 t.getRotationRate(),
485 t.getRotationAcceleration());
486 }
487
488 }