Calculating orbits

Once you have acquired a GPS Space Vehicle, you are than faced with the daunting task of calculating where it is at any given instant.

NAV frames 2 and 3 provide information about the orbit, and NAV frame 1 provides information about the time.

Using the following untested code it is possible to print out the Earth Centered, Earth Fixed location of the Space Vehicle at any given time.

I haven’t verified it yet in a working design, but plotting the output makes it look correct. I’ll revisit this code later after my design is working.

/************************************************
* orbit.c - calculate a Space Vehicle's position 
* Last changed at 2016-09-24 
************************************************/
#include <stdio.h>
#include <math.h>

#define PI 3.1415926535898

double eccAnom(double ec, double mean_anomaly, double dp) {
 int iterations = 30;
 double delta=pow(10,-dp);
 double estimate, correction;

/* First estimate */
 estimate = (ec<0.8) ? mean_anomaly : PI;
 correction = estimate - ec*sin(mean_anomaly) - mean_anomaly;
 
 /* Solve iteratively */
 while ((abs(correction)>delta) && iterations > 0) {
 estimate -= correction/(1.0-ec*cos(estimate));
 correction = estimate - ec*sin(estimate) - mean_anomaly;
 iterations--;
 }
 return estimate;
}

int orbit_calc_position(
  double t_now,
  /* Orbital parameters */
  double sqrt_A,
  double e,
  double w,
  /* Position at ephemeris */
  double time_of_ephemeris,
  double mean_motion_at_ephemeris,
  double omega_at_ephemeris,
  double inclination_at_ephemeris,
 
 /* Corrections */
  double delta_mean_motion,
  double Cus, double Cuc,
  double Cis, double Cic,
  double Crs, double Crc,
  double IDOT,
  double omegaDot,

  /* Outputs */
  double *ecef_x, double *ecef_y, double *ecef_z)
{
 const double mu = 3.986005e14; /* Earth's universal gravitation parameter */
 const double omegaDot_e = 7.2921151467e-5; /* Earth's rotation rate */


 double semi_major_axis;
 double computed_mean_motion;
 double time_from_ephemeris;
 double correct_mean_motion;
 double mean_anomaly;
 double ek;
 double true_anomaly;
 double argument_of_latitude;
 double argument_of_latitude_correction;
 double radius_correction;
 double correction_of_inclination;
 double corrected_argument_of_latitude;
 double corrected_radius;
 double corrected_inclination;
 double pos_in_orbital_plane_x;
 double pos_in_orbital_plane_y;
 double corrected_angle_of_ascending_node;

/***********************
 * Calculate orbit
 ***********************/
 semi_major_axis = sqrt_A * sqrt_A;
 computed_mean_motion = sqrt(mu / pow(semi_major_axis,3.0));
 time_from_ephemeris = t_now - time_of_ephemeris;
 correct_mean_motion = computed_mean_motion + delta_mean_motion; 
 /* Add on how much we have moved through the orbit since ephemeris */
 mean_anomaly = mean_motion_at_ephemeris + correct_mean_motion * time_from_ephemeris;

 /* solve for eccentric anomaly */
 ek = eccAnom(e,mean_anomaly,10);
 /* Now calculate the first approximation of the latitude */
 argument_of_latitude = w + ek;

/*****************************************
 * Second Harmonic Perbturbations 
 *****************************************/
 argument_of_latitude_correction = Cus * sin(2*argument_of_latitude) + Cuc * cos(2*argument_of_latitude);
 corrected_argument_of_latitude = argument_of_latitude+argument_of_latitude_correction;

 radius_correction = Crc * cos(2*argument_of_latitude) + Crs * sin(2*argument_of_latitude);
 corrected_radius = semi_major_axis * (1- e * cos(ek)) + radius_correction;
 
 correction_of_inclination = Cic * cos(2*argument_of_latitude) + Cic * sin(2*argument_of_latitude);
 corrected_inclination = inclination_at_ephemeris + correction_of_inclination + IDOT*time_from_ephemeris;

 pos_in_orbital_plane_x = corrected_radius * cos(corrected_argument_of_latitude);
 pos_in_orbital_plane_y = corrected_radius * sin(corrected_argument_of_latitude);


corrected_angle_of_ascending_node = omega_at_ephemeris + (omegaDot - omegaDot_e)*time_from_ephemeris - omegaDot_e * time_of_ephemeris;

/******************************************************
 * Project into Earth Centered, Earth Fixed coordinates
 ******************************************************/
 *ecef_x = pos_in_orbital_plane_x * cos(corrected_angle_of_ascending_node)
         - pos_in_orbital_plane_y *cos(corrected_inclination) * sin(corrected_angle_of_ascending_node);
 *ecef_y = pos_in_orbital_plane_x * sin(corrected_angle_of_ascending_node) 
         + pos_in_orbital_plane_y *cos(corrected_inclination) * cos(corrected_angle_of_ascending_node);
 *ecef_z = pos_in_orbital_plane_y * sin(corrected_inclination);

#if 0
 printf("Semi major axis                   %20.10f\n", semi_major_axis);
 printf("Computed mean motion              %20.10f\n", computed_mean_motion);
 printf("Time from ephemeris               %20.10f\n", time_from_ephemeris);
 printf("Corrected mean_motion             %20.10f\n", correct_mean_motion);
 printf("mean_anomaly                      %20.10f\n", mean_anomaly);
 printf("ek                                %20.10f\n", ek);
 printf("true anomaly                      %20.10f\n", true_anomaly); 
 printf("argument of latitude              %20.10f\n", argument_of_latitude); 
 printf("corrected_argument_of_latitude    %20.10f\n", corrected_argument_of_latitude);
 printf("corrected_radius                  %20.10f\n", corrected_radius);
 printf("correction_of_inclination         %20.10f\n", correction_of_inclination);
 printf("pos_in_orbital_plane_x            %20.10f\n", pos_in_orbital_plane_x);
 printf("pos_in_orbital_plane_y            %20.10f\n", pos_in_orbital_plane_y);
 printf("corrected_angle_of_ascending_node %20.10f\n",corrected_angle_of_ascending_node);
#endif
#if 0
 /* Checks that nothing is amiss */
 printf("%12.2f ", sqrt(pos_in_orbital_plane_x*pos_in_orbital_plane_x+pos_in_orbital_plane_y*pos_in_orbital_plane_y));
 printf("%12.2f\n", sqrt((*ecef_x)*(*ecef_x)+(*ecef_y)*(*ecef_y)+(*ecef_z)*(*ecef_z)));
#endif
}

