Commit 9ad52251 authored by Luc Maisonobe's avatar Luc Maisonobe

Improved phase measurement.

parent 0e52bb52
...@@ -47,7 +47,7 @@ import org.orekit.utils.TimeStampedPVCoordinates; ...@@ -47,7 +47,7 @@ import org.orekit.utils.TimeStampedPVCoordinates;
* @author Thierry Ceolin * @author Thierry Ceolin
* @author Luc Maisonobe * @author Luc Maisonobe
* @author Maxime Journot * @author Maxime Journot
* @since 8.0 * @since 9.2
*/ */
public class Phase extends AbstractMeasurement<Phase> { public class Phase extends AbstractMeasurement<Phase> {
...@@ -89,7 +89,6 @@ public class Phase extends AbstractMeasurement<Phase> { ...@@ -89,7 +89,6 @@ public class Phase extends AbstractMeasurement<Phase> {
* @param propagatorIndex index of the propagator related to this measurement * @param propagatorIndex index of the propagator related to this measurement
* @exception OrekitException if a {@link org.orekit.utils.ParameterDriver} * @exception OrekitException if a {@link org.orekit.utils.ParameterDriver}
* name conflict occurs * name conflict occurs
* @since 9.0
*/ */
public Phase(final GroundStation station, final AbsoluteDate date, public Phase(final GroundStation station, final AbsoluteDate date,
final double phase, final double wavelength, final double sigma, final double phase, final double wavelength, final double sigma,
...@@ -105,7 +104,7 @@ public class Phase extends AbstractMeasurement<Phase> { ...@@ -105,7 +104,7 @@ public class Phase extends AbstractMeasurement<Phase> {
station.getPolarDriftXDriver(), station.getPolarDriftXDriver(),
station.getPolarOffsetYDriver(), station.getPolarOffsetYDriver(),
station.getPolarDriftYDriver()); station.getPolarDriftYDriver());
this.station = station; this.station = station;
this.wavelength = wavelength; this.wavelength = wavelength;
} }
...@@ -184,8 +183,8 @@ public class Phase extends AbstractMeasurement<Phase> { ...@@ -184,8 +183,8 @@ public class Phase extends AbstractMeasurement<Phase> {
}); });
// Phase value // Phase value
final double cOver2 = Constants.SPEED_OF_LIGHT / wavelength; final double cOverLambda = Constants.SPEED_OF_LIGHT / wavelength;
final DerivativeStructure phase = tauD.multiply(cOver2); final DerivativeStructure phase = tauD.multiply(cOverLambda);
estimated.setEstimatedValue(phase.getValue()); estimated.setEstimatedValue(phase.getValue());
......
/* Copyright 2002-2018 CS Systèmes d'Information
* Licensed to CS Systèmes d'Information (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.estimation.measurements.modifiers;
import java.util.Collections;
import java.util.List;
import org.hipparchus.util.FastMath;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.EstimatedMeasurement;
import org.orekit.estimation.measurements.EstimationModifier;
import org.orekit.estimation.measurements.Phase;
import org.orekit.utils.ParameterDriver;
/** Class modifying theoretical phase measurement with ambiguity.
*
* @author Luc Maisonobe
* @since 9.2
*/
public class PhaseAmbiguityModifier implements EstimationModifier<Phase> {
/** Ambiguity scale factor.
* <p>
* We use a power of 2 to avoid numeric noise introduction
* in the multiplications/divisions sequences.
* </p>
*/
private static final double AMBIGUITY_SCALE = FastMath.scalb(1.0, 26);
/** Ambiguity parameter. */
private final ParameterDriver ambiguity;
/** Constructor.
* <p>
* It is expected that many different ambiguities will be used at the
* same time during an orbit determination, therefore they are keyed
* using a simple integer. All ambiguities using the same key will
* be enforced to be equal. It is the responsibility of the caller to
* use a proper counter to manage the ambiguities properly.
* </p>
* @param key key to identify the ambiguity
* @param ambiguity initial value of ambiguity
* @exception OrekitException if parameter scale is too close to zero (never happens)
*/
public PhaseAmbiguityModifier(final int key, final double ambiguity)
throws OrekitException {
this.ambiguity = new ParameterDriver("amgiguity-" + key,
ambiguity, AMBIGUITY_SCALE,
Double.NEGATIVE_INFINITY,
Double.POSITIVE_INFINITY);
}
/** {@inheritDoc} */
@Override
public List<ParameterDriver> getParametersDrivers() {
return Collections.singletonList(ambiguity);
}
@Override
public void modify(final EstimatedMeasurement<Phase> estimated)
throws OrekitException {
// apply the ambiguity to the measurement value
final double[] value = estimated.getEstimatedValue();
value[0] += ambiguity.getValue();
if (ambiguity.isSelected()) {
// add the partial derivatives
estimated.setParameterDerivatives(ambiguity, 1.0);
}
estimated.setEstimatedValue(value);
}
}
/* Copyright 2002-2018 CS Systèmes d'Information
* Licensed to CS Systèmes d'Information (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.estimation.measurements;
import java.util.Arrays;
import org.hipparchus.analysis.UnivariateFunction;
import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
import org.hipparchus.analysis.solvers.UnivariateSolver;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.util.FastMath;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitExceptionWrapper;
import org.orekit.estimation.Context;
import org.orekit.estimation.measurements.modifiers.PhaseAmbiguityModifier;
import org.orekit.frames.Frame;
import org.orekit.frames.Transform;
import org.orekit.gnss.Frequency;
import org.orekit.propagation.SpacecraftState;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.Constants;
import org.orekit.utils.ParameterDriver;
public class PhaseMeasurementCreator extends MeasurementCreator {
private final Context context;
private final double wavelength;
private final PhaseAmbiguityModifier ambiguity;
private final Vector3D antennaPhaseCenter;
public PhaseMeasurementCreator(final Context context, final Frequency frequency, final int ambiguity)
throws OrekitException {
this(context, frequency, ambiguity, Vector3D.ZERO);
}
public PhaseMeasurementCreator(final Context context, final Frequency frequency,
final int ambiguity, final Vector3D antennaPhaseCenter)
throws OrekitException {
this.context = context;
this.wavelength = Constants.SPEED_OF_LIGHT / (1.0e6 * frequency.getMHzFrequency());
this.ambiguity = new PhaseAmbiguityModifier(0, ambiguity);
this.antennaPhaseCenter = antennaPhaseCenter;
}
public void init(SpacecraftState s0, AbsoluteDate t, double step) {
for (final GroundStation station : context.stations) {
for (ParameterDriver driver : Arrays.asList(station.getEastOffsetDriver(),
station.getNorthOffsetDriver(),
station.getZenithOffsetDriver(),
station.getPrimeMeridianOffsetDriver(),
station.getPrimeMeridianDriftDriver(),
station.getPolarOffsetXDriver(),
station.getPolarDriftXDriver(),
station.getPolarOffsetYDriver(),
station.getPolarDriftYDriver())) {
if (driver.getReferenceDate() == null) {
driver.setReferenceDate(s0.getDate());
}
}
}
}
public void handleStep(final SpacecraftState currentState, final boolean isLast)
throws OrekitException {
try {
double n = ambiguity.getParametersDrivers().get(0).getValue();
for (final GroundStation station : context.stations) {
final AbsoluteDate date = currentState.getDate();
final Frame inertial = currentState.getFrame();
final Vector3D position = currentState.toTransform().getInverse().transformPosition(antennaPhaseCenter);
if (station.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0)) {
final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
final double downLinkDelay = solver.solve(1000, new UnivariateFunction() {
public double value(final double x) throws OrekitExceptionWrapper {
try {
final Transform t = station.getOffsetToInertial(inertial, date.shiftedBy(x));
final double d = Vector3D.distance(position, t.transformPosition(Vector3D.ZERO));
return d - x * Constants.SPEED_OF_LIGHT;
} catch (OrekitException oe) {
throw new OrekitExceptionWrapper(oe);
}
}
}, -1.0, 1.0);
final AbsoluteDate receptionDate = currentState.getDate().shiftedBy(downLinkDelay);
final Vector3D stationAtReception =
station.getOffsetToInertial(inertial, receptionDate).transformPosition(Vector3D.ZERO);
final double downLinkDistance = Vector3D.distance(position, stationAtReception);
final Phase phase = new Phase(station, receptionDate,
downLinkDistance / wavelength - n,
wavelength, 1.0, 10);
phase.addModifier(ambiguity);
addMeasurement(phase);
}
}
} catch (OrekitExceptionWrapper oew) {
throw new OrekitException(oew.getException());
} catch (OrekitException oe) {
throw new OrekitException(oe);
}
}
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment