Skip to content
GitLab
Commit 6096fc33 authored by Luc Maisonobe's avatar Luc Maisonobe
Browse files

Merge branch 'orekit-time_span_drivers' into develop

parents eeeb9ec9 15fae4b8
Pipeline #2959 failed with stages
in 39 minutes and 29 seconds
Hide whitespace changes
Inline Side-by-side
src/main/java/org/orekit/errors/OrekitMessages.java
View file @ 6096fc33
......@@ -153,6 +153,8 @@ public enum OrekitMessages implements Localizable {
UNEXPECTED_TWO_ELEVATION_VALUES_FOR_ONE_AZIMUTH(
"unexpected two elevation values: {0} and {1}, for one azimuth: {2}"),
UNSUPPORTED_PARAMETER_NAME("unsupported parameter name {0}, supported names: {1}"),
PARAMETER_WITH_SEVERAL_ESTIMATED_VALUES("{0} parameter contains several span in its value TimeSpanMap, the {1} method must be called"),
PARAMETER_PERIODS_HAS_ALREADY_BEEN_SET("setPeriod was already called once on {0} parameter, another parameter should be created if the periods have to be changed"),
TOO_SMALL_SCALE_FOR_PARAMETER("scale factor for parameter {0} is too small: {1}"),
UNKNOWN_ADDITIONAL_STATE("unknown additional state \"{0}\""),
UNKNOWN_MONTH("unknown month \"{0}\""),
......
src/main/java/org/orekit/estimation/leastsquares/AbstractBatchLSModel.java
View file @ 6096fc33
......@@ -47,6 +47,8 @@ import org.orekit.time.ChronologicalComparator;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.ParameterDriversList;
import org.orekit.utils.ParameterDriversList.DelegatingDriver;
import org.orekit.utils.TimeSpanMap.Span;
import org.orekit.utils.TimeSpanMap;
/** Bridge between {@link ObservedMeasurement measurements} and {@link
* org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem
......@@ -54,6 +56,7 @@ import org.orekit.utils.ParameterDriversList.DelegatingDriver;
* @author Luc Maisonobe
* @author Bryan Cazabonne
* @author Thomas Paulet
* @author Melina Vanel
* @since 11.0
*/
public abstract class AbstractBatchLSModel implements MultivariateJacobianFunction {
......@@ -61,7 +64,8 @@ public abstract class AbstractBatchLSModel implements MultivariateJacobianFuncti
/** Builders for propagators. */
private final PropagatorBuilder[] builders;
/** Array of each builder's selected orbit drivers.
/** Array of each builder's selected orbit drivers. Orbit drivers
* should have only 1 span on their value TimeSpanMap.
* @since 11.1
*/
private final ParameterDriversList[] estimatedOrbitalParameters;
......@@ -139,7 +143,7 @@ public abstract class AbstractBatchLSModel implements MultivariateJacobianFuncti
this.builders = propagatorBuilders.clone();
this.measurements = measurements;
this.estimatedMeasurementsParameters = estimatedMeasurementsParameters;
this.measurementParameterColumns = new HashMap<>(estimatedMeasurementsParameters.getDrivers().size());
this.measurementParameterColumns = new HashMap<>(estimatedMeasurementsParameters.getNbValuesToEstimate());
this.estimatedOrbitalParameters = new ParameterDriversList[builders.length];
this.estimatedPropagationParameters = new ParameterDriversList[builders.length];
this.evaluations = new IdentityHashMap<>(measurements.size());
......@@ -176,10 +180,20 @@ public abstract class AbstractBatchLSModel implements MultivariateJacobianFuncti
// The index i in array estimatedPropagationParameters (attribute of the class) is populated
// when the first call to getSelectedPropagationDriversForBuilder(i) is made
for (final DelegatingDriver delegating : getSelectedPropagationDriversForBuilder(i).getDrivers()) {
final String driverName = delegating.getName();
// Add the driver name if it has not been added yet
if (!estimatedPropagationParametersNames.contains(driverName)) {
estimatedPropagationParametersNames.add(driverName);
final TimeSpanMap<String> delegatingNameSpanMap = delegating.getNamesSpanMap();
// for each span (for each estimated value) corresponding name is added
Span<String> currentNameSpan = delegatingNameSpanMap.getFirstSpan();
// Add the driver name if it has not been added yet and the number of estimated values for this param
if (!estimatedPropagationParametersNames.contains(currentNameSpan.getData())) {
estimatedPropagationParametersNames.add(currentNameSpan.getData());
}
for (int spanNumber = 1; spanNumber < delegatingNameSpanMap.getSpansNumber(); ++spanNumber) {
currentNameSpan = delegatingNameSpanMap.getSpan(currentNameSpan.getEnd());
// Add the driver name if it has not been added yet and the number of estimated values for this param
if (!estimatedPropagationParametersNames.contains(currentNameSpan.getData())) {
estimatedPropagationParametersNames.add(currentNameSpan.getData());
}
}
}
}
......@@ -190,11 +204,12 @@ public abstract class AbstractBatchLSModel implements MultivariateJacobianFuncti
propagationParameterColumns.put(driverName, columns);
++columns;
}
// Populate the map of measurement drivers' columns and update the total number of columns
for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
measurementParameterColumns.put(parameter.getName(), columns);
++columns;
for (Span<String> span = parameter.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
measurementParameterColumns.put(span.getData(), columns);
columns++;
}
}
// Initialize point and value
......@@ -359,28 +374,42 @@ public abstract class AbstractBatchLSModel implements MultivariateJacobianFuncti
final Propagator[] propagators = new Propagator[builders.length];
// Set up the propagators
for (int i = 0; i < builders.length; ++i) {
// Get the number of selected orbital drivers in the builder
int element = 0;
// Get the number of values to estimate for selected orbital drivers in the builder
final int nbOrb = orbitsEndColumns[i] - orbitsStartColumns[i];
// Get the list of selected propagation drivers in the builder and its size
final ParameterDriversList selectedPropagationDrivers = getSelectedPropagationDriversForBuilder(i);
final int nbParams = selectedPropagationDrivers.getNbParams();
final int nbValuesToEstimate = selectedPropagationDrivers.getNbValuesToEstimate();
// Init the array of normalized parameters for the builder
final double[] propagatorArray = new double[nbOrb + nbParams];
final double[] propagatorArray = new double[nbOrb + nbValuesToEstimate];
// Add the orbital drivers normalized values
for (int j = 0; j < nbOrb; ++j) {
propagatorArray[j] = point.getEntry(orbitsStartColumns[i] + j);
propagatorArray[element++] = point.getEntry(orbitsStartColumns[i] + j);
}
// Add the propagation drivers normalized values
for (int j = 0; j < nbParams; ++j) {
propagatorArray[nbOrb + j] =
point.getEntry(propagationParameterColumns.get(selectedPropagationDrivers.getDrivers().get(j).getName()));
final DelegatingDriver driver = selectedPropagationDrivers.getDrivers().get(j);
final TimeSpanMap<String> delegatingNameSpanMap = driver.getNamesSpanMap();
// get point entry for each span (for each estimated value), point is sorted
// with following parameters values and for each parameter driver
// span value are sorted in chronological order
Span<String> currentNameSpan = delegatingNameSpanMap.getFirstSpan();
propagatorArray[element++] = point.getEntry(propagationParameterColumns.get(currentNameSpan.getData()));
for (int spanNumber = 1; spanNumber < delegatingNameSpanMap.getSpansNumber(); ++spanNumber) {
currentNameSpan = delegatingNameSpanMap.getSpan(currentNameSpan.getEnd());
propagatorArray[element++] = point.getEntry(propagationParameterColumns.get(currentNameSpan.getData()));
}
}
// Build the propagator
......@@ -452,11 +481,19 @@ public abstract class AbstractBatchLSModel implements MultivariateJacobianFuncti
if (nbParams > 0) {
final RealMatrix dYdPp = harvesters[p].getParametersJacobian(evaluationStates[k]);
final RealMatrix dMdPp = dMdY.multiply(dYdPp);
for (int i = 0; i < dMdPp.getRowDimension(); ++i) {
int col = 0;
// Add the propagation drivers normalized values
for (int j = 0; j < nbParams; ++j) {
final ParameterDriver delegating = selectedPropagationDrivers.getDrivers().get(j);
jacobian.addToEntry(index + i, propagationParameterColumns.get(delegating.getName()),
weight[i] * dMdPp.getEntry(i, j) / sigma[i] * delegating.getScale());
final TimeSpanMap<String> delegatingNameSpanMap = delegating.getNamesSpanMap();
// get point entry for each span (for each estimated value), point is sorted
for (Span<String> currentNameSpan = delegatingNameSpanMap.getFirstSpan(); currentNameSpan != null; currentNameSpan = currentNameSpan.next()) {
jacobian.addToEntry(index + i, propagationParameterColumns.get(currentNameSpan.getData()),
weight[i] * dMdPp.getEntry(i, col++) / sigma[i] * delegating.getScale());
}
}
}
}
......@@ -464,10 +501,12 @@ public abstract class AbstractBatchLSModel implements MultivariateJacobianFuncti
// Jacobian of the measurement with respect to measurements parameters
for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
if (driver.isSelected()) {
final double[] aMPm = evaluation.getParameterDerivatives(driver);
for (int i = 0; i < aMPm.length; ++i) {
jacobian.setEntry(index + i, measurementParameterColumns.get(driver.getName()),
weight[i] * aMPm[i] / sigma[i] * driver.getScale());
for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
final double[] aMPm = evaluation.getParameterDerivatives(driver, span.getStart());
for (int i = 0; i < aMPm.length; ++i) {
jacobian.setEntry(index + i, measurementParameterColumns.get(span.getData()),
weight[i] * aMPm[i] / sigma[i] * driver.getScale());
}
}
}
}
......@@ -483,7 +522,10 @@ public abstract class AbstractBatchLSModel implements MultivariateJacobianFuncti
// Set up the measurement parameters
int index = orbitsEndColumns[builders.length - 1] + propagationParameterColumns.size();
for (final ParameterDriver parameter : estimatedMeasurementsParameters.getDrivers()) {
parameter.setNormalizedValue(point.getEntry(index++));
for (Span<Double> span = parameter.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
parameter.setNormalizedValue(point.getEntry(index++), span.getStart());
}
}
// Set up measurements handler
......
src/main/java/org/orekit/estimation/leastsquares/BatchLSEstimator.java
View file @ 6096fc33
......@@ -51,9 +51,11 @@ import org.orekit.propagation.conversion.AbstractPropagatorBuilder;
import org.orekit.propagation.conversion.PropagatorBuilder;
import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.ParameterDriversList;
import org.orekit.utils.ParameterDriversList.DelegatingDriver;
import org.orekit.utils.TimeSpanMap.Span;
/** Least squares estimator for orbit determination.
......@@ -382,18 +384,34 @@ public class BatchLSEstimator {
final ParameterDriversList estimatedMeasurementsParameters = getMeasurementsParametersDrivers(true);
// create start point
final double[] start = new double[estimatedOrbitalParameters.getNbParams() +
estimatedPropagatorParameters.getNbParams() +
estimatedMeasurementsParameters.getNbParams()];
final double[] start = new double[estimatedOrbitalParameters.getNbValuesToEstimate() +
estimatedPropagatorParameters.getNbValuesToEstimate() +
estimatedMeasurementsParameters.getNbValuesToEstimate()];
int iStart = 0;
for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
start[iStart++] = driver.getNormalizedValue();
Span<Double> span = driver.getValueSpanMap().getFirstSpan();
start[iStart++] = driver.getNormalizedValue(span.getStart());
for (int spanNumber = 0; spanNumber < driver.getNbOfValues() - 1; ++spanNumber) {
span = driver.getValueSpanMap().getSpan(span.getEnd());
start[iStart++] = driver.getNormalizedValue(span.getStart());
}
}
for (final ParameterDriver driver : estimatedPropagatorParameters.getDrivers()) {
start[iStart++] = driver.getNormalizedValue();
Span<Double> span = driver.getValueSpanMap().getFirstSpan();
start[iStart++] = driver.getNormalizedValue(span.getStart());
for (int spanNumber = 0; spanNumber < driver.getNbOfValues() - 1; ++spanNumber) {
span = driver.getValueSpanMap().getSpan(span.getEnd());
start[iStart++] = driver.getNormalizedValue(span.getStart());
}
}
for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
start[iStart++] = driver.getNormalizedValue();
Span<Double> span = driver.getValueSpanMap().getFirstSpan();
start[iStart++] = driver.getNormalizedValue(span.getStart());
for (int spanNumber = 0; spanNumber < driver.getNbOfValues() - 1; ++spanNumber) {
span = driver.getValueSpanMap().getSpan(span.getEnd());
start[iStart++] = driver.getNormalizedValue(span.getStart());
}
}
lsBuilder.start(start);
......@@ -498,13 +516,19 @@ public class BatchLSEstimator {
final double[] scale = new double[covariances.getRowDimension()];
int index = 0;
for (final ParameterDriver driver : getOrbitalParametersDrivers(true).getDrivers()) {
scale[index++] = driver.getScale();
for (int i = 0; i < driver.getNbOfValues(); ++i) {
scale[index++] = driver.getScale();
}
}
for (final ParameterDriver driver : getPropagatorParametersDrivers(true).getDrivers()) {
scale[index++] = driver.getScale();
for (int i = 0; i < driver.getNbOfValues(); ++i) {
scale[index++] = driver.getScale();
}
}
for (final ParameterDriver driver : getMeasurementsParametersDrivers(true).getDrivers()) {
scale[index++] = driver.getScale();
for (int i = 0; i < driver.getNbOfValues(); ++i) {
scale[index++] = driver.getScale();
}
}
// unnormalize the matrix, to retrieve physical covariances
......@@ -712,20 +736,71 @@ public class BatchLSEstimator {
int i = 0;
for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
// let the parameter handle min/max clipping
driver.setNormalizedValue(params.getEntry(i));
params.setEntry(i++, driver.getNormalizedValue());
if (driver.getNbOfValues() == 1) {
driver.setNormalizedValue(params.getEntry(i), null);
params.setEntry(i++, driver.getNormalizedValue(null));
// If the parameter driver contains only 1 value to estimate over the all time range
} else {
// initialization getting the value of the first Span
Span<Double> span = driver.getValueSpanMap().getFirstSpan();
driver.setNormalizedValue(params.getEntry(i), span.getStart());
params.setEntry(i++, driver.getNormalizedValue(span.getStart()));
for (int spanNumber = 0; spanNumber < driver.getNbOfValues() - 1; ++spanNumber) {
final AbsoluteDate modificationDate = span.getEnd();
// get next span, previousSpan.getEnd = span.getStart
span = driver.getValueSpanMap().getSpan(modificationDate);
driver.setNormalizedValue(params.getEntry(i), modificationDate);
params.setEntry(i++, driver.getNormalizedValue(modificationDate));
}
}
}
for (final ParameterDriver driver : estimatedPropagatorParameters.getDrivers()) {
// let the parameter handle min/max clipping
driver.setNormalizedValue(params.getEntry(i));
params.setEntry(i++, driver.getNormalizedValue());
if (driver.getNbOfValues() == 1) {
driver.setNormalizedValue(params.getEntry(i), null);
params.setEntry(i++, driver.getNormalizedValue(null));
// If the parameter driver contains only 1 value to estimate over the all time range
} else {
// initialization getting the value of the first Span
Span<Double> span = driver.getValueSpanMap().getFirstSpan();
driver.setNormalizedValue(params.getEntry(i), span.getStart());
params.setEntry(i++, driver.getNormalizedValue(span.getStart()));
for (int spanNumber = 0; spanNumber < driver.getNbOfValues() - 1; ++spanNumber) {
final AbsoluteDate modificationDate = span.getEnd();
// get next span, previousSpan.getEnd = span.getStart
span = driver.getValueSpanMap().getSpan(modificationDate);
driver.setNormalizedValue(params.getEntry(i), modificationDate);
params.setEntry(i++, driver.getNormalizedValue(modificationDate));
}
}
}
for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
// let the parameter handle min/max clipping
driver.setNormalizedValue(params.getEntry(i));
params.setEntry(i++, driver.getNormalizedValue());
if (driver.getNbOfValues() == 1) {
driver.setNormalizedValue(params.getEntry(i), null);
params.setEntry(i++, driver.getNormalizedValue(null));
// If the parameter driver contains only 1 value to estimate over the all time range
} else {
// initialization getting the value of the first Span
Span<Double> span = driver.getValueSpanMap().getFirstSpan();
driver.setNormalizedValue(params.getEntry(i), span.getStart());
params.setEntry(i++, driver.getNormalizedValue(span.getStart()));
for (int spanNumber = 0; spanNumber < driver.getNbOfValues() - 1; ++spanNumber) {
final AbsoluteDate modificationDate = span.getEnd();
// get next span, previousSpan.getEnd = span.getStart
span = driver.getValueSpanMap().getSpan(modificationDate);
driver.setNormalizedValue(params.getEntry(i), modificationDate);
params.setEntry(i++, driver.getNormalizedValue(modificationDate));
}
}
}
return params;
}
}
......
src/main/java/org/orekit/estimation/measurements/AngularAzEl.java
View file @ 6096fc33
......@@ -33,6 +33,7 @@ import org.orekit.time.FieldAbsoluteDate;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.TimeStampedFieldPVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;
import org.orekit.utils.TimeSpanMap.Span;
/** Class modeling an Azimuth-Elevation measurement from a ground station.
* The motion of the spacecraft during the signal flight time is taken into
......@@ -105,7 +106,12 @@ public class AngularAzEl extends AbstractMeasurement<AngularAzEl> {
final Map<String, Integer> indices = new HashMap<>();
for (ParameterDriver driver : getParametersDrivers()) {
if (driver.isSelected()) {
indices.put(driver.getName(), nbParams++);
for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
if (!indices.containsKey(span.getData())) {
indices.put(span.getData(), nbParams++);
}
}
}
}
final Field<Gradient> field = GradientField.getField(nbParams);
......@@ -178,9 +184,12 @@ public class AngularAzEl extends AbstractMeasurement<AngularAzEl> {
// Set partial derivatives with respect to parameters
// (beware element at index 0 is the value, not a derivative)
for (final ParameterDriver driver : getParametersDrivers()) {
final Integer index = indices.get(driver.getName());
if (index != null) {
estimated.setParameterDerivatives(driver, azDerivatives[index], elDerivatives[index]);
for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
final Integer index = indices.get(span.getData());
if (index != null) {
estimated.setParameterDerivatives(driver, span.getStart(), azDerivatives[index], elDerivatives[index]);
}
}
}
......
src/main/java/org/orekit/estimation/measurements/AngularRaDec.java
View file @ 6096fc33
......@@ -33,6 +33,7 @@ import org.orekit.time.FieldAbsoluteDate;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.TimeStampedFieldPVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;
import org.orekit.utils.TimeSpanMap.Span;
/** Class modeling an Right Ascension - Declination measurement from a ground point (station, telescope).
* The angles are given in an inertial reference frame.
......@@ -119,7 +120,12 @@ public class AngularRaDec extends AbstractMeasurement<AngularRaDec> {
final Map<String, Integer> indices = new HashMap<>();
for (ParameterDriver driver : getParametersDrivers()) {
if (driver.isSelected()) {
indices.put(driver.getName(), nbParams++);
for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
if (!indices.containsKey(span.getData())) {
indices.put(span.getData(), nbParams++);
}
}
}
}
final FieldVector3D<Gradient> zero = FieldVector3D.getZero(GradientField.getField(nbParams));
......@@ -194,9 +200,11 @@ public class AngularRaDec extends AbstractMeasurement<AngularRaDec> {
// Partial derivatives with respect to parameters
// (beware element at index 0 is the value, not a derivative)
for (final ParameterDriver driver : getParametersDrivers()) {
final Integer index = indices.get(driver.getName());
if (index != null) {
estimated.setParameterDerivatives(driver, raDerivatives[index], decDerivatives[index]);
for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
final Integer index = indices.get(span.getData());
if (index != null) {
estimated.setParameterDerivatives(driver, span.getStart(), raDerivatives[index], decDerivatives[index]);
}
}
}
......
src/main/java/org/orekit/estimation/measurements/BistaticRange.java
View file @ 6096fc33
......@@ -32,6 +32,7 @@ import org.orekit.utils.Constants;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.TimeStampedFieldPVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;
import org.orekit.utils.TimeSpanMap.Span;
/**
* Class modeling a bistatic range measurement using
......@@ -144,7 +145,11 @@ public class BistaticRange extends AbstractMeasurement<BistaticRange> {
final Map<String, Integer> indices = new HashMap<>();
for (ParameterDriver driver : getParametersDrivers()) {
if (driver.isSelected()) {
indices.put(driver.getName(), nbParams++);
for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
if (!indices.containsKey(span.getData())) {
indices.put(span.getData(), nbParams++);
}
}
}
}
final FieldVector3D<Gradient> zero = FieldVector3D.getZero(GradientField.getField(nbParams));
......@@ -228,9 +233,11 @@ public class BistaticRange extends AbstractMeasurement<BistaticRange> {
// set partial derivatives with respect to parameters
// (beware element at index 0 is the value, not a derivative)
for (final ParameterDriver driver : getParametersDrivers()) {
final Integer index = indices.get(driver.getName());
if (index != null) {
estimated.setParameterDerivatives(driver, derivatives[index]);
for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
final Integer index = indices.get(span.getData());
if (index != null) {
estimated.setParameterDerivatives(driver, span.getStart(), derivatives[index]);
}
}
}
......
src/main/java/org/orekit/estimation/measurements/BistaticRangeRate.java
View file @ 6096fc33
......@@ -31,6 +31,7 @@ import org.orekit.time.FieldAbsoluteDate;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.TimeStampedFieldPVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;
import org.orekit.utils.TimeSpanMap.Span;
/** Class modeling a bistatic range rate measurement using
* an emitter ground station and a receiver ground station.
......@@ -139,8 +140,13 @@ public class BistaticRangeRate extends AbstractMeasurement<BistaticRangeRate> {
// we have to check for duplicate keys because emitter and receiver stations share
// pole and prime meridian parameters names that must be considered
// as one set only (they are combined together by the estimation engine)
if (driver.isSelected() && !indices.containsKey(driver.getName())) {
indices.put(driver.getName(), nbParams++);
if (driver.isSelected()) {
for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
if (!indices.containsKey(span.getData())) {
indices.put(span.getData(), nbParams++);
}
}
}
}
final FieldVector3D<Gradient> zero = FieldVector3D.getZero(GradientField.getField(nbParams));
......@@ -224,13 +230,16 @@ public class BistaticRangeRate extends AbstractMeasurement<BistaticRangeRate> {
// combine uplink and downlink partial derivatives with respect to parameters
evalDownlink.getDerivativesDrivers().forEach(driver -> {
final double[] pd1 = evalDownlink.getParameterDerivatives(driver);
final double[] pd2 = evalUplink.getParameterDerivatives(driver);
final double[] pd = new double[pd1.length];
for (int i = 0; i < pd.length; ++i) {
pd[i] = pd1[i] + pd2[i];
for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
final double[] pd1 = evalDownlink.getParameterDerivatives(driver, span.getStart());
final double[] pd2 = evalUplink.getParameterDerivatives(driver, span.getStart());
final double[] pd = new double[pd1.length];
for (int i = 0; i < pd.length; ++i) {
pd[i] = pd1[i] + pd2[i];
}
estimated.setParameterDerivatives(driver, span.getStart(), pd);
}
estimated.setParameterDerivatives(driver, pd);
});
return estimated;
......@@ -285,9 +294,11 @@ public class BistaticRangeRate extends AbstractMeasurement<BistaticRangeRate> {
// set partial derivatives with respect to parameters
for (final ParameterDriver driver : getParametersDrivers()) {
final Integer index = indices.get(driver.getName());
if (index != null) {
estimated.setParameterDerivatives(driver, derivatives[index]);
for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
final Integer index = indices.get(span.getData());
if (index != null) {
estimated.setParameterDerivatives(driver, span.getStart(), derivatives[index]);
}
}
}
......
src/main/java/org/orekit/estimation/measurements/EstimatedEarthFrameProvider.java
View file @ 6096fc33
......@@ -333,15 +333,15 @@ public class EstimatedEarthFrameProvider implements TransformProvider {
final Gradient theta = linearModel(freeParameters, date,
primeMeridianOffsetDriver, primeMeridianDriftDriver,
indices);
final Gradient thetaDot = primeMeridianDriftDriver.getValue(freeParameters, indices);
final Gradient thetaDot = primeMeridianDriftDriver.getValue(freeParameters, indices, date.toAbsoluteDate());
// pole shift parameters
final Gradient xpNeg = linearModel(freeParameters, date,
polarOffsetXDriver, polarDriftXDriver, indices).negate();
final Gradient ypNeg = linearModel(freeParameters, date,
polarOffsetYDriver, polarDriftYDriver, indices).negate();
final Gradient xpNegDot = polarDriftXDriver.getValue(freeParameters, indices).negate();
final Gradient ypNegDot = polarDriftYDriver.getValue(freeParameters, indices).negate();
final Gradient xpNegDot = polarDriftXDriver.getValue(freeParameters, indices, date.toAbsoluteDate()).negate();
final Gradient ypNegDot = polarDriftYDriver.getValue(freeParameters, indices, date.toAbsoluteDate()).negate();
return getTransform(date, theta, thetaDot, xpNeg, xpNegDot, ypNeg, ypNegDot);
......@@ -443,8 +443,8 @@ public class EstimatedEarthFrameProvider implements TransformProvider {
offsetDriver.getName());
}
final Gradient dt = date.durationFrom(offsetDriver.getReferenceDate());
final Gradient offset = offsetDriver.getValue(freeParameters, indices);
final Gradient drift = driftDriver.getValue(freeParameters, indices);
final Gradient offset = offsetDriver.getValue(freeParameters, indices, date.toAbsoluteDate());
final Gradient drift = driftDriver.getValue(freeParameters, indices, date.toAbsoluteDate());
return dt.multiply(drift).add(offset);
}
......
src/main/java/org/orekit/estimation/measurements/EstimatedMeasurement.java
View file @ 6096fc33
......@@ -21,11 +21,14 @@ import java.util.Map;
import java.util.stream.Stream;
import org.orekit.errors.OrekitIllegalArgumentException;
import org.orekit.errors.OrekitIllegalStateException;
import org.orekit.errors.OrekitMessages;
import org.orekit.propagation.SpacecraftState;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.TimeSpanMap;
import org.orekit.utils.TimeStampedPVCoordinates;
import org.orekit.utils.TimeSpanMap.Span;
/** Class holding an estimated theoretical value associated to an {@link ObservedMeasurement observed measurement}.
* @param <T> the type of the measurement
......@@ -59,7 +62,7 @@ public clas