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