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