OrbAnalysis9.m

%ORBIT ANALYZER
%Violet Attitude Control Subsystem
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Converts the State Vectors -
%1. Position Vector [Rx Ry Rz]
%2. Velocity Vector [Vx Vy Vz]
% into Kepler's Orbital Elements -
%1. Length of Semi-major Axis (a)
%2. Eccentricity (e)
%3. Inclination (inc)
%4. Right Ascension of Ascending Node (RAAN)
%5. Argument of Perigee (w)
%6. Initial Mean Anomaly (M0)
% after which it visually displays the orbit of the diagram in 3D space.
% The Blue Sphere denotes the Earth
% The Green Sphere denotes the initial position of the satellite
% The Red Spheres denote the positions of the satellites at fixed time
% intervals
% The dotted green line denotes the initial velocity vector of the
% satellite
% The three orthogonal black lines denote the 3 positive ECI axes
% 
% At the end, the altitude ranges are displayed, as well as the orbit type
% - LEO or MEO
% The simulation only takes into account the the influence of the Earth
%CODED BY DEBARGHYA DAS


%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%CUSTOMIZABLE STATE VECTORS%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%SAMPLE VALUES:
%MOLNIYA ORBIT
%r= [0 -7000 -14000];
%v= [2 1 -0.5];
%vmag= 6175;
%runspeed= 300;
%GEOSTATIONARY ORBIT
%r= [42241 0 0]; 
%v= [0 1 0];
%vmag= 3072;
%runspeed=1000;
%POLAR ORBIT COVERING ALL LONGITUDES IN ONE REV
%r= [42241 0 0];
%v= [0 0 1];
%vmag=3072;
%runspeed=1000;
%LOW ECCENTRICITY LOW EARTH ORBIT
%r=[7000 0 0];
%v=[0 1 1];
%vmag=7500;
%runspeed=100;
%NORMAL ORBIT
%r= [7000 0 0]; 
%v= [0 1 0.5];
%vmag=7500;
%runspeed=100;

%INITIAL POSITION VECTOR (in km)
r= [0 -7000 -14000];
%INITIAL VELOCITY DIRECTION VECTOR
v= [2 1 -0.5];
%INITIAL VELOCITY MAGNITUDE (in m/s)
vmag=6175;
v= vmag*(v/sqrt(dot(v, v))); %Velocity Vector
%RUN SPEED OF SIMULATION (Increasing this value increases the time interval
%between the plotting of the satellite)
runspeed=300;
%NUMBER OF REVOLUTIONS 
rev_no=5;
%Number of Days since Jan 1, 2000
J2000_days=103752/24; % = 4321 on 30th October, 2011 http://www.timeanddate.com/counters/year2000.html


% Constant parameters
mu = 398.6004418e12;  % Planetary gravitational constant for Earth, (mu = GMearth) (m^3/s^2)
earth_rad=6371000; % in m



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%CONVERTING STATE VECTORS INTO ORBITAL ELEMENTS%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

V=v;
r=r*1000; %Position vector conversion to meters

rmag = sqrt(dot(r, r)); %Position Magnitude
vmag = sqrt(dot(v, v)); %Velocity Magnitude

rhat = r/rmag; %Position Unit Vector
vhat = v/vmag; %Velocity Unit Vector

hv = cross(r, v); %Angular Momentum Vector
hmag = sqrt(dot(hv, hv)); %Angular Momentum Magnitude
hhat = hv/hmag; %Angular Momentum Unit Vector

%Eccentricity Vector
vtmp = v / mu;
ecc = cross(vtmp, hv);
ecc = ecc - rhat;

%SEMIMAJOR AXIS (a)
a = 1 / (2 / rmag - vmag * vmag / mu);

p = hhat(1) / (1 + hhat(3));
q = -hhat(2) / (1 + hhat(3));
const1 = 1 / (1 + p * p + q * q);
fhat(1) = const1 * (1 - p * p + q * q);
fhat(2) = const1 * 2 * p * q;
fhat(3) = -const1 * 2 * p;
ghat(1) = const1 * 2 * p * q;
ghat(2) = const1 * (1 + p * p - q * q);
ghat(3) = const1 * 2 * q;
h = dot(ecc, ghat);
xk = dot(ecc, fhat);
x1 = dot(r, fhat);
y1 = dot(r, ghat);

