function pdot=mrpevolve(t,p)

% program for calculating the time derivatives of the modified

Rodrigues
% parameters, with a constant angular velocity, w (3x1)
global w
S=[0 -p(3,1) p(2,1);p(3,1) 0 -p(1,1);-p(2,1) p(1,1) 0];
G=0.5*(eye(3)+S+p*p'-0.5*(1+p'*p)*eye(3));
pdot=G*w';
function q=quaternion(C)
T=trace(C);
qsq=[1+2*C(1,1)-T;1+2*C(2,2)-T;1+2*C(3,3)-T;1+T]/4;
[x,i]=max(qsq);
if i==4
q(4)=sqrt(x);
q(1)=(C(2,3)-C(3,2))/(4*q(4));
q(2)=(C(3,1)-C(1,3))/(4*q(4));
q(3)=(C(1,2)-C(2,1))/(4*q(4));
end
if i==3
q(3)=sqrt(x);
q(1)=(C(1,3)+C(3,1))/(4*q(3));
q(2)=(C(3,2)+C(2,3))/(4*q(3));
q(4)=(C(1,2)-C(2,1))/(4*q(3));
end
if i==2
q(2)=sqrt(x);
q(1)=(C(1,2)+C(2,1))/(4*q(2));
q(3)=(C(3,2)+C(2,3))/(4*q(2));
q(4)=(C(3,1)-C(1,3))/(4*q(2));
end
if i==1
q(1)=sqrt(x);
q(2)=(C(1,2)+C(2,1))/(4*q(1));
q(3)=(C(1,3)+C(3,1))/(4*q(1));
q(4)=(C(2,3)-C(3,2))/(4*q(1));
end
function q=quatevolve(q0,w,T)
%function for evolving the quaternion with a constant
%body-referenced angular velocity
%q0=quaternion at t=0 (4x1) vector
%w=angular velocity vector (3x1) (rad/s)
%T=final time (s)
S=[0 w(3,1) -w(2,1) w(1,1);-w(3,1) 0 w(1,1) w(2,1);
w(2,1) -w(1,1) 0 w(3,1);-w(1,1) -w(2,1) -w(3,1) 0];
q=expm(0.5*S*T)*q0;
function qpp=quatrot(q,qp)
% Program for combining two successive rotations, given by
quaternions
% q and qp, to produce the final orientation given by the
quaternion
% qpp. All the quaternions are stored here as row vectors.
qpp=q*[qp(4) qp(3) -qp(2) qp(1);...
-qp(3) qp(4) qp(1) qp(2);...
qp(2) -qp(1) qp(4) qp(3);...
-qp(1) -qp(2) -qp(3) qp(4)]';

function c=rotevolve(c0,w,T)
%function for evolving the rotation matrix with a constant
%body-referenced angular velocity
%c0=rotation matrix at t=0
%w=angular velocity vector (3x1) (rad/s)
%T=final time (s)
S=[0 -w(3,1) w(2,1); w(3,1) 0 -w(1,1); -w(2,1) w(1,1) 0];
dt=2*pi/(10^6*norm(w));
cdt=eye(3)-S*dt;
t=dt;
c=cdt*c0;
while t<=T
c=cdt*c;
t=t+dt;
end
function C=rotmrp(p)
% rotation matrix from the modified Rodrigues parameters
S=[0 -p(3,1) p(2,1);p(3,1) 0 -p(1,1);-p(2,1) p(1,1) 0];
C=eye(3)+4*(p'*p-1)*S/(1+p'*p)^2+8*S*S/(1+p'*p)^2;
function C=rotquat(q)
% rotation matrix from the quaternion
S=[0 -q(3,1) q(2,1);q(3,1) 0 -q(1,1);-q(2,1) q(1,1) 0];
C=(q(4,1)^2-q(1:3,1)'*q(1:3,1))*eye(3)+2*q(1:3,1)*q(1:3,1)'-2*q(4,1)
*S;