Merge pull request #45 from open-space-collective/users/robin/fix-j4

fix: J4 propagation
open-space-collective · Jul 21, 2021 · 5164921 · 5164921
2 parents 482a4c6 + e7e5804
commit 5164921
Show file tree

Hide file tree

Showing 11 changed files with 7,253 additions and 8 deletions.
diff --git a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler.cpp b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler.cpp
@@ -698,6 +698,9 @@ State                           Kepler::CalculateJ4StateAt                  (
     const Real semiMajorAxisAtEpoch_m = aClassicalOrbitalElementSet.getSemiMajorAxis().inMeters() ;
     const Real eccentricityAtEpoch = aClassicalOrbitalElementSet.getEccentricity() ;
     const Real inclinationAtEpoch_rad = aClassicalOrbitalElementSet.getInclination().inRadians() ;
+    const Real raanAtEpoch_rad = aClassicalOrbitalElementSet.getRaan().inRadians() ;
+    const Real aopAtEpoch_rad = aClassicalOrbitalElementSet.getAop().inRadians() ;
+    const Real meanAnomalyAtEpoch_rad = aClassicalOrbitalElementSet.getMeanAnomaly().inRadians() ;
 
     // Calculation
     // Ref: Vallado, D. A (2013). Fundamentals of Astrodynamics and Applications.
@@ -712,7 +715,7 @@ State                           Kepler::CalculateJ4StateAt                  (
     const Real sinInclinationSquared = sinInclination * sinInclination ;
 
     const Real eccentricityAtEpochSquared = eccentricityAtEpoch * eccentricityAtEpoch ;
-    const Real sqrtBeta = std::sqrt(1.0 - eccentricityAtEpochSquared);
+    const Real sqrtBeta = std::sqrt(1.0 - eccentricityAtEpochSquared) ;
 
     const Real expr = (3.0 / 2.0) * aJ2 * std::pow((equatorialRadius_m / p), 2) ;
 
@@ -742,16 +745,16 @@ State                           Kepler::CalculateJ4StateAt                  (
         1.0 + expr * (2.0 + eccentricityAtEpochSquared / 2.0 - 2.0 * sqrtBeta 
         - (43.0 / 24.0 - eccentricityAtEpochSquared / 48.0 - 3.0 * sqrtBeta) * sinInclinationSquared)
     )
-    - 45.0 / 36.0 * aJ2 * aJ2 * n_bar * std::pow(equatorialRadius_m / p, 4) * eccentricityAtEpochSquared * std::pow(cosInclination, 4)
+    - 45.0 / 36.0 * aJ2 * aJ2 * n * std::pow(equatorialRadius_m / p, 4) * eccentricityAtEpochSquared * std::pow(cosInclination, 4)
     - 35.0 / 8.0 * n * aJ4 * std::pow(equatorialRadius_m / p, 4) *
     (
         12.0 / 7.0 - 93.0 / 14.0 * sinInclinationSquared + 21.0 / 4.0 * std::pow(sinInclination, 4)
         + eccentricityAtEpochSquared * (27.0 / 14.0 - 189.0 / 28.0 * sinInclinationSquared + 81.0 / 16.0 * std::pow(sinInclination, 4))
     ) ;
 
-    const Real raan_bar_rad = raan_dot * durationFromEpoch_s ;
-    const Real aop_bar_rad = aop_dot * durationFromEpoch_s ;
-    const Real meanAnomaly_rad = n_bar * durationFromEpoch_s ;
+    const Real raan_bar_rad = raanAtEpoch_rad + raan_dot * durationFromEpoch_s ;
+    const Real aop_bar_rad = aopAtEpoch_rad + aop_dot * durationFromEpoch_s ;
+    const Real meanAnomaly_rad = meanAnomalyAtEpoch_rad + n_bar * durationFromEpoch_s ;
 
     // Orbital parameters at instant
 
@@ -824,7 +827,7 @@ Integer                         Kepler::CalculateJ4RevolutionNumberAt       (
             1.0 + expr * (2.0 + e * e / 2.0 - 2.0 * std::sqrt(1.0 - e * e)
             - (43.0 / 24.0 - e * e / 48.0 - 3.0 * std::sqrt(1.0 - e * e)) * std::pow(std::sin(i_rad), 2))
         )
-        - 45.0 / 36.0 * aJ2 * aJ2 * n_bar * std::pow(R_m / p, 4) * e * e * std::pow(std::cos(i_rad), 4)
+        - 45.0 / 36.0 * aJ2 * aJ2 * n_0 * std::pow(R_m / p, 4) * e * e * std::pow(std::cos(i_rad), 4)
         - 35.0 / 8.0 * n_0 * aJ4 * std::pow(R_m / p, 4) *
         (
             12.0 / 7.0 - 93.0 / 14.0 * std::pow(std::sin(i_rad), 2) + 21.0 / 4.0 * std::pow(std::sin(i_rad), 4)

diff --git a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler.test.cpp
@@ -700,7 +700,7 @@ TEST (OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Kepler, Test_5)
             ASSERT_EQ(*Frame::GCRF(), *velocity_GCRF.accessFrame()) ;
 
             ASSERT_GT(5e-0, (position_GCRF.accessCoordinates() - referencePosition_GCRF).norm()) ;
-            ASSERT_GT(1e-2, (velocity_GCRF.accessCoordinates() - referenceVelocity_GCRF).norm()) ;
+            ASSERT_GT(5e-3, (velocity_GCRF.accessCoordinates() - referenceVelocity_GCRF).norm()) ;
 
             const Shared<const Frame> itrfFrame = Frame::ITRF() ;
 
@@ -713,7 +713,124 @@ TEST (OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Kepler, Test_5)
             ASSERT_EQ(*Frame::ITRF(), *velocity_ITRF.accessFrame()) ;
 
             ASSERT_GT(5e-0, (position_ITRF.accessCoordinates() - referencePosition_ITRF).norm()) ;
-            ASSERT_GT(1e-2, (velocity_ITRF.accessCoordinates() - referenceVelocity_ITRF).norm()) ;
+            ASSERT_GT(5e-3, (velocity_ITRF.accessCoordinates() - referenceVelocity_ITRF).norm()) ;
+
+            // std::cout << "x @ GCRF = " << referencePosition_GCRF.toString(10) << " / " << position_GCRF.accessCoordinates().toString(10) << std::endl ;
+            // std::cout << "x @ ITRF = " << referencePosition_ITRF.toString(10) << " / " << position_ITRF.accessCoordinates().toString(10) << std::endl ;
+            // std::cout << "dx = " << (position_GCRF.accessCoordinates() - referencePosition_GCRF).norm() << " - " << (position_ITRF.accessCoordinates() - referencePosition_ITRF).norm() << std::endl ;
+
+            // std::cout << "v @ GCRF = " << referenceVelocity_GCRF.toString(10) << " / " << velocity_GCRF.accessCoordinates().toString(10) << std::endl ;
+            // std::cout << "v @ ITRF = " << referenceVelocity_ITRF.toString(10) << " / " << velocity_ITRF.accessCoordinates().toString(10) << std::endl ;
+            // std::cout << "dv = " << (velocity_GCRF.accessCoordinates() - referenceVelocity_GCRF).norm() << " - " << (velocity_ITRF.accessCoordinates() - referenceVelocity_ITRF).norm() << std::endl ;
+
+            // std::cout << "dx | dv = " << Real((position_GCRF.accessCoordinates() - referencePosition_GCRF).norm()).toString(12) << " - " << Real((position_ITRF.accessCoordinates() - referencePosition_ITRF).norm()).toString(12) << " | " << Real((velocity_GCRF.accessCoordinates() - referenceVelocity_GCRF).norm()).toString(12) << " - " << Real((velocity_ITRF.accessCoordinates() - referenceVelocity_ITRF).norm()).toString(12) << std::endl ;
+
+        }
+
+    }
+
+}
+
+TEST (OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Kepler, Test_6)
+{
+
+    using ostk::core::types::Shared ;
+    using ostk::core::types::Real ;
+    using ostk::core::ctnr::Array ;
+    using ostk::core::ctnr::Table ;
+    using ostk::core::fs::Path ;
+    using ostk::core::fs::File ;
+
+    using ostk::math::obj::Vector3d ;
+
+    using ostk::physics::units::Length ;
+    using ostk::physics::units::Angle ;
+    using ostk::physics::units::Derived ;
+    using ostk::physics::time::Scale ;
+    using ostk::physics::time::Instant ;
+    using ostk::physics::time::Duration ;
+    using ostk::physics::time::Interval ;
+    using ostk::physics::time::DateTime ;
+    using ostk::physics::coord::Frame ;
+    using ostk::physics::coord::Position ;
+    using ostk::physics::coord::Velocity ;
+    using ostk::physics::Environment ;
+    using ostk::physics::env::obj::celest::Earth ;
+
+    using ostk::astro::trajectory::Orbit ;
+    using ostk::astro::trajectory::State ;
+    using ostk::astro::trajectory::orbit::models::Kepler ;
+    using ostk::astro::trajectory::orbit::models::kepler::COE ;
+
+    {
+
+        // Environment setup
+
+        const Environment environment = Environment::Default() ;
+
+        // Orbital model setup
+
+        const Length semiMajorAxis = Length::Kilometers(6878.14) ;
+        const Real eccentricity = 0.0012 ;
+        const Angle inclination = Angle::Degrees(97.5106) ;
+        const Angle raan = Angle::Degrees(319.2343) ;
+        const Angle aop = Angle::Degrees(187.4562) ;
+        const Angle trueAnomaly = Angle::Degrees(172.6498) ;
+
+        const COE coe = { semiMajorAxis, eccentricity, inclination, raan, aop, trueAnomaly } ;
+
+        const Instant epoch = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC) ;
+        const Derived gravitationalParameter = Earth::Models::EGM96::GravitationalParameter ;
+        const Length equatorialRadius = Earth::Models::EGM96::EquatorialRadius ;
+        const Real J2 = Earth::Models::EGM96::J2 ;
+        const Real J4 = Earth::Models::EGM96::J4 ;
+
+        const Kepler keplerianModel = { coe, epoch, gravitationalParameter, equatorialRadius, J2, J4, Kepler::PerturbationType::J4 } ;
+
+        // Orbit setup
+
+        const Orbit orbit = { keplerianModel, environment.accessCelestialObjectWithName("Earth") } ;
+
+        // Reference data setup
+
+        const Table referenceData = Table::Load(File::Path(Path::Parse("/app/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Kepler/Test_6/Satellite Orbit.csv")), Table::Format::CSV, true) ;
+
+        // Orbit test
+
+        for (const auto& referenceRow : referenceData)
+        {
+
+            const Instant instant = Instant::DateTime(DateTime::Parse(referenceRow[0].accessString()), Scale::UTC) ;
+
+            const Vector3d referencePosition_GCRF = { referenceRow[1].accessReal(), referenceRow[2].accessReal(), referenceRow[3].accessReal() } ;
+            const Vector3d referenceVelocity_GCRF = { referenceRow[4].accessReal(), referenceRow[5].accessReal(), referenceRow[6].accessReal() } ;
+
+            const Vector3d referencePosition_ITRF = { referenceRow[7].accessReal(), referenceRow[8].accessReal(), referenceRow[9].accessReal() } ;
+            const Vector3d referenceVelocity_ITRF = { referenceRow[10].accessReal(), referenceRow[11].accessReal(), referenceRow[12].accessReal() } ;
+
+            const State state_GCRF = orbit.getStateAt(instant) ;
+
+            const Position position_GCRF = state_GCRF.accessPosition() ;
+            const Velocity velocity_GCRF = state_GCRF.accessVelocity() ;
+
+            ASSERT_EQ(*Frame::GCRF(), *position_GCRF.accessFrame()) ;
+            ASSERT_EQ(*Frame::GCRF(), *velocity_GCRF.accessFrame()) ;
+
+            ASSERT_GT(5e-0, (position_GCRF.accessCoordinates() - referencePosition_GCRF).norm()) ;
+            ASSERT_GT(5e-3, (velocity_GCRF.accessCoordinates() - referenceVelocity_GCRF).norm()) ;
+
+            const Shared<const Frame> itrfFrame = Frame::ITRF() ;
+
+            const State state_ITRF = state_GCRF.inFrame(itrfFrame) ;
+
+            const Position position_ITRF = state_ITRF.accessPosition() ;
+            const Velocity velocity_ITRF = state_ITRF.accessVelocity() ;
+
+            ASSERT_EQ(*Frame::ITRF(), *position_ITRF.accessFrame()) ;
+            ASSERT_EQ(*Frame::ITRF(), *velocity_ITRF.accessFrame()) ;
+
+            ASSERT_GT(5e-0, (position_ITRF.accessCoordinates() - referencePosition_ITRF).norm()) ;
+            ASSERT_GT(5e-3, (velocity_ITRF.accessCoordinates() - referenceVelocity_ITRF).norm()) ;
 
             // std::cout << "x @ GCRF = " << referencePosition_GCRF.toString(10) << " / " << position_GCRF.accessCoordinates().toString(10) << std::endl ;
             // std::cout << "x @ ITRF = " << referencePosition_ITRF.toString(10) << " / " << position_ITRF.accessCoordinates().toString(10) << std::endl ;