diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..5806ef1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,8 @@ +*_rtw/ +slprj/ +*.zip +*.dll +*.lib +*.exp +*.slxc +*.asv diff --git a/EulToQuat.m b/EulToQuat.m new file mode 100644 index 0000000..762c9cc --- /dev/null +++ b/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; + + diff --git a/NormQuat.m b/NormQuat.m new file mode 100644 index 0000000..7d1913e --- /dev/null +++ b/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; diff --git a/Quat2Tbn.m b/Quat2Tbn.m new file mode 100644 index 0000000..663d19c --- /dev/null +++ b/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]; \ No newline at end of file diff --git a/QuatToEul.m b/QuatToEul.m new file mode 100644 index 0000000..abd029b --- /dev/null +++ b/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)); \ No newline at end of file diff --git a/ins.sldd b/ins.sldd new file mode 100644 index 0000000..4409166 Binary files /dev/null and b/ins.sldd differ diff --git a/ins.slx b/ins.slx new file mode 100644 index 0000000..f7bd558 Binary files /dev/null and b/ins.slx differ diff --git a/quat_to_euler_error_transfer_matrix.m b/quat_to_euler_error_transfer_matrix.m new file mode 100644 index 0000000..ffcb87d --- /dev/null +++ b/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]);