%symbolic calculation for angular velocity with Euler Angles 313
close all
clc
clear all

R1_i=sym('R1_i','real');
R3_Om=sym('R3_Om','real'); 
R3_om=sym('R3_om','real'); 
inclination=sym('inclination','real'); 
RAAN=sym('RAAN','real'); 
arg_per=sym('arg_per','real');
RAAN_dot=sym('RAAN_dot','real'); 
inclination_dot=sym('inclination_dot','real');
arg_per_dot=sym('arg_per_dot','real');
A_om_B=sym('A_om_B','real');
om_matrix=sym('om_matrix','real');
det_om_matrix=sym('det_om_matrix','real');

R1_i = [1 0 0; 0 cos(inclination) sin(inclination); 0 -sin(inclination) cos(inclination)];
R3_Om = [cos(RAAN) sin(RAAN) 0; -sin(RAAN) cos(RAAN) 0; 0 0 1];
R3_om = [cos(arg_per) sin(arg_per) 0; -sin(arg_per) cos(arg_per) 0; 0 0 1];

prime_basis = inv(R1_i)*inv(R3_om);
prime_basis = simplify(prime_basis);

A_om_B = RAAN_dot*prime_basis(3,1:3)' + inclination_dot*prime_basis(1,1:3)' + arg_per_dot*[0;0;1];
A_om_B = simplify(A_om_B)

om_matrix = [sin(arg_per)*sin(inclination)  cos(arg_per) 0;
             cos(arg_per)*sin(inclination) -sin(arg_per) 0;
             cos(inclination)                0           1]
         
simplify(inv(om_matrix))
det_om_matrix = simplify(det(om_matrix))
