sc_formation.h

Go to the documentation of this file.

/* fflib/formation.h.  Generated by configure.  */
/*
 * formation.h
 *
 * Programmer(s): Joe Mueller, David Wilson, Lavanya Swetharanyan, Stephanie Thomas
 *
 * Copyright 2005, Princeton Satellite Systems. All rights reserved.
 *
 *
 *
 *
 * NOTES
 * =====
 * 1.   I placed a "*sctlib" comment next to the functions that I think should go in sctlib. The group of 3-4
 *              functions at the bottom of this header should definitely go in sctlib.
 *
 * 2.   We should probably create a new structure "assignment_params" that includes {N, M, P, Pu, Q, phi, u, f}. 
 *              The first 7 params are returned from SetupAssignmentProblem, and the last one is the cost matrix that 
 *              gets filled during the distributed assignment process, created with InitializeCostMatrix.
 * 
 * 3.   The "NewtRaph" function is called by 3 other functions (FFEccXExt, ""YExt, ""ZExt). If its too hard to 
 *              implement as a seperate function, we can just perform the newton-raphson method seperately in each one.
 *
 * 4.   In the prototype, both julian date (JD) and seconds since 1970 (SS1970) units are used. In this library,
 *              we will use MET (a double in seconds) as the absolute time for referencing all time-tags. By using MET, 
 *              we can consistently use seconds to represent all times (orbit periods, timesteps, timetags, etc). 
 *              MET will be computed in the MANTA system by first creating a global ma_time_record called MST (mission
 *              start time), and then subtracting it from the current time.
 *
 *              side-note:  The ThrusterAlignment function requires the Julian Date because it needs to compute the
 *                                      location of the sun and moon with respect to the Earth. 
 *
 * 5.   There are many cases in which I've used "int", but we may want to use "unsigned short" to use less
 *              memory. For example, spacecraft ID numbers, which will always be postive and less than 10,000.
 *
 *
 *
 */
#ifndef __FORMATION__
#define __FORMATION__
 
