1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.measurements;
18
19 import org.hipparchus.Field;
20 import org.hipparchus.analysis.differentiation.Gradient;
21 import org.hipparchus.analysis.differentiation.GradientField;
22 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
23 import org.hipparchus.geometry.euclidean.threed.Rotation;
24 import org.hipparchus.geometry.euclidean.threed.Vector3D;
25 import org.orekit.frames.FieldTransform;
26 import org.orekit.frames.Transform;
27 import org.orekit.propagation.SpacecraftState;
28 import org.orekit.time.AbsoluteDate;
29 import org.orekit.time.FieldAbsoluteDate;
30 import org.orekit.utils.AngularCoordinates;
31 import org.orekit.utils.Constants;
32 import org.orekit.utils.PVCoordinates;
33 import org.orekit.utils.ParameterDriver;
34 import org.orekit.utils.TimeStampedFieldPVCoordinates;
35 import org.orekit.utils.TimeStampedPVCoordinates;
36
37 import java.util.Arrays;
38 import java.util.HashMap;
39 import java.util.Map;
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 public class TurnAroundRangeAnalytic extends TurnAroundRange {
67
68
69 public static final String MEASUREMENT_TYPE = "TurnAroundRangeAnalytic";
70
71
72
73
74 public TurnAroundRangeAnalytic(final TurnAroundRange turnAroundRange) {
75 super(turnAroundRange.getPrimaryStation(), turnAroundRange.getSecondaryStation(),
76 turnAroundRange.getDate(), turnAroundRange.getObservedValue()[0],
77 turnAroundRange.getTheoreticalStandardDeviation()[0],
78 turnAroundRange.getBaseWeight()[0],
79 new ObservableSatellite(0));
80 }
81
82
83
84
85
86
87
88
89
90
91
92
93 protected EstimatedMeasurement<TurnAroundRange> theoreticalEvaluationAnalytic(final int iteration, final int evaluation,
94 final SpacecraftState initialState,
95 final SpacecraftState state) {
96
97
98 final GroundStation primaryGroundStation = this.getPrimaryStation();
99 final GroundStation secondaryGroundStation = this.getSecondaryStation();
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122 final AbsoluteDate measurementDate = this.getDate();
123 final Transform primaryTopoToInert =
124 primaryGroundStation.getOffsetToInertial(state.getFrame(), measurementDate, false);
125 final TimeStampedPVCoordinates primaryArrival =
126 primaryTopoToInert.transformPVCoordinates(new TimeStampedPVCoordinates(measurementDate,
127 PVCoordinates.ZERO));
128
129
130 final double tMd = signalTimeOfFlightAdjustableEmitter(state.getPVCoordinates(), primaryArrival.getPosition(),
131 measurementDate, state.getFrame());
132
133
134
135 final double delta = getDate().durationFrom(state.getDate());
136
137
138 final SpacecraftState transitStateLeg2 = state.shiftedBy(delta - tMd);
139 final AbsoluteDate transitDateLeg2 = transitStateLeg2.getDate();
140
141
142 final Transform secondaryTopoToInertTransitLeg2 =
143 secondaryGroundStation.getOffsetToInertial(state.getFrame(), transitDateLeg2, false);
144 final TimeStampedPVCoordinates QsecondaryTransitLeg2PV = secondaryTopoToInertTransitLeg2.
145 transformPVCoordinates(new TimeStampedPVCoordinates(transitDateLeg2, PVCoordinates.ZERO));
146
147
148 final double tSu = signalTimeOfFlightAdjustableEmitter(QsecondaryTransitLeg2PV,
149 transitStateLeg2.getPosition(),
150 transitDateLeg2,
151 state.getFrame());
152
153
154 final double t2 = tMd + tSu;
155
156
157
158
159
160
161 final AbsoluteDate secondaryStationArrivalDate = measurementDate.shiftedBy(-t2);
162
163
164 final Transform secondaryTopoToInertArrivalDate =
165 secondaryGroundStation.getOffsetToInertial(state.getFrame(), secondaryStationArrivalDate, false);
166 final TimeStampedPVCoordinates secondaryRebound =
167 secondaryTopoToInertArrivalDate.transformPVCoordinates(new TimeStampedPVCoordinates(secondaryStationArrivalDate,
168 PVCoordinates.ZERO));
169
170
171 final double tSd = signalTimeOfFlightAdjustableEmitter(transitStateLeg2.getPVCoordinates(), secondaryRebound.getPosition(),
172 secondaryStationArrivalDate, state.getFrame());
173
174
175 final SpacecraftState transitStateLeg1 = state.shiftedBy(delta -tMd -tSu -tSd);
176 final AbsoluteDate transitDateLeg1 = transitStateLeg1.getDate();
177
178
179 final Transform primaryTopoToInertTransitLeg1 =
180 primaryGroundStation.getOffsetToInertial(state.getFrame(), transitDateLeg1, false);
181 final TimeStampedPVCoordinates QPrimaryTransitLeg1PV = primaryTopoToInertTransitLeg1.
182 transformPVCoordinates(new TimeStampedPVCoordinates(transitDateLeg1, PVCoordinates.ZERO));
183
184
185 final double tMu = signalTimeOfFlightAdjustableEmitter(QPrimaryTransitLeg1PV,
186 transitStateLeg1.getPosition(),
187 transitDateLeg1,
188 state.getFrame());
189 final AbsoluteDate emissionDate = transitDateLeg1.shiftedBy(-tMu);
190 final TimeStampedPVCoordinates primaryDeparture =
191 primaryTopoToInertTransitLeg1.shiftedBy(emissionDate.durationFrom(primaryTopoToInertTransitLeg1.getDate())).
192 transformPVCoordinates(new TimeStampedPVCoordinates(emissionDate, PVCoordinates.ZERO));
193
194 final double t1 = tSd + tMu;
195
196
197
198
199
200
201
202
203
204
205
206
207
208 final EstimatedMeasurement<TurnAroundRange> estimated =
209 new EstimatedMeasurement<>(this, iteration, evaluation,
210 new SpacecraftState[] {
211 transitStateLeg2.shiftedBy(-tSu)
212 }, new TimeStampedPVCoordinates[] {
213 primaryDeparture,
214 transitStateLeg1.getPVCoordinates(),
215 secondaryRebound,
216 transitStateLeg2.getPVCoordinates(),
217 primaryArrival
218 });
219
220
221 final double cOver2 = 0.5 * Constants.SPEED_OF_LIGHT;
222 final double tau = t1 + t2;
223 estimated.setEstimatedValue(tau * cOver2);
224
225
226
227
228
229
230
231
232
233 final Vector3D vel = state.getPVCoordinates().getVelocity();
234 final Transform FMt = primaryGroundStation.getOffsetToInertial(state.getFrame(), getDate(), false);
235 final PVCoordinates QMt = FMt.transformPVCoordinates(PVCoordinates.ZERO);
236 final Vector3D QMt_V = QMt.getVelocity();
237 final Vector3D pos2 = transitStateLeg2.getPosition();
238 final Vector3D P2_QMt = QMt.getPosition().subtract(pos2);
239 final double dMDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tMd -
240 Vector3D.dotProduct(P2_QMt, vel);
241
242
243 final double dtMddPx = -P2_QMt.getX() / dMDown;
244 final double dtMddPy = -P2_QMt.getY() / dMDown;
245 final double dtMddPz = -P2_QMt.getZ() / dMDown;
246
247 final double dt = delta - tMd;
248 final double dtMddVx = dtMddPx*dt;
249 final double dtMddVy = dtMddPy*dt;
250 final double dtMddVz = dtMddPz*dt;
251
252
253
254
255
256
257 final Transform FSt = secondaryGroundStation.getOffsetToInertial(state.getFrame(), getDate(), false);
258 final PVCoordinates QSt = FSt.transformPVCoordinates(PVCoordinates.ZERO);
259 final Vector3D QSt_V = QSt.getVelocity();
260
261
262 final Transform FSt2 = secondaryGroundStation.getOffsetToInertial(state.getFrame(), getDate().shiftedBy(-t2), false);
263 final PVCoordinates QSt2 = FSt2.transformPVCoordinates(PVCoordinates.ZERO);
264
265 final Vector3D QSt2_P2 = pos2.subtract(QSt2.getPosition());
266 final double dSUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tSu -
267 Vector3D.dotProduct(QSt2_P2, QSt_V);
268
269
270 final double alphaSu = 1./dSUp*QSt2_P2.dotProduct(QSt_V.subtract(vel));
271 final double dtSudPx = 1./dSUp*QSt2_P2.getX() + alphaSu*dtMddPx;
272 final double dtSudPy = 1./dSUp*QSt2_P2.getY() + alphaSu*dtMddPy;
273 final double dtSudPz = 1./dSUp*QSt2_P2.getZ() + alphaSu*dtMddPz;
274 final double dtSudVx = dtSudPx*dt;
275 final double dtSudVy = dtSudPy*dt;
276 final double dtSudVz = dtSudPz*dt;
277
278
279
280
281
282 double dt2dPx = dtSudPx + dtMddPx;
283 double dt2dPy = dtSudPy + dtMddPy;
284 double dt2dPz = dtSudPz + dtMddPz;
285 double dt2dVx = dtSudVx + dtMddVx;
286 double dt2dVy = dtSudVy + dtMddVy;
287 double dt2dVz = dtSudVz + dtMddVz;
288
289
290
291
292
293 final Vector3D pos1 = transitStateLeg1.getPosition();
294 final Vector3D P1_QSt2 = QSt2.getPosition().subtract(pos1);
295 final double dSDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tSd -
296 Vector3D.dotProduct(P1_QSt2, vel);
297
298
299 final double alphaSd = 1./dSDown*P1_QSt2.dotProduct(vel.subtract(QSt_V));
300 final double dtSddPx = -1./ dSDown*P1_QSt2.getX() + alphaSd*dt2dPx;
301 final double dtSddPy = -1./ dSDown*P1_QSt2.getY() + alphaSd*dt2dPy;
302 final double dtSddPz = -1./ dSDown*P1_QSt2.getZ() + alphaSd*dt2dPz;
303
304 final double dt2 = delta - t2 - tSd;
305 final double dtSddVx = -dt2/ dSDown*P1_QSt2.getX()+alphaSd*dt2dVx;
306 final double dtSddVy = -dt2/ dSDown*P1_QSt2.getY()+alphaSd*dt2dVy;
307 final double dtSddVz = -dt2/ dSDown*P1_QSt2.getZ()+alphaSd*dt2dVz;
308
309
310
311
312
313 final Transform FMt1 = primaryGroundStation.getOffsetToInertial(state.getFrame(), getDate().shiftedBy(-t1-t2), false);
314 final PVCoordinates QMt1 = FMt1.transformPVCoordinates(PVCoordinates.ZERO);
315
316 final Vector3D QMt1_P1 = pos1.subtract(QMt1.getPosition());
317 final double dMUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tMu -
318 Vector3D.dotProduct(QMt1_P1, QMt_V);
319
320
321 final double alphaMu = 1./dMUp*QMt1_P1.dotProduct(QMt_V.subtract(vel));
322 final double dtMudPx = 1./dMUp*QMt1_P1.getX() + alphaMu*(dt2dPx+dtSddPx);
323 final double dtMudPy = 1./dMUp*QMt1_P1.getY() + alphaMu*(dt2dPy+dtSddPy);
324 final double dtMudPz = 1./dMUp*QMt1_P1.getZ() + alphaMu*(dt2dPz+dtSddPz);
325
326 final double dtMudVx = dt2/dMUp*QMt1_P1.getX() + alphaMu*(dt2dVx+dtSddVx);
327 final double dtMudVy = dt2/dMUp*QMt1_P1.getY() + alphaMu*(dt2dVy+dtSddVy);
328 final double dtMudVz = dt2/dMUp*QMt1_P1.getZ() + alphaMu*(dt2dVz+dtSddVz);
329
330
331
332
333
334
335
336 double dt1dPx = dtSddPx + dtMudPx;
337 double dt1dPy = dtSddPy + dtMudPy;
338 double dt1dPz = dtSddPz + dtMudPz;
339 double dt1dVx = dtSddVx + dtMudVx;
340 double dt1dVy = dtSddVy + dtMudVy;
341 double dt1dVz = dtSddVz + dtMudVz;
342
343
344
345
346
347
348 double dRdPx = (dt1dPx + dt2dPx)*cOver2;
349 double dRdPy = (dt1dPy + dt2dPy)*cOver2;
350 double dRdPz = (dt1dPz + dt2dPz)*cOver2;
351 double dRdVx = (dt1dVx + dt2dVx)*cOver2;
352 double dRdVy = (dt1dVy + dt2dVy)*cOver2;
353 double dRdVz = (dt1dVz + dt2dVz)*cOver2;
354
355 estimated.setStateDerivatives(0, new double[] {dRdPx, dRdPy, dRdPz,
356 dRdVx, dRdVy, dRdVz
357 });
358
359
360
361
362
363 if (primaryGroundStation.getEastOffsetDriver().isSelected() ||
364 primaryGroundStation.getNorthOffsetDriver().isSelected() ||
365 primaryGroundStation.getZenithOffsetDriver().isSelected()||
366 secondaryGroundStation.getEastOffsetDriver().isSelected() ||
367 secondaryGroundStation.getNorthOffsetDriver().isSelected() ||
368 secondaryGroundStation.getZenithOffsetDriver().isSelected()) {
369
370
371
372
373
374 final AngularCoordinates acM = FMt.getAngular().revert();
375 final Rotation rotationPrimaryTopoToInert = acM.getRotation();
376 final Vector3D OmegaM = acM.getRotationRate();
377
378
379 final AngularCoordinates acS = FSt.getAngular().revert();
380 final Rotation rotationsecondaryTopoToInert = acS.getRotation();
381 final Vector3D OmegaS = acS.getRotationRate();
382
383
384 final double dtMddQMx_I = P2_QMt.getX() / dMDown;
385 final double dtMddQMy_I = P2_QMt.getY() / dMDown;
386 final double dtMddQMz_I = P2_QMt.getZ() / dMDown;
387
388
389 final double dtMddQSx_I = 0.;
390 final double dtMddQSy_I = 0.;
391 final double dtMddQSz_I = 0.;
392
393
394
395 final Vector3D dtMddQM = rotationPrimaryTopoToInert.
396 applyTo(new Vector3D(dtMddQMx_I,
397 dtMddQMy_I,
398 dtMddQMz_I));
399 final Vector3D dtMddQS = rotationsecondaryTopoToInert.
400 applyTo(new Vector3D(dtMddQSx_I,
401 dtMddQSy_I,
402 dtMddQSz_I));
403
404
405
406
407
408 final double dtSudQMx_I = dtMddQMx_I*alphaSu;
409 final double dtSudQMy_I = dtMddQMy_I*alphaSu;
410 final double dtSudQMz_I = dtMddQMz_I*alphaSu;
411
412
413 final double dtSudQSx_I = 1./dSUp
414 *QSt2_P2.dotProduct(Vector3D.MINUS_I
415 .add(OmegaS.crossProduct(Vector3D.PLUS_I).scalarMultiply(t2)));
416 final double dtSudQSy_I = 1./dSUp
417 *QSt2_P2.dotProduct(Vector3D.MINUS_J
418 .add(OmegaS.crossProduct(Vector3D.PLUS_J).scalarMultiply(t2)));
419 final double dtSudQSz_I = 1./dSUp
420 *QSt2_P2.dotProduct(Vector3D.MINUS_K
421 .add(OmegaS.crossProduct(Vector3D.PLUS_K).scalarMultiply(t2)));
422
423
424 final Vector3D dtSudQM = rotationPrimaryTopoToInert.
425 applyTo(new Vector3D(dtSudQMx_I,
426 dtSudQMy_I,
427 dtSudQMz_I));
428 final Vector3D dtSudQS = rotationsecondaryTopoToInert.
429 applyTo(new Vector3D(dtSudQSx_I,
430 dtSudQSy_I,
431 dtSudQSz_I));
432
433
434
435
436 final double dt2dQSx_I = dtMddQSx_I + dtSudQSx_I;
437 final double dt2dQSy_I = dtMddQSy_I + dtSudQSy_I;
438 final double dt2dQSz_I = dtMddQSz_I + dtSudQSz_I;
439 final double dt2dQMx_I = dtMddQMx_I + dtSudQMx_I;
440 final double dt2dQMy_I = dtMddQMy_I + dtSudQMy_I;
441 final double dt2dQMz_I = dtMddQMz_I + dtSudQMz_I;
442
443 final Vector3D dt2dQM = dtSudQM.add(dtMddQM);
444 final Vector3D dt2dQS = dtSudQS.add(dtMddQS);
445
446
447
448
449
450
451 final double dtSddQMx_I = dt2dQMx_I*alphaSd;
452 final double dtSddQMy_I = dt2dQMy_I*alphaSd;
453 final double dtSddQMz_I = dt2dQMz_I*alphaSd;
454
455
456 final double dtSddQSx_I = dt2dQSx_I*alphaSd + 1./dSDown
457 *P1_QSt2.dotProduct(Vector3D.PLUS_I
458 .subtract(OmegaS.crossProduct(Vector3D.PLUS_I).scalarMultiply(t2)));
459 final double dtSddQSy_I = dt2dQSy_I*alphaSd + 1./dSDown
460 *P1_QSt2.dotProduct(Vector3D.PLUS_J
461 .subtract(OmegaS.crossProduct(Vector3D.PLUS_J).scalarMultiply(t2)));
462
463 final double dtSddQSz_I = dt2dQSz_I*alphaSd + 1./dSDown
464 *P1_QSt2.dotProduct(Vector3D.PLUS_K
465 .subtract(OmegaS.crossProduct(Vector3D.PLUS_K).scalarMultiply(t2)));
466
467
468 final Vector3D dtSddQM = rotationPrimaryTopoToInert.
469 applyTo(new Vector3D(dtSddQMx_I,
470 dtSddQMy_I,
471 dtSddQMz_I));
472 final Vector3D dtSddQS = rotationsecondaryTopoToInert.
473 applyTo(new Vector3D(dtSddQSx_I,
474 dtSddQSy_I,
475 dtSddQSz_I));
476
477
478
479
480
481 final double dtMudQMx_I = alphaMu*(dt2dQMx_I+dtSddQMx_I) + 1/dMUp*
482 QMt1_P1.dotProduct(Vector3D.MINUS_I
483 .add(OmegaM.crossProduct(Vector3D.PLUS_I).scalarMultiply(tau)));
484 final double dtMudQMy_I = alphaMu*(dt2dQMy_I+dtSddQMy_I) + 1/dMUp*
485 QMt1_P1.dotProduct(Vector3D.MINUS_J
486 .add(OmegaM.crossProduct(Vector3D.PLUS_J).scalarMultiply(tau)));
487 final double dtMudQMz_I = alphaMu*(dt2dQMz_I+dtSddQMz_I) + 1/dMUp*
488 QMt1_P1.dotProduct(Vector3D.MINUS_K
489 .add(OmegaM.crossProduct(Vector3D.PLUS_K).scalarMultiply(tau)));
490
491
492 final double dtMudQSx_I = alphaMu*(dt2dQSx_I+dtSddQSx_I);
493 final double dtMudQSy_I = alphaMu*(dt2dQSy_I+dtSddQSy_I);
494 final double dtMudQSz_I = alphaMu*(dt2dQSz_I+dtSddQSz_I);
495
496
497
498 final Vector3D dtMudQM = rotationPrimaryTopoToInert.
499 applyTo(new Vector3D(dtMudQMx_I,
500 dtMudQMy_I,
501 dtMudQMz_I));
502 final Vector3D dtMudQS = rotationsecondaryTopoToInert.
503 applyTo(new Vector3D(dtMudQSx_I,
504 dtMudQSy_I,
505 dtMudQSz_I));
506
507
508
509
510 final Vector3D dt1dQM = dtMudQM.add(dtSddQM);
511 final Vector3D dt1dQS = dtMudQS.add(dtSddQS);
512
513
514
515
516 final Vector3D dRdQM = (dt1dQM.add(dt2dQM)).scalarMultiply(cOver2);
517 final Vector3D dRdQS = (dt1dQS.add(dt2dQS)).scalarMultiply(cOver2);
518
519
520 if (primaryGroundStation.getEastOffsetDriver().isSelected()) {
521 estimated.setParameterDerivatives(primaryGroundStation.getEastOffsetDriver(), new AbsoluteDate(), dRdQM.getX());
522 }
523 if (primaryGroundStation.getNorthOffsetDriver().isSelected()) {
524 estimated.setParameterDerivatives(primaryGroundStation.getNorthOffsetDriver(), new AbsoluteDate(), dRdQM.getY());
525 }
526 if (primaryGroundStation.getZenithOffsetDriver().isSelected()) {
527 estimated.setParameterDerivatives(primaryGroundStation.getZenithOffsetDriver(), new AbsoluteDate(), dRdQM.getZ());
528 }
529
530
531 if (secondaryGroundStation.getEastOffsetDriver().isSelected()) {
532 estimated.setParameterDerivatives(secondaryGroundStation.getEastOffsetDriver(), new AbsoluteDate(), dRdQS.getX());
533 }
534 if (secondaryGroundStation.getNorthOffsetDriver().isSelected()) {
535 estimated.setParameterDerivatives(secondaryGroundStation.getNorthOffsetDriver(), new AbsoluteDate(), dRdQS.getY());
536 }
537 if (secondaryGroundStation.getZenithOffsetDriver().isSelected()) {
538 estimated.setParameterDerivatives(secondaryGroundStation.getZenithOffsetDriver(), new AbsoluteDate(), dRdQS.getZ());
539 }
540 }
541
542 return estimated;
543
544 }
545
546
547
548
549
550
551
552
553
554
555 protected EstimatedMeasurement<TurnAroundRange> theoreticalEvaluationValidation(final int iteration, final int evaluation,
556 final SpacecraftState state) {
557
558 final GroundStation primaryGroundStation = getPrimaryStation();
559 final GroundStation secondaryGroundStation = getSecondaryStation();
560 int nbParams = 6;
561 final Map<String, Integer> indices = new HashMap<>();
562 for (ParameterDriver driver : getParametersDrivers()) {
563
564
565
566 if (driver.isSelected() && !indices.containsKey(driver.getName())) {
567 indices.put(driver.getName(), nbParams++);
568 }
569 }
570 final Field<Gradient> field = GradientField.getField(nbParams);
571 final FieldVector3D<Gradient> zero = FieldVector3D.getZero(field);
572
573
574 final TimeStampedFieldPVCoordinates<Gradient> pvaDS = getCoordinates(state, 0, nbParams);
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597 final AbsoluteDate measurementDate = getDate();
598 final FieldAbsoluteDate<Gradient> measurementDateDS = new FieldAbsoluteDate<>(field, measurementDate);
599 final double delta = measurementDate.durationFrom(state.getDate());
600
601
602
603 final FieldTransform<Gradient> primaryToInert =
604 primaryGroundStation.getOffsetToInertial(state.getFrame(), measurementDate, nbParams, indices);
605
606
607 final FieldVector3D<Gradient> QPrimary = primaryToInert.transformPosition(zero);
608
609
610 final Gradient primaryTauD = signalTimeOfFlightAdjustableEmitter(pvaDS, QPrimary, measurementDateDS, state.getFrame());
611
612
613 final Gradient dtLeg2 = primaryTauD.negate().add(delta);
614
615
616 final SpacecraftState transitStateLeg2 = state.shiftedBy(dtLeg2.getValue());
617
618
619 final TimeStampedFieldPVCoordinates<Gradient> transitStateLeg2PV = pvaDS.shiftedBy(dtLeg2);
620
621
622
623 final FieldAbsoluteDate<Gradient> approxReboundDate = measurementDateDS.shiftedBy(-delta);
624 final FieldTransform<Gradient> secondaryToInertApprox =
625 secondaryGroundStation.getOffsetToInertial(state.getFrame(), approxReboundDate, nbParams, indices);
626
627
628 final TimeStampedFieldPVCoordinates<Gradient> QsecondaryApprox =
629 secondaryToInertApprox.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(approxReboundDate,
630 zero, zero, zero));
631
632
633 final Gradient secondaryTauU =
634 signalTimeOfFlightAdjustableEmitter(QsecondaryApprox,
635 transitStateLeg2PV.getPosition(),
636 transitStateLeg2PV.getDate(),
637 state.getFrame());
638
639
640 final Gradient tauLeg2 = primaryTauD.add(secondaryTauU);
641
642
643
644
645
646 final FieldAbsoluteDate<Gradient> reboundDateDS = measurementDateDS.shiftedBy(tauLeg2.negate());
647 final FieldTransform<Gradient> secondaryToInert =
648 secondaryGroundStation.getOffsetToInertial(state.getFrame(), reboundDateDS, nbParams, indices);
649
650
651 final FieldVector3D<Gradient> Qsecondary = secondaryToInert.transformPosition(zero);
652
653
654 final Gradient secondaryTauD = signalTimeOfFlightAdjustableEmitter(transitStateLeg2PV, Qsecondary,
655 reboundDateDS, state.getFrame());
656
657
658
659 final Gradient dtLeg1 = dtLeg2.subtract(secondaryTauU).subtract(secondaryTauD);
660
661
662 final TimeStampedFieldPVCoordinates<Gradient> transitStateLeg1PV = pvaDS.shiftedBy(dtLeg1);
663
664
665
666 final FieldAbsoluteDate<Gradient> approxEmissionDate =
667 measurementDateDS.shiftedBy(-2 * (secondaryTauU.getValue() + primaryTauD.getValue()));
668 final FieldTransform<Gradient> primaryToInertApprox =
669 primaryGroundStation.getOffsetToInertial(state.getFrame(), approxEmissionDate, nbParams, indices);
670
671
672 final TimeStampedFieldPVCoordinates<Gradient> QPrimaryApprox =
673 primaryToInertApprox.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(approxEmissionDate,
674 zero, zero, zero));
675
676
677 final Gradient primaryTauU = signalTimeOfFlightAdjustableEmitter(QPrimaryApprox,
678 transitStateLeg1PV.getPosition(),
679 transitStateLeg1PV.getDate(),
680 state.getFrame());
681
682
683 final Gradient tauLeg1 = secondaryTauD.add(primaryTauU);
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699 final EstimatedMeasurement<TurnAroundRange> estimated =
700 new EstimatedMeasurement<>(this, iteration, evaluation,
701 new SpacecraftState[] {
702 transitStateLeg2.shiftedBy(-secondaryTauU.getValue())
703 }, null);
704
705
706 final double cOver2 = 0.5 * Constants.SPEED_OF_LIGHT;
707 final Gradient turnAroundRange = (tauLeg2.add(tauLeg1)).multiply(cOver2);
708 estimated.setEstimatedValue(turnAroundRange.getValue());
709
710
711
712 final double[] derivatives = turnAroundRange.getGradient();
713 estimated.setStateDerivatives(0, Arrays.copyOfRange(derivatives, 0, 6));
714
715
716
717 for (final ParameterDriver driver : getParametersDrivers()) {
718 final Integer index = indices.get(driver.getName());
719 if (index != null) {
720 estimated.setParameterDerivatives(driver, new AbsoluteDate(), derivatives[index]);
721 }
722 }
723
724
725
726
727
728
729
730
731
732
733
734
735
736 final Transform primaryTopoToInert =
737 primaryGroundStation.getOffsetToInertial(state.getFrame(), measurementDate, false);
738 final TimeStampedPVCoordinates QMt = primaryTopoToInert.
739 transformPVCoordinates(new TimeStampedPVCoordinates(measurementDate, PVCoordinates.ZERO));
740
741
742 final Transform secondaryTopoToInert =
743 secondaryGroundStation.getOffsetToInertial(state.getFrame(), measurementDate, false);
744 final TimeStampedPVCoordinates QSt = secondaryTopoToInert.
745 transformPVCoordinates(new TimeStampedPVCoordinates(measurementDate, PVCoordinates.ZERO));
746
747
748 final double tMd = signalTimeOfFlightAdjustableEmitter(state.getPVCoordinates(), QMt.getPosition(),
749 measurementDate, state.getFrame());
750
751
752 final SpacecraftState state2 = state.shiftedBy(delta - tMd);
753 final AbsoluteDate transitDateLeg2 = transitStateLeg2.getDate();
754
755
756 final Transform secondaryTopoToInertTransitLeg2 =
757 secondaryGroundStation.getOffsetToInertial(state.getFrame(), transitDateLeg2, false);
758 final TimeStampedPVCoordinates QSdate2PV = secondaryTopoToInertTransitLeg2.
759 transformPVCoordinates(new TimeStampedPVCoordinates(transitDateLeg2, PVCoordinates.ZERO));
760
761
762 final double tSu = signalTimeOfFlightAdjustableEmitter(QSdate2PV,
763 state2.getPosition(),
764 transitDateLeg2,
765 state.getFrame());
766
767
768 final double t2 = tMd + tSu;
769
770
771
772
773
774
775 final AbsoluteDate tQSA = measurementDate.shiftedBy(-t2);
776
777
778 final Transform secondaryTopoToInertArrivalDate =
779 secondaryGroundStation.getOffsetToInertial(state.getFrame(), tQSA, false);
780 final Vector3D QSA = secondaryTopoToInertArrivalDate.transformPosition(Vector3D.ZERO);
781
782
783 final double tSd = signalTimeOfFlightAdjustableEmitter(state2.getPVCoordinates(), QSA, tQSA, state2.getFrame());
784
785
786
787 final SpacecraftState state1 = state.shiftedBy(delta -tMd -tSu -tSd);
788 final AbsoluteDate transitDateLeg1 = transitStateLeg1PV.getDate().toAbsoluteDate();
789
790
791 final Transform primaryTopoToInertTransitLeg1 =
792 primaryGroundStation.getOffsetToInertial(state.getFrame(), transitDateLeg1, false);
793 final TimeStampedPVCoordinates QMdate1PV = primaryTopoToInertTransitLeg1.
794 transformPVCoordinates(new TimeStampedPVCoordinates(transitDateLeg1, PVCoordinates.ZERO));
795
796
797 final double tMu = signalTimeOfFlightAdjustableEmitter(QMdate1PV,
798 state1.getPosition(),
799 transitDateLeg1,
800 state1.getFrame());
801
802
803 final double t1 = tSd + tMu;
804
805
806 final double t = t1+t2;
807
808
809 final double TAR = t*cOver2;
810
811
812 final double dTAR = turnAroundRange.getValue() - TAR;
813
814
815
816
817
818 final Vector3D vel = state.getPVCoordinates().getVelocity();
819 final PVCoordinates QMt_PV = primaryTopoToInert.transformPVCoordinates(PVCoordinates.ZERO);
820 final Vector3D QMt_V = QMt_PV.getVelocity();
821 final Vector3D pos2 = state2.getPosition();
822 final Vector3D P2_QMt = QMt_PV.getPosition().subtract(pos2);
823 final double dMDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tMd -
824 Vector3D.dotProduct(P2_QMt, vel);
825
826
827 final double dtMddPx = -P2_QMt.getX() / dMDown;
828 final double dtMddPy = -P2_QMt.getY() / dMDown;
829 final double dtMddPz = -P2_QMt.getZ() / dMDown;
830
831 final double dt = delta - tMd;
832 final double dtMddVx = dtMddPx*dt;
833 final double dtMddVy = dtMddPy*dt;
834 final double dtMddVz = dtMddPz*dt;
835
836
837 final double dtMddPxDS = primaryTauD.getPartialDerivative(0);
838 final double dtMddPyDS = primaryTauD.getPartialDerivative(1);
839 final double dtMddPzDS = primaryTauD.getPartialDerivative(2);
840 final double dtMddVxDS = primaryTauD.getPartialDerivative(3);
841 final double dtMddVyDS = primaryTauD.getPartialDerivative(4);
842 final double dtMddVzDS = primaryTauD.getPartialDerivative(5);
843
844
845 final double d_dtMddPx = dtMddPxDS-dtMddPx;
846 final double d_dtMddPy = dtMddPyDS-dtMddPy;
847 final double d_dtMddPz = dtMddPzDS-dtMddPz;
848 final double d_dtMddVx = dtMddVxDS-dtMddVx;
849 final double d_dtMddVy = dtMddVyDS-dtMddVy;
850 final double d_dtMddVz = dtMddVzDS-dtMddVz;
851
852
853
854
855
856
857
858
859 final Vector3D QSt_V = QSt.getVelocity();
860
861
862 final PVCoordinates QSt2 = secondaryTopoToInertArrivalDate.transformPVCoordinates(PVCoordinates.ZERO);
863
864 final Vector3D QSt2_P2 = pos2.subtract(QSt2.getPosition());
865 final double dSUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tSu -
866 Vector3D.dotProduct(QSt2_P2, QSt_V);
867
868 final double alphaSu = 1./dSUp*QSt2_P2.dotProduct(QSt_V.subtract(vel));
869
870 final double dtSudPx = 1./dSUp*QSt2_P2.getX() + alphaSu*dtMddPx;
871 final double dtSudPy = 1./dSUp*QSt2_P2.getY() + alphaSu*dtMddPy;
872 final double dtSudPz = 1./dSUp*QSt2_P2.getZ() + alphaSu*dtMddPz;
873
874 final double dtSudVx = dtSudPx*dt;
875 final double dtSudVy = dtSudPy*dt;
876 final double dtSudVz = dtSudPz*dt;
877
878
879
880 final double dtSudPxDS = secondaryTauU.getPartialDerivative(0);
881 final double dtSudPyDS = secondaryTauU.getPartialDerivative(1);
882 final double dtSudPzDS = secondaryTauU.getPartialDerivative(2);
883 final double dtSudVxDS = secondaryTauU.getPartialDerivative(3);
884 final double dtSudVyDS = secondaryTauU.getPartialDerivative(4);
885 final double dtSudVzDS = secondaryTauU.getPartialDerivative(5);
886
887
888 final double d_dtSudPx = dtSudPxDS-dtSudPx;
889 final double d_dtSudPy = dtSudPyDS-dtSudPy;
890 final double d_dtSudPz = dtSudPzDS-dtSudPz;
891 final double d_dtSudVx = dtSudVxDS-dtSudVx;
892 final double d_dtSudVy = dtSudVyDS-dtSudVy;
893 final double d_dtSudVz = dtSudVzDS-dtSudVz;
894
895
896
897
898
899
900 double dt2dPx = dtSudPx + dtMddPx;
901 double dt2dPy = dtSudPy + dtMddPy;
902 double dt2dPz = dtSudPz + dtMddPz;
903 double dt2dVx = dtSudVx + dtMddVx;
904 double dt2dVy = dtSudVy + dtMddVy;
905 double dt2dVz = dtSudVz + dtMddVz;
906
907
908 double dt2dPxDS = tauLeg2.getPartialDerivative(0);
909 double dt2dPyDS = tauLeg2.getPartialDerivative(1);
910 double dt2dPzDS = tauLeg2.getPartialDerivative(2);
911 double dt2dVxDS = tauLeg2.getPartialDerivative(3);
912 double dt2dVyDS = tauLeg2.getPartialDerivative(4);
913 double dt2dVzDS = tauLeg2.getPartialDerivative(5);
914
915
916 final double d_dt2dPx = dt2dPxDS-dt2dPx;
917 final double d_dt2dPy = dt2dPyDS-dt2dPy;
918 final double d_dt2dPz = dt2dPzDS-dt2dPz;
919 final double d_dt2dVx = dt2dVxDS-dt2dVx;
920 final double d_dt2dVy = dt2dVyDS-dt2dVy;
921 final double d_dt2dVz = dt2dVzDS-dt2dVz;
922
923
924
925
926
927 final Vector3D pos1 = state1.getPosition();
928 final Vector3D P1_QSt2 = QSt2.getPosition().subtract(pos1);
929 final double dSDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tSd -
930 Vector3D.dotProduct(P1_QSt2, vel);
931
932
933 final double alphaSd = 1./dSDown*P1_QSt2.dotProduct(vel.subtract(QSt_V));
934 final double dtSddPx = -1./ dSDown*P1_QSt2.getX() + alphaSd*dt2dPx;
935 final double dtSddPy = -1./ dSDown*P1_QSt2.getY() + alphaSd*dt2dPy;
936 final double dtSddPz = -1./ dSDown*P1_QSt2.getZ() + alphaSd*dt2dPz;
937
938 final double dt2 = delta - t2 - tSd;
939 final double dtSddVx = -dt2/ dSDown*P1_QSt2.getX()+alphaSd*dt2dVx;
940 final double dtSddVy = -dt2/ dSDown*P1_QSt2.getY()+alphaSd*dt2dVy;
941 final double dtSddVz = -dt2/ dSDown*P1_QSt2.getZ()+alphaSd*dt2dVz;
942
943
944 final double dtSddPxDS = secondaryTauD.getPartialDerivative(0);
945 final double dtSddPyDS = secondaryTauD.getPartialDerivative(1);
946 final double dtSddPzDS = secondaryTauD.getPartialDerivative(2);
947 final double dtSddVxDS = secondaryTauD.getPartialDerivative(3);
948 final double dtSddVyDS = secondaryTauD.getPartialDerivative(4);
949 final double dtSddVzDS = secondaryTauD.getPartialDerivative(5);
950
951
952 final double d_dtSddPx = dtSddPxDS-dtSddPx;
953 final double d_dtSddPy = dtSddPyDS-dtSddPy;
954 final double d_dtSddPz = dtSddPzDS-dtSddPz;
955 final double d_dtSddVx = dtSddVxDS-dtSddVx;
956 final double d_dtSddVy = dtSddVyDS-dtSddVy;
957 final double d_dtSddVz = dtSddVzDS-dtSddVz;
958
959
960
961
962
963
964 final Transform FMt1 = primaryGroundStation.getOffsetToInertial(state.getFrame(), measurementDate.shiftedBy(-t1-t2), false);
965 final PVCoordinates QMt1 = FMt1.transformPVCoordinates(PVCoordinates.ZERO);
966
967 final Vector3D QMt1_P1 = pos1.subtract(QMt1.getPosition());
968 final double dMUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tMu -
969 Vector3D.dotProduct(QMt1_P1, QMt_V);
970
971
972 final double alphaMu = 1./dMUp*QMt1_P1.dotProduct(QMt_V.subtract(vel));
973 final double dtMudPx = 1./dMUp*QMt1_P1.getX() + alphaMu*(dt2dPx+dtSddPx);
974 final double dtMudPy = 1./dMUp*QMt1_P1.getY() + alphaMu*(dt2dPy+dtSddPy);
975 final double dtMudPz = 1./dMUp*QMt1_P1.getZ() + alphaMu*(dt2dPz+dtSddPz);
976
977 final double dtMudVx = dt2/dMUp*QMt1_P1.getX() + alphaMu*(dt2dVx+dtSddVx);
978 final double dtMudVy = dt2/dMUp*QMt1_P1.getY() + alphaMu*(dt2dVy+dtSddVy);
979 final double dtMudVz = dt2/dMUp*QMt1_P1.getZ() + alphaMu*(dt2dVz+dtSddVz);
980
981
982 final double dtMudPxDS = primaryTauU.getPartialDerivative(0);
983 final double dtMudPyDS = primaryTauU.getPartialDerivative(1);
984 final double dtMudPzDS = primaryTauU.getPartialDerivative(2);
985 final double dtMudVxDS = primaryTauU.getPartialDerivative(3);
986 final double dtMudVyDS = primaryTauU.getPartialDerivative(4);
987 final double dtMudVzDS = primaryTauU.getPartialDerivative(5);
988
989
990 final double d_dtMudPx = dtMudPxDS-dtMudPx;
991 final double d_dtMudPy = dtMudPyDS-dtMudPy;
992 final double d_dtMudPz = dtMudPzDS-dtMudPz;
993 final double d_dtMudVx = dtMudVxDS-dtMudVx;
994 final double d_dtMudVy = dtMudVyDS-dtMudVy;
995 final double d_dtMudVz = dtMudVzDS-dtMudVz;
996
997
998
999
1000
1001
1002 double dt1dPx = dtSddPx + dtMudPx;
1003 double dt1dPy = dtSddPy + dtMudPy;
1004 double dt1dPz = dtSddPz + dtMudPz;
1005 double dt1dVx = dtSddVx + dtMudVx;
1006 double dt1dVy = dtSddVy + dtMudVy;
1007 double dt1dVz = dtSddVz + dtMudVz;
1008
1009
1010 double dt1dPxDS = tauLeg1.getPartialDerivative(0);
1011 double dt1dPyDS = tauLeg1.getPartialDerivative(1);
1012 double dt1dPzDS = tauLeg1.getPartialDerivative(2);
1013 double dt1dVxDS = tauLeg1.getPartialDerivative(3);
1014 double dt1dVyDS = tauLeg1.getPartialDerivative(4);
1015 double dt1dVzDS = tauLeg1.getPartialDerivative(5);
1016
1017
1018 final double d_dt1dPx = dt1dPxDS-dt1dPx;
1019 final double d_dt1dPy = dt1dPyDS-dt1dPy;
1020 final double d_dt1dPz = dt1dPzDS-dt1dPz;
1021 final double d_dt1dVx = dt1dVxDS-dt1dVx;
1022 final double d_dt1dVy = dt1dVyDS-dt1dVy;
1023 final double d_dt1dVz = dt1dVzDS-dt1dVz;
1024
1025
1026
1027
1028
1029
1030 double dRdPx = (dt1dPx + dt2dPx)*cOver2;
1031 double dRdPy = (dt1dPy + dt2dPy)*cOver2;
1032 double dRdPz = (dt1dPz + dt2dPz)*cOver2;
1033 double dRdVx = (dt1dVx + dt2dVx)*cOver2;
1034 double dRdVy = (dt1dVy + dt2dVy)*cOver2;
1035 double dRdVz = (dt1dVz + dt2dVz)*cOver2;
1036
1037
1038 double dRdPxDS = turnAroundRange.getPartialDerivative(0);
1039 double dRdPyDS = turnAroundRange.getPartialDerivative(1);
1040 double dRdPzDS = turnAroundRange.getPartialDerivative(2);
1041 double dRdVxDS = turnAroundRange.getPartialDerivative(3);
1042 double dRdVyDS = turnAroundRange.getPartialDerivative(4);
1043 double dRdVzDS = turnAroundRange.getPartialDerivative(5);
1044
1045
1046 final double d_dRdPx = dRdPxDS-dRdPx;
1047 final double d_dRdPy = dRdPyDS-dRdPy;
1048 final double d_dRdPz = dRdPzDS-dRdPz;
1049 final double d_dRdVx = dRdVxDS-dRdVx;
1050 final double d_dRdVy = dRdVyDS-dRdVy;
1051 final double d_dRdVz = dRdVzDS-dRdVz;
1052
1053
1054
1055
1056
1057
1058 final AngularCoordinates acM = primaryTopoToInert.getAngular().revert();
1059 final Rotation rotationPrimaryTopoToInert = acM.getRotation();
1060 final Vector3D OmegaM = acM.getRotationRate();
1061
1062
1063 final AngularCoordinates acS = secondaryTopoToInert.getAngular().revert();
1064 final Rotation rotationsecondaryTopoToInert = acS.getRotation();
1065 final Vector3D OmegaS = acS.getRotationRate();
1066
1067
1068 final double dtMddQMx_I = P2_QMt.getX() / dMDown;
1069 final double dtMddQMy_I = P2_QMt.getY() / dMDown;
1070 final double dtMddQMz_I = P2_QMt.getZ() / dMDown;
1071
1072
1073 final double dtMddQSx_I = 0.;
1074 final double dtMddQSy_I = 0.;
1075 final double dtMddQSz_I = 0.;
1076
1077
1078
1079 final Vector3D dtMddQM = rotationPrimaryTopoToInert.
1080 applyTo(new Vector3D(dtMddQMx_I,
1081 dtMddQMy_I,
1082 dtMddQMz_I));
1083 final Vector3D dtMddQS = rotationsecondaryTopoToInert.
1084 applyTo(new Vector3D(dtMddQSx_I,
1085 dtMddQSy_I,
1086 dtMddQSz_I));
1087
1088
1089 double dtMddQMx_DS = primaryTauD.getPartialDerivative(6);
1090 double dtMddQMy_DS = primaryTauD.getPartialDerivative(7);
1091 double dtMddQMz_DS = primaryTauD.getPartialDerivative(8);
1092
1093 double dtMddQSx_DS = primaryTauD.getPartialDerivative(9);
1094 double dtMddQSy_DS = primaryTauD.getPartialDerivative(10);
1095 double dtMddQSz_DS = primaryTauD.getPartialDerivative(11);
1096
1097
1098
1099 final double d_dtMddQMx = dtMddQMx_DS-dtMddQM.getX();
1100 final double d_dtMddQMy = dtMddQMy_DS-dtMddQM.getY();
1101 final double d_dtMddQMz = dtMddQMz_DS-dtMddQM.getZ();
1102
1103 final double d_dtMddQSx = dtMddQSx_DS-dtMddQS.getX();
1104 final double d_dtMddQSy = dtMddQSy_DS-dtMddQS.getY();
1105 final double d_dtMddQSz = dtMddQSz_DS-dtMddQS.getZ();
1106
1107
1108
1109
1110
1111
1112
1113 final double dtSudQMx_I = dtMddQMx_I*alphaSu;
1114 final double dtSudQMy_I = dtMddQMy_I*alphaSu;
1115 final double dtSudQMz_I = dtMddQMz_I*alphaSu;
1116
1117
1118 final double dtSudQSx_I = 1./dSUp*QSt2_P2
1119 .dotProduct(Vector3D.MINUS_I
1120 .add(OmegaS.crossProduct(Vector3D.PLUS_I).scalarMultiply(t2)));
1121 final double dtSudQSy_I = 1./dSUp*QSt2_P2
1122 .dotProduct(Vector3D.MINUS_J
1123 .add(OmegaS.crossProduct(Vector3D.PLUS_J).scalarMultiply(t2)));
1124 final double dtSudQSz_I = 1./dSUp*QSt2_P2
1125 .dotProduct(Vector3D.MINUS_K
1126 .add(OmegaS.crossProduct(Vector3D.PLUS_K).scalarMultiply(t2)));
1127
1128
1129 final Vector3D dtSudQM = rotationPrimaryTopoToInert.
1130 applyTo(new Vector3D(dtSudQMx_I,
1131 dtSudQMy_I,
1132 dtSudQMz_I));
1133 final Vector3D dtSudQS = rotationsecondaryTopoToInert.
1134 applyTo(new Vector3D(dtSudQSx_I,
1135 dtSudQSy_I,
1136 dtSudQSz_I));
1137
1138 double dtSudQMx_DS = secondaryTauU.getPartialDerivative(6);
1139 double dtSudQMy_DS = secondaryTauU.getPartialDerivative(7);
1140 double dtSudQMz_DS = secondaryTauU.getPartialDerivative(8);
1141
1142 double dtSudQSx_DS = secondaryTauU.getPartialDerivative(9);
1143 double dtSudQSy_DS = secondaryTauU.getPartialDerivative(10);
1144 double dtSudQSz_DS = secondaryTauU.getPartialDerivative(11);
1145
1146
1147
1148 final double d_dtSudQMx = dtSudQMx_DS-dtSudQM.getX();
1149 final double d_dtSudQMy = dtSudQMy_DS-dtSudQM.getY();
1150 final double d_dtSudQMz = dtSudQMz_DS-dtSudQM.getZ();
1151 final double d_dtSudQSx = dtSudQSx_DS-dtSudQS.getX();
1152 final double d_dtSudQSy = dtSudQSy_DS-dtSudQS.getY();
1153 final double d_dtSudQSz = dtSudQSz_DS-dtSudQS.getZ();
1154
1155
1156
1157
1158
1159 final double dt2dQMx_I = dtMddQMx_I + dtSudQMx_I;
1160 final double dt2dQMy_I = dtMddQMy_I + dtSudQMy_I;
1161 final double dt2dQMz_I = dtMddQMz_I + dtSudQMz_I;
1162 final double dt2dQSx_I = dtMddQSx_I + dtSudQSx_I;
1163 final double dt2dQSy_I = dtMddQSy_I + dtSudQSy_I;
1164 final double dt2dQSz_I = dtMddQSz_I + dtSudQSz_I;
1165
1166 final Vector3D dt2dQM = dtSudQM.add(dtMddQM);
1167 final Vector3D dt2dQS = dtSudQS.add(dtMddQS);
1168
1169
1170 double dt2dQMx_DS = tauLeg2.getPartialDerivative(6);
1171 double dt2dQMy_DS = tauLeg2.getPartialDerivative(7);
1172 double dt2dQMz_DS = tauLeg2.getPartialDerivative(8);
1173 double dt2dQSx_DS = tauLeg2.getPartialDerivative(9);
1174 double dt2dQSy_DS = tauLeg2.getPartialDerivative(10);
1175 double dt2dQSz_DS = tauLeg2.getPartialDerivative(11);
1176
1177
1178
1179 final double d_dt2dQMx = dt2dQMx_DS-dt2dQM.getX();
1180 final double d_dt2dQMy = dt2dQMy_DS-dt2dQM.getY();
1181 final double d_dt2dQMz = dt2dQMz_DS-dt2dQM.getZ();
1182 final double d_dt2dQSx = dt2dQSx_DS-dt2dQS.getX();
1183 final double d_dt2dQSy = dt2dQSy_DS-dt2dQS.getY();
1184 final double d_dt2dQSz = dt2dQSz_DS-dt2dQS.getZ();
1185
1186
1187
1188
1189
1190
1191 final double dtSddQMx_I = dt2dQMx_I*alphaSd;
1192 final double dtSddQMy_I = dt2dQMy_I*alphaSd;
1193 final double dtSddQMz_I = dt2dQMz_I*alphaSd;
1194
1195
1196 final double dtSddQSx_I = dt2dQSx_I*alphaSd + 1./dSDown
1197 *P1_QSt2.dotProduct(Vector3D.PLUS_I
1198 .subtract(OmegaS.crossProduct(Vector3D.PLUS_I).scalarMultiply(t2)));
1199 final double dtSddQSy_I = dt2dQSy_I*alphaSd + 1./dSDown
1200 *P1_QSt2.dotProduct(Vector3D.PLUS_J
1201 .subtract(OmegaS.crossProduct(Vector3D.PLUS_J).scalarMultiply(t2)));
1202
1203 final double dtSddQSz_I = dt2dQSz_I*alphaSd + 1./dSDown
1204 *P1_QSt2.dotProduct(Vector3D.PLUS_K
1205 .subtract(OmegaS.crossProduct(Vector3D.PLUS_K).scalarMultiply(t2)));
1206
1207
1208 final Vector3D dtSddQM = rotationPrimaryTopoToInert.
1209 applyTo(new Vector3D(dtSddQMx_I,
1210 dtSddQMy_I,
1211 dtSddQMz_I));
1212 final Vector3D dtSddQS = rotationsecondaryTopoToInert.
1213 applyTo(new Vector3D(dtSddQSx_I,
1214 dtSddQSy_I,
1215 dtSddQSz_I));
1216
1217 double dtSddQMx_DS = secondaryTauD.getPartialDerivative(6);
1218 double dtSddQMy_DS = secondaryTauD.getPartialDerivative(7);
1219 double dtSddQMz_DS = secondaryTauD.getPartialDerivative(8);
1220 double dtSddQSx_DS = secondaryTauD.getPartialDerivative(9);
1221 double dtSddQSy_DS = secondaryTauD.getPartialDerivative(10);
1222 double dtSddQSz_DS = secondaryTauD.getPartialDerivative(11);
1223
1224
1225
1226 final double d_dtSddQMx = dtSddQMx_DS-dtSddQM.getX();
1227 final double d_dtSddQMy = dtSddQMy_DS-dtSddQM.getY();
1228 final double d_dtSddQMz = dtSddQMz_DS-dtSddQM.getZ();
1229 final double d_dtSddQSx = dtSddQSx_DS-dtSddQS.getX();
1230 final double d_dtSddQSy = dtSddQSy_DS-dtSddQS.getY();
1231 final double d_dtSddQSz = dtSddQSz_DS-dtSddQS.getZ();
1232
1233
1234
1235
1236
1237
1238 final double dtMudQMx_I = -QMt1_P1.getX()/dMUp + alphaMu*(dt2dQMx_I+dtSddQMx_I)
1239 + t/dMUp*QMt1_P1.dotProduct(OmegaM.crossProduct(Vector3D.PLUS_I));
1240
1241 final double dtMudQMy_I = -QMt1_P1.getY()/dMUp + alphaMu*(dt2dQMy_I+dtSddQMy_I)
1242 + t/dMUp*QMt1_P1.dotProduct(OmegaM.crossProduct(Vector3D.PLUS_J));
1243
1244 final double dtMudQMz_I = -QMt1_P1.getZ()/dMUp + alphaMu*(dt2dQMz_I+dtSddQMz_I)
1245 + t/dMUp*QMt1_P1.dotProduct(OmegaM.crossProduct(Vector3D.PLUS_K));
1246
1247
1248
1249 final double dtMudQSx_I = alphaMu*(dt2dQSx_I+dtSddQSx_I);
1250 final double dtMudQSy_I = alphaMu*(dt2dQSy_I+dtSddQSy_I);
1251 final double dtMudQSz_I = alphaMu*(dt2dQSz_I+dtSddQSz_I);
1252
1253
1254
1255 final Vector3D dtMudQM = rotationPrimaryTopoToInert.
1256 applyTo(new Vector3D(dtMudQMx_I,
1257 dtMudQMy_I,
1258 dtMudQMz_I));
1259 final Vector3D dtMudQS = rotationsecondaryTopoToInert.
1260 applyTo(new Vector3D(dtMudQSx_I,
1261 dtMudQSy_I,
1262 dtMudQSz_I));
1263
1264 double dtMudQMx_DS = primaryTauU.getPartialDerivative(6);
1265 double dtMudQMy_DS = primaryTauU.getPartialDerivative(7);
1266 double dtMudQMz_DS = primaryTauU.getPartialDerivative(8);
1267 double dtMudQSx_DS = primaryTauU.getPartialDerivative(9);
1268 double dtMudQSy_DS = primaryTauU.getPartialDerivative(10);
1269 double dtMudQSz_DS = primaryTauU.getPartialDerivative(11);
1270
1271
1272 final double d_dtMudQMx = dtMudQMx_DS-dtMudQM.getX();
1273 final double d_dtMudQMy = dtMudQMy_DS-dtMudQM.getY();
1274 final double d_dtMudQMz = dtMudQMz_DS-dtMudQM.getZ();
1275 final double d_dtMudQSx = dtMudQSx_DS-dtMudQS.getX();
1276 final double d_dtMudQSy = dtMudQSy_DS-dtMudQS.getY();
1277 final double d_dtMudQSz = dtMudQSz_DS-dtMudQS.getZ();
1278
1279
1280
1281
1282
1283 final Vector3D dt1dQM = dtMudQM.add(dtSddQM);
1284 final Vector3D dt1dQS = dtMudQS.add(dtSddQS);
1285
1286
1287 double dt1dQMx_DS = tauLeg1.getPartialDerivative(6);
1288 double dt1dQMy_DS = tauLeg1.getPartialDerivative(7);
1289 double dt1dQMz_DS = tauLeg1.getPartialDerivative(8);
1290 double dt1dQSx_DS = tauLeg1.getPartialDerivative(9);
1291 double dt1dQSy_DS = tauLeg1.getPartialDerivative(10);
1292 double dt1dQSz_DS = tauLeg1.getPartialDerivative(11);
1293
1294
1295
1296 final double d_dt1dQMx = dt1dQMx_DS-dt1dQM.getX();
1297 final double d_dt1dQMy = dt1dQMy_DS-dt1dQM.getY();
1298 final double d_dt1dQMz = dt1dQMz_DS-dt1dQM.getZ();
1299 final double d_dt1dQSx = dt1dQSx_DS-dt1dQS.getX();
1300 final double d_dt1dQSy = dt1dQSy_DS-dt1dQS.getY();
1301 final double d_dt1dQSz = dt1dQSz_DS-dt1dQS.getZ();
1302
1303
1304
1305
1306
1307 final Vector3D dRdQM = (dt1dQM.add(dt2dQM)).scalarMultiply(cOver2);
1308 final Vector3D dRdQS = (dt1dQS.add(dt2dQS)).scalarMultiply(cOver2);
1309
1310
1311 double dRdQMx_DS = turnAroundRange.getPartialDerivative(6);
1312 double dRdQMy_DS = turnAroundRange.getPartialDerivative(7);
1313 double dRdQMz_DS = turnAroundRange.getPartialDerivative(8);
1314 double dRdQSx_DS = turnAroundRange.getPartialDerivative(9);
1315 double dRdQSy_DS = turnAroundRange.getPartialDerivative(10);
1316 double dRdQSz_DS = turnAroundRange.getPartialDerivative(11);
1317
1318
1319
1320 final double d_dRdQMx = dRdQMx_DS-dRdQM.getX();
1321 final double d_dRdQMy = dRdQMy_DS-dRdQM.getY();
1322 final double d_dRdQMz = dRdQMz_DS-dRdQM.getZ();
1323 final double d_dRdQSx = dRdQSx_DS-dRdQS.getX();
1324 final double d_dRdQSy = dRdQSy_DS-dRdQS.getY();
1325 final double d_dRdQSz = dRdQSz_DS-dRdQS.getZ();
1326
1327
1328
1329 final boolean printResults = false;
1330
1331 if (printResults) {
1332 System.out.println("dTAR = " + dTAR);
1333
1334 System.out.println("d_dtMddPx = " + d_dtMddPx);
1335 System.out.println("d_dtMddPy = " + d_dtMddPy);
1336 System.out.println("d_dtMddPz = " + d_dtMddPz);
1337 System.out.println("d_dtMddVx = " + d_dtMddVx);
1338 System.out.println("d_dtMddVy = " + d_dtMddVy);
1339 System.out.println("d_dtMddVz = " + d_dtMddVz);
1340
1341 System.out.println("d_dtSudPx = " + d_dtSudPx);
1342 System.out.println("d_dtSudPy = " + d_dtSudPy);
1343 System.out.println("d_dtSudPz = " + d_dtSudPz);
1344 System.out.println("d_dtSudVx = " + d_dtSudVx);
1345 System.out.println("d_dtSudVy = " + d_dtSudVy);
1346 System.out.println("d_dtSudVz = " + d_dtSudVz);
1347
1348 System.out.println("d_dt2dPx = " + d_dt2dPx);
1349 System.out.println("d_dt2dPy = " + d_dt2dPy);
1350 System.out.println("d_dt2dPz = " + d_dt2dPz);
1351 System.out.println("d_dt2dVx = " + d_dt2dVx);
1352 System.out.println("d_dt2dVy = " + d_dt2dVy);
1353 System.out.println("d_dt2dVz = " + d_dt2dVz);
1354
1355 System.out.println("d_dtSddPx = " + d_dtSddPx);
1356 System.out.println("d_dtSddPy = " + d_dtSddPy);
1357 System.out.println("d_dtSddPz = " + d_dtSddPz);
1358 System.out.println("d_dtSddVx = " + d_dtSddVx);
1359 System.out.println("d_dtSddVy = " + d_dtSddVy);
1360 System.out.println("d_dtSddVz = " + d_dtSddVz);
1361
1362 System.out.println("d_dtMudPx = " + d_dtMudPx);
1363 System.out.println("d_dtMudPy = " + d_dtMudPy);
1364 System.out.println("d_dtMudPz = " + d_dtMudPz);
1365 System.out.println("d_dtMudVx = " + d_dtMudVx);
1366 System.out.println("d_dtMudVy = " + d_dtMudVy);
1367 System.out.println("d_dtMudVz = " + d_dtMudVz);
1368
1369 System.out.println("d_dt1dPx = " + d_dt1dPx);
1370 System.out.println("d_dt1dPy = " + d_dt1dPy);
1371 System.out.println("d_dt1dPz = " + d_dt1dPz);
1372 System.out.println("d_dt1dVx = " + d_dt1dVx);
1373 System.out.println("d_dt1dVy = " + d_dt1dVy);
1374 System.out.println("d_dt1dVz = " + d_dt1dVz);
1375
1376 System.out.println("d_dRdPx = " + d_dRdPx);
1377 System.out.println("d_dRdPy = " + d_dRdPy);
1378 System.out.println("d_dRdPz = " + d_dRdPz);
1379 System.out.println("d_dRdVx = " + d_dRdVx);
1380 System.out.println("d_dRdVy = " + d_dRdVy);
1381 System.out.println("d_dRdVz = " + d_dRdVz);
1382
1383 System.out.println("d_dtMddQMx = " + d_dtMddQMx);
1384 System.out.println("d_dtMddQMy = " + d_dtMddQMy);
1385 System.out.println("d_dtMddQMz = " + d_dtMddQMz);
1386 System.out.println("d_dtMddQSx = " + d_dtMddQSx);
1387 System.out.println("d_dtMddQSy = " + d_dtMddQSy);
1388 System.out.println("d_dtMddQSz = " + d_dtMddQSz);
1389
1390 System.out.println("d_dtSudQMx = " + d_dtSudQMx);
1391 System.out.println("d_dtSudQMy = " + d_dtSudQMy);
1392 System.out.println("d_dtSudQMz = " + d_dtSudQMz);
1393 System.out.println("d_dtSudQSx = " + d_dtSudQSx);
1394 System.out.println("d_dtSudQSy = " + d_dtSudQSy);
1395 System.out.println("d_dtSudQSz = " + d_dtSudQSz);
1396
1397 System.out.println("d_dt2dQMx = " + d_dt2dQMx);
1398 System.out.println("d_dt2dQMy = " + d_dt2dQMy);
1399 System.out.println("d_dt2dQMz = " + d_dt2dQMz);
1400 System.out.println("d_dt2dQSx = " + d_dt2dQSx);
1401 System.out.println("d_dt2dQSy = " + d_dt2dQSy);
1402 System.out.println("d_dt2dQSz = " + d_dt2dQSz);
1403
1404 System.out.println("d_dtSddQMx = " + d_dtSddQMx);
1405 System.out.println("d_dtSddQMy = " + d_dtSddQMy);
1406 System.out.println("d_dtSddQMz = " + d_dtSddQMz);
1407 System.out.println("d_dtSddQSx = " + d_dtSddQSx);
1408 System.out.println("d_dtSddQSy = " + d_dtSddQSy);
1409 System.out.println("d_dtSddQSz = " + d_dtSddQSz);
1410
1411 System.out.println("d_dtMudQMx = " + d_dtMudQMx);
1412 System.out.println("d_dtMudQMy = " + d_dtMudQMy);
1413 System.out.println("d_dtMudQMz = " + d_dtMudQMz);
1414 System.out.println("d_dtMudQSx = " + d_dtMudQSx);
1415 System.out.println("d_dtMudQSy = " + d_dtMudQSy);
1416 System.out.println("d_dtMudQSz = " + d_dtMudQSz);
1417
1418 System.out.println("d_dt1dQMx = " + d_dt1dQMx);
1419 System.out.println("d_dt1dQMy = " + d_dt1dQMy);
1420 System.out.println("d_dt1dQMz = " + d_dt1dQMz);
1421 System.out.println("d_dt1dQSx = " + d_dt1dQSx);
1422 System.out.println("d_dt1dQSy = " + d_dt1dQSy);
1423 System.out.println("d_dt1dQSz = " + d_dt1dQSz);
1424
1425 System.out.println("d_dRdQMx = " + d_dRdQMx);
1426 System.out.println("d_dRdQMy = " + d_dRdQMy);
1427 System.out.println("d_dRdQMz = " + d_dRdQMz);
1428 System.out.println("d_dRdQSx = " + d_dRdQSx);
1429 System.out.println("d_dRdQSy = " + d_dRdQSy);
1430 System.out.println("d_dRdQSz = " + d_dRdQSz);
1431
1432
1433 }
1434
1435
1436 return estimated;
1437
1438
1439 }
1440 }