diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index 6e396809..256bbdd4 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -91,13 +91,13 @@ double QuasiNonsingularRelativeOrbitalElements::CalcDiffMeanArgLat_rad(const Qua const double cos_theta = cos(qns_oe_reference.GetTrueLatAng_rad()); const double sin_theta = sin(qns_oe_reference.GetTrueLatAng_rad()); const double e_cos_f = q1 * cos_theta + q2 * sin_theta; - const double sin_f = (q1 * sin_theta - q2 * cos_theta) / e; - const double denominator = (1.0 + e_cos_f) * (1.0 + e_cos_f); - // Difference Info const double arg_peri_ref_rad = atan2(qns_oe_reference.GetEccentricityY(), qns_oe_reference.GetEccentricityX()); const double true_anomaly_ref_rad = qns_oe_reference.GetTrueLatAng_rad() - arg_peri_ref_rad; + const double sin_f = sin(true_anomaly_ref_rad); + + // Difference Info const double arg_peri_target_rad = atan2(qns_oe_target.GetEccentricityY(), qns_oe_target.GetEccentricityX()); const double true_anomaly_target_rad = qns_oe_target.GetTrueLatAng_rad() - arg_peri_target_rad; const double d_true_anomaly_rad = true_anomaly_target_rad - true_anomaly_ref_rad; @@ -105,7 +105,6 @@ double QuasiNonsingularRelativeOrbitalElements::CalcDiffMeanArgLat_rad(const Qua const double q1_target = qns_oe_reference.GetEccentricityX(); const double q2_target = qns_oe_reference.GetEccentricityY(); const double e_target = sqrt(q1_target * q1_target + q2_target * q2_target); - const double d_e = e_target - e; const double d_mean_arg_lat_rad = (eta / denominator) * (eta2 * d_true_anomaly_rad - sin_f * (2.0 + e_cos_f) * d_e); diff --git a/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp index c9d53a9e..af597407 100644 --- a/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp @@ -21,6 +21,18 @@ TEST(QuasiNonsingularRelativeOrbitalElements, ConstructorWithOe) { const double target_true_latitude_angle_rad = 0.5; QuasiNonsingularOrbitalElements target_qn_oe(target_semi_major_axis_m, target_eccentricity_x, target_eccentricity_y, target_inclination_rad, target_raan_rad, target_true_latitude_angle_rad); + + QuasiNonsingularRelativeOrbitalElements qn_roe(reference_qn_oe, target_qn_oe); + + EXPECT_NEAR((target_semi_major_axis_m - reference_semi_major_axis_m) / reference_semi_major_axis_m, qn_roe.GetDeltaSemiMajor(), 1); + // When reference is circular orbit + EXPECT_NEAR(target_true_latitude_angle_rad - reference_true_latitude_angle_rad, qn_roe.GetDeltaMeanLongitude(), 1e-6); + + EXPECT_NEAR(target_eccentricity_x - reference_eccentricity_x, qn_roe.GetDeltaEccentricityX(), 1e-6); + EXPECT_NEAR(target_eccentricity_y - reference_eccentricity_y, qn_roe.GetDeltaEccentricityY(), 1e-6); + + EXPECT_NEAR(target_inclination_rad - reference_inclination_rad, qn_roe.GetDeltaInclinationX(), 1e-3); + EXPECT_NEAR((target_raan_rad - reference_raan_rad) * sin(reference_inclination_rad), qn_roe.GetDeltaInclinationY(), 1e-3); } TEST(QuasiNonsingularRelativeOrbitalElements, ConstructorWithPositionVelocity) {