1 /* Copyright 2002-2021 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.estimation.measurements.gnss;
18
19 import java.util.Collections;
20 import java.util.List;
21
22 import org.hipparchus.geometry.euclidean.threed.Rotation;
23 import org.hipparchus.geometry.euclidean.threed.Vector3D;
24 import org.hipparchus.util.FastMath;
25 import org.hipparchus.util.MathUtils;
26 import org.orekit.estimation.measurements.EstimatedMeasurement;
27 import org.orekit.estimation.measurements.EstimationModifier;
28 import org.orekit.estimation.measurements.GroundStation;
29 import org.orekit.frames.Frame;
30 import org.orekit.utils.ParameterDriver;
31 import org.orekit.utils.TimeStampedPVCoordinates;
32
33 /** Modifier for wind-up effect in GNSS {@link Phase phase measurements}.
34 * @see <a href="https://gssc.esa.int/navipedia/index.php/Carrier_Phase_Wind-up_Effect">Carrier Phase Wind-up Effect</a>
35 * @see WindUpFactory
36 * @author Luc Maisonobe
37 * @since 10.1
38 */
39 public class WindUp implements EstimationModifier<Phase> {
40
41 /** Cached angular value of wind-up. */
42 private double angularWindUp;
43
44 /** Simple constructor.
45 * <p>
46 * The constructor is package protected to enforce use of {@link WindUpFactory}
47 * and preserve phase continuity for successive measurements involving the same
48 * satellite/receiver pair.
49 * </p>
50 */
51 WindUp() {
52 angularWindUp = 0.0;
53 }
54
55 /** {@inheritDoc}
56 * <p>
57 * Wind-up effect has no parameters, the returned list is always empty.
58 * </p>
59 */
60 @Override
61 public List<ParameterDriver> getParametersDrivers() {
62 return Collections.emptyList();
63 }
64
65 /** {@inheritDoc} */
66 @Override
67 public void modify(final EstimatedMeasurement<Phase> estimated) {
68
69 // signal line of sight
70 final TimeStampedPVCoordinates[] participants = estimated.getParticipants();
71 final Vector3D los = participants[1].getPosition().subtract(participants[0].getPosition()).normalize();
72
73 // get ground antenna dipole
74 final Frame inertial = estimated.getStates()[0].getFrame();
75 final GroundStation station = estimated.getObservedMeasurement().getStation();
76 final Rotation offsetToInert = station.getOffsetToInertial(inertial, estimated.getDate()).getRotation();
77 final Vector3D iGround = offsetToInert.applyTo(Vector3D.PLUS_I);
78 final Vector3D jGround = offsetToInert.applyTo(Vector3D.PLUS_J);
79 final Vector3D dGround = new Vector3D(1.0, iGround, -Vector3D.dotProduct(iGround, los), los).
80 add(Vector3D.crossProduct(los, jGround));
81
82 // get satellite dipole
83 // we don't use the basic yaw steering attitude model from ESA navipedia page
84 // but rely on the attitude that was computed by the propagator, which takes
85 // into account the proper noon and midnight turns for each satellite model
86 final Rotation satToInert = estimated.getStates()[0].toTransform().getRotation().revert();
87 final Vector3D iSat = satToInert.applyTo(Vector3D.PLUS_I);
88 final Vector3D jSat = satToInert.applyTo(Vector3D.PLUS_J);
89 final Vector3D dSat = new Vector3D(1.0, iSat, -Vector3D.dotProduct(iSat, los), los).
90 subtract(Vector3D.crossProduct(los, jSat));
91
92 // raw correction
93 final double correction = FastMath.copySign(Vector3D.angle(dSat, dGround),
94 Vector3D.dotProduct(los, Vector3D.crossProduct(dSat, dGround)));
95
96 // ensure continuity accross measurements
97 // we assume the various measurements are close enough in time
98 // (less the one satellite half-turn) so the angles remain close
99 angularWindUp = MathUtils.normalizeAngle(correction, angularWindUp);
100
101 // update estimate
102 estimated.setEstimatedValue(estimated.getEstimatedValue()[0] + angularWindUp / MathUtils.TWO_PI);
103
104 }
105
106 }