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.Arrays;
20 import java.util.List;
21
22 import org.hipparchus.linear.MatrixUtils;
23 import org.hipparchus.linear.RealMatrix;
24 import org.hipparchus.linear.RealVector;
25 import org.hipparchus.util.FastMath;
26 import org.orekit.errors.OrekitException;
27 import org.orekit.errors.OrekitMessages;
28 import org.orekit.estimation.measurements.EstimatedMeasurement;
29 import org.orekit.estimation.measurements.EstimationModifier;
30 import org.orekit.estimation.measurements.ObservableSatellite;
31 import org.orekit.estimation.measurements.ObservedMeasurement;
32 import org.orekit.estimation.measurements.PV;
33 import org.orekit.estimation.measurements.Position;
34 import org.orekit.estimation.measurements.modifiers.DynamicOutlierFilter;
35 import org.orekit.propagation.SpacecraftState;
36 import org.orekit.time.AbsoluteDate;
37 import org.orekit.utils.ParameterDriver;
38 import org.orekit.utils.ParameterDriversList;
39
40 /**
41 * Utility class for Kalman Filter.
42 * <p>
43 * This class includes common methods used by the different Kalman
44 * models in Orekit (i.e., Extended, Unscented, and Semi-analytical)
45 * </p>
46 * @since 11.3
47 */
48 public class KalmanEstimatorUtil {
49
50 /** Private constructor.
51 * <p>This class is a utility class, it should neither have a public
52 * nor a default constructor. This private constructor prevents
53 * the compiler from generating one automatically.</p>
54 */
55 private KalmanEstimatorUtil() {
56 }
57
58 /** Decorate an observed measurement.
59 * <p>
60 * The "physical" measurement noise matrix is the covariance matrix of the measurement.
61 * Normalizing it consists in applying the following equation: Rn[i,j] = R[i,j]/σ[i]/σ[j]
62 * Thus the normalized measurement noise matrix is the matrix of the correlation coefficients
63 * between the different components of the measurement.
64 * </p>
65 * @param observedMeasurement the measurement
66 * @param referenceDate reference date
67 * @return decorated measurement
68 */
69 public static MeasurementDecorator decorate(final ObservedMeasurement<?> observedMeasurement,
70 final AbsoluteDate referenceDate) {
71
72 // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
73 // of the measurement on its non-diagonal elements.
74 // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
75 // Normalizing it leaves us with the matrix of the correlation coefficients
76 final RealMatrix covariance;
77 if (observedMeasurement.getMeasurementType().equals(PV.MEASUREMENT_TYPE)) {
78 // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
79 final PV pv = (PV) observedMeasurement;
80 covariance = MatrixUtils.createRealMatrix(pv.getCorrelationCoefficientsMatrix());
81 } else if (observedMeasurement.getMeasurementType().equals(Position.MEASUREMENT_TYPE)) {
82 // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
83 final Position position = (Position) observedMeasurement;
84 covariance = MatrixUtils.createRealMatrix(position.getCorrelationCoefficientsMatrix());
85 } else {
86 // For other measurements we do not have a covariance matrix.
87 // Thus the correlation coefficients matrix is an identity matrix.
88 covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
89 }
90
91 return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);
92
93 }
94
95 /** Decorate an observed measurement for an Unscented Kalman Filter.
96 * <p>
97 * This method uses directly the measurement's covariance matrix, without any normalization.
98 * </p>
99 * @param observedMeasurement the measurement
100 * @param referenceDate reference date
101 * @return decorated measurement
102 * @since 11.3.2
103 */
104 public static MeasurementDecorator decorateUnscented(final ObservedMeasurement<?> observedMeasurement,
105 final AbsoluteDate referenceDate) {
106
107 // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
108 // of the measurement on its non-diagonal elements.
109 // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
110
111 final RealMatrix covariance;
112 if (observedMeasurement.getMeasurementType().equals(PV.MEASUREMENT_TYPE)) {
113 // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
114 final PV pv = (PV) observedMeasurement;
115 covariance = MatrixUtils.createRealMatrix(pv.getCovarianceMatrix());
116 } else if (observedMeasurement.getMeasurementType().equals(Position.MEASUREMENT_TYPE)) {
117 // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
118 final Position position = (Position) observedMeasurement;
119 covariance = MatrixUtils.createRealMatrix(position.getCovarianceMatrix());
120 } else {
121 // For other measurements we do not have a covariance matrix.
122 // Thus the correlation coefficients matrix is an identity matrix.
123 covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
124 final double[] sigma = observedMeasurement.getTheoreticalStandardDeviation();
125 for (int i = 0; i < sigma.length; i++) {
126 covariance.setEntry(i, i, sigma[i] * sigma[i]);
127 }
128
129 }
130
131 return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);
132
133 }
134
135 /** Check dimension.
136 * @param dimension dimension to check
137 * @param orbitalParameters orbital parameters
138 * @param propagationParameters propagation parameters
139 * @param measurementParameters measurements parameters
140 */
141 public static void checkDimension(final int dimension,
142 final ParameterDriversList orbitalParameters,
143 final ParameterDriversList propagationParameters,
144 final ParameterDriversList measurementParameters) {
145
146 // count parameters
147 int requiredDimension = 0;
148 for (final ParameterDriver driver : orbitalParameters.getDrivers()) {
149 if (driver.isSelected()) {
150 ++requiredDimension;
151 }
152 }
153 for (final ParameterDriver driver : propagationParameters.getDrivers()) {
154 if (driver.isSelected()) {
155 ++requiredDimension;
156 }
157 }
158 for (final ParameterDriver driver : measurementParameters.getDrivers()) {
159 if (driver.isSelected()) {
160 ++requiredDimension;
161 }
162 }
163
164 if (dimension != requiredDimension) {
165 // there is a problem, set up an explicit error message
166 final StringBuilder sBuilder = new StringBuilder();
167 for (final ParameterDriver driver : orbitalParameters.getDrivers()) {
168 if (sBuilder.length() > 0) {
169 sBuilder.append(", ");
170 }
171 sBuilder.append(driver.getName());
172 }
173 for (final ParameterDriver driver : propagationParameters.getDrivers()) {
174 if (driver.isSelected()) {
175 sBuilder.append(driver.getName());
176 }
177 }
178 for (final ParameterDriver driver : measurementParameters.getDrivers()) {
179 if (driver.isSelected()) {
180 sBuilder.append(driver.getName());
181 }
182 }
183 throw new OrekitException(OrekitMessages.DIMENSION_INCONSISTENT_WITH_PARAMETERS,
184 dimension, sBuilder.toString());
185 }
186
187 }
188
189 /** Filter relevant states for a measurement.
190 * @param observedMeasurement measurement to consider
191 * @param allStates all states
192 * @return array containing only the states relevant to the measurement
193 */
194 public static SpacecraftState[] filterRelevant(final ObservedMeasurement<?> observedMeasurement,
195 final SpacecraftState[] allStates) {
196 final List<ObservableSatellite> satellites = observedMeasurement.getSatellites();
197 final SpacecraftState[] relevantStates = new SpacecraftState[satellites.size()];
198 for (int i = 0; i < relevantStates.length; ++i) {
199 relevantStates[i] = allStates[satellites.get(i).getPropagatorIndex()];
200 }
201 return relevantStates;
202 }
203
204 /** Set and apply a dynamic outlier filter on a measurement.<p>
205 * Loop on the modifiers to see if a dynamic outlier filter needs to be applied.<p>
206 * Compute the sigma array using the matrix in input and set the filter.<p>
207 * Apply the filter by calling the modify method on the estimated measurement.<p>
208 * Reset the filter.
209 * @param measurement measurement to filter
210 * @param innovationCovarianceMatrix So called innovation covariance matrix S, with:<p>
211 * S = H.Ppred.Ht + R<p>
212 * Where:<p>
213 * - H is the normalized measurement matrix (Ht its transpose)<p>
214 * - Ppred is the normalized predicted covariance matrix<p>
215 * - R is the normalized measurement noise matrix
216 * @param <T> the type of measurement
217 */
218 public static <T extends ObservedMeasurement<T>> void applyDynamicOutlierFilter(final EstimatedMeasurement<T> measurement,
219 final RealMatrix innovationCovarianceMatrix) {
220
221 // Observed measurement associated to the predicted measurement
222 final ObservedMeasurement<T> observedMeasurement = measurement.getObservedMeasurement();
223
224 // Check if a dynamic filter was added to the measurement
225 // If so, update its sigma value and apply it
226 for (EstimationModifier<T> modifier : observedMeasurement.getModifiers()) {
227 if (modifier instanceof DynamicOutlierFilter<?>) {
228 final DynamicOutlierFilter<T> dynamicOutlierFilter = (DynamicOutlierFilter<T>) modifier;
229
230 // Initialize the values of the sigma array used in the dynamic filter
231 final double[] sigmaDynamic = new double[innovationCovarianceMatrix.getColumnDimension()];
232 final double[] sigmaMeasurement = observedMeasurement.getTheoreticalStandardDeviation();
233
234 // Set the sigma value for each element of the measurement
235 // Here we do use the value suggested by David A. Vallado (see [1]§10.6):
236 // sigmaDynamic[i] = sqrt(diag(S))*sigma[i]
237 // With S = H.Ppred.Ht + R
238 // Where:
239 // - S is the measurement error matrix in input
240 // - H is the normalized measurement matrix (Ht its transpose)
241 // - Ppred is the normalized predicted covariance matrix
242 // - R is the normalized measurement noise matrix
243 // - sigma[i] is the theoretical standard deviation of the ith component of the measurement.
244 // It is used here to un-normalize the value before it is filtered
245 for (int i = 0; i < sigmaDynamic.length; i++) {
246 sigmaDynamic[i] = FastMath.sqrt(innovationCovarianceMatrix.getEntry(i, i)) * sigmaMeasurement[i];
247 }
248 dynamicOutlierFilter.setSigma(sigmaDynamic);
249
250 // Apply the modifier on the estimated measurement
251 modifier.modify(measurement);
252
253 // Re-initialize the value of the filter for the next measurement of the same type
254 dynamicOutlierFilter.setSigma(null);
255 }
256 }
257 }
258
259 /**
260 * Compute the unnormalized innovation vector from the given predicted measurement.
261 * @param predicted predicted measurement
262 * @return the innovation vector
263 */
264 public static RealVector computeInnovationVector(final EstimatedMeasurement<?> predicted) {
265 final double[] sigmas = new double[predicted.getObservedMeasurement().getDimension()];
266 Arrays.fill(sigmas, 1.0);
267 return computeInnovationVector(predicted, sigmas);
268 }
269
270 /**
271 * Compute the normalized innovation vector from the given predicted measurement.
272 * @param predicted predicted measurement
273 * @param sigma measurement standard deviation
274 * @return the innovation vector
275 */
276 public static RealVector computeInnovationVector(final EstimatedMeasurement<?> predicted, final double[] sigma) {
277
278 if (predicted.getStatus() == EstimatedMeasurement.Status.REJECTED) {
279 // set innovation to null to notify filter measurement is rejected
280 return null;
281 } else {
282 // Normalized innovation of the measurement (Nx1)
283 final double[] observed = predicted.getObservedMeasurement().getObservedValue();
284 final double[] estimated = predicted.getEstimatedValue();
285 final double[] residuals = new double[observed.length];
286
287 for (int i = 0; i < observed.length; i++) {
288 residuals[i] = (observed[i] - estimated[i]) / sigma[i];
289 }
290 return MatrixUtils.createRealVector(residuals);
291 }
292
293 }
294
295 /**
296 * Normalize a covariance matrix.
297 * @param physicalP "physical" covariance matrix in input
298 * @param parameterScales scale factor of estimated parameters
299 * @return the normalized covariance matrix
300 */
301 public static RealMatrix normalizeCovarianceMatrix(final RealMatrix physicalP,
302 final double[] parameterScales) {
303
304 // Initialize output matrix
305 final int nbParams = physicalP.getRowDimension();
306 final RealMatrix normalizedP = MatrixUtils.createRealMatrix(nbParams, nbParams);
307
308 // Normalize the state matrix
309 for (int i = 0; i < nbParams; ++i) {
310 for (int j = 0; j < nbParams; ++j) {
311 normalizedP.setEntry(i, j, physicalP.getEntry(i, j) / (parameterScales[i] * parameterScales[j]));
312 }
313 }
314 return normalizedP;
315 }
316
317 /**
318 * Un-nomalized the covariance matrix.
319 * @param normalizedP normalized covariance matrix
320 * @param parameterScales scale factor of estimated parameters
321 * @return the un-normalized covariance matrix
322 */
323 public static RealMatrix unnormalizeCovarianceMatrix(final RealMatrix normalizedP,
324 final double[] parameterScales) {
325 // Initialize physical covariance matrix
326 final int nbParams = normalizedP.getRowDimension();
327 final RealMatrix physicalP = MatrixUtils.createRealMatrix(nbParams, nbParams);
328
329 // Un-normalize the covairance matrix
330 for (int i = 0; i < nbParams; ++i) {
331 for (int j = 0; j < nbParams; ++j) {
332 physicalP.setEntry(i, j, normalizedP.getEntry(i, j) * parameterScales[i] * parameterScales[j]);
333 }
334 }
335 return physicalP;
336 }
337
338 /**
339 * Un-nomalized the state transition matrix.
340 * @param normalizedSTM normalized state transition matrix
341 * @param parameterScales scale factor of estimated parameters
342 * @return the un-normalized state transition matrix
343 */
344 public static RealMatrix unnormalizeStateTransitionMatrix(final RealMatrix normalizedSTM,
345 final double[] parameterScales) {
346 // Initialize physical matrix
347 final int nbParams = normalizedSTM.getRowDimension();
348 final RealMatrix physicalSTM = MatrixUtils.createRealMatrix(nbParams, nbParams);
349
350 // Un-normalize the matrix
351 for (int i = 0; i < nbParams; ++i) {
352 for (int j = 0; j < nbParams; ++j) {
353 physicalSTM.setEntry(i, j,
354 normalizedSTM.getEntry(i, j) * parameterScales[i] / parameterScales[j]);
355 }
356 }
357 return physicalSTM;
358 }
359
360 /**
361 * Un-normalize the measurement matrix.
362 * @param normalizedH normalized measurement matrix
363 * @param parameterScales scale factor of estimated parameters
364 * @param sigmas measurement theoretical standard deviation
365 * @return the un-normalized measurement matrix
366 */
367 public static RealMatrix unnormalizeMeasurementJacobian(final RealMatrix normalizedH,
368 final double[] parameterScales,
369 final double[] sigmas) {
370 // Initialize physical matrix
371 final int nbLine = normalizedH.getRowDimension();
372 final int nbCol = normalizedH.getColumnDimension();
373 final RealMatrix physicalH = MatrixUtils.createRealMatrix(nbLine, nbCol);
374
375 // Un-normalize the matrix
376 for (int i = 0; i < nbLine; ++i) {
377 for (int j = 0; j < nbCol; ++j) {
378 physicalH.setEntry(i, j, normalizedH.getEntry(i, j) * sigmas[i] / parameterScales[j]);
379 }
380 }
381 return physicalH;
382 }
383
384 /**
385 * Un-normalize the innovation covariance matrix.
386 * @param normalizedS normalized innovation covariance matrix
387 * @param sigmas measurement theoretical standard deviation
388 * @return the un-normalized innovation covariance matrix
389 */
390 public static RealMatrix unnormalizeInnovationCovarianceMatrix(final RealMatrix normalizedS,
391 final double[] sigmas) {
392 // Initialize physical matrix
393 final int nbMeas = sigmas.length;
394 final RealMatrix physicalS = MatrixUtils.createRealMatrix(nbMeas, nbMeas);
395
396 // Un-normalize the matrix
397 for (int i = 0; i < nbMeas; ++i) {
398 for (int j = 0; j < nbMeas; ++j) {
399 physicalS.setEntry(i, j, normalizedS.getEntry(i, j) * sigmas[i] * sigmas[j]);
400 }
401 }
402 return physicalS;
403 }
404
405 /**
406 * Un-normalize the Kalman gain matrix.
407 * @param normalizedK normalized Kalman gain matrix
408 * @param parameterScales scale factor of estimated parameters
409 * @param sigmas measurement theoretical standard deviation
410 * @return the un-normalized Kalman gain matrix
411 */
412 public static RealMatrix unnormalizeKalmanGainMatrix(final RealMatrix normalizedK,
413 final double[] parameterScales,
414 final double[] sigmas) {
415 // Initialize physical matrix
416 final int nbLine = normalizedK.getRowDimension();
417 final int nbCol = normalizedK.getColumnDimension();
418 final RealMatrix physicalK = MatrixUtils.createRealMatrix(nbLine, nbCol);
419
420 // Un-normalize the matrix
421 for (int i = 0; i < nbLine; ++i) {
422 for (int j = 0; j < nbCol; ++j) {
423 physicalK.setEntry(i, j, normalizedK.getEntry(i, j) * parameterScales[i] / sigmas[j]);
424 }
425 }
426 return physicalK;
427 }
428
429 }