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;
18  
19  import java.util.Arrays;
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      /** 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      };
55  
56      /** Covariance matrix of the PV measurement (size 6x6). */
57      private final double[][] covarianceMatrix;
58  
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      }
82  
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     }
106 
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     }
135 
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     }
159 
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     }
183 
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     }
205 
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     }
226 
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     }
245 
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     }
261 
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     }
287 
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     }
311 
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     }
333 
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     }
355 
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     }
374 
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     }
399 
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     }
407 
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     }
415 
416     /** Get the covariance matrix.
417      * @return the covariance matrix
418      */
419     public double[][] getCovarianceMatrix() {
420         return covarianceMatrix;
421     }
422 
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() {
433 
434         // Get the standard deviations
435         final double[] sigmas = getTheoreticalStandardDeviation();
436 
437         // Initialize the correlation coefficients matric to the covariance matrix
438         final double[][] corrCoefMatrix = new double[sigmas.length][sigmas.length];
439 
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     }
448 
449     /** {@inheritDoc} */
450     @Override
451     protected EstimatedMeasurement<PV> theoreticalEvaluation(final int iteration, final int evaluation,
452                                                              final SpacecraftState[] states) {
453 
454         // PV value
455         final ObservableSatellite      satellite = getSatellites().get(0);
456         final SpacecraftState          state     = states[satellite.getPropagatorIndex()];
457         final TimeStampedPVCoordinates pv        = state.getPVCoordinates();
458 
459         // prepare the evaluation
460         final EstimatedMeasurement<PV> estimated =
461                         new EstimatedMeasurement<>(this, iteration, evaluation, states,
462                                                    new TimeStampedPVCoordinates[] {
463                                                        pv
464                                                    });
465 
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         });
470 
471         // partial derivatives with respect to state
472         estimated.setStateDerivatives(0, IDENTITY);
473 
474         return estimated;
475     }
476 
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) {
484 
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         }
491 
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     }
499 
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         }
519 
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     }
530 
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);
541 
542         }
543 
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     }
551 
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) {
560 
561         // Check the sizes of the vectors first
562         if (sigmaPosition.length != 3) {
563             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaPosition.length, 3);
564 
565         }
566         if (sigmaVelocity.length != 3) {
567             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaVelocity.length, 3);
568 
569         }
570 
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 }