Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix Relative Orbit Dynamics with Yamanaka-Ankersen's STM #696

Open
wants to merge 2 commits into
base: feature/carter_stm
Choose a base branch
from
Open

Fix Relative Orbit Dynamics with Yamanaka-Ankersen's STM #696

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
28a7245
fix transition matrix for Yamanaka-Ankersen's STM
TomokiMochizuki Sep 24, 2024
bfbef2a
ffix *.ini file
TomokiMochizuki Sep 24, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion settings/sample_satellite/satellite_sub.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ relative_orbit_update_method = 1
relative_dynamics_model_type = 0
// STM Relative Dynamics model type (only valid for STM update)
// 0: HCW, 1: Melton, 2: SS, 3: Sabatini, 4: Carter, 5: Yamanaka-Ankersen
stm_model_type = 4
stm_model_type = 5
// Initial satellite position relative to the reference satellite in LVLH frame[m]
// * The coordinate system is defined at [PLANET_SELECTION] in SampleSimBase.ini
initial_relative_position_lvlh_m(0) = 0.0
Expand Down
2 changes: 1 addition & 1 deletion src/dynamics/orbit/relative_orbit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ void RelativeOrbit::InitializeStmMatrix(orbit::StmModel stm_model_type, const Or
relative_orbit_carter_.CalculateInitialInverseMatrix(gravity_constant_m3_s2, f_ref_rad, &reference_oe);
break;
case orbit::StmModel::kYamakawaAnkersen:
relative_orbit_yamanaka_ankersen_.CalculateInitialInverseMatrix(f_ref_rad, &reference_oe);
relative_orbit_yamanaka_ankersen_.CalculateInitialInverseMatrix(gravity_constant_m3_s2, f_ref_rad, &reference_oe);
break;

default:
Expand Down
11 changes: 9 additions & 2 deletions src/math_physics/orbit/relative_orbit_yamanaka_ankersen.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

#include <environment/global/physical_constants.hpp>

#include "./relative_orbit_models.hpp"
#include "./sgp4/sgp4unit.h" // for getgravconst()

namespace orbit {
Expand All @@ -14,9 +15,12 @@ RelativeOrbitYamanakaAnkersen::RelativeOrbitYamanakaAnkersen() {}

RelativeOrbitYamanakaAnkersen::~RelativeOrbitYamanakaAnkersen() {}

void RelativeOrbitYamanakaAnkersen::CalculateInitialInverseMatrix(double f_ref_rad, OrbitalElements* reference_oe) {
void RelativeOrbitYamanakaAnkersen::CalculateInitialInverseMatrix(double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) {
// The following variables are defined in Spacecraft Formation Flying (Section 5.6.4)
const double e = reference_oe->GetEccentricity();
const double a = reference_oe->GetSemiMajorAxis_m();
const double h = pow(a * (1.0 - pow(e, 2)) * gravity_constant_m3_s2, 0.5); // angular momentum

const double eta = pow(1 - pow(e, 2.0), 0.5);
const double k = 1 + e * cos(f_ref_rad);
const double c = k * cos(f_ref_rad);
Expand Down Expand Up @@ -69,6 +73,9 @@ void RelativeOrbitYamanakaAnkersen::CalculateInitialInverseMatrix(double f_ref_r
initial_inverse_matrix_[i][j] = 1 / pow(eta, 2.0) * initial_inverse_matrix_[i][j];
}
}

initial_inverse_matrix_ =
initial_inverse_matrix_ * orbit::CalcStateTransformationMatrixLvlhToTschaunerHampel(gravity_constant_m3_s2, e, h, f_ref_rad);
}

math::Matrix<6, 6> RelativeOrbitYamanakaAnkersen::CalculateSTM(double gravity_constant_m3_s2, double elapsed_time_s, double f_ref_rad,
Expand Down Expand Up @@ -128,7 +135,7 @@ math::Matrix<6, 6> RelativeOrbitYamanakaAnkersen::CalculateSTM(double gravity_co
update_matrix[5][4] = 0.0;
update_matrix[5][5] = cos(f_ref_rad);

return update_matrix * initial_inverse_matrix_;
return orbit::CalcStateTransformationMatrixTschaunerHampelToLvlh(gravity_constant_m3_s2, e, h, f_ref_rad) * update_matrix * initial_inverse_matrix_;
}

} // namespace orbit
3 changes: 2 additions & 1 deletion src/math_physics/orbit/relative_orbit_yamanaka_ankersen.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,10 +31,11 @@ class RelativeOrbitYamanakaAnkersen {
/**
* @fn CalculateInitialInverseMatrix
* @brief Calculate position and velocity with Kepler orbit propagation
* @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2]
* @param [in] f_ref_rad: True anomaly of the reference satellite [rad]
* @param [in] reference_oe: Orbital elements of reference satellite
*/
void CalculateInitialInverseMatrix(double f_ref_rad, OrbitalElements* reference_oe);
void CalculateInitialInverseMatrix(double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe);

/**
* @fn CalculateSTM
Expand Down
Loading