%ECCENTRICITY (e) %0<e<1
e = sqrt(h * h + xk * xk)

%INCLINATION (inc) %in rad
inc = 2 * atan(sqrt(p * p + q * q));

xlambdat = atan3(y1,x1);

%RIGHT ASCENSION OF ASCENDING NODE (RAAN) %in rad
if (inc > 0.00000001)
    RAAN = atan3(p,q);
else
   RAAN = 0;
end

%ARGUMENT OF PERIGEE (w) %in rad
if (e > 0.00000001)
   w = atan3(h,xk)-RAAN; 
else
   w = 0;
end

%True Anomaly %in rad
v = xlambdat - RAAN - w;

%MEAN ANOMALY (M0)
M0 = 2*atan(sqrt((1-e)/(1+e))*tan(v/2)) - e*sqrt(1-e^2)*sin(v)/(1+e*cos(v)); %in rad



%ERROR if Launch position is inside the Earth
if (sqrt (r(1)*r(1)+r(2)*r(2)+r(3)*r(3)) <= 6731000)
    blast (r(1), r(2), r(3), 2000000);
    ErrorMsg='Launch Position Inside Earth'
    return;
end


%Final Adjustments to RAAN
RAAN=pi/2-RAAN;
if RAAN < 0
    RAAN = RAAN + 2*pi;
end


%Final Adjustments to Argument of Perigee
w=2*pi-w;



%Final Adjustments to Initial Mean Anomaly
if M0<0
    M0=-M0;
end
    E=M0;
    for i=1:5
        E = E + (M0 + e*sin(E) - E)/(1 - e*cos(E));
    end
    v= 2*atan(sqrt((1+e)/(1-e))*tan(E/2));
    R = a*(1-e*cos(E));
    Xeci = R*(cos(w + v)*cos(RAAN) - sin(w+v)*sin(RAAN)*cos(inc));
    Yeci = R*(cos(w + v)*sin(RAAN) + sin(w+v)*cos(RAAN)*cos(inc));
    Zeci = R*(sin(w + v)*sin(inc));
    c=0;
    while abs(r(1)-Xeci)>100 && abs(r(2)-Yeci)>100 && abs(r(3)-Zeci)>100 && c<15
        if c~=0
            if (c<3)
                M0=2*pi-M0;
            end
            if (c<5 && c>=3)
                M0=-M0;
            end
            if (c<10 && c>=5)
                M0=M0+(pi/2);
                if (c==9)
                    M0=iniM0;
                end
            end
            if (c>=10 && c<15)
                M0=M0-(pi/2);
                if (c==15)
                    M0=iniM0;
                end
            end 
        end
        E=M0;
        for i=1:5
            E = E + (M0 + e*sin(E) - E)/(1 - e*cos(E));
        end
        v= 2*atan(sqrt((1+e)/(1-e))*tan(E/2));
        R = a*(1-e*cos(E));
        b  = a*sqrt(1-e^2);
        Xeci = R*(cos(w + v)*cos(RAAN) - sin(w+v)*sin(RAAN)*cos(inc));
        Yeci = R*(cos(w + v)*sin(RAAN) + sin(w+v)*cos(RAAN)*cos(inc));
        Zeci = R*(sin(w + v)*sin(inc));
        c=c+1;
end
if (M0>2*pi)
    M0=M0-2*pi;
end
if (M0<0)
    M0=M0+2*pi;     
end
  
%Orbital Period 
orbital_period = sqrt((a*a*a*4*pi*pi)/mu)
%DISPLAYING ORBITAL ELEMENTS
a_km= a/1000 %Semi-major axis (in m)
b_km  = a*sqrt(1-e^2)/1000
e %Eccentricity
inc_deg= inc*180/pi %Inclination (in degrees)
RAAN_deg= RAAN*180/pi %Right Ascension of Ascending Node (in degrees)
w_deg= w*180/pi %Argument of Perigee (in degrees)
M0_deg= M0*180/pi %Mean Anomaly (in degrees)

