From b3c6dc9b5d0da2f985c499a423b7f5475cdfb472 Mon Sep 17 00:00:00 2001 From: shiva_iyer Date: Sat, 28 Sep 2019 11:58:41 -0500 Subject: [PATCH 1/2] Implemented initial orbit determination using the Laplace method. --- .../org/orekit/estimation/iod/IodGooding.java | 4 +- .../org/orekit/estimation/iod/IodLaplace.java | 152 ++++++++++++++++ .../orekit/estimation/iod/IodLaplaceTest.java | 165 ++++++++++++++++++ 3 files changed, 319 insertions(+), 2 deletions(-) create mode 100644 src/main/java/org/orekit/estimation/iod/IodLaplace.java create mode 100644 src/test/java/org/orekit/estimation/iod/IodLaplaceTest.java diff --git a/src/main/java/org/orekit/estimation/iod/IodGooding.java b/src/main/java/org/orekit/estimation/iod/IodGooding.java index 325d57000..544eeb974 100644 --- a/src/main/java/org/orekit/estimation/iod/IodGooding.java +++ b/src/main/java/org/orekit/estimation/iod/IodGooding.java @@ -135,9 +135,9 @@ public class IodGooding { * @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 dateObs2 date of observation 2 * @param lineOfSight3 line of sight 3 - * @param dateObs3 date of observation 1 + * @param dateObs3 date of observation 3 * @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 diff --git a/src/main/java/org/orekit/estimation/iod/IodLaplace.java b/src/main/java/org/orekit/estimation/iod/IodLaplace.java new file mode 100644 index 000000000..44c6191a1 --- /dev/null +++ b/src/main/java/org/orekit/estimation/iod/IodLaplace.java @@ -0,0 +1,152 @@ +/* Copyright 2002-2019 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 org.orekit.estimation.iod; + +import org.hipparchus.analysis.solvers.LaguerreSolver; +import org.hipparchus.complex.Complex; +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.hipparchus.linear.Array2DRowRealMatrix; +import org.hipparchus.linear.LUDecomposition; +import org.hipparchus.util.FastMath; +import org.orekit.frames.Frame; +import org.orekit.orbits.CartesianOrbit; +import org.orekit.time.AbsoluteDate; +import org.orekit.utils.PVCoordinates; + +/** + * Laplace angles-only initial orbit determination, assuming Keplerian motion. + * An orbit is determined from three angular observations from the same site. + * + * + * Reference: + * Bate, R., Mueller, D. D., & White, J. E. (1971). Fundamentals of astrodynamics. + * New York: Dover Publications. + * + * @author Shiva Iyer + * @since 10.1 + */ +public class IodLaplace { + /** Gravitational constant. */ + private final double mu; + + /** Constructor. + * + * @param mu gravitational constant + */ + public IodLaplace(final double mu) { + this.mu = mu; + } + + /** Estimate orbit from three line of sight angles from the same location. + * + * @param frame Intertial frame for observer coordinates and orbit estimate + * @param obsPva Observer coordinates at time obsDate2 + * @param obsDate1 date of observation 1 + * @param los1 line of sight unit vector 1 + * @param obsDate2 date of observation 2 + * @param los2 line of sight unit vector 2 + * @param obsDate3 date of observation 3 + * @param los3 line of sight unit vector 3 + * @return estimate of the orbit at the central date dateObs2 or null if + * no estimate is possible with the given data + */ + public CartesianOrbit estimate(final Frame frame, final PVCoordinates obsPva, + final AbsoluteDate obsDate1, final Vector3D los1, + final AbsoluteDate obsDate2, final Vector3D los2, + final AbsoluteDate obsDate3, final Vector3D los3) { + // The first observation is taken as t1 = 0 + final double t2 = obsDate2.durationFrom(obsDate1); + final double t3 = obsDate3.durationFrom(obsDate1); + + // Calculate the first and second derivatives of the Line Of Sight vector at t2 + final Vector3D Ldot = los1.scalarMultiply((t2-t3)/(t2*t3)) + .add(los2.scalarMultiply((2.0*t2-t3)/(t2*(t2-t3)))) + .add(los3.scalarMultiply(t2/(t3*(t3-t2)))); + final Vector3D Ldotdot = los1.scalarMultiply(2.0/(t2*t3)) + .add(los2.scalarMultiply(2.0/(t2*(t2-t3)))) + .add(los3.scalarMultiply(2.0/(t3*(t3-t2)))); + + // The determinant will vanish if the observer lies in the plane of the orbit at t2 + final double D = 2.0*getDeterminant(los2, Ldot, Ldotdot); + if (FastMath.abs(D) < 1.0E-14) + return(null); + + final double Dsq = D*D; + final double R = obsPva.getPosition().getNorm(); + final double RdotL = obsPva.getPosition().dotProduct(los2); + + final double D1 = getDeterminant(los2, Ldot, obsPva.getAcceleration()); + final double D2 = getDeterminant(los2, Ldot, obsPva.getPosition()); + + // Coefficients of the 8th order polynomial we need to solve to determine "r" + final double[] coeff = new double[]{-4.0*mu*mu*D2*D2/Dsq, 0.0, 0.0, + 4.0*mu*D2*(RdotL/D-2.0*D1/Dsq), 0.0, 0.0, + 4.0*D1*RdotL/D-4.0*D1*D1/Dsq-R*R, 0.0, 1.0}; + + // Use the Laguerre polynomial solver and take the initial guess to be + // 5 times the observer's position magnitude + final LaguerreSolver solver = new LaguerreSolver(1E-10, 1E-10, 1E-10); + final Complex[] roots = solver.solveAllComplex(coeff, 5.0*R); + + // We consider "r" to be the positive real root with the largest magnitude + double rMag = 0.0; + for (int i = 0; i < roots.length; i++) { + if (roots[i].getReal() > rMag && + FastMath.abs(roots[i].getImaginary()) < solver.getAbsoluteAccuracy()) { + rMag = roots[i].getReal(); + } + } + if (rMag == 0.0) + return(null); + + // Calculate rho, the slant range from the observer to the satellite at t2. + // This yields the "r" vector, which is the satellite's position vector at t2. + final double rCubed = FastMath.pow(rMag, 3); + final double rho = -2.0*D1/D - 2.0*mu*D2/(D*rCubed); + final Vector3D posVec = los2.scalarMultiply(rho) + .add(obsPva.getPosition()); + + // Calculate rho_dot at t2, which will yield the satellite's velocity vector at t2 + final double D3 = getDeterminant(los2, obsPva.getAcceleration(), Ldotdot); + final double D4 = getDeterminant(los2, obsPva.getPosition(), Ldotdot); + final double rhoDot = -D3/D - mu*D4/(D*rCubed); + final Vector3D velVec = los2.scalarMultiply(rhoDot) + .add(Ldot.scalarMultiply(rho)) + .add(obsPva.getVelocity()); + + // Return the estimated orbit + return(new CartesianOrbit(new PVCoordinates(posVec, velVec), frame, obsDate2, mu)); + } + + /** Calculate the determinant of the matrix with given column vectors. + * + * @param col0 Matrix column 0 + * @param col1 Matrix column 1 + * @param col2 Matrix column 2 + * @return matrix determinant + * + */ + private double getDeterminant(final Vector3D col0, final Vector3D col1, + final Vector3D col2) { + Array2DRowRealMatrix mat = new Array2DRowRealMatrix(3, 3); + mat.setColumn(0, col0.toArray()); + mat.setColumn(1, col1.toArray()); + mat.setColumn(2, col2.toArray()); + return(new LUDecomposition(mat).getDeterminant()); + } +} diff --git a/src/test/java/org/orekit/estimation/iod/IodLaplaceTest.java b/src/test/java/org/orekit/estimation/iod/IodLaplaceTest.java new file mode 100644 index 000000000..871f93ac1 --- /dev/null +++ b/src/test/java/org/orekit/estimation/iod/IodLaplaceTest.java @@ -0,0 +1,165 @@ +/* Copyright 2002-2019 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 org.orekit.estimation.iod; + +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.hipparchus.util.FastMath; +import org.junit.Assert; +import org.junit.Before; +import org.junit.Test; +import org.orekit.bodies.GeodeticPoint; +import org.orekit.bodies.OneAxisEllipsoid; +import org.orekit.estimation.measurements.AngularRaDec; +import org.orekit.estimation.measurements.GroundStation; +import org.orekit.estimation.measurements.ObservableSatellite; +import org.orekit.frames.Frame; +import org.orekit.frames.FramesFactory; +import org.orekit.frames.TopocentricFrame; +import org.orekit.orbits.CartesianOrbit; +import org.orekit.propagation.SpacecraftState; +import org.orekit.propagation.analytical.tle.TLE; +import org.orekit.propagation.analytical.tle.TLEPropagator; +import org.orekit.time.AbsoluteDate; +import org.orekit.utils.Constants; +import org.orekit.utils.IERSConventions; +import org.orekit.utils.AbsolutePVCoordinates; +import org.orekit.utils.TimeStampedPVCoordinates; +import org.orekit.Utils; + +/** + * @author Shiva Iyer + * @since 10.1 + */ +public class IodLaplaceTest { + private Frame gcrf; + private Frame itrf; + + @Before + public void setUp() { + Utils.setDataRoot("regular-data"); + this.gcrf = FramesFactory.getGCRF(); + this.itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, false); + } + + @Test + public void testLaplace1() { + // ISS (ZARYA) + final String tle1 = "1 25544U 98067A 19271.53261574 .00000256 00000-0 12497-4 0 9993"; + final String tle2 = "2 25544 51.6447 208.7465 0007429 92.6235 253.7389 15.50110361191281"; + final double[] error = estimateFromTLE(tle1, tle2, 30.0, 60.0).getErrorNorm(); + + // With only 3 measurements, an error of 5km in position and 10 m/s in velocity is acceptable + Assert.assertEquals(0.0, error[0], 5000.0); + Assert.assertEquals(0.0, error[1], 10.0); + } + + @Test + public void testLaplace2() { + // COSMOS 382 + final String tle1 = "1 4786U 70103A 19270.85687399 -.00000025 +00000-0 +00000-0 0 9998"; + final String tle2 = "2 4786 055.8645 163.2517 1329144 016.0116 045.4806 08.42042146501862"; + final double[] error = estimateFromTLE(tle1, tle2, 30.0, 60.0).getErrorNorm(); + Assert.assertEquals(0.0, error[0], 5000.0); + Assert.assertEquals(0.0, error[1], 10.0); + } + + @Test + public void testLaplace3() { + // GALAXY 15 + final String tle1 = "1 28884U 05041A 19270.71989132 .00000058 00000-0 00000+0 0 9991"; + final String tle2 = "2 28884 0.0023 190.3430 0001786 354.8402 307.2011 1.00272290 51019"; + final double[] error = estimateFromTLE(tle1, tle2, 300.0, 600.0).getErrorNorm(); + Assert.assertEquals(0.0, error[0], 5000.0); + Assert.assertEquals(0.0, error[1], 10.0); + } + + private Result estimateFromTLE(final String tle1, final String tle2, + final double t2, final double t3) + { + // Use the TLE propagator + final TLE tleParser = new TLE(tle1, tle2); + final TLEPropagator tleProp = TLEPropagator.selectExtrapolator(tleParser); + + // The ground station is set to Austin, Texas, U.S.A + final OneAxisEllipsoid body = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, + Constants.WGS84_EARTH_FLATTENING, itrf); + final GroundStation observer = new GroundStation( + new TopocentricFrame(body, new GeodeticPoint(0.528253, -1.705768, 0.0), "Austin")); + observer.getPrimeMeridianOffsetDriver().setReferenceDate(AbsoluteDate.J2000_EPOCH); + observer.getPolarOffsetXDriver().setReferenceDate(AbsoluteDate.J2000_EPOCH); + observer.getPolarOffsetYDriver().setReferenceDate(AbsoluteDate.J2000_EPOCH); + + // Generate 3 Line Of Sight angles measurements + final AbsoluteDate obsDate1 = tleParser.getDate(); + final Vector3D los1 = getLOSAngles(tleProp, obsDate1, observer); + + final AbsoluteDate obsDate2 = obsDate1.shiftedBy(t2); + final Vector3D los2 = getLOSAngles(tleProp, obsDate2, observer); + + final AbsoluteDate obsDate3 = obsDate1.shiftedBy(t3); + final Vector3D los3 = getLOSAngles(tleProp, obsDate3, observer); + + final TimeStampedPVCoordinates obsPva = observer + .getBaseFrame().getPVCoordinates(obsDate2, gcrf); + + // Estimate the orbit using the classical Laplace method + final TimeStampedPVCoordinates truth = tleProp.getPVCoordinates(obsDate2, gcrf); + final CartesianOrbit estOrbit = new IodLaplace(Constants.EGM96_EARTH_MU) + .estimate(gcrf, obsPva, obsDate1, los1, obsDate2, los2, obsDate3, los3); + return(new Result(truth, estOrbit)); + } + + // Helper function to generate a Line of Sight angles measurement for the given + // observer and date using the TLE propagator. + private Vector3D getLOSAngles(final TLEPropagator tleProp, final AbsoluteDate date, + final GroundStation observer) { + final TimeStampedPVCoordinates satPvc = tleProp.getPVCoordinates(date, gcrf); + final AngularRaDec raDec = new AngularRaDec(observer, gcrf, date, new double[] {0.0, 0.0}, + new double[] {1.0, 1.0}, + new double[] {1.0, 1.0}, new ObservableSatellite(0)); + + final double[] angular = raDec.estimate(0, 0, new SpacecraftState[]{new SpacecraftState( + new AbsolutePVCoordinates(gcrf, satPvc))}).getEstimatedValue(); + final double azi = angular[0]; + final double ele = angular[1]; + + return(new Vector3D(FastMath.cos(ele)*FastMath.cos(azi), + FastMath.cos(ele)*FastMath.sin(azi), FastMath.sin(ele))); + } + + // Private class to calculate the errors between truth and estimated orbits at + // the central observation time. + private class Result + { + private double[] errorNorm; + + public Result(final TimeStampedPVCoordinates truth, final CartesianOrbit estOrbit) + { + this.errorNorm = new double[2]; + this.errorNorm[0] = Vector3D.distance(truth.getPosition(), + estOrbit.getPVCoordinates().getPosition()); + this.errorNorm[1] = Vector3D.distance(truth.getVelocity(), + estOrbit.getPVCoordinates().getVelocity()); + } + + public double[] getErrorNorm() + { + return(this.errorNorm); + } + } +} -- GitLab From b672cf4019ebaa1f0577131bf0a0c5d35b4f9ecd Mon Sep 17 00:00:00 2001 From: shiva_iyer Date: Tue, 1 Oct 2019 09:02:54 -0500 Subject: [PATCH 2/2] Added Laplace IOD test cases using just Keplerian motion. --- .../orekit/estimation/iod/IodLaplaceTest.java | 122 +++++++++++++----- 1 file changed, 87 insertions(+), 35 deletions(-) diff --git a/src/test/java/org/orekit/estimation/iod/IodLaplaceTest.java b/src/test/java/org/orekit/estimation/iod/IodLaplaceTest.java index 871f93ac1..2beac943d 100644 --- a/src/test/java/org/orekit/estimation/iod/IodLaplaceTest.java +++ b/src/test/java/org/orekit/estimation/iod/IodLaplaceTest.java @@ -31,10 +31,15 @@ import org.orekit.frames.Frame; import org.orekit.frames.FramesFactory; import org.orekit.frames.TopocentricFrame; import org.orekit.orbits.CartesianOrbit; +import org.orekit.orbits.KeplerianOrbit; +import org.orekit.orbits.PositionAngle; +import org.orekit.propagation.Propagator; import org.orekit.propagation.SpacecraftState; +import org.orekit.propagation.analytical.KeplerianPropagator; import org.orekit.propagation.analytical.tle.TLE; import org.orekit.propagation.analytical.tle.TLEPropagator; import org.orekit.time.AbsoluteDate; +import org.orekit.time.TimeScalesFactory; import org.orekit.utils.Constants; import org.orekit.utils.IERSConventions; import org.orekit.utils.AbsolutePVCoordinates; @@ -47,78 +52,125 @@ import org.orekit.Utils; */ public class IodLaplaceTest { private Frame gcrf; + private Frame itrf; + private GroundStation observer; + @Before public void setUp() { Utils.setDataRoot("regular-data"); + this.gcrf = FramesFactory.getGCRF(); this.itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, false); + + // The ground station is set to Austin, Texas, U.S.A + final OneAxisEllipsoid body = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, + Constants.WGS84_EARTH_FLATTENING, itrf); + this.observer = new GroundStation( + new TopocentricFrame(body, new GeodeticPoint(0.528253, -1.705768, 0.0), "Austin")); + this.observer.getPrimeMeridianOffsetDriver().setReferenceDate(AbsoluteDate.J2000_EPOCH); + this.observer.getPolarOffsetXDriver().setReferenceDate(AbsoluteDate.J2000_EPOCH); + this.observer.getPolarOffsetYDriver().setReferenceDate(AbsoluteDate.J2000_EPOCH); } + // Estimate the orbit of ISS (ZARYA) based on Keplerian motion @Test - public void testLaplace1() { - // ISS (ZARYA) + public void testLaplaceKeplerian1() { + final AbsoluteDate date = new AbsoluteDate(2019, 9, 29, 22, 0, 2.0, TimeScalesFactory.getUTC()); + final KeplerianOrbit kep = new KeplerianOrbit(6798938.970424857, 0.0021115522920270016, 0.9008866630545347, + 1.8278985811406743, -2.7656136723308524, + 0.8823034512437679, PositionAngle.MEAN, gcrf, + date, Constants.EGM96_EARTH_MU); + final KeplerianPropagator prop = new KeplerianPropagator(kep); + + // With only 3 measurements, we can expect ~400 meters error in position and ~1 m/s in velocity + final double[] error = estimateOrbit(prop, date, 30.0, 60.0).getErrorNorm(); + Assert.assertEquals(0.0, error[0], 275.0); + Assert.assertEquals(0.0, error[1], 0.8); + } + + // Estimate the orbit of Galaxy 15 based on Keplerian motion + @Test + public void testLaplaceKeplerian2() { + final AbsoluteDate date = new AbsoluteDate(2019, 9, 29, 22, 0, 2.0, TimeScalesFactory.getUTC()); + final KeplerianOrbit kep = new KeplerianOrbit(42165414.60406032, 0.00021743441091199163, 0.0019139259842569903, + 1.8142608912728584, 1.648821262690012, + 0.11710513241172144, PositionAngle.MEAN, gcrf, + date, Constants.EGM96_EARTH_MU); + final KeplerianPropagator prop = new KeplerianPropagator(kep); + + final double[] error = estimateOrbit(prop, date, 60.0, 120.0).getErrorNorm(); + Assert.assertEquals(0.0, error[0], 395.0); + Assert.assertEquals(0.0, error[1], 0.03); + } + + // Estimate the orbit of ISS (ZARYA) based on TLE propagation + @Test + public void testLaplaceTLE1() { final String tle1 = "1 25544U 98067A 19271.53261574 .00000256 00000-0 12497-4 0 9993"; final String tle2 = "2 25544 51.6447 208.7465 0007429 92.6235 253.7389 15.50110361191281"; - final double[] error = estimateFromTLE(tle1, tle2, 30.0, 60.0).getErrorNorm(); + + final TLE tleParser = new TLE(tle1, tle2); + final TLEPropagator tleProp = TLEPropagator.selectExtrapolator(tleParser); + final AbsoluteDate obsDate1 = tleParser.getDate(); + + final double[] error = estimateOrbit(tleProp, obsDate1, 30.0, 60.0).getErrorNorm(); // With only 3 measurements, an error of 5km in position and 10 m/s in velocity is acceptable + // because the Laplace method uses only two-body dynamics Assert.assertEquals(0.0, error[0], 5000.0); Assert.assertEquals(0.0, error[1], 10.0); } + // Estimate the orbit of COSMOS 382 based on TLE propagation @Test - public void testLaplace2() { - // COSMOS 382 + public void testLaplaceTLE2() { final String tle1 = "1 4786U 70103A 19270.85687399 -.00000025 +00000-0 +00000-0 0 9998"; final String tle2 = "2 4786 055.8645 163.2517 1329144 016.0116 045.4806 08.42042146501862"; - final double[] error = estimateFromTLE(tle1, tle2, 30.0, 60.0).getErrorNorm(); + + final TLE tleParser = new TLE(tle1, tle2); + final TLEPropagator tleProp = TLEPropagator.selectExtrapolator(tleParser); + final AbsoluteDate obsDate1 = tleParser.getDate(); + + final double[] error = estimateOrbit(tleProp, obsDate1, 30.0, 60.0).getErrorNorm(); Assert.assertEquals(0.0, error[0], 5000.0); Assert.assertEquals(0.0, error[1], 10.0); } + // Estimate the orbit of GALAXY 15 based on TLE propagation @Test - public void testLaplace3() { - // GALAXY 15 + public void testLaplaceTLE3() { final String tle1 = "1 28884U 05041A 19270.71989132 .00000058 00000-0 00000+0 0 9991"; final String tle2 = "2 28884 0.0023 190.3430 0001786 354.8402 307.2011 1.00272290 51019"; - final double[] error = estimateFromTLE(tle1, tle2, 300.0, 600.0).getErrorNorm(); - Assert.assertEquals(0.0, error[0], 5000.0); - Assert.assertEquals(0.0, error[1], 10.0); - } - private Result estimateFromTLE(final String tle1, final String tle2, - final double t2, final double t3) - { - // Use the TLE propagator final TLE tleParser = new TLE(tle1, tle2); final TLEPropagator tleProp = TLEPropagator.selectExtrapolator(tleParser); + final AbsoluteDate obsDate1 = tleParser.getDate(); - // The ground station is set to Austin, Texas, U.S.A - final OneAxisEllipsoid body = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, - Constants.WGS84_EARTH_FLATTENING, itrf); - final GroundStation observer = new GroundStation( - new TopocentricFrame(body, new GeodeticPoint(0.528253, -1.705768, 0.0), "Austin")); - observer.getPrimeMeridianOffsetDriver().setReferenceDate(AbsoluteDate.J2000_EPOCH); - observer.getPolarOffsetXDriver().setReferenceDate(AbsoluteDate.J2000_EPOCH); - observer.getPolarOffsetYDriver().setReferenceDate(AbsoluteDate.J2000_EPOCH); + final double[] error = estimateOrbit(tleProp, obsDate1, 300.0, 600.0).getErrorNorm(); + Assert.assertEquals(0.0, error[0], 5000.0); + Assert.assertEquals(0.0, error[1], 10.0); + } + // Helper function to generate measurements and estimate orbit for the given propagator + private Result estimateOrbit(final Propagator prop, final AbsoluteDate obsDate1, + final double t2, final double t3) + { // Generate 3 Line Of Sight angles measurements - final AbsoluteDate obsDate1 = tleParser.getDate(); - final Vector3D los1 = getLOSAngles(tleProp, obsDate1, observer); + final Vector3D los1 = getLOSAngles(prop, obsDate1, observer); final AbsoluteDate obsDate2 = obsDate1.shiftedBy(t2); - final Vector3D los2 = getLOSAngles(tleProp, obsDate2, observer); + final Vector3D los2 = getLOSAngles(prop, obsDate2, observer); final AbsoluteDate obsDate3 = obsDate1.shiftedBy(t3); - final Vector3D los3 = getLOSAngles(tleProp, obsDate3, observer); + final Vector3D los3 = getLOSAngles(prop, obsDate3, observer); final TimeStampedPVCoordinates obsPva = observer .getBaseFrame().getPVCoordinates(obsDate2, gcrf); // Estimate the orbit using the classical Laplace method - final TimeStampedPVCoordinates truth = tleProp.getPVCoordinates(obsDate2, gcrf); + final TimeStampedPVCoordinates truth = prop.getPVCoordinates(obsDate2, gcrf); final CartesianOrbit estOrbit = new IodLaplace(Constants.EGM96_EARTH_MU) .estimate(gcrf, obsPva, obsDate1, los1, obsDate2, los2, obsDate3, los3); return(new Result(truth, estOrbit)); @@ -126,20 +178,20 @@ public class IodLaplaceTest { // Helper function to generate a Line of Sight angles measurement for the given // observer and date using the TLE propagator. - private Vector3D getLOSAngles(final TLEPropagator tleProp, final AbsoluteDate date, + private Vector3D getLOSAngles(final Propagator prop, final AbsoluteDate date, final GroundStation observer) { - final TimeStampedPVCoordinates satPvc = tleProp.getPVCoordinates(date, gcrf); + final TimeStampedPVCoordinates satPvc = prop.getPVCoordinates(date, gcrf); final AngularRaDec raDec = new AngularRaDec(observer, gcrf, date, new double[] {0.0, 0.0}, new double[] {1.0, 1.0}, new double[] {1.0, 1.0}, new ObservableSatellite(0)); final double[] angular = raDec.estimate(0, 0, new SpacecraftState[]{new SpacecraftState( new AbsolutePVCoordinates(gcrf, satPvc))}).getEstimatedValue(); - final double azi = angular[0]; - final double ele = angular[1]; + final double ra = angular[0]; + final double dec = angular[1]; - return(new Vector3D(FastMath.cos(ele)*FastMath.cos(azi), - FastMath.cos(ele)*FastMath.sin(azi), FastMath.sin(ele))); + return(new Vector3D(FastMath.cos(dec)*FastMath.cos(ra), + FastMath.cos(dec)*FastMath.sin(ra), FastMath.sin(dec))); } // Private class to calculate the errors between truth and estimated orbits at -- GitLab