Maple_CCt_identity.m

syms th psi phi real

% Set up the direction cosine matrix for phi theta and psi ...

C1 = [ 1 0 0 ; 0 cos(phi) sin(phi) ; 0 -sin(phi) cos(phi) ];
C2 = [ cos(th) 0 -sin(th); 0 1 0 ; sin(th) 0 cos(th) ];
C3 = [ cos(psi) sin(psi) 0; -sin(psi) cos(psi) 0; 0 0 1 ];

CBI = C1*C2*C3

% Show that dCBI(:)/dt  = dCBI/dphi*p + dCBI/dth*q + dCBI/psi*r
%                       = -Omega*CBI 
%                       = [Cx1*omega Cx2*omega Cx3*omega]

syms p q r real

omega = [p q r]';
Omega = [0 r -q; -r 0 p; q -p 0];

% Set up the cross product matrices for the DCM x omega ...        
Cx1=[0 -CBI(3,1) CBI(2,1);CBI(3,1) 0 -CBI(1,1); -CBI(2,1) CBI(1,1) 0]
Cx2=[0 -CBI(3,2) CBI(2,2);CBI(3,2) 0 -CBI(1,2); -CBI(2,2) CBI(1,2) 0]
Cx3=[0 -CBI(3,3) CBI(2,3);CBI(3,3) 0 -CBI(1,3); -CBI(2,3) CBI(1,3) 0]

C = [Cx1;Cx2;Cx3]

Cdot1 = Omega*CBI
pause

Cdot2 = [Cx1*omega Cx2*omega Cx3*omega]
pause

simplify(Cdot1-Cdot2)
pause

% Show that C'C=2I ...
simplify(C'*C)

pause

% Show that C'CBI(:) = 0 ...
simplify(C'*CBI(:))

pause


syms th nx ny nz real

% Set up quaternion vector and matrices:
q = [nx*sin(th/2) ny*sin(th/2) nz*sin(th/2) cos(th/2)]'

%  Make n a unit vector in q and shw result:
nx = sqrt(1-ny^2-nz^2);
q = subs(q)

% Verify that norm of q =1:
normq = simplify(q.'*q)

pause


% Generate the  matrix Q and show Q'Q=I

Q   = [q(4) -q(3) q(2); q(3) q(4) -q(1); -q(2) q(1) q(4); -q(1) -q(2) -q(3)];
QtQ = simplify(Q.'*Q)

pause

Qtq = simplify(Q.'*q)

pause