UnivariateProcessNoise.java

  1. /* Copyright 2002-2018 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 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.  * <p>
  64.  * WARNING: as of release 9.2, this feature is still considered experimental
  65.  * </p>
  66.  * @author Luc Maisonobe
  67.  * @author Maxime Journot
  68.  * @since 9.2
  69.  */
  70. public class UnivariateProcessNoise extends AbstractCovarianceMatrixProvider {

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

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

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

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

  84.     /** Simple constructor.
  85.      * @param initialCovarianceMatrix initial covariance matrix
  86.      * @param lofType the LOF type used
  87.      * @param positionAngle the position angle used for the computation of the process noise
  88.      * @param lofCartesianOrbitalParametersEvolution Array of univariate functions for the six orbital parameters process noise evolution in LOF frame and Cartesian orbit type
  89.      * @param propagationParametersEvolution Array of univariate functions for the propagation parameters process noise evolution
  90.      * @throws OrekitException if lofOrbitalParametersEvolution array size is different from 6
  91.      */
  92.     public UnivariateProcessNoise(final RealMatrix initialCovarianceMatrix,
  93.                                   final LOFType lofType,
  94.                                   final PositionAngle positionAngle,
  95.                                   final UnivariateFunction[] lofCartesianOrbitalParametersEvolution,
  96.                                   final UnivariateFunction[] propagationParametersEvolution)
  97.         throws OrekitException {
  98.         super(initialCovarianceMatrix);
  99.         this.lofType = lofType;
  100.         this.positionAngle = positionAngle;
  101.         this.lofCartesianOrbitalParametersEvolution  = lofCartesianOrbitalParametersEvolution;
  102.         this.propagationParametersEvolution = propagationParametersEvolution;

  104.         // Ensure that the orbital evolution array size is 6
  105.         if (lofCartesianOrbitalParametersEvolution.length != 6) {
  106.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
  107.                                       lofCartesianOrbitalParametersEvolution, 6);
  108.         }
  109.     }

  111.     /** Getter for the lofType.
  112.      * @return the lofType
  113.      */
  114.     public LOFType getLofType() {
  115.         return lofType;
  116.     }

  118.     /** Getter for the positionAngle.
  119.      * @return the positionAngle
  120.      */
  121.     public PositionAngle getPositionAngle() {
  122.         return positionAngle;
  123.     }

  125.     /** Getter for the lofCartesianOrbitalParametersEvolution.
  126.      * @return the lofCartesianOrbitalParametersEvolution
  127.      */
  128.     public UnivariateFunction[] getLofCartesianOrbitalParametersEvolution() {
  129.         return lofCartesianOrbitalParametersEvolution;
  130.     }

  132.     /** Getter for the propagationParametersEvolution.
  133.      * @return the propagationParametersEvolution
  134.      */
  135.     public UnivariateFunction[] getPropagationParametersEvolution() {
  136.         return propagationParametersEvolution;
  137.     }

  139.     /** {@inheritDoc} */
  140.     @Override
  141.     public RealMatrix getProcessNoiseMatrix(final SpacecraftState previous,
  142.                                             final SpacecraftState current) {

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

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

  149.             // no propagation parameters contribution, just return the orbital part
  150.             return inertialOrbitalProcessNoiseMatrix;

  152.         } else {

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

  157.             // Concatenate the matrices
  158.             final int        orbitalMatrixSize     = lofCartesianOrbitalParametersEvolution.length;
  159.             final int        propagationMatrixSize = propagationParametersEvolution.length;
  160.             final RealMatrix processNoiseMatrix    = MatrixUtils.createRealMatrix(orbitalMatrixSize + propagationMatrixSize,
  161.                                                                                   orbitalMatrixSize + propagationMatrixSize);
  162.             processNoiseMatrix.setSubMatrix(inertialOrbitalProcessNoiseMatrix.getData(), 0, 0);
  163.             processNoiseMatrix.setSubMatrix(propagationProcessNoiseMatrix.getData(), orbitalMatrixSize, orbitalMatrixSize);
  164.             return processNoiseMatrix;

  166.         }
  167.     }

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

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

  180.         // Evaluate the functions, using ΔT as argument
  181.         final int      lofOrbitalprocessNoiseLength = lofCartesianOrbitalParametersEvolution.length;
  182.         final double[] lofOrbitalProcessNoiseValues = new double[lofOrbitalprocessNoiseLength];

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

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

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

  198.         // Jacobian from LOF to inertial frame
  199.         final RealMatrix jacLofToInertial = MatrixUtils.createRealMatrix(6, 6);
  200.         jacLofToInertial.setSubMatrix(lofToInertialRotation, 0, 0);
  201.         jacLofToInertial.setSubMatrix(lofToInertialRotation, 3, 3);

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

  216.         // Jacobian of orbit parameters with respect to Cartesian parameters
  217.         final double[][] dYdC = new double[6][6];
  218.         current.getOrbit().getJacobianWrtCartesian(positionAngle, dYdC);
  219.         final RealMatrix jacParametersWrtCartesian = MatrixUtils.createRealMatrix(dYdC);

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

  224.         // Return the orbital process noise matrix in inertial frame and proper orbit type
  225.         final RealMatrix inertialOrbitalProcessNoiseMatrix = jacobian.
  226.                          multiply(lofCartesianProcessNoiseMatrix).
  227.                          multiply(jacobian.transpose());
  228.         return inertialOrbitalProcessNoiseMatrix;
  229.     }

  231.     /** Get the process noise for the propagation parameters.
  232.      * @param previous previous state
  233.      * @param current current state
  234.      * @return physical (i.e. non normalized) propagation process noise matrix
  235.      */
  236.     private RealMatrix getPropagationProcessNoiseMatrix(final SpacecraftState previous,
  237.                                                         final SpacecraftState current) {

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

  242.         // Evaluate the functions, using ΔT as argument
  243.         final int      propagationProcessNoiseLength = propagationParametersEvolution.length;
  244.         final double[] propagationProcessNoiseValues = new double[propagationProcessNoiseLength];

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

  253.         // Form the diagonal matrix corresponding to propagation parameters process noise
  254.         return MatrixUtils.createRealDiagonalMatrix(propagationProcessNoiseValues);
  255.     }

  257. }
Created with JaCoCo 0.7.9.201702052155