% Attitude Dynamics Equations, Torque Free.
% Darren Pais
% October 10, 2006

%---------------------
% Term Definitions

function [data]=attitude() 
% Mass moments Kg m^2
Ix=.23689;
Iy=.28393;
Iz=.35307;

% Moments about body CG projected in <b1 b2 b3>
M1=0;
M2=0;
M3=0;

% Orbit parameter n for polar circular orbit
alt_km=500 ; a=6378+alt_km;
n=sqrt(3.986*10^5/a^3);

% Initial Conditions

% Angular Velocity vector projected in <b1 b2 b3>
wx_o=0; wy_o=0; wz_o=0;
% Roll Pitch Yaw angles of BRF wrt MRF
phi_o=0; theta_o=0; psi_o=0;

% Simulation Parameters

% Simulations time in mins
t_mts=10;
% Convert to seconds
t_max=t_mts*60;
%---------------------


%numerically intgrate to get angular velocities
        
    [t,Y] = ode45(@ang_vels,[0:1:t_max],[wx_o wy_o wz_o]);

    %angular velocity values
    wx=Y(:,1) ; wy=Y(:,2) ; wz=Y(:,3) ;
    
    
    
%compute angular velocity of BRF wrt MRF
    
    w1=wx + n*sin(phi)*sin(theta)*cos(psi)+n*cos(phi)*sin(psi) ;
    w2=wy - n*sin(phi)*sin(theta)*sin(psi)+n*cos(phi)*cos(psi) ;
    w3=wz-n*sin(phi)*cos(theta) ;
    
    
    %numerically intgrate to get euler angles at next time step
    
    
    [t,Y] = ode45(@euler_angles,[0 1],[phi theta psi]);

    phi=Y(1); theta=Y(2); psi=Y(3);
    data(i+1,1)=phi; data(i+1,2)=theta; data(i+1,3)=psi;
    
    
    
 
    

function dy = euler_angles(t,y)
    dy = zeros(3,1);    % a column vector
    dy(1) = cos(y(3))/cos(y(2))*w1 - sin(y(3))/sin(y(2))*w2;
    dy(2) = sin(y(3))*w1+cos(y(3))*w2;
    dy(3) = -sin(y(2))*cos(y(3))/cos(y(2))*w1 + sin(y(2))*sin(y(3))/cos(y(2))*w2 + w3 ;
    
end

function dy = ang_vels(t,y)
    dy = zeros(3,1);    % a column vector
    dy(1) = 1/Ix* (M1-(Iz-Iy)*y(2)*y(3)) ;
    dy(2) = 1/Ix* (M2-(Ix-Iz)*y(1)*y(3)) ;
    dy(3) = 1/Ix* (M3-(Iy-Ix)*y(1)*y(2)) ;
end

end