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 org.hipparchus.filtering.kalman.ProcessEstimate;
20 import org.hipparchus.filtering.kalman.extended.NonLinearEvolution;
21 import org.hipparchus.filtering.kalman.extended.NonLinearProcess;
22 import org.hipparchus.linear.Array2DRowRealMatrix;
23 import org.hipparchus.linear.MatrixUtils;
24 import org.hipparchus.linear.RealMatrix;
25 import org.hipparchus.linear.RealVector;
26 import org.orekit.estimation.measurements.EstimatedMeasurement;
27 import org.orekit.estimation.measurements.ObservedMeasurement;
28 import org.orekit.orbits.Orbit;
29 import org.orekit.propagation.MatricesHarvester;
30 import org.orekit.propagation.Propagator;
31 import org.orekit.propagation.SpacecraftState;
32 import org.orekit.propagation.conversion.AbstractPropagatorBuilder;
33 import org.orekit.propagation.conversion.PropagatorBuilder;
34 import org.orekit.time.AbsoluteDate;
35 import org.orekit.utils.ParameterDriver;
36 import org.orekit.utils.ParameterDriversList;
37 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
38
39 import java.util.List;
40 import java.util.Map;
41
42 /** Class defining the process model dynamics to use with a {@link KalmanEstimator}.
43 * @author Romain Gerbaud
44 * @author Maxime Journot
45 * @since 9.2
46 */
47 public class KalmanModel extends AbstractKalmanEstimationCommon implements NonLinearProcess<MeasurementDecorator> {
48
49
50 /** Harvesters for extracting Jacobians from integrated states. */
51 private MatricesHarvester[] harvesters;
52
53 /** Propagators for the reference trajectories, up to current date. */
54 private Propagator[] referenceTrajectories;
55
56 /** Kalman process model constructor.
57 * @param propagatorBuilders propagators builders used to evaluate the orbits.
58 * @param covarianceMatricesProviders providers for covariance matrices
59 * @param estimatedMeasurementParameters measurement parameters to estimate
60 * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
61 */
62 public KalmanModel(final List<PropagatorBuilder> propagatorBuilders,
63 final List<CovarianceMatrixProvider> covarianceMatricesProviders,
64 final ParameterDriversList estimatedMeasurementParameters,
65 final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
66 super(propagatorBuilders, covarianceMatricesProviders, estimatedMeasurementParameters, measurementProcessNoiseMatrix);
67 // Build the reference propagators and add their partial derivatives equations implementation
68 updateReferenceTrajectories(getEstimatedPropagators());
69 }
70
71 /** Update the reference trajectories using the propagators as input.
72 * @param propagators The new propagators to use
73 */
74 protected void updateReferenceTrajectories(final Propagator[] propagators) {
75
76 // Update the reference trajectory propagator
77 setReferenceTrajectories(propagators);
78
79 // Jacobian harvesters
80 harvesters = new MatricesHarvester[propagators.length];
81
82 for (int k = 0; k < propagators.length; ++k) {
83 // Link the partial derivatives to this new propagator
84 final String equationName = KalmanEstimator.class.getName() + "-derivatives-" + k;
85 harvesters[k] = getReferenceTrajectories()[k].setupMatricesComputation(equationName, null, null);
86 }
87
88 }
89
90 /** Get the normalized error state transition matrix (STM) from previous point to current point.
91 * The STM contains the partial derivatives of current state with respect to previous state.
92 * The STM is an mxm matrix where m is the size of the state vector.
93 * m = nbOrb + nbPropag + nbMeas
94 * @return the normalized error state transition matrix
95 */
96 private RealMatrix getErrorStateTransitionMatrix() {
97
98 /* The state transition matrix is obtained as follows, with:
99 * - Y : Current state vector
100 * - Y0 : Initial state vector
101 * - Pp : Current propagation parameter
102 * - Pp0: Initial propagation parameter
103 * - Mp : Current measurement parameter
104 * - Mp0: Initial measurement parameter
105 *
106 * | | | | | | | . |
107 * | dY/dY0 | dY/dPp | dY/dMp | | dY/dY0 | dY/dPp | ..0.. |
108 * | | | | | | | . |
109 * |--------|---------|---------| |--------|--------|--------|
110 * | | | | | . | 1 0 0..| . |
111 * STM = | dP/dY0 | dP/dPp0 | dP/dMp | = | ..0.. | 0 1 0..| ..0.. |
112 * | | | | | . | 0 0 1..| . |
113 * |--------|---------|---------| |--------|--------|--------|
114 * | | | | | . | . | 1 0 0..|
115 * | dM/dY0 | dM/dPp0 | dM/dMp0 | | ..0.. | ..0.. | 0 1 0..|
116 * | | | | | . | . | 0 0 1..|
117 */
118
119 // Initialize to the proper size identity matrix
120 final RealMatrix stm = MatrixUtils.createRealIdentityMatrix(getCorrectedEstimate().getState().getDimension());
121
122 // loop over all orbits
123 final SpacecraftState[] predictedSpacecraftStates = getPredictedSpacecraftStates();
124 final int[][] covarianceIndirection = getCovarianceIndirection();
125 final ParameterDriversList[] estimatedOrbitalParameters = getEstimatedOrbitalParametersArray();
126 final ParameterDriversList[] estimatedPropagationParameters = getEstimatedPropagationParametersArray();
127 final double[] scale = getScale();
128 for (int k = 0; k < predictedSpacecraftStates.length; ++k) {
129
130 // Orbital drivers
131 final List<DelegatingDriver> orbitalParameterDrivers =
132 getBuilders().get(k).getOrbitalParametersDrivers().getDrivers();
133
134 // Indexes
135 final int[] indK = covarianceIndirection[k];
136
137 // Derivatives of the state vector with respect to initial state vector
138 final int nbOrbParams = estimatedOrbitalParameters[k].getNbParams();
139 if (nbOrbParams > 0) {
140
141 // Reset reference (for example compute short periodic terms in DSST)
142 harvesters[k].setReferenceState(predictedSpacecraftStates[k]);
143
144 final RealMatrix dYdY0 = harvesters[k].getStateTransitionMatrix(predictedSpacecraftStates[k]);
145
146 // Fill upper left corner (dY/dY0)
147 int stmRow = 0;
148 for (int i = 0; i < dYdY0.getRowDimension(); ++i) {
149 int stmCol = 0;
150 if (orbitalParameterDrivers.get(i).isSelected()) {
151 for (int j = 0; j < nbOrbParams; ++j) {
152 if (orbitalParameterDrivers.get(j).isSelected()) {
153 stm.setEntry(indK[stmRow], indK[stmCol], dYdY0.getEntry(i, j));
154 stmCol += 1;
155 }
156 }
157 stmRow += 1;
158 }
159 }
160 }
161
162 // Derivatives of the state vector with respect to propagation parameters
163 final int nbParams = estimatedPropagationParameters[k].getNbParams();
164 if (nbOrbParams > 0 && nbParams > 0) {
165 final RealMatrix dYdPp = harvesters[k].getParametersJacobian(predictedSpacecraftStates[k]);
166
167 // Fill 1st row, 2nd column (dY/dPp)
168 int stmRow = 0;
169 for (int i = 0; i < dYdPp.getRowDimension(); ++i) {
170 if (orbitalParameterDrivers.get(i).isSelected()) {
171 for (int j = 0; j < nbParams; ++j) {
172 stm.setEntry(indK[stmRow], indK[j + nbOrbParams], dYdPp.getEntry(i, j));
173 }
174 stmRow += 1;
175 }
176 }
177
178 }
179
180 }
181
182 // Normalization of the STM
183 // normalized(STM)ij = STMij*Sj/Si
184 for (int i = 0; i < scale.length; i++) {
185 for (int j = 0; j < scale.length; j++ ) {
186 stm.setEntry(i, j, stm.getEntry(i, j) * scale[j] / scale[i]);
187 }
188 }
189
190 // Return the error state transition matrix
191 return stm;
192
193 }
194
195 /** Get the normalized measurement matrix H.
196 * H contains the partial derivatives of the measurement with respect to the state.
197 * H is an nxm matrix where n is the size of the measurement vector and m the size of the state vector.
198 * @return the normalized measurement matrix H
199 */
200 private RealMatrix getMeasurementMatrix() {
201
202 // Observed measurement characteristics
203 final EstimatedMeasurement<?> predictedMeasurement = getPredictedMeasurement();
204 final SpacecraftState[] evaluationStates = predictedMeasurement.getStates();
205 final ObservedMeasurement<?> observedMeasurement = predictedMeasurement.getObservedMeasurement();
206 final double[] sigma = observedMeasurement.getTheoreticalStandardDeviation();
207
208 // Initialize measurement matrix H: nxm
209 // n: Number of measurements in current measurement
210 // m: State vector size
211 final RealMatrix measurementMatrix = MatrixUtils.
212 createRealMatrix(observedMeasurement.getDimension(),
213 getCorrectedEstimate().getState().getDimension());
214
215 // loop over all orbits involved in the measurement
216 final int[] orbitsStartColumns = getOrbitsStartColumns();
217 final ParameterDriversList[] estimatedPropagationParameters = getEstimatedPropagationParametersArray();
218 final Map<String, Integer> propagationParameterColumns = getPropagationParameterColumns();
219 final Map<String, Integer> measurementParameterColumns = getMeasurementParameterColumns();
220 for (int k = 0; k < evaluationStates.length; ++k) {
221 final int p = observedMeasurement.getSatellites().get(k).getPropagatorIndex();
222
223 // Predicted orbit
224 final Orbit predictedOrbit = evaluationStates[k].getOrbit();
225
226 // Measurement matrix's columns related to orbital parameters
227 // ----------------------------------------------------------
228
229 // Partial derivatives of the current Cartesian coordinates with respect to current orbital state
230 final double[][] aCY = new double[6][6];
231 predictedOrbit.getJacobianWrtParameters(getBuilders().get(p).getPositionAngleType(), aCY); //dC/dY
232 final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);
233
234 // Jacobian of the measurement with respect to current Cartesian coordinates
235 final RealMatrix dMdC = new Array2DRowRealMatrix(predictedMeasurement.getStateDerivatives(k), false);
236
237 // Jacobian of the measurement with respect to current orbital state
238 final RealMatrix dMdY = dMdC.multiply(dCdY);
239
240 // Fill the normalized measurement matrix's columns related to estimated orbital parameters
241 for (int i = 0; i < dMdY.getRowDimension(); ++i) {
242 int jOrb = orbitsStartColumns[p];
243 for (int j = 0; j < dMdY.getColumnDimension(); ++j) {
244 final ParameterDriver driver = getBuilders().get(p).getOrbitalParametersDrivers().getDrivers().get(j);
245 if (driver.isSelected()) {
246 measurementMatrix.setEntry(i, jOrb++,
247 dMdY.getEntry(i, j) / sigma[i] * driver.getScale());
248 }
249 }
250 }
251
252 // Normalized measurement matrix's columns related to propagation parameters
253 // --------------------------------------------------------------
254
255 // Jacobian of the measurement with respect to propagation parameters
256 final int nbParams = estimatedPropagationParameters[p].getNbParams();
257 if (nbParams > 0) {
258 final RealMatrix dYdPp = harvesters[p].getParametersJacobian(evaluationStates[k]);
259 final RealMatrix dMdPp = dMdY.multiply(dYdPp);
260 for (int i = 0; i < dMdPp.getRowDimension(); ++i) {
261 for (int j = 0; j < nbParams; ++j) {
262 final ParameterDriver delegating = estimatedPropagationParameters[p].getDrivers().get(j);
263 measurementMatrix.setEntry(i, propagationParameterColumns.get(delegating.getName()),
264 dMdPp.getEntry(i, j) / sigma[i] * delegating.getScale());
265 }
266 }
267 }
268
269 // Normalized measurement matrix's columns related to measurement parameters
270 // --------------------------------------------------------------
271
272 // Jacobian of the measurement with respect to measurement parameters
273 // Gather the measurement parameters linked to current measurement
274 for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
275 if (driver.isSelected()) {
276 // Derivatives of current measurement w/r to selected measurement parameter
277 final double[] aMPm = predictedMeasurement.getParameterDerivatives(driver);
278
279 // Check that the measurement parameter is managed by the filter
280 if (measurementParameterColumns.get(driver.getName()) != null) {
281 // Column of the driver in the measurement matrix
282 final int driverColumn = measurementParameterColumns.get(driver.getName());
283
284 // Fill the corresponding indexes of the measurement matrix
285 for (int i = 0; i < aMPm.length; ++i) {
286 measurementMatrix.setEntry(i, driverColumn,
287 aMPm[i] / sigma[i] * driver.getScale());
288 }
289 }
290 }
291 }
292 }
293
294 // Return the normalized measurement matrix
295 return measurementMatrix;
296
297 }
298
299 /** {@inheritDoc} */
300 @Override
301 public NonLinearEvolution getEvolution(final double previousTime, final RealVector previousState,
302 final MeasurementDecorator measurement) {
303
304 // Set a reference date for all measurements parameters that lack one (including the not estimated ones)
305 final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
306 for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
307 if (driver.getReferenceDate() == null) {
308 driver.setReferenceDate(getBuilders().get(0).getInitialOrbitDate());
309 }
310 }
311
312 incrementCurrentMeasurementNumber();
313 setCurrentDate(measurement.getObservedMeasurement().getDate());
314
315 // Note:
316 // - n = size of the current measurement
317 // Example:
318 // * 1 for Range, RangeRate and TurnAroundRange
319 // * 2 for Angular (Azimuth/Elevation or Right-ascension/Declination)
320 // * 6 for Position/Velocity
321 // - m = size of the state vector. n = nbOrb + nbPropag + nbMeas
322
323 // Predict the state vector (mx1)
324 final RealVector predictedState = predictState(observedMeasurement.getDate());
325
326 // Get the error state transition matrix (mxm)
327 final RealMatrix stateTransitionMatrix = getErrorStateTransitionMatrix();
328
329 // Predict the measurement based on predicted spacecraft state
330 // Compute the innovations (i.e. residuals of the predicted measurement)
331 // ------------------------------------------------------------
332
333 // Predicted measurement
334 // Note: here the "iteration/evaluation" formalism from the batch LS method
335 // is twisted to fit the need of the Kalman filter.
336 // The number of "iterations" is actually the number of measurements processed by the filter
337 // so far. We use this to be able to apply the OutlierFilter modifiers on the predicted measurement.
338 setPredictedMeasurement(observedMeasurement.estimate(getCurrentMeasurementNumber(),
339 getCurrentMeasurementNumber(),
340 KalmanEstimatorUtil.filterRelevant(observedMeasurement, getPredictedSpacecraftStates())));
341
342 // Normalized measurement matrix (nxm)
343 final RealMatrix measurementMatrix = getMeasurementMatrix();
344
345 // compute process noise matrix
346 final RealMatrix normalizedProcessNoise = getNormalizedProcessNoise(previousState.getDimension());
347
348 return new NonLinearEvolution(measurement.getTime(), predictedState,
349 stateTransitionMatrix, normalizedProcessNoise, measurementMatrix);
350 }
351
352
353 /** {@inheritDoc} */
354 @Override
355 public RealVector getInnovation(final MeasurementDecorator measurement, final NonLinearEvolution evolution,
356 final RealMatrix innovationCovarianceMatrix) {
357
358 // Apply the dynamic outlier filter, if it exists
359 final EstimatedMeasurement<?> predictedMeasurement = getPredictedMeasurement();
360 KalmanEstimatorUtil.applyDynamicOutlierFilter(predictedMeasurement, innovationCovarianceMatrix);
361 // Compute the innovation vector
362 return KalmanEstimatorUtil.computeInnovationVector(predictedMeasurement, predictedMeasurement.getObservedMeasurement().getTheoreticalStandardDeviation());
363 }
364
365 /** Finalize estimation.
366 * @param observedMeasurement measurement that has just been processed
367 * @param estimate corrected estimate
368 */
369 public void finalizeEstimation(final ObservedMeasurement<?> observedMeasurement,
370 final ProcessEstimate estimate) {
371 // Update the parameters with the estimated state
372 // The min/max values of the parameters are handled by the ParameterDriver implementation
373 setCorrectedEstimate(estimate);
374 updateParameters();
375
376 // Get the estimated propagator (mirroring parameter update in the builder)
377 // and the estimated spacecraft state
378 final Propagator[] estimatedPropagators = getEstimatedPropagators();
379 for (int k = 0; k < estimatedPropagators.length; ++k) {
380 setCorrectedSpacecraftState(estimatedPropagators[k].getInitialState(), k);
381 }
382
383 // Compute the estimated measurement using estimated spacecraft state
384 setCorrectedMeasurement(observedMeasurement.estimate(getCurrentMeasurementNumber(),
385 getCurrentMeasurementNumber(),
386 KalmanEstimatorUtil.filterRelevant(observedMeasurement, getCorrectedSpacecraftStates())));
387 // Update the trajectory
388 // ---------------------
389 updateReferenceTrajectories(estimatedPropagators);
390
391 }
392
393 /** Set the predicted normalized state vector.
394 * The predicted/propagated orbit is used to update the state vector
395 * @param date prediction date
396 * @return predicted state
397 */
398 private RealVector predictState(final AbsoluteDate date) {
399
400 // Predicted state is initialized to previous estimated state
401 final RealVector predictedState = getCorrectedEstimate().getState().copy();
402
403 // Orbital parameters counter
404 int jOrb = 0;
405
406 for (int k = 0; k < getPredictedSpacecraftStates().length; ++k) {
407
408 // Propagate the reference trajectory to measurement date
409 final SpacecraftState predictedSpacecraftState = referenceTrajectories[k].propagate(date);
410 setPredictedSpacecraftState(predictedSpacecraftState, k);
411
412 // Update the builder with the predicted orbit
413 // This updates the orbital drivers with the values of the predicted orbit
414 getBuilders().get(k).resetOrbit(predictedSpacecraftState.getOrbit());
415
416 // Additionally, for PropagatorBuilders which use mass, update the builder with the predicted mass value.
417 // If any mass changes have occurred during this estimation step, such as maneuvers,
418 // the updated mass value must be carried over so that new Propagators from this builder start with the updated mass.
419 if (getBuilders().get(k) instanceof AbstractPropagatorBuilder) {
420 ((AbstractPropagatorBuilder<?>) (getBuilders().get(k))).setMass(predictedSpacecraftState.getMass());
421 }
422
423 // The orbital parameters in the state vector are replaced with their predicted values
424 // The propagation & measurement parameters are not changed by the prediction (i.e. the propagation)
425 // As the propagator builder was previously updated with the predicted orbit,
426 // the selected orbital drivers are already up to date with the prediction
427 for (DelegatingDriver orbitalDriver : getBuilders().get(k).getOrbitalParametersDrivers().getDrivers()) {
428 if (orbitalDriver.isSelected()) {
429 predictedState.setEntry(jOrb++, orbitalDriver.getNormalizedValue());
430 }
431 }
432
433 }
434
435 return predictedState;
436
437 }
438
439 /** Update the estimated parameters after the correction phase of the filter.
440 * The min/max allowed values are handled by the parameter themselves.
441 */
442 private void updateParameters() {
443 final RealVector correctedState = getCorrectedEstimate().getState();
444 int i = 0;
445 for (final DelegatingDriver driver : getEstimatedOrbitalParameters().getDrivers()) {
446 // let the parameter handle min/max clipping
447 driver.setNormalizedValue(correctedState.getEntry(i));
448 correctedState.setEntry(i++, driver.getNormalizedValue());
449 }
450 for (final DelegatingDriver driver : getEstimatedPropagationParameters().getDrivers()) {
451 // let the parameter handle min/max clipping
452 driver.setNormalizedValue(correctedState.getEntry(i));
453 correctedState.setEntry(i++, driver.getNormalizedValue());
454 }
455 for (final DelegatingDriver driver : getEstimatedMeasurementsParameters().getDrivers()) {
456 // let the parameter handle min/max clipping
457 driver.setNormalizedValue(correctedState.getEntry(i));
458 correctedState.setEntry(i++, driver.getNormalizedValue());
459 }
460 }
461
462 /** Getter for the reference trajectories.
463 * @return the referencetrajectories
464 */
465 public Propagator[] getReferenceTrajectories() {
466 return referenceTrajectories.clone();
467 }
468
469 /** Setter for the reference trajectories.
470 * @param referenceTrajectories the reference trajectories to be setted
471 */
472 public void setReferenceTrajectories(final Propagator[] referenceTrajectories) {
473 this.referenceTrajectories = referenceTrajectories.clone();
474 }
475
476 }