SemiAnalyticalMeasurementHandler.java
/* Copyright 2002-2022 CS GROUP
* Licensed to CS GROUP (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 java.util.List;
import org.hipparchus.exception.MathRuntimeException;
import org.hipparchus.filtering.kalman.KalmanFilter;
import org.hipparchus.filtering.kalman.ProcessEstimate;
import org.orekit.errors.OrekitException;
import org.orekit.estimation.measurements.ObservedMeasurement;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.sampling.OrekitStepHandler;
import org.orekit.propagation.sampling.OrekitStepInterpolator;
import org.orekit.time.AbsoluteDate;
/** {@link org.orekit.propagation.sampling.OrekitStepHandler Step handler} picking up
* {@link ObservedMeasurement measurements} for both {@link SemiAnalyticalUnscentedKalmanEstimator} and {@link SemiAnalyticalKalmanEstimator}.
* @author Gaƫtan Pierre
* @author Bryan Cazabonne
* @author Julie Bayard
* @author Maxime Journot
* @since 11.3
*/
public class SemiAnalyticalMeasurementHandler implements OrekitStepHandler {
/** Index of the next measurement component in the model. */
private int index;
/** Reference date. */
private AbsoluteDate referenceDate;
/** Kalman model. */
private final SemiAnalyticalProcess model;
/** Kalman Filter. */
private final KalmanFilter<MeasurementDecorator> filter;
/** Underlying measurements. */
private final List<ObservedMeasurement<?>> observedMeasurements;
/** Flag indicating if the handler is used by a unscented kalman filter. */
private final boolean isUnscented;
/** Simple constructor.
* <p>
* Using this constructor, the Kalman filter is supposed to be extended.
* </p>
* @param model semi-analytical kalman model
* @param filter kalman filter instance
* @param observedMeasurements list of observed measurements
* @param referenceDate reference date
* @see #SemiAnalyticalMeasurementHandler(SemiAnalyticalProcess, KalmanFilter, List, AbsoluteDate, boolean)
*/
public SemiAnalyticalMeasurementHandler(final SemiAnalyticalProcess model,
final KalmanFilter<MeasurementDecorator> filter,
final List<ObservedMeasurement<?>> observedMeasurements,
final AbsoluteDate referenceDate) {
this(model, filter, observedMeasurements, referenceDate, false);
}
/** Simple constructor.
* @param model semi-analytical kalman model
* @param filter kalman filter instance
* @param observedMeasurements list of observed measurements
* @param referenceDate reference date
* @param isUnscented true if the Kalman filter is unscented
* @since 11.3.2
*/
public SemiAnalyticalMeasurementHandler(final SemiAnalyticalProcess model,
final KalmanFilter<MeasurementDecorator> filter,
final List<ObservedMeasurement<?>> observedMeasurements,
final AbsoluteDate referenceDate,
final boolean isUnscented) {
this.model = model;
this.filter = filter;
this.observedMeasurements = observedMeasurements;
this.referenceDate = referenceDate;
this.isUnscented = isUnscented;
}
/** {@inheritDoc} */
@Override
public void init(final SpacecraftState s0, final AbsoluteDate t) {
this.index = 0;
// Initialize short periodic terms.
model.initializeShortPeriodicTerms(s0);
model.updateShortPeriods(s0);
}
/** {@inheritDoc} */
@Override
public void handleStep(final OrekitStepInterpolator interpolator) {
// Current date
final AbsoluteDate currentDate = interpolator.getCurrentState().getDate();
// 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) {
try {
// Update predicted spacecraft state
model.updateNominalSpacecraftState(interpolator.getInterpolatedState(observedMeasurements.get(index).getDate()));
// Process the current observation
final MeasurementDecorator decorated = isUnscented ?
KalmanEstimatorUtil.decorateUnscented(observedMeasurements.get(index), referenceDate) :
KalmanEstimatorUtil.decorate(observedMeasurements.get(index), referenceDate);
final ProcessEstimate estimate = filter.estimationStep(decorated);
// Finalize the estimation
model.finalizeEstimation(observedMeasurements.get(index), estimate);
} catch (MathRuntimeException mrte) {
throw new OrekitException(mrte);
}
// Increment the measurement index
index += 1;
}
// Reset the initial state of the propagator
model.finalizeOperationsObservationGrid();
}
}