Commit aaf2fbef authored by Bryan Cazabonne's avatar Bryan Cazabonne
Browse files

Fixed indexes when build state transition matrix for multi sat Kalman.

Fixes #850
parent 08563720
Pipeline #1493 passed with stages
in 24 minutes and 38 seconds
......@@ -688,6 +688,9 @@ public abstract class AbstractKalmanModel implements KalmanEstimation, NonLinear
// loop over all orbits
for (int k = 0; k < predictedSpacecraftStates.length; ++k) {
// Indexes
final int[] indK = covarianceIndirection[k];
// Short period derivatives
analyticalDerivativeComputations(mappers[k], predictedSpacecraftStates[k]);
......@@ -700,10 +703,9 @@ public abstract class AbstractKalmanModel implements KalmanEstimation, NonLinear
builders.get(k).getOrbitalParametersDrivers().getDrivers();
for (int i = 0; i < dYdY0.length; ++i) {
if (drivers.get(i).isSelected()) {
int jOrb = orbitsStartColumns[k];
for (int j = 0; j < dYdY0[i].length; ++j) {
if (drivers.get(j).isSelected()) {
stm.setEntry(i, jOrb++, dYdY0[i][j]);
stm.setEntry(indK[i], indK[j], dYdY0[i][j]);
}
}
}
......@@ -718,7 +720,7 @@ public abstract class AbstractKalmanModel implements KalmanEstimation, NonLinear
// Fill 1st row, 2nd column (dY/dPp)
for (int i = 0; i < dYdPp.length; ++i) {
for (int j = 0; j < nbParams; ++j) {
stm.setEntry(i, orbitsEndColumns[k] + j, dYdPp[i][j]);
stm.setEntry(indK[i], indK[j + 6], dYdPp[i][j]);
}
}
......
......@@ -35,10 +35,12 @@ import org.orekit.estimation.EstimationTestUtils;
import org.orekit.estimation.measurements.AngularAzElMeasurementCreator;
import org.orekit.estimation.measurements.AngularRaDecMeasurementCreator;
import org.orekit.estimation.measurements.InterSatellitesRangeMeasurementCreator;
import org.orekit.estimation.measurements.MultiplexedMeasurement;
import org.orekit.estimation.measurements.ObservableSatellite;
import org.orekit.estimation.measurements.ObservedMeasurement;
import org.orekit.estimation.measurements.PVMeasurementCreator;
import org.orekit.estimation.measurements.Position;
import org.orekit.estimation.measurements.PositionMeasurementCreator;
import org.orekit.estimation.measurements.Range;
import org.orekit.estimation.measurements.RangeMeasurementCreator;
import org.orekit.estimation.measurements.RangeRateMeasurementCreator;
......@@ -847,12 +849,12 @@ public class KalmanEstimatorTest {
new double[] { 38.3, 172.3 }, new double[] { 0.1, 0.1 },
new double[] { 0.015, 0.068 }, new double[] { 1.0e-3, 1.0e-3 },
new double[][] {
{ 5.6e5, 1.8e5, 1.9e5 },
{ 5.3e5, 1.8e5, 1.8e5 }
{ 6.9e5, 6.0e5, 12.5e5 },
{ 6.8e5, 5.9e5, 12.7e5 }
}, new double[] { 1e4, 1e4 },
new double[][] {
{ 8.8e2, 1.8e2, 3.1e2 },
{ 8.8e2, 1.8e2, 3.1e2 }
{ 5.0e2, 5.3e2, 1.4e2 },
{ 5.0e2, 5.3e2, 1.4e2 }
}, new double[] { 1.0e1, 1.0e1 });
// after the call to estimate, the parameters lacking a user-specified reference date
......@@ -1091,12 +1093,12 @@ public class KalmanEstimatorTest {
new double[] { 38.3, 172.3 }, new double[] { 0.1, 0.1 },
new double[] { 0.015, 0.068 }, new double[] { 1.0e-3, 1.0e-3 },
new double[][] {
{ 5.6e5, 1.8e5, 1.9e5 },
{ 5.3e5, 1.8e5, 1.8e5 }
{ 6.9e5, 6.0e5, 12.5e5 },
{ 6.8e5, 5.9e5, 12.7e5 }
}, new double[] { 1e4, 1e4 },
new double[][] {
{ 8.8e2, 1.8e2, 3.1e2 },
{ 8.8e2, 1.8e2, 3.1e2 }
{ 5.0e2, 5.3e2, 1.4e2 },
{ 5.0e2, 5.3e2, 1.4e2 }
}, new double[] { 1.0e1, 1.0e1 });
// after the call to estimate, the parameters lacking a user-specified reference date
......@@ -1113,6 +1115,74 @@ public class KalmanEstimatorTest {
}
@Test
public void tesIssue850() {
Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
// Create propagator builder
final NumericalPropagatorBuilder propagatorBuilder1 =
context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
1.0e-6, 60.0, 1.0);
propagatorBuilder1.getPropagationParametersDrivers().getDrivers().forEach(driver -> driver.setSelected(true));
final NumericalPropagatorBuilder propagatorBuilder2 =
context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
1.0e-6, 60.0, 1.0);
propagatorBuilder2.getPropagationParametersDrivers().getDrivers().forEach(driver -> driver.setSelected(true));
// Generate measurement for both propagators
final Propagator propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
propagatorBuilder1);
final List<ObservedMeasurement<?>> measurements1 =
EstimationTestUtils.createMeasurements(propagator1,
new PositionMeasurementCreator(),
0.0, 3.0, 300.0);
final Propagator propagator2 = EstimationTestUtils.createPropagator(context.initialOrbit,
propagatorBuilder2);
final List<ObservedMeasurement<?>> measurements2 =
EstimationTestUtils.createMeasurements(propagator2,
new PositionMeasurementCreator(),
0.0, 3.0, 300.0);
// Create a multiplexed measurement
final List<ObservedMeasurement<?>> measurements = new ArrayList<>();
measurements.add(measurements1.get(0));
measurements.add(measurements2.get(0));
final ObservedMeasurement<?> multiplexed = new MultiplexedMeasurement(measurements);
// 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(propagatorBuilder1, new ConstantProcessNoise(initialP, Q)).
addPropagationConfiguration(propagatorBuilder2, new ConstantProcessNoise(initialP, Q)).
build();
// Perform an estimation at the first measurment epoch (estimated states must be identical to initial orbit)
Propagator[] estimated = kalman.estimationStep(multiplexed);
final Vector3D pos1 = estimated[0].getInitialState().getPVCoordinates().getPosition();
final Vector3D pos2 = estimated[1].getInitialState().getPVCoordinates().getPosition();
// Verify
Assert.assertEquals(0.0, pos1.distance(pos2), 1.0e-12);
Assert.assertEquals(0.0, pos1.distance(context.initialOrbit.getPVCoordinates().getPosition()), 1.0e-12);
Assert.assertEquals(0.0, pos2.distance(context.initialOrbit.getPVCoordinates().getPosition()), 1.0e-12);
}
private static class DummyException extends OrekitException {
private static final long serialVersionUID = 1L;
public DummyException() {
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment