From ba4f31b264a351b026d6910b182b3a742193da05 Mon Sep 17 00:00:00 2001
From: Jonathan Guinet <jonathan.guinet@c-s.fr>
Date: Fri, 28 Oct 2016 11:39:01 -0400
Subject: [PATCH] add attitude law
---
.../orekit/rugged/los/FixedZHomothety.java | 2 +-
.../java/AffinagePleiades/AffinageRugged.java | 48 +++-
.../java/AffinagePleiades/OrbitModel.java | 271 ++++++++++++------
3 files changed, 225 insertions(+), 96 deletions(-)
diff --git a/src/main/java/org/orekit/rugged/los/FixedZHomothety.java b/src/main/java/org/orekit/rugged/los/FixedZHomothety.java
index 3c0dbeda..f1eb3db9 100644
--- a/src/main/java/org/orekit/rugged/los/FixedZHomothety.java
+++ b/src/main/java/org/orekit/rugged/los/FixedZHomothety.java
@@ -57,7 +57,7 @@ public class FixedZHomothety implements TimeIndependentLOSTransform {
* The single parameter is the homothety factor.
* </p>
* @param name name of the homothety (used for estimated parameters identification)
- * @param factor homothety factor
+ * @param factorvalue homothety factor
*/
public FixedZHomothety(final String name, final double factorvalue) {
this.factor = factorvalue;
diff --git a/src/tutorials/java/AffinagePleiades/AffinageRugged.java b/src/tutorials/java/AffinagePleiades/AffinageRugged.java
index e1485707..f1cb304f 100644
--- a/src/tutorials/java/AffinagePleiades/AffinageRugged.java
+++ b/src/tutorials/java/AffinagePleiades/AffinageRugged.java
@@ -22,8 +22,10 @@ import org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresOptimizer.
import java.io.File;
import java.util.Locale;
import java.util.Collections;
-
-
+import java.io.FileWriter;
+import java.io.StringWriter;
+import java.io.PrintWriter;
+import java.io.IOException;
import org.orekit.bodies.BodyShape;
import org.orekit.bodies.GeodeticPoint;
@@ -51,8 +53,9 @@ import org.orekit.utils.PVCoordinates;
/**
- * Parameter estimation context
+ * Parameter refining context
* @author Jonathan Guinet
+ * @author Lucie Labatallee
*
*/
public class AffinageRugged {
@@ -81,6 +84,11 @@ public class AffinageRugged {
Orbit orbit = orbitmodel.createOrbit(gravityField.getMu(), refDate);
+ final double [] rollPoly = {0.0,0.0,0.0};
+ double[] pitchPoly = {0.0,0.0};
+ double[] yawPoly = {0.0,0.0,0.0};
+ orbitmodel.setLOFTransform(rollPoly, pitchPoly, yawPoly, minDate);
+
PVCoordinates PV = orbit.getPVCoordinates(earth.getBodyFrame());
GeodeticPoint gp = earth.transform(PV.getPosition(), earth.getBodyFrame(), orbit.getDate());
@@ -131,9 +139,31 @@ public class AffinageRugged {
System.out.format(Locale.US, "center geodetic position : φ = %8.10f °, λ = %8.10f °, h = %8.3f m%n",
FastMath.toDegrees(centerPoint.getLatitude()),
FastMath.toDegrees(centerPoint.getLongitude()),centerPoint.getAltitude());
+
+ /*try {
+ FileWriter out = new FileWriter("geodetic_position.txt");
+ out.write("LATITUDE, LONGITUDE, LINE, PIX \n");
-
+ for (double line = 0; line < pleiadesViewingModel.dimension; line += 1000) {
+
+ AbsoluteDate date = lineSensor.getDate(line);
+ for (int pixel = 0; pixel < lineSensor.getNbPixels(); pixel += 1000) {
+ GeodeticPoint gp2 = rugged.directLocation(date, lineSensor.getPosition(),
+ lineSensor.getLOS(date, pixel));
+ String result = String.format("%8.10f, %8.10f, %2.1f, %2.1f \n ",FastMath.toDegrees(gp2.getLatitude()),
+ FastMath.toDegrees(gp2.getLongitude()), line,(double) pixel);
+ //System.out.println(result);
+ out.write(result);
+ }
+ }
+ out.close();
+ }
+ catch (IOException e) {
+ System.err.println(e.getLocalizedMessage());
+ System.exit(1);
+ }*/
+
int pixelPosition = pleiadesViewingModel.dimension-1;
los = lineSensor.getLOS(firstLineDate,pixelPosition);
GeodeticPoint upperRight = rugged.directLocation(firstLineDate, position, los);
@@ -188,13 +218,13 @@ public class AffinageRugged {
//double altErr = 0.0;
final double[] pixErr = new double[4];
pixErr[0] = 0; // lat mean
- pixErr[1] = 0.1; // lat std
+ pixErr[1] = 0.0; // lat std
pixErr[2] = 0; // lon mean
- pixErr[3] = 0.1; // lon std
+ pixErr[3] = 0.0; // lon std
final double[] altErr = new double[2];
- altErr[0] = 1.0; //mean
- altErr[1] = 3.0; //std
+ altErr[0] = 0.0; //mean
+ altErr[1] = 0.0; //std
measure.CreateNoisyMeasure(lineSampling, pixelSampling,pixErr,altErr); // Test with noisy measures
System.out.format("nb TiePoints %d %n", measure.getMeasureCount());
@@ -242,7 +272,7 @@ public class AffinageRugged {
System.out.format(" **** Start optimization **** %n");
// perform parameters estimation
int maxIterations = 100;
- double convergenceThreshold = 1e-10;
+ double convergenceThreshold = 1e-14;
System.out.format("iterations max %d convergence threshold %3.6e \n",maxIterations, convergenceThreshold);
diff --git a/src/tutorials/java/AffinagePleiades/OrbitModel.java b/src/tutorials/java/AffinagePleiades/OrbitModel.java
index 4c0c7c59..52fa3f3b 100644
--- a/src/tutorials/java/AffinagePleiades/OrbitModel.java
+++ b/src/tutorials/java/AffinagePleiades/OrbitModel.java
@@ -1,32 +1,32 @@
-/* Copyright 2013-2016 CS Systèmes d'Information
- * Licensed to CS Systèmes d'Information (CS) under one or more
- * contributor license agreements. See the NOTICE file distributed with
- * this work for additional information regarding copyright ownership.
- * CS licenses this file to You under the Apache License, Version 2.0
- * (the "License"); you may not use this file except in compliance with
- * the License. You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
+/*
+ * Copyright 2013-2016 CS Systèmes d'Information Licensed to CS Systèmes
+ * d'Information (CS) under one or more contributor license agreements. See the
+ * NOTICE file distributed with this work for additional information regarding
+ * copyright ownership. CS licenses this file to You under the Apache License,
+ * Version 2.0 (the "License"); you may not use this file except in compliance
+ * with the License. You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law
+ * or agreed to in writing, software distributed under the License is
+ * distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied. See the License for the specific language
+ * governing permissions and limitations under the License.
*/
package AffinagePleiades;
-
import org.hipparchus.geometry.euclidean.threed.Rotation;
+import org.hipparchus.geometry.euclidean.threed.RotationOrder;
+import org.hipparchus.geometry.euclidean.threed.RotationConvention;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
import org.hipparchus.util.FastMath;
import java.util.ArrayList;
import java.util.List;
-
+import java.util.Arrays;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.attitudes.NadirPointing;
import org.orekit.attitudes.YawCompensation;
+import org.orekit.attitudes.LofOffset;
+import org.orekit.attitudes.TabulatedLofOffset;
import org.orekit.bodies.BodyShape;
import org.orekit.bodies.CelestialBodyFactory;
import org.orekit.bodies.OneAxisEllipsoid;
@@ -38,6 +38,7 @@ import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.Transform;
+import org.orekit.frames.LOFType;
import org.orekit.orbits.CircularOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
@@ -48,26 +49,36 @@ import org.orekit.propagation.analytical.KeplerianPropagator;
import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.propagation.sampling.OrekitFixedStepHandler;
import org.orekit.time.AbsoluteDate;
+
import org.orekit.time.TimeScale;
import org.orekit.utils.Constants;
import org.orekit.utils.IERSConventions;
import org.orekit.utils.PVCoordinates;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;
-
+import org.orekit.utils.AngularDerivativesFilter;
/**
- * Orbit Model class to generate PV and Q
+ * Orbit Model class to generate PV and Q
+ *
* @author jguinet
- *
*/
public class OrbitModel {
+ private double[] LOFTransformRollPoly;
+
+ private double[] LOFTransformPitchPoly;
+
+ private double[] LOFTransformYawPoly;
+
+ private AbsoluteDate refDate;
+
public static void addSatellitePV(TimeScale gps, Frame eme2000, Frame itrf,
ArrayList<TimeStampedPVCoordinates> satellitePVList,
- String absDate,
- double px, double py, double pz, double vx, double vy, double vz)
+ String absDate, double px, double py,
+ double pz, double vx, double vy,
+ double vz)
throws OrekitException {
AbsoluteDate ephemerisDate = new AbsoluteDate(absDate, gps);
Vector3D position = new Vector3D(px, py, pz);
@@ -76,23 +87,30 @@ public class OrbitModel {
Transform transform = itrf.getTransformTo(eme2000, ephemerisDate);
Vector3D pEME2000 = transform.transformPosition(pvITRF.getPosition());
Vector3D vEME2000 = transform.transformVector(pvITRF.getVelocity());
- satellitePVList.add(new TimeStampedPVCoordinates(ephemerisDate, pEME2000, vEME2000, Vector3D.ZERO));
+ satellitePVList
+ .add(new TimeStampedPVCoordinates(ephemerisDate, pEME2000, vEME2000,
+ Vector3D.ZERO));
}
- public void addSatelliteQ(TimeScale gps, ArrayList<TimeStampedAngularCoordinates> satelliteQList,
- String absDate, double q0, double q1, double q2, double q3) {
+ public void addSatelliteQ(TimeScale gps,
+ ArrayList<TimeStampedAngularCoordinates> satelliteQList,
+ String absDate, double q0, double q1, double q2,
+ double q3) {
AbsoluteDate attitudeDate = new AbsoluteDate(absDate, gps);
Rotation rotation = new Rotation(q0, q1, q2, q3, true);
- TimeStampedAngularCoordinates pair =
- new TimeStampedAngularCoordinates(attitudeDate, rotation, Vector3D.ZERO, Vector3D.ZERO);
+ TimeStampedAngularCoordinates pair = new TimeStampedAngularCoordinates(attitudeDate,
+ rotation,
+ Vector3D.ZERO,
+ Vector3D.ZERO);
satelliteQList.add(pair);
}
- public BodyShape createEarth()
- throws OrekitException {
+ public BodyShape createEarth()
+ throws OrekitException {
return new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
Constants.WGS84_EARTH_FLATTENING,
- FramesFactory.getITRF(IERSConventions.IERS_2010, true));
+ FramesFactory.getITRF(
+ IERSConventions.IERS_2010, true));
}
public NormalizedSphericalHarmonicsProvider createGravityField()
@@ -100,78 +118,156 @@ public class OrbitModel {
return GravityFieldFactory.getNormalizedProvider(12, 12);
}
- public Orbit createOrbit(double mu,AbsoluteDate date)
+ public Orbit createOrbit(double mu, AbsoluteDate date)
throws OrekitException {
// the following orbital parameters have been computed using
// Orekit tutorial about phasing, using the following configuration:
//
- // orbit.date = 2012-01-01T00:00:00.000
- // phasing.orbits.number = 143
- // phasing.days.number = 10
- // sun.synchronous.reference.latitude = 0
- // sun.synchronous.reference.ascending = false
- // sun.synchronous.mean.solar.time = 10:30:00
- // gravity.field.degree = 12
- // gravity.field.order = 12
-
+ // orbit.date = 2012-01-01T00:00:00.000
+ // phasing.orbits.number = 143
+ // phasing.days.number = 10
+ // sun.synchronous.reference.latitude = 0
+ // sun.synchronous.reference.ascending = false
+ // sun.synchronous.mean.solar.time = 10:30:00
+ // gravity.field.degree = 12
+ // gravity.field.order = 12
+
Frame eme2000 = FramesFactory.getEME2000();
- //return new CircularOrbit(7173352.811913891,
- return new CircularOrbit(694000.0 + Constants.WGS84_EARTH_EQUATORIAL_RADIUS,//ajouter Rayon
+ // return new CircularOrbit(7173352.811913891,
+ return new CircularOrbit(694000.0 +
+ Constants.WGS84_EARTH_EQUATORIAL_RADIUS, // ajouter
+ // Rayon
-4.029194321683225E-4, 0.0013530362644647786,
- FastMath.toRadians(98.2), //pleiades inclinaison 98.2
- //FastMath.toRadians(-86.47),
- FastMath.toRadians(-86.47+180),
- FastMath.toRadians(135.9+0.3), PositionAngle.TRUE,
- eme2000, date, mu);
+ FastMath.toRadians(98.2), // pleiades
+ // inclinaison 98.2
+ // FastMath.toRadians(-86.47),
+ FastMath.toRadians(-86.47 + 180),
+ FastMath.toRadians(135.9 + 0.3),
+ PositionAngle.TRUE, eme2000, date, mu);
+ }
+
+ public void setLOFTransform(double[] rollPoly, double[] pitchPoly,
+ double[] yawPoly, AbsoluteDate refDate) {
+ LOFTransformRollPoly = rollPoly.clone();
+ LOFTransformPitchPoly = pitchPoly.clone();
+ LOFTransformYawPoly = yawPoly.clone();
+ this.refDate = refDate;
+ }
+
+ private double getPoly(double[] poly, double shift) {
+ double val = 0.0;
+ for (int coef = 0; coef < poly.length; coef++) {
+ val = val + poly[coef] * Math.pow(shift, coef);
+ }
+ return val;
+ }
+
+ private Rotation getOffset(BodyShape earth, Orbit orbit, double shift)
+ throws OrekitException {
+ LOFType type = LOFType.VVLH;
+ double roll = getPoly(LOFTransformRollPoly, shift);
+ double pitch = getPoly(LOFTransformPitchPoly, shift);
+ double yaw = getPoly(LOFTransformYawPoly, shift);
+ System.out.format("at shift %f roll %f pitch %f yaw %f \n ", shift,
+ roll, pitch, yaw);
+
+ LofOffset law = new LofOffset(orbit.getFrame(), type, RotationOrder.XYZ,
+ roll, pitch, yaw);
+ Rotation offsetAtt = law
+ .getAttitude(orbit, orbit.getDate().shiftedBy(shift),
+ orbit.getFrame())
+ .getRotation();
+ NadirPointing nadirPointing = new NadirPointing(orbit.getFrame(),
+ earth);
+ Rotation nadirAtt = nadirPointing
+ .getAttitude(orbit, orbit.getDate().getDate().shiftedBy(shift),
+ orbit.getFrame())
+ .getRotation();
+ Rotation offsetProper = offsetAtt
+ .compose(nadirAtt.revert(), RotationConvention.VECTOR_OPERATOR);
+ return offsetProper;
+ }
+
+ public AttitudeProvider createAttitudeProvider(BodyShape earth, Orbit orbit)
+ throws OrekitException {
+ LOFType type = LOFType.VVLH;
+
+ ArrayList<TimeStampedAngularCoordinates> list = new ArrayList<TimeStampedAngularCoordinates>();
+
+ for (double shift = -10.0; shift < +10.0; shift += 1e-2) {
+ list.add(new TimeStampedAngularCoordinates(refDate.shiftedBy(shift),
+ getOffset(earth, orbit,
+ shift),
+ Vector3D.ZERO,
+ Vector3D.ZERO));
+ }
+
+ TabulatedLofOffset tabulated = new TabulatedLofOffset(orbit
+ .getFrame(), type, list, 2, AngularDerivativesFilter.USE_R);
+
+ return tabulated;
}
public Propagator createPropagator(BodyShape earth,
- NormalizedSphericalHarmonicsProvider gravityField,
- Orbit orbit)
+ NormalizedSphericalHarmonicsProvider gravityField,
+ Orbit orbit)
throws OrekitException {
- AttitudeProvider yawCompensation = new YawCompensation(orbit.getFrame(), new NadirPointing(orbit.getFrame(), earth));
- SpacecraftState state = new SpacecraftState(orbit,
- yawCompensation.getAttitude(orbit,
- orbit.getDate(),
- orbit.getFrame()),
- 1180.0);
+ AttitudeProvider attidudeProvider = createAttitudeProvider(earth,
+ orbit);
+
+ SpacecraftState state = new SpacecraftState(orbit, attidudeProvider
+ .getAttitude(orbit, orbit.getDate(), orbit.getFrame()), 1180.0);
// numerical model for improving orbit
OrbitType type = OrbitType.CIRCULAR;
- double[][] tolerances = NumericalPropagator.tolerances(0.1, orbit, type);
- DormandPrince853Integrator integrator =
- new DormandPrince853Integrator(1.0e-4 * orbit.getKeplerianPeriod(),
- 1.0e-1 * orbit.getKeplerianPeriod(),
- tolerances[0], tolerances[1]);
+ double[][] tolerances = NumericalPropagator.tolerances(0.1, orbit,
+ type);
+ DormandPrince853Integrator integrator = new DormandPrince853Integrator(1.0e-4 *
+ orbit
+ .getKeplerianPeriod(),
+ 1.0e-1 * orbit
+ .getKeplerianPeriod(),
+ tolerances[0],
+ tolerances[1]);
integrator.setInitialStepSize(1.0e-2 * orbit.getKeplerianPeriod());
NumericalPropagator numericalPropagator = new NumericalPropagator(integrator);
- numericalPropagator.addForceModel(new HolmesFeatherstoneAttractionModel(earth.getBodyFrame(), gravityField));
- numericalPropagator.addForceModel(new ThirdBodyAttraction(CelestialBodyFactory.getSun()));
- numericalPropagator.addForceModel(new ThirdBodyAttraction(CelestialBodyFactory.getMoon()));
+ numericalPropagator
+ .addForceModel(new HolmesFeatherstoneAttractionModel(earth
+ .getBodyFrame(), gravityField));
+ numericalPropagator
+ .addForceModel(new ThirdBodyAttraction(CelestialBodyFactory
+ .getSun()));
+ numericalPropagator
+ .addForceModel(new ThirdBodyAttraction(CelestialBodyFactory
+ .getMoon()));
numericalPropagator.setOrbitType(type);
numericalPropagator.setInitialState(state);
- numericalPropagator.setAttitudeProvider(yawCompensation);
+ numericalPropagator.setAttitudeProvider(attidudeProvider);
return numericalPropagator;
}
-
- public List<TimeStampedPVCoordinates> orbitToPV(Orbit orbit, BodyShape earth,
- AbsoluteDate minDate, AbsoluteDate maxDate,
- double step)
- throws OrekitException {
+ public List<TimeStampedPVCoordinates>
+ orbitToPV(Orbit orbit, BodyShape earth, AbsoluteDate minDate,
+ AbsoluteDate maxDate, double step)
+ throws OrekitException {
Propagator propagator = new KeplerianPropagator(orbit);
- propagator.setAttitudeProvider(new YawCompensation(orbit.getFrame(), new NadirPointing(orbit.getFrame(), earth)));
+ propagator.setAttitudeProvider(createAttitudeProvider(earth, orbit));
propagator.propagate(minDate);
final List<TimeStampedPVCoordinates> list = new ArrayList<TimeStampedPVCoordinates>();
propagator.setMasterMode(step, new OrekitFixedStepHandler() {
+
public void init(SpacecraftState s0, AbsoluteDate t) {
- }
- public void handleStep(SpacecraftState currentState, boolean isLast) {
- list.add(new TimeStampedPVCoordinates(currentState.getDate(),
- currentState.getPVCoordinates().getPosition(),
- currentState.getPVCoordinates().getVelocity(),
+ }
+
+ public void handleStep(SpacecraftState currentState,
+ boolean isLast) {
+ list.add(new TimeStampedPVCoordinates(currentState
+ .getDate(), currentState.getPVCoordinates().getPosition(),
+ currentState
+ .getPVCoordinates()
+ .getVelocity(),
Vector3D.ZERO));
}
});
@@ -179,21 +275,25 @@ public class OrbitModel {
return list;
}
- public List<TimeStampedAngularCoordinates> orbitToQ(Orbit orbit, BodyShape earth,
- AbsoluteDate minDate, AbsoluteDate maxDate,
- double step)
- throws OrekitException {
+ public List<TimeStampedAngularCoordinates>
+ orbitToQ(Orbit orbit, BodyShape earth, AbsoluteDate minDate,
+ AbsoluteDate maxDate, double step)
+ throws OrekitException {
Propagator propagator = new KeplerianPropagator(orbit);
- propagator.setAttitudeProvider(new YawCompensation(orbit.getFrame(), new NadirPointing(orbit.getFrame(), earth)));
+ propagator.setAttitudeProvider(createAttitudeProvider(earth, orbit));
propagator.propagate(minDate);
final List<TimeStampedAngularCoordinates> list = new ArrayList<TimeStampedAngularCoordinates>();
propagator.setMasterMode(step, new OrekitFixedStepHandler() {
+
public void init(SpacecraftState s0, AbsoluteDate t) {
- }
- public void handleStep(SpacecraftState currentState, boolean isLast) {
- list.add(new TimeStampedAngularCoordinates(currentState.getDate(),
- currentState.getAttitude().getRotation(),
- Vector3D.ZERO, Vector3D.ZERO));
+ }
+
+ public void handleStep(SpacecraftState currentState,
+ boolean isLast) {
+ list.add(new TimeStampedAngularCoordinates(currentState
+ .getDate(), currentState.getAttitude().getRotation(),
+ Vector3D.ZERO,
+ Vector3D.ZERO));
}
});
propagator.propagate(maxDate);
@@ -201,4 +301,3 @@ public class OrbitModel {
}
}
-
--
GitLab