diff --git a/src/changes/changes.xml b/src/changes/changes.xml index 70da8a6521a503b1b1c4019998823a8c00d68b6e..b87939b7c3507768b9cce67b39921ab4b943da29 100644 --- a/src/changes/changes.xml +++ b/src/changes/changes.xml @@ -21,6 +21,11 @@ + + Fixed bugs in the derivatives computation in IodGooding. + Fixed bugs in IodLambert when there's more than an half revolution + between start and final position. + Fixed parsing of compact RINEX files with wrong key in header produced by some Septentrio receivers. diff --git a/src/main/java/org/orekit/estimation/iod/IodGibbs.java b/src/main/java/org/orekit/estimation/iod/IodGibbs.java index 964c8d82929820ab95719f1531c833970474b85a..1c9a179884ec9b533f52217741f7994b17182c6e 100644 --- a/src/main/java/org/orekit/estimation/iod/IodGibbs.java +++ b/src/main/java/org/orekit/estimation/iod/IodGibbs.java @@ -118,6 +118,26 @@ public class IodGibbs { final AbsoluteDate date = date2; // compute the equivalent Keplerian orbit - return new KeplerianOrbit(pv, frame, date, mu); + KeplerianOrbit orbit = new KeplerianOrbit(pv, frame, date, mu); + + + //define the reverse orbit + final PVCoordinates pv2 = new PVCoordinates(r2, vlEci.scalarMultiply(-1)); + KeplerianOrbit orbit2 =new KeplerianOrbit(pv2, frame, date, mu); + + //check which orbit is correct + Vector3D estP3 = orbit.shiftedBy(date3.durationFrom(date2)). + getPVCoordinates().getPosition(); + double dist = estP3.subtract(r3).getNorm(); + Vector3D estP3_2 = orbit2.shiftedBy(date3.durationFrom(date2)). + getPVCoordinates().getPosition(); + double dist2 = estP3_2.subtract(r3).getNorm(); + + + if(dist <= dist2){ + return orbit; + } else { + return orbit2; + } } } diff --git a/src/main/java/org/orekit/estimation/iod/IodGooding.java b/src/main/java/org/orekit/estimation/iod/IodGooding.java index 325d5700024580e930397163a4bb2ccad8c6d2c8..7ce991b15f00a01c3238651c2418ee0274a5aa05 100644 --- a/src/main/java/org/orekit/estimation/iod/IodGooding.java +++ b/src/main/java/org/orekit/estimation/iod/IodGooding.java @@ -140,13 +140,16 @@ public class IodGooding { * @param dateObs3 date of observation 1 * @param rho1init initial guess of the range problem. range 1, in meters * @param rho3init initial guess of the range problem. range 3, in meters + * @param nRev number of complete revolutions between observation1 and 3 + * @param direction true if posigrade (short way) * @return an estimate of the Keplerian orbit */ public KeplerianOrbit estimate(final Vector3D O1, final Vector3D O2, final Vector3D O3, final Vector3D lineOfSight1, final AbsoluteDate dateObs1, final Vector3D lineOfSight2, final AbsoluteDate dateObs2, final Vector3D lineOfSight3, final AbsoluteDate dateObs3, - final double rho1init, final double rho3init) { + final double rho1init, final double rho3init, int nRev, + boolean direction) { this.date1 = dateObs1; @@ -167,8 +170,8 @@ public class IodGooding { // solve the range problem solveRangeProblem(rho1init / R, rho3init / R, dateObs3.durationFrom(dateObs1) / T, dateObs2.durationFrom(dateObs1) / T, - 0, - true, + nRev, + direction, lineOfSight1, lineOfSight2, lineOfSight3, maxiter); @@ -179,6 +182,35 @@ public class IodGooding { final Vector3D p3 = vObserverPosition3.add(lineOfSight3.scalarMultiply(rho3)).scalarMultiply(R); return gibbs.estimate(frame, p1, dateObs1, p2, dateObs2, p3, dateObs3); } + + + /** Orbit got from Observed Three Lines of Sight (angles only). + * assuming there was less than an half revolution between start and final date + * + * @param O1 Observer position 1 + * @param O2 Observer position 2 + * @param O3 Observer position 3 + * @param lineOfSight1 line of sight 1 + * @param dateObs1 date of observation 1 + * @param lineOfSight2 line of sight 2 + * @param dateObs2 date of observation 1 + * @param lineOfSight3 line of sight 3 + * @param dateObs3 date of observation 1 + * @param rho1init initial guess of the range problem. range 1, in meters + * @param rho3init initial guess of the range problem. range 3, in meters + * @return an estimate of the Keplerian orbit + */ + public KeplerianOrbit estimate(final Vector3D O1, final Vector3D O2, final Vector3D O3, + final Vector3D lineOfSight1, final AbsoluteDate dateObs1, + final Vector3D lineOfSight2, final AbsoluteDate dateObs2, + final Vector3D lineOfSight3, final AbsoluteDate dateObs3, + final double rho1init, final double rho3init) { + + return this.estimate(O1, O2, O3, lineOfSight1, dateObs1, lineOfSight2, dateObs2, + lineOfSight3, dateObs3, rho1init, rho3init, 0, true); + } + + /** Solve the range problem when three line of sight are given. * @@ -248,13 +280,16 @@ public class IodGooding { // They should be zero when line of sight 2 and current direction for 2 from O2 are aligned. final Vector3D u = lineOfSight2.crossProduct(C); final Vector3D P = (u.crossProduct(lineOfSight2)).normalize(); - final Vector3D EN = (lineOfSight2.crossProduct(P)).normalize(); + Vector3D ENt = (lineOfSight2.crossProduct(P)); - // if EN is zero we have a solution! - final double ENR = EN.getNorm(); + // if ENt is zero we have a solution! + final double ENR = ENt.getNorm(); if (ENR == 0.) { return true; } + + //Normalize EN + final Vector3D EN = ENt.normalize(); // Coordinate along 'F function' final double Fc = P.dotProduct(C); @@ -385,8 +420,8 @@ public class IodGooding { final double Gp1 = EN.dotProduct(Cp1); // derivatives df/drho1 and dg/drho1 - final double Fx = (Fp1 - Fm1) / (2 * dx); - final double Gx = (Gp1 - Gm1) / (2 * dx); + final double Fx = (Fp1 - Fm1) / (2. * dx); + final double Gx = (Gp1 - Gm1) / (2. * dx); final Vector3D Cm3 = getPositionOnLoS2 (lineOfSight1, x, lineOfSight3, y - dy, @@ -406,12 +441,12 @@ public class IodGooding { final double Fy = (Fp3 - Fm3) / (2. * dy); final double Gy = (Gp3 - Gm3) / (2. * dy); final double detJac = Fx * Gy - Fy * Gx; - + // Coefficients for the classical Newton-Raphson iterative method - FD[0] = Fx / detJac; - FD[1] = Fy / detJac; - GD[0] = Gx / detJac; - GD[1] = Gy / detJac; + FD[0] = Fx ; + FD[1] = Fy ; + GD[0] = Gx ; + GD[1] = Gy ; // Modified Newton-Raphson process, with Halley's method to have cubic convergence. // This requires computing second order derivatives. @@ -431,17 +466,17 @@ public class IodGooding { T13, T12, nrev, direction).subtract(vObserverPosition2); // f function value at (x1+dx1, x3+dx3) - final double Fp13 = P.dotProduct(Cp13) - F; + final double Fp13 = P.dotProduct(Cp13); // g function value at (x1+dx1, x3+dx3) final double Gp13 = EN.dotProduct(Cp13); - final Vector3D Cm13 = getPositionOnLoS2 (lineOfSight1, x + dx, - lineOfSight3, y + dy, + final Vector3D Cm13 = getPositionOnLoS2 (lineOfSight1, x - dx, + lineOfSight3, y - dy, T13, T12, nrev, direction).subtract(vObserverPosition2); - // f function value at (x1+dx1, x3+dx3) - final double Fm13 = P.dotProduct(Cm13) - F; - // g function value at (x1+dx1, x3+dx3) + // f function value at (x1-dx1, x3-dx3) + final double Fm13 = P.dotProduct(Cm13); + // g function value at (x1-dx1, x3-dx3) final double Gm13 = EN.dotProduct(Cm13); // Second order derivatives: d^2f / drho1drho3 and d^2g / drho1drho3 @@ -449,8 +484,8 @@ public class IodGooding { // 0.5 * (Fxx * dx / dy + Fyy * dy / dx); //double Gxy = Gp13 / (dx * dy) - (Gx / dy + Gy / dx) - // 0.5 * (Gxx * dx / dy + Gyy * dy / dx); - final double Fxy = (Fp13 + Fm13) / (2 * dx * dy) - 1.0 * (Fxx / 2 + Fyy / 2) - F / (dx * dy); - final double Gxy = (Gp13 + Gm13) / (2 * dx * dy) - 1.0 * (Gxx / 2 + Gyy / 2) - F / (dx * dy); + final double Fxy = (Fp13 + Fm13) / (2 * dx * dy) - 0.5 * (Fxx * dx / dy + Fyy * dy / dx) - F / (dx * dy); + final double Gxy = (Gp13 + Gm13) / (2 * dx * dy) - 0.5 * (Gxx * dx / dy + Gyy * dy / dx) - F / (dx * dy); // delta Newton Raphson, 1st order step final double dx3NR = -Gy * F / detJac; @@ -464,7 +499,7 @@ public class IodGooding { final double FxH = Fx + 0.5 * (Fxx * dx3NR + Fxy * dx1NR); final double FyH = Fy + 0.5 * (Fxy * dx3NR + Fxx * dx1NR); final double GxH = Gx + 0.5 * (Gxx * dx3NR + Gxy * dx1NR); - final double GyH = Gy + 0.5 * (Gxy * dx3NR + Fxy * dx1NR); + final double GyH = Gy + 0.5 * (Gxy * dx3NR + Gyy * dx1NR); // New Halley's method "Jacobian" FD[0] = FxH; @@ -489,7 +524,7 @@ public class IodGooding { private Vector3D getPositionOnLoS2(final Vector3D E1, final double RO1, final Vector3D E3, final double RO3, final double T13, final double T12, - final double nRev, final boolean posigrade) { + final int nRev, final boolean posigrade) { final Vector3D P1 = vObserverPosition1.add(E1.scalarMultiply(RO1)); R1 = P1.getNorm(); @@ -504,14 +539,13 @@ public class IodGooding { // compute the number of revolutions if (!posigrade) { - TH = FastMath.PI - TH; + TH = 2 * FastMath.PI - TH; } - TH = TH + nRev * FastMath.PI; - + // Solve the Lambert's problem to get the velocities at endpoints final double[] V1 = new double[2]; // work with non-dimensional units (MU=1) - final boolean exitflag = lambert.solveLambertPb(R1, R3, TH, T13, 0, V1); + final boolean exitflag = lambert.solveLambertPb(R1, R3, TH, T13, nRev, V1); if (exitflag) { // basis vectors @@ -526,14 +560,14 @@ public class IodGooding { // velocity vector at P1 final Vector3D Vel1 = new Vector3D(V1[0] / R1, P1, V1[1] / RT, Pt); - + // estimate the position at the second observation time // propagate (P1, V1) during TAU + T12 to get (P2, V2) final Vector3D P2 = propagatePV(P1, Vel1, T12); return P2; } - + return null; } diff --git a/src/main/java/org/orekit/estimation/iod/IodLambert.java b/src/main/java/org/orekit/estimation/iod/IodLambert.java index 5be53e67a1e3cf96ee00a7d66d995e0fac29be71..c5970959ed2bc577997571c8b13b1627664de75e 100644 --- a/src/main/java/org/orekit/estimation/iod/IodLambert.java +++ b/src/main/java/org/orekit/estimation/iod/IodLambert.java @@ -51,7 +51,7 @@ public class IodLambert { *

