1   /* Copyright 2002-2025 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;
18  
19  import java.util.Collections;
20  
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;
28  
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> {
38  
39      /** Type of the measurement. */
40      public static final String MEASUREMENT_TYPE = "PV";
41  
42      /** Identity matrix, for states derivatives. */
43      private static final double[][] IDENTITY = new double[][] {
44          {
45              1, 0, 0, 0, 0, 0
46          }, {
47              0, 1, 0, 0, 0, 0
48          }, {
49              0, 0, 1, 0, 0, 0
50          }, {
51              0, 0, 0, 1, 0, 0
52          }, {
53              0, 0, 0, 0, 1, 0
54          }, {
55              0, 0, 0, 0, 0, 1
56          }
57      };
58  
59      /** Covariance matrix of the PV measurement (size 6x6). */
60      private final double[][] covarianceMatrix;
61  
62      /** Constructor with two double for the standard deviations.
63       * <p>The first double is the position's standard deviation, common to the 3 position's components.
64       * The second double is the position's standard deviation, common to the 3 position's components.</p>
65       * <p>
66       * The measurement must be in the orbit propagation frame.
67       * </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       * @param satellite satellite related to this measurement
75       * @since 9.3
76       */
77      public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
78                final double sigmaPosition, final double sigmaVelocity, final double baseWeight,
79                final ObservableSatellite satellite) {
80          this(date, position, velocity,
81               new double[] {
82                   sigmaPosition,
83                   sigmaPosition,
84                   sigmaPosition,
85                   sigmaVelocity,
86                   sigmaVelocity,
87                   sigmaVelocity
88               }, baseWeight, satellite);
89      }
90  
91      /** Constructor with two vectors for the standard deviations.
92       * <p>One 3-sized vectors for position standard deviations.
93       * One 3-sized vectors for velocity standard deviations.
94       * The 3-sized vectors are the square root of the diagonal elements of the covariance matrix.</p>
95       * <p>The measurement must be in the orbit propagation frame.</p>
96       * @param date date of the measurement
97       * @param position position
98       * @param velocity velocity
99       * @param sigmaPosition 3-sized vector of the standard deviations of the position
100      * @param sigmaVelocity 3-sized vector of the standard deviations of the velocity
101      * @param baseWeight base weight
102      * @param satellite satellite related to this measurement
103      * @since 9.3
104      */
105     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
106               final double[] sigmaPosition, final double[] sigmaVelocity,
107               final double baseWeight, final ObservableSatellite satellite) {
108         this(date, position, velocity,
109              buildPvCovarianceMatrix(sigmaPosition, sigmaVelocity),
110              baseWeight, satellite);
111     }
112 
113     /** Constructor with one vector for the standard deviations.
114      * <p>The 6-sized vector is the square root of the diagonal elements of the covariance matrix.</p>
115      * <p>The measurement must be in the orbit propagation frame.</p>
116      * @param date date of the measurement
117      * @param position position
118      * @param velocity velocity
119      * @param sigmaPV 6-sized vector of the standard deviations
120      * @param baseWeight base weight
121      * @param satellite satellite related to this measurement
122      * @since 9.3
123      */
124     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
125               final double[] sigmaPV, final double baseWeight, final ObservableSatellite satellite) {
126         this(date, position, velocity, buildPvCovarianceMatrix(sigmaPV), baseWeight, satellite);
127     }
128 
129     /**
130      * Constructor with 2 smaller covariance matrices.
131      * <p>One 3x3 covariance matrix for position and one 3x3 covariance matrix for velocity.
132      * The fact that the covariance matrices are symmetric and positive definite is not checked.</p>
133      * <p>The measurement must be in the orbit propagation frame.</p>
134      * @param date date of the measurement
135      * @param position position
136      * @param velocity velocity
137      * @param positionCovarianceMatrix 3x3 covariance matrix of the position
138      * @param velocityCovarianceMatrix 3x3 covariance matrix of the velocity
139      * @param baseWeight base weight
140      * @param satellite satellite related to this measurement
141      * @since 9.3
142      */
143     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
144               final double[][] positionCovarianceMatrix, final double[][] velocityCovarianceMatrix,
145               final double baseWeight, final ObservableSatellite satellite) {
146         this(date, position, velocity,
147              buildPvCovarianceMatrix(positionCovarianceMatrix, velocityCovarianceMatrix),
148              baseWeight, satellite);
149     }
150 
151     /** Constructor with full covariance matrix and all inputs.
152      * <p>The fact that the covariance matrix is symmetric and positive definite is not checked.</p>
153      * <p>The measurement must be in the orbit propagation frame.</p>
154      * @param date date of the measurement
155      * @param position position
156      * @param velocity velocity
157      * @param covarianceMatrix 6x6 covariance matrix of the PV measurement
158      * @param baseWeight base weight
159      * @param satellite satellite related to this measurement
160      * @since 9.3
161      */
162     public PV(final AbsoluteDate date, final Vector3D position, final Vector3D velocity,
163               final double[][] covarianceMatrix, final double baseWeight, final ObservableSatellite satellite) {
164         super(date,
165               new double[] {
166                   position.getX(), position.getY(), position.getZ(),
167                   velocity.getX(), velocity.getY(), velocity.getZ()
168               }, extractSigmas(covarianceMatrix),
169               new double[] {
170                   baseWeight, baseWeight, baseWeight,
171                   baseWeight, baseWeight, baseWeight
172               }, Collections.singletonList(satellite));
173         this.covarianceMatrix = covarianceMatrix.clone();
174     }
175 
176     /** Get the position.
177      * @return position
178      */
179     public Vector3D getPosition() {
180         final double[] pv = getObservedValue();
181         return new Vector3D(pv[0], pv[1], pv[2]);
182     }
183 
184     /** Get the velocity.
185      * @return velocity
186      */
187     public Vector3D getVelocity() {
188         final double[] pv = getObservedValue();
189         return new Vector3D(pv[3], pv[4], pv[5]);
190     }
191 
192     /** Get the covariance matrix.
193      * @return the covariance matrix
194      */
195     public double[][] getCovarianceMatrix() {
196         return covarianceMatrix.clone();
197     }
198 
199     /** Get the correlation coefficients matrix.
200      * <p>This is the 6x6 matrix M such that:
201      * <p>Mij = Pij/(σi.σj)
202      * <p>Where:
203      * <ul>
204      * <li>P is the covariance matrix
205      * <li>σi is the i-th standard deviation (σi² = Pii)
206      * </ul>
207      * @return the correlation coefficient matrix (6x6)
208      */
209     public double[][] getCorrelationCoefficientsMatrix() {
210 
211         // Get the standard deviations
212         final double[] sigmas = getTheoreticalStandardDeviation();
213 
214         // Initialize the correlation coefficients matric to the covariance matrix
215         final double[][] corrCoefMatrix = new double[sigmas.length][sigmas.length];
216 
217         // Divide by the standard deviations
218         for (int i = 0; i < sigmas.length; i++) {
219             for (int j = 0; j < sigmas.length; j++) {
220                 corrCoefMatrix[i][j] = covarianceMatrix[i][j] / (sigmas[i] * sigmas[j]);
221             }
222         }
223         return corrCoefMatrix;
224     }
225 
226     /** {@inheritDoc} */
227     @Override
228     protected EstimatedMeasurementBase<PV> theoreticalEvaluationWithoutDerivatives(final int iteration, final int evaluation,
229                                                                                    final SpacecraftState[] states) {
230 
231         // PV value
232         final TimeStampedPVCoordinates pv = states[0].getPVCoordinates();
233 
234         // prepare the evaluation
235         final EstimatedMeasurementBase<PV> estimated =
236                         new EstimatedMeasurementBase<>(this, iteration, evaluation, states,
237                                                        new TimeStampedPVCoordinates[] {
238                                                            pv
239                                                        });
240 
241         estimated.setEstimatedValue(pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
242                 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ());
243 
244         return estimated;
245 
246     }
247 
248     /** {@inheritDoc} */
249     @Override
250     protected EstimatedMeasurement<PV> theoreticalEvaluation(final int iteration, final int evaluation,
251                                                              final SpacecraftState[] states) {
252         final EstimatedMeasurement<PV> estimated = new EstimatedMeasurement<>(theoreticalEvaluationWithoutDerivatives(iteration, evaluation, states));
253 
254         // partial derivatives with respect to state
255         estimated.setStateDerivatives(0, IDENTITY);
256 
257         return estimated;
258     }
259 
260     /** Extract standard deviations from a 6x6 PV covariance matrix.
261      * Check the size of the PV covariance matrix first.
262      * @param pvCovarianceMatrix the 6x6 PV covariance matrix
263      * @return the standard deviations (6-sized vector), they are
264      * the square roots of the diagonal elements of the covariance matrix in input.
265      */
266     private static double[] extractSigmas(final double[][] pvCovarianceMatrix) {
267 
268         // Check the size of the covariance matrix, should be 6x6
269         if (pvCovarianceMatrix.length != 6 || pvCovarianceMatrix[0].length != 6) {
270             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
271                                       pvCovarianceMatrix.length, pvCovarianceMatrix[0],
272                                       6, 6);
273         }
274 
275         // Extract the standard deviations (square roots of the diagonal elements)
276         final double[] sigmas = new double[6];
277         for (int i = 0; i < sigmas.length; i++) {
278             sigmas[i] = FastMath.sqrt(pvCovarianceMatrix[i][i]);
279         }
280         return sigmas;
281     }
282 
283     /** Build a 6x6 PV covariance matrix from two 3x3 matrices (covariances in position and velocity).
284      * Check the size of the matrices first.
285      * @param positionCovarianceMatrix the 3x3 covariance matrix in position
286      * @param velocityCovarianceMatrix the 3x3 covariance matrix in velocity
287      * @return the 6x6 PV covariance matrix
288      */
289     private static double[][] buildPvCovarianceMatrix(final double[][] positionCovarianceMatrix,
290                                                       final double[][] velocityCovarianceMatrix) {
291         // Check the sizes of the matrices first
292         if (positionCovarianceMatrix.length != 3 || positionCovarianceMatrix[0].length != 3) {
293             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
294                                       positionCovarianceMatrix.length, positionCovarianceMatrix[0],
295                                       3, 3);
296         }
297         if (velocityCovarianceMatrix.length != 3 || velocityCovarianceMatrix[0].length != 3) {
298             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
299                                       velocityCovarianceMatrix.length, velocityCovarianceMatrix[0],
300                                       3, 3);
301         }
302 
303         // Build the PV 6x6 covariance matrix
304         final double[][] pvCovarianceMatrix = new double[6][6];
305         for (int i = 0; i < 3; i++) {
306             for (int j = 0; j < 3; j++) {
307                 pvCovarianceMatrix[i][j]         = positionCovarianceMatrix[i][j];
308                 pvCovarianceMatrix[i + 3][j + 3] = velocityCovarianceMatrix[i][j];
309             }
310         }
311         return pvCovarianceMatrix;
312     }
313 
314     /** Build a 6x6 PV covariance matrix from a 6-sized vector (position and velocity standard deviations).
315      * Check the size of the vector first.
316      * @param sigmaPV 6-sized vector with position standard deviations on the first 3 elements
317      * and velocity standard deviations on the last 3 elements
318      * @return the 6x6 PV covariance matrix
319      */
320     private static double[][] buildPvCovarianceMatrix(final double[] sigmaPV) {
321         // Check the size of the vector first
322         if (sigmaPV.length != 6) {
323             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaPV.length, 6);
324 
325         }
326 
327         // Build the PV 6x6 covariance matrix
328         final double[][] pvCovarianceMatrix = new double[6][6];
329         for (int i = 0; i < sigmaPV.length; i++) {
330             pvCovarianceMatrix[i][i] =  sigmaPV[i] * sigmaPV[i];
331         }
332         return pvCovarianceMatrix;
333     }
334 
335     /** Build a 6x6 PV covariance matrix from two 3-sized vectors (position and velocity standard deviations).
336      * Check the sizes of the vectors first.
337      * @param sigmaPosition standard deviations of the position (3-size vector)
338      * @param sigmaVelocity standard deviations of the velocity (3-size vector)
339      * @return the 6x6 PV covariance matrix
340      */
341     private static double[][] buildPvCovarianceMatrix(final double[] sigmaPosition,
342                                                       final double[] sigmaVelocity) {
343 
344         // Check the sizes of the vectors first
345         if (sigmaPosition.length != 3) {
346             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaPosition.length, 3);
347 
348         }
349         if (sigmaVelocity.length != 3) {
350             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaVelocity.length, 3);
351 
352         }
353 
354         // Build the PV 6x6 covariance matrix
355         final double[][] pvCovarianceMatrix = new double[6][6];
356         for (int i = 0; i < sigmaPosition.length; i++) {
357             pvCovarianceMatrix[i][i]         =  sigmaPosition[i] * sigmaPosition[i];
358             pvCovarianceMatrix[i + 3][i + 3] =  sigmaVelocity[i] * sigmaVelocity[i];
359         }
360         return pvCovarianceMatrix;
361     }
362 }