#include <stdlib.h>
#include <math.h>
#include "sc_constants.h"
#include "sc_coord.h"
#include "sc_environs.h"
#include "sc_ephemeris.h"
#include "sc_gravity.h"
#include "sc_math.h"
#include "sc_orbit.h"
#ifdef AS_OS_WINDOWS
#include "matrixlib.h"
#else
#include <MatrixLib/MatrixLib.h>
#endif
 
 
extern "C++"
{
 
// Define the size of the J2 perturbation
#ifdef FF_NO_J2
#define FF_J2 0.0
#else
#define FF_J2 0.001082
#endif
 
#if 0
#pragma mark Error Codes 
#endif
typedef enum {
    FF_NO_ERROR = 0,                
    FF_NO_SOLN  = 1,                            
    FF_MAX_ITER = 2,                
    FF_BAD_VALUE = 3,                           
} ff_error_codes;
 
#if 0
#pragma mark Formation Types
#endif
typedef enum {
        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,   
} ff_formation_types;
 
#define FF_TOL 1e-8
 
#define FF_MAX_ITER 100
 
#define FF_MAX_BURNS 10
 
#define FF_MAX_MEMBERS 8
 
/* Linux OS */
/* #undef SC_OS_LINUX */
 
#define SC_OS_MACOSX 
 
#define SC_OS_UNIX 
 
#if 0
#pragma mark Data Structures 
#endif
 
typedef struct alf_orb_data_s {
    double sma;                 
    double inc;                 
    double raan;                
    double true_lat;                    
    double q1;                                  
    double q2;                                  
    double epoch;               
} alf_orb_data_t;
 
typedef struct team_s {
    int teamID;                 
    int nMembers;               
    ml_int_array memID; 
    int refID;                  
    int captainID;              
    int level;                  
} team_t;
 
typedef struct state_s {
    orb_data_t el;              
    alf_orb_data_t elA; 
    alf_orb_data_t dEl; 
    ml_matrix xH;               
    double tM;                  
    double mass;        
} state_t;
 
typedef struct geom_s {
        double y0;                      
        double aE;                      
        double beta;            
        double zInc;            
        double zLan;            
} geom_t;
 
typedef struct ecc_geom_s {
        double y0;                      
        double xMax;            
        double nu_xMax;         
        double zMax;            
        double nu_zMax;         
} ecc_geom_t;
 
typedef struct ecc_geom_xy_s {
        double y0;                      
        double x;                       
        double y;                       
        double nu_xy;           
        double zMax;            
        double nu_zMax;         
} ecc_geom_xy_t;
 
typedef struct window_s {
        double startTime;               
        double nOrbMin;                 
        double nOrbMax;                 
        int nManeuvers;                 
        double timeWeightExp;   
} window_t;
 
typedef struct plan_param_s {
        double fNom;            
        double horizon;         
        double dTMin;           
        double maxDeltaV;   
        int nSPOCoarse;         
        int nSPOFine;           
        double eTol;            
} plan_param_t;
 
typedef struct constraints_s {
        bool variable;                          
        int nRestrict;                          
        ml_int_array restrictID;        
        int nDuplicates;                        
        ml_matrix phase;                        
} constraints_t;
 
typedef struct team_goals_s {
        int nU;                                                                         
        int teamID;                                                                     
        geom_t geometry[FF_MAX_MEMBERS];                        
        constraints_t constraints[FF_MAX_MEMBERS];      
        double dPhi;                                                            
} team_goals_t;
 
typedef struct ecc_team_goals_s {
        int nU;                                                                         
        int teamID;                                                                     
        ecc_geom_t geometry[FF_MAX_MEMBERS];            
        constraints_t constraints[FF_MAX_MEMBERS];      
        double dPhi;                                                            
} ecc_team_goals_t;
 
typedef struct cost_s {
        int nU;                                         
        int memID;                                      
        ml_int_array targetIndex;   
        ml_int_array costLength;        
        ml_matrix cost;                         
} cost_t;
 
typedef struct burn_s {
        double t;               
        double dT;              
        double dV;              
        double uX;              
        double uY;              
        double uZ;              
        int iD;                 
} burn_t;
 
typedef struct maneuver_s {
        bool achieve;           
        int nBurns;                     
        burn_t burnData[FF_MAX_BURNS];  
        double t0;                      
        double tF;                      
        int iD;                         
} maneuver_t;
 
typedef struct dv_command_s {
        int kTank;                      
        double pulsewidth;  
        double timeTag;         
        int iD;                         
} dv_command_t;
 
typedef struct orientation_s {
        ml_matrix qHB;                  
        double timeTag;                 
        double dT;                              
        int iD;                                 
} orientation_t;
 
typedef struct assign_s {
        int N;                                  
        int M;                                  
        int P;                                  
        int Pu;                                 
        int Q;                                  
        ml_matrix phi;                  
        ml_int_array u;                 
} assign_t;
 
typedef struct coll_mon_data_s {
    orb_data_t el_ref;              
    ml_matrix init_bounds;          
    double sigma;                   
    double Pmin;                    
    double lenSC;                   
    ml_matrix Ssc;                  
    ml_matrix S0;                   
    int nPts;                       
} coll_mon_data_t;
 
#if 0
#pragma mark Control Law 
#endif
maneuver_s IterativeImpulsiveManeuver(state_t state, geom_t goals_circ, window_t window, plan_param_t param, bool &foundSoln);
 
maneuver_s IterativeImpulsiveManeuver(state_t state, ecc_geom_t goals_ecc, window_t window, plan_param_t param, bool &foundSoln);
 
maneuver_s ImpulsiveLPManeuver(state_t state, geom_t goals_circ, window_t window, plan_param_t param, double &maxDV);
 
maneuver_s ImpulsiveLPManeuver(state_t state, ecc_geom_t goals_ecc, window_t window, plan_param_t param, double &maxDV);
 
maneuver_s ImpulsiveManeuver(state_t state, geom_t goals, window_t window, plan_param_t param, double &maxDV);
 
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 );
 
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 );
 
burn_t OutOfPlane( alf_orb_data_t meas_elem, alf_orb_data_t delta_elem, double accNom, double dTMin, unsigned short &nBurns, double &th1 );
    