* The logic for setting {@code posigrade} and {@code nRev} is that the * sweep angle Δυ travelled by the object between {@code t1} and {@code t2} is - * 2π {@code nRev} - α if {@code posigrade} is false and 2π {@code nRev} + α + * 2π {@code nRev +1} - α if {@code posigrade} is false and 2π {@code nRev} + α * if {@code posigrade} is true, where α is the separation angle between * {@code p1} and {@code p2}, which is always computed between 0 and π * (because in 3D without a normal reference, vector angles cannot go past π). @@ -68,7 +68,7 @@ public class IodLambert { * then {@code posigrade} should be {@code true} and {@code nRev} should be 0. * If {@code t2} is more than half a period after {@code t1} but less than * one period after {@code t1}, {@code posigrade} should be {@code false} and - * {@code nRev} should be 1. + * {@code nRev} should be 0. *

* @param frame frame * @param posigrade flag indicating the direction of motion @@ -99,8 +99,7 @@ public class IodLambert { if (!posigrade) { dth = 2 * FastMath.PI - dth; } - dth = dth + nRev * 2 * FastMath.PI; - + // velocity vectors in the orbital plane, in the R-T frame final double[] Vdep = new double[2]; @@ -123,7 +122,7 @@ public class IodLambert { // velocity vector at P1 final Vector3D Vel1 = new Vector3D(V * Vdep[0] / r1, p1, V * Vdep[1] / RT, Pt); - + // compute the equivalent Keplerian orbit return new KeplerianOrbit(new PVCoordinates(p1, Vel1), frame, t1, mu); } @@ -146,19 +145,16 @@ public class IodLambert { final int mRev, final double[] V1) { // decide whether to use the left or right branch (for multi-revolution // problems), and the long- or short way. - final boolean leftbranch = FastMath.signum(mRev) > 0; - int longway = 0; - if (tau > 0) { - longway = 1; + final boolean leftbranch = dth < Math.PI; + int longway = 1; + if (dth > Math.PI) { + longway = -1; } final int m = FastMath.abs(mRev); final double rtof = FastMath.abs(tau); double theta = dth; - if (longway < 0) { - theta = 2 * FastMath.PI - dth; - } - + // non-dimensional chord ||r2-r1|| final double chord = FastMath.sqrt(r1 * r1 + r2 * r2 - 2 * r1 * r2 * FastMath.cos(theta)); @@ -169,8 +165,8 @@ public class IodLambert { final double minSma = speri / 2.; // lambda parameter (Eq 7.6) - final double lambda = FastMath.sqrt(1 - chord / speri); - + final double lambda = longway*FastMath.sqrt(1 - chord / speri); + // reference tof value for the Newton solver final double logt = FastMath.log(rtof); diff --git a/src/test/java/org/orekit/estimation/iod/IodLambertTest.java b/src/test/java/org/orekit/estimation/iod/IodLambertTest.java index ce2952e4fd9ad515565d6edf89804227e2b36d27..9f23b1647ee66ae5cc1bec407263738ae177a007 100644 --- a/src/test/java/org/orekit/estimation/iod/IodLambertTest.java +++ b/src/test/java/org/orekit/estimation/iod/IodLambertTest.java @@ -134,7 +134,7 @@ public class IodLambertTest { Assert.assertEquals(0.0, Vector3D.distance(p1, k0.getPVCoordinates(t1, teme).getPosition()), 2.0e-8); Assert.assertEquals(0.0, Vector3D.distance(p2, k0.getPVCoordinates(t2, teme).getPosition()), 2.0e-7); - KeplerianOrbit k1 = lambert.estimate(teme, false, 1, p1, t1, p3, t3); + KeplerianOrbit k1 = lambert.estimate(teme, true, 1, p1, t1, p3, t3); Assert.assertEquals(5.97e-4, k1.getE(), 1.0e-6); Assert.assertEquals(8.55, FastMath.toDegrees(k1.getI()), 0.01); Assert.assertEquals(0.0, Vector3D.distance(p1, k1.getPVCoordinates(t1, teme).getPosition()), 7.0e-9);