PV.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.measurements;

  19. import java.util.Arrays;

  21. import org.hipparchus.exception.LocalizedCoreFormats;
  22. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  23. import org.hipparchus.util.FastMath;
  24. import org.orekit.errors.OrekitException;
  25. import org.orekit.propagation.SpacecraftState;
  26. import org.orekit.time.AbsoluteDate;
  27. import org.orekit.utils.TimeStampedPVCoordinates;

  29. /** Class modeling a position-velocity measurement.
  30.  * <p>
  31.  * For position-only measurement see {@link Position}.
  32.  * </p>
  33.  * @see Position
  34.  * @author Luc Maisonobe
  35.  * @since 8.0
  36.  */
  37. public class PV extends AbstractMeasurement<PV> {

  39.     /** Identity matrix, for states derivatives. */
  40.     private static final double[][] IDENTITY = new double[][] {
  41.         {
  42.             1, 0, 0, 0, 0, 0
  43.         }, {
  44.             0, 1, 0, 0, 0, 0
  45.         }, {
  46.             0, 0, 1, 0, 0, 0
  47.         }, {
  48.             0, 0, 0, 1, 0, 0
  49.         }, {
  50.             0, 0, 0, 0, 1, 0
  51.         }, {
  52.             0, 0, 0, 0, 0, 1
  53.         }
  54.     };

  56.     /** Covariance matrix of the PV measurement (size 6x6). */
  57.     private final double[][] covarianceMatrix;

  59.     /** Constructor with two double for the standard deviations.
  60.      * <p>The first double is the position's standard deviation, common to the 3 position's components.
  61.      * The second double is the position's standard deviation, common to the 3 position's components.</p>
  62.      * <p>
  63.      * The measurement must be in the orbit propagation frame.
  64.      * </p>
  65.      * @param date date of the measurement
  66.      * @param position position
  67.      * @param velocity velocity
  68.      * @param sigmaPosition theoretical standard deviation on position components
  69.      * @param sigmaVelocity theoretical standard deviation on velocity components
  70.      * @param baseWeight base weight
  71.      * @param satellite satellite related to this measurement
  72.      * @since 9.3
  73.      */
  74.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  75.               final double sigmaPosition, final double sigmaVelocity, final double baseWeight,
  76.               final ObservableSatellite satellite) {
  77.         this(date, position, velocity,
  78.              new double[] {
  79.                  sigmaPosition,
  80.                  sigmaPosition,
  81.                  sigmaPosition,
  82.                  sigmaVelocity,
  83.                  sigmaVelocity,
  84.                  sigmaVelocity
  85.              }, baseWeight, satellite);
  86.     }

  88.     /** Constructor with two vectors for the standard deviations.
  89.      * <p>One 3-sized vectors for position standard deviations.
  90.      * One 3-sized vectors for velocity standard deviations.
  91.      * The 3-sized vectors are the square root of the diagonal elements of the covariance matrix.</p>
  92.      * <p>The measurement must be in the orbit propagation frame.</p>
  93.      * @param date date of the measurement
  94.      * @param position position
  95.      * @param velocity velocity
  96.      * @param sigmaPosition 3-sized vector of the standard deviations of the position
  97.      * @param sigmaVelocity 3-sized vector of the standard deviations of the velocity
  98.      * @param baseWeight base weight
  99.      * @param satellite satellite related to this measurement
  100.      * @since 9.3
  101.      */
  102.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  103.               final double[] sigmaPosition, final double[] sigmaVelocity,
  104.               final double baseWeight, final ObservableSatellite satellite) {
  105.         this(date, position, velocity,
  106.              buildPvCovarianceMatrix(sigmaPosition, sigmaVelocity),
  107.              baseWeight, satellite);
  108.     }

  110.     /** Constructor with one vector for the standard deviations.
  111.      * <p>The 6-sized vector is the square root of the diagonal elements of the covariance matrix.</p>
  112.      * <p>The measurement must be in the orbit propagation frame.</p>
  113.      * @param date date of the measurement
  114.      * @param position position
  115.      * @param velocity velocity
  116.      * @param sigmaPV 6-sized vector of the standard deviations
  117.      * @param baseWeight base weight
  118.      * @param satellite satellite related to this measurement
  119.      * @since 9.3
  120.      */
  121.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  122.               final double[] sigmaPV, final double baseWeight, final ObservableSatellite satellite) {
  123.         this(date, position, velocity, buildPvCovarianceMatrix(sigmaPV), baseWeight, satellite);
  124.     }

  126.     /**
  127.      * Constructor with 2 smaller covariance matrices.
  128.      * <p>One 3x3 covariance matrix for position and one 3x3 covariance matrix for velocity.
  129.      * The fact that the covariance matrices are symmetric and positive definite is not checked.</p>
  130.      * <p>The measurement must be in the orbit propagation frame.</p>
  131.      * @param date date of the measurement
  132.      * @param position position
  133.      * @param velocity velocity
  134.      * @param positionCovarianceMatrix 3x3 covariance matrix of the position
  135.      * @param velocityCovarianceMatrix 3x3 covariance matrix of the velocity
  136.      * @param baseWeight base weight
  137.      * @param satellite satellite related to this measurement
  138.      * @since 9.3
  139.      */
  140.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  141.               final double[][] positionCovarianceMatrix, final double[][] velocityCovarianceMatrix,
  142.               final double baseWeight, final ObservableSatellite satellite) {
  143.         this(date, position, velocity,
  144.              buildPvCovarianceMatrix(positionCovarianceMatrix, velocityCovarianceMatrix),
  145.              baseWeight, satellite);
  146.     }

  148.     /** Constructor with full covariance matrix and all inputs.
  149.      * <p>The fact that the covariance matrix is symmetric and positive definite is not checked.</p>
  150.      * <p>The measurement must be in the orbit propagation frame.</p>
  151.      * @param date date of the measurement
  152.      * @param position position
  153.      * @param velocity velocity
  154.      * @param covarianceMatrix 6x6 covariance matrix of the PV measurement
  155.      * @param baseWeight base weight
  156.      * @param satellite satellite related to this measurement
  157.      * @since 9.3
  158.      */
  159.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  160.               final double[][] covarianceMatrix, final double baseWeight, final ObservableSatellite satellite) {
  161.         super(date,
  162.               new double[] {
  163.                   position.getX(), position.getY(), position.getZ(),
  164.                   velocity.getX(), velocity.getY(), velocity.getZ()
  165.               }, extractSigmas(covarianceMatrix),
  166.               new double[] {
  167.                   baseWeight, baseWeight, baseWeight,
  168.                   baseWeight, baseWeight, baseWeight
  169.               }, Arrays.asList(satellite));
  170.         this.covarianceMatrix = covarianceMatrix.clone();
  171.     }

  173.     /** Get the position.
  174.      * @return position
  175.      */
  176.     public Vector3D getPosition() {
  177.         final double[] pv = getObservedValue();
  178.         return new Vector3D(pv[0], pv[1], pv[2]);
  179.     }

  181.     /** Get the velocity.
  182.      * @return velocity
  183.      */
  184.     public Vector3D getVelocity() {
  185.         final double[] pv = getObservedValue();
  186.         return new Vector3D(pv[3], pv[4], pv[5]);
  187.     }

  189.     /** Get the covariance matrix.
  190.      * @return the covariance matrix
  191.      */
  192.     public double[][] getCovarianceMatrix() {
  193.         return covarianceMatrix.clone();
  194.     }

  196.     /** Get the correlation coefficients matrix.
  197.      * <p>This is the 6x6 matrix M such that:
  198.      * <p>Mij = Pij/(σi.σj)
  199.      * <p>Where:
  200.      * <ul>
  201.      * <li>P is the covariance matrix
  202.      * <li>σi is the i-th standard deviation (σi² = Pii)
  203.      * </ul>
  204.      * @return the correlation coefficient matrix (6x6)
  205.      */
  206.     public double[][] getCorrelationCoefficientsMatrix() {

  208.         // Get the standard deviations
  209.         final double[] sigmas = getTheoreticalStandardDeviation();

  211.         // Initialize the correlation coefficients matric to the covariance matrix
  212.         final double[][] corrCoefMatrix = new double[sigmas.length][sigmas.length];

  214.         // Divide by the standard deviations
  215.         for (int i = 0; i < sigmas.length; i++) {
  216.             for (int j = 0; j < sigmas.length; j++) {
  217.                 corrCoefMatrix[i][j] = covarianceMatrix[i][j] / (sigmas[i] * sigmas[j]);
  218.             }
  219.         }
  220.         return corrCoefMatrix;
  221.     }

  223.     /** {@inheritDoc} */
  224.     @Override
  225.     protected EstimatedMeasurement<PV> theoreticalEvaluation(final int iteration, final int evaluation,
  226.                                                              final SpacecraftState[] states) {

  228.         // PV value
  229.         final TimeStampedPVCoordinates pv = states[0].getPVCoordinates();

  231.         // prepare the evaluation
  232.         final EstimatedMeasurement<PV> estimated =
  233.                         new EstimatedMeasurement<>(this, iteration, evaluation, states,
  234.                                                    new TimeStampedPVCoordinates[] {
  235.                                                        pv
  236.                                                    });

  238.         estimated.setEstimatedValue(new double[] {
  239.             pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
  240.             pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ()
  241.         });

  243.         // partial derivatives with respect to state
  244.         estimated.setStateDerivatives(0, IDENTITY);

  246.         return estimated;
  247.     }

  249.     /** Extract standard deviations from a 6x6 PV covariance matrix.
  250.      * Check the size of the PV covariance matrix first.
  251.      * @param pvCovarianceMatrix the 6x6 PV covariance matrix
  252.      * @return the standard deviations (6-sized vector), they are
  253.      * the square roots of the diagonal elements of the covariance matrix in input.
  254.      */
  255.     private static double[] extractSigmas(final double[][] pvCovarianceMatrix) {

  257.         // Check the size of the covariance matrix, should be 6x6
  258.         if (pvCovarianceMatrix.length != 6 || pvCovarianceMatrix[0].length != 6) {
  259.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
  260.                                       pvCovarianceMatrix.length, pvCovarianceMatrix[0],
  261.                                       6, 6);
  262.         }

  264.         // Extract the standard deviations (square roots of the diagonal elements)
  265.         final double[] sigmas = new double[6];
  266.         for (int i = 0; i < sigmas.length; i++) {
  267.             sigmas[i] = FastMath.sqrt(pvCovarianceMatrix[i][i]);
  268.         }
  269.         return sigmas;
  270.     }

  272.     /** Build a 6x6 PV covariance matrix from two 3x3 matrices (covariances in position and velocity).
  273.      * Check the size of the matrices first.
  274.      * @param positionCovarianceMatrix the 3x3 covariance matrix in position
  275.      * @param velocityCovarianceMatrix the 3x3 covariance matrix in velocity
  276.      * @return the 6x6 PV covariance matrix
  277.      */
  278.     private static double[][] buildPvCovarianceMatrix(final double[][] positionCovarianceMatrix,
  279.                                                       final double[][] velocityCovarianceMatrix) {
  280.         // Check the sizes of the matrices first
  281.         if (positionCovarianceMatrix.length != 3 || positionCovarianceMatrix[0].length != 3) {
  282.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
  283.                                       positionCovarianceMatrix.length, positionCovarianceMatrix[0],
  284.                                       3, 3);
  285.         }
  286.         if (velocityCovarianceMatrix.length != 3 || velocityCovarianceMatrix[0].length != 3) {
  287.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
  288.                                       velocityCovarianceMatrix.length, velocityCovarianceMatrix[0],
  289.                                       3, 3);
  290.         }

  292.         // Build the PV 6x6 covariance matrix
  293.         final double[][] pvCovarianceMatrix = new double[6][6];
  294.         for (int i = 0; i < 3; i++) {
  295.             for (int j = 0; j < 3; j++) {
  296.                 pvCovarianceMatrix[i][j]         = positionCovarianceMatrix[i][j];
  297.                 pvCovarianceMatrix[i + 3][j + 3] = velocityCovarianceMatrix[i][j];
  298.             }
  299.         }
  300.         return pvCovarianceMatrix;
  301.     }

  303.     /** Build a 6x6 PV covariance matrix from a 6-sized vector (position and velocity standard deviations).
  304.      * Check the size of the vector first.
  305.      * @param sigmaPV 6-sized vector with position standard deviations on the first 3 elements
  306.      * and velocity standard deviations on the last 3 elements
  307.      * @return the 6x6 PV covariance matrix
  308.      */
  309.     private static double[][] buildPvCovarianceMatrix(final double[] sigmaPV) {
  310.         // Check the size of the vector first
  311.         if (sigmaPV.length != 6) {
  312.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaPV.length, 6);

  314.         }

  316.         // Build the PV 6x6 covariance matrix
  317.         final double[][] pvCovarianceMatrix = new double[6][6];
  318.         for (int i = 0; i < sigmaPV.length; i++) {
  319.             pvCovarianceMatrix[i][i] =  sigmaPV[i] * sigmaPV[i];
  320.         }
  321.         return pvCovarianceMatrix;
  322.     }

  324.     /** Build a 6x6 PV covariance matrix from two 3-sized vectors (position and velocity standard deviations).
  325.      * Check the sizes of the vectors first.
  326.      * @param sigmaPosition standard deviations of the position (3-size vector)
  327.      * @param sigmaVelocity standard deviations of the velocity (3-size vector)
  328.      * @return the 6x6 PV covariance matrix
  329.      */
  330.     private static double[][] buildPvCovarianceMatrix(final double[] sigmaPosition,
  331.                                                       final double[] sigmaVelocity) {

  333.         // Check the sizes of the vectors first
  334.         if (sigmaPosition.length != 3) {
  335.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaPosition.length, 3);

  337.         }
  338.         if (sigmaVelocity.length != 3) {
  339.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaVelocity.length, 3);

  341.         }

  343.         // Build the PV 6x6 covariance matrix
  344.         final double[][] pvCovarianceMatrix = new double[6][6];
  345.         for (int i = 0; i < sigmaPosition.length; i++) {
  346.             pvCovarianceMatrix[i][i]         =  sigmaPosition[i] * sigmaPosition[i];
  347.             pvCovarianceMatrix[i + 3][i + 3] =  sigmaVelocity[i] * sigmaVelocity[i];
  348.         }
  349.         return pvCovarianceMatrix;
  350.     }
  351. }
Created with JaCoCo 0.8.6.202009150832