SemiAnalyticalMeasurementHandler.java

  1. /* Copyright 2002-2022 CS GROUP
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.estimation.sequential;

  18. import java.util.List;

  19. import org.hipparchus.exception.MathRuntimeException;
  20. import org.hipparchus.filtering.kalman.KalmanFilter;
  21. import org.hipparchus.filtering.kalman.ProcessEstimate;
  22. import org.orekit.errors.OrekitException;
  23. import org.orekit.estimation.measurements.ObservedMeasurement;
  24. import org.orekit.propagation.SpacecraftState;
  25. import org.orekit.propagation.sampling.OrekitStepHandler;
  26. import org.orekit.propagation.sampling.OrekitStepInterpolator;
  27. import org.orekit.time.AbsoluteDate;

  28. /** {@link org.orekit.propagation.sampling.OrekitStepHandler Step handler} picking up
  29.  * {@link ObservedMeasurement measurements} for both {@link SemiAnalyticalUnscentedKalmanEstimator} and {@link SemiAnalyticalKalmanEstimator}.
  30.  * @author GaĆ«tan Pierre
  31.  * @author Bryan Cazabonne
  32.  * @author Julie Bayard
  33.  * @author Maxime Journot
  34.  * @since 11.3
  35.  */
  36. public class SemiAnalyticalMeasurementHandler implements OrekitStepHandler {

  37.     /** Index of the next measurement component in the model. */
  38.     private int index;

  39.     /** Reference date. */
  40.     private AbsoluteDate referenceDate;

  41.     /** Kalman model. */
  42.     private final SemiAnalyticalProcess model;

  43.     /** Kalman Filter. */
  44.     private final KalmanFilter<MeasurementDecorator> filter;

  45.     /** Underlying measurements. */
  46.     private final List<ObservedMeasurement<?>> observedMeasurements;

  47.     /** Simple constructor.
  48.      * @param model semi-analytical kalman model
  49.      * @param filter kalman filter instance
  50.      * @param observedMeasurements list of observed measurements
  51.      * @param referenceDate reference date
  52.      */
  53.     public SemiAnalyticalMeasurementHandler(final SemiAnalyticalProcess model,
  54.                                   final KalmanFilter<MeasurementDecorator> filter,
  55.                                   final List<ObservedMeasurement<?>> observedMeasurements,
  56.                                   final AbsoluteDate referenceDate) {
  57.         this.model                = model;
  58.         this.filter               = filter;
  59.         this.observedMeasurements = observedMeasurements;
  60.         this.referenceDate        = referenceDate;
  61.     }

  62.     /** {@inheritDoc} */
  63.     @Override
  64.     public void init(final SpacecraftState s0, final AbsoluteDate t) {
  65.         this.index = 0;
  66.         // Initialize short periodic terms.
  67.         model.initializeShortPeriodicTerms(s0);
  68.         model.updateShortPeriods(s0);
  69.     }

  70.     /** {@inheritDoc} */
  71.     @Override
  72.     public void handleStep(final OrekitStepInterpolator interpolator) {

  73.         // Current date
  74.         final AbsoluteDate currentDate = interpolator.getCurrentState().getDate();

  75.         // Update the short period terms with the current MEAN state
  76.         model.updateShortPeriods(interpolator.getCurrentState());

  77.         // Process the measurements between previous step and current step
  78.         while (index < observedMeasurements.size() && observedMeasurements.get(index).getDate().compareTo(currentDate) < 0) {

  79.             try {

  80.                 // Update predicted spacecraft state
  81.                 model.updateNominalSpacecraftState(interpolator.getInterpolatedState(observedMeasurements.get(index).getDate()));

  82.                 // Process the current observation
  83.                 final ProcessEstimate estimate = filter.estimationStep(KalmanEstimatorUtil.decorate(observedMeasurements.get(index), referenceDate));

  84.                 // Finalize the estimation
  85.                 model.finalizeEstimation(observedMeasurements.get(index), estimate);

  86.             } catch (MathRuntimeException mrte) {
  87.                 throw new OrekitException(mrte);
  88.             }

  89.             // Increment the measurement index
  90.             index += 1;

  91.         }

  92.         // Reset the initial state of the propagator
  93.         model.finalizeOperationsObservationGrid();

  94.     }

  95. }