1 /* Copyright 2002-2025 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 19 import java.util.List; 20 21 import org.hipparchus.exception.MathRuntimeException; 22 import org.hipparchus.filtering.kalman.KalmanFilter; 23 import org.hipparchus.filtering.kalman.ProcessEstimate; 24 import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter; 25 import org.hipparchus.linear.MatrixDecomposer; 26 import org.orekit.errors.OrekitException; 27 import org.orekit.estimation.measurements.ObservedMeasurement; 28 import org.orekit.propagation.Propagator; 29 import org.orekit.propagation.analytical.BrouwerLyddanePropagator; 30 import org.orekit.propagation.analytical.EcksteinHechlerPropagator; 31 import org.orekit.propagation.analytical.Ephemeris; 32 import org.orekit.propagation.analytical.KeplerianPropagator; 33 import org.orekit.propagation.analytical.tle.TLEPropagator; 34 import org.orekit.propagation.conversion.PropagatorBuilder; 35 import org.orekit.propagation.numerical.NumericalPropagator; 36 import org.orekit.propagation.semianalytical.dsst.DSSTPropagator; 37 import org.orekit.utils.ParameterDriver; 38 import org.orekit.utils.ParameterDriversList; 39 40 41 /** 42 * Implementation of a Kalman filter to perform orbit determination. 43 * <p> 44 * The filter uses a {@link PropagatorBuilder} to initialize its reference trajectory. 45 * The Kalman estimator can be used with a {@link NumericalPropagator}, {@link TLEPropagator}, 46 * {@link BrouwerLyddanePropagator}, {@link EcksteinHechlerPropagator}, {@link KeplerianPropagator}, 47 * or {@link Ephemeris}. 48 * </p> 49 * <p> 50 * Kalman estimation using a {@link DSSTPropagator semi-analytical orbit propagator} must be done using 51 * the {@link SemiAnalyticalKalmanEstimator}. 52 * </p> 53 * <p> 54 * The estimated parameters are driven by {@link ParameterDriver} objects. They are of 3 different types:<ol> 55 * <li><b>Orbital parameters</b>:The position and velocity of the spacecraft, or, more generally, its orbit.<br> 56 * These parameters are retrieved from the reference trajectory propagator builder when the filter is initialized.</li> 57 * <li><b>Propagation parameters</b>: Some parameters modelling physical processes (SRP or drag coefficients etc...).<br> 58 * They are also retrieved from the propagator builder during the initialization phase.</li> 59 * <li><b>Measurements parameters</b>: Parameters related to measurements (station biases, positions etc...).<br> 60 * They are passed down to the filter in its constructor.</li> 61 * </ol> 62 * <p> 63 * The total number of estimated parameters is m, the size of the state vector. 64 * </p> 65 * <p> 66 * The Kalman filter implementation used is provided by the underlying mathematical library Hipparchus. 67 * All the variables seen by Hipparchus (states, covariances, measurement matrices...) are normalized 68 * using a specific scale for each estimated parameters or standard deviation noise for each measurement components. 69 * </p> 70 * 71 * <p>A {@link KalmanEstimator} object is built using the {@link KalmanEstimatorBuilder#build() build} 72 * method of a {@link KalmanEstimatorBuilder}.</p> 73 * 74 * @author Romain Gerbaud 75 * @author Maxime Journot 76 * @author Luc Maisonobe 77 * @since 9.2 78 */ 79 public class KalmanEstimator extends AbstractKalmanEstimator { 80 81 /** Kalman filter process model. */ 82 private final KalmanModel processModel; 83 84 /** Filter. */ 85 private final KalmanFilter<MeasurementDecorator> filter; 86 87 /** Kalman filter estimator constructor (package private). 88 * @param decomposer decomposer to use for the correction phase 89 * @param propagatorBuilders propagators builders used to evaluate the orbit. 90 * @param processNoiseMatricesProviders providers for process noise matrices 91 * @param estimatedMeasurementParameters measurement parameters to estimate 92 * @param measurementProcessNoiseMatrix provider for measurement process noise matrix 93 * @since 10.3 94 */ 95 KalmanEstimator(final MatrixDecomposer decomposer, 96 final List<PropagatorBuilder> propagatorBuilders, 97 final List<CovarianceMatrixProvider> processNoiseMatricesProviders, 98 final ParameterDriversList estimatedMeasurementParameters, 99 final CovarianceMatrixProvider measurementProcessNoiseMatrix) { 100 super(decomposer, propagatorBuilders); 101 102 // Build the process model and measurement model 103 this.processModel = new KalmanModel(propagatorBuilders, 104 processNoiseMatricesProviders, 105 estimatedMeasurementParameters, 106 measurementProcessNoiseMatrix); 107 108 this.filter = new ExtendedKalmanFilter<>(decomposer, processModel, processModel.getEstimate()); 109 110 } 111 112 /** {@inheritDoc}. */ 113 @Override 114 protected KalmanEstimation getKalmanEstimation() { 115 return processModel; 116 } 117 118 /** {@inheritDoc}. */ 119 @Override 120 protected KalmanFilter<MeasurementDecorator> getKalmanFilter() { 121 return filter; 122 } 123 124 /** {@inheritDoc}. */ 125 @Override 126 protected double[] getScale() { 127 return processModel.getScale(); 128 } 129 130 /** Process a single measurement. 131 * <p> 132 * Update the filter with the new measurement by calling the estimate method. 133 * </p> 134 * @param observedMeasurement the measurement to process 135 * @return estimated propagators 136 */ 137 public Propagator[] estimationStep(final ObservedMeasurement<?> observedMeasurement) { 138 try { 139 final ProcessEstimate estimate = filter.estimationStep(KalmanEstimatorUtil.decorate(observedMeasurement, getReferenceDate())); 140 processModel.finalizeEstimation(observedMeasurement, estimate); 141 if (getObserver() != null) { 142 getObserver().evaluationPerformed(processModel); 143 } 144 return processModel.getEstimatedPropagators(); 145 } catch (MathRuntimeException mrte) { 146 throw new OrekitException(mrte); 147 } 148 } 149 150 /** Process several measurements. 151 * @param observedMeasurements the measurements to process in <em>chronologically sorted</em> order 152 * @return estimated propagators 153 */ 154 public Propagator[] processMeasurements(final Iterable<ObservedMeasurement<?>> observedMeasurements) { 155 Propagator[] propagators = null; 156 for (ObservedMeasurement<?> observedMeasurement : observedMeasurements) { 157 propagators = estimationStep(observedMeasurement); 158 } 159 return propagators; 160 } 161 162 }