#if 0
#pragma mark Guidance Law 
#endif
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 );
 
double NearestOffset( double y0, double width, ml_matrix extrema, double minSepDistance, double maxSepDistance );
 
ml_matrix InitializeCostMatrix( team_goals_t teamGoals, int nRelatives );
 
ml_matrix InitializeCostMatrix( ecc_team_goals_t teamGoals, int nRelatives );
 
int PopulateCostMatrix( ml_matrix& f, cost_t costEstimate, team_goals_t teamGoals, ml_int_array relativeIDs );
 
int PopulateCostMatrix( ml_matrix& f, cost_t costEstimate, ecc_team_goals_t teamGoals, ml_int_array relativeIDs );
 
void CostMatrixRows( team_goals_t teamGoals, int index, int& a, int& b );
 
void CostMatrixRows( ecc_team_goals_t teamGoals, int index, int& a, int& b );
 
cost_s EstimateCost( alf_orb_data_t el0, alf_orb_data_t dEl, team_goals_t teamGoals, int memID, window_t window, double weight);
 
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);
 
team_goals_s GenerateTeamGoals( alf_orb_data_t el0, int fType, double fSize, unsigned short nRels, int teamID, double angRes );
 
ecc_team_goals_s FFEccGenerateTeamGoals( orb_data_t el0, int fType, double fSize, unsigned short nRels, int teamID, double nu, const ml_matrix& eul);
 
ecc_geom_xy_s *FFEccTetrahedronGeometry( double nu, double d, const ml_matrix& eul );
 
geom_s PCGoals( double R, double alpha0, double sgn, double y0 );
 
void SortTeamGoals( team_goals_t& teamGoals );
 
void SortTeamGoals( ecc_team_goals_t& teamGoals );
 
assign_s SetupAssignmentProblem( team_goals_t teamGoals, double orbFrac = 1);
 
assign_s SetupAssignmentProblem( ecc_team_goals_t teamGoals, double orbFrac = 1);
 
int RestrictIDSet( ml_int_array& relIDs, constraints_t constraints[], int numC );
 
int VerifyAssignmentParams( assign_t a, const ml_matrix& f );
 
int PrivilegedAssignment( assign_t assign, const ml_matrix& f, int method, ml_int_array& optOrder, ml_matrix& optPhi, double& optCost );
 
int OptimalAssignment( assign_t assign, ml_matrix f, ml_int_array& optOrder, ml_matrix& optPhi, double& optCost );
 
bool EquallyPhased( const ml_matrix& phi, ml_int_array u, int Pu );
 
ml_int_array FindMinSet( ml_matrix& mat, double& minSum );
 
void permute(ml_int_array arr, int start, int num, const ml_matrix& mat, double& cost, ml_int_array& best);
 
 
#if 0
#pragma mark Linear Programming 
#endif
    
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);
 
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);
 
void LPCircularTimeWeight( orb_data_t el0, const ml_matrix& xH0, geom_t goals, window_t window, int nSPO, ml_matrix& xHF, double& nOrbMvr );
 
void LPEccentricTimeWeight( orb_data_t el0, const ml_matrix& xH0, ecc_geom_t goals, window_t window, int nSPO, ml_matrix& xHF, double& nOrbMvr );
 
void GVEErrorDynamics( orb_data_t el0, ml_matrix& A, ml_matrix &B );
 
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);
 
#if 0
#pragma mark Eccentric Geometry
#endif
void FFEccLinOrb( double n, double nu, double e, ml_matrix& a, ml_matrix& b, double dT );
 
ml_matrix FFEccLinOrb( const ml_matrix& x, const ml_matrix& acc, double n, double nu, double e );
 
ml_matrix FFEccProp( const ml_matrix& D, const ml_matrix& nu, double e, double dH );
 
ml_matrix FFEccProp( const ml_matrix& D, double nu, double e, double dH );
 
double FFEccDH( double nu0, double e );
 
void FFEccGoals( double e, ecc_geom_t goals, ml_matrix& D, ml_matrix& xH );
 
