1 /* Copyright 2002-2024 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(new double[] {
242 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
243 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ()
244 });
245
246 return estimated;
247
248 }
249
250 /** {@inheritDoc} */
251 @Override
252 protected EstimatedMeasurement<PV> theoreticalEvaluation(final int iteration, final int evaluation,
253 final SpacecraftState[] states) {
254
255 // PV value
256 final TimeStampedPVCoordinates pv = states[0].getPVCoordinates();
257
258 // prepare the evaluation
259 final EstimatedMeasurement<PV> estimated =
260 new EstimatedMeasurement<>(this, iteration, evaluation, states,
261 new TimeStampedPVCoordinates[] {
262 pv
263 });
264
265 estimated.setEstimatedValue(new double[] {
266 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
267 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ()
268 });
269
270 // partial derivatives with respect to state
271 estimated.setStateDerivatives(0, IDENTITY);
272
273 return estimated;
274 }
275
276 /** Extract standard deviations from a 6x6 PV covariance matrix.
277 * Check the size of the PV covariance matrix first.
278 * @param pvCovarianceMatrix the 6x6 PV covariance matrix
279 * @return the standard deviations (6-sized vector), they are
280 * the square roots of the diagonal elements of the covariance matrix in input.
281 */
282 private static double[] extractSigmas(final double[][] pvCovarianceMatrix) {
283
284 // Check the size of the covariance matrix, should be 6x6
285 if (pvCovarianceMatrix.length != 6 || pvCovarianceMatrix[0].length != 6) {
286 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
287 pvCovarianceMatrix.length, pvCovarianceMatrix[0],
288 6, 6);
289 }
290
291 // Extract the standard deviations (square roots of the diagonal elements)
292 final double[] sigmas = new double[6];
293 for (int i = 0; i < sigmas.length; i++) {
294 sigmas[i] = FastMath.sqrt(pvCovarianceMatrix[i][i]);
295 }
296 return sigmas;
297 }
298
299 /** Build a 6x6 PV covariance matrix from two 3x3 matrices (covariances in position and velocity).
300 * Check the size of the matrices first.
301 * @param positionCovarianceMatrix the 3x3 covariance matrix in position
302 * @param velocityCovarianceMatrix the 3x3 covariance matrix in velocity
303 * @return the 6x6 PV covariance matrix
304 */
305 private static double[][] buildPvCovarianceMatrix(final double[][] positionCovarianceMatrix,
306 final double[][] velocityCovarianceMatrix) {
307 // Check the sizes of the matrices first
308 if (positionCovarianceMatrix.length != 3 || positionCovarianceMatrix[0].length != 3) {
309 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
310 positionCovarianceMatrix.length, positionCovarianceMatrix[0],
311 3, 3);
312 }
313 if (velocityCovarianceMatrix.length != 3 || velocityCovarianceMatrix[0].length != 3) {
314 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
315 velocityCovarianceMatrix.length, velocityCovarianceMatrix[0],
316 3, 3);
317 }
318
319 // Build the PV 6x6 covariance matrix
320 final double[][] pvCovarianceMatrix = new double[6][6];
321 for (int i = 0; i < 3; i++) {
322 for (int j = 0; j < 3; j++) {
323 pvCovarianceMatrix[i][j] = positionCovarianceMatrix[i][j];
324 pvCovarianceMatrix[i + 3][j + 3] = velocityCovarianceMatrix[i][j];
325 }
326 }
327 return pvCovarianceMatrix;
328 }
329
330 /** Build a 6x6 PV covariance matrix from a 6-sized vector (position and velocity standard deviations).
331 * Check the size of the vector first.
332 * @param sigmaPV 6-sized vector with position standard deviations on the first 3 elements
333 * and velocity standard deviations on the last 3 elements
334 * @return the 6x6 PV covariance matrix
335 */
336 private static double[][] buildPvCovarianceMatrix(final double[] sigmaPV) {
337 // Check the size of the vector first
338 if (sigmaPV.length != 6) {
339 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaPV.length, 6);
340
341 }
342
343 // Build the PV 6x6 covariance matrix
344 final double[][] pvCovarianceMatrix = new double[6][6];
345 for (int i = 0; i < sigmaPV.length; i++) {
346 pvCovarianceMatrix[i][i] = sigmaPV[i] * sigmaPV[i];
347 }
348 return pvCovarianceMatrix;
349 }
350
351 /** Build a 6x6 PV covariance matrix from two 3-sized vectors (position and velocity standard deviations).
352 * Check the sizes of the vectors first.
353 * @param sigmaPosition standard deviations of the position (3-size vector)
354 * @param sigmaVelocity standard deviations of the velocity (3-size vector)
355 * @return the 6x6 PV covariance matrix
356 */
357 private static double[][] buildPvCovarianceMatrix(final double[] sigmaPosition,
358 final double[] sigmaVelocity) {
359
360 // Check the sizes of the vectors first
361 if (sigmaPosition.length != 3) {
362 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaPosition.length, 3);
363
364 }
365 if (sigmaVelocity.length != 3) {
366 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaVelocity.length, 3);
367
368 }
369
370 // Build the PV 6x6 covariance matrix
371 final double[][] pvCovarianceMatrix = new double[6][6];
372 for (int i = 0; i < sigmaPosition.length; i++) {
373 pvCovarianceMatrix[i][i] = sigmaPosition[i] * sigmaPosition[i];
374 pvCovarianceMatrix[i + 3][i + 3] = sigmaVelocity[i] * sigmaVelocity[i];
375 }
376 return pvCovarianceMatrix;
377 }
378 }