List of usage examples for org.apache.commons.math3.geometry.euclidean.threed Rotation getQ2
public double getQ2()
From source file:fr.cs.examples.attitude.EarthObservation_day_night_switch_with_spinned_transitions.java
/** Program entry point. * @param args program arguments (unused here) *///from w ww.ja va 2s.c o m public static void main(String[] args) { try { // configure Orekit Autoconfiguration.configureOrekit(); final SortedSet<String> output = new TreeSet<String>(); //---------------------------------------- // Initial state definition : date, orbit //---------------------------------------- final AbsoluteDate initialDate = new AbsoluteDate(2004, 01, 02, 00, 00, 00.000, TimeScalesFactory.getUTC()); final Vector3D position = new Vector3D(-6142438.668, 3492467.560, -25767.25680); final Vector3D velocity = new Vector3D(505.8479685, 942.7809215, 7435.922231); final Orbit initialOrbit = new KeplerianOrbit(new PVCoordinates(position, velocity), FramesFactory.getEME2000(), initialDate, Constants.EIGEN5C_EARTH_MU); //------------------------------ // Attitudes sequence definition //------------------------------ final AttitudesSequence attitudesSequence = new AttitudesSequence(); // Attitude laws definition final double settingRate = FastMath.toRadians(1.0); final AttitudeProvider dayObservationLaw = new LofOffset(initialOrbit.getFrame(), LOFType.VVLH, RotationOrder.XYZ, FastMath.toRadians(20), FastMath.toRadians(40), 0); final AttitudeProvider nightRestingLaw = new LofOffset(initialOrbit.getFrame(), LOFType.VVLH); final AttitudeProvider transitionLaw = new LofOffset(initialOrbit.getFrame(), LOFType.VVLH, RotationOrder.XYZ, FastMath.toRadians(20), 0, 0); final AttitudeProvider rollSetUpLaw = new SpinStabilized(nightRestingLaw, AbsoluteDate.J2000_EPOCH, Vector3D.PLUS_I, settingRate); final AttitudeProvider pitchSetUpLaw = new SpinStabilized(transitionLaw, AbsoluteDate.J2000_EPOCH, Vector3D.PLUS_J, settingRate); final AttitudeProvider pitchTearDownLaw = new SpinStabilized(dayObservationLaw, AbsoluteDate.J2000_EPOCH, Vector3D.PLUS_J, -settingRate); final AttitudeProvider rollTearDownLaw = new SpinStabilized(transitionLaw, AbsoluteDate.J2000_EPOCH, Vector3D.PLUS_I, -settingRate); // Event detectors definition //--------------------------- final PVCoordinatesProvider sun = CelestialBodyFactory.getSun(); final PVCoordinatesProvider earth = CelestialBodyFactory.getEarth(); // Detectors : end day-night rdv 2 final DateDetector endDayNightRdV2Event_increase = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (increasing) { output.add(s.getDate() + ": switching to night law"); System.out.println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-day-night-2 night-mode"); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); final DateDetector endDayNightRdV2Event_decrease = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (!increasing) { output.add(s.getDate() + ": switching to night law"); System.out.println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-day-night-2 night-mode"); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); // Detectors : end day-night rdv 1 final DateDetector endDayNightRdV1Event_increase = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (increasing) { output.add(s.getDate() + ": switching to day-night rdv 2 law"); System.out .println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-day-night-1 day-night-rdv2-mode"); endDayNightRdV2Event_increase.addEventDate(s.getDate().shiftedBy(20)); endDayNightRdV2Event_decrease.addEventDate(s.getDate().shiftedBy(20)); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); final DateDetector endDayNightRdV1Event_decrease = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (!increasing) { output.add(s.getDate() + ": switching to day-night rdv 2 law"); System.out .println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-day-night-1 day-night-rdv2-mode"); endDayNightRdV2Event_increase.addEventDate(s.getDate().shiftedBy(20)); endDayNightRdV2Event_decrease.addEventDate(s.getDate().shiftedBy(20)); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); // Detector : eclipse entry final EventDetector dayNightEvent = new EclipseDetector(sun, 696000000., earth, Constants.WGS84_EARTH_EQUATORIAL_RADIUS).withHandler(new EventHandler<EclipseDetector>() { public Action eventOccurred(final SpacecraftState s, final EclipseDetector detector, final boolean increasing) { if (!increasing) { output.add(s.getDate() + ": switching to day-night rdv 1 law"); System.out .println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " eclipse-entry day-night-rdv1-mode"); endDayNightRdV1Event_increase.addEventDate(s.getDate().shiftedBy(40)); endDayNightRdV1Event_decrease.addEventDate(s.getDate().shiftedBy(40)); } return Action.CONTINUE; } public SpacecraftState resetState(EclipseDetector detector, SpacecraftState oldState) { return oldState; } }); // Detectors : end night-day rdv 2 final DateDetector endNightDayRdV2Event_increase = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (increasing) { output.add(s.getDate() + ": switching to day law"); System.out.println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-night-day-2 day-mode"); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); final DateDetector endNightDayRdV2Event_decrease = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (!increasing) { output.add(s.getDate() + ": switching to day law"); System.out.println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-night-day-2 day-mode"); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); // Detectors : end night-day rdv 1 final DateDetector endNightDayRdV1Event_increase = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (increasing) { output.add(s.getDate() + ": switching to night-day rdv 2 law"); System.out .println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-night-day-1 night-day-rdv2-mode"); endNightDayRdV2Event_increase.addEventDate(s.getDate().shiftedBy(40)); endNightDayRdV2Event_decrease.addEventDate(s.getDate().shiftedBy(40)); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); final DateDetector endNightDayRdV1Event_decrease = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (!increasing) { output.add(s.getDate() + ": switching to night-day rdv 2 law"); System.out .println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-night-day-1 night-day-rdv2-mode"); endNightDayRdV2Event_increase.addEventDate(s.getDate().shiftedBy(40)); endNightDayRdV2Event_decrease.addEventDate(s.getDate().shiftedBy(40)); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); // Detector : eclipse exit final EventDetector nightDayEvent = new EclipseDetector(sun, 696000000., earth, Constants.WGS84_EARTH_EQUATORIAL_RADIUS).withHandler(new EventHandler<EclipseDetector>() { public Action eventOccurred(final SpacecraftState s, final EclipseDetector detector, final boolean increasing) { if (increasing) { output.add(s.getDate() + ": switching to night-day rdv 1 law"); System.out .println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " eclipse-exit night-day-rdv1-mode"); endNightDayRdV1Event_increase.addEventDate(s.getDate().shiftedBy(20)); endNightDayRdV1Event_decrease.addEventDate(s.getDate().shiftedBy(20)); } return Action.CONTINUE; } public SpacecraftState resetState(EclipseDetector detector, SpacecraftState oldState) { return oldState; } }); // Attitude sequences definition //------------------------------ attitudesSequence.addSwitchingCondition(dayObservationLaw, dayNightEvent, false, true, pitchTearDownLaw); attitudesSequence.addSwitchingCondition(pitchTearDownLaw, endDayNightRdV1Event_increase, true, false, rollTearDownLaw); attitudesSequence.addSwitchingCondition(pitchTearDownLaw, endDayNightRdV1Event_decrease, false, true, rollTearDownLaw); attitudesSequence.addSwitchingCondition(rollTearDownLaw, endDayNightRdV2Event_increase, true, false, nightRestingLaw); attitudesSequence.addSwitchingCondition(rollTearDownLaw, endDayNightRdV2Event_decrease, false, true, nightRestingLaw); attitudesSequence.addSwitchingCondition(nightRestingLaw, nightDayEvent, true, false, rollSetUpLaw); attitudesSequence.addSwitchingCondition(rollSetUpLaw, endNightDayRdV1Event_increase, true, false, pitchSetUpLaw); attitudesSequence.addSwitchingCondition(rollSetUpLaw, endNightDayRdV1Event_decrease, false, true, pitchSetUpLaw); attitudesSequence.addSwitchingCondition(pitchSetUpLaw, endNightDayRdV2Event_increase, true, false, dayObservationLaw); attitudesSequence.addSwitchingCondition(pitchSetUpLaw, endNightDayRdV2Event_decrease, false, true, dayObservationLaw); // Initialisation //--------------- if (dayNightEvent.g(new SpacecraftState(initialOrbit)) >= 0) { // initial position is in daytime attitudesSequence.resetActiveProvider(dayObservationLaw); System.out .println("# " + (initialDate.durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " begin with day law"); } else { // initial position is in nighttime attitudesSequence.resetActiveProvider(nightRestingLaw); System.out .println("# " + (initialDate.durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " begin with night law"); } //---------------------- // Propagator definition //---------------------- // Propagator : consider the analytical Eckstein-Hechler model final Propagator propagator = new EcksteinHechlerPropagator(initialOrbit, attitudesSequence, Constants.EIGEN5C_EARTH_EQUATORIAL_RADIUS, Constants.EIGEN5C_EARTH_MU, Constants.EIGEN5C_EARTH_C20, Constants.EIGEN5C_EARTH_C30, Constants.EIGEN5C_EARTH_C40, Constants.EIGEN5C_EARTH_C50, Constants.EIGEN5C_EARTH_C60); // Register the switching events to the propagator attitudesSequence.registerSwitchEvents(propagator); propagator.setMasterMode(10.0, new OrekitFixedStepHandler() { private DecimalFormat f1 = new DecimalFormat("0.0000000000000000E00", new DecimalFormatSymbols(Locale.US)); private Vector3DFormat f2 = new Vector3DFormat(" ", " ", " ", f1); private PVCoordinatesProvider sun = CelestialBodyFactory.getSun(); private PVCoordinatesProvider moon = CelestialBodyFactory.getMoon(); private Frame eme2000 = FramesFactory.getEME2000(); private Frame itrf2005 = FramesFactory.getITRF(IERSConventions.IERS_2010, true); private String printVector3D(final String name, final Vector3D v) { return name + " " + f2.format(v); } private String printRotation(final String name, final Rotation r) { return name + " " + f1.format(r.getQ1()) + " " + f1.format(r.getQ2()) + " " + f1.format(r.getQ3()) + " " + f1.format(r.getQ0()); } private String printRotation2(final String name, final Rotation r) { return name + " " + f1.format(-r.getQ1()) + " " + f1.format(-r.getQ2()) + " " + f1.format(-r.getQ3()) + " " + f1.format(-r.getQ0()); } public void init(final SpacecraftState s0, final AbsoluteDate t) { } public void handleStep(SpacecraftState currentState, boolean isLast) throws PropagationException { try { // the Earth position in spacecraft should be along spacecraft Z axis // during nigthtime and away from it during daytime due to roll and pitch offsets final Vector3D earth = currentState.toTransform().transformPosition(Vector3D.ZERO); final double pointingOffset = Vector3D.angle(earth, Vector3D.PLUS_K); // the g function is the eclipse indicator, its an angle between Sun and Earth limb, // positive when Sun is outside of Earth limb, negative when Sun is hidden by Earth limb final double eclipseAngle = dayNightEvent.g(currentState); final double endNightDayTimer1 = endNightDayRdV1Event_decrease.g(currentState); final double endNightDayTimer2 = endNightDayRdV2Event_decrease.g(currentState); final double endDayNightTimer1 = endDayNightRdV1Event_decrease.g(currentState); final double endDayNightTimer2 = endDayNightRdV2Event_decrease.g(currentState); output.add(currentState.getDate() + " " + FastMath.toDegrees(eclipseAngle) + " " + endNightDayTimer1 + " " + endNightDayTimer2 + " " + endDayNightTimer1 + " " + endDayNightTimer2 + " " + FastMath.toDegrees(pointingOffset)); final AbsoluteDate date = currentState.getDate(); final PVCoordinates pv = currentState.getPVCoordinates(eme2000); final Rotation lvlhRot = new Rotation(pv.getPosition(), pv.getMomentum(), Vector3D.MINUS_K, Vector3D.MINUS_J); final Rotation earthRot = eme2000.getTransformTo(itrf2005, date).getRotation(); System.out.println("Scenario::setVectorMap 0x960b7e0 " + (date.durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " " + printVector3D("sun", sun.getPVCoordinates(date, eme2000).getPosition()) + " " + printVector3D("moon", moon.getPVCoordinates(date, eme2000).getPosition()) + " " + printVector3D("satPos", pv.getPosition()) + " " + printVector3D("satVel", pv.getVelocity()) + " " + printVector3D("orbMom", pv.getMomentum())); System.out.println("Scenario::setQuatMap 0x960b7e0 " + (date.durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " " + printRotation("earthFrame", earthRot) + " " + printRotation("LVLHFrame", lvlhRot)); System.out.println("Scenario::computeStep 0x960b7e0 " + (date.durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY)); System.out.println(" -> " + printRotation2("", currentState.getAttitude().getRotation()) + " " + printVector3D("", currentState.getAttitude().getSpin())); } catch (OrekitException oe) { throw new PropagationException(oe); } } }); //---------- // Propagate //---------- // Propagate from the initial date for the fixed duration propagator.propagate(initialDate.shiftedBy(1.75 * 3600.)); //-------------- // Print results //-------------- // we print the lines according to lexicographic order, which is chronological order here // to make sure out of orders calls between step handler and event handlers don't mess things up for (final String line : output) { System.out.println(line); } } catch (OrekitException oe) { System.err.println(oe.getMessage()); } }
From source file:fr.cs.examples.attitude.EarthObservation_day_night_switch_with_fixed_transitions.java
/** Program entry point. * @param args program arguments (unused here) *///w ww. ja v a 2s .co m public static void main(String[] args) { try { // configure Orekit Autoconfiguration.configureOrekit(); final SortedSet<String> output = new TreeSet<String>(); //---------------------------------------- // Initial state definition : date, orbit //---------------------------------------- final AbsoluteDate initialDate = new AbsoluteDate(2004, 01, 02, 00, 00, 00.000, TimeScalesFactory.getUTC()); final Vector3D position = new Vector3D(-6142438.668, 3492467.560, -25767.25680); final Vector3D velocity = new Vector3D(505.8479685, 942.7809215, 7435.922231); final Orbit initialOrbit = new KeplerianOrbit(new PVCoordinates(position, velocity), FramesFactory.getEME2000(), initialDate, Constants.EIGEN5C_EARTH_MU); //------------------------------ // Attitudes sequence definition //------------------------------ final AttitudesSequence attitudesSequence = new AttitudesSequence(); // Attitude laws definition //------------------------- // Mode : day final AttitudeProvider dayObservationLaw = new LofOffset(initialOrbit.getFrame(), LOFType.VVLH, RotationOrder.XYZ, FastMath.toRadians(20), FastMath.toRadians(40), 0); // Mode : night final AttitudeProvider nightRestingLaw = new LofOffset(initialOrbit.getFrame(), LOFType.VVLH); // Mode : day-night rdv 1 final AttitudeProvider dayNightRdV1Law = new LofOffset(initialOrbit.getFrame(), LOFType.VVLH, RotationOrder.XYZ, FastMath.toRadians(20), FastMath.toRadians(20), 0); // Mode : day-night rdv 2 final AttitudeProvider dayNightRdV2Law = new LofOffset(initialOrbit.getFrame(), LOFType.VVLH, RotationOrder.XYZ, FastMath.toRadians(20), 0, 0); // Mode : night-day rdv 1 final AttitudeProvider nightDayRdV1Law = new LofOffset(initialOrbit.getFrame(), LOFType.VVLH, RotationOrder.XYZ, FastMath.toRadians(20), 0, 0); // Mode : night-day rdv 2 final AttitudeProvider nightDayRdV2Law = new LofOffset(initialOrbit.getFrame(), LOFType.VVLH, RotationOrder.XYZ, FastMath.toRadians(20), FastMath.toRadians(20), 0); // Event detectors definition //--------------------------- final PVCoordinatesProvider sun = CelestialBodyFactory.getSun(); final PVCoordinatesProvider earth = CelestialBodyFactory.getEarth(); // Detectors : end day-night rdv 2 final DateDetector endDayNightRdV2Event_increase = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (increasing) { output.add(s.getDate() + ": switching to night law"); System.out.println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-day-night-2 night-mode"); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); final DateDetector endDayNightRdV2Event_decrease = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (!increasing) { output.add(s.getDate() + ": switching to night law"); System.out.println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-day-night-2 night-mode"); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); // Detectors : end day-night rdv 1 final DateDetector endDayNightRdV1Event_increase = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (increasing) { output.add(s.getDate() + ": switching to day-night rdv 2 law"); System.out .println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-day-night-1 day-night-rdv2-mode"); endDayNightRdV2Event_increase.addEventDate(s.getDate().shiftedBy(20)); endDayNightRdV2Event_decrease.addEventDate(s.getDate().shiftedBy(20)); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); final DateDetector endDayNightRdV1Event_decrease = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (!increasing) { output.add(s.getDate() + ": switching to day-night rdv 2 law"); System.out .println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-day-night-1 day-night-rdv2-mode"); endDayNightRdV2Event_increase.addEventDate(s.getDate().shiftedBy(20)); endDayNightRdV2Event_decrease.addEventDate(s.getDate().shiftedBy(20)); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); // Detector : eclipse entry final EventDetector dayNightEvent = new EclipseDetector(sun, 696000000., earth, Constants.WGS84_EARTH_EQUATORIAL_RADIUS).withHandler(new EventHandler<EclipseDetector>() { public Action eventOccurred(final SpacecraftState s, final EclipseDetector detector, final boolean increasing) { if (!increasing) { output.add(s.getDate() + ": switching to day-night rdv 1 law"); System.out .println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " eclipse-entry day-night-rdv1-mode"); endDayNightRdV1Event_increase.addEventDate(s.getDate().shiftedBy(40)); endDayNightRdV1Event_decrease.addEventDate(s.getDate().shiftedBy(40)); } return Action.CONTINUE; } public SpacecraftState resetState(EclipseDetector detector, SpacecraftState oldState) { return oldState; } }); // Detectors : end night-day rdv 2 final DateDetector endNightDayRdV2Event_increase = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (increasing) { output.add(s.getDate() + ": switching to day law"); System.out.println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-night-day-2 day-mode"); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); final DateDetector endNightDayRdV2Event_decrease = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (!increasing) { output.add(s.getDate() + ": switching to day law"); System.out.println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-night-day-2 day-mode"); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); // Detectors : end night-day rdv 1 final DateDetector endNightDayRdV1Event_increase = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (increasing) { output.add(s.getDate() + ": switching to night-day rdv 2 law"); System.out .println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-night-day-1 night-day-rdv2-mode"); endNightDayRdV2Event_increase.addEventDate(s.getDate().shiftedBy(40)); endNightDayRdV2Event_decrease.addEventDate(s.getDate().shiftedBy(40)); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); final DateDetector endNightDayRdV1Event_decrease = new DateDetector(10, 1e-04) .withHandler(new EventHandler<DateDetector>() { public Action eventOccurred(final SpacecraftState s, final DateDetector detector, final boolean increasing) { if (!increasing) { output.add(s.getDate() + ": switching to night-day rdv 2 law"); System.out .println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " end-night-day-1 night-day-rdv2-mode"); endNightDayRdV2Event_increase.addEventDate(s.getDate().shiftedBy(40)); endNightDayRdV2Event_decrease.addEventDate(s.getDate().shiftedBy(40)); } return Action.CONTINUE; } public SpacecraftState resetState(DateDetector detector, SpacecraftState oldState) { return oldState; } }); // Detector : eclipse exit final EventDetector nightDayEvent = new EclipseDetector(sun, 696000000., earth, Constants.WGS84_EARTH_EQUATORIAL_RADIUS).withHandler(new EventHandler<EclipseDetector>() { public Action eventOccurred(final SpacecraftState s, final EclipseDetector detector, final boolean increasing) { if (increasing) { output.add(s.getDate() + ": switching to night-day rdv 1 law"); System.out .println("# " + (s.getDate().durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " eclipse-exit night-day-rdv1-mode"); endNightDayRdV1Event_increase.addEventDate(s.getDate().shiftedBy(20)); endNightDayRdV1Event_decrease.addEventDate(s.getDate().shiftedBy(20)); } return Action.CONTINUE; } public SpacecraftState resetState(EclipseDetector detector, SpacecraftState oldState) { return oldState; } }); // Attitude sequences definition //------------------------------ attitudesSequence.addSwitchingCondition(dayObservationLaw, dayNightEvent, false, true, dayNightRdV1Law); attitudesSequence.addSwitchingCondition(dayNightRdV1Law, endDayNightRdV1Event_increase, true, false, dayNightRdV2Law); attitudesSequence.addSwitchingCondition(dayNightRdV1Law, endDayNightRdV1Event_decrease, false, true, dayNightRdV2Law); attitudesSequence.addSwitchingCondition(dayNightRdV2Law, endDayNightRdV2Event_increase, true, false, nightRestingLaw); attitudesSequence.addSwitchingCondition(dayNightRdV2Law, endDayNightRdV2Event_decrease, false, true, nightRestingLaw); attitudesSequence.addSwitchingCondition(nightRestingLaw, nightDayEvent, true, false, nightDayRdV1Law); attitudesSequence.addSwitchingCondition(nightDayRdV1Law, endNightDayRdV1Event_increase, true, false, nightDayRdV2Law); attitudesSequence.addSwitchingCondition(nightDayRdV1Law, endNightDayRdV1Event_decrease, false, true, nightDayRdV2Law); attitudesSequence.addSwitchingCondition(nightDayRdV2Law, endNightDayRdV2Event_increase, true, false, dayObservationLaw); attitudesSequence.addSwitchingCondition(nightDayRdV2Law, endNightDayRdV2Event_decrease, false, true, dayObservationLaw); // Initialisation //--------------- if (dayNightEvent.g(new SpacecraftState(initialOrbit)) >= 0) { // initial position is in daytime attitudesSequence.resetActiveProvider(dayObservationLaw); System.out .println("# " + (initialDate.durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " begin with day law"); } else { // initial position is in nighttime attitudesSequence.resetActiveProvider(nightRestingLaw); System.out .println("# " + (initialDate.durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " begin with night law"); } //---------------------- // Propagator definition //---------------------- // Propagator : consider the analytical Eckstein-Hechler model final Propagator propagator = new EcksteinHechlerPropagator(initialOrbit, attitudesSequence, Constants.EIGEN5C_EARTH_EQUATORIAL_RADIUS, Constants.EIGEN5C_EARTH_MU, Constants.EIGEN5C_EARTH_C20, Constants.EIGEN5C_EARTH_C30, Constants.EIGEN5C_EARTH_C40, Constants.EIGEN5C_EARTH_C50, Constants.EIGEN5C_EARTH_C60); // Register the switching events to the propagator attitudesSequence.registerSwitchEvents(propagator); propagator.setMasterMode(10.0, new OrekitFixedStepHandler() { private DecimalFormat f1 = new DecimalFormat("0.0000000000000000E00", new DecimalFormatSymbols(Locale.US)); private Vector3DFormat f2 = new Vector3DFormat(" ", " ", " ", f1); private PVCoordinatesProvider sun = CelestialBodyFactory.getSun(); private PVCoordinatesProvider moon = CelestialBodyFactory.getMoon(); private Frame eme2000 = FramesFactory.getEME2000(); private Frame itrf2005 = FramesFactory.getITRF(IERSConventions.IERS_2010, true); private String printVector3D(final String name, final Vector3D v) { return name + " " + f2.format(v); } private String printRotation(final String name, final Rotation r) { return name + " " + f1.format(r.getQ1()) + " " + f1.format(r.getQ2()) + " " + f1.format(r.getQ3()) + " " + f1.format(r.getQ0()); } private String printRotation2(final String name, final Rotation r) { return name + " " + f1.format(-r.getQ1()) + " " + f1.format(-r.getQ2()) + " " + f1.format(-r.getQ3()) + " " + f1.format(-r.getQ0()); } public void init(final SpacecraftState s0, final AbsoluteDate t) { } public void handleStep(SpacecraftState currentState, boolean isLast) throws PropagationException { try { // the Earth position in spacecraft should be along spacecraft Z axis // during nigthtime and away from it during daytime due to roll and pitch offsets final Vector3D earth = currentState.toTransform().transformPosition(Vector3D.ZERO); final double pointingOffset = Vector3D.angle(earth, Vector3D.PLUS_K); // the g function is the eclipse indicator, its an angle between Sun and Earth limb, // positive when Sun is outside of Earth limb, negative when Sun is hidden by Earth limb final double eclipseAngle = dayNightEvent.g(currentState); final double endNightDayTimer1 = endNightDayRdV1Event_decrease.g(currentState); final double endNightDayTimer2 = endNightDayRdV2Event_decrease.g(currentState); final double endDayNightTimer1 = endDayNightRdV1Event_decrease.g(currentState); final double endDayNightTimer2 = endDayNightRdV2Event_decrease.g(currentState); output.add(currentState.getDate() + " " + FastMath.toDegrees(eclipseAngle) + " " + endNightDayTimer1 + " " + endNightDayTimer2 + " " + endDayNightTimer1 + " " + endDayNightTimer2 + " " + FastMath.toDegrees(pointingOffset)); final AbsoluteDate date = currentState.getDate(); final PVCoordinates pv = currentState.getPVCoordinates(eme2000); final Rotation lvlhRot = new Rotation(pv.getPosition(), pv.getMomentum(), Vector3D.MINUS_K, Vector3D.MINUS_J); final Rotation earthRot = eme2000.getTransformTo(itrf2005, date).getRotation(); System.out.println("Scenario::setVectorMap 0x960b7e0 " + (date.durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " " + printVector3D("sun", sun.getPVCoordinates(date, eme2000).getPosition()) + " " + printVector3D("moon", moon.getPVCoordinates(date, eme2000).getPosition()) + " " + printVector3D("satPos", pv.getPosition()) + " " + printVector3D("satVel", pv.getVelocity()) + " " + printVector3D("orbMom", pv.getMomentum())); System.out.println("Scenario::setQuatMap 0x960b7e0 " + (date.durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY) + " " + printRotation("earthFrame", earthRot) + " " + printRotation("LVLHFrame", lvlhRot)); System.out.println("Scenario::computeStep 0x960b7e0 " + (date.durationFrom(AbsoluteDate.J2000_EPOCH) / Constants.JULIAN_DAY)); System.out.println(" -> " + printRotation2("", currentState.getAttitude().getRotation()) + " " + printVector3D("", currentState.getAttitude().getSpin())); } catch (OrekitException oe) { throw new PropagationException(oe); } } }); //---------- // Propagate //---------- // Propagate from the initial date for the fixed duration propagator.propagate(initialDate.shiftedBy(1.75 * 3600.)); //-------------- // Print results //-------------- // we print the lines according to lexicographic order, which is chronological order here // to make sure out of orders calls between step handler and event handlers don't mess things up for (final String line : output) { System.out.println(line); } } catch (OrekitException oe) { System.err.println(oe.getMessage()); } }
From source file:nova.core.util.math.RotationUtil.java
/** * Returns the slerp interpolation of Rotations {@code a} and {@code b}, at * time {@code t}./*from w w w . j a v a 2s. c o m*/ * <p> * {@code t} should range in {@code [0,1]}. Result is a when {@code t=0 } and * {@code b} when {@code t=1}. * <p> * When {@code allowFlip} is true (default) the slerp interpolation will * always use the "shortest path" between the Rotations' orientations, by * "flipping" the source Rotation if needed. * @param a the first Rotation * @param b the second Rotation * @param t the t interpolation parameter * @param allowFlip tells whether or not the interpolation allows axis flip */ public static Rotation slerp(Rotation a, Rotation b, double t, boolean allowFlip) { // Warning: this method should not normalize the Rotation double cosAngle = dotProduct(a, b); double c1, c2; // Linear interpolation for close orientations if ((1.0 - FastMath.abs(cosAngle)) < 0.01) { c1 = 1.0f - t; c2 = t; } else { // Spherical interpolation double angle = FastMath.acos(FastMath.abs(cosAngle)); double sinAngle = FastMath.sin(angle); c1 = FastMath.sin(angle * (1.0f - t)) / sinAngle; c2 = FastMath.sin(angle * t) / sinAngle; } // Use the shortest path if (allowFlip && (cosAngle < 0.0)) { c1 = -c1; } return new Rotation(c1 * a.getQ1() + c2 * b.getQ1(), c1 * a.getQ2() + c2 * b.getQ2(), c1 * a.getQ3() + c2 * b.getQ3(), c1 * a.getQ0() + c2 * b.getQ0(), false); }
From source file:nova.core.util.math.RotationUtil.java
/** * Returns the "dot" product of this Quaternion and {@code b}: * <p>//from w w w .ja v a 2s .co m * {@code this.x * b.x + this.y * b.y + this.z * b.z + this.w * b.w} * @param b the Quaternion */ public static double dotProduct(Rotation a, Rotation b) { return a.getQ0() * b.getQ0() + a.getQ1() * b.getQ1() + a.getQ2() * b.getQ2() + a.getQ3() * b.getQ3(); }
From source file:org.orekit.attitudes.BodyCenterPointingTest.java
@Test public void testQDot() throws OrekitException { Utils.setDataRoot("regular-data"); final double ehMu = 3.9860047e14; final double ae = 6.378137e6; final double c20 = -1.08263e-3; final double c30 = 2.54e-6; final double c40 = 1.62e-6; final double c50 = 2.3e-7; final double c60 = -5.5e-7; final AbsoluteDate date = AbsoluteDate.J2000_EPOCH.shiftedBy(584.); final Vector3D position = new Vector3D(3220103., 69623., 6449822.); final Vector3D velocity = new Vector3D(6414.7, -2006., -3180.); final CircularOrbit initialOrbit = new CircularOrbit(new PVCoordinates(position, velocity), FramesFactory.getEME2000(), date, ehMu); EcksteinHechlerPropagator propagator = new EcksteinHechlerPropagator(initialOrbit, ae, ehMu, c20, c30, c40, c50, c60);//from www . j ava2s .c o m propagator.setAttitudeProvider(earthCenterAttitudeLaw); List<WeightedObservedPoint> w0 = new ArrayList<WeightedObservedPoint>(); List<WeightedObservedPoint> w1 = new ArrayList<WeightedObservedPoint>(); List<WeightedObservedPoint> w2 = new ArrayList<WeightedObservedPoint>(); List<WeightedObservedPoint> w3 = new ArrayList<WeightedObservedPoint>(); for (double dt = -1; dt < 1; dt += 0.01) { Rotation rP = propagator.propagate(date.shiftedBy(dt)).getAttitude().getRotation(); w0.add(new WeightedObservedPoint(1, dt, rP.getQ0())); w1.add(new WeightedObservedPoint(1, dt, rP.getQ1())); w2.add(new WeightedObservedPoint(1, dt, rP.getQ2())); w3.add(new WeightedObservedPoint(1, dt, rP.getQ3())); } double q0DotRef = PolynomialCurveFitter.create(2).fit(w0)[1]; double q1DotRef = PolynomialCurveFitter.create(2).fit(w1)[1]; double q2DotRef = PolynomialCurveFitter.create(2).fit(w2)[1]; double q3DotRef = PolynomialCurveFitter.create(2).fit(w3)[1]; Attitude a0 = propagator.propagate(date).getAttitude(); double q0 = a0.getRotation().getQ0(); double q1 = a0.getRotation().getQ1(); double q2 = a0.getRotation().getQ2(); double q3 = a0.getRotation().getQ3(); double oX = a0.getSpin().getX(); double oY = a0.getSpin().getY(); double oZ = a0.getSpin().getZ(); // first time-derivatives of the quaternion double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ); double q1Dot = 0.5 * MathArrays.linearCombination(q0, oX, -q3, oY, q2, oZ); double q2Dot = 0.5 * MathArrays.linearCombination(q3, oX, q0, oY, -q1, oZ); double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX, q1, oY, q0, oZ); Assert.assertEquals(q0DotRef, q0Dot, 5.0e-9); Assert.assertEquals(q1DotRef, q1Dot, 5.0e-9); Assert.assertEquals(q2DotRef, q2Dot, 5.0e-9); Assert.assertEquals(q3DotRef, q3Dot, 5.0e-9); }
From source file:org.orekit.frames.TransformTest.java
@Test public void testShiftDerivatives() { RandomGenerator random = new Well19937a(0x5acda4f605aadce7l); for (int i = 0; i < 10; ++i) { Transform t = randomTransform(random); for (double dt = -10.0; dt < 10.0; dt += 0.125) { Transform t0 = t.shiftedBy(dt); double v = t0.getVelocity().getNorm(); double a = t0.getAcceleration().getNorm(); double omega = t0.getRotationRate().getNorm(); double omegaDot = t0.getRotationAcceleration().getNorm(); // numerical derivatives double h = 0.01 / omega; Transform tm4h = t.shiftedBy(dt - 4 * h); Transform tm3h = t.shiftedBy(dt - 3 * h); Transform tm2h = t.shiftedBy(dt - 2 * h); Transform tm1h = t.shiftedBy(dt - 1 * h); Transform tp1h = t.shiftedBy(dt + 1 * h); Transform tp2h = t.shiftedBy(dt + 2 * h); Transform tp3h = t.shiftedBy(dt + 3 * h); Transform tp4h = t.shiftedBy(dt + 4 * h); double numXDot = derivative(h, tm4h.getTranslation().getX(), tm3h.getTranslation().getX(), tm2h.getTranslation().getX(), tm1h.getTranslation().getX(), tp1h.getTranslation().getX(), tp2h.getTranslation().getX(), tp3h.getTranslation().getX(), tp4h.getTranslation().getX()); double numYDot = derivative(h, tm4h.getTranslation().getY(), tm3h.getTranslation().getY(), tm2h.getTranslation().getY(), tm1h.getTranslation().getY(), tp1h.getTranslation().getY(), tp2h.getTranslation().getY(), tp3h.getTranslation().getY(), tp4h.getTranslation().getY()); double numZDot = derivative(h, tm4h.getTranslation().getZ(), tm3h.getTranslation().getZ(), tm2h.getTranslation().getZ(), tm1h.getTranslation().getZ(), tp1h.getTranslation().getZ(), tp2h.getTranslation().getZ(), tp3h.getTranslation().getZ(), tp4h.getTranslation().getZ()); double numXDot2 = derivative(h, tm4h.getVelocity().getX(), tm3h.getVelocity().getX(), tm2h.getVelocity().getX(), tm1h.getVelocity().getX(), tp1h.getVelocity().getX(), tp2h.getVelocity().getX(), tp3h.getVelocity().getX(), tp4h.getVelocity().getX()); double numYDot2 = derivative(h, tm4h.getVelocity().getY(), tm3h.getVelocity().getY(), tm2h.getVelocity().getY(), tm1h.getVelocity().getY(), tp1h.getVelocity().getY(), tp2h.getVelocity().getY(), tp3h.getVelocity().getY(), tp4h.getVelocity().getY()); double numZDot2 = derivative(h, tm4h.getVelocity().getZ(), tm3h.getVelocity().getZ(), tm2h.getVelocity().getZ(), tm1h.getVelocity().getZ(), tp1h.getVelocity().getZ(), tp2h.getVelocity().getZ(), tp3h.getVelocity().getZ(), tp4h.getVelocity().getZ()); double numQ0Dot = derivative(h, tm4h.getRotation().getQ0(), tm3h.getRotation().getQ0(), tm2h.getRotation().getQ0(), tm1h.getRotation().getQ0(), tp1h.getRotation().getQ0(), tp2h.getRotation().getQ0(), tp3h.getRotation().getQ0(), tp4h.getRotation().getQ0()); double numQ1Dot = derivative(h, tm4h.getRotation().getQ1(), tm3h.getRotation().getQ1(), tm2h.getRotation().getQ1(), tm1h.getRotation().getQ1(), tp1h.getRotation().getQ1(), tp2h.getRotation().getQ1(), tp3h.getRotation().getQ1(), tp4h.getRotation().getQ1()); double numQ2Dot = derivative(h, tm4h.getRotation().getQ2(), tm3h.getRotation().getQ2(), tm2h.getRotation().getQ2(), tm1h.getRotation().getQ2(), tp1h.getRotation().getQ2(), tp2h.getRotation().getQ2(), tp3h.getRotation().getQ2(), tp4h.getRotation().getQ2()); double numQ3Dot = derivative(h, tm4h.getRotation().getQ3(), tm3h.getRotation().getQ3(), tm2h.getRotation().getQ3(), tm1h.getRotation().getQ3(), tp1h.getRotation().getQ3(), tp2h.getRotation().getQ3(), tp3h.getRotation().getQ3(), tp4h.getRotation().getQ3()); double numOxDot = derivative(h, tm4h.getRotationRate().getX(), tm3h.getRotationRate().getX(), tm2h.getRotationRate().getX(), tm1h.getRotationRate().getX(), tp1h.getRotationRate().getX(), tp2h.getRotationRate().getX(), tp3h.getRotationRate().getX(), tp4h.getRotationRate().getX()); double numOyDot = derivative(h, tm4h.getRotationRate().getY(), tm3h.getRotationRate().getY(), tm2h.getRotationRate().getY(), tm1h.getRotationRate().getY(), tp1h.getRotationRate().getY(), tp2h.getRotationRate().getY(), tp3h.getRotationRate().getY(), tp4h.getRotationRate().getY()); double numOzDot = derivative(h, tm4h.getRotationRate().getZ(), tm3h.getRotationRate().getZ(), tm2h.getRotationRate().getZ(), tm1h.getRotationRate().getZ(), tp1h.getRotationRate().getZ(), tp2h.getRotationRate().getZ(), tp3h.getRotationRate().getZ(), tp4h.getRotationRate().getZ()); // theoretical derivatives double theXDot = t0.getVelocity().getX(); double theYDot = t0.getVelocity().getY(); double theZDot = t0.getVelocity().getZ(); double theXDot2 = t0.getAcceleration().getX(); double theYDot2 = t0.getAcceleration().getY(); double theZDot2 = t0.getAcceleration().getZ(); Rotation r0 = t0.getRotation(); Vector3D w = t0.getRotationRate(); Vector3D q = new Vector3D(r0.getQ1(), r0.getQ2(), r0.getQ3()); Vector3D qw = Vector3D.crossProduct(q, w); double theQ0Dot = -0.5 * Vector3D.dotProduct(q, w); double theQ1Dot = 0.5 * (r0.getQ0() * w.getX() + qw.getX()); double theQ2Dot = 0.5 * (r0.getQ0() * w.getY() + qw.getY()); double theQ3Dot = 0.5 * (r0.getQ0() * w.getZ() + qw.getZ()); double theOxDot2 = t0.getRotationAcceleration().getX(); double theOyDot2 = t0.getRotationAcceleration().getY(); double theOzDot2 = t0.getRotationAcceleration().getZ(); // check consistency Assert.assertEquals(theXDot, numXDot, 1.0e-13 * v); Assert.assertEquals(theYDot, numYDot, 1.0e-13 * v); Assert.assertEquals(theZDot, numZDot, 1.0e-13 * v); Assert.assertEquals(theXDot2, numXDot2, 1.0e-13 * a); Assert.assertEquals(theYDot2, numYDot2, 1.0e-13 * a); Assert.assertEquals(theZDot2, numZDot2, 1.0e-13 * a); Assert.assertEquals(theQ0Dot, numQ0Dot, 1.0e-13 * omega); Assert.assertEquals(theQ1Dot, numQ1Dot, 1.0e-13 * omega); Assert.assertEquals(theQ2Dot, numQ2Dot, 1.0e-13 * omega); Assert.assertEquals(theQ3Dot, numQ3Dot, 1.0e-13 * omega); Assert.assertEquals(theOxDot2, numOxDot, 1.0e-12 * omegaDot); Assert.assertEquals(theOyDot2, numOyDot, 1.0e-12 * omegaDot); Assert.assertEquals(theOzDot2, numOzDot, 1.0e-12 * omegaDot); }/* www . j a va 2 s . c o m*/ } }
From source file:org.orekit.propagation.numerical.PartialDerivativesEquations.java
/** {@inheritDoc} */ public double[] computeDerivatives(final SpacecraftState s, final double[] pDot) throws OrekitException { final int dim = 3; // Lazy initialization if (dirty) {//from ww w . j a v a 2s .co m // if step has not been set by user, set a default value if (Double.isNaN(hPos)) { hPos = FastMath.sqrt(Precision.EPSILON) * s.getPVCoordinates().getPosition().getNorm(); } // set up derivatives providers derivativesProviders.clear(); derivativesProviders.addAll(propagator.getForceModels()); derivativesProviders.add(propagator.getNewtonianAttractionForceModel()); // check all parameters are handled by at least one Jacobian provider for (final ParameterConfiguration param : selectedParameters) { final String parameterName = param.getParameterName(); boolean found = false; for (final ForceModel provider : derivativesProviders) { if (provider.isSupported(parameterName)) { param.setProvider(provider); found = true; } } if (!found) { // build the list of supported parameters, avoiding duplication final SortedSet<String> set = new TreeSet<String>(); for (final ForceModel provider : derivativesProviders) { for (final String forceModelParameter : provider.getParametersNames()) { set.add(forceModelParameter); } } final StringBuilder builder = new StringBuilder(); for (final String forceModelParameter : set) { if (builder.length() > 0) { builder.append(", "); } builder.append(forceModelParameter); } throw new OrekitException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME, parameterName, builder.toString()); } } // check the numbers of parameters and matrix size agree if (selectedParameters.size() != paramDim) { throw new OrekitException(OrekitMessages.INITIAL_MATRIX_AND_PARAMETERS_NUMBER_MISMATCH, paramDim, selectedParameters.size()); } dAccdParam = new double[dim]; dAccdPos = new double[dim][dim]; dAccdVel = new double[dim][dim]; dAccdM = (stateDim > 6) ? new double[dim] : null; dirty = false; } // initialize acceleration Jacobians to zero for (final double[] row : dAccdPos) { Arrays.fill(row, 0.0); } for (final double[] row : dAccdVel) { Arrays.fill(row, 0.0); } if (dAccdM != null) { Arrays.fill(dAccdM, 0.0); } // prepare derivation variables, 3 for position, 3 for velocity and optionally 1 for mass final int nbVars = (dAccdM == null) ? 6 : 7; // position corresponds three free parameters final Vector3D position = s.getPVCoordinates().getPosition(); final FieldVector3D<DerivativeStructure> dsP = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(nbVars, 1, 0, position.getX()), new DerivativeStructure(nbVars, 1, 1, position.getY()), new DerivativeStructure(nbVars, 1, 2, position.getZ())); // velocity corresponds three free parameters final Vector3D velocity = s.getPVCoordinates().getVelocity(); final FieldVector3D<DerivativeStructure> dsV = new FieldVector3D<DerivativeStructure>( new DerivativeStructure(nbVars, 1, 3, velocity.getX()), new DerivativeStructure(nbVars, 1, 4, velocity.getY()), new DerivativeStructure(nbVars, 1, 5, velocity.getZ())); // mass corresponds either to a constant or to one free parameter final DerivativeStructure dsM = (dAccdM == null) ? new DerivativeStructure(nbVars, 1, s.getMass()) : new DerivativeStructure(nbVars, 1, 6, s.getMass()); // we should compute attitude partial derivatives with respect to position/velocity // see issue #200 final Rotation rotation = s.getAttitude().getRotation(); final FieldRotation<DerivativeStructure> dsR = new FieldRotation<DerivativeStructure>( new DerivativeStructure(nbVars, 1, rotation.getQ0()), new DerivativeStructure(nbVars, 1, rotation.getQ1()), new DerivativeStructure(nbVars, 1, rotation.getQ2()), new DerivativeStructure(nbVars, 1, rotation.getQ3()), false); // compute acceleration Jacobians for (final ForceModel derivativesProvider : derivativesProviders) { final FieldVector3D<DerivativeStructure> acceleration = derivativesProvider .accelerationDerivatives(s.getDate(), s.getFrame(), dsP, dsV, dsR, dsM); addToRow(acceleration.getX(), 0); addToRow(acceleration.getY(), 1); addToRow(acceleration.getZ(), 2); } // the variational equations of the complete state Jacobian matrix have the // following form for 7x7, i.e. when mass partial derivatives are also considered // (when mass is not considered, only the A, B, D and E matrices are used along // with their derivatives): // [ | | ] [ | | ] [ | | ] // [ Adot | Bdot | Cdot ] [ dVel/dPos = 0 | dVel/dVel = Id | dVel/dm = 0 ] [ A | B | C ] // [ | | ] [ | | ] [ | | ] // --------+--------+--- ---- ------------------+------------------+---------------- -----+-----+----- // [ | | ] [ | | ] [ | | ] // [ Ddot | Edot | Fdot ] = [ dAcc/dPos | dAcc/dVel | dAcc/dm ] * [ D | E | F ] // [ | | ] [ | | ] [ | | ] // --------+--------+--- ---- ------------------+------------------+---------------- -----+-----+----- // [ Gdot | Hdot | Idot ] [ dmDot/dPos = 0 | dmDot/dVel = 0 | dmDot/dm = 0 ] [ G | H | I ] // The A, B, D and E sub-matrices and their derivatives (Adot ...) are 3x3 matrices, // the C and F sub-matrices and their derivatives (Cdot ...) are 3x1 matrices, // the G and H sub-matrices and their derivatives (Gdot ...) are 1x3 matrices, // the I sub-matrix and its derivative (Idot) is a 1x1 matrix. // The expanded multiplication above can be rewritten to take into account // the fixed values found in the sub-matrices in the left factor. This leads to: // [ Adot ] = [ D ] // [ Bdot ] = [ E ] // [ Cdot ] = [ F ] // [ Ddot ] = [ dAcc/dPos ] * [ A ] + [ dAcc/dVel ] * [ D ] + [ dAcc/dm ] * [ G ] // [ Edot ] = [ dAcc/dPos ] * [ B ] + [ dAcc/dVel ] * [ E ] + [ dAcc/dm ] * [ H ] // [ Fdot ] = [ dAcc/dPos ] * [ C ] + [ dAcc/dVel ] * [ F ] + [ dAcc/dm ] * [ I ] // [ Gdot ] = [ 0 ] // [ Hdot ] = [ 0 ] // [ Idot ] = [ 0 ] // The following loops compute these expressions taking care of the mapping of the // (A, B, ... I) matrices into the single dimension array p and of the mapping of the // (Adot, Bdot, ... Idot) matrices into the single dimension array pDot. // copy D, E and F into Adot, Bdot and Cdot final double[] p = s.getAdditionalState(getName()); System.arraycopy(p, dim * stateDim, pDot, 0, dim * stateDim); // compute Ddot, Edot and Fdot for (int i = 0; i < dim; ++i) { final double[] dAdPi = dAccdPos[i]; final double[] dAdVi = dAccdVel[i]; for (int j = 0; j < stateDim; ++j) { pDot[(dim + i) * stateDim + j] = dAdPi[0] * p[j] + dAdPi[1] * p[j + stateDim] + dAdPi[2] * p[j + 2 * stateDim] + dAdVi[0] * p[j + 3 * stateDim] + dAdVi[1] * p[j + 4 * stateDim] + dAdVi[2] * p[j + 5 * stateDim] + ((dAccdM == null) ? 0.0 : dAccdM[i] * p[j + 6 * stateDim]); } } if (dAccdM != null) { // set Gdot, Hdot and Idot to 0 Arrays.fill(pDot, 6 * stateDim, 7 * stateDim, 0.0); } for (int k = 0; k < paramDim; ++k) { // compute the acceleration gradient with respect to current parameter final ParameterConfiguration param = selectedParameters.get(k); final ForceModel provider = param.getProvider(); final FieldVector3D<DerivativeStructure> accDer = provider.accelerationDerivatives(s, param.getParameterName()); dAccdParam[0] = accDer.getX().getPartialDerivative(1); dAccdParam[1] = accDer.getY().getPartialDerivative(1); dAccdParam[2] = accDer.getZ().getPartialDerivative(1); // the variational equations of the parameters Jacobian matrix are computed // one column at a time, they have the following form: // [ ] [ | | ] [ ] [ ] // [ Jdot ] [ dVel/dPos = 0 | dVel/dVel = Id | dVel/dm = 0 ] [ J ] [ dVel/dParam = 0 ] // [ ] [ | | ] [ ] [ ] // -------- ------------------+------------------+---------------- ----- -------------------- // [ ] [ | | ] [ ] [ ] // [ Kdot ] = [ dAcc/dPos | dAcc/dVel | dAcc/dm ] * [ K ] + [ dAcc/dParam ] // [ ] [ | | ] [ ] [ ] // -------- ------------------+------------------+---------------- ----- -------------------- // [ Ldot ] [ dmDot/dPos = 0 | dmDot/dVel = 0 | dmDot/dm = 0 ] [ L ] [ dmDot/dParam = 0 ] // The J and K sub-columns and their derivatives (Jdot ...) are 3 elements columns, // the L sub-colums and its derivative (Ldot) are 1 elements columns. // The expanded multiplication and addition above can be rewritten to take into // account the fixed values found in the sub-matrices in the left factor. This leads to: // [ Jdot ] = [ K ] // [ Kdot ] = [ dAcc/dPos ] * [ J ] + [ dAcc/dVel ] * [ K ] + [ dAcc/dm ] * [ L ] + [ dAcc/dParam ] // [ Ldot ] = [ 0 ] // The following loops compute these expressions taking care of the mapping of the // (J, K, L) columns into the single dimension array p and of the mapping of the // (Jdot, Kdot, Ldot) columns into the single dimension array pDot. // copy K into Jdot final int columnTop = stateDim * stateDim + k; pDot[columnTop] = p[columnTop + 3 * paramDim]; pDot[columnTop + paramDim] = p[columnTop + 4 * paramDim]; pDot[columnTop + 2 * paramDim] = p[columnTop + 5 * paramDim]; // compute Kdot for (int i = 0; i < dim; ++i) { final double[] dAdPi = dAccdPos[i]; final double[] dAdVi = dAccdVel[i]; pDot[columnTop + (dim + i) * paramDim] = dAccdParam[i] + dAdPi[0] * p[columnTop] + dAdPi[1] * p[columnTop + paramDim] + dAdPi[2] * p[columnTop + 2 * paramDim] + dAdVi[0] * p[columnTop + 3 * paramDim] + dAdVi[1] * p[columnTop + 4 * paramDim] + dAdVi[2] * p[columnTop + 5 * paramDim] + ((dAccdM == null) ? 0.0 : dAccdM[i] * p[columnTop + 6 * paramDim]); } if (dAccdM != null) { // set Ldot to 0 pDot[columnTop + 6 * paramDim] = 0; } } // these equations have no effect of the main state itself return null; }