%ERROR if eccentricity is greater than supported
if (e<0)
    if (e>0.95)
        Errormsg='Eccentricity greater than supported'
    end
    if (e>1)
        Errormsg='Eccentricity invalid'
    end
   return;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%DRAWING THE STATIC VISUALIZATION COMPONENTS%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Initializing the Drawing Space
close all
set(gcf,'Menubar','default','Name','Orbit Visualization', ... 
    'NumberTitle','off','Position',[10,350,750,750], ... 
    'Color',[0.38 0.26 0.67]); 
lim=(1+e)*a;%Setting the limits of the graph
clf
axis([-lim, lim, -lim, lim, -lim, lim])	
view(150,15) 
axis equal
shg
hold on
grid on
title('Orbital Visualization');

%Plotting the Earth
equat_rad=6378137.00;
    polar_rad=6356752.3142;
    [xx yy zz]=ellipsoid (0,0,0,equat_rad, equat_rad, polar_rad);
    load('topo.mat','topo','topomap1');
    topo2 = [topo(:,181:360) topo(:,1:180)];
    pro.FaceColor= 'texture';
    pro.EdgeColor = 'none';
    pro.FaceLighting = 'phong';
    pro.Cdata = topo2;
   earth= surface(xx,yy,zz,pro)
    colormap(topomap1)
omega_earth = 7.292115855377074e-005; % (rad/sec)  
Go = 1.727564365843028; % (rad) http://www.amsat.org/amsat/articles/g3ruh/106.html 
GMST = Go + omega_earth*86400*(J2000_days + 0.5);
GMST = GMST - 2*pi*floor(GMST/(2*pi));
GMST_deg=GMST*(180/pi)
rotate (earth, [0 0 1], GMST_deg);
Xaxis= line([0 lim],[0 0],[0 0],'Color', 'red', 'Marker','.','LineStyle','-');
Yaxis= line([0 0],[0 lim],[0 0],'Color', 'red', 'Marker','.','LineStyle','-');
rotate (Xaxis, [0 0 1], GMST_deg);
rotate (Yaxis, [0 0 1], GMST_deg);
%Sun=light('Position',[1 0 0],'Style','infinite');


%Plotting the ECI Axes
line([0 lim],[0 0],[0 0],'Color', 'black', 'Marker','.','LineStyle','-')
line([0 0],[0 lim],[0 0],'Color', 'black', 'Marker','.','LineStyle','-')
line([0 0],[0 0],[0 lim],'Color', 'black', 'Marker','.','LineStyle','-')

%Plotting Initial Velocity Vector
line([r(1) r(1)+1000*V(1)],[r(2) r(2)+1000*V(2)],[r(3) r(3)+1000*V(3)],'Color', 'green','Marker','.','LineWidth', 2, 'MarkerSize', 8,'LineStyle','-');

%Plotting the initial poisition of the satellite
plot3 (r(1), r(2), r(3),'o', 'MarkerEdgeColor', 'black','MarkerFaceColor','green','MarkerSize', 10);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%DRAWING THE DYNAMIC VISUALIZATION COMPONENTS%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k=0;
long=1:rev_no*ceil(orbital_period/runspeed);
%Plotting the movement of the satellite
Xcoord(1)=r(1);
Ycoord(1)=r(2);
Zcoord(1)=r(3);
%Sun=light('Position',[1 0 0],'Style','infinite', 'Visible', 'on');

