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