PV.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.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.      * <p>This constructor uses 0 as the index of the propagator related
  66.      * to this measurement, thus being well suited for mono-satellite
  67.      * orbit determination.</p>
  68.      * @param date date of the measurement
  69.      * @param position position
  70.      * @param velocity velocity
  71.      * @param sigmaPosition theoretical standard deviation on position components
  72.      * @param sigmaVelocity theoretical standard deviation on velocity components
  73.      * @param baseWeight base weight
  74.      * @deprecated as of 9.3, replaced by {@link #PV(AbsoluteDate, Vector3D, Vector3D,
  75.      * double, double, double, ObservableSatellite)}
  76.      */
  77.     @Deprecated
  78.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  79.               final double sigmaPosition, final double sigmaVelocity, final double baseWeight) {
  80.         this(date, position, velocity, sigmaPosition, sigmaVelocity, baseWeight, new ObservableSatellite(0));
  81.     }

  83.     /** Constructor with two double for the standard deviations.
  84.      * <p>The first double is the position's standard deviation, common to the 3 position's components.
  85.      * The second double is the position's standard deviation, common to the 3 position's components.</p>
  86.      * <p>
  87.      * The measurement must be in the orbit propagation frame.
  88.      * </p>
  89.      * @param date date of the measurement
  90.      * @param position position
  91.      * @param velocity velocity
  92.      * @param sigmaPosition theoretical standard deviation on position components
  93.      * @param sigmaVelocity theoretical standard deviation on velocity components
  94.      * @param baseWeight base weight
  95.      * @param propagatorIndex index of the propagator related to this measurement
  96.      * @since 9.0
  97.      * @deprecated as of 9.3, replaced by {@link #PV(AbsoluteDate, Vector3D, Vector3D,
  98.      * double, double, double, ObservableSatellite)}
  99.      */
  100.     @Deprecated
  101.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  102.               final double sigmaPosition, final double sigmaVelocity, final double baseWeight,
  103.               final int propagatorIndex) {
  104.         this(date, position, velocity, sigmaPosition, sigmaVelocity, baseWeight, new ObservableSatellite(propagatorIndex));
  105.     }

  107.     /** Constructor with two double for the standard deviations.
  108.      * <p>The first double is the position's standard deviation, common to the 3 position's components.
  109.      * The second double is the position's standard deviation, common to the 3 position's components.</p>
  110.      * <p>
  111.      * The measurement must be in the orbit propagation frame.
  112.      * </p>
  113.      * @param date date of the measurement
  114.      * @param position position
  115.      * @param velocity velocity
  116.      * @param sigmaPosition theoretical standard deviation on position components
  117.      * @param sigmaVelocity theoretical standard deviation on velocity components
  118.      * @param baseWeight base weight
  119.      * @param satellite satellite related to this measurement
  120.      * @since 9.3
  121.      */
  122.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  123.               final double sigmaPosition, final double sigmaVelocity, final double baseWeight,
  124.               final ObservableSatellite satellite) {
  125.         this(date, position, velocity,
  126.              new double[] {
  127.                  sigmaPosition,
  128.                  sigmaPosition,
  129.                  sigmaPosition,
  130.                  sigmaVelocity,
  131.                  sigmaVelocity,
  132.                  sigmaVelocity
  133.              }, baseWeight, satellite);
  134.     }

  136.     /** Constructor with two vectors for the standard deviations and default value for propagator index.
  137.      * <p>One 3-sized vectors for position standard deviations.
  138.      * One 3-sized vectors for velocity standard deviations.
  139.      * The 3-sized vectors are the square root of the diagonal elements of the covariance matrix.</p>
  140.      * <p>The measurement must be in the orbit propagation frame.</p>
  141.      * <p>This constructor uses 0 as the index of the propagator related
  142.      * to this measurement, thus being well suited for mono-satellite
  143.      * orbit determination.</p>
  144.      * @param date date of the measurement
  145.      * @param position position
  146.      * @param velocity velocity
  147.      * @param sigmaPosition 3-sized vector of the standard deviations of the position
  148.      * @param sigmaVelocity 3-sized vector of the standard deviations of the velocity
  149.      * @param baseWeight base weight
  150.      * @since 9.2
  151.      * @deprecated as of 9.3, replaced by {@link #PV(AbsoluteDate, Vector3D, Vector3D,
  152.      * double[], double[], double, ObservableSatellite)}
  153.      */
  154.     @Deprecated
  155.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  156.               final double[] sigmaPosition, final double[] sigmaVelocity, final double baseWeight) {
  157.         this(date, position, velocity, sigmaPosition, sigmaVelocity, baseWeight, new ObservableSatellite(0));
  158.     }

  160.     /** Constructor with two vectors for the standard deviations.
  161.      * <p>One 3-sized vectors for position standard deviations.
  162.      * One 3-sized vectors for velocity standard deviations.
  163.      * The 3-sized vectors are the square root of the diagonal elements of the covariance matrix.</p>
  164.      * <p>The measurement must be in the orbit propagation frame.</p>
  165.      * @param date date of the measurement
  166.      * @param position position
  167.      * @param velocity velocity
  168.      * @param sigmaPosition 3-sized vector of the standard deviations of the position
  169.      * @param sigmaVelocity 3-sized vector of the standard deviations of the velocity
  170.      * @param baseWeight base weight
  171.      * @param propagatorIndex index of the propagator related to this measurement
  172.      * @since 9.2
  173.      * @deprecated as of 9.3, replaced by {@link #PV(AbsoluteDate, Vector3D, Vector3D,
  174.      * double[], double[], double, ObservableSatellite)}
  175.      */
  176.     @Deprecated
  177.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  178.               final double[] sigmaPosition, final double[] sigmaVelocity,
  179.               final double baseWeight, final int propagatorIndex) {
  180.         this(date, position, velocity, sigmaPosition, sigmaVelocity,
  181.              baseWeight, new ObservableSatellite(propagatorIndex));
  182.     }

  184.     /** Constructor with two vectors for the standard deviations.
  185.      * <p>One 3-sized vectors for position standard deviations.
  186.      * One 3-sized vectors for velocity standard deviations.
  187.      * The 3-sized vectors are the square root of the diagonal elements of the covariance matrix.</p>
  188.      * <p>The measurement must be in the orbit propagation frame.</p>
  189.      * @param date date of the measurement
  190.      * @param position position
  191.      * @param velocity velocity
  192.      * @param sigmaPosition 3-sized vector of the standard deviations of the position
  193.      * @param sigmaVelocity 3-sized vector of the standard deviations of the velocity
  194.      * @param baseWeight base weight
  195.      * @param satellite satellite related to this measurement
  196.      * @since 9.3
  197.      */
  198.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  199.               final double[] sigmaPosition, final double[] sigmaVelocity,
  200.               final double baseWeight, final ObservableSatellite satellite) {
  201.         this(date, position, velocity,
  202.              buildPvCovarianceMatrix(sigmaPosition, sigmaVelocity),
  203.              baseWeight, satellite);
  204.     }

  206.     /** Constructor with one vector for the standard deviations and default value for propagator index.
  207.      * <p>The 6-sized vector is the square root of the diagonal elements of the covariance matrix.</p>
  208.      * <p>The measurement must be in the orbit propagation frame.</p>
  209.      * <p>This constructor uses 0 as the index of the propagator related
  210.      * to this measurement, thus being well suited for mono-satellite
  211.      * orbit determination.</p>
  212.      * @param date date of the measurement
  213.      * @param position position
  214.      * @param velocity velocity
  215.      * @param sigmaPV 6-sized vector of the standard deviations
  216.      * @param baseWeight base weight
  217.      * @since 9.2
  218.      * @deprecated as of 9.3, replaced by {@link #PV(AbsoluteDate, Vector3D, Vector3D,
  219.      * double[], double, ObservableSatellite)}
  220.      */
  221.     @Deprecated
  222.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  223.               final double[] sigmaPV, final double baseWeight) {
  224.         this(date, position, velocity, sigmaPV, baseWeight, new ObservableSatellite(0));
  225.     }

  227.     /** Constructor with one vector for the standard deviations.
  228.      * <p>The 6-sized vector is the square root of the diagonal elements of the covariance matrix.</p>
  229.      * <p>The measurement must be in the orbit propagation frame.</p>
  230.      * @param date date of the measurement
  231.      * @param position position
  232.      * @param velocity velocity
  233.      * @param sigmaPV 6-sized vector of the standard deviations
  234.      * @param baseWeight base weight
  235.      * @param propagatorIndex index of the propagator related to this measurement
  236.      * @since 9.2
  237.      * @deprecated as of 9.3, replaced by {@link #PV(AbsoluteDate, Vector3D, Vector3D,
  238.      * double[], double, ObservableSatellite)}
  239.      */
  240.     @Deprecated
  241.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  242.               final double[] sigmaPV, final double baseWeight, final int propagatorIndex) {
  243.         this(date, position, velocity, sigmaPV, baseWeight, new ObservableSatellite(0));
  244.     }

  246.     /** Constructor with one vector for the standard deviations.
  247.      * <p>The 6-sized vector is the square root of the diagonal elements of the covariance matrix.</p>
  248.      * <p>The measurement must be in the orbit propagation frame.</p>
  249.      * @param date date of the measurement
  250.      * @param position position
  251.      * @param velocity velocity
  252.      * @param sigmaPV 6-sized vector of the standard deviations
  253.      * @param baseWeight base weight
  254.      * @param satellite satellite related to this measurement
  255.      * @since 9.3
  256.      */
  257.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  258.               final double[] sigmaPV, final double baseWeight, final ObservableSatellite satellite) {
  259.         this(date, position, velocity, buildPvCovarianceMatrix(sigmaPV), baseWeight, satellite);
  260.     }

  262.     /**
  263.      * Constructor with 2 smaller covariance matrices and default value for propagator index.
  264.      * <p>One 3x3 covariance matrix for position and one 3x3 covariance matrix for velocity.
  265.      * The fact that the covariance matrices are symmetric and positive definite is not checked.</p>
  266.      * <p>The measurement must be in the orbit propagation frame.</p>
  267.      * <p>This constructor uses 0 as the index of the propagator related
  268.      * to this measurement, thus being well suited for mono-satellite
  269.      * orbit determination.</p>
  270.      * @param date date of the measurement
  271.      * @param position position
  272.      * @param velocity velocity
  273.      * @param positionCovarianceMatrix 3x3 covariance matrix of the position
  274.      * @param velocityCovarianceMatrix 3x3 covariance matrix of the velocity
  275.      * @param baseWeight base weight
  276.      * @since 9.2
  277.      * @deprecated as of 9.3, replaced by {@link #PV(AbsoluteDate, Vector3D, Vector3D,
  278.      * double[][], double[][], double, ObservableSatellite)}
  279.      */
  280.     @Deprecated
  281.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  282.               final double[][] positionCovarianceMatrix, final double[][] velocityCovarianceMatrix,
  283.               final double baseWeight) {
  284.         this(date, position, velocity, positionCovarianceMatrix, velocityCovarianceMatrix,
  285.              baseWeight, new ObservableSatellite(0));
  286.     }

  288.     /**
  289.      * Constructor with 2 smaller covariance matrices.
  290.      * <p>One 3x3 covariance matrix for position and one 3x3 covariance matrix for velocity.
  291.      * The fact that the covariance matrices are symmetric and positive definite is not checked.</p>
  292.      * <p>The measurement must be in the orbit propagation frame.</p>
  293.      * @param date date of the measurement
  294.      * @param position position
  295.      * @param velocity velocity
  296.      * @param positionCovarianceMatrix 3x3 covariance matrix of the position
  297.      * @param velocityCovarianceMatrix 3x3 covariance matrix of the velocity
  298.      * @param baseWeight base weight
  299.      * @param propagatorIndex index of the propagator related to this measurement
  300.      * @since 9.2
  301.      * @deprecated as of 9.3, replaced by {@link #PV(AbsoluteDate, Vector3D, Vector3D,
  302.      * double[][], double[][], double, ObservableSatellite)}
  303.      */
  304.     @Deprecated
  305.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  306.               final double[][] positionCovarianceMatrix, final double[][] velocityCovarianceMatrix,
  307.               final double baseWeight, final int propagatorIndex) {
  308.         this(date, position, velocity, positionCovarianceMatrix, velocityCovarianceMatrix,
  309.              baseWeight, new ObservableSatellite(propagatorIndex));
  310.     }

  312.     /**
  313.      * Constructor with 2 smaller covariance matrices.
  314.      * <p>One 3x3 covariance matrix for position and one 3x3 covariance matrix for velocity.
  315.      * The fact that the covariance matrices are symmetric and positive definite is not checked.</p>
  316.      * <p>The measurement must be in the orbit propagation frame.</p>
  317.      * @param date date of the measurement
  318.      * @param position position
  319.      * @param velocity velocity
  320.      * @param positionCovarianceMatrix 3x3 covariance matrix of the position
  321.      * @param velocityCovarianceMatrix 3x3 covariance matrix of the velocity
  322.      * @param baseWeight base weight
  323.      * @param satellite satellite related to this measurement
  324.      * @since 9.3
  325.      */
  326.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  327.               final double[][] positionCovarianceMatrix, final double[][] velocityCovarianceMatrix,
  328.               final double baseWeight, final ObservableSatellite satellite) {
  329.         this(date, position, velocity,
  330.              buildPvCovarianceMatrix(positionCovarianceMatrix, velocityCovarianceMatrix),
  331.              baseWeight, satellite);
  332.     }

  334.     /**
  335.      * Constructor with full covariance matrix but default index for propagator.
  336.      * <p>The fact that the covariance matrix is symmetric and positive definite is not checked.</p>
  337.      * <p>The measurement must be in the orbit propagation frame.</p>
  338.      * <p>This constructor uses 0 as the index of the propagator related
  339.      * to this measurement, thus being well suited for mono-satellite
  340.      * orbit determination.</p>
  341.      * @param date date of the measurement
  342.      * @param position position
  343.      * @param velocity velocity
  344.      * @param covarianceMatrix 6x6 covariance matrix of the PV measurement
  345.      * @param baseWeight base weight
  346.      * @since 9.2
  347.      * @deprecated as of 9.3, replaced by {@link #PV(AbsoluteDate, Vector3D, Vector3D,
  348.      * double[][], double, ObservableSatellite)}
  349.      */
  350.     @Deprecated
  351.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  352.               final double[][] covarianceMatrix, final double baseWeight) {
  353.         this(date, position, velocity, covarianceMatrix, baseWeight, new ObservableSatellite(0));
  354.     }

  356.     /** Constructor with full covariance matrix and all inputs.
  357.      * <p>The fact that the covariance matrix is symmetric and positive definite is not checked.</p>
  358.      * <p>The measurement must be in the orbit propagation frame.</p>
  359.      * @param date date of the measurement
  360.      * @param position position
  361.      * @param velocity velocity
  362.      * @param covarianceMatrix 6x6 covariance matrix of the PV measurement
  363.      * @param baseWeight base weight
  364.      * @param propagatorIndex index of the propagator related to this measurement
  365.      * @since 9.2
  366.      * @deprecated as of 9.3, replaced by {@link #PV(AbsoluteDate, Vector3D, Vector3D,
  367.      * double[][], double, ObservableSatellite)}
  368.      */
  369.     @Deprecated
  370.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  371.               final double[][] covarianceMatrix, final double baseWeight, final int propagatorIndex) {
  372.         this(date, position, velocity, covarianceMatrix, baseWeight, new ObservableSatellite(propagatorIndex));
  373.     }

  375.     /** Constructor with full covariance matrix and all inputs.
  376.      * <p>The fact that the covariance matrix is symmetric and positive definite is not checked.</p>
  377.      * <p>The measurement must be in the orbit propagation frame.</p>
  378.      * @param date date of the measurement
  379.      * @param position position
  380.      * @param velocity velocity
  381.      * @param covarianceMatrix 6x6 covariance matrix of the PV measurement
  382.      * @param baseWeight base weight
  383.      * @param satellite satellite related to this measurement
  384.      * @since 9.3
  385.      */
  386.     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
  387.               final double[][] covarianceMatrix, final double baseWeight, final ObservableSatellite satellite) {
  388.         super(date,
  389.               new double[] {
  390.                   position.getX(), position.getY(), position.getZ(),
  391.                   velocity.getX(), velocity.getY(), velocity.getZ()
  392.               }, extractSigmas(covarianceMatrix),
  393.               new double[] {
  394.                   baseWeight, baseWeight, baseWeight,
  395.                   baseWeight, baseWeight, baseWeight
  396.               }, Arrays.asList(satellite));
  397.         this.covarianceMatrix = covarianceMatrix;
  398.     }

  400.     /** Get the position.
  401.      * @return position
  402.      */
  403.     public Vector3D getPosition() {
  404.         final double[] pv = getObservedValue();
  405.         return new Vector3D(pv[0], pv[1], pv[2]);
  406.     }

  408.     /** Get the velocity.
  409.      * @return velocity
  410.      */
  411.     public Vector3D getVelocity() {
  412.         final double[] pv = getObservedValue();
  413.         return new Vector3D(pv[3], pv[4], pv[5]);
  414.     }

  416.     /** Get the covariance matrix.
  417.      * @return the covariance matrix
  418.      */
  419.     public double[][] getCovarianceMatrix() {
  420.         return covarianceMatrix;
  421.     }

  423.     /** Get the correlation coefficients matrix.
  424.      * <br>This is the 6x6 matrix M such that:</br>
  425.      * <br>Mij = Pij/(σi.σj)</br>
  426.      * <br>Where: <ul>
  427.      * <li> P is the covariance matrix
  428.      * <li> σi is the i-th standard deviation (σi² = Pii)
  429.      * </ul>
  430.      * @return the correlation coefficient matrix (6x6)
  431.      */
  432.     public double[][] getCorrelationCoefficientsMatrix() {

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

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

  440.         // Divide by the standard deviations
  441.         for (int i = 0; i < sigmas.length; i++) {
  442.             for (int j = 0; j < sigmas.length; j++) {
  443.                 corrCoefMatrix[i][j] = covarianceMatrix[i][j] / (sigmas[i] * sigmas[j]);
  444.             }
  445.         }
  446.         return corrCoefMatrix;
  447.     }

  449.     /** {@inheritDoc} */
  450.     @Override
  451.     protected EstimatedMeasurement<PV> theoreticalEvaluation(final int iteration, final int evaluation,
  452.                                                              final SpacecraftState[] states) {

  454.         // PV value
  455.         final ObservableSatellite      satellite = getSatellites().get(0);
  456.         final SpacecraftState          state     = states[satellite.getPropagatorIndex()];
  457.         final TimeStampedPVCoordinates pv        = state.getPVCoordinates();

  459.         // prepare the evaluation
  460.         final EstimatedMeasurement<PV> estimated =
  461.                         new EstimatedMeasurement<>(this, iteration, evaluation, states,
  462.                                                    new TimeStampedPVCoordinates[] {
  463.                                                        pv
  464.                                                    });

  466.         estimated.setEstimatedValue(new double[] {
  467.             pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
  468.             pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ()
  469.         });

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

  474.         return estimated;
  475.     }

  477.     /** Extract standard deviations from a 6x6 PV covariance matrix.
  478.      * Check the size of the PV covariance matrix first.
  479.      * @param pvCovarianceMatrix the 6x6 PV covariance matrix
  480.      * @return the standard deviations (6-sized vector), they are
  481.      * the square roots of the diagonal elements of the covariance matrix in input.
  482.      */
  483.     private static double[] extractSigmas(final double[][] pvCovarianceMatrix) {

  485.         // Check the size of the covariance matrix, should be 6x6
  486.         if (pvCovarianceMatrix.length != 6 || pvCovarianceMatrix[0].length != 6) {
  487.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
  488.                                       pvCovarianceMatrix.length, pvCovarianceMatrix[0],
  489.                                       6, 6);
  490.         }

  492.         // Extract the standard deviations (square roots of the diagonal elements)
  493.         final double[] sigmas = new double[6];
  494.         for (int i = 0; i < sigmas.length; i++) {
  495.             sigmas[i] = FastMath.sqrt(pvCovarianceMatrix[i][i]);
  496.         }
  497.         return sigmas;
  498.     }

  500.     /** Build a 6x6 PV covariance matrix from two 3x3 matrices (covariances in position and velocity).
  501.      * Check the size of the matrices first.
  502.      * @param positionCovarianceMatrix the 3x3 covariance matrix in position
  503.      * @param velocityCovarianceMatrix the 3x3 covariance matrix in velocity
  504.      * @return the 6x6 PV covariance matrix
  505.      */
  506.     private static double[][] buildPvCovarianceMatrix(final double[][] positionCovarianceMatrix,
  507.                                                       final double[][] velocityCovarianceMatrix) {
  508.         // Check the sizes of the matrices first
  509.         if (positionCovarianceMatrix.length != 3 || positionCovarianceMatrix[0].length != 3) {
  510.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
  511.                                       positionCovarianceMatrix.length, positionCovarianceMatrix[0],
  512.                                       3, 3);
  513.         }
  514.         if (velocityCovarianceMatrix.length != 3 || velocityCovarianceMatrix[0].length != 3) {
  515.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
  516.                                       velocityCovarianceMatrix.length, velocityCovarianceMatrix[0],
  517.                                       3, 3);
  518.         }

  520.         // Build the PV 6x6 covariance matrix
  521.         final double[][] pvCovarianceMatrix = new double[6][6];
  522.         for (int i = 0; i < 3; i++) {
  523.             for (int j = 0; j < 3; j++) {
  524.                 pvCovarianceMatrix[i][j]         = positionCovarianceMatrix[i][j];
  525.                 pvCovarianceMatrix[i + 3][j + 3] = velocityCovarianceMatrix[i][j];
  526.             }
  527.         }
  528.         return pvCovarianceMatrix;
  529.     }

  531.     /** Build a 6x6 PV covariance matrix from a 6-sized vector (position and velocity standard deviations).
  532.      * Check the size of the vector first.
  533.      * @param sigmaPV 6-sized vector with position standard deviations on the first 3 elements
  534.      * and velocity standard deviations on the last 3 elements
  535.      * @return the 6x6 PV covariance matrix
  536.      */
  537.     private static double[][] buildPvCovarianceMatrix(final double[] sigmaPV) {
  538.         // Check the size of the vector first
  539.         if (sigmaPV.length != 6) {
  540.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaPV.length, 6);

  542.         }

  544.         // Build the PV 6x6 covariance matrix
  545.         final double[][] pvCovarianceMatrix = new double[6][6];
  546.         for (int i = 0; i < sigmaPV.length; i++) {
  547.             pvCovarianceMatrix[i][i] =  sigmaPV[i] * sigmaPV[i];
  548.         }
  549.         return pvCovarianceMatrix;
  550.     }

  552.     /** Build a 6x6 PV covariance matrix from two 3-sized vectors (position and velocity standard deviations).
  553.      * Check the sizes of the vectors first.
  554.      * @param sigmaPosition standard deviations of the position (3-size vector)
  555.      * @param sigmaVelocity standard deviations of the velocity (3-size vector)
  556.      * @return the 6x6 PV covariance matrix
  557.      */
  558.     private static double[][] buildPvCovarianceMatrix(final double[] sigmaPosition,
  559.                                                       final double[] sigmaVelocity) {

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

  565.         }
  566.         if (sigmaVelocity.length != 3) {
  567.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaVelocity.length, 3);

  569.         }

  571.         // Build the PV 6x6 covariance matrix
  572.         final double[][] pvCovarianceMatrix = new double[6][6];
  573.         for (int i = 0; i < sigmaPosition.length; i++) {
  574.             pvCovarianceMatrix[i][i]         =  sigmaPosition[i] * sigmaPosition[i];
  575.             pvCovarianceMatrix[i + 3][i + 3] =  sigmaVelocity[i] * sigmaVelocity[i];
  576.         }
  577.         return pvCovarianceMatrix;
  578.     }
  579. }
Created with JaCoCo 0.8.2.201808211720