UnivariateProcessNoise.java

  1. /* Copyright 2002-2020 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;

  19. import org.hipparchus.analysis.UnivariateFunction;
  20. import org.hipparchus.linear.MatrixUtils;
  21. import org.hipparchus.linear.RealMatrix;
  22. import org.orekit.frames.LOFType;
  23. import org.orekit.orbits.PositionAngle;
  24. import org.orekit.propagation.SpacecraftState;
  25. import org.orekit.utils.CartesianDerivativesFilter;

  27. /** Provider for a temporal evolution of the process noise matrix.
  28.  * All parameters (orbital or propagation) are time dependent and provided as {@link UnivariateFunction}.
  29.  * The argument of the functions is a duration in seconds (between current and previous spacecraft state).
  30.  * The output of the functions must be of the dimension of a standard deviation.
  31.  * The method {@link #getProcessNoiseMatrix} then square the values so that they are consistent with a covariance matrix.
  32.  * <p>
  33.  * The orbital parameters evolutions are provided in LOF frame and Cartesian (PV);
  34.  * then converted in inertial frame and current {@link org.orekit.orbits.OrbitType} and {@link PositionAngle}
  35.  * when method {@link #getProcessNoiseMatrix} is called.
  36.  * </p>
  37.  * <p>
  38.  * The time-dependent functions define a process noise matrix that is diagonal
  39.  * <em>in the Local Orbital Frame</em>, corresponds to Cartesian elements, abd represents
  40.  * the temporal evolution of (the standard deviation of) the process noise model. The
  41.  * first function is therefore the standard deviation along the LOF X axis, the second
  42.  * function represents the standard deviation along the LOF Y axis...
  43.  * This allows to set up simply a process noise representing an uncertainty that grows
  44.  * mainly along the track. The 6x6 upper left part of output matrix will however not be
  45.  * diagonal as it will be converted to the same inertial frame and orbit type as the
  46.  * {@link SpacecraftState state} used by the {@link KalmanEstimator Kalman estimator}.
  47.  * </p>
  48.  * <p>
  49.  * The propagation and measurements parameters are not associated to a specific frame and
  50.  * are appended as is in the lower right part diagonal of the output matrix. This implies
  51.  * this simplified model does not include correlation between the parameters and the orbit,
  52.  * but only evolution of the parameters themselves. If such correlations are needed, users
  53.  * must set up a custom {@link CovarianceMatrixProvider covariance matrix provider}. In most
  54.  * cases, the parameters are constant and their evolution noise is always 0, so the
  55.  * functions can be set to {@code x -> 0}.
  56.  * </p>
  57.  * <p>
  58.  * This class always provides one initial noise matrix or initial covariance matrix and one process noise matrix.
  59.  * </p>
  60.  * @author Luc Maisonobe
  61.  * @author Maxime Journot
  62.  * @since 9.2
  63.  */
  64. public class UnivariateProcessNoise extends AbstractCovarianceMatrixProvider {

  66.     /** Local Orbital Frame (LOF) type used. */
  67.     private final LOFType lofType;

  69.     /** Position angle for the orbital process noise matrix. */
  70.     private final PositionAngle positionAngle;

  72.     /** Array of univariate functions for the six orbital parameters process noise evolution in LOF frame and Cartesian formalism. */
  73.     private final UnivariateFunction[] lofCartesianOrbitalParametersEvolution;

  75.     /** Array of univariate functions for the propagation parameters process noise evolution. */
  76.     private final UnivariateFunction[] propagationParametersEvolution;

  78.     /** Array of univariate functions for the measurements parameters process noise evolution. */
  79.     private final UnivariateFunction[] measurementsParametersEvolution;

  81.     /** Simple constructor.
  82.      * @param initialCovarianceMatrix initial covariance matrix
  83.      * @param lofType the LOF type used
  84.      * @param positionAngle the position angle used for the computation of the process noise
  85.      * @param lofCartesianOrbitalParametersEvolution Array of univariate functions for the six orbital parameters process noise evolution in LOF frame and Cartesian orbit type
  86.      * @param propagationParametersEvolution Array of univariate functions for the propagation parameters process noise evolution
  87.      */
  88.     public UnivariateProcessNoise(final RealMatrix initialCovarianceMatrix,
  89.                                   final LOFType lofType,
  90.                                   final PositionAngle positionAngle,
  91.                                   final UnivariateFunction[] lofCartesianOrbitalParametersEvolution,
  92.                                   final UnivariateFunction[] propagationParametersEvolution) {

  94.         // Call the new constructor with an empty array for measurements parameters
  95.         this(initialCovarianceMatrix, lofType, positionAngle, lofCartesianOrbitalParametersEvolution, propagationParametersEvolution, new UnivariateFunction[0]);
  96.     }

  98.     /** Simple constructor.
  99.      * @param initialCovarianceMatrix initial covariance matrix
  100.      * @param lofType the LOF type used
  101.      * @param positionAngle the position angle used for the computation of the process noise
  102.      * @param lofCartesianOrbitalParametersEvolution Array of univariate functions for the six orbital parameters process noise evolution in LOF frame and Cartesian orbit type
  103.      * @param propagationParametersEvolution Array of univariate functions for the propagation parameters process noise evolution
  104.      * @param measurementsParametersEvolution Array of univariate functions for the measurements parameters process noise evolution
  105.      * @since 10.3
  106.      */
  107.     public UnivariateProcessNoise(final RealMatrix initialCovarianceMatrix,
  108.                                   final LOFType lofType,
  109.                                   final PositionAngle positionAngle,
  110.                                   final UnivariateFunction[] lofCartesianOrbitalParametersEvolution,
  111.                                   final UnivariateFunction[] propagationParametersEvolution,
  112.                                   final UnivariateFunction[] measurementsParametersEvolution) {

  114.         super(initialCovarianceMatrix);
  115.         this.lofType = lofType;
  116.         this.positionAngle = positionAngle;
  117.         this.lofCartesianOrbitalParametersEvolution  = lofCartesianOrbitalParametersEvolution.clone();
  118.         this.propagationParametersEvolution = propagationParametersEvolution.clone();
  119.         this.measurementsParametersEvolution = measurementsParametersEvolution.clone();
  120.     }

  122.     /** Getter for the lofType.
  123.      * @return the lofType
  124.      */
  125.     public LOFType getLofType() {
  126.         return lofType;
  127.     }

  129.     /** Getter for the positionAngle.
  130.      * @return the positionAngle
  131.      */
  132.     public PositionAngle getPositionAngle() {
  133.         return positionAngle;
  134.     }

  136.     /** Getter for the lofCartesianOrbitalParametersEvolution.
  137.      * @return the lofCartesianOrbitalParametersEvolution
  138.      */
  139.     public UnivariateFunction[] getLofCartesianOrbitalParametersEvolution() {
  140.         return lofCartesianOrbitalParametersEvolution.clone();
  141.     }

  143.     /** Getter for the propagationParametersEvolution.
  144.      * @return the propagationParametersEvolution
  145.      */
  146.     public UnivariateFunction[] getPropagationParametersEvolution() {
  147.         return propagationParametersEvolution.clone();
  148.     }

  150.     /** Getter for the measurementsParametersEvolution.
  151.      * @return the measurementsParametersEvolution
  152.      */
  153.     public UnivariateFunction[] getMeasurementsParametersEvolution() {
  154.         return measurementsParametersEvolution.clone();
  155.     }

  157.     /** {@inheritDoc} */
  158.     @Override
  159.     public RealMatrix getProcessNoiseMatrix(final SpacecraftState previous,
  160.                                             final SpacecraftState current) {

  162.         // Number of estimated parameters
  163.         final int nbOrb    = lofCartesianOrbitalParametersEvolution.length;
  164.         final int nbPropag = propagationParametersEvolution.length;
  165.         final int nbMeas   = measurementsParametersEvolution.length;

  167.         // Initialize process noise matrix
  168.         final RealMatrix processNoiseMatrix = MatrixUtils.createRealMatrix(nbOrb + nbPropag + nbMeas,
  169.                                                                            nbOrb + nbPropag + nbMeas);

  171.         // Orbital parameters
  172.         if (nbOrb != 0) {
  173.             // Orbital parameters process noise matrix in inertial frame and current orbit type
  174.             final RealMatrix inertialOrbitalProcessNoiseMatrix = getInertialOrbitalProcessNoiseMatrix(previous, current);
  175.             processNoiseMatrix.setSubMatrix(inertialOrbitalProcessNoiseMatrix.getData(), 0, 0);
  176.         }

  178.         // Propagation parameters
  179.         if (nbPropag != 0) {
  180.             // Propagation parameters process noise matrix
  181.             final RealMatrix propagationProcessNoiseMatrix = getPropagationProcessNoiseMatrix(previous, current);
  182.             processNoiseMatrix.setSubMatrix(propagationProcessNoiseMatrix.getData(), nbOrb, nbOrb);
  183.         }

  185.         // Measurement parameters
  186.         if (nbMeas != 0) {
  187.             // Measurement parameters process noise matrix
  188.             final RealMatrix measurementsProcessNoiseMatrix = getMeasurementsProcessNoiseMatrix(previous, current);
  189.             processNoiseMatrix.setSubMatrix(measurementsProcessNoiseMatrix.getData(), nbOrb + nbPropag, nbOrb + nbPropag);
  190.         }

  192.         return processNoiseMatrix;
  193.     }

  195.     /** Get the process noise for the six orbital parameters in current spacecraft inertial frame and current orbit type.
  196.      * @param previous previous state
  197.      * @param current current state
  198.      * @return physical (i.e. non normalized) orbital process noise matrix in inertial frame
  199.      */
  200.     private RealMatrix getInertialOrbitalProcessNoiseMatrix(final SpacecraftState previous,
  201.                                                             final SpacecraftState current) {

  203.         // ΔT = duration in seconds from previous to current spacecraft state
  204.         final double deltaT = current.getDate().durationFrom(previous.getDate());

  206.         // Evaluate the functions, using ΔT as argument
  207.         final int      lofOrbitalprocessNoiseLength = lofCartesianOrbitalParametersEvolution.length;
  208.         final double[] lofOrbitalProcessNoiseValues = new double[lofOrbitalprocessNoiseLength];

  210.         // The function return a value which dimension is that of a standard deviation
  211.         // It needs to be squared before being put in the process noise covariance matrix
  212.         for (int i = 0; i < lofOrbitalprocessNoiseLength; i++) {
  213.             final double functionValue =  lofCartesianOrbitalParametersEvolution[i].value(deltaT);
  214.             lofOrbitalProcessNoiseValues[i] = functionValue * functionValue;
  215.         }

  217.         // Form the diagonal matrix in LOF frame and Cartesian formalism
  218.         final RealMatrix lofCartesianProcessNoiseMatrix = MatrixUtils.createRealDiagonalMatrix(lofOrbitalProcessNoiseValues);

  220.         // Get the Jacobian from LOF to Inertial
  221.         final double[][] dLofdInertial = new double[6][6];
  222.         lofType.transformFromInertial(current.getDate(),
  223.                                       current.getPVCoordinates()).getInverse().getJacobian(CartesianDerivativesFilter.USE_PV,
  224.                                                                                            dLofdInertial);
  225.         final RealMatrix jacLofToInertial = MatrixUtils.createRealMatrix(dLofdInertial);

  227.         // Jacobian of orbit parameters with respect to Cartesian parameters
  228.         final double[][] dYdC = new double[6][6];
  229.         current.getOrbit().getJacobianWrtCartesian(positionAngle, dYdC);
  230.         final RealMatrix jacOrbitWrtCartesian = MatrixUtils.createRealMatrix(dYdC);

  232.         // Complete Jacobian of the transformation
  233.         final RealMatrix jacobian = jacOrbitWrtCartesian.multiply(jacLofToInertial);

  235.         // Return the orbital process noise matrix in inertial frame and proper orbit type
  236.         final RealMatrix inertialOrbitalProcessNoiseMatrix = jacobian.
  237.                          multiply(lofCartesianProcessNoiseMatrix).
  238.                          multiplyTransposed(jacobian);
  239.         return inertialOrbitalProcessNoiseMatrix;
  240.     }

  242.     /** Get the process noise for the propagation parameters.
  243.      * @param previous previous state
  244.      * @param current current state
  245.      * @return physical (i.e. non normalized) propagation process noise matrix
  246.      */
  247.     private RealMatrix getPropagationProcessNoiseMatrix(final SpacecraftState previous,
  248.                                                         final SpacecraftState current) {

  250.         // ΔT = duration from previous to current spacecraft state (in seconds)
  251.         final double deltaT = current.getDate().durationFrom(previous.getDate());

  253.         // Evaluate the functions, using ΔT as argument
  254.         final int      propagationProcessNoiseLength = propagationParametersEvolution.length;
  255.         final double[] propagationProcessNoiseValues = new double[propagationProcessNoiseLength];

  257.         // The function return a value which dimension is that of a standard deviation
  258.         // It needs to be squared before being put in the process noise covariance matrix
  259.         for (int i = 0; i < propagationProcessNoiseLength; i++) {
  260.             final double functionValue =  propagationParametersEvolution[i].value(deltaT);
  261.             propagationProcessNoiseValues[i] = functionValue * functionValue;
  262.         }

  264.         // Form the diagonal matrix corresponding to propagation parameters process noise
  265.         return MatrixUtils.createRealDiagonalMatrix(propagationProcessNoiseValues);
  266.     }

  268.     /** Get the process noise for the measurements parameters.
  269.      * @param previous previous state
  270.      * @param current current state
  271.      * @return physical (i.e. non normalized) measurements process noise matrix
  272.      */
  273.     private RealMatrix getMeasurementsProcessNoiseMatrix(final SpacecraftState previous,
  274.                                                          final SpacecraftState current) {

  276.         // ΔT = duration from previous to current spacecraft state (in seconds)
  277.         final double deltaT = current.getDate().durationFrom(previous.getDate());

  279.         // Evaluate the functions, using ΔT as argument
  280.         final int      measurementsProcessNoiseLength = measurementsParametersEvolution.length;
  281.         final double[] measurementsProcessNoiseValues = new double[measurementsProcessNoiseLength];

  283.         // The function return a value which dimension is that of a standard deviation
  284.         // It needs to be squared before being put in the process noise covariance matrix
  285.         for (int i = 0; i < measurementsProcessNoiseLength; i++) {
  286.             final double functionValue =  measurementsParametersEvolution[i].value(deltaT);
  287.             measurementsProcessNoiseValues[i] = functionValue * functionValue;
  288.         }

  290.         // Form the diagonal matrix corresponding to propagation parameters process noise
  291.         return MatrixUtils.createRealDiagonalMatrix(measurementsProcessNoiseValues);
  292.     }

  294. }
Created with JaCoCo 0.8.6.202009150832