void FFEccGoals( double e, ecc_geom_xy_t goals, ml_matrix& D, ml_matrix& xH );
 
void FFEccIntConst( const ml_matrix& xH0, double nu0, double e, ml_matrix& D, double& dH, ml_matrix& R );
 
ml_matrix FFEccRMat( double nu, double e, double dH );
 
ml_matrix FFEccRMat( double nu, double e );
 
void FFEccXExt( double e, const ml_matrix& D, double dH, double epsilon, int nMax, ml_matrix& x, ml_matrix& nu );
 
void FFEccYExt( double e, const ml_matrix& D, double dH, double epsilon, int nMax, ml_matrix& y, ml_matrix& nu );
 
void FFEccZExt( double e, const ml_matrix& D, double dH, double epsilon, int nMax, ml_matrix& z, ml_matrix& nu );
 
void NuDot( double n, double e, double nu, double& nuDot, double& nuDotDot );
 
double FFEccDX( double nu, double e, const ml_matrix& D, double dH );
 
double FFEccDY( double nu, double e, const ml_matrix& D, double dH );
 
double FFEccDZ( double nu, double e, const ml_matrix& D, double dH );
 
double FFEccDDX( double nu, double e, const ml_matrix& D, double dH );
 
double FFEccDDY( double nu, double e, const ml_matrix& D, double dH );
 
double FFEccDDZ( double nu, double e, const ml_matrix& D, double dH );
 
#if 0
#pragma mark Coordinate Frames 
#endif
orb_data_t Alfriend2El( alf_orb_data_t elA );
 
alf_orb_data_t El2Alfriend( orb_data_t el );
 
alf_orb_data_t El2Alfriend( orb_data_t el, double true_anom );
 
alf_orb_data_s add_elements( alf_orb_data_t el1, alf_orb_data_t el2 );
 
orb_data_s add_elements( orb_data_t el1, orb_data_t el2 );
 
alf_orb_data_s sub_elements( alf_orb_data_t el1, alf_orb_data_t el2 );
 
orb_data_s sub_elements( orb_data_t el1, orb_data_t el2 );
 
double CirclePhase( double beta );
 
double EllipsePhase( double alpha );
 
alf_orb_data_t OrbElemDiff( alf_orb_data_t el0, alf_orb_data_t el );
 
orb_data_t OrbElemDiff( orb_data_t el0, orb_data_t el );
 
alf_orb_data_t Goals2DeltaElem( alf_orb_data_t el0, geom_t goals );
 
geom_t DeltaElem2Goals( alf_orb_data_t el0, alf_orb_data_t dEl );
 
ml_matrix DeltaElem2Hills( alf_orb_data_t elA, alf_orb_data_t dEl );
 
alf_orb_data_s Hills2DeltaElem( alf_orb_data_t el0, const ml_matrix& xH );
 
geom_s Hills2Goals( alf_orb_data_t el0, const ml_matrix& xH );
 
ml_matrix Goals2Hills( alf_orb_data_t el0, geom_t goals );
 
orb_data_s FFEccHills2DeltaElem( orb_data_t el0, const ml_matrix& xH );
 
alf_orb_data_s FFEccHills2DeltaElem( alf_orb_data_t el0, const ml_matrix& xH );
 
ml_matrix FFEccDeltaElem2Hills( alf_orb_data_t el0, alf_orb_data_t dEl );
 
ecc_geom_t FFEccDeltaElem2Goals( alf_orb_data_t el0, alf_orb_data_t dEl );
 
ml_matrix FFEccGoals2Hills( double e, double nu, ecc_geom_t g, double n );
 
ml_matrix FFEccGoals2Hills( double e, double nu, ecc_geom_xy_t g, double n );
 
ml_matrix FFEccGoals2Hills( double e, double nu, ecc_geom_t g );
 
ecc_geom_t FFEccHills2Goals( double e, double nu, ml_matrix xH, double n );
 
ecc_geom_t FFEccHills2Goals( double e, double nu, const ml_matrix& xH );
 
void Nu2TimeDomain( ml_matrix& x, double n, double e, double nu );
 
