init

.gitignore

@@ -0,0 +1,8 @@
+*_rtw/
+slprj/
+*.zip
+*.dll
+*.lib
+*.exp
+*.slxc
+*.asv

EulToQuat.m

@@ -0,0 +1,23 @@
+function quaterion = EulToQuat(Euler)
+
+% Convert from a 321 Euler rotation sequence specified in radians to a
+% Quaternion
+
+quaterion = single(zeros(4,1));
+
+Euler    = Euler * single(0.5);
+cosPhi   = cos(Euler(1));
+sinPhi   = sin(Euler(1));
+cosTheta = cos(Euler(2));
+sinTheta = sin(Euler(2));
+cosPsi   = cos(Euler(3));
+sinPsi   = sin(Euler(3));
+
+quaterion(1,1) = (cosPhi*cosTheta*cosPsi + sinPhi*sinTheta*sinPsi);
+quaterion(2,1) = (sinPhi*cosTheta*cosPsi - cosPhi*sinTheta*sinPsi);
+quaterion(3,1) = (cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi);
+quaterion(4,1) = (cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi);
+
+return;
+
+   

NormQuat.m

@@ -0,0 +1,5 @@
+% normalise the quaternion
+function quaternion = normQuat(quaternion)
+
+quatMag = sqrt(quaternion(1)^2 + quaternion(2)^2 + quaternion(3)^2 + quaternion(4)^2);
+quaternion(1:4) = quaternion / quatMag;

Quat2Tbn.m

@@ -0,0 +1,14 @@
+function Tbn = Quat2Tbn(quat)
+
+% Convert from quaternions defining the flight vehicles rotation to
+% the direction cosine matrix defining the rotation from  body to navigation
+% coordinates
+
+q0 = quat(1);
+q1 = quat(2);
+q2 = quat(3);
+q3 = quat(4);
+  
+Tbn = [q0^2 + q1^2 - q2^2 - q3^2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2); ...
+     2*(q1*q2 + q0*q3), q0^2 - q1^2 + q2^2 - q3^2, 2*(q2*q3 - q0*q1); ...
+     2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2];

QuatToEul.m

@@ -0,0 +1,9 @@
+% Convert from a quaternion to a 321 Euler rotation sequence in radians
+
+function Euler = QuatToEul(quat)
+
+Euler = single(zeros(3,1));
+
+Euler(1) = atan2(2*(quat(3)*quat(4)+quat(1)*quat(2)),  quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3) + quat(4)*quat(4));
+Euler(2) = -asin(2*(quat(2)*quat(4)-quat(1)*quat(3)));
+Euler(3) = atan2(2*(quat(2)*quat(3)+quat(1)*quat(4)),  quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3) - quat(4)*quat(4));

quat_to_euler_error_transfer_matrix.m

@@ -0,0 +1,38 @@
+function error_transfer_matrix = quat_to_euler_error_transfer_matrix(q0,q1,q2,q3)
+%QUAT_TO_EULER_ERROR_TRANSFER_MATRIX
+%    ERROR_TRANSFER_MATRIX = QUAT_TO_EULER_ERROR_TRANSFER_MATRIX(Q0,Q1,Q2,Q3)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    13-Jul-2017 08:41:41
+
+t8 = q0.*q1.*2.0;
+t9 = q2.*q3.*2.0;
+t2 = t8+t9;
+t4 = q0.^2;
+t5 = q1.^2;
+t6 = q2.^2;
+t7 = q3.^2;
+t3 = t4-t5-t6+t7;
+t10 = t3.^2;
+t11 = t2.^2;
+t12 = t10+t11;
+t13 = 1.0./t12;
+t14 = 1.0./t3;
+t15 = 1.0./t3.^2;
+t17 = q0.*q2.*2.0;
+t18 = q1.*q3.*2.0;
+t16 = t17-t18;
+t19 = t16.^2;
+t20 = -t19+1.0;
+t21 = 1.0./sqrt(t20);
+t24 = q0.*q3.*2.0;
+t25 = q1.*q2.*2.0;
+t22 = t24+t25;
+t23 = t4+t5-t6-t7;
+t26 = t23.^2;
+t27 = t22.^2;
+t28 = t26+t27;
+t29 = 1.0./t28;
+t30 = 1.0./t23;
+t31 = 1.0./t23.^2;
+error_transfer_matrix = reshape([t10.*t13.*(q1.*t14.*2.0-q0.*t2.*t15.*2.0),q2.*t21.*2.0,t26.*t29.*(q3.*t30.*2.0-q0.*t22.*t31.*2.0),t10.*t13.*(q0.*t14.*2.0+q1.*t2.*t15.*2.0),q3.*t21.*-2.0,t26.*t29.*(q2.*t30.*2.0-q1.*t22.*t31.*2.0),t10.*t13.*(q3.*t14.*2.0+q2.*t2.*t15.*2.0),q0.*t21.*2.0,t26.*t29.*(q1.*t30.*2.0+q2.*t22.*t31.*2.0),t10.*t13.*(q2.*t14.*2.0-q3.*t2.*t15.*2.0),q1.*t21.*-2.0,t26.*t29.*(q0.*t30.*2.0+q3.*t22.*t31.*2.0)],[3, 4]);