int main(int c, char *v[]) {
 double t_now = 7241; /* zero time */

/* Orbital parameters, transmitted by satellite */
 double sqrt_A = 5153.800608;
 double time_of_ephemeris = 7241;
 double delta_n = 4.279821129e-009;
 double M0 = -2.600551;
 double e = 0.030228948;
 double w = -1.048966714;
 double Cuc = -0.000020975247; 
 double Cus = 0.000004233792;
 double Cic = -0.000000014901;
 double Cis = 0.000000130385;
 double Crc = 305.562500;
 double Crs = 3.812500;
 double i0 = 0.978481871;
 double IDOT = 1.689356e-010;
 double omega_0 = -0.494186143;
 double omegaDot = 0.000005984;

/* Outputs */
 double ecef_x, ecef_y, ecef_z;

 /* Print out position 24 minutes apart */
 for(t_now == 7241; t_now < 7241+60*60*24*3; t_now += 60*24) {
   orbit_calc_position(
     t_now,
     /* orbital parameters */
     sqrt_A, e, w, 
     /* Time and position at ephemeris */
     time_of_ephemeris, M0, omega_0, i0,
     /* correction factors */
     delta_n, Cus, Cuc, Cis, Cic, Crs, Crc,
     IDOT, omegaDot,
     /* Outputs */ 
     &ecef_x, &ecef_y, &ecef_z);

   printf("ECEF at %10.1f = ( %12.2f, %12.2f, %12.2f )\n", t_now, ecef_x, ecef_y, ecef_z);
 }
return 0;
}
Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s