% Attitude Dynamics Equations, Torque Free.
% Darren Pais
% October 12, 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=25;
% Convert to seconds
t_max=t_mts*60;
%---------------------

%Initialize Data
wx=wx_o;wy=wy_o;wz=wz_o;t=0;
phi=phi_o; theta=theta_o; psi=psi_o;
data(1,:)=[wx wy wz phi theta psi 0] ;

%numerically intgrate to get angular velocities at each second (1 sec step)
    
    for i=1:1:t_max
        wx=data(i,1); wy=data(i,2); wz=data(i,3);
        phi=data(i,4); theta=data(i,5); psi=data(i,6);
        
        %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]=get_angles(w1,w2,w3,phi,theta,psi,i);
        
        phi=Y(end,1); theta=Y(end,2); psi=Y(end,3);
        
        %numerically intgrate to get angular velocity values at next time step
        
        [t,Y] = ode45(@ang_vels,[i-1 i],[data(i,1) data(i,2) data(i,3)]);
        wx=Y(end,1) ; wy=Y(end,2) ; wz=Y(end,3) ; t=i ;
        
        %store data at time step
        data(i+1,:)=[wx wy wz phi theta psi t] ;
    end


% Euler Angles Definition System ODE
    function [t,Y]=get_angles(w1,w2,w3,phi,theta,psi,i)
        [t,Y] = ode45(@euler_angles,[i-1 i],[phi theta 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))/cos(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
    end


% Angular Velocities Definition System ODE
    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

% Plotting Commands
subplot 221
plot(data(:,7)/60,data(:,4)*180/pi) ;

subplot 222
plot(data(:,7)/60,data(:,5)*180/pi) ;

subplot 223
plot(data(:,7)/60,data(:,6)*180/pi) ;
end