Position.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 only measurement.
  30.  * <p>
  31.  * For position-velocity measurement see {@link PV}.
  32.  * </p>
  33.  * @see PV
  34.  * @author Luc Maisonobe
  35.  * @since 9.3
  36.  */
  37. public class Position extends AbstractMeasurement<Position> {

  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.     };

  50.     /** Covariance matrix of the position only measurement (size 3x3). */
  51.     private final double[][] covarianceMatrix;

  53.     /** Constructor with one double for the standard deviation.
  54.      * <p>The double is the position's standard deviation, common to the 3 position's components.</p>
  55.      * <p>
  56.      * The measurement must be in the orbit propagation frame.
  57.      * </p>
  58.      * @param date date of the measurement
  59.      * @param position position
  60.      * @param sigmaPosition theoretical standard deviation on position components
  61.      * @param baseWeight base weight
  62.      * @param satellite satellite related to this measurement
  63.      * @since 9.3
  64.      */
  65.     public Position(final AbsoluteDate date, final Vector3D position,
  66.                     final double sigmaPosition, final double baseWeight,
  67.                     final ObservableSatellite satellite) {
  68.         this(date, position,
  69.              new double[] {
  70.                  sigmaPosition,
  71.                  sigmaPosition,
  72.                  sigmaPosition
  73.              }, baseWeight, satellite);
  74.     }

  76.     /** Constructor with one vector for the standard deviation.
  77.      * <p>The 3-sized vector represents the square root of the diagonal elements of the covariance matrix.</p>
  78.      * <p>The measurement must be in the orbit propagation frame.</p>
  79.      * @param date date of the measurement
  80.      * @param position position
  81.      * @param sigmaPosition 3-sized vector of the standard deviations of the position
  82.      * @param baseWeight base weight
  83.      * @param satellite satellite related to this measurement
  84.      * @since 9.3
  85.      */
  86.     public Position(final AbsoluteDate date, final Vector3D position,
  87.                     final double[] sigmaPosition, final double baseWeight, final ObservableSatellite satellite) {
  88.         this(date, position, buildPvCovarianceMatrix(sigmaPosition), baseWeight, satellite);
  89.     }

  91.     /** Constructor with full covariance matrix and all inputs.
  92.      * <p>The fact that the covariance matrix is symmetric and positive definite is not checked.</p>
  93.      * <p>The measurement must be in the orbit propagation frame.</p>
  94.      * @param date date of the measurement
  95.      * @param position position
  96.      * @param covarianceMatrix 3x3 covariance matrix of the position only measurement
  97.      * @param baseWeight base weight
  98.      * @param satellite satellite related to this measurement
  99.      * @since 9.3
  100.      */
  101.     public Position(final AbsoluteDate date, final Vector3D position,
  102.                     final double[][] covarianceMatrix, final double baseWeight,
  103.                     final ObservableSatellite satellite) {
  104.         super(date,
  105.               new double[] {
  106.                   position.getX(), position.getY(), position.getZ()
  107.               }, extractSigmas(covarianceMatrix),
  108.               new double[] {
  109.                   baseWeight, baseWeight, baseWeight
  110.               }, Arrays.asList(satellite));
  111.         this.covarianceMatrix = covarianceMatrix.clone();
  112.     }

  114.     /** Get the position.
  115.      * @return position
  116.      */
  117.     public Vector3D getPosition() {
  118.         final double[] pv = getObservedValue();
  119.         return new Vector3D(pv[0], pv[1], pv[2]);
  120.     }

  122.     /** Get the covariance matrix.
  123.      * @return the covariance matrix
  124.      */
  125.     public double[][] getCovarianceMatrix() {
  126.         return covarianceMatrix.clone();
  127.     }

  129.     /** Get the correlation coefficients matrix.
  130.      * <p>This is the 3x3 matrix M such that:
  131.      * <p>Mij = Pij/(σi.σj)
  132.      * <p>Where:
  133.      * <ul>
  134.      * <li>P is the covariance matrix
  135.      * <li>σi is the i-th standard deviation (σi² = Pii)
  136.      * </ul>
  137.      * @return the correlation coefficient matrix (3x3)
  138.      */
  139.     public double[][] getCorrelationCoefficientsMatrix() {

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

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

  147.         // Divide by the standard deviations
  148.         for (int i = 0; i < sigmas.length; i++) {
  149.             for (int j = 0; j < sigmas.length; j++) {
  150.                 corrCoefMatrix[i][j] = covarianceMatrix[i][j] / (sigmas[i] * sigmas[j]);
  151.             }
  152.         }
  153.         return corrCoefMatrix;
  154.     }

  156.     /** {@inheritDoc} */
  157.     @Override
  158.     protected EstimatedMeasurement<Position> theoreticalEvaluation(final int iteration, final int evaluation,
  159.                                                                    final SpacecraftState[] states) {

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

  164.         // prepare the evaluation
  165.         final EstimatedMeasurement<Position> estimated =
  166.                         new EstimatedMeasurement<>(this, iteration, evaluation, states,
  167.                                                    new TimeStampedPVCoordinates[] {
  168.                                                        pv
  169.                                                    });

  171.         estimated.setEstimatedValue(new double[] {
  172.             pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ()
  173.         });

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

  178.         return estimated;
  179.     }

  181.     /** Extract standard deviations from a 3x3 position covariance matrix.
  182.      * Check the size of the position covariance matrix first.
  183.      * @param pCovarianceMatrix the 3x" position covariance matrix
  184.      * @return the standard deviations (3-sized vector), they are
  185.      * the square roots of the diagonal elements of the covariance matrix in input.
  186.      */
  187.     private static double[] extractSigmas(final double[][] pCovarianceMatrix) {

  189.         // Check the size of the covariance matrix, should be 3x3
  190.         if (pCovarianceMatrix.length != 3 || pCovarianceMatrix[0].length != 3) {
  191.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
  192.                                       pCovarianceMatrix.length, pCovarianceMatrix[0],
  193.                                       3, 3);
  194.         }

  196.         // Extract the standard deviations (square roots of the diagonal elements)
  197.         final double[] sigmas = new double[3];
  198.         for (int i = 0; i < sigmas.length; i++) {
  199.             sigmas[i] = FastMath.sqrt(pCovarianceMatrix[i][i]);
  200.         }
  201.         return sigmas;
  202.     }

  204.     /** Build a 3x3 position covariance matrix from a 3-sized vector (position standard deviations).
  205.      * Check the size of the vector first.
  206.      * @param sigmaP 3-sized vector with position standard deviations
  207.      * @return the 3x3 position covariance matrix
  208.      */
  209.     private static double[][] buildPvCovarianceMatrix(final double[] sigmaP) {
  210.         // Check the size of the vector first
  211.         if (sigmaP.length != 3) {
  212.             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaP.length, 3);

  214.         }

  216.         // Build the 3x3 position covariance matrix
  217.         final double[][] pvCovarianceMatrix = new double[3][3];
  218.         for (int i = 0; i < sigmaP.length; i++) {
  219.             pvCovarianceMatrix[i][i] =  sigmaP[i] * sigmaP[i];
  220.         }
  221.         return pvCovarianceMatrix;
  222.     }

  224. }
Created with JaCoCo 0.8.6.202009150832