UnivariateProcessNoise.java

  1. /* Copyright 2002-2019 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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.exception.LocalizedCoreFormats;
  21. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  22. import org.hipparchus.linear.MatrixUtils;
  23. import org.hipparchus.linear.RealMatrix;
  24. import org.orekit.errors.OrekitException;
  25. import org.orekit.frames.LOFType;
  26. import org.orekit.frames.Transform;
  27. import org.orekit.orbits.PositionAngle;
  28. import org.orekit.propagation.SpacecraftState;

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

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

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

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

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

  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) {
  93.         super(initialCovarianceMatrix);
  94.         this.lofType = lofType;
  95.         this.positionAngle = positionAngle;
  96.         this.lofCartesianOrbitalParametersEvolution  = lofCartesianOrbitalParametersEvolution;
  97.         this.propagationParametersEvolution = propagationParametersEvolution;

  99.         // Ensure that the orbital evolution array size is 6
  100.         if (lofCartesianOrbitalParametersEvolution.length != 6) {
  101.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
  102.                                       lofCartesianOrbitalParametersEvolution, 6);
  103.         }
  104.     }

  106.     /** Getter for the lofType.
  107.      * @return the lofType
  108.      */
  109.     public LOFType getLofType() {
  110.         return lofType;
  111.     }

  113.     /** Getter for the positionAngle.
  114.      * @return the positionAngle
  115.      */
  116.     public PositionAngle getPositionAngle() {
  117.         return positionAngle;
  118.     }

  120.     /** Getter for the lofCartesianOrbitalParametersEvolution.
  121.      * @return the lofCartesianOrbitalParametersEvolution
  122.      */
  123.     public UnivariateFunction[] getLofCartesianOrbitalParametersEvolution() {
  124.         return lofCartesianOrbitalParametersEvolution;
  125.     }

  127.     /** Getter for the propagationParametersEvolution.
  128.      * @return the propagationParametersEvolution
  129.      */
  130.     public UnivariateFunction[] getPropagationParametersEvolution() {
  131.         return propagationParametersEvolution;
  132.     }

  134.     /** {@inheritDoc} */
  135.     @Override
  136.     public RealMatrix getProcessNoiseMatrix(final SpacecraftState previous,
  137.                                             final SpacecraftState current) {

  139.         // Orbital parameters process noise matrix in inertial frame and current orbit type
  140.         final RealMatrix inertialOrbitalProcessNoiseMatrix = getInertialOrbitalProcessNoiseMatrix(previous, current);

  142.         if (propagationParametersEvolution.length == 0) {

  144.             // no propagation parameters contribution, just return the orbital part
  145.             return inertialOrbitalProcessNoiseMatrix;

  147.         } else {

  149.             // Propagation parameters process noise matrix
  150.             final RealMatrix propagationProcessNoiseMatrix = getPropagationProcessNoiseMatrix(previous, current);

  152.             // Concatenate the matrices
  153.             final int        orbitalMatrixSize     = lofCartesianOrbitalParametersEvolution.length;
  154.             final int        propagationMatrixSize = propagationParametersEvolution.length;
  155.             final RealMatrix processNoiseMatrix    = MatrixUtils.createRealMatrix(orbitalMatrixSize + propagationMatrixSize,
  156.                                                                                   orbitalMatrixSize + propagationMatrixSize);
  157.             processNoiseMatrix.setSubMatrix(inertialOrbitalProcessNoiseMatrix.getData(), 0, 0);
  158.             processNoiseMatrix.setSubMatrix(propagationProcessNoiseMatrix.getData(), orbitalMatrixSize, orbitalMatrixSize);
  159.             return processNoiseMatrix;

  161.         }
  162.     }

  164.     /** Get the process noise for the six orbital parameters in current spacecraft inertial frame and current orbit type.
  165.      * @param previous previous state
  166.      * @param current current state
  167.      * @return physical (i.e. non normalized) orbital process noise matrix in inertial frame
  168.      */
  169.     private RealMatrix getInertialOrbitalProcessNoiseMatrix(final SpacecraftState previous,
  170.                                                             final SpacecraftState current) {

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

  175.         // Evaluate the functions, using ΔT as argument
  176.         final int      lofOrbitalprocessNoiseLength = lofCartesianOrbitalParametersEvolution.length;
  177.         final double[] lofOrbitalProcessNoiseValues = new double[lofOrbitalprocessNoiseLength];

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

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

  189.         // Get the rotation matrix from LOF to inertial frame
  190.         final double[][] lofToInertialRotation = lofType.rotationFromInertial(current.getPVCoordinates()).
  191.                                                  revert().getMatrix();

  193.         // Jacobian from LOF to inertial frame
  194.         final RealMatrix jacLofToInertial = MatrixUtils.createRealMatrix(6, 6);
  195.         jacLofToInertial.setSubMatrix(lofToInertialRotation, 0, 0);
  196.         jacLofToInertial.setSubMatrix(lofToInertialRotation, 3, 3);

  198.         // FIXME: Trying to fix the transform from LOF to inertial
  199.         final Transform lofToInertial = lofType.transformFromInertial(current.getDate(), current.getPVCoordinates()).getInverse();
  200.         final Vector3D OM = lofToInertial.getRotationRate().negate();
  201.         final double[][] MOM = new double[3][3];
  202.         MOM[0][1] = -OM.getZ();
  203.         MOM[0][2] = OM.getY();
  204.         MOM[1][0] = OM.getZ();
  205.         MOM[1][2] = -OM.getX();
  206.         MOM[2][0] = -OM.getY();
  207.         MOM[2][1] = OM.getX();
  208.         //jacLofToInertial.setSubMatrix(MOM, 3, 0);
  209.         //debug

  211.         // Jacobian of orbit parameters with respect to Cartesian parameters
  212.         final double[][] dYdC = new double[6][6];
  213.         current.getOrbit().getJacobianWrtCartesian(positionAngle, dYdC);
  214.         final RealMatrix jacParametersWrtCartesian = MatrixUtils.createRealMatrix(dYdC);

  216.         // Complete Jacobian of the transformation
  217.         final RealMatrix jacobian = jacParametersWrtCartesian.multiply(jacLofToInertial);

  219.         // Return the orbital process noise matrix in inertial frame and proper orbit type
  220.         final RealMatrix inertialOrbitalProcessNoiseMatrix = jacobian.
  221.                          multiply(lofCartesianProcessNoiseMatrix).
  222.                          multiply(jacobian.transpose());
  223.         return inertialOrbitalProcessNoiseMatrix;
  224.     }

  226.     /** Get the process noise for the propagation parameters.
  227.      * @param previous previous state
  228.      * @param current current state
  229.      * @return physical (i.e. non normalized) propagation process noise matrix
  230.      */
  231.     private RealMatrix getPropagationProcessNoiseMatrix(final SpacecraftState previous,
  232.                                                         final SpacecraftState current) {

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

  237.         // Evaluate the functions, using ΔT as argument
  238.         final int      propagationProcessNoiseLength = propagationParametersEvolution.length;
  239.         final double[] propagationProcessNoiseValues = new double[propagationProcessNoiseLength];

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

  248.         // Form the diagonal matrix corresponding to propagation parameters process noise
  249.         return MatrixUtils.createRealDiagonalMatrix(propagationProcessNoiseValues);
  250.     }

  252. }
Created with JaCoCo 0.8.2.201808211720