for time = 1:rev_no*ceil(orbital_period/runspeed)
    
    k=k+1;
    %Computing Eccentric Anomaly
    E=M0;
    for i=1:5
        E = E + (M0 + e*sin(E) - E)/(1 - e*cos(E));
    end
   
    %Computing the True Anomaly
    v= 2*atan(sqrt((1+e)/(1-e))*tan(E/2));
    
    %Computing 'r' in polar coordinates
    r = a*(1-e*cos(E));
    GMST=GMST+(runspeed/86400)*2*pi;
    %Computes the Cartesian Co-ordinates in ECI frame from 'r' and orbital
    %elements
    Xeci = r*(cos(w + v)*cos(RAAN) - sin(w+v)*sin(RAAN)*cos(inc));
    Yeci = r*(cos(w + v)*sin(RAAN) + sin(w+v)*cos(RAAN)*cos(inc));
    Zeci = r*(sin(w + v)*sin(inc));
    rotate (earth, [0 0 1], (runspeed)*(360/86400))
    
    rotate (Xaxis, [0 0 1], (runspeed)*(360/86400))
    rotate (Yaxis, [0 0 1], (runspeed)*(360/86400))

     
    
    %Drawing the red sphere
    array(k)=plot3 (Xeci, Yeci, Zeci,'o', 'MarkerEdgeColor', 'k','MarkerFaceColor','r','MarkerSize', 6);
    position(k)=line([0 Xeci],[0 Yeci], [0 Zeci],'Color', 'yellow', 'LineWidth', 2);
    
    if (k~=1)
    set (array(k-1), 'Visible', 'off');
    set (position(k-1), 'Visible', 'off');
    end
    if (time~=1 && time<=ceil(orbital_period/runspeed)+1)
        Xcoord(k)=Xeci;
        Ycoord(k)=Yeci;
        Zcoord(k)=Zeci;
        line([Xcoord(k-1) Xcoord(k)],[Ycoord(k-1) Ycoord(k)], [Zcoord(k-1) Zcoord(k)],'Color', 'black', 'LineWidth', 2);
    end
    
    
  
    if (GMST>2*pi)
        GMST=GMST-2*pi
    end
    
    lat(k)=atan(Zeci/sqrt(Xeci*Xeci+Yeci*Yeci))*(180/pi);
    ECIX=[cos(GMST) sin(GMST) 0];
    Pos=[Xeci Yeci 0];
    %Posmag = sqrt(dot(Pos, Pos));
    cvec = cross(ECIX,Pos);
    angleyz = mod(sign(dot([0 0 1],cvec))*atan2(norm(cvec),dot(ECIX,Pos)),2*pi);
    long(k) =(180/pi)* angleyz;
    
        
    %Pause
    pause (0.01);
    
    %Blast condition
    if (sqrt (Xeci*Xeci+Yeci*Yeci+Zeci*Zeci) <= 6731000)
        blast (Xeci, Yeci, Zeci, 2000000);
        ErrorMsg='Blast Effected'
        break;
    end
    
    M0=M0+sqrt(mu/(a*a*a))*runspeed; %Updating Mean Anomaly for next iteration
        
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%DISPLAYING ALTITUDE RANGE AND ORBIT TYPE%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

perigee=a*(1-e);
apogee=a*(1+e);
altitude_low=(perigee-earth_rad)/1000
altitude_high=(apogee-earth_rad)/1000
if altitude_low>200 && altitude_high<2000
    'Low Earth Orbit'
end
if altitude_low>2000 && altitude_high<35786
    'Middle Earth Orbit'
end
figure (2);
set(gcf,'Menubar','none','Name','Earth Track', ... 
    'NumberTitle','off','Position',[10,350,1000,500], ... 
    'Color',[0.4 0.3 0.7]); 
hold on
image([0 360],[-90 90],topo,'CDataMapping', 'scaled');
colormap(topomap1);
axis equal
axis ([0 360 -90 90]);
plot (167.717,8.717,'o', 'MarkerEdgeColor', 'k','MarkerFaceColor','y','MarkerSize', 10);
plot (360-76.496, 42.440,'o', 'MarkerEdgeColor', 'k','MarkerFaceColor','y','MarkerSize', 10);
for i=1:k
    plot (long(i),lat(i),'o', 'MarkerEdgeColor', 'k','MarkerFaceColor','r','MarkerSize', 6);
    if (i~=1 && abs(long(i-1)-long(i))<100)
        line([long(i-1) long(i)],[lat(i-1) lat(i)],'Color', 'red', 'LineWidth', 2);
    end
    
    pause (0.001);
end