void Time2NuDomain( ml_matrix& x, double n, double e, double nu );
 
ecc_geom_t GeometryCirc2Ecc( double w, geom_t gCirc );
 
geom_t GeometryEcc2Circ( double w, ecc_geom_t gEcc );
 
geom_t RotateState( geom_t goals, double phi );
 
double MeanAnom2TrueLat( double e, double w, double M );
 
void GetHillsMats( const ml_matrix& r0, const ml_matrix& v0, ml_matrix& A, ml_matrix& Adot );
 
void AbsRelECI2Hills( ml_matrix r0, ml_matrix v0, ml_matrix dr, ml_matrix dv, ml_matrix& rH, ml_matrix& vH );
 
alf_orb_data_t Osc2Mean( alf_orb_data_t el, double J2 );
 
alf_orb_data_t *ECI2MeanElements( const ml_matrix& xRefECI, const ml_matrix& xRelECI, double J2, alf_orb_data_t& elRefMean );
 
bool AlignThruster( const ml_matrix& aH, const ml_matrix& bTh, ml_matrix& qHB );
 
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 );
 
ml_matrix Hills2Frenet( const ml_matrix &xH, const double &e, const double &nu, const double &n );
    
#if 0
#pragma mark Propagation 
#endif
ml_matrix HillsEqns( const ml_matrix& xH0, double n, double t );
 
ml_matrix FFEccHillsEqns( ml_matrix xH0, double nu0, const ml_matrix& nu, double e, double n );
 
ml_matrix FFEccHillsEqns( const ml_matrix& xH0, double nu0, const ml_matrix& nu, double e );
 
ml_matrix DiscreteHills( const ml_matrix& x0, double n, const ml_matrix& aC, double dT );
 
ml_matrix FFEccDiscreteHills( double e, double n, const ml_matrix& x0, double nu0, const ml_matrix& aC, const ml_matrix& t );
#if 0
#pragma mark Collision Monitoring 
#endif
double CollProbSet( double sigma, const ml_matrix& S, const ml_matrix& xc, const ml_matrix& Ssc, int n);
 
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 );
 
ml_matrix CollisionSurvey( ml_matrix& y, double t0, maneuver_t mvr1, maneuver_t mvr2[],  int n_points, coll_mon_data_s d );
 
ml_matrix DeltaElem2HillsMat( const ml_matrix& elA );
 
ml_matrix DeltaEl2AlfriendMat( orb_data_t el );
 
double distant_pt_ell( const ml_matrix& S, const ml_matrix& U, const ml_matrix& xc, const ml_matrix& x0, ml_matrix& x );
 
double Laguerre( const ml_matrix& a, double x_guess );
 
ml_matrix GenerateTimeVector(orb_data_t d,const double tF, const int nPts, ml_matrix& M,ml_matrix& nu);
 
ml_matrix ManeuverStruct2AccelVector( maneuver_t mvr, const ml_matrix& tProp );
 
#if 0
#pragma mark Utility 
#endif
bool TeamLevels( team_t teams[], int num, ml_int_array& levels );
 
int IsRelative( int iD, team_t teams[], int num );
 
bool FindUpperTeams( int kBot, team_t teams[], int num, ml_int_array& kUpper);
 
double TimeUntilTheta( double a, double w, double e, double theta0, double& theta1 );
 
void AccelVector2ManeuverStruct( const ml_matrix& aC, ml_matrix t, double tRef, double nomAccel, double minSepTime, maneuver_t& mvr, ml_matrix& dV );
 
void ManeuverStruct2AccelVector( maneuver_t mvr, double dT, ml_matrix& aC, ml_matrix& t );
 
ml_matrix NOrbVector( window_t window );
 
ml_matrix TargetTrueAnom( double e, double nu0, ml_matrix nOrb );
 
ml_matrix Tetrahedron( double d, const ml_matrix& eul );
 
double vsum( const ml_matrix& a );
 
typedef double (*nr_function)(double x, double e, const ml_matrix& D, double dH);
 
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);
 
char *matout( ml_matrix mat );
 
}
#endif