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