Spacecraft Control Framework 1.0
Spacecraft Control Library
|
Holds the formation flying data structures and function definitions. More...
Go to the source code of this file.
Classes | |
struct | alf_orb_data_s |
Alfriend orbital elements data structure. More... | |
struct | team_s |
Team data structure. More... | |
struct | state_s |
State data structure. More... | |
struct | geom_s |
Geometry data structure. More... | |
struct | ecc_geom_s |
Eccentric geometry data structure. More... | |
struct | ecc_geom_xy_s |
Eccentric geometry data structure (xy) More... | |
struct | window_s |
Time window data structure. More... | |
struct | plan_param_s |
Planning parameters data structure. More... | |
struct | constraints_s |
Constraints data structure. More... | |
struct | team_goals_s |
Team goals data structure. More... | |
struct | ecc_team_goals_s |
Eccentric team goals data structure. More... | |
struct | cost_s |
Cost estimate data structure. More... | |
struct | burn_s |
Burn data structure. More... | |
struct | maneuver_s |
Maneuver data structure. More... | |
struct | dv_command_s |
Delta-V command data structure. More... | |
struct | orientation_s |
Orientation data structure. More... | |
struct | assign_s |
Assignment data structure. More... | |
struct | coll_mon_data_s |
Collision monitor data. More... | |
Typedefs | |
typedef struct alf_orb_data_s | alf_orb_data_t |
Alfriend orbital elements data structure. | |
typedef struct team_s | team_t |
Team data structure. | |
typedef struct state_s | state_t |
State data structure. | |
typedef struct geom_s | geom_t |
Geometry data structure. | |
typedef struct ecc_geom_s | ecc_geom_t |
Eccentric geometry data structure. | |
typedef struct ecc_geom_xy_s | ecc_geom_xy_t |
Eccentric geometry data structure (xy) | |
typedef struct window_s | window_t |
Time window data structure. | |
typedef struct plan_param_s | plan_param_t |
Planning parameters data structure. | |
typedef struct constraints_s | constraints_t |
Constraints data structure. | |
typedef struct team_goals_s | team_goals_t |
Team goals data structure. | |
typedef struct ecc_team_goals_s | ecc_team_goals_t |
Eccentric team goals data structure. | |
typedef struct cost_s | cost_t |
Cost estimate data structure. | |
typedef struct burn_s | burn_t |
Burn data structure. | |
typedef struct maneuver_s | maneuver_t |
Maneuver data structure. | |
typedef struct dv_command_s | dv_command_t |
Delta-V command data structure. | |
typedef struct orientation_s | orientation_t |
Orientation data structure. | |
typedef struct assign_s | assign_t |
Assignment data structure. | |
typedef struct coll_mon_data_s | coll_mon_data_t |
Collision monitor data. | |
Enumerations | |
enum | ff_error_codes { FF_NO_ERROR = 0 , FF_NO_SOLN = 1 , FF_MAX_ITER = 2 , FF_BAD_VALUE = 3 } |
Error codes for invalid matrix operations. More... | |
enum | ff_formation_types { FF_IPLF = 1 , FF_OOPLF_RGT = 2 , FF_CIPE_REF_CENTER = 3 , FF_CIPE_REF_ON = 4 , FF_POS_PROJ_CIRC_REF_CENTER = 5 , FF_POS_PROJ_CIRC_REF_ON = 6 , FF_NEG_PROJ_CIRC_REF_CENTER = 7 , FF_NEG_PROJ_CIRC_REF_ON = 8 , FF_DUAL_PROJ_CIRC_REF_CENTER = 9 , FF_DUAL_PROJ_CIRC_REF_ON = 10 , FF_TETRA = 11 } |
Formation type enumerations. More... | |
Functions | |
Control Law | |
maneuver_s | IterativeImpulsiveManeuver (state_t state, geom_t goals_circ, window_t window, plan_param_t param, bool &foundSoln) |
Computes the delta-v's required to implement a formation flying maneuver for a single spacecraft. More... | |
maneuver_s | IterativeImpulsiveManeuver (state_t state, ecc_geom_t goals_ecc, window_t window, plan_param_t param, bool &foundSoln) |
Computes the delta-v's required to implement a formation flying maneuver for a single spacecraft. (eccentric orbit) More... | |
maneuver_s | ImpulsiveLPManeuver (state_t state, geom_t goals_circ, window_t window, plan_param_t param, double &maxDV) |
Plans an impulsive burn sequence (using simplex) to achieve the specified trajectory within the given time window. (circular orbit) More... | |
maneuver_s | ImpulsiveLPManeuver (state_t state, ecc_geom_t goals_ecc, window_t window, plan_param_t param, double &maxDV) |
Plans an impulsive burn sequence (using simplex) to achieve the specified trajectory within the given time window. (eccentric orbit) More... | |
maneuver_s | ImpulsiveManeuver (state_t state, geom_t goals, window_t window, plan_param_t param, double &maxDV) |
Plans an impulsive burn sequence (using closed-form equations) to achieve the specified trajectory within the given time window. For circular orbits only. More... | |
bool | OptimalInPlaneDeltaV (double nOrbMin, double nOrbMax, double a, double th0, double th1, double delta_a, double delta_th0, double delta_q1, double delta_q2, double &dV1, double &dV2, double &dV3, unsigned short &M, unsigned short &N) |
Computes the in-plane delta-v sequence for the given time window that achieves the desired element differences and results in the minimum total delta-v. For circular orbits only. More... | |
burn_t * | InPlane (alf_orb_data_t meas_elem, alf_orb_data_t delta_elem, double accNom, double dTMin, double nOrbMin, double nOrbMax, double horizon, double &t0, double &tF, unsigned short &nBurns) |
Computes the in-plane burn sequence for the given time window that achieves the desired element differences. For circular orbits only. More... | |
burn_t | OutOfPlane (alf_orb_data_t meas_elem, alf_orb_data_t delta_elem, double accNom, double dTMin, unsigned short &nBurns, double &th1) |
Compute the out-of-plane burn sequence for the given time window that achieves the desired element differences. For circular orbits only. More... | |
Guidance Law | |
geom_t | AutoFormGeometry (state_t state, geom_t memberGoals[], int num, double minSepDist, double maxSepDist) |
ecc_geom_t | AutoFormGeometry (state_t state, ecc_geom_t memberGoals[], int num, double minSepDist, double maxSepDist) |
Define new geometric goals for a single satellite, such that any semi-major axis difference is eliminated, any radial oscillation is eliminated, and the new trajectory maintains a minimum separation distance from all other known trajectories. More... | |
double | NearestOffset (double y0, double width, ml_matrix extrema, double minSepDistance, double maxSepDistance) |
Determine the nearest along-track offset for a trajectory that is safe such that the minimum and maximum required separation distances are respected. More... | |
ml_matrix | InitializeCostMatrix (team_goals_t teamGoals, int nRelatives) |
Given the team goals, initialize the cost matrix "f" with the right size. More... | |
ml_matrix | InitializeCostMatrix (ecc_team_goals_t teamGoals, int nRelatives) |
Given the team goals, initialize the cost matrix "f" with the right size. More... | |
int | PopulateCostMatrix (ml_matrix &f, cost_t costEstimate, team_goals_t teamGoals, ml_int_array relativeIDs) |
Fill in a single column of the cost matrix. More... | |
int | PopulateCostMatrix (ml_matrix &f, cost_t costEstimate, ecc_team_goals_t teamGoals, ml_int_array relativeIDs) |
Fill in a single column of the cost matrix. More... | |
void | CostMatrixRows (team_goals_t teamGoals, int index, int &a, int &b) |
Compute the starting and ending rows in the cost matrix that correspond to a give target state index. More... | |
void | CostMatrixRows (ecc_team_goals_t teamGoals, int index, int &a, int &b) |
Compute the starting and ending rows in the cost matrix that correspond to a give target state index. More... | |
cost_s | EstimateCost (alf_orb_data_t el0, alf_orb_data_t dEl, team_goals_t teamGoals, int memID, window_t window, double weight) |
Estimate the (weighted) cost to achieve all specified unique target states. More... | |
cost_s | FFEccEstimateCost (orb_data_t el0, const ml_matrix &xH0, ecc_team_goals_t teamGoals, int memID, window_t window, double weight, int nSPO) |
Estimate the (weighted) cost to achieve all specified unique target states. More... | |
team_goals_s | GenerateTeamGoals (alf_orb_data_t el0, int fType, double fSize, unsigned short nRels, int teamID, double angRes) |
Generate a set of team goals given the formation type and size. More... | |
ecc_team_goals_s | FFEccGenerateTeamGoals (orb_data_t el0, int fType, double fSize, unsigned short nRels, int teamID, double nu, const ml_matrix &eul) |
Generate a set of team goals given the formation type, size, location and orientation. More... | |
ecc_geom_xy_s * | FFEccTetrahedronGeometry (double nu, double d, const ml_matrix &eul) |
Compute the geometric goals for a formation that achieves a tetrahedron shape at a given true anomaly in an eccentric reference orbit. More... | |
geom_s | PCGoals (double R, double alpha0, double sgn, double y0) |
Generate the geometric goals for a projected circular formation. More... | |
void | SortTeamGoals (team_goals_t &teamGoals) |
Sort the team goals with fixed states listed before variable states. More... | |
void | SortTeamGoals (ecc_team_goals_t &teamGoals) |
Sort the team goals with fixed states listed before variable states. More... | |
assign_s | SetupAssignmentProblem (team_goals_t teamGoals, double orbFrac=1) |
Set up the assignment problem given the team goals by computing the number of fixed, variable and unique variable states, plus with the phases and indices of repeated variable states. More... | |
assign_s | SetupAssignmentProblem (ecc_team_goals_t teamGoals, double orbFrac=1) |
Set up the assignment problem given the team goals by computing the number of fixed, variable and unique variable states, plus with the phases and indices of repeated variable states. More... | |
int | RestrictIDSet (ml_int_array &relIDs, constraints_t constraints[], int numC) |
Restrict the set of relative IDs to respect the specified assignment constraints. More... | |
int | VerifyAssignmentParams (assign_t a, const ml_matrix &f) |
Check for inconsistencies in the assignment parameters. More... | |
int | PrivilegedAssignment (assign_t assign, const ml_matrix &f, int method, ml_int_array &optOrder, ml_matrix &optPhi, double &optCost) |
Assign N targets to N satellites one at a time, so that the satellite with the largest minimum cost is assigned its min-cost target first. More... | |
int | OptimalAssignment (assign_t assign, ml_matrix f, ml_int_array &optOrder, ml_matrix &optPhi, double &optCost) |
Assign N targets to N satellites so that the weighted sum of costs to reach all targets is minimized. More... | |
bool | EquallyPhased (const ml_matrix &phi, ml_int_array u, int Pu) |
Determine if the phase angles are equally phased. More... | |
ml_int_array | FindMinSet (ml_matrix &mat, double &minSum) |
Find the order in which to arrange the columns so that the matrix diagonal sum is minimized. More... | |
void | permute (ml_int_array arr, int start, int num, const ml_matrix &mat, double &cost, ml_int_array &best) |
Permute the supplied array, called recursively by FindMinSet. More... | |
Linear Programming | |
void | LPCircular (const ml_matrix &x0, const ml_matrix &xF, double n, double duration, double dT, unsigned short constraintType, double maxConstraint, ml_matrix &aC, ml_matrix &t, bool &exitFlag) |
Computes the thrust trajectory to go from an initial state to a final state in Hills frame. For circular orbits. More... | |
void | LPEccentric (double e, double n, const ml_matrix &x0, const ml_matrix &xF, double nu0, double nuF, int nS, ml_matrix cW, unsigned short constraintType, double maxConstraint, ml_matrix &aC, ml_matrix &t, bool &exitFlag) |
Computes the thrust trajectory to go from an initial state to a final state in Hills frame. For eccentric orbits. More... | |
void | LPCircularTimeWeight (orb_data_t el0, const ml_matrix &xH0, geom_t goals, window_t window, int nSPO, ml_matrix &xHF, double &nOrbMvr) |
Determine the target state on the desired trajectory that gives the minimum time-weighted cost. For circular orbits. More... | |
void | LPEccentricTimeWeight (orb_data_t el0, const ml_matrix &xH0, ecc_geom_t goals, window_t window, int nSPO, ml_matrix &xHF, double &nOrbMvr) |
Determine the target state on the desired trajectory that gives the minimum time-weighted cost. For eccentric orbits. More... | |
void | GVEErrorDynamics (orb_data_t el0, ml_matrix &A, ml_matrix &B) |
Compute continuous-time dynamics for Gauss' variational equations. More... | |
void | LPEccentricGVE (orb_data_t el0, const ml_matrix &x0, const ml_matrix &xF, double MF, int nS, ml_matrix cW, unsigned short constraintType, double maxConstraint, ml_matrix &aC, ml_matrix &t, bool &exitFlag) |
Computes the thrust trajectory to go from an initial state to a final state in Hills frame. For eccentric orbits. More... | |
Eccentric Geometry | |
void | FFEccLinOrb (double n, double nu, double e, ml_matrix &a, ml_matrix &b, double dT) |
Compute the continuous A,B matrices for linearized relative motion in an eccentric reference orbit, discretized with a zero-order hold with timestep dT. More... | |
ml_matrix | FFEccLinOrb (const ml_matrix &x, const ml_matrix &acc, double n, double nu, double e) |
Compute the right-hand-side for linearized relative motion in an eccentric reference orbit. More... | |
ml_matrix | FFEccProp (const ml_matrix &D, const ml_matrix &nu, double e, double dH) |
Compute Hills frame state at specified true anomaly given integration constants and eccentricity. More... | |
ml_matrix | FFEccProp (const ml_matrix &D, double nu, double e, double dH) |
Compute Hills frame state at specified true anomaly given integration constants and eccentricity. More... | |
double | FFEccDH (double nu0, double e) |
Compute integration constant dH for homogeneous solution to LTV differential equations of relative orbit motion. Found by setting H=0 at initial true anomaly. More... | |
void | FFEccGoals (double e, ecc_geom_t goals, ml_matrix &D, ml_matrix &xH) |
Compute integration constants and initial state from the geometric goals. More... | |
void | FFEccGoals (double e, ecc_geom_xy_t goals, ml_matrix &D, ml_matrix &xH) |
Compute integration constants and initial state from the geometric goals. More... | |
void | FFEccIntConst (const ml_matrix &xH0, double nu0, double e, ml_matrix &D, double &dH, ml_matrix &R) |
Compute integration constants for homogeneous solution to LTV differential equations of relative orbit motion. More... | |
ml_matrix | FFEccRMat (double nu, double e, double dH) |
Compute the state-transition matrix, R, given the eccentricity and true anomaly so that xH = R*D, where D is the vector of integration constants found from initial conditions. More... | |
ml_matrix | FFEccRMat (double nu, double e) |
Compute the state-transition matrix, R, given the eccentricity and true anomaly so that xH = R*D, where D is the vector of integration constants found from initial conditions. Here, the integration constant "dH" is computed using the first element of the "nu" matrix. More... | |
void | FFEccXExt (double e, const ml_matrix &D, double dH, double epsilon, int nMax, ml_matrix &x, ml_matrix &nu) |
Compute extreme x-values and associated true anomalies for given relative motion. More... | |
void | FFEccYExt (double e, const ml_matrix &D, double dH, double epsilon, int nMax, ml_matrix &y, ml_matrix &nu) |
Compute extreme y-values and associated true anomalies for given relative motion. More... | |
void | FFEccZExt (double e, const ml_matrix &D, double dH, double epsilon, int nMax, ml_matrix &z, ml_matrix &nu) |
Compute extreme z-values and associated true anomalies for given relative motion. More... | |
void | NuDot (double n, double e, double nu, double &nuDot, double &nuDotDot) |
Compute the time-derivative of the true anomaly. *sctlib. More... | |
double | FFEccDX (double nu, double e, const ml_matrix &D, double dH) |
Compute the first derivative of x with respect to true anomaly. More... | |
double | FFEccDY (double nu, double e, const ml_matrix &D, double dH) |
Compute the first derivative of y with respect to true anomaly. More... | |
double | FFEccDZ (double nu, double e, const ml_matrix &D, double dH) |
Compute the first derivative of z with respect to true anomaly. More... | |
double | FFEccDDX (double nu, double e, const ml_matrix &D, double dH) |
Compute the second derivative of x with respect to true anomaly. More... | |
double | FFEccDDY (double nu, double e, const ml_matrix &D, double dH) |
Compute the second derivative of y with respect to true anomaly. More... | |
double | FFEccDDZ (double nu, double e, const ml_matrix &D, double dH) |
Compute the second derivative of z with respect to true anomaly. More... | |
Coordinate Frames | |
orb_data_t | Alfriend2El (alf_orb_data_t elA) |
Convert an Alfriend orbital element set into the standard orbital element set *sctlib. More... | |
alf_orb_data_t | El2Alfriend (orb_data_t el) |
Convert a standard orbital element set into the Alfriend orbital element set *sctlib. More... | |
alf_orb_data_t | El2Alfriend (orb_data_t el, double true_anom) |
Convert a standard orbital element set into the Alfriend orbital element set *sctlib. More... | |
alf_orb_data_s | add_elements (alf_orb_data_t el1, alf_orb_data_t el2) |
Add two sets of orbital elements *sctlib. More... | |
orb_data_s | add_elements (orb_data_t el1, orb_data_t el2) |
Add two sets of orbital elements *sctlib. More... | |
alf_orb_data_s | sub_elements (alf_orb_data_t el1, alf_orb_data_t el2) |
Subtract one element set from another. More... | |
orb_data_s | sub_elements (orb_data_t el1, orb_data_t el2) |
Subtract one element set from another. More... | |
double | CirclePhase (double beta) |
Compute the desired phase on a circle from the desired phase on the ellipse. The circle is superscribed about the ellipse. More... | |
double | EllipsePhase (double alpha) |
Compute the desired phase on an ellipse from the desired phase on the circle. The circle is superscribed about the ellipse. More... | |
alf_orb_data_t | OrbElemDiff (alf_orb_data_t el0, alf_orb_data_t el) |
Computes the differences between Alfriend orbital element vectors. More... | |
orb_data_t | OrbElemDiff (orb_data_t el0, orb_data_t el) |
Computes the differences between standard orbital element vectors. More... | |
alf_orb_data_t | Goals2DeltaElem (alf_orb_data_t el0, geom_t goals) |
Computes the desired element differences, given the geometric goals and the measured orbital elements. More... | |
geom_t | DeltaElem2Goals (alf_orb_data_t el0, alf_orb_data_t dEl) |
Computes the geoemtric goals, given the desired element differences and the measured orbital elements. More... | |
ml_matrix | DeltaElem2Hills (alf_orb_data_t elA, alf_orb_data_t dEl) |
Computes the Hills frame state from the element differences and reference elements. More... | |
alf_orb_data_s | Hills2DeltaElem (alf_orb_data_t el0, const ml_matrix &xH) |
Computes the element differences from the Hills frame state and reference elements. More... | |
geom_s | Hills2Goals (alf_orb_data_t el0, const ml_matrix &xH) |
Computes the geometric goals from the Hills frame state and reference elements. More... | |
ml_matrix | Goals2Hills (alf_orb_data_t el0, geom_t goals) |
Computes the Hills frame state from the geometric goals and reference elements. More... | |
orb_data_s | FFEccHills2DeltaElem (orb_data_t el0, const ml_matrix &xH) |
Computes the orbital element differences from the Hills frame state and the reference orbital elements. More... | |
alf_orb_data_s | FFEccHills2DeltaElem (alf_orb_data_t el0, const ml_matrix &xH) |
Computes the orbital element differences from the Hills frame state and the reference orbital elements. More... | |
ml_matrix | FFEccDeltaElem2Hills (alf_orb_data_t el0, alf_orb_data_t dEl) |
Convert element differences to Hills frame coordinates in an eccentric orbit. More... | |
ecc_geom_t | FFEccDeltaElem2Goals (alf_orb_data_t el0, alf_orb_data_t dEl) |
Convert element differences to eccentric geometric goals. More... | |
ml_matrix | FFEccGoals2Hills (double e, double nu, ecc_geom_t g, double n) |
Compute Hills frame state (in time-domain) given geometric goals and orbit data. More... | |
ml_matrix | FFEccGoals2Hills (double e, double nu, ecc_geom_xy_t g, double n) |
Compute Hills frame state (in time-domain) given geometric goals and orbit data. More... | |
ml_matrix | FFEccGoals2Hills (double e, double nu, ecc_geom_t g) |
Compute Hills frame state (in nu-domain) given geometric goals and orbit data. More... | |
ecc_geom_t | FFEccHills2Goals (double e, double nu, ml_matrix xH, double n) |
Compute geometric goals given Hills frame state (in time-domain) and orbit data. More... | |
ecc_geom_t | FFEccHills2Goals (double e, double nu, const ml_matrix &xH) |
Compute geometric goals given Hills frame state (in nu-domain) and orbit data. More... | |
void | Nu2TimeDomain (ml_matrix &x, double n, double e, double nu) |
Convert a relative orbit state from the nu-domain to the time-domain. More... | |
void | Time2NuDomain (ml_matrix &x, double n, double e, double nu) |
Convert a relative orbit state from the time-domain to the nu-domain. More... | |
ecc_geom_t | GeometryCirc2Ecc (double w, geom_t gCirc) |
Convert a circular geometry structure to an eccentric geometry structure. More... | |
geom_t | GeometryEcc2Circ (double w, ecc_geom_t gEcc) |
Convert an eccentric geometry structure to a circular geometry structure. More... | |
geom_t | RotateState (geom_t goals, double phi) |
Rotate a geometric goal set to the circular phase angle phi. More... | |
double | MeanAnom2TrueLat (double e, double w, double M) |
Convert mean anomlay to true latitude. More... | |
void | GetHillsMats (const ml_matrix &r0, const ml_matrix &v0, ml_matrix &A, ml_matrix &Adot) |
Computes the A and Adot matrices used for transformation to Hills frame. More... | |
void | AbsRelECI2Hills (ml_matrix r0, ml_matrix v0, ml_matrix dr, ml_matrix dv, ml_matrix &rH, ml_matrix &vH) |
Compute the relative position and velocity in Hills frame given an absolute ECI position & velocity and a relative inertial position & velocity. More... | |
alf_orb_data_t | Osc2Mean (alf_orb_data_t el, double J2) |
Transforms osculating orbital elements to mean orbital elements. More... | |
alf_orb_data_t * | ECI2MeanElements (const ml_matrix &xRefECI, const ml_matrix &xRelECI, double J2, alf_orb_data_t &elRefMean) |
Computes mean orbital elements and mean orbital element differences from reference and relative ECI position and velocity. More... | |
bool | AlignThruster (const ml_matrix &aH, const ml_matrix &bTh, ml_matrix &qHB) |
Computes desired Hills-to-body quaternion for a thruster firing. Rotation about thruster-axis is ignored. More... | |
bool | AlignThruster (const ml_matrix &aH, const ml_matrix &bTh, const ml_matrix &bST, const ml_matrix &r, const ml_matrix &v, double jD, double sep, ml_matrix &qHB) |
Computes desired Hills-to-body quaternion for a thruster firing such that the star tracker is pointed as far away as possible from the sun, earth and moon. More... | |
ml_matrix | Hills2Frenet (const ml_matrix &xH, const double &e, const double &nu, const double &n) |
Transforms from the Hills frame to the Frenet frame. More... | |
Propagation | |
ml_matrix | HillsEqns (const ml_matrix &xH0, double n, double t) |
Closed form solution of relative orbital motion using Hills equations. More... | |
ml_matrix | FFEccHillsEqns (ml_matrix xH0, double nu0, const ml_matrix &nu, double e, double n) |
Compute Hills frame state (in time-domain) at future true anomaly(s). For eccentric or circular orbits. More... | |
ml_matrix | FFEccHillsEqns (const ml_matrix &xH0, double nu0, const ml_matrix &nu, double e) |
Compute Hills frame state (in nu-domain) at future true anomaly(s). For eccentric or circular orbits. More... | |
ml_matrix | DiscreteHills (const ml_matrix &x0, double n, const ml_matrix &aC, double dT) |
Compute the force relative trajectory from the initial state and time-history of applied accelerations. For circular orbits only. More... | |
ml_matrix | FFEccDiscreteHills (double e, double n, const ml_matrix &x0, double nu0, const ml_matrix &aC, const ml_matrix &t) |
Compute the force relative trajectory from the initial state and time-history of applied accelerations. For circular orbits only. More... | |
Collision Monitoring | |
double | CollProbSet (double sigma, const ml_matrix &S, const ml_matrix &xc, const ml_matrix &Ssc, int n) |
Collision probability for relative frame (single ellipsoid to point at origin) using ellipsoid shells. More... | |
ml_matrix | MonitoringAlg (ml_matrix &y, ml_matrix &t, ml_matrix &M, ml_matrix &nu, ml_matrix &accel, ml_matrix &aDiff, coll_mon_data_t d) |
Collision monitoring algorithm implementation used for both modes, monitoring and surveying. More... | |
ml_matrix | CollisionSurvey (ml_matrix &y, double t0, maneuver_t mvr1, maneuver_t mvr2[], int n_points, coll_mon_data_s d) |
Collision survey implementation which changes maneuvers to acceleration vectors and calls monitoring algorithm. More... | |
ml_matrix | DeltaElem2HillsMat (const ml_matrix &elA) |
Compute transformation matrix from delta elements to Hills. More... | |
ml_matrix | DeltaEl2AlfriendMat (orb_data_t el) |
Compute transformation matrix from standard differential elements to Alfriend differential elements. More... | |
double | distant_pt_ell (const ml_matrix &S, const ml_matrix &U, const ml_matrix &xc, const ml_matrix &x0, ml_matrix &x) |
Compute minimum distance from a distance point to an ellipsoid's surface. More... | |
double | Laguerre (const ml_matrix &a, double x_guess) |
Laguerre root finding algorithm adapted to real roots. More... | |
ml_matrix | GenerateTimeVector (orb_data_t d, const double tF, const int nPts, ml_matrix &M, ml_matrix &nu) |
Generate a time vector evenly spaced over true anomaly. More... | |
ml_matrix | ManeuverStruct2AccelVector (maneuver_t mvr, const ml_matrix &tProp) |
Compute a 3xN acceleration vector from a maneuver data structure. More... | |
Utility | |
typedef double(* | nr_function) (double x, double e, const ml_matrix &D, double dH) |
Newton-Raphson function. | |
bool | TeamLevels (team_t teams[], int num, ml_int_array &levels) |
Assign a hierarchical level to each team in the array. More... | |
int | IsRelative (int iD, team_t teams[], int num) |
Determine whether the given satellite ID is a relative of any team. More... | |
bool | FindUpperTeams (int kBot, team_t teams[], int num, ml_int_array &kUpper) |
Find teams above this one in the hierarchy. More... | |
double | TimeUntilTheta (double a, double w, double e, double theta0, double &theta1) |
Computes the time in seconds until the new latitude is reached from the original latitude. More... | |
void | AccelVector2ManeuverStruct (const ml_matrix &aC, ml_matrix t, double tRef, double nomAccel, double minSepTime, maneuver_t &mvr, ml_matrix &dV) |
Build a maneuver structure from acceleration and time vectors. More... | |
void | ManeuverStruct2AccelVector (maneuver_t mvr, double dT, ml_matrix &aC, ml_matrix &t) |
Compute a 3xN acceleration vector from a maneuver data structure. More... | |
ml_matrix | NOrbVector (window_t window) |
Compute a vector of maneuver durations (in orbits) from time window data. More... | |
ml_matrix | TargetTrueAnom (double e, double nu0, ml_matrix nOrb) |
Compute the future true anomaly (unwrapped) at the specified number of orbits after the initial true anomaly. More... | |
ml_matrix | Tetrahedron (double d, const ml_matrix &eul) |
Compute the 4 points of a regular tetrahedron, the surface area and volume. More... | |
double | vsum (const ml_matrix &a) |
Compute the sum of all elements in a row or column vector. | |
double | newton_raphson (nr_function f, nr_function df, double x, double epsilon, int &n_max, double e, const ml_matrix &D, const double &dH) |
Newton-Raphson (optimized for FFEccXExt and similar) | |
char * | matout (ml_matrix mat) |
Output a matrix to a string with higher precision than built-in "to_string" function. More... | |
Holds the formation flying data structures and function definitions.
enum ff_error_codes |
enum ff_formation_types |
Formation type enumerations.
maneuver_s IterativeImpulsiveManeuver | ( | state_t | state, |
geom_t | goals_circ, | ||
window_t | window, | ||
plan_param_t | param, | ||
bool & | foundSoln | ||
) |
Computes the delta-v's required to implement a formation flying maneuver for a single spacecraft.
Computes the delta-v's required to implement a formation flying maneuver for a single spacecraft.(circular orbit)
(circular orbit)
state | Absolute and relative state information |
goals_circ | Geometric goals for relative trajectory (circular) |
window | Time window for the maneuver |
param | Planning parameters for maneuver |
foundSoln | Flag indicating whether a solution was found or not. Passed by reference. |
References orb_data_s::ecc, state_s::el, state_s::elA, plan_param_s::eTol, FFEccGoals2Hills(), FFEccHills2Goals(), GeometryCirc2Ecc(), Goals2Hills(), Hills2Goals(), ImpulsiveLPManeuver(), ImpulsiveManeuver(), plan_param_s::maxDeltaV, orb_data_s::mean_anom, mean_to_true(), NOrbVector(), orb_rate(), orb_data_s::perigee, sc_log(), orb_data_s::sma, and state_s::xH.
maneuver_s IterativeImpulsiveManeuver | ( | state_t | state, |
ecc_geom_t | goals_ecc, | ||
window_t | window, | ||
plan_param_t | param, | ||
bool & | foundSoln | ||
) |
Computes the delta-v's required to implement a formation flying maneuver for a single spacecraft. (eccentric orbit)
Computes the delta-v's required to implement a formation flying maneuver for a single spacecraft. (eccentric orbit)
(eccentric orbit)
state | Absolute and relative state information |
goals_ecc | Geometric goals for relative trajectory (eccentric) |
window | Time window for the maneuver |
param | Planning parameters for maneuver |
foundSoln | Flag indicating whether a solution was found or not. Passed by reference. |
References orb_data_s::ecc, state_s::el, state_s::elA, plan_param_s::eTol, FFEccGoals2Hills(), FFEccHills2Goals(), GeometryEcc2Circ(), Goals2Hills(), Hills2Goals(), ImpulsiveLPManeuver(), ImpulsiveManeuver(), plan_param_s::maxDeltaV, orb_data_s::mean_anom, mean_to_true(), NOrbVector(), orb_rate(), orb_data_s::perigee, sc_log(), orb_data_s::sma, and state_s::xH.
maneuver_s ImpulsiveLPManeuver | ( | state_t | state, |
geom_t | goals_circ, | ||
window_t | window, | ||
plan_param_t | param, | ||
double & | maxDV | ||
) |
Plans an impulsive burn sequence (using simplex) to achieve the specified trajectory within the given time window. (circular orbit)
Plans an impulsive burn sequence (using simplex) to achieve the specified trajectory within the given time window. (circular orbit)
For circular orbits.
state | Data structure of Orbit state |
goals_circ | Data structure of geometric goal information |
window | Data structure of commanded time window |
param | Data structure of planning parameters |
maxDV | The maximum delta-V of the sequence |
References AccelVector2ManeuverStruct(), maneuver_s::achieve, diff(), orb_data_s::ecc, state_s::el, plan_param_s::eTol, FFEccHillsEqns(), plan_param_s::fNom, GeometryCirc2Ecc(), HillsEqns(), plan_param_s::horizon, LPCircular(), LPCircularTimeWeight(), LPEccentric(), LPEccentricTimeWeight(), state_s::mass, orb_data_s::mean_anom, mean_to_true(), plan_param_s::nSPOCoarse, plan_param_s::nSPOFine, orb_rate(), orb_data_s::perigee, sc_log(), orb_data_s::sma, window_s::startTime, TargetTrueAnom(), state_s::tM, TWO_PI, UnwrapPhase(), and state_s::xH.
Referenced by IterativeImpulsiveManeuver().
maneuver_s ImpulsiveLPManeuver | ( | state_t | state, |
ecc_geom_t | goals_ecc, | ||
window_t | window, | ||
plan_param_t | param, | ||
double & | maxDV | ||
) |
Plans an impulsive burn sequence (using simplex) to achieve the specified trajectory within the given time window. (eccentric orbit)
Plans an impulsive burn sequence (using simplex) to achieve the specified trajectory within the given time window. (eccentric orbit)
For eccentric orbits.
state | Data structure of Orbit state |
goals_ecc | Data structure of geometric goal information |
window | Data structure of commanded time window |
param | Data structure of planning parameters |
maxDV | The maximum delta-V of the sequence. Passed by reference. |
References AccelVector2ManeuverStruct(), maneuver_s::achieve, diff(), orb_data_s::ecc, state_s::el, plan_param_s::eTol, FFEccHillsEqns(), plan_param_s::fNom, GeometryEcc2Circ(), HillsEqns(), plan_param_s::horizon, LPCircular(), LPCircularTimeWeight(), LPEccentric(), LPEccentricTimeWeight(), state_s::mass, orb_data_s::mean_anom, mean_to_true(), plan_param_s::nSPOCoarse, plan_param_s::nSPOFine, orb_rate(), orb_data_s::perigee, sc_log(), orb_data_s::sma, window_s::startTime, TargetTrueAnom(), state_s::tM, TWO_PI, UnwrapPhase(), and state_s::xH.
maneuver_s ImpulsiveManeuver | ( | state_t | state, |
geom_t | goals, | ||
window_t | window, | ||
plan_param_t | param, | ||
double & | maxDV | ||
) |
Plans an impulsive burn sequence (using closed-form equations) to achieve the specified trajectory within the given time window. For circular orbits only.
Plan an impulsive burn sequence (using closed-form equations) to achieve the specified trajectory within the given time window. For circular orbits only. \param state Data structure of Orbit state
goals | Data structure of geometric goal information |
window | Data structure of commanded time window |
param | Data structure of planning parameters |
maxDV | The maximum delta-V of the sequence |
References maneuver_s::achieve, Alfriend2El(), maneuver_s::burnData, burn_s::dT, plan_param_s::dTMin, burn_s::dV, orb_data_s::ecc, state_s::el, el_to_rv(), state_s::elA, plan_param_s::fNom, Goals2DeltaElem(), Hills2DeltaElem(), plan_param_s::horizon, burn_s::iD, maneuver_s::iD, alf_orb_data_s::inc, orb_data_s::inc, InPlane(), state_s::mass, MU_EARTH, maneuver_s::nBurns, window_s::nOrbMin, NOrbVector(), orb_rate(), OutOfPlane(), alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::raan, sct_sign(), slr(), alf_orb_data_s::sma, orb_data_s::sma, window_s::startTime, burn_s::t, maneuver_s::t0, maneuver_s::tF, window_s::timeWeightExp, state_s::tM, alf_orb_data_s::true_lat, TWO_PI, burn_s::uZ, and state_s::xH.
Referenced by IterativeImpulsiveManeuver().
bool OptimalInPlaneDeltaV | ( | double | nOrbMin, |
double | nOrbMax, | ||
double | a, | ||
double | th0, | ||
double | th1, | ||
double | delta_a, | ||
double | delta_th0, | ||
double | delta_q1, | ||
double | delta_q2, | ||
double & | dV1, | ||
double & | dV2, | ||
double & | dV3, | ||
unsigned short & | M, | ||
unsigned short & | N | ||
) |
Computes the in-plane delta-v sequence for the given time window that achieves the desired element differences and results in the minimum total delta-v. For circular orbits only.
Compute the in-plane delta-v sequence for the given time window that achieves the desired element differences and results in the minimum total delta-v. For circular orbits only.
nOrbMin | Minimum number of orbits the maneuver may last |
nOrbMax | Maximum number of orbits the maneuver may last |
a | Mean semi-major axis of reference [km] |
th0 | Latitude of reference at measurement [rad] |
th1 | Latitude at first burn [rad] |
delta_a | Error in semi-major axis difference [km] |
delta_th0 | Error in latitude difference [rad] |
delta_q1 | Error in q1 difference |
delta_q2 | Error in q2 difference |
dV1 | First delta-v [km/s]. Passed by reference. |
dV2 | Second delta-v [km/s]. Passed by reference. |
dV3 | Third delta-v [km/s]. Passed by reference. |
M | Number of half-orbits between burn 1 and burn 2. Passed by reference. |
N | Number of half-orbits between burn 1 and burn 3. Passed by reference. |
Referenced by EstimateCost(), and InPlane().
burn_t * InPlane | ( | alf_orb_data_t | meas_elem, |
alf_orb_data_t | delta_elem, | ||
double | accNom, | ||
double | dTMin, | ||
double | nOrbMin, | ||
double | nOrbMax, | ||
double | horizon, | ||
double & | t0, | ||
double & | tF, | ||
unsigned short & | nBurns | ||
) |
Computes the in-plane burn sequence for the given time window that achieves the desired element differences. For circular orbits only.
Compute the in-plane burn sequence for the given time window that achieves the desired element differences. For circular orbits only.
meas_elem | Reference orbital elements (Alfriend format) |
delta_elem | Error in orbital element differences (Alfriend format) |
accNom | Nominal acceleration [km/s/s] |
dTMin | Minimum achievable burn duration [sec] |
nOrbMin | Minimum number of orbits maneuver may last |
nOrbMax | Maximum number of orbits maneuver may last |
horizon | Minimum amount of time required prior to first burn [sec] |
t0 | First burn time [sec from meas time]. Passed by reference. |
tF | Final burn time [sec from meas time]. Passed by reference. |
nBurns | Number of burns required. Passed by reference. |
References burn_s::dT, burn_s::dV, MU_EARTH, OptimalInPlaneDeltaV(), PI, alf_orb_data_s::q1, alf_orb_data_s::q2, sct_sign(), alf_orb_data_s::sma, burn_s::t, TimeUntilTheta(), alf_orb_data_s::true_lat, TWO_PI, burn_s::uX, burn_s::uY, burn_s::uZ, and WrapPhase().
Referenced by ImpulsiveManeuver().
burn_t OutOfPlane | ( | alf_orb_data_t | meas_elem, |
alf_orb_data_t | delta_elem, | ||
double | accNom, | ||
double | dTMin, | ||
unsigned short & | nBurns, | ||
double & | th1 | ||
) |
Compute the out-of-plane burn sequence for the given time window that achieves the desired element differences. For circular orbits only.
Compute the out-of-plane burn sequence for the given time window that achieves the desired element differences. For circular orbits only.
meas_elem | Reference orbital elements (Alfriend format) |
delta_elem | Error in orbital element differences (Alfriend format) |
accNom | Nominal acceleration [km/s/s] |
dTMin | Minimum achievable burn duration [sec] |
nBurns | Number of burns required. Passed by reference. |
th1 | Argument of latitude at which burn is applied. Passed by reference. |
References burn_s::dT, burn_s::dV, alf_orb_data_s::inc, MU_EARTH, alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::raan, sct_sign(), alf_orb_data_s::sma, burn_s::t, TimeUntilTheta(), alf_orb_data_s::true_lat, TWO_PI, burn_s::uX, burn_s::uY, and burn_s::uZ.
Referenced by ImpulsiveManeuver().
geom_t AutoFormGeometry | ( | state_t | state, |
geom_t | memberGoals[], | ||
int | num, | ||
double | minSepDist, | ||
double | maxSepDist | ||
) |
Define new geometric goals for a single satellite, such that any semi-major axis difference is eliminated, any radial oscillation is eliminated, and the new trajectory maintains a minimum separation distance from all other known trajectories.
Define new geometric goals for a single satellite, such that any semi-major axis difference is eliminated, any radial oscillation is eliminated, and the new trajectory maintains a minimum separation distance from all other known trajectories.
state | Orbital state data structure |
memberGoals | Geometric goals for all other members in the cluster |
num | Number of samples to use |
minSepDist | Minimum allowable separation distance between satellites |
maxSepDist | Maximum allowable separation distance between satellites |
References geom_s::aE, state_s::dEl, DeltaElem2Goals(), state_s::elA, NearestOffset(), alf_orb_data_s::sma, and geom_s::y0.
ecc_geom_t AutoFormGeometry | ( | state_t | state, |
ecc_geom_t | memberGoals[], | ||
int | num, | ||
double | minSepDist, | ||
double | maxSepDist | ||
) |
Define new geometric goals for a single satellite, such that any semi-major axis difference is eliminated, any radial oscillation is eliminated, and the new trajectory maintains a minimum separation distance from all other known trajectories.
Define new geometric goals for a single satellite, such that any semi-major axis difference is eliminated, any radial oscillation is eliminated, and the new trajectory maintains a minimum separation distance from all other known trajectories.
state | Orbital state data structure |
memberGoals | Geometric goals for all other members in the cluster |
num | Number of samples to use |
minSepDist | Minimum allowable separation distance between satellites |
maxSepDist | Maximum allowable separation distance between satellites |
References state_s::dEl, orb_data_s::ecc, state_s::el, state_s::elA, FFEccDeltaElem2Goals(), FFEccDH(), FFEccGoals(), FFEccYExt(), orb_data_s::mean_anom, mean_to_true(), NearestOffset(), alf_orb_data_s::sma, ecc_geom_s::xMax, and ecc_geom_s::y0.
double NearestOffset | ( | double | y0, |
double | width, | ||
ml_matrix | extrema, | ||
double | minSepDistance, | ||
double | maxSepDistance = 1e9 |
||
) |
Determine the nearest along-track offset for a trajectory that is safe such that the minimum and maximum required separation distances are respected.
Determine the nearest along-track offset for a trajectory that is safe such that the minimum and maximum required separation distances are respected.
y0 | Original along-track offset (on y-axis) |
width | Width of trajectory on y-axis |
extrema | Extrema pairs (min, max) on y-axis for N other trajectories |
minSepDistance | Minimum separation distance between trajectories |
maxSepDistance | Maximum separation distance between trajectories |
References diff().
Referenced by AutoFormGeometry().
ml_matrix InitializeCostMatrix | ( | team_goals_t | teamGoals, |
int | nRelatives | ||
) |
Given the team goals, initialize the cost matrix "f" with the right size.
teamGoals | Team goals data structure |
nRelatives | Number of relatives in the team |
References team_goals_s::constraints, team_goals_s::dPhi, team_goals_s::nU, TWO_PI, and constraints_s::variable.
Referenced by PopulateCostMatrix().
ml_matrix InitializeCostMatrix | ( | ecc_team_goals_t | teamGoals, |
int | nRelatives | ||
) |
Given the team goals, initialize the cost matrix "f" with the right size.
teamGoals | Team goals data structure |
nRelatives | Number of relatives in the team |
References ecc_team_goals_s::constraints, ecc_team_goals_s::dPhi, constraints_s::nDuplicates, ecc_team_goals_s::nU, TWO_PI, and constraints_s::variable.
int PopulateCostMatrix | ( | ml_matrix & | f, |
cost_t | costEstimate, | ||
team_goals_t | teamGoals, | ||
ml_int_array | relativeIDs | ||
) |
Fill in a single column of the cost matrix.
f | Initial cost matrix |
costEstimate | Cost estimate data structure supplied from a team member |
teamGoals | Team goals data structure used to generate the cost estimates |
relativeIDs | Array of member IDs for all relatives in the team |
References cost_s::cost, cost_s::costLength, CostMatrixRows(), team_goals_s::dPhi, InitializeCostMatrix(), cost_s::memID, team_goals_s::nU, cost_s::nU, cost_s::targetIndex, and TWO_PI.
int PopulateCostMatrix | ( | ml_matrix & | f, |
cost_t | costEstimate, | ||
ecc_team_goals_t | teamGoals, | ||
ml_int_array | relativeIDs | ||
) |
Fill in a single column of the cost matrix.
Fill in a single column of the cost matrix.
f | Initial cost matrix |
costEstimate | Cost estimate data structure supplied from a team member |
teamGoals | Team goals data structure used to generate the cost estimates |
relativeIDs | Array of member IDs for all relatives in the team |
References cost_s::cost, cost_s::costLength, CostMatrixRows(), ecc_team_goals_s::dPhi, InitializeCostMatrix(), cost_s::memID, ecc_team_goals_s::nU, cost_s::nU, cost_s::targetIndex, and TWO_PI.
void CostMatrixRows | ( | team_goals_t | teamGoals, |
int | index, | ||
int & | a, | ||
int & | b | ||
) |
Compute the starting and ending rows in the cost matrix that correspond to a give target state index.
Compute the starting and ending rows in the cost matrix that correspond to a give target state index.
teamGoals | Team goals data structure |
index | Target state index |
a | Starting row. Passed by reference. |
b | Ending row. Passed by reference. |
References team_goals_s::constraints, team_goals_s::dPhi, team_goals_s::nU, TWO_PI, and constraints_s::variable.
Referenced by PopulateCostMatrix().
void CostMatrixRows | ( | ecc_team_goals_t | teamGoals, |
int | index, | ||
int & | a, | ||
int & | b | ||
) |
Compute the starting and ending rows in the cost matrix that correspond to a give target state index.
Compute the starting and ending rows in the cost matrix that correspond to a give target state index.
teamGoals | Team goals data structure |
index | Target state index |
a | Starting row. Passed by reference. |
b | Ending row. Passed by reference. |
References ecc_team_goals_s::constraints, ecc_team_goals_s::dPhi, ecc_team_goals_s::nU, TWO_PI, and constraints_s::variable.
cost_s EstimateCost | ( | alf_orb_data_t | el0, |
alf_orb_data_t | dEl, | ||
team_goals_t | teamGoals, | ||
int | memID, | ||
window_t | window, | ||
double | weight | ||
) |
Estimate the (weighted) cost to achieve all specified unique target states.
Estimate the (weighted) cost to achieve all specified unique target states.
el0 | Initial reference orbital elements (Alfriend format) [a,theta,i,q1,q2,W] |
dEl | Initial orbital element differences (Alfriend format) |
teamGoals | Team goals data structure defining desired relative motion for team |
memID | Unique member ID |
window | Time window data structure |
weight | Scalar weight to be applied to all costs for this spacecraft |
References team_goals_s::constraints, cost_s::cost, cost_s::costLength, team_goals_s::dPhi, team_goals_s::geometry, Goals2DeltaElem(), alf_orb_data_s::inc, assign_s::M, cost_s::memID, MU_EARTH, window_s::nOrbMax, window_s::nOrbMin, constraints_s::nRestrict, team_goals_s::nU, cost_s::nU, OptimalInPlaneDeltaV(), assign_s::Pu, assign_s::Q, alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::raan, constraints_s::restrictID, RotateState(), SetupAssignmentProblem(), alf_orb_data_s::sma, cost_s::targetIndex, alf_orb_data_s::true_lat, and constraints_s::variable.
cost_s FFEccEstimateCost | ( | orb_data_t | el0, |
const ml_matrix & | xH0, | ||
ecc_team_goals_t | teamGoals, | ||
int | memID, | ||
window_t | window, | ||
double | weight, | ||
int | nSPO | ||
) |
Estimate the (weighted) cost to achieve all specified unique target states.
Estimate the (weighted) cost to achieve all specified unique target states.
el0 | Initial reference orbital elements (Alfriend format) [a,theta,i,q1,q2,W] |
xH0 | Initial relative state, Hills frame |
teamGoals | Team goals data structure defining desired relative motion for team |
memID | Unique member ID |
window | Time window data structure |
weight | Scalar weight to be applied to all costs for this spacecraft |
nSPO | Number of samples to use per orbit for LP algorithm |
References ecc_team_goals_s::constraints, cost_s::cost, cost_s::costLength, diff(), orb_data_s::ecc, FFEccGoals2Hills(), ecc_team_goals_s::geometry, LPEccentric(), orb_data_s::mean_anom, mean_to_true(), cost_s::memID, window_s::nOrbMax, constraints_s::nRestrict, ecc_team_goals_s::nU, cost_s::nU, orb_rate(), constraints_s::restrictID, orb_data_s::sma, cost_s::targetIndex, TWO_PI, and vsum().
team_goals_s GenerateTeamGoals | ( | alf_orb_data_t | el0, |
int | fType, | ||
double | fSize, | ||
unsigned short | nRels, | ||
int | teamID, | ||
double | angRes | ||
) |
Generate a set of team goals given the formation type and size.
Generate a set of team goals given the formation type and size.
el0 | reference orbital elements [a,th,i,q1,q2,W] |
fType | formation type |
fSize | formation size |
nRels | number of relatives in the team |
teamID | unique integer team ID |
angRes | angular resolution for discretized search with variable states [rad] |
References geom_s::aE, geom_s::beta, CirclePhase(), team_goals_s::constraints, team_goals_s::dPhi, EllipsePhase(), FF_CIPE_REF_CENTER, FF_CIPE_REF_ON, FF_DUAL_PROJ_CIRC_REF_CENTER, FF_DUAL_PROJ_CIRC_REF_ON, FF_IPLF, FF_NEG_PROJ_CIRC_REF_CENTER, FF_NEG_PROJ_CIRC_REF_ON, FF_OOPLF_RGT, FF_POS_PROJ_CIRC_REF_CENTER, FF_POS_PROJ_CIRC_REF_ON, team_goals_s::geometry, Goals2Hills(), Hills2Goals(), alf_orb_data_s::inc, constraints_s::nDuplicates, constraints_s::nRestrict, team_goals_s::nU, PCGoals(), period(), constraints_s::phase, SECS_TO_DAYS, alf_orb_data_s::sma, team_goals_s::teamID, TWO_PI, constraints_s::variable, WrapPhase(), geom_s::y0, geom_s::zInc, and geom_s::zLan.
Referenced by FFEccGenerateTeamGoals().
ecc_team_goals_s FFEccGenerateTeamGoals | ( | orb_data_t | el0, |
int | fType, | ||
double | fSize, | ||
unsigned short | nRels, | ||
int | teamID, | ||
double | nu, | ||
const ml_matrix & | eul | ||
) |
Generate a set of team goals given the formation type, size, location and orientation.
el0 | Orbital elements |
fType | Formation type |
fSize | Formation size |
nRels | Number of relatives |
teamID | Team ID |
nu | True anomaly |
eul | Euler angles |
References team_goals_s::constraints, ecc_team_goals_s::constraints, ecc_team_goals_s::dPhi, orb_data_s::ecc, El2Alfriend(), FF_TETRA, FFEccGoals2Hills(), FFEccHills2Goals(), FFEccTetrahedronGeometry(), GenerateTeamGoals(), team_goals_s::geometry, ecc_team_goals_s::geometry, Goals2Hills(), constraints_s::nDuplicates, constraints_s::nRestrict, team_goals_s::nU, ecc_team_goals_s::nU, ecc_geom_xy_s::nu_xy, ecc_geom_xy_s::nu_zMax, orb_rate(), orb_data_s::perigee, constraints_s::phase, alf_orb_data_s::q1, alf_orb_data_s::q2, RotateState(), orb_data_s::sma, ecc_team_goals_s::teamID, constraints_s::variable, ecc_geom_xy_s::x, ecc_geom_xy_s::y, geom_s::y0, ecc_geom_xy_s::y0, and ecc_geom_xy_s::zMax.
ecc_geom_xy_s * FFEccTetrahedronGeometry | ( | double | nu, |
double | d, | ||
const ml_matrix & | eul | ||
) |
Compute the geometric goals for a formation that achieves a tetrahedron shape at a given true anomaly in an eccentric reference orbit.
Compute the geometric goals for a formation that achieves a tetrahedron shape at a given true anomaly in an eccentric reference orbit.
nu | True anomaly where tetrahedron occurs |
d | Length of each side |
eul | Euler angles defining the orientation |
References ecc_geom_xy_s::nu_xy, ecc_geom_xy_s::nu_zMax, Tetrahedron(), ecc_geom_xy_s::x, ecc_geom_xy_s::y, ecc_geom_xy_s::y0, and ecc_geom_xy_s::zMax.
Referenced by FFEccGenerateTeamGoals().
geom_s PCGoals | ( | double | R, |
double | alpha0, | ||
double | sgn, | ||
double | y0 | ||
) |
Generate the geometric goals for a projected circular formation.
Generate the geometric goals for a projected circular formation.
R | radius of the projected circle [km] |
alpha0 | angular offset around circle [rad] |
sgn | orientation of plane |
y0 | along-track offset [km] |
References geom_s::aE, geom_s::beta, EllipsePhase(), geom_s::y0, geom_s::zInc, and geom_s::zLan.
Referenced by GenerateTeamGoals().
void SortTeamGoals | ( | team_goals_t & | teamGoals | ) |
Sort the team goals with fixed states listed before variable states.
teamGoals | Team goals data structure, supplied from ground. Passed by reference. |
References team_goals_s::constraints, team_goals_s::geometry, team_goals_s::nU, and constraints_s::variable.
void SortTeamGoals | ( | ecc_team_goals_t & | teamGoals | ) |
Sort the team goals with fixed states listed before variable states.
Sort the team goals with fixed states listed before variable states.
teamGoals | Team goals data structure, supplied from ground. Passed by reference. |
References ecc_team_goals_s::constraints, ecc_team_goals_s::geometry, ecc_team_goals_s::nU, and constraints_s::variable.
assign_s SetupAssignmentProblem | ( | team_goals_t | teamGoals, |
double | orbFrac | ||
) |
Set up the assignment problem given the team goals by computing the number of fixed, variable and unique variable states, plus with the phases and indices of repeated variable states.
Set up the assignment problem given the team goals by computing the number of fixed, variable and unique variable states, plus with the phases and indices of repeated variable states. \param teamGoals Team goals data structure \param orbFrac fraction of orbit
References geom_s::beta, CirclePhase(), team_goals_s::constraints, team_goals_s::dPhi, team_goals_s::geometry, assign_s::M, assign_s::N, constraints_s::nDuplicates, team_goals_s::nU, assign_s::P, constraints_s::phase, assign_s::phi, assign_s::Pu, assign_s::Q, TWO_PI, assign_s::u, and constraints_s::variable.
Referenced by EstimateCost().
assign_s SetupAssignmentProblem | ( | ecc_team_goals_t | teamGoals, |
double | orbFrac | ||
) |
Set up the assignment problem given the team goals by computing the number of fixed, variable and unique variable states, plus with the phases and indices of repeated variable states.
Set up the assignment problem given the team goals by computing the number of fixed, variable and unique variable states, plus with the phases and indices of repeated variable states.
teamGoals | Team goals data structure |
orbFrac | fraction of orbit |
References assign_s::M, assign_s::N, ecc_team_goals_s::nU, assign_s::P, assign_s::Pu, and assign_s::Q.
int RestrictIDSet | ( | ml_int_array & | relIDs, |
constraints_t | constraints[], | ||
int | numC | ||
) |
Restrict the set of relative IDs to respect the specified assignment constraints.
relIDs | Unique set of relative IDs. There must be no repeated IDs in this list. |
constraints | Array of constraint data structures. Each one contains a list of IDs |
numC | Number of constraint data structures. |
References constraints_s::nRestrict.
int VerifyAssignmentParams | ( | assign_t | a, |
const ml_matrix & | f | ||
) |
Check for inconsistencies in the assignment parameters.
a | Assignment parameters structure |
f | Cost matrix, with N columns for N satellites |
References assign_s::N, assign_s::P, assign_s::phi, assign_s::Pu, assign_s::Q, and assign_s::u.
Referenced by OptimalAssignment(), and PrivilegedAssignment().
int PrivilegedAssignment | ( | assign_t | assign, |
const ml_matrix & | f, | ||
int | method, | ||
ml_int_array & | optOrder, | ||
ml_matrix & | optPhi, | ||
double & | optCost | ||
) |
Assign N targets to N satellites one at a time, so that the satellite with the largest minimum cost is assigned its min-cost target first.
assign | Assignment parameters, dictated by the team goals. |
f | Cost matrix, with N columns for N satellites |
method | Indicates whether to use the minimum cost (1) or the average cost (not 1) as the cost metric. |
optOrder | The order in which the target states should be assigned to each satellite. Passed by reference. |
optPhi | The corresponding phase angles associated with each target. Passed by reference. |
optCost | The total cost to achieve all target states. Passed by reference. |
References assign_s::N, assign_s::P, assign_s::phi, assign_s::Q, TWO_PI, assign_s::u, VerifyAssignmentParams(), and vsum().
int OptimalAssignment | ( | assign_t | assign, |
ml_matrix | f, | ||
ml_int_array & | optOrder, | ||
ml_matrix & | optPhi, | ||
double & | optCost | ||
) |
Assign N targets to N satellites so that the weighted sum of costs to reach all targets is minimized.
Assign N targets to N satellites so that the weighted sum of costs to reach all targets is minimized. \param assign Assignment parameters data structure
f | Set of weighted cost vectors |
optOrder | Optimal order of the target states. Passed by reference. |
optPhi | Optimal phases for variable states. Passed by reference. |
optCost | Total cost to achieve the optimal configuration. Passed by reference. |
References diff(), EquallyPhased(), FindMinSet(), assign_s::N, assign_s::P, assign_s::phi, assign_s::Pu, assign_s::Q, TWO_PI, assign_s::u, and VerifyAssignmentParams().
bool EquallyPhased | ( | const ml_matrix & | phi, |
ml_int_array | u, | ||
int | Pu | ||
) |
Determine if the phase angles are equally phased.
Determine if the phase angles are equally phased.
phi | Phase angle array |
u | Unique variable indices |
Pu | Number of unique variable states |
Referenced by OptimalAssignment().
ml_int_array FindMinSet | ( | ml_matrix & | mat, |
double & | minSum | ||
) |
Find the order in which to arrange the columns so that the matrix diagonal sum is minimized.
mat | Matrix of costs. Passed by reference. |
minSum | Minimum sum solution. Passed by reference. |
References permute(), and vsum().
Referenced by OptimalAssignment().
void permute | ( | ml_int_array | arr, |
int | num, | ||
int | start, | ||
const ml_matrix & | mat, | ||
double & | cost, | ||
ml_int_array & | best | ||
) |
Permute the supplied array, called recursively by FindMinSet.
arr | Array to be permuted |
num | Number of remaining rows to permute. |
start | Starting row for current permutation. |
mat | Matrix of costs. Passed by reference. |
cost | Current cost. |
best | Best cost thus far. Passed by reference. |
References permute(), and vsum().
Referenced by FindMinSet(), and permute().
void LPCircular | ( | const ml_matrix & | x0, |
const ml_matrix & | xF, | ||
double | n, | ||
double | duration, | ||
double | dT, | ||
unsigned short | constraintType, | ||
double | maxConstraint, | ||
ml_matrix & | aC, | ||
ml_matrix & | t, | ||
bool & | exitFlag | ||
) |
Computes the thrust trajectory to go from an initial state to a final state in Hills frame. For circular orbits.
Computes the thrust trajectory to go from an initial state to a final state in Hills frame. For circular orbits.
For circular orbits.
x0 | Initial state in Hill's frame |
xF | Final state in Hill's frame |
n | Reference orbit rate (rad/sec) |
duration | Maneuver duration (secs) |
dT | Thruster time step |
constraintType | Flag [0: Equality; 1: Inequality constraint] |
maxConstraint | Maximum constraint on u |
aC | Commanded acceleration in Hill's frame |
t | Time vector (secs) |
exitFlag | Flag [0: No feasible solution; 1: Feasible solution] |
References c_to_d_zoh(), lin_orb(), and sc_log().
Referenced by ImpulsiveLPManeuver(), and LPCircularTimeWeight().
void LPEccentric | ( | double | e, |
double | n, | ||
const ml_matrix & | x0, | ||
const ml_matrix & | xF, | ||
double | nu0, | ||
double | nuF, | ||
int | nS, | ||
ml_matrix | cW, | ||
unsigned short | constraintType, | ||
double | maxConstraint, | ||
ml_matrix & | aC, | ||
ml_matrix & | t, | ||
bool & | exitFlag | ||
) |
Computes the thrust trajectory to go from an initial state to a final state in Hills frame. For eccentric orbits.
Computes the thrust trajectory to go from an initial state to a final state in Hills frame. For eccentric orbits.
For eccentric orbits.
e | Orbit eccentricity |
n | Mean orbit rate (rad/sec) |
x0 | Initial state in Hill's frame |
xF | Final state in Hill's frame |
nu0 | Initial true anomaly (rad) |
nuF | Final true anomaly (rad) |
nS | Number of samples to use for control vector |
cW | Cost weighting vector |
constraintType | Flag [0: Equality; 1: Inequality constraint] |
maxConstraint | Maximum constraint on u |
aC | Commanded acceleration in Hill's frame. Passed by reference. |
t | Time vector (secs). Passed by reference. |
exitFlag | Flag [0: No feasible solution; 1: Feasible solution]. Passed by reference. |
References FFEccLinOrb(), sc_log(), true_to_mean(), and UnwrapPhase().
Referenced by FFEccEstimateCost(), ImpulsiveLPManeuver(), and LPEccentricTimeWeight().
void LPCircularTimeWeight | ( | orb_data_t | el0, |
const ml_matrix & | xH0, | ||
geom_t | goals, | ||
window_t | window, | ||
int | nSPO, | ||
ml_matrix & | xHF, | ||
double & | nOrbMvr | ||
) |
Determine the target state on the desired trajectory that gives the minimum time-weighted cost. For circular orbits.
Determine the target state on the desired trajectory that gives the minimum time-weighted cost. For circular orbits.
For circular orbits.
el0 | Initial orbital element set [a,i,W,w,e,M] |
xH0 | Initial state in Hill's frame |
goals | Geometric goals data structure |
window | Maneuver time window data structure, containing:
|
nSPO | Number of samples to use for control vector (per orbit of maneuver duration) |
xHF | Target Hills-frame state (at maneuver completion). Passed by reference. |
nOrbMvr | Chosen maneuver duration (in orbits). Passed by reference. |
References orb_data_s::ecc, El2Alfriend(), Goals2Hills(), LPCircular(), orb_data_s::mean_anom, MeanAnom2TrueLat(), NOrbVector(), orb_rate(), orb_data_s::perigee, PI, orb_data_s::sma, window_s::timeWeightExp, alf_orb_data_s::true_lat, and TWO_PI.
Referenced by ImpulsiveLPManeuver().
void LPEccentricTimeWeight | ( | orb_data_t | el0, |
const ml_matrix & | xH0, | ||
ecc_geom_t | goals, | ||
window_t | window, | ||
int | nSPO, | ||
ml_matrix & | xHF, | ||
double & | nOrbMvr | ||
) |
Determine the target state on the desired trajectory that gives the minimum time-weighted cost. For eccentric orbits.
Determine the target state on the desired trajectory that gives the minimum time-weighted cost. For eccentric orbits.
For eccentric orbits.
el0 | Initial orbital element set [a,i,W,w,e,M] |
xH0 | Initial state in Hill's frame |
goals | Geometric goals data structure |
window | Maneuver time window data structure, containing: |
nSPO | Number of samples to use for control vector (per orbit of maneuver duration) |
xHF | Target Hills-frame state (at maneuver completion). Passed by reference. |
nOrbMvr | Chosen maneuver duration (in orbits). Passed by reference. |
References diff(), orb_data_s::ecc, FFEccGoals2Hills(), LPEccentric(), orb_data_s::mean_anom, mean_to_true(), NOrbVector(), orb_rate(), orb_data_s::sma, TargetTrueAnom(), and window_s::timeWeightExp.
Referenced by ImpulsiveLPManeuver().
void GVEErrorDynamics | ( | orb_data_t | el0, |
ml_matrix & | A, | ||
ml_matrix & | B | ||
) |
Compute continuous-time dynamics for Gauss' variational equations.
el0 | Standard orbital elements |
A | State dynamics. Passed by reference. |
B | Input effect matrix. Passed by reference. |
References orb_data_s::ecc, orb_data_s::inc, orb_data_s::mean_anom, mean_to_true(), orb_data_s::perigee, and orb_data_s::sma.
Referenced by LPEccentricGVE(), and MonitoringAlg().
void LPEccentricGVE | ( | orb_data_t | el0, |
const ml_matrix & | x0, | ||
const ml_matrix & | xF, | ||
double | MF, | ||
int | nS, | ||
ml_matrix | cW, | ||
unsigned short | constraintType, | ||
double | maxConstraint, | ||
ml_matrix & | aC, | ||
ml_matrix & | t, | ||
bool & | exitFlag | ||
) |
Computes the thrust trajectory to go from an initial state to a final state in Hills frame. For eccentric orbits.
Computes the thrust trajectory to go from an initial state to a final state in Hills frame. For eccentric orbits.
For eccentric orbits.
el0 | Initial orbital elements |
x0 | Initial state in Hill's frame |
xF | Final state in Hill's frame |
MF | Final mean anomaly (rad) |
nS | Number of samples to use for control vector |
cW | Cost weighting vector |
constraintType | Flag [0: Equality; 1: Inequality constraint] |
maxConstraint | Maximum constraint on u |
aC | Commanded acceleration in Hill's frame. Passed by reference. |
t | Time vector (secs). Passed by reference. |
exitFlag | Flag [0: No feasible solution; 1: Feasible solution]. Passed by reference. |
References c_to_d_zoh(), diff(), orb_data_s::ecc, GVEErrorDynamics(), orb_data_s::mean_anom, mean_to_true(), orb_rate(), PI, orb_data_s::sma, true_to_mean(), and UnwrapPhase().
void FFEccLinOrb | ( | double | n, |
double | nu, | ||
double | e, | ||
ml_matrix & | a, | ||
ml_matrix & | b, | ||
double | dT | ||
) |
Compute the continuous A,B matrices for linearized relative motion in an eccentric reference orbit, discretized with a zero-order hold with timestep dT.
Compute the continous A,B matrices for linearized relative motion in an eccentric reference orbit, discretized with a zero-order hold with timestep dT. *sctlib (overload LinOrb)
*sctlib (overload LinOrb)
n | Mean orbit rate [rad/s] |
nu | True anomaly [rad] |
e | Eccentricity |
a | Plant matrix. Passed by reference. |
b | Input matrix. Passed by reference. |
dT | Time-step for discretization |
References c_to_d_zoh(), and NuDot().
Referenced by FFEccDiscreteHills(), LPEccentric(), and MonitoringAlg().
ml_matrix FFEccLinOrb | ( | const ml_matrix & | x, |
const ml_matrix & | acc, | ||
double | n, | ||
double | nu, | ||
double | e | ||
) |
Compute the right-hand-side for linearized relative motion in an eccentric reference orbit.
Compute the right-hand-side for linearized relative motion in an eccentric reference orbit.
x | State vector |
acc | Acceleration vector |
n | Mean orbit rate [rad/s] |
nu | True anomaly [rad] |
e | Eccentricity |
References NuDot().
ml_matrix FFEccProp | ( | const ml_matrix & | D, |
const ml_matrix & | nu, | ||
double | e, | ||
double | dH | ||
) |
Compute Hills frame state at specified true anomaly given integration constants and eccentricity.
D | Integration constants computed from i.c.'s |
nu | True anomaly [rad] |
e | Eccentricity |
dH | Integration constant found by setting H = 0 at nu0 |
References FFEccRMat().
Referenced by FFEccGoals2Hills().
ml_matrix FFEccProp | ( | const ml_matrix & | D, |
double | nu, | ||
double | e, | ||
double | dH | ||
) |
Compute Hills frame state at specified true anomaly given integration constants and eccentricity.
D | Integration constants computed from i.c.'s |
nu | True anomaly [rad] |
e | Eccentricity |
dH | Integration constant found by setting H = 0 at nu0 |
References FFEccRMat().
double FFEccDH | ( | double | nu0, |
double | e | ||
) |
Compute integration constant dH for homogeneous solution to LTV differential equations of relative orbit motion. Found by setting H=0 at initial true anomaly.
Compute integration constant dH for homogeneous solution to LTV differential equations of relative orbit motion. Found by setting H=0 at initial true anomaly.
Found by setting H=0 at initial true anomaly.
nu0 | True Anomaly (at initial state) [rad] |
e | Eccentricity |
References PI.
Referenced by AutoFormGeometry(), FFEccGoals(), FFEccGoals2Hills(), FFEccHills2Goals(), and FFEccIntConst().
void FFEccGoals | ( | double | e, |
ecc_geom_t | goals, | ||
ml_matrix & | D, | ||
ml_matrix & | xH | ||
) |
Compute integration constants and initial state from the geometric goals.
e | Eccentricity |
goals | Eccentric geometric goals |
D | Integration constants computed from i.c.'s. Passed by reference. |
xH | Hills frame state at nu = 0. Passed by reference. |
References FFEccDH(), FFEccRMat(), FFEccYExt(), ecc_geom_s::nu_xMax, ecc_geom_s::nu_zMax, PI, WrapPhase(), ecc_geom_s::xMax, ecc_geom_s::y0, and ecc_geom_s::zMax.
Referenced by AutoFormGeometry(), and FFEccGoals2Hills().
void FFEccGoals | ( | double | e, |
ecc_geom_xy_t | goals, | ||
ml_matrix & | D, | ||
ml_matrix & | xH | ||
) |
Compute integration constants and initial state from the geometric goals.
e | Eccentricity |
goals | Eccentric geometric goals (xy) |
D | Integration constants computed from i.c.'s. Passed by reference. |
xH | Hills frame state at nu = 0. Passed by reference. |
References FFEccDH(), FFEccRMat(), FFEccYExt(), ecc_geom_xy_s::nu_xy, ecc_geom_xy_s::nu_zMax, PI, WrapPhase(), ecc_geom_xy_s::x, ecc_geom_xy_s::y, ecc_geom_xy_s::y0, and ecc_geom_xy_s::zMax.
void FFEccIntConst | ( | const ml_matrix & | xH0, |
double | nu0, | ||
double | e, | ||
ml_matrix & | D, | ||
double & | dH, | ||
ml_matrix & | R | ||
) |
Compute integration constants for homogeneous solution to LTV differential equations of relative orbit motion.
xH0 | Initial state in Hills frame |
nu0 | True anomaly (at initial state) [rad] |
e | Eccentricity |
D | Vector of integration constants. Passed by reference. |
dH | Integration constant found by setting H = 0 at nu0. Passed by reference. |
R | State transition matrix. Passed by reference. |
References FFEccDH(), and FFEccRMat().
Referenced by FFEccHillsEqns().
ml_matrix FFEccRMat | ( | double | nu, |
double | e, | ||
double | dH | ||
) |
Compute the state-transition matrix, R, given the eccentricity and true anomaly so that xH = R*D, where D is the vector of integration constants found from initial conditions.
nu | True Anomaly [rad] |
e | Eccentricity |
dH | Integration constant found by setting H = 0 at nu0 |
References PI.
Referenced by FFEccDDX(), FFEccDDY(), FFEccDDZ(), FFEccDX(), FFEccDY(), FFEccDZ(), FFEccGoals(), FFEccHills2Goals(), FFEccHillsEqns(), FFEccIntConst(), FFEccProp(), FFEccXExt(), FFEccYExt(), and FFEccZExt().
ml_matrix FFEccRMat | ( | double | nu, |
double | e | ||
) |
Compute the state-transition matrix, R, given the eccentricity and true anomaly so that xH = R*D, where D is the vector of integration constants found from initial conditions. Here, the integration constant "dH" is computed using the first element of the "nu" matrix.
Compute the state-transition matrix, R, given the eccentricity and true anomaly so that xH = R*D, where D is the vector of integration constants found from initial conditions. Here, the integration constant "dH" is computed using the first element of the "nu" matrix.
Here, the integration constant "dH" is computed using the first element of the "nu" matrix.
nu | True Anomaly [rad] |
e | Eccentricity |
References PI.
void FFEccXExt | ( | double | e, |
const ml_matrix & | D, | ||
double | dH, | ||
double | epsilon, | ||
int | nMax, | ||
ml_matrix & | x, | ||
ml_matrix & | nu | ||
) |
Compute extreme x-values and associated true anomalies for given relative motion.
e | Eccentricity |
D | Integration constants computed from i.c.'s |
dH | Integration constant found by setting H = 0 at nu0 |
epsilon | Angular tolerance [rad] |
nMax | Maximum number of iterations |
x | Extreme values of radial oscillations [km]. Passed by reference. |
nu | True anomaly where extreme values occur [rad]. Passed by reference. |
References diff(), FFEccDDX(), FFEccDX(), FFEccRMat(), newton_raphson(), PI, and WrapPhase().
Referenced by FFEccHills2Goals().
void FFEccYExt | ( | double | e, |
const ml_matrix & | D, | ||
double | dH, | ||
double | epsilon, | ||
int | nMax, | ||
ml_matrix & | y, | ||
ml_matrix & | nu | ||
) |
Compute extreme y-values and associated true anomalies for given relative motion.
e | Eccentricity |
D | Integration constants computed from i.c.'s |
dH | Integration constant found by setting H = 0 at nu0 |
epsilon | Angular tolerance [rad] |
nMax | Maximum number of iterations |
y | Extreme values of along-track offsets [km]. Passed by reference. |
nu | True anomaly where extreme along-track offsets occur [rad]. Passed by reference. |
References diff(), FFEccDDY(), FFEccDY(), FFEccRMat(), newton_raphson(), PI, and WrapPhase().
Referenced by AutoFormGeometry(), FFEccGoals(), and FFEccHills2Goals().
void FFEccZExt | ( | double | e, |
const ml_matrix & | D, | ||
double | dH, | ||
double | epsilon, | ||
int | nMax, | ||
ml_matrix & | z, | ||
ml_matrix & | nu | ||
) |
Compute extreme z-values and associated true anomalies for given relative motion.
e | Eccentricity |
D | Integration constants computed from i.c.'s |
dH | Integration constant found by setting H = 0 at nu0 |
epsilon | Angular tolerance [rad] |
nMax | Maximum number of iterations |
z | Extreme values of cross track amplitude [km]. Passed by reference. |
nu | True anomaly where extreme amplitudes occur [rad]. Passed by reference. |
References diff(), FFEccDDZ(), FFEccDZ(), FFEccRMat(), newton_raphson(), PI, and WrapPhase().
Referenced by FFEccHills2Goals().
void NuDot | ( | double | n, |
double | e, | ||
double | nu, | ||
double & | nuDot, | ||
double & | nuDotDot | ||
) |
Compute the time-derivative of the true anomaly. *sctlib.
Compute the time-derivative of the true anomaly. *sctlib.
*sctlib
n | Mean orbit rate [rad/s] |
e | Eccentricity |
nu | True Anomaly [rad] |
nuDot | Time derivative of true anomaly [rad/s]. Passed by reference. |
nuDotDot | Second time derivative of true anomaly [rad/s]. Passed by reference. |
Referenced by FFEccLinOrb(), Nu2TimeDomain(), and Time2NuDomain().
double FFEccDX | ( | double | nu, |
double | e, | ||
const ml_matrix & | D, | ||
double | dH | ||
) |
Compute the first derivative of x with respect to true anomaly.
nu | True anomaly [rad] |
e | Eccentricity |
D | Integration constants computed from i.c.'s |
dH | Integration constant computed from H(nu0)=0 |
References FFEccRMat().
Referenced by FFEccXExt().
double FFEccDY | ( | double | nu, |
double | e, | ||
const ml_matrix & | D, | ||
double | dH | ||
) |
Compute the first derivative of y with respect to true anomaly.
nu | True anomaly [rad] |
e | Eccentricity |
D | Integration constants computed from i.c.'s |
dH | Integration constant computed from H(nu0)=0 |
References FFEccRMat().
Referenced by FFEccYExt().
double FFEccDZ | ( | double | nu, |
double | e, | ||
const ml_matrix & | D, | ||
double | dH | ||
) |
Compute the first derivative of z with respect to true anomaly.
nu | True anomaly [rad] |
e | Eccentricity |
D | Integration constants computed from i.c.'s |
dH | Integration constant computed from H(nu0)=0 |
References FFEccRMat().
Referenced by FFEccZExt().
double FFEccDDX | ( | double | nu, |
double | e, | ||
const ml_matrix & | D, | ||
double | dH | ||
) |
Compute the second derivative of x with respect to true anomaly.
nu | True anomaly [rad] |
e | Eccentricity |
D | Integration constants computed from i.c.'s |
dH | Integration constant computed from H(nu0)=0 |
References FFEccRMat().
Referenced by FFEccXExt().
double FFEccDDY | ( | double | nu, |
double | e, | ||
const ml_matrix & | D, | ||
double | dH | ||
) |
Compute the second derivative of y with respect to true anomaly.
nu | True anomaly [rad] |
e | Eccentricity |
D | Integration constants computed from i.c.'s |
dH | Integration constant computed from H(nu0)=0 |
References FFEccRMat().
Referenced by FFEccYExt().
double FFEccDDZ | ( | double | nu, |
double | e, | ||
const ml_matrix & | D, | ||
double | dH | ||
) |
Compute the second derivative of z with respect to true anomaly.
nu | True anomaly [rad] |
e | Eccentricity |
D | Integration constants computed from i.c.'s |
dH | Integration constant computed from H(nu0)=0 |
References FFEccRMat().
Referenced by FFEccZExt().
orb_data_t Alfriend2El | ( | alf_orb_data_t | elA | ) |
Convert an Alfriend orbital element set into the standard orbital element set *sctlib.
Convert an Alfriend orbital element set into the standard orbital element set *sctlib
elA | Alfriend orbital elements [a,theta,i,q1,q2,W] |
References orb_data_s::ecc, alf_orb_data_s::inc, orb_data_s::inc, orb_data_s::mean_anom, orb_data_s::perigee, alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::raan, orb_data_s::raan, alf_orb_data_s::sma, orb_data_s::sma, orb_data_s::true_anom, alf_orb_data_s::true_lat, and true_to_mean().
Referenced by FFEccDeltaElem2Hills(), FFEccHills2DeltaElem(), and ImpulsiveManeuver().
alf_orb_data_t El2Alfriend | ( | orb_data_t | el | ) |
Convert a standard orbital element set into the Alfriend orbital element set *sctlib.
Convert a standard orbital element set into the Alfriend orbital element set *sctlib.
el | Standard orbital elements [a,i,W,w,e,M] |
References orb_data_s::ecc, alf_orb_data_s::inc, orb_data_s::inc, orb_data_s::mean_anom, mean_to_true(), orb_data_s::perigee, alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::raan, orb_data_s::raan, alf_orb_data_s::sma, orb_data_s::sma, alf_orb_data_s::true_lat, TWO_PI, and WrapPhase().
Referenced by ECI2MeanElements(), FFEccGenerateTeamGoals(), FFEccHills2DeltaElem(), LPCircularTimeWeight(), and MonitoringAlg().
alf_orb_data_t El2Alfriend | ( | orb_data_t | el, |
double | true_anom | ||
) |
Convert a standard orbital element set into the Alfriend orbital element set *sctlib.
Convert a standard orbital element set into the Alfriend orbital element set *sctlib.
el | Standard orbital elements [a,i,W,w,e,M] |
true_anom | True anomaly |
References orb_data_s::ecc, alf_orb_data_s::inc, orb_data_s::inc, orb_data_s::perigee, alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::raan, orb_data_s::raan, alf_orb_data_s::sma, orb_data_s::sma, alf_orb_data_s::true_lat, TWO_PI, and WrapPhase().
alf_orb_data_s add_elements | ( | alf_orb_data_t | el1, |
alf_orb_data_t | el2 | ||
) |
Add two sets of orbital elements *sctlib.
Add two sets of orbital elements *sctlib.
el1 | First element set |
el2 | Second element set |
References alf_orb_data_s::inc, alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::raan, alf_orb_data_s::sma, alf_orb_data_s::true_lat, and WrapPhase().
Referenced by FFEccDeltaElem2Hills().
orb_data_s add_elements | ( | orb_data_t | el1, |
orb_data_t | el2 | ||
) |
Add two sets of orbital elements *sctlib.
Add two sets of orbital elements *sctlib.
epoch and orb_rate are not set.
el1 | First element set |
el2 | Second element set |
References orb_data_s::ecc, orb_data_s::inc, orb_data_s::mean_anom, orb_data_s::perigee, orb_data_s::raan, orb_data_s::sma, orb_data_s::true_anom, and WrapPhase().
alf_orb_data_s sub_elements | ( | alf_orb_data_t | el1, |
alf_orb_data_t | el2 | ||
) |
Subtract one element set from another.
el1 | First element set |
el2 | Second element set, subtracted from first |
References alf_orb_data_s::inc, alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::raan, alf_orb_data_s::sma, alf_orb_data_s::true_lat, and WrapPhase().
orb_data_s sub_elements | ( | orb_data_t | el1, |
orb_data_t | el2 | ||
) |
Subtract one element set from another.
epoch and orb_rate are not set.
el1 | First element set |
el2 | Second element set, subtracted from first |
References orb_data_s::ecc, orb_data_s::inc, orb_data_s::mean_anom, orb_data_s::perigee, orb_data_s::raan, orb_data_s::sma, orb_data_s::true_anom, and WrapPhase().
double CirclePhase | ( | double | beta | ) |
Compute the desired phase on a circle from the desired phase on the ellipse. The circle is superscribed about the ellipse.
Compute the desired phase on a circle from the desired phase on the ellipse. The circle is superscribed about the ellipse.
The circle is superscribed about the ellipse.
beta | Phase angle measured on ellipse [rad] |
References PI, and WrapPhase().
Referenced by GenerateTeamGoals(), GeometryCirc2Ecc(), Goals2Hills(), RotateState(), and SetupAssignmentProblem().
double EllipsePhase | ( | double | alpha | ) |
Compute the desired phase on an ellipse from the desired phase on the circle. The circle is superscribed about the ellipse.
Compute the desired phase on an ellipse from the desired phase on the circle. The circle is superscribed about the ellipse.
The circle is superscribed about the ellipse.
alpha | Phase angle measured on superscribed circle [rad] |
References PI, and WrapPhase().
Referenced by GenerateTeamGoals(), GeometryEcc2Circ(), Hills2Goals(), PCGoals(), and RotateState().
alf_orb_data_t OrbElemDiff | ( | alf_orb_data_t | el0, |
alf_orb_data_t | el | ||
) |
Computes the differences between Alfriend orbital element vectors.
el0 | Reference orbital elements (Alfriend format) |
el | Secondary orbital elements (Alfriend format) |
References alf_orb_data_s::inc, alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::raan, alf_orb_data_s::sma, alf_orb_data_s::true_lat, and WrapPhase().
Referenced by ECI2MeanElements(), and FFEccHills2DeltaElem().
orb_data_t OrbElemDiff | ( | orb_data_t | el0, |
orb_data_t | el | ||
) |
Computes the differences between standard orbital element vectors.
epoch and orb_rate are not set.
el0 | Reference orbital elements (standard format) |
el | Secondary orbital elements (standard format) |
References orb_data_s::ecc, orb_data_s::inc, orb_data_s::mean_anom, orb_data_s::perigee, orb_data_s::raan, orb_data_s::sma, orb_data_s::true_anom, and WrapPhase().
alf_orb_data_t Goals2DeltaElem | ( | alf_orb_data_t | el0, |
geom_t | goals | ||
) |
Computes the desired element differences, given the geometric goals and the measured orbital elements.
el0 | Measured orbital elements (Alfriend format) |
goals | Geometric goals data structure |
References Goals2Hills(), and Hills2DeltaElem().
Referenced by EstimateCost(), and ImpulsiveManeuver().
geom_t DeltaElem2Goals | ( | alf_orb_data_t | el0, |
alf_orb_data_t | dEl | ||
) |
Computes the geoemtric goals, given the desired element differences and the measured orbital elements.
el0 | Measured orbital elements (Alfriend format) [a,theta,i,q1,q2,W] |
dEl | Desired orbital element differences |
References DeltaElem2Hills(), and Hills2Goals().
Referenced by AutoFormGeometry().
ml_matrix DeltaElem2Hills | ( | alf_orb_data_t | elA, |
alf_orb_data_t | dEl | ||
) |
Computes the Hills frame state from the element differences and reference elements.
Computes the Hills frame state from the element differences and reference elements.
elA | Reference orbital elements [Alfriend format] |
dEl | Orbital element difference |
References alf_orb_data_s::inc, MU_EARTH, alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::raan, alf_orb_data_s::sma, and alf_orb_data_s::true_lat.
Referenced by DeltaElem2Goals().
alf_orb_data_s Hills2DeltaElem | ( | alf_orb_data_t | el0, |
const ml_matrix & | xH | ||
) |
Computes the element differences from the Hills frame state and reference elements.
el0 | Chief orbital elements in alfriend format |
xH | Relative position and velocity in Hills frame |
References alf_orb_data_s::inc, MU_EARTH, alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::raan, RADIUS_EARTH, alf_orb_data_s::sma, and alf_orb_data_s::true_lat.
Referenced by Goals2DeltaElem(), and ImpulsiveManeuver().
geom_s Hills2Goals | ( | alf_orb_data_t | el0, |
const ml_matrix & | xH | ||
) |
Computes the geometric goals from the Hills frame state and reference elements.
el0 | Reference orbital elements |
xH | Relative state in Hills frame |
References geom_s::aE, geom_s::beta, EllipsePhase(), MU_EARTH, alf_orb_data_s::sma, alf_orb_data_s::true_lat, geom_s::y0, geom_s::zInc, and geom_s::zLan.
Referenced by DeltaElem2Goals(), GenerateTeamGoals(), and IterativeImpulsiveManeuver().
ml_matrix Goals2Hills | ( | alf_orb_data_t | el0, |
geom_t | goals | ||
) |
Computes the Hills frame state from the geometric goals and reference elements.
el0 | Reference orbital elements |
goals | Geometric goals |
References geom_s::aE, geom_s::beta, CirclePhase(), orb_rate(), alf_orb_data_s::sma, alf_orb_data_s::true_lat, geom_s::y0, geom_s::zInc, and geom_s::zLan.
Referenced by FFEccGenerateTeamGoals(), GenerateTeamGoals(), Goals2DeltaElem(), IterativeImpulsiveManeuver(), and LPCircularTimeWeight().
orb_data_s FFEccHills2DeltaElem | ( | orb_data_t | el0, |
const ml_matrix & | xH | ||
) |
Computes the orbital element differences from the Hills frame state and the reference orbital elements.
[standard format]
el0 | Reference orbital elements |
xH | Relative position and velocity in Hills frame [km, km/s] |
References el_to_rv(), hills_to_eci(), OrbElemDiff(), and rv_to_el().
alf_orb_data_s FFEccHills2DeltaElem | ( | alf_orb_data_t | el0, |
const ml_matrix & | xH | ||
) |
Computes the orbital element differences from the Hills frame state and the reference orbital elements.
[alfriend format]
el0 | Reference orbital elements |
xH | Relative position and velocity in Hills frame [km, km/s] |
References Alfriend2El(), El2Alfriend(), el_to_rv(), hills_to_eci(), OrbElemDiff(), rv_to_el(), and orb_data_s::true_anom.
ml_matrix FFEccDeltaElem2Hills | ( | alf_orb_data_t | el0, |
alf_orb_data_t | dEl | ||
) |
Convert element differences to Hills frame coordinates in an eccentric orbit.
el0 | Reference orbital elements [Alfriend format] |
dEl | Orbit element differences |
References add_elements(), Alfriend2El(), eci_to_hills(), and el_to_rv().
Referenced by FFEccDeltaElem2Goals().
ecc_geom_t FFEccDeltaElem2Goals | ( | alf_orb_data_t | el0, |
alf_orb_data_t | dEl | ||
) |
Convert element differences to eccentric geometric goals.
el0 | Orbital elements [Alfriend format] |
dEl | Orbital element differences |
References FFEccDeltaElem2Hills(), FFEccHills2Goals(), orb_rate(), alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::sma, and alf_orb_data_s::true_lat.
Referenced by AutoFormGeometry().
ml_matrix FFEccGoals2Hills | ( | double | e, |
double | nu, | ||
ecc_geom_t | g, | ||
double | n | ||
) |
Compute Hills frame state (in time-domain) given geometric goals and orbit data.
e | Eccentricity |
nu | True anomaly [rad] |
g | Data structure of geometric goals |
n | Mean orbit rate [rad/s] |
References FFEccDH(), FFEccGoals(), FFEccProp(), and Nu2TimeDomain().
Referenced by FFEccEstimateCost(), FFEccGenerateTeamGoals(), IterativeImpulsiveManeuver(), and LPEccentricTimeWeight().
ml_matrix FFEccGoals2Hills | ( | double | e, |
double | nu, | ||
ecc_geom_xy_t | g, | ||
double | n | ||
) |
Compute Hills frame state (in time-domain) given geometric goals and orbit data.
e | Eccentricity |
nu | True anomaly [rad] |
g | Data structure of geometric goals |
n | Mean orbit rate [rad/s] |
References FFEccDH(), FFEccGoals(), FFEccProp(), and Nu2TimeDomain().
ml_matrix FFEccGoals2Hills | ( | double | e, |
double | nu, | ||
ecc_geom_t | g | ||
) |
Compute Hills frame state (in nu-domain) given geometric goals and orbit data.
e | Eccentricity |
nu | True anomaly [rad] |
g | Data structure of geometric goals |
References FFEccDH(), FFEccGoals(), and FFEccProp().
ecc_geom_t FFEccHills2Goals | ( | double | e, |
double | nu, | ||
ml_matrix | xH, | ||
double | n | ||
) |
Compute geometric goals given Hills frame state (in time-domain) and orbit data.
e | Eccentricity |
nu | True anomaly [rad] |
xH | Hills frame state |
n | Mean orbit rate [rad/s] |
References FFEccDH(), FFEccRMat(), FFEccXExt(), FFEccYExt(), FFEccZExt(), ecc_geom_s::nu_xMax, ecc_geom_s::nu_zMax, PI, Time2NuDomain(), WrapPhase(), ecc_geom_s::xMax, ecc_geom_s::y0, and ecc_geom_s::zMax.
Referenced by FFEccDeltaElem2Goals(), FFEccGenerateTeamGoals(), and IterativeImpulsiveManeuver().
ecc_geom_t FFEccHills2Goals | ( | double | e, |
double | nu, | ||
const ml_matrix & | xH | ||
) |
Compute geometric goals given Hills frame state (in nu-domain) and orbit data.
e | Eccentricity |
nu | True anomaly [rad] |
xH | Hills frame state |
References FFEccDH(), FFEccRMat(), FFEccXExt(), FFEccYExt(), FFEccZExt(), ecc_geom_s::nu_xMax, ecc_geom_s::nu_zMax, PI, WrapPhase(), ecc_geom_s::xMax, ecc_geom_s::y0, and ecc_geom_s::zMax.
void Nu2TimeDomain | ( | ml_matrix & | x, |
double | n, | ||
double | e, | ||
double | nu | ||
) |
Convert a relative orbit state from the nu-domain to the time-domain.
x | Relative State in nu-domain. Passed by reference. |
n | Mean orbit rate [rad/s] |
e | Eccentricity |
nu | True anomaly [rad] |
References NuDot().
Referenced by FFEccGoals2Hills(), and FFEccHillsEqns().
void Time2NuDomain | ( | ml_matrix & | x, |
double | n, | ||
double | e, | ||
double | nu | ||
) |
Convert a relative orbit state from the time-domain to the nu-domain.
x | Relative State in time-domain |
n | Mean orbit rate [rad/s] |
e | Eccentricity |
nu | True anomaly [rad] |
x | Relative State in nu-domain. Passed by reference. |
References NuDot().
Referenced by FFEccHills2Goals(), and FFEccHillsEqns().
ecc_geom_t GeometryCirc2Ecc | ( | double | w, |
geom_t | gCirc | ||
) |
Convert a circular geometry structure to an eccentric geometry structure.
w | Argument of perigee [rad] |
gCirc | Geometry goals structure for circular orbits |
References geom_s::aE, geom_s::beta, CirclePhase(), ecc_geom_s::nu_xMax, ecc_geom_s::nu_zMax, PI, ecc_geom_s::xMax, geom_s::y0, ecc_geom_s::y0, geom_s::zInc, geom_s::zLan, and ecc_geom_s::zMax.
Referenced by ImpulsiveLPManeuver(), and IterativeImpulsiveManeuver().
geom_t GeometryEcc2Circ | ( | double | w, |
ecc_geom_t | gEcc | ||
) |
Convert an eccentric geometry structure to a circular geometry structure.
w | Argument of perigee [rad] |
gEcc | Geometry goals structure for Eccentric orbits |
References geom_s::aE, geom_s::beta, EllipsePhase(), ecc_geom_s::nu_xMax, ecc_geom_s::nu_zMax, PI, ecc_geom_s::xMax, geom_s::y0, ecc_geom_s::y0, geom_s::zInc, geom_s::zLan, and ecc_geom_s::zMax.
Referenced by ImpulsiveLPManeuver(), and IterativeImpulsiveManeuver().
Rotate a geometric goal set to the circular phase angle phi.
goals | Geometric goals data structure |
phi | Circular phase angle |
References geom_s::beta, CirclePhase(), EllipsePhase(), geom_s::zInc, and geom_s::zLan.
Referenced by EstimateCost(), and FFEccGenerateTeamGoals().
double MeanAnom2TrueLat | ( | double | e, |
double | w, | ||
double | M | ||
) |
Convert mean anomlay to true latitude.
Convert mean anomlay to true latitude.
e | Eccentricity |
w | Argument of Perigee [rad] |
M | Mean Anomaly [rad] |
References mean_to_true(), TWO_PI, and WrapPhase().
Referenced by LPCircularTimeWeight().
void GetHillsMats | ( | const ml_matrix & | r0, |
const ml_matrix & | v0, | ||
ml_matrix & | A, | ||
ml_matrix & | Adot | ||
) |
Computes the A and Adot matrices used for transformation to Hills frame.
r0 | Position, ECI frame |
v0 | Velocity, ECI frame |
A | Rotation Matrix (position). Passed by reference. |
Adot | Rotation Matrix (velocity). Passed by reference. |
References orb_data_s::ecc, MU_EARTH, rv_to_el(), orb_data_s::sma, and orb_data_s::true_anom.
Referenced by AbsRelECI2Hills().
void AbsRelECI2Hills | ( | ml_matrix | r0, |
ml_matrix | v0, | ||
ml_matrix | dr, | ||
ml_matrix | dv, | ||
ml_matrix & | rH, | ||
ml_matrix & | vH | ||
) |
Compute the relative position and velocity in Hills frame given an absolute ECI position & velocity and a relative inertial position & velocity.
r0 | Reference position in ECI frame |
v0 | Reference velocity in ECI frame |
dr | Relative position in ECI frame |
dv | Relative velocity in ECI frame |
rH | Curvilinear Hills frame position [dR; r1*dTheta; dZ ]. Passed by reference. |
vH | Curvilinear Hills frame velocity [dRDot; r1*dThetaDot + r1Dot*dTheta; dZDot]. Passed by reference. |
References GetHillsMats().
alf_orb_data_t Osc2Mean | ( | alf_orb_data_t | el, |
double | J2 | ||
) |
Transforms osculating orbital elements to mean orbital elements.
el | Osculating orbital elements |
J2 | J2 perturbation |
References alf_orb_data_s::inc, alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::raan, RADIUS_EARTH, alf_orb_data_s::sma, and alf_orb_data_s::true_lat.
Referenced by ECI2MeanElements().
alf_orb_data_t * ECI2MeanElements | ( | const ml_matrix & | xRefECI, |
const ml_matrix & | xRelECI, | ||
double | J2, | ||
alf_orb_data_t & | elRefMean | ||
) |
Computes mean orbital elements and mean orbital element differences from reference and relative ECI position and velocity.
xRefECI | Reference position & velocity in ECI frame |
xRelECI | Relative positions & velocities in ECI frame |
J2 | Size of J2 perturbation |
elRefMean | Mean orbital elements of the reference orbit. Passed by reference. |
References El2Alfriend(), OrbElemDiff(), Osc2Mean(), rv_to_el(), and orb_data_s::true_anom.
bool AlignThruster | ( | const ml_matrix & | aH, |
const ml_matrix & | bTh, | ||
ml_matrix & | qHB | ||
) |
Computes desired Hills-to-body quaternion for a thruster firing. Rotation about thruster-axis is ignored.
Computes desired Hills-to-body quaternion for a thruster firing. Rotation about thruster-axis is ignored.
Rotation about thruster-axis is ignored.
aH | Unit Hills frame vector of applied acceleration |
bTh | Unit body vector of Hall Effect Thruster nozzle |
qHB | Desired ECI-to-body quaternion. Passed by reference. |
References u_to_q().
bool AlignThruster | ( | const ml_matrix & | aH, |
const ml_matrix & | bTh, | ||
const ml_matrix & | bST, | ||
const ml_matrix & | r, | ||
const ml_matrix & | v, | ||
double | jD, | ||
double | sep, | ||
ml_matrix & | qHB | ||
) |
Computes desired Hills-to-body quaternion for a thruster firing such that the star tracker is pointed as far away as possible from the sun, earth and moon.
aH | Unit Hills frame vector of applied acceleration |
bTh | Unit body vector of Hall Effect Thruster nozzle |
bST | Unit body vector of star tracker bore-sight |
r | Position in ECI at burn time [km] |
v | Velocity in ECI at burn time [km/s] |
jD | Julian Date of burn time [JD] |
sep | Required angular separation between bore-sight and bright body horizon [rad] |
qHB | Desired ECI-to-body quaternion. Passed by reference. |
References au_to_q(), moon_vector(), PI, q_eci_to_hills(), sun_vector(), and u_to_q().
ml_matrix Hills2Frenet | ( | const ml_matrix & | xH, |
const double & | e, | ||
const double & | nu, | ||
const double & | n | ||
) |
Transforms from the Hills frame to the Frenet frame.
Transforms from the Hills frame to the Frenet frame.
xH | (6,1) Relative position and velocity in Hills-frame |
e | (1,1) Orbit eccentricity |
nu | (1,1) True anomaly (rad) |
n | (1,1) Mean orbit rate (rad/s) |
ml_matrix HillsEqns | ( | const ml_matrix & | xH0, |
double | n, | ||
double | t | ||
) |
Closed form solution of relative orbital motion using Hills equations.
Closed form solution of relative orbital motion using Hills equations. For circular orbits only.
For circular orbits only.
xH0 | Initial state in Hills frame |
n | Orbital Rate (rad/s) |
t | Time (sec) |
Referenced by ImpulsiveLPManeuver().
ml_matrix FFEccHillsEqns | ( | ml_matrix | xH0, |
double | nu0, | ||
const ml_matrix & | nu, | ||
double | e, | ||
double | n | ||
) |
Compute Hills frame state (in time-domain) at future true anomaly(s). For eccentric or circular orbits.
Compute Hills frame state (in time-domain) at future true anomaly(s). For eccentric or circular orbits.
For eccentric or circular orbits
xH0 | Initial Hills frame state |
nu0 | True anomaly (at initial state) [rad] |
nu | True anomaly (at new state) [rad] |
e | Eccentricity |
n | Mean orbit rate [rad/s] |
References FFEccIntConst(), FFEccRMat(), Nu2TimeDomain(), and Time2NuDomain().
Referenced by ImpulsiveLPManeuver().
ml_matrix FFEccHillsEqns | ( | const ml_matrix & | xH0, |
double | nu0, | ||
const ml_matrix & | nu, | ||
double | e | ||
) |
Compute Hills frame state (in nu-domain) at future true anomaly(s). For eccentric or circular orbits.
Compute Hills frame state (in nu-domain) at future true anomaly(s). For eccentric or circular orbits.
For eccentric or circular orbits
xH0 | Initial Hills frame state |
nu0 | True anomaly (at initial state) [rad] |
nu | True anomaly (at new state) [rad] |
e | Eccentricity |
References FFEccIntConst(), and FFEccRMat().
ml_matrix DiscreteHills | ( | const ml_matrix & | x0, |
double | n, | ||
const ml_matrix & | aC, | ||
double | dT | ||
) |
Compute the force relative trajectory from the initial state and time-history of applied accelerations. For circular orbits only.
Compute the force relative trajectory from the initial state and time-history of applied accelerations. For circular orbits only.
For circular orbits only.
x0 | Initial state |
n | Reference orbit rate (rad/sec) |
aC | Commanded accelerations |
dT | Thruster sampling time |
References lin_orb().
ml_matrix FFEccDiscreteHills | ( | double | e, |
double | n, | ||
const ml_matrix & | x0, | ||
double | nu0, | ||
const ml_matrix & | aC, | ||
const ml_matrix & | t | ||
) |
Compute the force relative trajectory from the initial state and time-history of applied accelerations. For circular orbits only.
Compute the force relative trajectory from the initial state and time-history of applied accelerations. For circular orbits only.
For circular orbits only.
e | Eccentricity |
n | Mean orbit rate (rad/sec) |
x0 | Initial relative state |
nu0 | Initial true anomaly |
aC | Commanded accelerations |
t | Time vector (final time corresponds to final state) |
References diff(), FFEccLinOrb(), mean_to_true(), true_to_mean(), and UnwrapPhase().
double CollProbSet | ( | double | sigma, |
const ml_matrix & | S, | ||
const ml_matrix & | xc, | ||
const ml_matrix & | Ssc0, | ||
int | n | ||
) |
Collision probability for relative frame (single ellipsoid to point at origin) using ellipsoid shells.
Collision probability calculation using sets of ellipsoids
The units of the inputs should all be consistent, for example km and km2. Only the position of the ellipsoid shold be input.
sigma | Standard deviations of input ellipsoid, ex. 1.0 or 2.0 |
S | Ellipsoid matrix, 3x3, [length2] |
xc | Ellipsoid center, 3x1, [length] |
Ssc0 | Hard-body ellipsoid for single spacecraft [length2] |
n | Number of shells in computation |
References PI.
Referenced by MonitoringAlg().
ml_matrix MonitoringAlg | ( | ml_matrix & | y, |
ml_matrix & | t, | ||
ml_matrix & | M, | ||
ml_matrix & | nu, | ||
ml_matrix & | accel, | ||
ml_matrix & | aDiff, | ||
coll_mon_data_s | d | ||
) |
Collision monitoring algorithm implementation used for both modes, monitoring and surveying.
Collision monitoring algorithm implementation used for both modes, monitoring and surveying.
Requires an acceleration matrix for each relative spacecraft, which will be zero for monitoring. The differential accelerations aDiff may not be zero or else the data output will be corrupted. Currently only the first element of aDiff is used to scale Q assuming that it is a worst-case representation. The function GenerateTimeVector can be used to create t, M, and nu given the total time window.
y | Relative state, 6xm |
t | Time vector (sec), 1xn |
M | Mean anomaly, 1xn |
nu | True anomaly, 1xn |
accel | Stacked relative accleration vectors, 3mxn |
aDiff | Differential accelerations, 3x1 |
d | Collision monitoring data structure |
References c_to_d_zoh(), CollProbSet(), DeltaEl2AlfriendMat(), DeltaElem2HillsMat(), diff(), distant_pt_ell(), orb_data_s::ecc, El2Alfriend(), coll_mon_data_s::el_ref, FFEccLinOrb(), GVEErrorDynamics(), alf_orb_data_s::inc, coll_mon_data_s::lenSC, orb_data_s::mean_anom, mean_to_true(), coll_mon_data_s::nPts, orb_rate(), orb_data_s::perigee, coll_mon_data_s::Pmin, alf_orb_data_s::q1, alf_orb_data_s::q2, alf_orb_data_s::raan, coll_mon_data_s::S0, coll_mon_data_s::sigma, alf_orb_data_s::sma, orb_data_s::sma, coll_mon_data_s::Ssc, alf_orb_data_s::true_lat, UnwrapPhase(), and WrapPhase().
Referenced by CollisionSurvey().
ml_matrix CollisionSurvey | ( | ml_matrix & | y, |
double | t0, | ||
maneuver_t | mvr1, | ||
maneuver_t | mvr2[], | ||
int | n_points, | ||
coll_mon_data_s | d | ||
) |
Collision survey implementation which changes maneuvers to acceleration vectors and calls monitoring algorithm.
y | Relative state (6,n). Passed by reference. |
t0 | Current time (MET) |
mvr1 | Maneuver data structure for self |
mvr2 | Maneuver structure array for relative spacecraft, must be [n] |
n_points | Number of points for time vector |
d | Collision monitor data structure |
References coll_mon_data_s::el_ref, GenerateTimeVector(), ManeuverStruct2AccelVector(), MonitoringAlg(), maneuver_s::nBurns, and maneuver_s::tF.
ml_matrix DeltaElem2HillsMat | ( | const ml_matrix & | elA | ) |
Compute transformation matrix from delta elements to Hills.
Compute transformation matrix from delta elements to Hills.
elA | Reference orbital elements [Alfriend format] |
References MU_EARTH.
Referenced by MonitoringAlg().
ml_matrix DeltaEl2AlfriendMat | ( | orb_data_t | el | ) |
Compute transformation matrix from standard differential elements to Alfriend differential elements.
Compute transformation matrix from standard differential elements to Alfriend differential elements.
el | Reference orbit data |
References diff(), orb_data_s::ecc, orb_data_s::mean_anom, mean_to_true(), orb_data_s::perigee, and UnwrapPhase().
Referenced by MonitoringAlg().
double distant_pt_ell | ( | const ml_matrix & | S, |
const ml_matrix & | U, | ||
const ml_matrix & | xc, | ||
const ml_matrix & | x0, | ||
ml_matrix & | x | ||
) |
Compute minimum distance from a distance point to an ellipsoid's surface.
Compute minimum distance from a distance point to an ellipsoid's surface.
S | Singular value matrix of ellipsoid (3,3) |
U | Rotation matrix of ellipsoid (3,3) |
xc | Center of ellipsoid (3,1) |
x0 | Outside point (3D) (3,1) |
x | Point on the ellipsoid (3,1) |
References Laguerre().
Referenced by MonitoringAlg().
double Laguerre | ( | const ml_matrix & | a, |
double | x_guess | ||
) |
Laguerre root finding algorithm adapted to real roots.
Laguerre root finding algorithm adapted to real roots.
a | Vector containing the coefficients of the polynomial, a[i] x^i |
x_guess | Initial guess for the root |
Referenced by distant_pt_ell().
ml_matrix GenerateTimeVector | ( | orb_data_t | d, |
const double | tF, | ||
const int | nPts, | ||
ml_matrix & | M, | ||
ml_matrix & | nu | ||
) |
Generate a time vector evenly spaced over true anomaly.
Also returns the corresponding mean and true anomaly vectors.
d | Orbit elements data |
tF | Final time |
nPts | Number of points per orbit |
M | Matrix for returning mean anomaly vector |
nu | Matrix for returning true anomaly vector |
References orb_data_s::ecc, orb_data_s::mean_anom, mean_to_true(), orb_rate(), PI, orb_data_s::sma, true_to_mean(), TWO_PI, and UnwrapPhase().
Referenced by CollisionSurvey().
ml_matrix ManeuverStruct2AccelVector | ( | maneuver_t | mvr, |
const ml_matrix & | tProp | ||
) |
Compute a 3xN acceleration vector from a maneuver data structure.
Compute a 3xN acceleration vector from a maneuver data structure.
mvr | Maneuver data structure, containing an array of burn data structures |
tProp | Time history to use for acceleration vector [sec] |
References maneuver_s::burnData, burn_s::dT, burn_s::dV, maneuver_s::nBurns, burn_s::t, burn_s::uX, burn_s::uY, and burn_s::uZ.
Referenced by CollisionSurvey().
bool TeamLevels | ( | team_t | teams[], |
int | num, | ||
ml_int_array & | levels | ||
) |
Assign a hierarchical level to each team in the array.
Assign a hierarchical level to each team in the array.
teams | Array of team data structures. |
num | Number of team data structures. |
levels | Array of team hierarchical team levels. Passed by reference. |
References FindUpperTeams().
int IsRelative | ( | int | iD, |
team_t | teams[], | ||
int | num | ||
) |
Determine whether the given satellite ID is a relative of any team.
Determine whether the given satellite ID is a relative of any team.
iD | Unique integer ID of the satellite being checked. |
teams[] | Array of team data structures. |
num | Number of team data structures. |
References team_s::memID.
Referenced by FindUpperTeams().
bool FindUpperTeams | ( | int | kBot, |
team_t | teams[], | ||
int | num, | ||
ml_int_array & | kUpper | ||
) |
Find teams above this one in the hierarchy.
Find teams above this one in the hierarchy.
kBot | Index of team to consider. Find all teams above this one. |
teams | Array of team data structures. |
num | Number of team data structures. |
kUpper | Array of team indices that are above this one in the hierarchy. Passed by reference. |
References IsRelative().
Referenced by TeamLevels().
double TimeUntilTheta | ( | double | a, |
double | w, | ||
double | e, | ||
double | theta0, | ||
double & | theta1 | ||
) |
Computes the time in seconds until the new latitude is reached from the original latitude.
a | Semi-major axis [km] |
w | Argument of perigee [rad] |
e | Eccentricity |
theta0 | Initial true latitude [rad] |
theta1 | Final true latitude [rad] |
References orb_rate(), true_to_mean(), and TWO_PI.
Referenced by InPlane(), and OutOfPlane().
void AccelVector2ManeuverStruct | ( | const ml_matrix & | aC, |
ml_matrix | t, | ||
double | tRef, | ||
double | nomAccel, | ||
double | minSepTime, | ||
maneuver_t & | mvr, | ||
ml_matrix & | dV | ||
) |
Build a maneuver structure from acceleration and time vectors.
aC | 3xN acceleration matrix. [km/s/s] |
t | Time vector in seconds, starting at 0. |
tRef | Reference time in seconds, [MET]. This is added to the time vector "t" after differencing to avoid numerical rounding errors. |
nomAccel | Nominal acceleration in [km/s]. Each burn is assumed to apply this acceleration. This relates the burn duration to the delta-v magnitude. |
minSepTime | Minimum allowable separation time between end of one burn and beginning of next. Multiple burns closer together than this will be grouped into a single burn. |
mvr | Maneuver data structure with burn info. Passed by reference. |
dV | Non-zero delta-v magnitudes. Passed by reference. |
References maneuver_s::burnData, diff(), burn_s::dT, burn_s::dV, FF_TOL, burn_s::iD, maneuver_s::iD, maneuver_s::nBurns, burn_s::t, maneuver_s::t0, maneuver_s::tF, burn_s::uX, burn_s::uY, and burn_s::uZ.
Referenced by ImpulsiveLPManeuver().
void ManeuverStruct2AccelVector | ( | maneuver_t | mvr, |
double | dT, | ||
ml_matrix & | aC, | ||
ml_matrix & | t | ||
) |
Compute a 3xN acceleration vector from a maneuver data structure.
mvr | Maneuver data structure, containing an array of burn data structures |
dT | Time-step to use in creating the acceleration time-history [sec] |
aC | 3xN acceleration matrix [km/s/s]. Passed by reference. |
t | 1xN+1 time vector [sec]. Passed by reference. |
References maneuver_s::burnData, burn_s::dT, burn_s::dV, maneuver_s::nBurns, burn_s::t, burn_s::uX, burn_s::uY, and burn_s::uZ.
ml_matrix NOrbVector | ( | window_t | win | ) |
Compute a vector of maneuver durations (in orbits) from time window data.
win | Time window data structure |
References window_s::nManeuvers, window_s::nOrbMax, and window_s::nOrbMin.
Referenced by ImpulsiveManeuver(), IterativeImpulsiveManeuver(), LPCircularTimeWeight(), and LPEccentricTimeWeight().
ml_matrix TargetTrueAnom | ( | double | e, |
double | nu0, | ||
ml_matrix | nOrb | ||
) |
Compute the future true anomaly (unwrapped) at the specified number of orbits after the initial true anomaly.
e | Eccentricity |
nu0 | Initial true anomaly [rad] |
nOrb | Number of orbits into the future to evaluate the true anomaly |
References mean_to_true(), true_to_mean(), TWO_PI, and WrapPhase().
Referenced by ImpulsiveLPManeuver(), and LPEccentricTimeWeight().
ml_matrix Tetrahedron | ( | double | d, |
const ml_matrix & | eul | ||
) |
Compute the 4 points of a regular tetrahedron, the surface area and volume.
d | Distance of each side. |
eul | Euler angles for rotating the tetrahedron. [rad] |
References eul_to_q().
Referenced by FFEccTetrahedronGeometry().
char * matout | ( | ml_matrix | mat | ) |
Output a matrix to a string with higher precision than built-in "to_string" function.
Output a matrix to a string with higher precision than built-in "to_string" function.
mat | Matrix to convert to a string. |