KalmanEstimatorUtil.java
- /* Copyright 2002-2024 CS GROUP
- * Licensed to CS GROUP (CS) under one or more
- * contributor license agreements. See the NOTICE file distributed with
- * this work for additional information regarding copyright ownership.
- * CS licenses this file to You under the Apache License, Version 2.0
- * (the "License"); you may not use this file except in compliance with
- * the License. You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
- package org.orekit.estimation.sequential;
- import java.util.Arrays;
- import java.util.List;
- import org.hipparchus.linear.MatrixUtils;
- import org.hipparchus.linear.RealMatrix;
- import org.hipparchus.linear.RealVector;
- import org.hipparchus.util.FastMath;
- import org.orekit.errors.OrekitException;
- import org.orekit.errors.OrekitMessages;
- import org.orekit.estimation.measurements.EstimatedMeasurement;
- import org.orekit.estimation.measurements.EstimationModifier;
- import org.orekit.estimation.measurements.ObservableSatellite;
- import org.orekit.estimation.measurements.ObservedMeasurement;
- import org.orekit.estimation.measurements.PV;
- import org.orekit.estimation.measurements.Position;
- import org.orekit.estimation.measurements.modifiers.DynamicOutlierFilter;
- import org.orekit.propagation.SpacecraftState;
- import org.orekit.time.AbsoluteDate;
- import org.orekit.utils.ParameterDriver;
- import org.orekit.utils.ParameterDriversList;
- /**
- * Utility class for Kalman Filter.
- * <p>
- * This class includes common methods used by the different Kalman
- * models in Orekit (i.e., Extended, Unscented, and Semi-analytical)
- * </p>
- * @since 11.3
- */
- public class KalmanEstimatorUtil {
- /** Private constructor.
- * <p>This class is a utility class, it should neither have a public
- * nor a default constructor. This private constructor prevents
- * the compiler from generating one automatically.</p>
- */
- private KalmanEstimatorUtil() {
- }
- /** Decorate an observed measurement.
- * <p>
- * The "physical" measurement noise matrix is the covariance matrix of the measurement.
- * Normalizing it consists in applying the following equation: Rn[i,j] = R[i,j]/σ[i]/σ[j]
- * Thus the normalized measurement noise matrix is the matrix of the correlation coefficients
- * between the different components of the measurement.
- * </p>
- * @param observedMeasurement the measurement
- * @param referenceDate reference date
- * @return decorated measurement
- */
- public static MeasurementDecorator decorate(final ObservedMeasurement<?> observedMeasurement,
- final AbsoluteDate referenceDate) {
- // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
- // of the measurement on its non-diagonal elements.
- // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
- // Normalizing it leaves us with the matrix of the correlation coefficients
- final RealMatrix covariance;
- if (observedMeasurement.getMeasurementType().equals(PV.MEASUREMENT_TYPE)) {
- // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
- final PV pv = (PV) observedMeasurement;
- covariance = MatrixUtils.createRealMatrix(pv.getCorrelationCoefficientsMatrix());
- } else if (observedMeasurement.getMeasurementType().equals(Position.MEASUREMENT_TYPE)) {
- // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
- final Position position = (Position) observedMeasurement;
- covariance = MatrixUtils.createRealMatrix(position.getCorrelationCoefficientsMatrix());
- } else {
- // For other measurements we do not have a covariance matrix.
- // Thus the correlation coefficients matrix is an identity matrix.
- covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
- }
- return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);
- }
- /** Decorate an observed measurement for an Unscented Kalman Filter.
- * <p>
- * This method uses directly the measurement's covariance matrix, without any normalization.
- * </p>
- * @param observedMeasurement the measurement
- * @param referenceDate reference date
- * @return decorated measurement
- * @since 11.3.2
- */
- public static MeasurementDecorator decorateUnscented(final ObservedMeasurement<?> observedMeasurement,
- final AbsoluteDate referenceDate) {
- // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
- // of the measurement on its non-diagonal elements.
- // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
- final RealMatrix covariance;
- if (observedMeasurement.getMeasurementType().equals(PV.MEASUREMENT_TYPE)) {
- // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
- final PV pv = (PV) observedMeasurement;
- covariance = MatrixUtils.createRealMatrix(pv.getCovarianceMatrix());
- } else if (observedMeasurement.getMeasurementType().equals(Position.MEASUREMENT_TYPE)) {
- // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
- final Position position = (Position) observedMeasurement;
- covariance = MatrixUtils.createRealMatrix(position.getCovarianceMatrix());
- } else {
- // For other measurements we do not have a covariance matrix.
- // Thus the correlation coefficients matrix is an identity matrix.
- covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
- final double[] sigma = observedMeasurement.getTheoreticalStandardDeviation();
- for (int i = 0; i < sigma.length; i++) {
- covariance.setEntry(i, i, sigma[i] * sigma[i]);
- }
- }
- return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);
- }
- /** Check dimension.
- * @param dimension dimension to check
- * @param orbitalParameters orbital parameters
- * @param propagationParameters propagation parameters
- * @param measurementParameters measurements parameters
- */
- public static void checkDimension(final int dimension,
- final ParameterDriversList orbitalParameters,
- final ParameterDriversList propagationParameters,
- final ParameterDriversList measurementParameters) {
- // count parameters
- int requiredDimension = 0;
- for (final ParameterDriver driver : orbitalParameters.getDrivers()) {
- if (driver.isSelected()) {
- ++requiredDimension;
- }
- }
- for (final ParameterDriver driver : propagationParameters.getDrivers()) {
- if (driver.isSelected()) {
- ++requiredDimension;
- }
- }
- for (final ParameterDriver driver : measurementParameters.getDrivers()) {
- if (driver.isSelected()) {
- ++requiredDimension;
- }
- }
- if (dimension != requiredDimension) {
- // there is a problem, set up an explicit error message
- final StringBuilder sBuilder = new StringBuilder();
- for (final ParameterDriver driver : orbitalParameters.getDrivers()) {
- if (sBuilder.length() > 0) {
- sBuilder.append(", ");
- }
- sBuilder.append(driver.getName());
- }
- for (final ParameterDriver driver : propagationParameters.getDrivers()) {
- if (driver.isSelected()) {
- sBuilder.append(driver.getName());
- }
- }
- for (final ParameterDriver driver : measurementParameters.getDrivers()) {
- if (driver.isSelected()) {
- sBuilder.append(driver.getName());
- }
- }
- throw new OrekitException(OrekitMessages.DIMENSION_INCONSISTENT_WITH_PARAMETERS,
- dimension, sBuilder.toString());
- }
- }
- /** Filter relevant states for a measurement.
- * @param observedMeasurement measurement to consider
- * @param allStates all states
- * @return array containing only the states relevant to the measurement
- */
- public static SpacecraftState[] filterRelevant(final ObservedMeasurement<?> observedMeasurement,
- final SpacecraftState[] allStates) {
- final List<ObservableSatellite> satellites = observedMeasurement.getSatellites();
- final SpacecraftState[] relevantStates = new SpacecraftState[satellites.size()];
- for (int i = 0; i < relevantStates.length; ++i) {
- relevantStates[i] = allStates[satellites.get(i).getPropagatorIndex()];
- }
- return relevantStates;
- }
- /** Set and apply a dynamic outlier filter on a measurement.<p>
- * Loop on the modifiers to see if a dynamic outlier filter needs to be applied.<p>
- * Compute the sigma array using the matrix in input and set the filter.<p>
- * Apply the filter by calling the modify method on the estimated measurement.<p>
- * Reset the filter.
- * @param measurement measurement to filter
- * @param innovationCovarianceMatrix So called innovation covariance matrix S, with:<p>
- * S = H.Ppred.Ht + R<p>
- * Where:<p>
- * - H is the normalized measurement matrix (Ht its transpose)<p>
- * - Ppred is the normalized predicted covariance matrix<p>
- * - R is the normalized measurement noise matrix
- * @param <T> the type of measurement
- */
- public static <T extends ObservedMeasurement<T>> void applyDynamicOutlierFilter(final EstimatedMeasurement<T> measurement,
- final RealMatrix innovationCovarianceMatrix) {
- // Observed measurement associated to the predicted measurement
- final ObservedMeasurement<T> observedMeasurement = measurement.getObservedMeasurement();
- // Check if a dynamic filter was added to the measurement
- // If so, update its sigma value and apply it
- for (EstimationModifier<T> modifier : observedMeasurement.getModifiers()) {
- if (modifier instanceof DynamicOutlierFilter<?>) {
- final DynamicOutlierFilter<T> dynamicOutlierFilter = (DynamicOutlierFilter<T>) modifier;
- // Initialize the values of the sigma array used in the dynamic filter
- final double[] sigmaDynamic = new double[innovationCovarianceMatrix.getColumnDimension()];
- final double[] sigmaMeasurement = observedMeasurement.getTheoreticalStandardDeviation();
- // Set the sigma value for each element of the measurement
- // Here we do use the value suggested by David A. Vallado (see [1]§10.6):
- // sigmaDynamic[i] = sqrt(diag(S))*sigma[i]
- // With S = H.Ppred.Ht + R
- // Where:
- // - S is the measurement error matrix in input
- // - H is the normalized measurement matrix (Ht its transpose)
- // - Ppred is the normalized predicted covariance matrix
- // - R is the normalized measurement noise matrix
- // - sigma[i] is the theoretical standard deviation of the ith component of the measurement.
- // It is used here to un-normalize the value before it is filtered
- for (int i = 0; i < sigmaDynamic.length; i++) {
- sigmaDynamic[i] = FastMath.sqrt(innovationCovarianceMatrix.getEntry(i, i)) * sigmaMeasurement[i];
- }
- dynamicOutlierFilter.setSigma(sigmaDynamic);
- // Apply the modifier on the estimated measurement
- modifier.modify(measurement);
- // Re-initialize the value of the filter for the next measurement of the same type
- dynamicOutlierFilter.setSigma(null);
- }
- }
- }
- /**
- * Compute the unnormalized innovation vector from the given predicted measurement.
- * @param predicted predicted measurement
- * @return the innovation vector
- */
- public static RealVector computeInnovationVector(final EstimatedMeasurement<?> predicted) {
- final double[] sigmas = new double[predicted.getObservedMeasurement().getDimension()];
- Arrays.fill(sigmas, 1.0);
- return computeInnovationVector(predicted, sigmas);
- }
- /**
- * Compute the normalized innovation vector from the given predicted measurement.
- * @param predicted predicted measurement
- * @param sigma measurement standard deviation
- * @return the innovation vector
- */
- public static RealVector computeInnovationVector(final EstimatedMeasurement<?> predicted, final double[] sigma) {
- if (predicted.getStatus() == EstimatedMeasurement.Status.REJECTED) {
- // set innovation to null to notify filter measurement is rejected
- return null;
- } else {
- // Normalized innovation of the measurement (Nx1)
- final double[] observed = predicted.getObservedMeasurement().getObservedValue();
- final double[] estimated = predicted.getEstimatedValue();
- final double[] residuals = new double[observed.length];
- for (int i = 0; i < observed.length; i++) {
- residuals[i] = (observed[i] - estimated[i]) / sigma[i];
- }
- return MatrixUtils.createRealVector(residuals);
- }
- }
- /**
- * Normalize a covariance matrix.
- * @param physicalP "physical" covariance matrix in input
- * @param parameterScales scale factor of estimated parameters
- * @return the normalized covariance matrix
- */
- public static RealMatrix normalizeCovarianceMatrix(final RealMatrix physicalP,
- final double[] parameterScales) {
- // Initialize output matrix
- final int nbParams = physicalP.getRowDimension();
- final RealMatrix normalizedP = MatrixUtils.createRealMatrix(nbParams, nbParams);
- // Normalize the state matrix
- for (int i = 0; i < nbParams; ++i) {
- for (int j = 0; j < nbParams; ++j) {
- normalizedP.setEntry(i, j, physicalP.getEntry(i, j) / (parameterScales[i] * parameterScales[j]));
- }
- }
- return normalizedP;
- }
- /**
- * Un-nomalized the covariance matrix.
- * @param normalizedP normalized covariance matrix
- * @param parameterScales scale factor of estimated parameters
- * @return the un-normalized covariance matrix
- */
- public static RealMatrix unnormalizeCovarianceMatrix(final RealMatrix normalizedP,
- final double[] parameterScales) {
- // Initialize physical covariance matrix
- final int nbParams = normalizedP.getRowDimension();
- final RealMatrix physicalP = MatrixUtils.createRealMatrix(nbParams, nbParams);
- // Un-normalize the covairance matrix
- for (int i = 0; i < nbParams; ++i) {
- for (int j = 0; j < nbParams; ++j) {
- physicalP.setEntry(i, j, normalizedP.getEntry(i, j) * parameterScales[i] * parameterScales[j]);
- }
- }
- return physicalP;
- }
- /**
- * Un-nomalized the state transition matrix.
- * @param normalizedSTM normalized state transition matrix
- * @param parameterScales scale factor of estimated parameters
- * @return the un-normalized state transition matrix
- */
- public static RealMatrix unnormalizeStateTransitionMatrix(final RealMatrix normalizedSTM,
- final double[] parameterScales) {
- // Initialize physical matrix
- final int nbParams = normalizedSTM.getRowDimension();
- final RealMatrix physicalSTM = MatrixUtils.createRealMatrix(nbParams, nbParams);
- // Un-normalize the matrix
- for (int i = 0; i < nbParams; ++i) {
- for (int j = 0; j < nbParams; ++j) {
- physicalSTM.setEntry(i, j,
- normalizedSTM.getEntry(i, j) * parameterScales[i] / parameterScales[j]);
- }
- }
- return physicalSTM;
- }
- /**
- * Un-normalize the measurement matrix.
- * @param normalizedH normalized measurement matrix
- * @param parameterScales scale factor of estimated parameters
- * @param sigmas measurement theoretical standard deviation
- * @return the un-normalized measurement matrix
- */
- public static RealMatrix unnormalizeMeasurementJacobian(final RealMatrix normalizedH,
- final double[] parameterScales,
- final double[] sigmas) {
- // Initialize physical matrix
- final int nbLine = normalizedH.getRowDimension();
- final int nbCol = normalizedH.getColumnDimension();
- final RealMatrix physicalH = MatrixUtils.createRealMatrix(nbLine, nbCol);
- // Un-normalize the matrix
- for (int i = 0; i < nbLine; ++i) {
- for (int j = 0; j < nbCol; ++j) {
- physicalH.setEntry(i, j, normalizedH.getEntry(i, j) * sigmas[i] / parameterScales[j]);
- }
- }
- return physicalH;
- }
- /**
- * Un-normalize the innovation covariance matrix.
- * @param normalizedS normalized innovation covariance matrix
- * @param sigmas measurement theoretical standard deviation
- * @return the un-normalized innovation covariance matrix
- */
- public static RealMatrix unnormalizeInnovationCovarianceMatrix(final RealMatrix normalizedS,
- final double[] sigmas) {
- // Initialize physical matrix
- final int nbMeas = sigmas.length;
- final RealMatrix physicalS = MatrixUtils.createRealMatrix(nbMeas, nbMeas);
- // Un-normalize the matrix
- for (int i = 0; i < nbMeas; ++i) {
- for (int j = 0; j < nbMeas; ++j) {
- physicalS.setEntry(i, j, normalizedS.getEntry(i, j) * sigmas[i] * sigmas[j]);
- }
- }
- return physicalS;
- }
- /**
- * Un-normalize the Kalman gain matrix.
- * @param normalizedK normalized Kalman gain matrix
- * @param parameterScales scale factor of estimated parameters
- * @param sigmas measurement theoretical standard deviation
- * @return the un-normalized Kalman gain matrix
- */
- public static RealMatrix unnormalizeKalmanGainMatrix(final RealMatrix normalizedK,
- final double[] parameterScales,
- final double[] sigmas) {
- // Initialize physical matrix
- final int nbLine = normalizedK.getRowDimension();
- final int nbCol = normalizedK.getColumnDimension();
- final RealMatrix physicalK = MatrixUtils.createRealMatrix(nbLine, nbCol);
- // Un-normalize the matrix
- for (int i = 0; i < nbLine; ++i) {
- for (int j = 0; j < nbCol; ++j) {
- physicalK.setEntry(i, j, normalizedK.getEntry(i, j) * parameterScales[i] / sigmas[j]);
- }
- }
- return physicalK;
- }
- }