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 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> {
38
39 /** Type of the measurement. */
40 public static final String MEASUREMENT_TYPE = "Position";
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 };
52
53 /** Covariance matrix of the position only measurement (size 3x3). */
54 private final double[][] covarianceMatrix;
55
56 /** Constructor with one double for the standard deviation.
57 * <p>The double is the position's standard deviation, common to the 3 position's components.</p>
58 * <p>
59 * The measurement must be in the orbit propagation frame.
60 * </p>
61 * @param date date of the measurement
62 * @param position position
63 * @param sigmaPosition theoretical standard deviation on position components
64 * @param baseWeight base weight
65 * @param satellite satellite related to this measurement
66 * @since 9.3
67 */
68 public Position(final AbsoluteDate date, final Vector3D position,
69 final double sigmaPosition, final double baseWeight,
70 final ObservableSatellite satellite) {
71 this(date, position,
72 new double[] {
73 sigmaPosition,
74 sigmaPosition,
75 sigmaPosition
76 }, baseWeight, satellite);
77 }
78
79 /** Constructor with one vector for the standard deviation.
80 * <p>The 3-sized vector represents the square root of the diagonal elements of the covariance matrix.</p>
81 * <p>The measurement must be in the orbit propagation frame.</p>
82 * @param date date of the measurement
83 * @param position position
84 * @param sigmaPosition 3-sized vector of the standard deviations of the position
85 * @param baseWeight base weight
86 * @param satellite satellite related to this measurement
87 * @since 9.3
88 */
89 public Position(final AbsoluteDate date, final Vector3D position,
90 final double[] sigmaPosition, final double baseWeight, final ObservableSatellite satellite) {
91 this(date, position, buildPvCovarianceMatrix(sigmaPosition), baseWeight, satellite);
92 }
93
94 /** Constructor with full covariance matrix and all inputs.
95 * <p>The fact that the covariance matrix is symmetric and positive definite is not checked.</p>
96 * <p>The measurement must be in the orbit propagation frame.</p>
97 * @param date date of the measurement
98 * @param position position
99 * @param covarianceMatrix 3x3 covariance matrix of the position only measurement
100 * @param baseWeight base weight
101 * @param satellite satellite related to this measurement
102 * @since 9.3
103 */
104 public Position(final AbsoluteDate date, final Vector3D position,
105 final double[][] covarianceMatrix, final double baseWeight,
106 final ObservableSatellite satellite) {
107 super(date,
108 new double[] {
109 position.getX(), position.getY(), position.getZ()
110 }, extractSigmas(covarianceMatrix),
111 new double[] {
112 baseWeight, baseWeight, baseWeight
113 }, Collections.singletonList(satellite));
114 this.covarianceMatrix = covarianceMatrix.clone();
115 }
116
117 /** Get the position.
118 * @return position
119 */
120 public Vector3D getPosition() {
121 final double[] pv = getObservedValue();
122 return new Vector3D(pv[0], pv[1], pv[2]);
123 }
124
125 /** Get the covariance matrix.
126 * @return the covariance matrix
127 */
128 public double[][] getCovarianceMatrix() {
129 return covarianceMatrix.clone();
130 }
131
132 /** Get the correlation coefficients matrix.
133 * <p>This is the 3x3 matrix M such that:
134 * <p>Mij = Pij/(σi.σj)
135 * <p>Where:
136 * <ul>
137 * <li>P is the covariance matrix
138 * <li>σi is the i-th standard deviation (σi² = Pii)
139 * </ul>
140 * @return the correlation coefficient matrix (3x3)
141 */
142 public double[][] getCorrelationCoefficientsMatrix() {
143
144 // Get the standard deviations
145 final double[] sigmas = getTheoreticalStandardDeviation();
146
147 // Initialize the correlation coefficients matric to the covariance matrix
148 final double[][] corrCoefMatrix = new double[sigmas.length][sigmas.length];
149
150 // Divide by the standard deviations
151 for (int i = 0; i < sigmas.length; i++) {
152 for (int j = 0; j < sigmas.length; j++) {
153 corrCoefMatrix[i][j] = covarianceMatrix[i][j] / (sigmas[i] * sigmas[j]);
154 }
155 }
156 return corrCoefMatrix;
157 }
158
159 /** {@inheritDoc} */
160 @Override
161 protected EstimatedMeasurementBase<Position> theoreticalEvaluationWithoutDerivatives(final int iteration, final int evaluation,
162 final SpacecraftState[] states) {
163
164 // PV value
165 final TimeStampedPVCoordinates pv = states[0].getPVCoordinates();
166
167 // prepare the evaluation
168 final EstimatedMeasurementBase<Position> estimated =
169 new EstimatedMeasurementBase<>(this, iteration, evaluation, states,
170 new TimeStampedPVCoordinates[] {
171 pv
172 });
173
174 estimated.setEstimatedValue(new double[] {
175 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ()
176 });
177
178 return estimated;
179
180 }
181
182 /** {@inheritDoc} */
183 @Override
184 protected EstimatedMeasurement<Position> theoreticalEvaluation(final int iteration, final int evaluation,
185 final SpacecraftState[] states) {
186
187 // PV value
188 final TimeStampedPVCoordinates pv = states[0].getPVCoordinates();
189
190 // prepare the evaluation
191 final EstimatedMeasurement<Position> estimated =
192 new EstimatedMeasurement<>(this, iteration, evaluation, states,
193 new TimeStampedPVCoordinates[] {
194 pv
195 });
196
197 estimated.setEstimatedValue(new double[] {
198 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ()
199 });
200
201 // partial derivatives with respect to state
202 estimated.setStateDerivatives(0, IDENTITY);
203
204 return estimated;
205 }
206
207 /** Extract standard deviations from a 3x3 position covariance matrix.
208 * Check the size of the position covariance matrix first.
209 * @param pCovarianceMatrix the 3x" position covariance matrix
210 * @return the standard deviations (3-sized vector), they are
211 * the square roots of the diagonal elements of the covariance matrix in input.
212 */
213 private static double[] extractSigmas(final double[][] pCovarianceMatrix) {
214
215 // Check the size of the covariance matrix, should be 3x3
216 if (pCovarianceMatrix.length != 3 || pCovarianceMatrix[0].length != 3) {
217 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
218 pCovarianceMatrix.length, pCovarianceMatrix[0],
219 3, 3);
220 }
221
222 // Extract the standard deviations (square roots of the diagonal elements)
223 final double[] sigmas = new double[3];
224 for (int i = 0; i < sigmas.length; i++) {
225 sigmas[i] = FastMath.sqrt(pCovarianceMatrix[i][i]);
226 }
227 return sigmas;
228 }
229
230 /** Build a 3x3 position covariance matrix from a 3-sized vector (position standard deviations).
231 * Check the size of the vector first.
232 * @param sigmaP 3-sized vector with position standard deviations
233 * @return the 3x3 position covariance matrix
234 */
235 private static double[][] buildPvCovarianceMatrix(final double[] sigmaP) {
236 // Check the size of the vector first
237 if (sigmaP.length != 3) {
238 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, sigmaP.length, 3);
239
240 }
241
242 // Build the 3x3 position covariance matrix
243 final double[][] pvCovarianceMatrix = new double[3][3];
244 for (int i = 0; i < sigmaP.length; i++) {
245 pvCovarianceMatrix[i][i] = sigmaP[i] * sigmaP[i];
246 }
247 return pvCovarianceMatrix;
248 }
249
250 }