1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.sequential;
18
19 import org.hipparchus.analysis.UnivariateFunction;
20 import org.hipparchus.exception.LocalizedCoreFormats;
21 import org.hipparchus.geometry.euclidean.threed.Vector3D;
22 import org.hipparchus.linear.MatrixUtils;
23 import org.hipparchus.linear.RealMatrix;
24 import org.orekit.errors.OrekitException;
25 import org.orekit.frames.LOFType;
26 import org.orekit.frames.Transform;
27 import org.orekit.orbits.PositionAngle;
28 import org.orekit.propagation.SpacecraftState;
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67 public class UnivariateProcessNoise extends AbstractCovarianceMatrixProvider {
68
69
70 private final LOFType lofType;
71
72
73 private final PositionAngle positionAngle;
74
75
76 private final UnivariateFunction[] lofCartesianOrbitalParametersEvolution;
77
78
79 private final UnivariateFunction[] propagationParametersEvolution;
80
81
82
83
84
85
86
87
88 public UnivariateProcessNoise(final RealMatrix initialCovarianceMatrix,
89 final LOFType lofType,
90 final PositionAngle positionAngle,
91 final UnivariateFunction[] lofCartesianOrbitalParametersEvolution,
92 final UnivariateFunction[] propagationParametersEvolution) {
93 super(initialCovarianceMatrix);
94 this.lofType = lofType;
95 this.positionAngle = positionAngle;
96 this.lofCartesianOrbitalParametersEvolution = lofCartesianOrbitalParametersEvolution;
97 this.propagationParametersEvolution = propagationParametersEvolution;
98
99
100 if (lofCartesianOrbitalParametersEvolution.length != 6) {
101 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
102 lofCartesianOrbitalParametersEvolution, 6);
103 }
104 }
105
106
107
108
109 public LOFType getLofType() {
110 return lofType;
111 }
112
113
114
115
116 public PositionAngle getPositionAngle() {
117 return positionAngle;
118 }
119
120
121
122
123 public UnivariateFunction[] getLofCartesianOrbitalParametersEvolution() {
124 return lofCartesianOrbitalParametersEvolution;
125 }
126
127
128
129
130 public UnivariateFunction[] getPropagationParametersEvolution() {
131 return propagationParametersEvolution;
132 }
133
134
135 @Override
136 public RealMatrix getProcessNoiseMatrix(final SpacecraftState previous,
137 final SpacecraftState current) {
138
139
140 final RealMatrix inertialOrbitalProcessNoiseMatrix = getInertialOrbitalProcessNoiseMatrix(previous, current);
141
142 if (propagationParametersEvolution.length == 0) {
143
144
145 return inertialOrbitalProcessNoiseMatrix;
146
147 } else {
148
149
150 final RealMatrix propagationProcessNoiseMatrix = getPropagationProcessNoiseMatrix(previous, current);
151
152
153 final int orbitalMatrixSize = lofCartesianOrbitalParametersEvolution.length;
154 final int propagationMatrixSize = propagationParametersEvolution.length;
155 final RealMatrix processNoiseMatrix = MatrixUtils.createRealMatrix(orbitalMatrixSize + propagationMatrixSize,
156 orbitalMatrixSize + propagationMatrixSize);
157 processNoiseMatrix.setSubMatrix(inertialOrbitalProcessNoiseMatrix.getData(), 0, 0);
158 processNoiseMatrix.setSubMatrix(propagationProcessNoiseMatrix.getData(), orbitalMatrixSize, orbitalMatrixSize);
159 return processNoiseMatrix;
160
161 }
162 }
163
164
165
166
167
168
169 private RealMatrix getInertialOrbitalProcessNoiseMatrix(final SpacecraftState previous,
170 final SpacecraftState current) {
171
172
173 final double deltaT = current.getDate().durationFrom(previous.getDate());
174
175
176 final int lofOrbitalprocessNoiseLength = lofCartesianOrbitalParametersEvolution.length;
177 final double[] lofOrbitalProcessNoiseValues = new double[lofOrbitalprocessNoiseLength];
178
179
180
181 for (int i = 0; i < lofOrbitalprocessNoiseLength; i++) {
182 final double functionValue = lofCartesianOrbitalParametersEvolution[i].value(deltaT);
183 lofOrbitalProcessNoiseValues[i] = functionValue * functionValue;
184 }
185
186
187 final RealMatrix lofCartesianProcessNoiseMatrix = MatrixUtils.createRealDiagonalMatrix(lofOrbitalProcessNoiseValues);
188
189
190 final double[][] lofToInertialRotation = lofType.rotationFromInertial(current.getPVCoordinates()).
191 revert().getMatrix();
192
193
194 final RealMatrix jacLofToInertial = MatrixUtils.createRealMatrix(6, 6);
195 jacLofToInertial.setSubMatrix(lofToInertialRotation, 0, 0);
196 jacLofToInertial.setSubMatrix(lofToInertialRotation, 3, 3);
197
198
199 final Transform lofToInertial = lofType.transformFromInertial(current.getDate(), current.getPVCoordinates()).getInverse();
200 final Vector3D OM = lofToInertial.getRotationRate().negate();
201 final double[][] MOM = new double[3][3];
202 MOM[0][1] = -OM.getZ();
203 MOM[0][2] = OM.getY();
204 MOM[1][0] = OM.getZ();
205 MOM[1][2] = -OM.getX();
206 MOM[2][0] = -OM.getY();
207 MOM[2][1] = OM.getX();
208
209
210
211
212 final double[][] dYdC = new double[6][6];
213 current.getOrbit().getJacobianWrtCartesian(positionAngle, dYdC);
214 final RealMatrix jacParametersWrtCartesian = MatrixUtils.createRealMatrix(dYdC);
215
216
217 final RealMatrix jacobian = jacParametersWrtCartesian.multiply(jacLofToInertial);
218
219
220 final RealMatrix inertialOrbitalProcessNoiseMatrix = jacobian.
221 multiply(lofCartesianProcessNoiseMatrix).
222 multiply(jacobian.transpose());
223 return inertialOrbitalProcessNoiseMatrix;
224 }
225
226
227
228
229
230
231 private RealMatrix getPropagationProcessNoiseMatrix(final SpacecraftState previous,
232 final SpacecraftState current) {
233
234
235 final double deltaT = current.getDate().durationFrom(previous.getDate());
236
237
238 final int propagationProcessNoiseLength = propagationParametersEvolution.length;
239 final double[] propagationProcessNoiseValues = new double[propagationProcessNoiseLength];
240
241
242
243 for (int i = 0; i < propagationProcessNoiseLength; i++) {
244 final double functionValue = propagationParametersEvolution[i].value(deltaT);
245 propagationProcessNoiseValues[i] = functionValue * functionValue;
246 }
247
248
249 return MatrixUtils.createRealDiagonalMatrix(propagationProcessNoiseValues);
250 }
251
252 }