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.Collections;
20 import java.util.List;
21
22 import org.hipparchus.exception.MathRuntimeException;
23 import org.hipparchus.filtering.kalman.KalmanFilter;
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.conversion.DSSTPropagatorBuilder;
29 import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
30 import org.orekit.utils.ParameterDriver;
31 import org.orekit.utils.ParameterDriversList;
32
33 /**
34 * Implementation of an Extended Semi-analytical Kalman Filter (ESKF) to perform orbit determination.
35 * <p>
36 * The filter uses a {@link DSSTPropagatorBuilder}.
37 * </p>
38 * <p>
39 * The estimated parameters are driven by {@link ParameterDriver} objects. They are of 3 different types:<ol>
40 * <li><b>Orbital parameters</b>:The position and velocity of the spacecraft, or, more generally, its orbit.<br>
41 * These parameters are retrieved from the reference trajectory propagator builder when the filter is initialized.</li>
42 * <li><b>Propagation parameters</b>: Some parameters modelling physical processes (SRP or drag coefficients).<br>
43 * They are also retrieved from the propagator builder during the initialization phase.</li>
44 * <li><b>Measurements parameters</b>: Parameters related to measurements (station biases, positions etc...).<br>
45 * They are passed down to the filter in its constructor.</li>
46 * </ol>
47 * <p>
48 * The Kalman filter implementation used is provided by the underlying mathematical library Hipparchus.
49 * All the variables seen by Hipparchus (states, covariances, measurement matrices...) are normalized
50 * using a specific scale for each estimated parameters or standard deviation noise for each measurement components.
51 * </p>
52 *
53 * @see "Folcik Z., Orbit Determination Using Modern Filters/Smoothers and Continuous Thrust Modeling,
54 * Master of Science Thesis, Department of Aeronautics and Astronautics, MIT, June, 2008."
55 *
56 * @see "Cazabonne B., Bayard J., Journot M., and Cefola P. J., A Semi-analytical Approach for Orbit
57 * Determination based on Extended Kalman Filter, AAS Paper 21-614, AAS/AIAA Astrodynamics
58 * Specialist Conference, Big Sky, August 2021."
59 *
60 * @author Julie Bayard
61 * @author Bryan Cazabonne
62 * @author Maxime Journot
63 * @since 11.1
64 */
65 public class SemiAnalyticalKalmanEstimator extends AbstractKalmanEstimator {
66
67 /** Kalman filter process model. */
68 private final SemiAnalyticalKalmanModel processModel;
69
70 /** Filter. */
71 private final ExtendedKalmanFilter<MeasurementDecorator> filter;
72
73 /** Kalman filter estimator constructor (package private).
74 * @param decomposer decomposer to use for the correction phase
75 * @param propagatorBuilder propagator builder used to evaluate the orbit.
76 * @param covarianceMatrixProvider provider for process noise matrix
77 * @param estimatedMeasurementParameters measurement parameters to estimate
78 * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
79 */
80 public SemiAnalyticalKalmanEstimator(final MatrixDecomposer decomposer,
81 final DSSTPropagatorBuilder propagatorBuilder,
82 final CovarianceMatrixProvider covarianceMatrixProvider,
83 final ParameterDriversList estimatedMeasurementParameters,
84 final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
85 super(decomposer, Collections.singletonList(propagatorBuilder));
86 // Build the process model and measurement model
87 this.processModel = new SemiAnalyticalKalmanModel(propagatorBuilder, covarianceMatrixProvider,
88 estimatedMeasurementParameters, measurementProcessNoiseMatrix);
89
90 // Extended Kalman Filter of Hipparchus
91 this.filter = new ExtendedKalmanFilter<>(decomposer, processModel, processModel.getEstimate());
92
93 }
94
95 /** {@inheritDoc}. */
96 @Override
97 protected KalmanEstimation getKalmanEstimation() {
98 return processModel;
99 }
100
101 /** {@inheritDoc}. */
102 @Override
103 protected KalmanFilter<MeasurementDecorator> getKalmanFilter() {
104 return filter;
105 }
106
107 /** {@inheritDoc}. */
108 @Override
109 protected double[] getScale() {
110 return processModel.getScale();
111 }
112
113 /** {@inheritDoc}. */
114 @Override
115 public void setObserver(final KalmanObserver observer) {
116 processModel.setObserver(observer);
117 observer.init(getKalmanEstimation());
118 }
119
120 /** {@inheritDoc}. */
121 @Override
122 public KalmanObserver getObserver() {
123 return processModel.getObserver();
124 }
125
126 /** Process a single measurement.
127 * <p>
128 * Update the filter with the new measurement by calling the estimate method.
129 * </p>
130 * @param observedMeasurements the list of measurements to process
131 * @return estimated propagators
132 */
133 public DSSTPropagator processMeasurements(final List<ObservedMeasurement<?>> observedMeasurements) {
134 try {
135 return processModel.processMeasurements(observedMeasurements, filter);
136 } catch (MathRuntimeException mrte) {
137 throw new OrekitException(mrte);
138 }
139 }
140
141 }
142