diff --git a/src/main/java/org/orekit/estimation/sequential/EskfMeasurementHandler.java b/src/main/java/org/orekit/estimation/sequential/EskfMeasurementHandler.java
index f072250587f2de187bea19c406fe79438b18bbac..75f3a9d27e1d713b86acc99da11decfa5ad25202 100644
--- a/src/main/java/org/orekit/estimation/sequential/EskfMeasurementHandler.java
+++ b/src/main/java/org/orekit/estimation/sequential/EskfMeasurementHandler.java
@@ -21,12 +21,8 @@ import java.util.List;
import org.hipparchus.exception.MathRuntimeException;
import org.hipparchus.filtering.kalman.ProcessEstimate;
import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter;
-import org.hipparchus.linear.MatrixUtils;
-import org.hipparchus.linear.RealMatrix;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.ObservedMeasurement;
-import org.orekit.estimation.measurements.PV;
-import org.orekit.estimation.measurements.Position;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.sampling.OrekitStepHandler;
import org.orekit.propagation.sampling.OrekitStepInterpolator;
@@ -41,7 +37,7 @@ import org.orekit.time.AbsoluteDate;
*/
public class EskfMeasurementHandler implements OrekitStepHandler {
- /** Least squares model. */
+ /** ESKF model. */
private final SemiAnalyticalKalmanModel model;
/** Extended Kalman Filter. */
@@ -104,7 +100,7 @@ public class EskfMeasurementHandler implements OrekitStepHandler {
model.updateNominalSpacecraftState(interpolator.getInterpolatedState(observedMeasurements.get(index).getDate()));
// Process the current observation
- final ProcessEstimate estimate = filter.estimationStep(decorate(observedMeasurements.get(index)));
+ final ProcessEstimate estimate = filter.estimationStep(KalmanEstimatorUtil.decorate(observedMeasurements.get(index), referenceDate));
// Finalize the estimation
model.finalizeEstimation(observedMeasurements.get(index), estimate);
@@ -128,39 +124,4 @@ public class EskfMeasurementHandler implements OrekitStepHandler {
}
- /** Decorate an observed measurement.
- * <p>
- * The "physical" measurement noise matrix is the covariance matrix of the measurement.
- * Normalizing it consists in applying the following equation: Rn[i,j] = R[i,j]/σ[i]/σ[j]
- * Thus the normalized measurement noise matrix is the matrix of the correlation coefficients
- * between the different components of the measurement.
- * </p>
- * @param observedMeasurement the measurement
- * @return decorated measurement
- */
- private MeasurementDecorator decorate(final ObservedMeasurement<?> observedMeasurement) {
-
- // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
- // of the measurement on its non-diagonal elements.
- // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
- // Normalizing it leaves us with the matrix of the correlation coefficients
- final RealMatrix covariance;
- if (observedMeasurement instanceof PV) {
- // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
- final PV pv = (PV) observedMeasurement;
- covariance = MatrixUtils.createRealMatrix(pv.getCorrelationCoefficientsMatrix());
- } else if (observedMeasurement instanceof Position) {
- // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
- final Position position = (Position) observedMeasurement;
- covariance = MatrixUtils.createRealMatrix(position.getCorrelationCoefficientsMatrix());
- } else {
- // For other measurements we do not have a covariance matrix.
- // Thus the correlation coefficients matrix is an identity matrix.
- covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
- }
-
- return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);
-
- }
-
}
diff --git a/src/main/java/org/orekit/estimation/sequential/KalmanEstimator.java b/src/main/java/org/orekit/estimation/sequential/KalmanEstimator.java
index 6faa7a4fb120d8ae41e9586f4fa84740cc95da92..6cb291a12cc87cad56c20a9da144f824a7d8b12d 100644
--- a/src/main/java/org/orekit/estimation/sequential/KalmanEstimator.java
+++ b/src/main/java/org/orekit/estimation/sequential/KalmanEstimator.java
@@ -22,13 +22,10 @@ import org.hipparchus.exception.MathRuntimeException;
import org.hipparchus.filtering.kalman.ProcessEstimate;
import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter;
import org.hipparchus.linear.MatrixDecomposer;
-import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.ObservedMeasurement;
-import org.orekit.estimation.measurements.PV;
-import org.orekit.estimation.measurements.Position;
import org.orekit.propagation.Propagator;
import org.orekit.propagation.conversion.OrbitDeterminationPropagatorBuilder;
import org.orekit.propagation.conversion.PropagatorBuilder;
@@ -218,7 +215,7 @@ public class KalmanEstimator {
*/
public Propagator[] estimationStep(final ObservedMeasurement<?> observedMeasurement) {
try {
- final ProcessEstimate estimate = filter.estimationStep(decorate(observedMeasurement));
+ final ProcessEstimate estimate = filter.estimationStep(KalmanEstimatorUtil.decorate(observedMeasurement, referenceDate));
processModel.finalizeEstimation(observedMeasurement, estimate);
if (observer != null) {
observer.evaluationPerformed(processModel);
@@ -241,39 +238,4 @@ public class KalmanEstimator {
return propagators;
}
- /** Decorate an observed measurement.
- * <p>
- * The "physical" measurement noise matrix is the covariance matrix of the measurement.
- * Normalizing it consists in applying the following equation: Rn[i,j] = R[i,j]/σ[i]/σ[j]
- * Thus the normalized measurement noise matrix is the matrix of the correlation coefficients
- * between the different components of the measurement.
- * </p>
- * @param observedMeasurement the measurement
- * @return decorated measurement
- */
- private MeasurementDecorator decorate(final ObservedMeasurement<?> observedMeasurement) {
-
- // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
- // of the measurement on its non-diagonal elements.
- // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
- // Normalizing it leaves us with the matrix of the correlation coefficients
- final RealMatrix covariance;
- if (observedMeasurement instanceof PV) {
- // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
- final PV pv = (PV) observedMeasurement;
- covariance = MatrixUtils.createRealMatrix(pv.getCorrelationCoefficientsMatrix());
- } else if (observedMeasurement instanceof Position) {
- // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
- final Position position = (Position) observedMeasurement;
- covariance = MatrixUtils.createRealMatrix(position.getCorrelationCoefficientsMatrix());
- } else {
- // For other measurements we do not have a covariance matrix.
- // Thus the correlation coefficients matrix is an identity matrix.
- covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
- }
-
- return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);
-
- }
-
}
diff --git a/src/main/java/org/orekit/estimation/sequential/KalmanEstimatorUtil.java b/src/main/java/org/orekit/estimation/sequential/KalmanEstimatorUtil.java
index 93e89dfb70e1cfb2b51a205f28f0d06730e907c7..fddddbe19a950dac2c8a7732de0f688a2841d8bc 100644
--- a/src/main/java/org/orekit/estimation/sequential/KalmanEstimatorUtil.java
+++ b/src/main/java/org/orekit/estimation/sequential/KalmanEstimatorUtil.java
@@ -16,8 +16,14 @@
*/
package org.orekit.estimation.sequential;
+import org.hipparchus.linear.MatrixUtils;
+import org.hipparchus.linear.RealMatrix;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
+import org.orekit.estimation.measurements.ObservedMeasurement;
+import org.orekit.estimation.measurements.PV;
+import org.orekit.estimation.measurements.Position;
+import org.orekit.time.AbsoluteDate;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.ParameterDriversList;
@@ -34,6 +40,43 @@ public class KalmanEstimatorUtil {
private KalmanEstimatorUtil() {
}
+ /** Decorate an observed measurement.
+ * <p>
+ * The "physical" measurement noise matrix is the covariance matrix of the measurement.
+ * Normalizing it consists in applying the following equation: Rn[i,j] = R[i,j]/σ[i]/σ[j]
+ * Thus the normalized measurement noise matrix is the matrix of the correlation coefficients
+ * between the different components of the measurement.
+ * </p>
+ * @param observedMeasurement the measurement
+ * @param referenceDate reference date
+ * @return decorated measurements
+ */
+ public static MeasurementDecorator decorate(final ObservedMeasurement<?> observedMeasurement,
+ final AbsoluteDate referenceDate) {
+
+ // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
+ // of the measurement on its non-diagonal elements.
+ // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
+ // Normalizing it leaves us with the matrix of the correlation coefficients
+ final RealMatrix covariance;
+ if (observedMeasurement instanceof PV) {
+ // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
+ final PV pv = (PV) observedMeasurement;
+ covariance = MatrixUtils.createRealMatrix(pv.getCorrelationCoefficientsMatrix());
+ } else if (observedMeasurement instanceof Position) {
+ // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
+ final Position position = (Position) observedMeasurement;
+ covariance = MatrixUtils.createRealMatrix(position.getCorrelationCoefficientsMatrix());
+ } else {
+ // For other measurements we do not have a covariance matrix.
+ // Thus the correlation coefficients matrix is an identity matrix.
+ covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
+ }
+
+ return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);
+
+ }
+
/** Check dimension.
* @param dimension dimension to check
* @param orbitalParameters orbital parameters
diff --git a/src/main/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanEstimator.java b/src/main/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanEstimator.java
index c54a555c9c1abab0762d738b54308918b3631469..dda5fbb66eed6cb5a3bdb72344f7d24ff2a9d33a 100644
--- a/src/main/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanEstimator.java
+++ b/src/main/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanEstimator.java
@@ -27,8 +27,6 @@ import org.hipparchus.util.UnscentedTransformProvider;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.ObservedMeasurement;
import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
-import org.orekit.propagation.conversion.OrbitDeterminationPropagatorBuilder;
-import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.ParameterDriver;
@@ -38,22 +36,18 @@ import org.orekit.utils.ParameterDriversList.DelegatingDriver;
/**
* Implementation of an Unscented Semi-analytical Kalman filter (USKF) to perform orbit determination.
* <p>
- * The filter uses a {@link OrbitDeterminationPropagatorBuilder} to initialize its reference trajectory {@link NumericalPropagator}
- * or {@link DSSTPropagator} .
+ * The filter uses a {@link DSSTPropagatorBuilder}.
* </p>
* <p>
* The estimated parameters are driven by {@link ParameterDriver} objects. They are of 3 different types:<ol>
* <li><b>Orbital parameters</b>:The position and velocity of the spacecraft, or, more generally, its orbit.<br>
* These parameters are retrieved from the reference trajectory propagator builder when the filter is initialized.</li>
- * <li><b>Propagation parameters</b>: Some parameters modelling physical processes (SRP or drag coefficients etc...).<br>
+ * <li><b>Propagation parameters</b>: Some parameters modeling physical processes (SRP or drag coefficients etc...).<br>
* They are also retrieved from the propagator builder during the initialization phase.</li>
* <li><b>Measurements parameters</b>: Parameters related to measurements (station biases, positions etc...).<br>
* They are passed down to the filter in its constructor.</li>
* </ol>
* <p>
- * The total number of estimated parameters is m, the size of the state vector.
- * </p>
- * <p>
* The Kalman filter implementation used is provided by the underlying mathematical library Hipparchus.
* All the variables seen by Hipparchus (states, covariances...) are normalized
* using a specific scale for each estimated parameters or standard deviation noise for each measurement components.
@@ -68,7 +62,7 @@ import org.orekit.utils.ParameterDriversList.DelegatingDriver;
public class SemiAnalyticalUnscentedKalmanEstimator {
/** Builders for orbit propagators. */
- private OrbitDeterminationPropagatorBuilder propagatorBuilder;
+ private DSSTPropagatorBuilder propagatorBuilder;
/** Unscented Kalman filter process model. */
private final SemiAnalyticalUnscentedKalmanModel processModel;
@@ -87,22 +81,25 @@ public class SemiAnalyticalUnscentedKalmanEstimator {
* @param measurementProcessNoiseMatrix provider for measurement process noise matrix
* @param utProvider provider for the unscented transform
*/
-
SemiAnalyticalUnscentedKalmanEstimator(final MatrixDecomposer decomposer,
- final DSSTPropagatorBuilder propagatorBuilder,
- final CovarianceMatrixProvider processNoiseMatricesProvider,
- final ParameterDriversList estimatedMeasurementParameters,
- final CovarianceMatrixProvider measurementProcessNoiseMatrix,
- final UnscentedTransformProvider utProvider) {
+ final DSSTPropagatorBuilder propagatorBuilder,
+ final CovarianceMatrixProvider processNoiseMatricesProvider,
+ final ParameterDriversList estimatedMeasurementParameters,
+ final CovarianceMatrixProvider measurementProcessNoiseMatrix,
+ final UnscentedTransformProvider utProvider) {
this.propagatorBuilder = propagatorBuilder;
this.observer = null;
- this.processModel = new SemiAnalyticalUnscentedKalmanModel(propagatorBuilder, processNoiseMatricesProvider,
- estimatedMeasurementParameters, measurementProcessNoiseMatrix);
- this.filter = new UnscentedKalmanFilter<>(decomposer, processModel, processModel.getEstimate(), utProvider);
+ // Build the process model and measurement model
+ this.processModel = new SemiAnalyticalUnscentedKalmanModel(propagatorBuilder, processNoiseMatricesProvider,
+ estimatedMeasurementParameters, measurementProcessNoiseMatrix);
+
+ // Unscented Kalman Filter of Hipparchus
+ this.filter = new UnscentedKalmanFilter<>(decomposer, processModel, processModel.getEstimate(), utProvider);
}
+
/** Set the observer.
* @param observer the observer
*/
@@ -178,6 +175,7 @@ public class SemiAnalyticalUnscentedKalmanEstimator {
public ParameterDriversList getEstimatedMeasurementsParameters() {
return processModel.getEstimatedMeasurementsParameters();
}
+
/** Process a single measurement.
* <p>
* Update the filter with the new measurement by calling the estimate method.
@@ -193,5 +191,6 @@ public class SemiAnalyticalUnscentedKalmanEstimator {
throw new OrekitException(mrte);
}
}
+
}
diff --git a/src/main/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanEstimatorBuilder.java b/src/main/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanEstimatorBuilder.java
index b645f725990c0cf7c629947fa76011e4384bf1f4..307fa92d75927adb8b3d66d918cb2a131e1b1f99 100644
--- a/src/main/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanEstimatorBuilder.java
+++ b/src/main/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanEstimatorBuilder.java
@@ -18,19 +18,16 @@ package org.orekit.estimation.sequential;
import org.hipparchus.linear.MatrixDecomposer;
import org.hipparchus.linear.QRDecomposer;
-import org.hipparchus.util.MerweUnscentedTransform;
import org.hipparchus.util.UnscentedTransformProvider;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
-import org.orekit.utils.ParameterDriver;
import org.orekit.utils.ParameterDriversList;
/** Builder for an Unscented Semi-analytical Kalman filter estimator.
* @author Gaëtan Pierre
* @author Bryan Cazabonne
*/
-
public class SemiAnalyticalUnscentedKalmanEstimatorBuilder {
/** Decomposer to use for the correction phase. */
@@ -53,24 +50,26 @@ public class SemiAnalyticalUnscentedKalmanEstimatorBuilder {
/** Default constructor.
* Set an Unscented Semi-analytical Kalman filter.
- * By default, a {@link MerweUnscentedTransform} provider is used. It considers that only the orbital parameters are estimated.
- * It is possible to override the default configuration by using {@link #unscentedTransformProvider(UnscentedTransformProvider)}.
*/
-
public SemiAnalyticalUnscentedKalmanEstimatorBuilder() {
this.decomposer = new QRDecomposer(1.0e-15);
this.propagatorBuilder = null;
this.estimatedMeasurementsParameters = new ParameterDriversList();
this.processNoiseMatrixProvider = null;
this.measurementProcessNoiseMatrix = null;
- this.utProvider = new MerweUnscentedTransform(6);
+ this.utProvider = null;
}
/** Construct a {@link SemiAnalyticalUnscentedKalmanEstimator} from the data in this builder.
* <p>
- * Before this method is called, {@link #addPropagationConfiguration(propagatorBuilder,
+ * Before this method is called, {@link #addPropagationConfiguration(DSSTPropagatorBuilder,
* CovarianceMatrixProvider) addPropagationConfiguration()} must have been called
* at least once, otherwise configuration is incomplete and an exception will be raised.
+ * <p>
+ * In addition, the {@link #unscentedTransformProvider(UnscentedTransformProvider)
+ * unscentedTransformProvider()} must be called to configure the unscented transform
+ * provider use during the estimation process, otherwise configuration is
+ * incomplete and an exception will be raised.
* </p>
* @return a new {@link SemiAnalyticalUnscentedKalmanEstimator}.
*/
@@ -78,30 +77,12 @@ public class SemiAnalyticalUnscentedKalmanEstimatorBuilder {
if (propagatorBuilder == null) {
throw new OrekitException(OrekitMessages.NO_PROPAGATOR_CONFIGURED);
}
- // Number of estimated parameters
- int columns = 0;
- for (final ParameterDriver driver : propagatorBuilder.getOrbitalParametersDrivers().getDrivers()) {
- if (driver.isSelected()) {
- columns++;
- }
- }
- for (final ParameterDriver driver : propagatorBuilder.getPropagationParametersDrivers().getDrivers()) {
- if (driver.isSelected()) {
- columns++;
- }
- }
-
- columns = columns + estimatedMeasurementsParameters.getNbParams();
-
- // Check if the number of the selected parameters for the estimation (Orbital + Propagation + Measurement)
- // is equal to the dimension of the state initialized in the unscented transform provider.
- // If not, re-initialize the unscented transform with the appropriate dimension.
- if (columns != (utProvider.getWc().getDimension() - 1) / 2) {
- this.utProvider = new MerweUnscentedTransform(columns);
+ if (utProvider == null) {
+ throw new OrekitException(OrekitMessages.NO_UNSCENTED_TRANSFORM_CONFIGURED);
}
-
return new SemiAnalyticalUnscentedKalmanEstimator(decomposer, propagatorBuilder, processNoiseMatrixProvider,
- estimatedMeasurementsParameters, measurementProcessNoiseMatrix, utProvider);
+ estimatedMeasurementsParameters, measurementProcessNoiseMatrix,
+ utProvider);
}
/** Configure the matrix decomposer.
@@ -132,8 +113,8 @@ public class SemiAnalyticalUnscentedKalmanEstimatorBuilder {
* @return this object.
*/
public SemiAnalyticalUnscentedKalmanEstimatorBuilder addPropagationConfiguration(final DSSTPropagatorBuilder builder,
- final CovarianceMatrixProvider provider) {
- propagatorBuilder = builder;
+ final CovarianceMatrixProvider provider) {
+ propagatorBuilder = builder;
processNoiseMatrixProvider = provider;
return this;
}
@@ -147,10 +128,10 @@ public class SemiAnalyticalUnscentedKalmanEstimatorBuilder {
* @return this object.
*/
public SemiAnalyticalUnscentedKalmanEstimatorBuilder estimatedMeasurementsParameters(final ParameterDriversList estimatedMeasurementsParams,
- final CovarianceMatrixProvider provider) {
+ final CovarianceMatrixProvider provider) {
estimatedMeasurementsParameters = estimatedMeasurementsParams;
measurementProcessNoiseMatrix = provider;
return this;
}
-}
+}
diff --git a/src/main/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanModel.java b/src/main/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanModel.java
index ab0a9fd971f0b85b021b573d96f525adc5d9cb55..f7ed2a966e6ffcbcb5857dffda16c0a3d632f385 100644
--- a/src/main/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanModel.java
+++ b/src/main/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanModel.java
@@ -22,7 +22,6 @@ import java.util.HashMap;
import java.util.List;
import java.util.Map;
-import org.hipparchus.exception.MathRuntimeException;
import org.hipparchus.filtering.kalman.ProcessEstimate;
import org.hipparchus.filtering.kalman.unscented.UnscentedEvolution;
import org.hipparchus.filtering.kalman.unscented.UnscentedKalmanFilter;
@@ -32,8 +31,6 @@ import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.hipparchus.util.FastMath;
-import org.orekit.errors.OrekitException;
-import org.orekit.errors.OrekitMessages;
import org.orekit.estimation.measurements.EstimatedMeasurement;
import org.orekit.estimation.measurements.EstimationModifier;
import org.orekit.estimation.measurements.ObservedMeasurement;
@@ -42,8 +39,6 @@ import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.PropagationType;
-import org.orekit.propagation.Propagator;
-import org.orekit.propagation.PropagatorsParallelizer;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
@@ -56,20 +51,15 @@ import org.orekit.utils.ParameterDriver;
import org.orekit.utils.ParameterDriversList;
import org.orekit.utils.ParameterDriversList.DelegatingDriver;
-/** Class defining the process model dynamics to use with a {@link UnscentedKalmanEstimator}.
+/** Class defining the process model dynamics to use with a {@link SemiAnalyticalUnscentedKalmanEstimator}.
* @author Gaëtan Pierre
+ * @author Bryan Cazabonne
*/
public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, UnscentedProcess<MeasurementDecorator> {
/** Initial builder for propagator. */
private final DSSTPropagatorBuilder builder;
- /** Builders for propagators. */
- private List<DSSTPropagatorBuilder> propagatorBuilders;
-
- /** Propagators. */
- private List<Propagator> propagators;
-
/** Estimated orbital parameters. */
private final ParameterDriversList estimatedOrbitalParameters;
@@ -79,15 +69,6 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
/** Estimated measurements parameters. */
private final ParameterDriversList estimatedMeasurementsParameters;
- /** Map for propagation parameters columns. */
- private final Map<String, Integer> propagationParameterColumns;
-
- /** Map for measurements parameters columns. */
- private final Map<String, Integer> measurementParameterColumns;
-
- /** Scaling factors. */
- private final double[] scale;
-
/** Provider for covariance matrice. */
private final CovarianceMatrixProvider covarianceMatrixProvider;
@@ -97,6 +78,9 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
/** Position angle type used during orbit determination. */
private final PositionAngle angleType;
+ /** Orbit type used during orbit determination. */
+ private final OrbitType orbitType;
+
/** Current corrected estimate. */
private ProcessEstimate correctedEstimate;
@@ -106,23 +90,20 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
/** Current number of measurement. */
private int currentMeasurementNumber;
- /** Reference date. */
- private AbsoluteDate referenceDate;
-
/** Current date. */
private AbsoluteDate currentDate;
- /** Predicted spacecraft states. */
- private SpacecraftState predictedSpacecraftState;
+ /** Nominal mean spacecraft state. */
+ private SpacecraftState nominalMeanSpacecraftState;
- /** Corrected spacecraft states. */
- private SpacecraftState correctedSpacecraftState;
+ /** Previous nominal mean spacecraft state. */
+ private SpacecraftState previousNominalMeanSpacecraftState;
- /** Nominal mean spacecraft states. */
- private List<SpacecraftState> nominalMeanSpacecraftStates;
+ /** Predicted osculating spacecraft state. */
+ private SpacecraftState predictedSpacecraftState;
- /** Previous nominal mean spacecraft states. */
- private List<SpacecraftState> previousNominalMeanSpacecraftStates;
+ /** Corrected mean spacecraft state. */
+ private SpacecraftState correctedSpacecraftState;
/** Predicted measurement. */
private EstimatedMeasurement<?> predictedMeasurement;
@@ -130,6 +111,14 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
/** Corrected measurement. */
private EstimatedMeasurement<?> correctedMeasurement;
+ /** Predicted mean element filter correction. */
+ private RealVector predictedFilterCorrection;
+
+ /** Corrected mean element filter correction. */
+ private RealVector correctedFilterCorrection;
+
+ /** Propagators for the reference trajectories, up to current date. */
+ private DSSTPropagator dsstPropagator;
/** Unscented Kalman process model constructor (package private).
* @param propagatorBuilder propagators builders used to evaluate the orbits.
@@ -138,26 +127,24 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
* @param measurementProcessNoiseMatrix provider for measurement process noise matrix
*/
protected SemiAnalyticalUnscentedKalmanModel(final DSSTPropagatorBuilder propagatorBuilder,
- final CovarianceMatrixProvider covarianceMatrixProvider,
- final ParameterDriversList estimatedMeasurementParameters,
- final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
+ final CovarianceMatrixProvider covarianceMatrixProvider,
+ final ParameterDriversList estimatedMeasurementParameters,
+ final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
this.builder = propagatorBuilder;
+ this.angleType = propagatorBuilder.getPositionAngle();
+ this.orbitType = propagatorBuilder.getOrbitType();
this.estimatedMeasurementsParameters = estimatedMeasurementParameters;
- this.measurementParameterColumns = new HashMap<>(estimatedMeasurementsParameters.getDrivers().size());
this.currentMeasurementNumber = 0;
- this.referenceDate = propagatorBuilder.getInitialOrbitDate();
- this.currentDate = referenceDate;
+ this.currentDate = propagatorBuilder.getInitialOrbitDate();
this.covarianceMatrixProvider = covarianceMatrixProvider;
this.measurementProcessNoiseMatrix = measurementProcessNoiseMatrix;
- this.propagators = new ArrayList<>();
- this.propagatorBuilders = new ArrayList<>();
- this.angleType = builder.getPositionAngle();
+
// Number of estimated parameters
int columns = 0;
// Set estimated orbital parameters
- estimatedOrbitalParameters = new ParameterDriversList();
+ this.estimatedOrbitalParameters = new ParameterDriversList();
for (final ParameterDriver driver : propagatorBuilder.getOrbitalParametersDrivers().getDrivers()) {
// Verify if the driver reference date has been set
@@ -174,7 +161,7 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
}
// Set estimated propagation parameters
- estimatedPropagationParameters = new ParameterDriversList();
+ this.estimatedPropagationParameters = new ParameterDriversList();
final List<String> estimatedPropagationParametersNames = new ArrayList<>();
for (final ParameterDriver driver : propagatorBuilder.getPropagationParametersDrivers().getDrivers()) {
@@ -197,7 +184,7 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
estimatedPropagationParametersNames.sort(Comparator.naturalOrder());
// Populate the map of propagation drivers' columns and update the total number of columns
- propagationParameterColumns = new HashMap<>(estimatedPropagationParametersNames.size());
+ final Map<String, Integer> propagationParameterColumns = new HashMap<>(estimatedPropagationParametersNames.size());
for (final String driverName : estimatedPropagationParametersNames) {
propagationParameterColumns.put(driverName, columns);
++columns;
@@ -208,37 +195,9 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
if (parameter.getReferenceDate() == null) {
parameter.setReferenceDate(currentDate);
}
- measurementParameterColumns.put(parameter.getName(), columns);
++columns;
}
- // Compute the scale factors
- this.scale = new double[columns];
- int index = 0;
- for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
- scale[index++] = driver.getScale();
- }
- for (final ParameterDriver driver : estimatedPropagationParameters.getDrivers()) {
- scale[index++] = driver.getScale();
- }
- for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
- scale[index++] = driver.getScale();
- }
-
- // Initialize the estimated normalized state and fill its values
- final RealVector correctedState = MatrixUtils.createRealVector(columns);
-
- int p = 0;
- for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
- correctedState.setEntry(p++, driver.getValue());
- }
- for (final ParameterDriver driver : estimatedPropagationParameters.getDrivers()) {
- correctedState.setEntry(p++, driver.getValue());
- }
- for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
- correctedState.setEntry(p++, driver.getValue());
- }
-
// Number of estimated measurement parameters
final int nbMeas = estimatedMeasurementParameters.getNbParams();
@@ -246,29 +205,36 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
final int nbDyn = estimatedOrbitalParameters.getNbParams() +
estimatedPropagationParameters.getNbParams();
- this.predictedSpacecraftState = propagatorBuilder.buildPropagator(propagatorBuilder.getSelectedNormalizedParameters()).getInitialState();
- this.correctedSpacecraftState = predictedSpacecraftState;
+ // Build the reference propagator
+ this.dsstPropagator = getEstimatedPropagator();
+ final SpacecraftState meanState = dsstPropagator.initialIsOsculating() ?
+ DSSTPropagator.computeMeanState(dsstPropagator.getInitialState(), dsstPropagator.getAttitudeProvider(), dsstPropagator.getAllForceModels()) :
+ dsstPropagator.getInitialState();
+ this.nominalMeanSpacecraftState = meanState;
+ this.predictedSpacecraftState = meanState;
+ this.correctedSpacecraftState = predictedSpacecraftState;
+ this.previousNominalMeanSpacecraftState = nominalMeanSpacecraftState;
+
+ // Initialize the estimated mean element filter correction
+ this.predictedFilterCorrection = MatrixUtils.createRealVector(columns);
+ this.correctedFilterCorrection = predictedFilterCorrection;
// Covariance matrix
final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
- final RealMatrix noiseP = covarianceMatrixProvider.getInitialCovarianceMatrix(predictedSpacecraftState);
-
+ final RealMatrix noiseP = covarianceMatrixProvider.getInitialCovarianceMatrix(nominalMeanSpacecraftState);
noiseK.setSubMatrix(noiseP.getData(), 0, 0);
-
if (measurementProcessNoiseMatrix != null) {
- final RealMatrix noiseM = measurementProcessNoiseMatrix.
- getInitialCovarianceMatrix(correctedSpacecraftState);
+ final RealMatrix noiseM = measurementProcessNoiseMatrix.getInitialCovarianceMatrix(nominalMeanSpacecraftState);
noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
}
- checkDimension(noiseK.getRowDimension(),
- propagatorBuilder.getOrbitalParametersDrivers(),
- propagatorBuilder.getPropagationParametersDrivers(),
- estimatedMeasurementsParameters);
-
+ KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
+ propagatorBuilder.getOrbitalParametersDrivers(),
+ propagatorBuilder.getPropagationParametersDrivers(),
+ estimatedMeasurementsParameters);
// Initialize corrected estimate
- this.correctedEstimate = new ProcessEstimate(0.0, correctedState, noiseK);
+ this.correctedEstimate = new ProcessEstimate(0.0, correctedFilterCorrection, noiseK);
}
@@ -286,59 +252,63 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
this.observer = observer;
}
- /** Process a single measurement.
- * <p>
- * Update the filter with the new measurements.
- * </p>
+ /** Get the orbit type used during the estimation process.
+ * @return the orbit type
+ */
+ public OrbitType getOrbitType() {
+ return orbitType;
+ }
+
+ /** Get the position angle type used during the estimation process.
+ * @return the position angle type
+ */
+ public PositionAngle getAngleType() {
+ return angleType;
+ }
+
+ /** Get the current corrected estimate.
+ * @return current corrected estimate
+ */
+ public ProcessEstimate getEstimate() {
+ return correctedEstimate;
+ }
+
+ /** Process measurements.
* @param observedMeasurements the list of measurements to process
- * @param filter Extended Kalman Filter
+ * @param filter Unscented Kalman Filter
* @return estimated propagator
*/
public DSSTPropagator processMeasurements(final List<ObservedMeasurement<?>> observedMeasurements,
final UnscentedKalmanFilter<MeasurementDecorator> filter) {
- try {
- // Compute sigma points
- final ProcessEstimate estimate = filter.getCorrected();
- final RealVector[] sigmaPoints = filter.getUnscentedTransformProvider().unscentedTransform(estimate.getState(), estimate.getCovariance());
-
- for (int k = 0; k < sigmaPoints.length; ++k) {
- // Current sigma point
- final double[] currentPoint = sigmaPoints[k].copy().toArray();
- // Create the corresponding orbit propagator
- final DSSTPropagator currentPropagator = createPropagator(currentPoint);
- // Add it to the list of propagators
- propagators.add(currentPropagator);
- }
-
- // Create builders using the sigma points
- propagatorBuilders = getEstimatedBuilders(sigmaPoints);
- // Initialize spacecraft states
- initializeMeanSpacecraftStates(sigmaPoints);
+ // Sort the measurement
+ observedMeasurements.sort(new ChronologicalComparator());
- // Sort the measurement
- observedMeasurements.sort(new ChronologicalComparator());
+ // Initialize step handler and set it to a parallelized propagator
+ final UskfMeasurementHandler stepHandler = new UskfMeasurementHandler(this, filter, observedMeasurements, builder.getInitialOrbitDate());
+ dsstPropagator.getMultiplexer().add(stepHandler);
+ dsstPropagator.propagate(observedMeasurements.get(0).getDate(), observedMeasurements.get(observedMeasurements.size() - 1).getDate());
- final UskfMeasurementHandler stepHandler = new UskfMeasurementHandler(this, filter, observedMeasurements, builder.getInitialOrbitDate(), propagators, propagatorBuilders, sigmaPoints);
- final PropagatorsParallelizer parallelizer = new PropagatorsParallelizer(propagators, stepHandler);
+ // Return the last estimated propagator
+ return getEstimatedPropagator();
- parallelizer.propagate(observedMeasurements.get(0).getDate(), observedMeasurements.get(observedMeasurements.size() - 1).getDate());
-
- // Return the last estimated propagator
- return createPropagator(correctedEstimate.getState().toArray());
+ }
- } catch (MathRuntimeException mrte) {
- throw new OrekitException(mrte);
- }
+ /** Get the propagator estimated with the values set in the propagator builder.
+ * @return propagator based on the current values in the builder
+ */
+ public DSSTPropagator getEstimatedPropagator() {
+ // Return propagator built with current instantiation of the propagator builder
+ return builder.buildPropagator(builder.getSelectedNormalizedParameters());
}
+
+ /** {@inheritDoc} */
@Override
public UnscentedEvolution getEvolution(final double previousTime, final RealVector[] sigmaPoints,
- final MeasurementDecorator measurement) {
+ final MeasurementDecorator measurement) {
// Set a reference date for all measurements parameters that lack one (including the not estimated ones)
final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
- final RealVector[] predictedMeasurements = new RealVector[sigmaPoints.length];
-
for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
if (driver.getReferenceDate() == null) {
driver.setReferenceDate(builder.getInitialOrbitDate());
@@ -350,25 +320,23 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
// Update the current date
currentDate = measurement.getObservedMeasurement().getDate();
- final RealVector[] nominalMeanElementsStates = computeMeanElementsStates();
- // Calculate the predicted osculating elements
- final RealVector[] osculating = computeOsculatingElementsStates();
- for (int i = 0; i < osculating.length; i++) {
- final Orbit osculatingOrbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(osculating[i].toArray(), null, builder.getPositionAngle(),
- currentDate, nominalMeanSpacecraftStates.get(i).getMu(),
- nominalMeanSpacecraftStates.get(i).getFrame());
+ // Estimate the measurement for each predicted state
+ final RealVector[] predictedMeasurements = new RealVector[sigmaPoints.length];
+ for (int k = 0; k < sigmaPoints.length; ++k) {
+ // Calculate the predicted osculating elements for the current mean state
+ final RealVector osculating = computeOsculatingElements(sigmaPoints[k], nominalMeanSpacecraftState);
+ final Orbit osculatingOrbit = orbitType.mapArrayToOrbit(osculating.toArray(), null, angleType,
+ currentDate, builder.getMu(), builder.getFrame());
+
+ // Then, estimate the measurement
final EstimatedMeasurement<?> estimated = observedMeasurement.estimate(currentMeasurementNumber,
- currentMeasurementNumber,
- new SpacecraftState[] {
- new SpacecraftState(osculatingOrbit,
- nominalMeanSpacecraftStates.get(i).getAttitude(),
- nominalMeanSpacecraftStates.get(i).getMass(),
- nominalMeanSpacecraftStates.get(i).getAdditionalStatesValues(),
- nominalMeanSpacecraftStates.get(i).getAdditionalStatesDerivatives())
- });
- predictedMeasurements[i] = new ArrayRealVector(estimated.getEstimatedValue());
+ currentMeasurementNumber,
+ new SpacecraftState[] {
+ new SpacecraftState(osculatingOrbit)
+ });
+ predictedMeasurements[k] = new ArrayRealVector(estimated.getEstimatedValue());
}
// Number of estimated measurement parameters
@@ -379,40 +347,40 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
// Covariance matrix
final RealMatrix noiseK = MatrixUtils.createRealMatrix(nbDyn + nbMeas, nbDyn + nbMeas);
- final RealMatrix noiseP = covarianceMatrixProvider.getProcessNoiseMatrix(correctedSpacecraftState, predictedSpacecraftState);
+ final RealMatrix noiseP = covarianceMatrixProvider.getProcessNoiseMatrix(previousNominalMeanSpacecraftState, nominalMeanSpacecraftState);
noiseK.setSubMatrix(noiseP.getData(), 0, 0);
if (measurementProcessNoiseMatrix != null) {
- final RealMatrix noiseM = measurementProcessNoiseMatrix.getProcessNoiseMatrix(correctedSpacecraftState, predictedSpacecraftState);
+ final RealMatrix noiseM = measurementProcessNoiseMatrix.getProcessNoiseMatrix(previousNominalMeanSpacecraftState, nominalMeanSpacecraftState);
noiseK.setSubMatrix(noiseM.getData(), nbDyn, nbDyn);
}
// Verify dimension
- checkDimension(noiseK.getRowDimension(),
- builder.getOrbitalParametersDrivers(),
- builder.getPropagationParametersDrivers(),
- estimatedMeasurementsParameters);
-
+ KalmanEstimatorUtil.checkDimension(noiseK.getRowDimension(),
+ builder.getOrbitalParametersDrivers(),
+ builder.getPropagationParametersDrivers(),
+ estimatedMeasurementsParameters);
+ // Return
+ return new UnscentedEvolution(measurement.getTime(), sigmaPoints, predictedMeasurements, noiseK);
- return new UnscentedEvolution(measurement.getTime(), nominalMeanElementsStates, predictedMeasurements, noiseK);
}
+ /** {@inheritDoc} */
@Override
- public RealVector getInnovation(final MeasurementDecorator measurement, final RealVector predictedMeas, final RealVector predictedState,
- final RealMatrix innovationCovarianceMatrix) {
-
+ public RealVector getInnovation(final MeasurementDecorator measurement, final RealVector predictedMeas,
+ final RealVector predictedState, final RealMatrix innovationCovarianceMatrix) {
+ // Predicted filter correction
+ predictedFilterCorrection = predictedState;
- // Calculate the corrected osculating elements
- final double[] osculating = computeOsculatingElements(predictedState);
- final Orbit osculatingOrbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(osculating, null, angleType,
- currentDate, builder.getMu(),
- builder.getFrame());
+ // Predicted measurement
+ final RealVector osculating = computeOsculatingElements(predictedFilterCorrection, nominalMeanSpacecraftState);
+ final Orbit osculatingOrbit = orbitType.mapArrayToOrbit(osculating.toArray(), null, angleType,
+ currentDate, builder.getMu(), builder.getFrame());
predictedSpacecraftState = new SpacecraftState(osculatingOrbit);
- predictedMeasurement = measurement.getObservedMeasurement().estimate(currentMeasurementNumber, currentMeasurementNumber, new SpacecraftState[] {predictedSpacecraftState});
-// predictedMeasurement.setEstimatedValue(predictedMeas.toArray()); // Regression on testRangeWithTesseral() and testRangeWithZonal() if uncommented
+ predictedMeasurement = measurement.getObservedMeasurement().estimate(currentMeasurementNumber, currentMeasurementNumber, getPredictedSpacecraftStates());
- // set estimated value to the predicted value by the filter
+ // Apply the dynamic outlier filter, if it exists
applyDynamicOutlierFilter(predictedMeasurement, innovationCovarianceMatrix);
if (predictedMeasurement.getStatus() == EstimatedMeasurement.Status.REJECTED) {
// set innovation to null to notify filter measurement is rejected
@@ -428,6 +396,7 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
}
return MatrixUtils.createRealVector(residuals);
}
+
}
@@ -437,70 +406,32 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
*/
public void finalizeEstimation(final ObservedMeasurement<?> observedMeasurement,
final ProcessEstimate estimate) {
+ // Update the process estimate
correctedEstimate = estimate;
- previousNominalMeanSpacecraftStates = nominalMeanSpacecraftStates;
+ // Corrected filter correction
+ correctedFilterCorrection = estimate.getState();
- final double[] osculating = computeOsculatingElements(estimate.getState());
- final Orbit osculatingOrbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(osculating, null, angleType,
- currentDate, builder.getMu(),
- builder.getFrame());
- correctedSpacecraftState = new SpacecraftState(osculatingOrbit);
+ // Update the previous nominal mean spacecraft state
+ previousNominalMeanSpacecraftState = nominalMeanSpacecraftState;
+
+ // Update the previous nominal mean spacecraft state
+ // Calculate the corrected osculating elements
+ final RealVector osculating = computeOsculatingElements(correctedFilterCorrection, nominalMeanSpacecraftState);
+ final Orbit osculatingOrbit = orbitType.mapArrayToOrbit(osculating.toArray(), null, builder.getPositionAngle(),
+ currentDate, builder.getMu(), builder.getFrame());
// Compute the corrected measurements
+ correctedSpacecraftState = new SpacecraftState(osculatingOrbit);
correctedMeasurement = observedMeasurement.estimate(currentMeasurementNumber,
currentMeasurementNumber,
- new SpacecraftState[] {
- new SpacecraftState(osculatingOrbit,
- correctedSpacecraftState.getAttitude(),
- correctedSpacecraftState.getMass(),
- correctedSpacecraftState.getAdditionalStatesValues(),
- correctedSpacecraftState.getAdditionalStatesDerivatives())
- });
+ getCorrectedSpacecraftStates());
- // Update current date in the builder
- builder.resetOrbit(correctedSpacecraftState.getOrbit());
}
-
- /** Get the current corrected estimate.
- * @return current corrected estimate
- */
- public ProcessEstimate getEstimate() {
- return correctedEstimate;
- }
-
-
- /** Get the builder estimated with the values of parameters.
- * @param parameters array containing orbital parameters
- * @return builder based on the values of parameters
- */
- public DSSTPropagatorBuilder getEstimatedBuilder(final double[] parameters) {
- final DSSTPropagatorBuilder copy = builder.copy();
- int j = 0;
- for (DelegatingDriver orbitalDriver : copy.getOrbitalParametersDrivers().getDrivers()) {
- if (orbitalDriver.isSelected()) {
- orbitalDriver.setValue(parameters[j++]);
- }
- }
- for (DelegatingDriver propagationDriver : copy.getPropagationParametersDrivers().getDrivers()) {
- if (propagationDriver.isSelected()) {
- propagationDriver.setValue(parameters[j++]);
-
- }
- }
-
- return copy;
- }
-
-
- public List<DSSTPropagatorBuilder> getEstimatedBuilders(final RealVector[] sigmaPoints) {
- /** Propagators */
- final List<DSSTPropagatorBuilder> builders = new ArrayList<DSSTPropagatorBuilder>();
- for (int i = 0; i < sigmaPoints.length; i++) {
- final double[] currentPoint = sigmaPoints[i].copy().toArray();
- builders.add(getEstimatedBuilder(currentPoint));
- }
- return builders;
+ /** Finalize estimation operations on the observation grid. */
+ public void finalizeOperationsObservationGrid() {
+ // Update parameters
+ updateParameters();
}
/** {@inheritDoc} */
@@ -539,7 +470,7 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
// Method {@link ParameterDriver#getValue()} is used to get
// the physical values of the state.
// The scales'array is used to get the size of the state vector
- final RealVector physicalEstimatedState = new ArrayRealVector(scale.length);
+ final RealVector physicalEstimatedState = new ArrayRealVector(getEstimate().getState().getDimension());
int i = 0;
for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
physicalEstimatedState.setEntry(i++, driver.getValue());
@@ -557,7 +488,6 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
/** {@inheritDoc} */
@Override
public RealMatrix getPhysicalEstimatedCovarianceMatrix() {
-
return correctedEstimate.getCovariance();
}
@@ -584,16 +514,19 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
public RealMatrix getPhysicalKalmanGain() {
return correctedEstimate.getKalmanGain();
}
+
/** {@inheritDoc} */
@Override
public int getCurrentMeasurementNumber() {
return currentMeasurementNumber;
}
+
/** {@inheritDoc} */
@Override
public AbsoluteDate getCurrentDate() {
return currentDate;
}
+
/** {@inheritDoc} */
@Override
public EstimatedMeasurement<?> getPredictedMeasurement() {
@@ -606,198 +539,36 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
return correctedMeasurement;
}
+ /** Update the nominal spacecraft state.
+ * @param nominal nominal spacecraft state
+ */
+ public void updateNominalSpacecraftState(final SpacecraftState nominal) {
+ this.nominalMeanSpacecraftState = nominal;
+ // Update the builder with the nominal mean elements orbit
+ builder.resetOrbit(nominal.getOrbit(), PropagationType.MEAN);
+ }
+
/** Update the DSST short periodic terms.
* @param state current mean state
- * @param dsstBuilder builder associated to the state
*/
- public void updateShortPeriods(final SpacecraftState state, final DSSTPropagatorBuilder dsstBuilder) {
+ public void updateShortPeriods(final SpacecraftState state) {
// Loop on DSST force models
- for (final DSSTForceModel model : dsstBuilder.getAllForceModels()) {
+ for (final DSSTForceModel model : dsstPropagator.getAllForceModels()) {
model.updateShortPeriodTerms(model.getParameters(), state);
}
-
- }
- public void initializeMeanSpacecraftStates(final RealVector[] sigmaPoints) {
- nominalMeanSpacecraftStates = new ArrayList<>();
- previousNominalMeanSpacecraftStates = new ArrayList<>();
- for (int i = 0; i < propagators.size(); i++) {
- final Orbit orbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(sigmaPoints[i].toArray(), null, angleType,
- currentDate, builder.getMu(),
- builder.getFrame());
- nominalMeanSpacecraftStates.add(new SpacecraftState(orbit));
- previousNominalMeanSpacecraftStates.add(nominalMeanSpacecraftStates.get(i));
- }
}
+
/** Initialize the short periodic terms for the Kalman Filter.
* @param meanState mean state for auxiliary elements
- * @param propagator propagator associated to the state
*/
- public void initializeShortPeriodicTerms(final SpacecraftState meanState, final Propagator propagator) {
-
+ public void initializeShortPeriodicTerms(final SpacecraftState meanState) {
final List<ShortPeriodTerms> shortPeriodTerms = new ArrayList<ShortPeriodTerms>();
- for (final DSSTForceModel force : ((DSSTPropagator) propagator).getAllForceModels()) {
+ for (final DSSTForceModel force : builder.getAllForceModels()) {
shortPeriodTerms.addAll(force.initializeShortPeriodTerms(new AuxiliaryElements(meanState.getOrbit(), 1), PropagationType.OSCULATING, force.getParameters()));
}
- ((DSSTPropagator) propagator).setShortPeriodTerms(shortPeriodTerms);
- }
- /**
- * Create a propagator for the given sigma point.
- * @param point input sigma point
- * @return the corresponding orbit propagator
- */
- private DSSTPropagator createPropagator(final double[] point) {
- // Create a new instance of the current propagator builder
- final DSSTPropagatorBuilder copy = builder.copy();
- // Convert the given sigma point to an orbit
- final Orbit orbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(point, null, angleType, copy.getInitialOrbitDate(),
- copy.getMu(), copy.getFrame());
- copy.resetOrbit(orbit);
- // Create the propagator
- final DSSTPropagator propagator = copy.buildPropagator(copy.getSelectedNormalizedParameters());
- return propagator;
- }
-
- /** Check dimension.
- * @param dimension dimension to check
- * @param orbitalParameters orbital parameters
- * @param propagationParameters propagation parameters
- * @param measurementParameters measurements parameters
- */
- private void checkDimension(final int dimension,
- final ParameterDriversList orbitalParameters,
- final ParameterDriversList propagationParameters,
- final ParameterDriversList measurementParameters) {
-
- // count parameters, taking care of counting all orbital parameters
- // regardless of them being estimated or not
- int requiredDimension = orbitalParameters.getNbParams();
- for (final ParameterDriver driver : propagationParameters.getDrivers()) {
- if (driver.isSelected()) {
- ++requiredDimension;
- }
- }
- for (final ParameterDriver driver : measurementParameters.getDrivers()) {
- if (driver.isSelected()) {
- ++requiredDimension;
- }
- }
-
- if (dimension != requiredDimension) {
- // there is a problem, set up an explicit error message
- final StringBuilder sBuilder = new StringBuilder();
- for (final ParameterDriver driver : orbitalParameters.getDrivers()) {
- if (sBuilder.length() > 0) {
- sBuilder.append(", ");
- }
- sBuilder.append(driver.getName());
- }
- for (final ParameterDriver driver : propagationParameters.getDrivers()) {
- if (driver.isSelected()) {
- sBuilder.append(driver.getName());
- }
- }
- for (final ParameterDriver driver : measurementParameters.getDrivers()) {
- if (driver.isSelected()) {
- sBuilder.append(driver.getName());
- }
- }
- throw new OrekitException(OrekitMessages.DIMENSION_INCONSISTENT_WITH_PARAMETERS,
- dimension, builder.toString());
- }
-
- }
-
-
- /** Compute the predicted osculating elements.
- * @return the predicted osculating element
- */
- private RealVector[] computeOsculatingElementsStates() {
-
- // Number of estimated orbital parameters
- final RealVector[] osculatingElementsStates = new RealVector[nominalMeanSpacecraftStates.size()];
- final int nbOrb = getNumberSelectedOrbitalDrivers();
- for (int i = 0; i < nominalMeanSpacecraftStates.size(); i++) {
- final double[] shortPeriodTerms = ((DSSTPropagator) propagators.get(i)).getShortPeriodTermsValue(nominalMeanSpacecraftStates.get(i));
- // Nominal mean elements
- final double[] nominalMeanElements = new double[nbOrb];
- OrbitType.EQUINOCTIAL.mapOrbitToArray(nominalMeanSpacecraftStates.get(i).getOrbit(), propagatorBuilders.get(i).getPositionAngle(), nominalMeanElements, null);
- // Ref [1] Eq. 3.6
- final double[] osculatingElements = new double[nbOrb];
- for (int j = 0; j < nbOrb; j++) {
- osculatingElements[j] = nominalMeanElements[j] +
- shortPeriodTerms[j];
- }
- osculatingElementsStates[i] = new ArrayRealVector(osculatingElements);
- }
-
- return osculatingElementsStates;
-
- }
- /** Convert the nominal mean spacecraft state into array of RealVector.
- * @return mean elements
- */
- private RealVector[] computeMeanElementsStates() {
- // Number of estimated orbital parameters
- final RealVector[] meanElementsStates = new RealVector[nominalMeanSpacecraftStates.size()];
- final int nbOrb = getNumberSelectedOrbitalDrivers();
- for (int i = 0; i < nominalMeanSpacecraftStates.size(); i++) {
- // Nominal mean elements
- final double[] nominalMeanElements = new double[nbOrb];
- OrbitType.EQUINOCTIAL.mapOrbitToArray(nominalMeanSpacecraftStates.get(i).getOrbit(), propagatorBuilders.get(i).getPositionAngle(), nominalMeanElements, null);
- meanElementsStates[i] = new ArrayRealVector(nominalMeanElements);
-
- }
- return meanElementsStates;
-
+ dsstPropagator.setShortPeriodTerms(shortPeriodTerms);
}
- /** Compute the predicted osculating elements.
- * @param state state whose osculating elements are supposed to be computed
- * @return the predicted osculating element
- */
- protected double[] computeOsculatingElements(final RealVector state) {
-
- // Number of estimated orbital parameters
- // update the predicted spacecraft state with predictedState
- final Orbit orbit = builder.getOrbitType().mapArrayToOrbit(state.toArray(), null, angleType,
- currentDate, builder.getMu(), builder.getFrame());
- final SpacecraftState s = new SpacecraftState(orbit);
- final int nbOrb = getNumberSelectedOrbitalDrivers();
- final double[] shortPeriodTerms = ((DSSTPropagator) propagators.get(0)).getShortPeriodTermsValue(s);
- // Nominal mean elements
- final double[] nominalMeanElements = new double[nbOrb];
- OrbitType.EQUINOCTIAL.mapOrbitToArray(s.getOrbit(), angleType, nominalMeanElements, null);
- // Ref [1] Eq. 3.6
- final double[] osculatingElements = new double[nbOrb];
- for (int j = 0; j < nbOrb; j++) {
- osculatingElements[j] = nominalMeanElements[j] +
- shortPeriodTerms[j];
- }
- return osculatingElements;
-
- }
- /** Compute the predicted osculating elements.
- * @param s Spacecraft state whose osculating elements are supposed to be computed
- * @param index the index associated to the state
- * @return the predicted osculating element
- */
- protected double[] computeOsculatingElements(final SpacecraftState s, final int index) {
-
- // Number of estimated orbital parameters
- final int nbOrb = getNumberSelectedOrbitalDrivers();
- final double[] shortPeriodTerms = ((DSSTPropagator) propagators.get(index)).getShortPeriodTermsValue(s);
- // Nominal mean elements
- final double[] nominalMeanElements = new double[nbOrb];
- OrbitType.EQUINOCTIAL.mapOrbitToArray(s.getOrbit(), angleType, nominalMeanElements, null);
- // Ref [1] Eq. 3.6
- final double[] osculatingElements = new double[nbOrb];
- for (int j = 0; j < nbOrb; j++) {
- osculatingElements[j] = nominalMeanElements[j] +
- shortPeriodTerms[j];
- }
- return osculatingElements;
-
- }
/** Set and apply a dynamic outlier filter on a measurement.<p>
* Loop on the modifiers to see if a dynamic outlier filter needs to be applied.<p>
* Compute the sigma array using the matrix in input and set the filter.<p>
@@ -842,53 +613,78 @@ public class SemiAnalyticalUnscentedKalmanModel implements KalmanEstimation, Uns
}
}
+ /** Compute the predicted osculating elements.
+ * @param filterCorrection physical kalman filter correction
+ * @param meanState mean spacecraft state
+ * @return the predicted osculating element
+ */
+ private RealVector computeOsculatingElements(final RealVector filterCorrection, final SpacecraftState meanState) {
+
+ // Convert the input predicted mean state to a SpacecraftState
+ final RealVector stateVector = toRealVector(meanState);
+
+ // Short periodic terms
+ final RealVector shortPeriodTerms = new ArrayRealVector(dsstPropagator.getShortPeriodTermsValue(meanState));
+
+ // Return
+ return stateVector.add(filterCorrection).add(shortPeriodTerms);
+
+ }
+
+ /** Convert a SpacecraftState to a RealVector.
+ * @param state the input SpacecraftState
+ * @return the corresponding RealVector
+ */
+ private RealVector toRealVector(final SpacecraftState state) {
+
+ // Convert orbit to array
+ final double[] stateArray = new double[6];
+ orbitType.mapOrbitToArray(state.getOrbit(), angleType, stateArray, null);
+
+ // Return the SpacecraftState
+ return new ArrayRealVector(stateArray);
+
+ }
+
/** Get the number of estimated orbital parameters.
* @return the number of estimated orbital parameters
*/
- private int getNumberSelectedOrbitalDrivers() {
+ public int getNumberSelectedOrbitalDrivers() {
return estimatedOrbitalParameters.getNbParams();
}
/** Get the number of estimated propagation parameters.
* @return the number of estimated propagation parameters
*/
- private int getNumberSelectedPropagationDrivers() {
+ public int getNumberSelectedPropagationDrivers() {
return estimatedPropagationParameters.getNbParams();
}
/** Get the number of estimated measurement parameters.
* @return the number of estimated measurement parameters
*/
- private int getNumberSelectedMeasurementDrivers() {
+ public int getNumberSelectedMeasurementDrivers() {
return estimatedMeasurementsParameters.getNbParams();
}
- /**
- * Update the nominal Spacecraft state using interpolated state.
- * @param interpolatedState the interpolated state
- * @param propagatorBuilder builder associated to the state
- * @param index index of the sigma point associated to the state
+ /** Update the estimated parameters after the correction phase of the filter.
+ * The min/max allowed values are handled by the parameter themselves.
*/
- public void updateNominalSpacecraftState(final SpacecraftState interpolatedState, final DSSTPropagatorBuilder propagatorBuilder, final int index) {
- previousNominalMeanSpacecraftStates.set(index, nominalMeanSpacecraftStates.get(index));
- nominalMeanSpacecraftStates.set(index, interpolatedState);
- propagatorBuilder.resetOrbit(interpolatedState.getOrbit(), PropagationType.MEAN);
- }
-
- public void finalizeOperationsObservationGrid(final RealVector[] sigmaPoints) {
- for (int i = 0; i < sigmaPoints.length; i++) {
- int j = 0;
- for (final DelegatingDriver driver : propagatorBuilders.get(i).getOrbitalParametersDrivers().getDrivers()) {
- if (driver.isSelected()) {
- driver.setValue(sigmaPoints[i].getEntry(j++));
- }
- }
- for (final DelegatingDriver driver : propagatorBuilders.get(i).getPropagationParametersDrivers().getDrivers()) {
- if (driver.isSelected()) {
- driver.setValue(sigmaPoints[i].getEntry(j++));
- }
- }
+ private void updateParameters() {
+ final RealVector correctedState = correctedEstimate.getState();
+ int i = 0;
+ for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
+ // let the parameter handle min/max clipping
+ driver.setValue(driver.getValue() + correctedState.getEntry(i++));
+ }
+ for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
+ // let the parameter handle min/max clipping
+ driver.setValue(driver.getValue() + correctedState.getEntry(i++));
+ }
+ for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
+ // let the parameter handle min/max clipping
+ driver.setValue(driver.getValue() + correctedState.getEntry(i++));
}
-
}
+
}
diff --git a/src/main/java/org/orekit/estimation/sequential/UnscentedKalmanEstimator.java b/src/main/java/org/orekit/estimation/sequential/UnscentedKalmanEstimator.java
index 99f271facef8cc070c95f1bb44d439c468641dc8..e656dd0f2cf21155f68b2d9ef3884d136eaa5393 100644
--- a/src/main/java/org/orekit/estimation/sequential/UnscentedKalmanEstimator.java
+++ b/src/main/java/org/orekit/estimation/sequential/UnscentedKalmanEstimator.java
@@ -20,14 +20,11 @@ import org.hipparchus.exception.MathRuntimeException;
import org.hipparchus.filtering.kalman.ProcessEstimate;
import org.hipparchus.filtering.kalman.unscented.UnscentedKalmanFilter;
import org.hipparchus.linear.MatrixDecomposer;
-import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.hipparchus.util.UnscentedTransformProvider;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.ObservedMeasurement;
-import org.orekit.estimation.measurements.PV;
-import org.orekit.estimation.measurements.Position;
import org.orekit.propagation.Propagator;
import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
import org.orekit.time.AbsoluteDate;
@@ -191,7 +188,7 @@ public class UnscentedKalmanEstimator {
*/
public Propagator estimationStep(final ObservedMeasurement<?> observedMeasurement) {
try {
- final ProcessEstimate estimate = filter.estimationStep(decorate(observedMeasurement));
+ final ProcessEstimate estimate = filter.estimationStep(KalmanEstimatorUtil.decorate(observedMeasurement, referenceDate));
processModel.finalizeEstimation(observedMeasurement, estimate);
if (observer != null) {
observer.evaluationPerformed(processModel);
@@ -214,39 +211,4 @@ public class UnscentedKalmanEstimator {
return propagator;
}
- /** Decorate an observed measurement.
- * <p>
- * The "physical" measurement noise matrix is the covariance matrix of the measurement.
- * Normalizing it consists in applying the following equation: Rn[i,j] = R[i,j]/σ[i]/σ[j]
- * Thus the normalized measurement noise matrix is the matrix of the correlation coefficients
- * between the different components of the measurement.
- * </p>
- * @param observedMeasurement the measurement
- * @return decorated measurement
- */
- private MeasurementDecorator decorate(final ObservedMeasurement<?> observedMeasurement) {
-
- // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
- // of the measurement on its non-diagonal elements.
- // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
- // Normalizing it leaves us with the matrix of the correlation coefficients
- final RealMatrix covariance;
- if (observedMeasurement instanceof PV) {
- // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
- final PV pv = (PV) observedMeasurement;
- covariance = MatrixUtils.createRealMatrix(pv.getCorrelationCoefficientsMatrix());
- } else if (observedMeasurement instanceof Position) {
- // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
- final Position position = (Position) observedMeasurement;
- covariance = MatrixUtils.createRealMatrix(position.getCorrelationCoefficientsMatrix());
- } else {
- // For other measurements we do not have a covariance matrix.
- // Thus the correlation coefficients matrix is an identity matrix.
- covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
- }
-
- return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);
-
- }
-
}
diff --git a/src/main/java/org/orekit/estimation/sequential/UskfMeasurementHandler.java b/src/main/java/org/orekit/estimation/sequential/UskfMeasurementHandler.java
index ea55cbb8dee25139b53a28ec0957267273458a2c..94d6baa5d09081f93454fb6e8e14a64d7b9979b2 100644
--- a/src/main/java/org/orekit/estimation/sequential/UskfMeasurementHandler.java
+++ b/src/main/java/org/orekit/estimation/sequential/UskfMeasurementHandler.java
@@ -21,17 +21,10 @@ import java.util.List;
import org.hipparchus.exception.MathRuntimeException;
import org.hipparchus.filtering.kalman.ProcessEstimate;
import org.hipparchus.filtering.kalman.unscented.UnscentedKalmanFilter;
-import org.hipparchus.linear.MatrixUtils;
-import org.hipparchus.linear.RealMatrix;
-import org.hipparchus.linear.RealVector;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.ObservedMeasurement;
-import org.orekit.estimation.measurements.PV;
-import org.orekit.estimation.measurements.Position;
-import org.orekit.propagation.Propagator;
import org.orekit.propagation.SpacecraftState;
-import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
-import org.orekit.propagation.sampling.MultiSatStepHandler;
+import org.orekit.propagation.sampling.OrekitStepHandler;
import org.orekit.propagation.sampling.OrekitStepInterpolator;
import org.orekit.time.AbsoluteDate;
@@ -40,22 +33,7 @@ import org.orekit.time.AbsoluteDate;
* @author Gaëtan Pierre
* @author Bryan Cazabonne
*/
-public class UskfMeasurementHandler implements MultiSatStepHandler {
-
- /** Least squares model. */
- private final SemiAnalyticalUnscentedKalmanModel model;
-
- /** DSST propagator builder. */
- private final List<DSSTPropagatorBuilder> builders;
-
- /** DSST propagator. */
- private final List<Propagator> propagators;
-
- /** Extended Kalman Filter. */
- private final UnscentedKalmanFilter<MeasurementDecorator> filter;
-
- /** Underlying measurements. */
- private final List<ObservedMeasurement<?>> observedMeasurements;
+public class UskfMeasurementHandler implements OrekitStepHandler {
/** Index of the next measurement component in the model. */
private int index;
@@ -66,127 +44,82 @@ public class UskfMeasurementHandler implements MultiSatStepHandler {
/** Observer to retrieve current estimation info. */
private KalmanObserver observer;
- /** Sigma points of the current step. */
- private RealVector[] sigmaPoints;
+ /** Kalman model. */
+ private final SemiAnalyticalUnscentedKalmanModel model;
+
+ /** Unscented Kalman Filter. */
+ private final UnscentedKalmanFilter<MeasurementDecorator> filter;
+
+ /** Underlying measurements. */
+ private final List<ObservedMeasurement<?>> observedMeasurements;
/** Simple constructor.
* @param model semi-analytical kalman model
* @param filter kalman filter instance
* @param observedMeasurements list of observed measurements
* @param referenceDate reference date
- * @param propagators propagators
- * @param builders builders
- * @param sigmaPoints sigma points
*/
public UskfMeasurementHandler(final SemiAnalyticalUnscentedKalmanModel model,
final UnscentedKalmanFilter<MeasurementDecorator> filter,
final List<ObservedMeasurement<?>> observedMeasurements,
- final AbsoluteDate referenceDate,
- final List<Propagator> propagators,
- final List<DSSTPropagatorBuilder> builders,
- final RealVector[] sigmaPoints) {
+ final AbsoluteDate referenceDate) {
this.model = model;
this.filter = filter;
this.observer = model.getObserver();
this.observedMeasurements = observedMeasurements;
this.referenceDate = referenceDate;
- this.propagators = propagators;
- this.builders = builders;
- this.sigmaPoints = sigmaPoints;
}
-
- public void init(final List<SpacecraftState> initialStates, final AbsoluteDate t) {
+ /** {@inheritDoc} */
+ @Override
+ public void init(final SpacecraftState s0, final AbsoluteDate t) {
this.index = 0;
// Initialize short periodic terms.
- for (int i = 0; i < initialStates.size(); i++) {
- model.initializeShortPeriodicTerms(initialStates.get(i), propagators.get(i));
- model.updateShortPeriods(initialStates.get(i), builders.get(i));
- }
-
+ model.initializeShortPeriodicTerms(s0);
+ model.updateShortPeriods(s0);
}
+ /** {@inheritDoc} */
@Override
- public void handleStep(final List<OrekitStepInterpolator> interpolators) {
- // TODO Auto-generated method stub
- final AbsoluteDate currentDate = interpolators.get(0).getCurrentState().getDate();
-
- for (int i = 0; i < interpolators.size(); i++) {
- // Update the short period terms with the current MEAN state
- model.updateShortPeriods(interpolators.get(i).getCurrentState(), builders.get(i));
- // Process the measurements between previous step and current step
- }
- while (index < observedMeasurements.size() && (observedMeasurements.get(index).getDate().compareTo(currentDate) < 0 || index == observedMeasurements.size()-1) ) {
-
- try {
-
- for (int i = 0; i < interpolators.size(); i++) {
- // Update the nominal state with the interpolated parameters
- SpacecraftState s = interpolators.get(i).getInterpolatedState(observedMeasurements.get(index).getDate());
- model.updateNominalSpacecraftState(s, builders.get(i), i);
- }
- // Process the current observation
- ProcessEstimate estimate = filter.predictionAndCorrectionSteps(decorate(observedMeasurements.get(index)), sigmaPoints);
- // Finalize the estimation
- model.finalizeEstimation(observedMeasurements.get(index), estimate);
- // Call the observer if the user add one
- if (observer != null) {
- observer.evaluationPerformed(model);
- }
-
- } catch (MathRuntimeException mrte) {
- throw new OrekitException(mrte);
- }
- // Increment the measurement index
- index += 1;
-
- }
+ public void handleStep(final OrekitStepInterpolator interpolator) {
+ // Current date
+ final AbsoluteDate currentDate = interpolator.getCurrentState().getDate();
- // Update the sigmaPoints
- final ProcessEstimate estimate = filter.getCorrected();
- sigmaPoints = filter.getUnscentedTransformProvider().unscentedTransform(estimate.getState(), estimate.getCovariance());
+ // Update the short period terms with the current MEAN state
+ model.updateShortPeriods(interpolator.getCurrentState());
+ // Process the measurements between previous step and current step
+ while (index < observedMeasurements.size() && observedMeasurements.get(index).getDate().compareTo(currentDate) < 0) {
- // Reset the initial state of the propagators with sigma points
- model.finalizeOperationsObservationGrid(sigmaPoints);
+ try {
- }
+ // Update predicted spacecraft state
+ model.updateNominalSpacecraftState(interpolator.getInterpolatedState(observedMeasurements.get(index).getDate()));
+
+ // Process the current observation
+ final ProcessEstimate estimate = filter.estimationStep(KalmanEstimatorUtil.decorate(observedMeasurements.get(index), referenceDate));
+
+ // Finalize the estimation
+ model.finalizeEstimation(observedMeasurements.get(index), estimate);
+
+ // Call the observer if the user add one
+ if (observer != null) {
+ observer.evaluationPerformed(model);
+ }
+
+ } catch (MathRuntimeException mrte) {
+ throw new OrekitException(mrte);
+ }
+
+ // Increment the measurement index
+ index += 1;
- /** Decorate an observed measurement.
- * <p>
- * The "physical" measurement noise matrix is the covariance matrix of the measurement.
- * Normalizing it consists in applying the following equation: Rn[i,j] = R[i,j]/σ[i]/σ[j]
- * Thus the normalized measurement noise matrix is the matrix of the correlation coefficients
- * between the different components of the measurement.
- * </p>
- * @param observedMeasurement the measurement
- * @return decorated measurement
- */
- private MeasurementDecorator decorate(final ObservedMeasurement<?> observedMeasurement) {
-
- // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
- // of the measurement on its non-diagonal elements.
- // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
- // Normalizing it leaves us with the matrix of the correlation coefficients
- final RealMatrix covariance;
- if (observedMeasurement instanceof PV) {
- // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
- final PV pv = (PV) observedMeasurement;
- covariance = MatrixUtils.createRealMatrix(pv.getCorrelationCoefficientsMatrix());
- } else if (observedMeasurement instanceof Position) {
- // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
- final Position position = (Position) observedMeasurement;
- covariance = MatrixUtils.createRealMatrix(position.getCorrelationCoefficientsMatrix());
- } else {
- // For other measurements we do not have a covariance matrix.
- // Thus the correlation coefficients matrix is an identity matrix.
- covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
}
- return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);
+ // Reset the initial state of the propagator
+ model.finalizeOperationsObservationGrid();
}
}
-
diff --git a/src/test/java/org/orekit/estimation/sequential/KalmanEstimatorTest.java b/src/test/java/org/orekit/estimation/sequential/KalmanEstimatorTest.java
index 83fc140f8aaee99948381c02c0e1d90aede92aee..2aabb442c92b2f1c9d9f8fa176a6aa2294e7f91c 100644
--- a/src/test/java/org/orekit/estimation/sequential/KalmanEstimatorTest.java
+++ b/src/test/java/org/orekit/estimation/sequential/KalmanEstimatorTest.java
@@ -16,8 +16,6 @@
*/
package org.orekit.estimation.sequential;
-import java.lang.reflect.InvocationTargetException;
-import java.lang.reflect.Method;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.List;
@@ -933,39 +931,8 @@ public class KalmanEstimatorTest {
new Vector3D(1.0, -1.0, 0.0),
0.5, 1.0, new ObservableSatellite(0));
- // Create propagator builder
- final NumericalPropagatorBuilder propagatorBuilder =
- context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.e-6, 60., 1.);
-
- // Covariance matrix initialization
- final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
- 1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5
- });
-
- // Process noise matrix
- RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double [] {
- 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8
- });
-
-
- // Build the Kalman filter
- final KalmanEstimator kalman = new KalmanEstimatorBuilder().
- addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
- build();
-
// Decorated measurement
- MeasurementDecorator decorated = null;
-
- // Call the private method "decorate" in KalmanEstimator class
- try {
- Method decorate = KalmanEstimator.class.getDeclaredMethod("decorate",
- ObservedMeasurement.class);
- decorate.setAccessible(true);
- decorated = (MeasurementDecorator) decorate.invoke(kalman, position);
- } catch (NoSuchMethodException | IllegalAccessException |
- IllegalArgumentException | InvocationTargetException e) {
-
- }
+ MeasurementDecorator decorated = KalmanEstimatorUtil.decorate(position, context.initialOrbit.getDate());
// Verify time
Assert.assertEquals(0.0, decorated.getTime(), 1.0e-15);
diff --git a/src/test/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanEstimatorTest.java b/src/test/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanEstimatorTest.java
index 5e7a8afca54a1186e09c22be48f6d39fc23f69fb..2afc321d3a4bcc3a7717ffecfba0454bdead2514 100644
--- a/src/test/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanEstimatorTest.java
+++ b/src/test/java/org/orekit/estimation/sequential/SemiAnalyticalUnscentedKalmanEstimatorTest.java
@@ -1,8 +1,5 @@
package org.orekit.estimation.sequential;
-import java.io.IOException;
-import java.io.PrintWriter;
-import java.util.ArrayList;
import java.util.List;
import org.hipparchus.linear.MatrixUtils;
@@ -18,10 +15,8 @@ import org.orekit.estimation.DSSTContext;
import org.orekit.estimation.DSSTEstimationTestUtils;
import org.orekit.estimation.measurements.EstimatedMeasurement;
import org.orekit.estimation.measurements.ObservedMeasurement;
-import org.orekit.estimation.measurements.PVMeasurementCreator;
import org.orekit.estimation.measurements.Range;
import org.orekit.estimation.measurements.RangeMeasurementCreator;
-import org.orekit.estimation.measurements.RangeRateMeasurementCreator;
import org.orekit.forces.gravity.potential.GravityFieldFactory;
import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
import org.orekit.orbits.Orbit;
@@ -35,7 +30,6 @@ import org.orekit.propagation.semianalytical.dsst.forces.DSSTTesseral;
import org.orekit.propagation.semianalytical.dsst.forces.DSSTZonal;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.Constants;
-import org.orekit.utils.ParameterDriver;
public class SemiAnalyticalUnscentedKalmanEstimatorTest {
@@ -44,24 +38,10 @@ public class SemiAnalyticalUnscentedKalmanEstimatorTest {
/** Residuals statistics. */
private StreamingStatistics stats;
-
- private List<Double> residualsID;
- private List<Double> residualsS;
-
- private List<Double> timeID;
- private List<Double> timeS;
-
- private Boolean isFirstEvaluation;
- private AbsoluteDate initialDate;
/** Constructor. */
public Observer() {
- this.stats = new StreamingStatistics();
- this.residualsID = new ArrayList<>();
- this.residualsS = new ArrayList<>();
- this.timeID = new ArrayList<>();
- this.timeS = new ArrayList<>();
- this.isFirstEvaluation = true;
+ this.stats = new StreamingStatistics();
}
/** {@inheritDoc} */
@@ -70,10 +50,7 @@ public class SemiAnalyticalUnscentedKalmanEstimatorTest {
// Estimated and observed measurements
final EstimatedMeasurement<?> estimatedMeasurement = estimation.getPredictedMeasurement();
- if (isFirstEvaluation) {
- initialDate = estimatedMeasurement.getDate();
- isFirstEvaluation = false;
- }
+
// Check
if (estimatedMeasurement.getObservedMeasurement() instanceof Range) {
final double[] estimated = estimatedMeasurement.getEstimatedValue();
@@ -81,14 +58,6 @@ public class SemiAnalyticalUnscentedKalmanEstimatorTest {
// Calculate residual
final double res = observed[0] - estimated[0];
stats.addValue(res);
- if (((Range)estimatedMeasurement.getObservedMeasurement()).getStation().getBaseFrame().getName().equals("Isla Desolación")) {
- residualsID.add(res);
- timeID.add(estimatedMeasurement.getDate().durationFrom(initialDate)/86400);
- }
- else {
- residualsS.add(res);
- timeS.add(estimatedMeasurement.getDate().durationFrom(initialDate)/86400);
- }
}
}
@@ -99,33 +68,9 @@ public class SemiAnalyticalUnscentedKalmanEstimatorTest {
public double getMeanResidual() {
return stats.getMean();
}
-
- public void printResults(String path, String path2) {
- PrintWriter writerID = null;
- PrintWriter writerS = null;
- try {
- String encoding = "UTF-8";
- writerID = new PrintWriter(path, encoding);
- writerS = new PrintWriter(path2, encoding);
- }
- catch (IOException e){
- System.out.println("An error occurred.");
- e.printStackTrace();
- }
- for (int i = 0; i < residualsID.size(); i++) {
- writerID.print(timeID.get(i));
- writerID.print(" ");
- writerID.println(residualsID.get(i));
- }
- for (int i = 0; i < residualsS.size(); i++) {
- writerS.print(timeS.get(i));
- writerS.print(" ");
- writerS.println(residualsS.get(i));
- }
- writerS.close();
- writerID.close();
- }
+
}
+
@Test
public void testMissingPropagatorBuilder() {
try {
@@ -137,209 +82,26 @@ public class SemiAnalyticalUnscentedKalmanEstimatorTest {
}
}
- /**
- * Perfect PV measurements with a perfect start.
- */
- @Test
- public void testPV() {
-
- // Create context
- DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
-
- // Create initial orbit and propagator builder
- final PositionAngle positionAngle = PositionAngle.MEAN;
- final boolean perfectStart = true;
- final double minStep = 120.0;
- final double maxStep = 1200.0;
- final double dP = 1.;
- final DSSTPropagatorBuilder propagatorBuilder =
- context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart,
- minStep, maxStep, dP);
- final DSSTPropagatorBuilder propagatorBuilderRef =
- context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart,
- minStep, maxStep, dP);
- // Create perfect PV measurements
- final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit,
- propagatorBuilder);
- final List<ObservedMeasurement<?>> measurements =
- DSSTEstimationTestUtils.createMeasurements(propagator,
- new PVMeasurementCreator(),
- 0.0, 6.0, 60.0);
- // Reference propagator for estimation performances
- final DSSTPropagator referencePropagator = propagatorBuilderRef.
- buildPropagator(propagatorBuilderRef.getSelectedNormalizedParameters());
-
- // Reference position/velocity at last measurement date
- final Orbit refOrbit = referencePropagator.
- propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
-
- // Covariance matrix initialization
- // final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double[] {0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001});
- final RealMatrix initialP = MatrixUtils.createRealMatrix(6, 6);
- // Process noise matrix
- RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
-
-
- // Build the Kalman filter
- final SemiAnalyticalUnscentedKalmanEstimator kalman = new SemiAnalyticalUnscentedKalmanEstimatorBuilder().
- addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
- build();
-
- // Filter the measurements and check the results
- final double expectedDeltaPos = 0.;
- final double posEps = 4e-9;
- final double expectedDeltaVel = 0.;
- final double velEps = 2.5e-12;
-
- DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
- refOrbit, positionAngle,
- expectedDeltaPos, posEps,
- expectedDeltaVel, velEps);
- }
-
-
- /**
- * Perfect Range measurements with a perfect start.
- */
@Test
- public void testCartesianRange() {
-
- // Create context
- DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
-
- // Create initial orbit and propagator builder
- final PositionAngle positionAngle = PositionAngle.MEAN;
- final boolean perfectStart = true;
- final double minStep = 1.e-6;
- final double maxStep = 60.;
- final double dP = 1.;
- final DSSTPropagatorBuilder propagatorBuilder =
- context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart,
- minStep, maxStep, dP);
-
- // Create perfect PV measurements
- final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit,
- propagatorBuilder);
- final List<ObservedMeasurement<?>> measurements =
- DSSTEstimationTestUtils.createMeasurements(propagator,
- new RangeMeasurementCreator(context),
- 0.0, 1.0, 300.0);
- // Reference propagator for estimation performances
- final DSSTPropagator referencePropagator = propagatorBuilder.
- buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
-
- // Reference position/velocity at last measurement date
- final Orbit refOrbit = referencePropagator.
- propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
-
- // Covariance matrix initialization
- final RealMatrix initialP = MatrixUtils.createRealMatrix(6, 6);
-
- // Process noise matrix
- RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
-
-
- // Build the Kalman filter
- final SemiAnalyticalUnscentedKalmanEstimator kalman = new SemiAnalyticalUnscentedKalmanEstimatorBuilder().
- addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
- build();
-
- // Filter the measurements and check the results
- final double expectedDeltaPos = 0.;
- final double posEps = 4e-3;
- final double expectedDeltaVel = 0.;
- final double velEps = 2.28e-5;
-
- DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
- refOrbit, positionAngle,
- expectedDeltaPos, posEps,
- expectedDeltaVel, velEps);
+ public void testMissingUnscentedTransform() {
+ try {
+ DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
+ final boolean perfectStart = true;
+ final double minStep = 1.e-6;
+ final double maxStep = 60.;
+ final double dP = 1.;
+ final DSSTPropagatorBuilder propagatorBuilder =
+ context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart,
+ minStep, maxStep, dP);
+ new SemiAnalyticalUnscentedKalmanEstimatorBuilder().
+ addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(MatrixUtils.createRealMatrix(6, 6))).
+ build();
+ Assert.fail("an exception should have been thrown");
+ } catch (OrekitException oe) {
+ Assert.assertEquals(OrekitMessages.NO_UNSCENTED_TRANSFORM_CONFIGURED, oe.getSpecifier());
+ }
}
-
- /**
- * Perfect range rate measurements with a perfect start
- * Cartesian formalism
- */
- @Test
- public void testCartesianRangeRate() {
-
- // Create context
- DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
-
- // Create initial orbit and propagator builder
- final OrbitType orbitType = OrbitType.EQUINOCTIAL;
- final PositionAngle positionAngle = PositionAngle.MEAN;
- final boolean perfectStart = true;
- final double minStep = 1.e-6;
- final double maxStep = 60.;
- final double dP = 1.;
- final DSSTPropagatorBuilder propagatorBuilder =
- context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart,
- minStep, maxStep, dP);
-
- // Create perfect range measurements
- final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit,
- propagatorBuilder);
- final double satClkDrift = 3.2e-10;
- final RangeRateMeasurementCreator creator = new RangeRateMeasurementCreator(context, false, satClkDrift);
- final List<ObservedMeasurement<?>> measurements =
- DSSTEstimationTestUtils.createMeasurements(propagator,
- creator,
- 1.0, 3.0, 300.0);
-
- // Reference propagator for estimation performances
- final DSSTPropagator referencePropagator = propagatorBuilder.
- buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
-
- // Reference position/velocity at last measurement date
- final Orbit refOrbit = referencePropagator.
- propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
-
- // Cartesian covariance matrix initialization
- // 100m on position / 1e-2m/s on velocity
- final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
- 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10
- });
-
- // Jacobian of the orbital parameters w/r to Cartesian
- final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
- final double[][] dYdC = new double[6][6];
- initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
- final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
-
- // Initial covariance matrix
- final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
-
- // Process noise matrix
- final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
- 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12
- });
- final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
-
- // Build the Kalman filter
- final SemiAnalyticalUnscentedKalmanEstimator kalman = new SemiAnalyticalUnscentedKalmanEstimatorBuilder().
- addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
- build();
-
- // Filter the measurements and check the results
- final double expectedDeltaPos = 0.;
- final double posEps = 4e-4;
- final double expectedDeltaVel = 0.;
- final double velEps = 2e-7;
- DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
- refOrbit, positionAngle,
- expectedDeltaPos, posEps,
- expectedDeltaVel, velEps);
- }
-
- /**
- * Perfect range measurements.
- * Only the Newtonian Attraction is used.
- * Case 1 of : "Cazabonne B., Bayard J., Journot M., and Cefola P. J., A Semi-analytical Approach for Orbit
- * Determination based on Extended Kalman Filter, AAS Paper 21-614, AAS/AIAA Astrodynamics
- * Specialist Conference, Big Sky, August 2021."
- */
@Test
public void testKeplerianRange() {
@@ -396,21 +158,22 @@ public class SemiAnalyticalUnscentedKalmanEstimatorTest {
// Build the Kalman filter
final SemiAnalyticalUnscentedKalmanEstimator kalman = new SemiAnalyticalUnscentedKalmanEstimatorBuilder().
addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
+ unscentedTransformProvider(new MerweUnscentedTransform(6)).
build();
final Observer observer = new Observer();
kalman.setObserver(observer);
// Filter the measurements and check the results
final double expectedDeltaPos = 0.;
- final double posEps = 6e-9;
+ final double posEps = 1.0e-15;
final double expectedDeltaVel = 0.;
- final double velEps = 2e-12;
+ final double velEps = 1.0e-15;
DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
refOrbit, positionAngle,
expectedDeltaPos, posEps,
expectedDeltaVel, velEps);
- Assert.assertEquals(0.0, observer.getMeanResidual(), 5.41e-8);
+ Assert.assertEquals(0.0, observer.getMeanResidual(), 4.98e-8);
Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
Assert.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
@@ -422,13 +185,6 @@ public class SemiAnalyticalUnscentedKalmanEstimatorTest {
}
- /**
- * Perfect range measurements.
- * J20 is added to the perturbation model compare to the previous test
- * Case 2 of : "Cazabonne B., Bayard J., Journot M., and Cefola P. J., A Semi-analytical Approach for Orbit
- * Determination based on Extended Kalman Filter, AAS Paper 21-614, AAS/AIAA Astrodynamics
- * Specialist Conference, Big Sky, August 2021."
- */
@Test
public void testRangeWithZonal() {
@@ -467,23 +223,19 @@ public class SemiAnalyticalUnscentedKalmanEstimatorTest {
final Orbit refOrbit = referencePropagator.
propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
- ParameterDriver aDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
- aDriver.setValue(aDriver.getValue() + 1.2);
-
- // Cartesian covariance matrix initialization
- // 100m on position / 1e-2m/s on velocity
- final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
- 100., 100., 100., 1e-2, 1e-2, 1e-2
+ // Equinictial covariance matrix initialization
+ final RealMatrix equinoctialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
+ 0., 0., 0., 0., 0., 0.
});
-
+
// Jacobian of the orbital parameters w/r to Cartesian
final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
final double[][] dYdC = new double[6][6];
- initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
+ initialOrbit.getJacobianWrtCartesian(PositionAngle.MEAN, dYdC);
final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
-
- // Keplerian initial covariance matrix
- final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
+
+ // Equinoctial initial covariance matrix
+ final RealMatrix initialP = Jac.multiply(equinoctialP.multiply(Jac.transpose()));
// Process noise matrix is set to 0 here
RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
@@ -491,21 +243,22 @@ public class SemiAnalyticalUnscentedKalmanEstimatorTest {
// Build the Kalman filter
final SemiAnalyticalUnscentedKalmanEstimator kalman = new SemiAnalyticalUnscentedKalmanEstimatorBuilder().
addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
+ unscentedTransformProvider(new MerweUnscentedTransform(6)).
build();
final Observer observer = new Observer();
kalman.setObserver(observer);
// Filter the measurements and check the results
final double expectedDeltaPos = 0.;
- final double posEps = 7.6e-1;
+ final double posEps = 1.1e-7;
final double expectedDeltaVel = 0.;
- final double velEps = 1.5e-4;
+ final double velEps = 3.9e-11;
DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
refOrbit, positionAngle,
expectedDeltaPos, posEps,
expectedDeltaVel, velEps);
- //Assert.assertEquals(0.0, observer.getMeanResidual(), 8.51e-3);
+ Assert.assertEquals(0.0, observer.getMeanResidual(), 2.59e-3);
Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
Assert.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
@@ -518,14 +271,6 @@ public class SemiAnalyticalUnscentedKalmanEstimatorTest {
}
- /**
- * Perfect range measurements.
- * J20 is added to the perturbation model
- * In addition, J21 and J22 are also added
- * Case 3 of : "Cazabonne B., Bayard J., Journot M., and Cefola P. J., A Semi-analytical Approach for Orbit
- * Determination based on Extended Kalman Filter, AAS Paper 21-614, AAS/AIAA Astrodynamics
- * Specialist Conference, Big Sky, August 2021."
- */
@Test
public void testRangeWithTesseral() {
@@ -572,25 +317,19 @@ public class SemiAnalyticalUnscentedKalmanEstimatorTest {
final Orbit refOrbit = referencePropagator.
propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
- ParameterDriver aDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
- aDriver.setValue(aDriver.getValue() + 1.2);
-
- // Cartesian covariance matrix initialization
- // 100m on position / 1e-2m/s on velocity
- // Process noise matrix is set to 0 here
-// final RealMatrix cartesianP = MatrixUtils.createRealMatrix(6, 6);
- final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
- 100., 100., 100., 1e-2, 1e-2, 1e-2
+ // Equinictial covariance matrix initialization
+ final RealMatrix equinoctialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
+ 0., 0., 0., 0., 0., 0.
});
-
+
// Jacobian of the orbital parameters w/r to Cartesian
final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
final double[][] dYdC = new double[6][6];
- initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
+ initialOrbit.getJacobianWrtCartesian(PositionAngle.MEAN, dYdC);
final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
-
- // Keplerian initial covariance matrix
- final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
+
+ // Equinoctial initial covariance matrix
+ final RealMatrix initialP = Jac.multiply(equinoctialP.multiply(Jac.transpose()));
// Process noise matrix is set to 0 here
RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
@@ -598,22 +337,23 @@ public class SemiAnalyticalUnscentedKalmanEstimatorTest {
final MerweUnscentedTransform utProvider = new MerweUnscentedTransform(6, 0.5, 2., 0.);
// Build the Kalman filter
final SemiAnalyticalUnscentedKalmanEstimator kalman = new SemiAnalyticalUnscentedKalmanEstimatorBuilder().
- addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).unscentedTransformProvider(utProvider).
+ addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
+ unscentedTransformProvider(utProvider).
build();
final Observer observer = new Observer();
kalman.setObserver(observer);
// Filter the measurements and check the results
final double expectedDeltaPos = 0.;
- final double posEps = 4.8e-1;
+ final double posEps = 4.2e-9;
final double expectedDeltaVel = 0.;
- final double velEps = 2.75e-4;
+ final double velEps = 1.7e-12;
DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
refOrbit, positionAngle,
expectedDeltaPos, posEps,
expectedDeltaVel, velEps);
- //Assert.assertEquals(0.0, observer.getMeanResidual(), 8.81e-3);
+ Assert.assertEquals(0.0, observer.getMeanResidual(), 2.55e-3);
Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
Assert.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());