1   /* Copyright 2002-2019 CS Systèmes d'Information
2    * Licensed to CS Systèmes d'Information (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.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   * Boilerplate computations for GNSS attitude.
43   *
44   * <p>
45   * This class is intended to hold throw-away data pertaining to <em>one</em> call
46   * to {@link GNSSAttitudeProvider#getAttitude(org.orekit.utils.PVCoordinatesProvider,
47   * org.orekit.time.AbsoluteDate, org.orekit.frames.Frame) getAttitude}.
48   * </p>
49   *
50   * @author Luc Maisonobe
51   * @since 9.2
52   */
53  class GNSSAttitudeContext implements TimeStamped {
54  
55      /** Derivation order. */
56      private static final int ORDER = 2;
57  
58      /** Time derivation factory. */
59      private static final DSFactory FACTORY = new DSFactory(1, ORDER);
60  
61      /** Constant Y axis. */
62      private static final PVCoordinates PLUS_Y_PV =
63              new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO);
64  
65      /** Constant Z axis. */
66      private static final PVCoordinates MINUS_Z_PV =
67              new PVCoordinates(Vector3D.MINUS_K, Vector3D.ZERO, Vector3D.ZERO);
68  
69      /** Limit value below which we shoud use replace beta by betaIni. */
70      private static final double BETA_SIGN_CHANGE_PROTECTION = FastMath.toRadians(0.07);
71  
72      /** Current date. */
73      private final AbsoluteDate date;
74  
75      /** Current date. */
76      private final FieldAbsoluteDate<DerivativeStructure> dateDS;
77  
78      /** Provider for Sun position. */
79      private final ExtendedPVCoordinatesProvider sun;
80  
81      /** Provider for spacecraft position. */
82      private final PVCoordinatesProvider pvProv;
83  
84      /** Inertial frame where velocity are computed. */
85      private final Frame inertialFrame;
86  
87      /** Cosine of the angle between spacecraft and Sun direction. */
88      private final DerivativeStructure svbCos;
89  
90      /** Morning/Evening half orbit indicator. */
91      private final boolean morning;
92  
93      /** Relative orbit angle to turn center. */
94      private final DerivativeStructure delta;
95  
96      /** Spacecraft angular velocity. */
97      private double muRate;
98  
99      /** Limit cosine for the midnight turn. */
100     private double cNight;
101 
102     /** Limit cosine for the noon turn. */
103     private double cNoon;
104 
105     /** Turn time data. */
106     private TurnSpan turnSpan;
107 
108     /** Simple constructor.
109      * @param date current date
110      * @param sun provider for Sun position
111      * @param pvProv provider for spacecraft position
112      * @param inertialFrame inertial frame where velocity are computed
113      * @param turnSpan turn time data, if a turn has already been identified in the date neighborhood,
114      * null otherwise
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             // night side
140             absDelta = inOrbitPlaneAbsoluteAngle(svbCos.acos().negate().add(FastMath.PI));
141         } else {
142             // Sun side
143             absDelta = inOrbitPlaneAbsoluteAngle(svbCos.acos());
144         }
145         delta = FastMath.copySign(absDelta, -absDelta.getPartialDerivative(1));
146 
147     }
148 
149     /** Convert a date, removing derivatives.
150      * @param d date to convert
151      * @return date without derivatives
152      */
153     private AbsoluteDate removeDerivatives(final FieldAbsoluteDate<DerivativeStructure> d) {
154         return d.toAbsoluteDate();
155     }
156 
157     /** Convert a date, adding derivatives.
158      * @param d date to convert
159      * @return date without derivatives
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     /** Compute nominal yaw steering.
167      * @param d computation date
168      * @return nominal yaw steering
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     /** Compute Sun elevation.
181      * @param d computation date
182      * @return Sun elevation
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     /** Compute Sun elevation.
190      * @param d computation date
191      * @return Sun elevation
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     /** Compute Sun elevation.
202      * @return Sun elevation
203      */
204     public DerivativeStructure betaDS() {
205         return betaDS(dateDS);
206     }
207 
208     /** {@inheritDoc} */
209     @Override
210     public AbsoluteDate getDate() {
211         return date;
212     }
213 
214     /** Get the turn span.
215      * @return turn span, may be null if context is outside of turn
216      */
217     public TurnSpan getTurnSpan() {
218         return turnSpan;
219     }
220 
221     /** Get the cosine of the angle between spacecraft and Sun direction.
222      * @return cosine of the angle between spacecraft and Sun direction.
223      */
224     public double getSVBcos() {
225         return svbCos.getValue();
226     }
227 
228     /** Get a Sun elevation angle that does not change sign within the turn.
229      * <p>
230      * This method either returns the current beta or replaces it with the
231      * value at turn start, so the sign remains constant throughout the
232      * turn. As of 9.2, it is used for GPS, Glonass and Galileo.
233      * </p>
234      * @return secured Sun elevation angle
235      * @see #beta(AbsoluteDate)
236      * @see #betaDS(FieldAbsoluteDate)
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     /** Check if a linear yaw model is still active or if we already reached target yaw.
246      * @param linearPhi value of the linear yaw model
247      * @param phiDot slope of the linear yaw model
248      * @return true if linear model is still active
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             // we have bracketed the wrong crossing
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     /** Set up the midnight/noon turn region.
288      * @param cosNight limit cosine for the midnight turn
289      * @param cosNoon limit cosine for the noon turn
290      * @return true if spacecraft is in the midnight/noon turn region
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             // we are within turn triggering zone
298             return true;
299         } else {
300             // we are outside of turn triggering zone,
301             // but we may still be trying to recover nominal attitude at the end of a turn
302             return inTurnTimeRange();
303         }
304 
305     }
306 
307     /** Get the relative orbit angle to turn center.
308      * @return relative orbit angle to turn center
309      */
310     public DerivativeStructure getDeltaDS() {
311         return delta;
312     }
313 
314     /** Get the orbit angle since solar midnight.
315      * @return orbit angle since solar midnight
316      */
317     public double getOrbitAngleSinceMidnight() {
318         final double absAngle = inOrbitPlaneAbsoluteAngle(FastMath.PI - FastMath.acos(svbCos.getValue()));
319         return morning ? absAngle : -absAngle;
320     }
321 
322     /** Check if spacecraft is in the half orbit closest to Sun.
323      * @return true if spacecraft is in the half orbit closest to Sun
324      */
325     public boolean inSunSide() {
326         return svbCos.getValue() > 0;
327     }
328 
329     /** Get yaw at turn start.
330      * @param sunBeta Sun elevation above orbital plane
331      * (it <em>may</em> be different from {@link #getBeta()} in
332      * some special cases)
333      * @return yaw at turn start
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     /** Get yaw at turn end.
341      * @param sunBeta Sun elevation above orbital plane
342      * (it <em>may</em> be different from {@link #getBeta()} in
343      * some special cases)
344      * @return yaw at turn end
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     /** Compute yaw rate.
352      * @param sunBeta Sun elevation above orbital plane
353      * (it <em>may</em> be different from {@link #getBeta()} in
354      * some special cases)
355      * @return yaw rate
356      */
357     public double yawRate(final double sunBeta) {
358         return (getYawEnd(sunBeta) - getYawStart(sunBeta)) / turnSpan.getTurnDuration();
359     }
360 
361     /** Get the spacecraft angular velocity.
362      * @return spacecraft angular velocity
363      */
364     public double getMuRate() {
365         return muRate;
366     }
367 
368     /** Project a spacecraft/Sun angle into orbital plane.
369      * <p>
370      * This method is intended to find the limits of the noon and midnight
371      * turns in orbital plane. The return angle is signed, depending on the
372      * spacecraft being before or after turn middle point.
373      * </p>
374      * @param angle spacecraft/Sun angle (or spacecraft/opposite-of-Sun)
375      * @return angle projected into orbital plane, always positive
376      */
377     private DerivativeStructure inOrbitPlaneAbsoluteAngle(final DerivativeStructure angle) {
378         return FastMath.acos(FastMath.cos(angle).divide(FastMath.cos(beta(getDate()))));
379     }
380 
381     /** Project a spacecraft/Sun angle into orbital plane.
382      * <p>
383      * This method is intended to find the limits of the noon and midnight
384      * turns in orbital plane. The return angle is always positive. The
385      * correct sign to apply depends on the spacecraft being before or
386      * after turn middle point.
387      * </p>
388      * @param angle spacecraft/Sun angle (or spacecraft/opposite-of-Sun)
389      * @return angle projected into orbital plane, always positive
390      */
391     public double inOrbitPlaneAbsoluteAngle(final double angle) {
392         return FastMath.acos(FastMath.cos(angle) / FastMath.cos(beta(getDate())));
393     }
394 
395     /** Compute yaw.
396      * @param sunBeta Sun elevation above orbital plane
397      * (it <em>may</em> be different from {@link #getBeta()} in
398      * some special cases)
399      * @param inOrbitPlaneAngle in orbit angle between spacecraft
400      * and Sun (or opposite of Sun) projection
401      * @return yaw angle
402      */
403     public double computePhi(final double sunBeta, final double inOrbitPlaneAngle) {
404         return FastMath.atan2(-FastMath.tan(sunBeta), FastMath.sin(inOrbitPlaneAngle));
405     }
406 
407     /** Set turn half span and compute corresponding turn time range.
408      * @param halfSpan half span of the turn region, as an angle in orbit plane
409      * @param endMargin margin in seconds after turn end
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     /** Check if context is within turn range.
426      * @return true if context is within range extended by end margin
427      */
428     public boolean inTurnTimeRange() {
429         return turnSpan != null && turnSpan.inTurnTimeRange(getDate());
430     }
431 
432     /** Get elapsed time since turn start.
433      * @return elapsed time from turn start to current date
434      */
435     public double timeSinceTurnStart() {
436         return getDate().durationFrom(turnSpan.getTurnStartDate());
437     }
438 
439     /** Generate an attitude with turn-corrected yaw.
440      * @param yaw yaw value to apply
441      * @param yawDot yaw first time derivative
442      * @return attitude with specified yaw
443      */
444     public TimeStampedAngularCoordinates turnCorrectedAttitude(final double yaw, final double yawDot) {
445         return turnCorrectedAttitude(FACTORY.build(yaw, yawDot, 0.0));
446     }
447 
448     /** Generate an attitude with turn-corrected yaw.
449      * @param yaw yaw value to apply
450      * @return attitude with specified yaw
451      */
452     public TimeStampedAngularCoordinates turnCorrectedAttitude(final DerivativeStructure yaw) {
453 
454         // Earth pointing (Z aligned with position) with linear yaw (momentum with known cos/sin in the X/Y plane)
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     /** Compute Orbit Normal (ON) yaw.
478      * @return Orbit Normal yaw, using inertial frame as reference
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 }