Commit c8e24826 authored by Luc Maisonobe's avatar Luc Maisonobe

Delegate to Hipparchus extended Kalman filter.

This is work in progress, it doesn't work yet.
It compiles, but all tests fail.
parent 915511d3
......@@ -44,7 +44,7 @@
<orekit.nexus-staging-maven-plugin.version>1.6.8</orekit.nexus-staging-maven-plugin.version>
<orekit.maven-gpg-plugin.version>1.6</orekit.maven-gpg-plugin.version>
<orekit.maven-install-plugin.version>2.5.2</orekit.maven-install-plugin.version>
<orekit.hipparchus.version>1.2</orekit.hipparchus.version>
<orekit.hipparchus.version>1.3-SNAPSHOT</orekit.hipparchus.version>
<orekit.junit.version>4.12</orekit.junit.version>
<orekit.compiler.source>1.8</orekit.compiler.source>
<orekit.compiler.target>1.8</orekit.compiler.target>
......@@ -296,6 +296,13 @@
<type>jar</type>
<optional>false</optional>
</dependency>
<dependency>
<groupId>org.hipparchus</groupId>
<artifactId>hipparchus-filtering</artifactId>
<version>${orekit.hipparchus.version}</version>
<type>jar</type>
<optional>false</optional>
</dependency>
<dependency>
<groupId>org.hipparchus</groupId>
<artifactId>hipparchus-stat</artifactId>
......
/* Copyright 2002-2018 CS Systèmes d'Information
* Licensed to CS Systèmes d'Information (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.estimation.sequential;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.orekit.estimation.measurements.EstimatedMeasurement;
import org.orekit.propagation.SpacecraftState;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.ParameterDriversList;
/** Interface for accessing {@link KalmanEstimator Kalman filter} estimations.
* @author Luc Maisonobe
* @since 9.2
*/
public interface KalmanEstimation {
/** Get the list of estimated orbital parameters.
* @return the list of estimated orbital parameters
*/
ParameterDriversList getEstimatedOrbitalParameters();
/** Get the list of estimated propagation parameters.
* @return the list of estimated propagation parameters
*/
ParameterDriversList getEstimatedPropagationParameters();
/** Get the list of estimated measurements parameters.
* @return the list of estimated measurements parameters
*/
ParameterDriversList getEstimatedMeasurementsParameters();
/** Get the predicted spacecraft states.
* @return predicted spacecraft states
*/
SpacecraftState[] getPredictedSpacecraftStates();
/** Get the corrected spacecraft states.
* @return corrected spacecraft states
*/
SpacecraftState[] getCorrectedSpacecraftStates();
/** Get the "physical" estimated state (i.e. not normalized)
* @return the "physical" estimated state
*/
RealVector getPhysicalEstimatedState();
/** Get the "physical" estimated covariance matrix (i.e. not normalized)
* @return the "physical" estimated covariance matrix
*/
RealMatrix getPhysicalEstimatedCovarianceMatrix();
/** Get the current measurement number.
* @return current measurement number
*/
int getCurrentMeasurementNumber();
/** Get the current date.
* @return current date
*/
AbsoluteDate getCurrentDate();
/** Get the predicted measurement.
* <p>
* This estimation has been evaluated on the last predicted orbits
* </p>
* @return predicted measurement
*/
EstimatedMeasurement<?> getPredictedMeasurement();
/** Get the estimated measurement.
* <p>
* This estimation has been evaluated on the last corrected orbits
* </p>
* @return corrected measurement
*/
EstimatedMeasurement<?> getCorrectedMeasurement();
}
......@@ -16,6 +16,8 @@
*/
package org.orekit.estimation.sequential;
import org.hipparchus.linear.MatrixDecomposer;
import org.hipparchus.linear.QRDecomposer;
import org.hipparchus.linear.RealMatrix;
import org.orekit.errors.OrekitException;
import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
......@@ -28,6 +30,9 @@ import org.orekit.utils.ParameterDriversList;
*/
public class KalmanEstimatorBuilder {
/** Decomposer to use for the correction phase. */
private MatrixDecomposer decomposer;
/** Builder for propagator. */
private NumericalPropagatorBuilder propagatorBuilder;
......@@ -46,6 +51,7 @@ public class KalmanEstimatorBuilder {
* Set an extended Kalman filter, with linearized covariance prediction.
*/
public KalmanEstimatorBuilder() {
this.decomposer = new QRDecomposer(1.0e-15);
this.propagatorBuilder = null;
this.estimatedMeasurementsParameters = null;
this.initialCovarianceMatrix = null;
......@@ -59,12 +65,22 @@ public class KalmanEstimatorBuilder {
public KalmanEstimator build()
throws OrekitException {
// FIXME: Add checks on the existence of the different arguments
return new KalmanEstimator(propagatorBuilder,
return new KalmanEstimator(decomposer,
propagatorBuilder,
estimatedMeasurementsParameters,
initialCovarianceMatrix,
processNoiseMatrix);
}
/** Configure the matrix decomposer.
* @param matrixDecomposer decomposer to use for the correction phase
* @return this object.
*/
public KalmanEstimatorBuilder decomposer(final MatrixDecomposer matrixDecomposer) {
decomposer = matrixDecomposer;
return this;
}
/** Configure the propagator builder.
* @param propBuilder The propagator builder to use in the Kalman filter.
* @return this object.
......
......@@ -17,12 +17,8 @@
package org.orekit.estimation.sequential;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.EstimatedMeasurement;
import org.orekit.estimation.sequential.KalmanEstimator.KalmanEvaluation;
import org.orekit.orbits.Orbit;
import org.orekit.utils.ParameterDriversList;
/** Observer for {@link KalmanEstimatorReal Kalma filter} estimations.
/** Observer for {@link KalmanEstimator Kalman filter} estimations.
* <p>
* This interface is intended to be implemented by users to monitor
* the progress of the Kalman filter estimator during estimation.
......@@ -34,25 +30,10 @@ import org.orekit.utils.ParameterDriversList;
public interface KalmanObserver {
/** Notification callback after each one of a Kalman filter estimation.
* @param predictedOrbits current orbits predicted by the filter
* @param estimatedOrbits current orbits estimated by the filter
* @param estimatedOrbitalParameters estimated orbital parameters
* @param estimatedPropagationParameters estimated propagation parameters
* @param estimatedMeasurementsParameters estimated measurements parameters
* @param predictedMeasurement current predicted measurement in the filter
* @param estimatedMeasurement current estimated measurement in the filter
* @param kalmanEvaluation current evaluation of the filter
* @exception OrekitException if some problem occurs (for example evaluationProviders not
* being able to provide an evaluation)
* @param estimation estimation performed by Kalman estimator
* @exception OrekitException if some problem occurs
*/
void evaluationPerformed(Orbit[] predictedOrbits,
Orbit[] estimatedOrbits,
ParameterDriversList estimatedOrbitalParameters,
ParameterDriversList estimatedPropagationParameters,
ParameterDriversList estimatedMeasurementsParameters,
EstimatedMeasurement<?> predictedMeasurement,
EstimatedMeasurement<?> estimatedMeasurement,
KalmanEvaluation kalmanEvaluation)
void evaluationPerformed(KalmanEstimation estimation)
throws OrekitException;
}
/* Copyright 2002-2018 CS Systèmes d'Information
* Licensed to CS Systèmes d'Information (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.estimation.sequential;
import org.hipparchus.filtering.kalman.Measurement;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.orekit.estimation.measurements.ObservedMeasurement;
import org.orekit.time.AbsoluteDate;
/** Decorator adding {@link Measurement} API to an {@link ObservedMeasurement}.
* @author Luc Maisonobe
* @since 9.2
*/
class MeasurementDecorator implements Measurement {
/** Wrapped observed measurement. */
private final ObservedMeasurement<?> observedMeasurement;
/** Covariance. */
private final RealMatrix covariance;
/** Reference date. */
private final AbsoluteDate reference;
/** Simple constructor.
* @param observedMeasurement observed measurement
* @param covariance measurement covariance
* @param reference reference date
*/
MeasurementDecorator(final ObservedMeasurement<?> observedMeasurement,
final RealMatrix covariance, final AbsoluteDate reference) {
this.observedMeasurement = observedMeasurement;
this.covariance = covariance;
this.reference = reference;
}
/** Get the observed measurement.
* @return observed measurement
*/
public ObservedMeasurement<?> getObservedMeasurement() {
return observedMeasurement;
}
/** {@inheritDoc} */
@Override
public double getTime() {
return observedMeasurement.getDate().durationFrom(reference);
}
/** {@inheritDoc} */
@Override
public RealVector getValue() {
return MatrixUtils.createRealVector(observedMeasurement.getObservedValue());
}
/** {@inheritDoc} */
@Override
public RealMatrix getCovariance() {
return covariance;
}
}
......@@ -276,17 +276,10 @@ public abstract class AbstractPropagatorBuilder implements PropagatorBuilder {
/** Reset the orbit in the propagator builder.
* Note: The orbit has to be the same type as the original orbit
* @param newOrbit New orbit to set in the propagator builder
* @throws OrekitException If the new orbit does not have the same type as the former
* @exception OrekitException if a parameter observer throws an exception during reset
*/
public void resetOrbit(final Orbit newOrbit)
throws OrekitException {
// Ensure the orbit type is the same
if (!newOrbit.getType().equals(getOrbitType())) {
// FIXME: Throw a proper Orekit Exception
throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
"Orbit do not have the same type" );
}
throws OrekitException {
// Map the new orbit in an array of double
final double[] orbitArray = new double[6];
......
......@@ -63,6 +63,7 @@ import org.orekit.orbits.Orbit;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.Propagator;
import org.orekit.propagation.conversion.PropagatorBuilder;
import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.time.TimeScalesFactory;
......@@ -367,15 +368,15 @@ public class EstimationTestUtils {
final double[] expectedSigmasPos,final double sigmaPosEps,
final double[] expectedSigmasVel,final double sigmaVelEps)
throws OrekitException {
// Add the measurements to the Kalman filter
kalman.processMeasurements(measurements);
NumericalPropagator estimated = kalman.processMeasurements(measurements);
// Check the number of measurements processed by the filter
Assert.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
// Get the last estimation
final Orbit estimatedOrbit = kalman.getEstimatedPropagator().getInitialState().getOrbit();
final Orbit estimatedOrbit = estimated.getInitialState().getOrbit();
final Vector3D estimatedPosition = estimatedOrbit.getPVCoordinates().getPosition();
final Vector3D estimatedVelocity = estimatedOrbit.getPVCoordinates().getVelocity();
......
......@@ -30,17 +30,12 @@ import org.orekit.estimation.Context;
import org.orekit.estimation.EstimationTestUtils;
import org.orekit.estimation.measurements.AngularAzElMeasurementCreator;
import org.orekit.estimation.measurements.AngularRaDecMeasurementCreator;
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.estimation.measurements.modifiers.OnBoardAntennaRangeModifier;
import org.orekit.estimation.sequential.KalmanEstimator;
import org.orekit.estimation.sequential.KalmanEstimatorBuilder;
import org.orekit.estimation.sequential.KalmanObserver;
import org.orekit.estimation.sequential.KalmanEstimator.KalmanEvaluation;
import org.orekit.frames.LOFType;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
......@@ -797,20 +792,9 @@ public class KalmanEstimatorTest {
kalmanBuilder.initialCovarianceMatrix(MatrixUtils.createRealMatrix(6, 6));
kalmanBuilder.processNoiseMatrix(MatrixUtils.createRealMatrix(6, 6));
final KalmanEstimator kalman = kalmanBuilder.build();
kalman.setObserver(new KalmanObserver() {
/** {@inheritDoc} */
@Override
public void evaluationPerformed(Orbit[] predictedOrbits,
Orbit[] estimatedOrbits,
ParameterDriversList estimatedOrbitalParameters,
ParameterDriversList estimatedPropagationParameters,
ParameterDriversList estimatedMeasurementsParameters,
EstimatedMeasurement<?> predictedMeasurement,
EstimatedMeasurement<?> estimatedMeasurement,
KalmanEvaluation kalmanEvaluation) throws DummyException {
kalman.setObserver(estimation -> {
throw new DummyException();
}
});
});
try {
......
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