From 26b5f2689149c73124f95e824ad5757b1720ec59 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 21 Jul 2017 10:10:03 +1000
Subject: [PATCH] matlab: Add scripts to calculate rotation conversions for 321
 or 312 Euler sequences

---
 matlab/scripts/Inertial Nav EKF/Tbn_312.c     | 15 ++++++++++
 matlab/scripts/Inertial Nav EKF/Tbn_321.c     | 15 ++++++++++
 matlab/scripts/Inertial Nav EKF/Tbn_quat.c    | 17 +++++++++++
 matlab/scripts/Inertial Nav EKF/quat2yaw312.m | 29 +++++++++++++++++++
 matlab/scripts/Inertial Nav EKF/quat2yaw321.m | 29 +++++++++++++++++++
 .../scripts/Inertial Nav EKF/yaw_input_312.c  |  2 ++
 .../scripts/Inertial Nav EKF/yaw_input_321.c  |  2 ++
 7 files changed, 109 insertions(+)
 create mode 100644 matlab/scripts/Inertial Nav EKF/Tbn_312.c
 create mode 100644 matlab/scripts/Inertial Nav EKF/Tbn_321.c
 create mode 100644 matlab/scripts/Inertial Nav EKF/Tbn_quat.c
 create mode 100644 matlab/scripts/Inertial Nav EKF/quat2yaw312.m
 create mode 100644 matlab/scripts/Inertial Nav EKF/quat2yaw321.m
 create mode 100644 matlab/scripts/Inertial Nav EKF/yaw_input_312.c
 create mode 100644 matlab/scripts/Inertial Nav EKF/yaw_input_321.c

diff --git a/matlab/scripts/Inertial Nav EKF/Tbn_312.c b/matlab/scripts/Inertial Nav EKF/Tbn_312.c
new file mode 100644
index 0000000000..d19f7b1b6a
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/Tbn_312.c	
@@ -0,0 +1,15 @@
+t2 = sin(yaw);
+t3 = cos(yaw);
+t4 = sin(pitch);
+t5 = cos(pitch);
+t6 = sin(roll);
+t7 = cos(roll);
+A0[0][0] = t3*t5-t2*t4*t6;
+A0[0][1] = -t2*t7;
+A0[0][2] = t3*t4+t2*t5*t6;
+A0[1][0] = t2*t5+t3*t4*t6;
+A0[1][1] = t3*t7;
+A0[1][2] = t2*t4-t3*t5*t6;
+A0[2][0] = -t4*t7;
+A0[2][1] = t6;
+A0[2][2] = t5*t7;
diff --git a/matlab/scripts/Inertial Nav EKF/Tbn_321.c b/matlab/scripts/Inertial Nav EKF/Tbn_321.c
new file mode 100644
index 0000000000..ca56049298
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/Tbn_321.c	
@@ -0,0 +1,15 @@
+t2 = cos(yaw);
+t3 = sin(roll);
+t4 = sin(yaw);
+t5 = cos(roll);
+t6 = sin(pitch);
+t7 = cos(pitch);
+A0[0][0] = t2*t7;
+A0[0][1] = -t4*t5+t2*t3*t6;
+A0[0][2] = t3*t4+t2*t5*t6;
+A0[1][0] = t4*t7;
+A0[1][1] = t2*t5+t3*t4*t6;
+A0[1][2] = -t2*t3+t4*t5*t6;
+A0[2][0] = -t6;
+A0[2][1] = t3*t7;
+A0[2][2] = t5*t7;
diff --git a/matlab/scripts/Inertial Nav EKF/Tbn_quat.c b/matlab/scripts/Inertial Nav EKF/Tbn_quat.c
new file mode 100644
index 0000000000..0618dd168f
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/Tbn_quat.c	
@@ -0,0 +1,17 @@
+float t2 = q1*q2*2.0;
+float t3 = q0*q0;
+float t4 = q1*q1;
+float t5 = q2*q2;
+float t6 = q3*q3;
+float t7 = q0*q2*2.0;
+float t8 = q1*q3*2.0;
+float t9 = q2*q3*2.0;
+A0[0][0] = t3+t4-t5-t6;
+A0[0][1] = t2-q0*q3*2.0;
+A0[0][2] = t7+t8;
+A0[1][0] = t2+q0*q3*2.0;
+A0[1][1] = t3-t4+t5-t6;
+A0[1][2] = t9-q0*q1*2.0;
+A0[2][0] = -t7+t8;
+A0[2][1] = t9+q0*q1*2.0;
+A0[2][2] = t3-t4-t5+t6;
diff --git a/matlab/scripts/Inertial Nav EKF/quat2yaw312.m b/matlab/scripts/Inertial Nav EKF/quat2yaw312.m
new file mode 100644
index 0000000000..5a9ef605d8
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/quat2yaw312.m	
@@ -0,0 +1,29 @@
+% define roll, pitch and yaw variables
+syms roll pitch yaw 'real'
+
+% Define yransformaton matrices for rotations about the X,Y and Z body axes
+Xrot = [1 0 0 ; 0 cos(roll) sin(roll) ; 0 -sin(roll) cos(roll)];
+Yrot = [cos(pitch) 0 -sin(pitch) ; 0 1 0 ; sin(pitch) 0 cos(pitch)];
+Zrot = [cos(yaw) sin(yaw) 0 ; -sin(yaw) cos(yaw) 0 ; 0 0 1];
+
+% calculate the tranformation matrix from body to navigation frame resulting from
+% a rotation in yaw, roll, pitch order
+Tbn_312 = transpose(Yrot*Xrot*Zrot)
+
+% convert to c code and save
+ccode(Tbn_312,'file','Tbn_312.c');
+fix_c_code('Tbn_312.c');
+
+% define the quaternion elements
+syms q0 q1 q2 q3 'real'
+
+% calculate the tranformation matrix from body to navigation frame as a
+% function of the quaternions
+Tbn_quat = Quat2Tbn([q0;q1;q2;q3]);
+
+% calculate the y,x terms required for calculation fo the yaw angle
+yaw_input_312 = [-Tbn_quat(1,2);Tbn_quat(2,2)];
+
+% convert to c code and save
+ccode(yaw_input_312,'file','yaw_input_312.c');
+fix_c_code('yaw_input_312.c');
diff --git a/matlab/scripts/Inertial Nav EKF/quat2yaw321.m b/matlab/scripts/Inertial Nav EKF/quat2yaw321.m
new file mode 100644
index 0000000000..ec9752154f
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/quat2yaw321.m	
@@ -0,0 +1,29 @@
+% define roll, pitch and yaw variables
+syms roll pitch yaw 'real'
+
+% Define yransformaton matrices for rotations about the X,Y and Z body axes
+Xrot = [1 0 0 ; 0 cos(roll) sin(roll) ; 0 -sin(roll) cos(roll)];
+Yrot = [cos(pitch) 0 -sin(pitch) ; 0 1 0 ; sin(pitch) 0 cos(pitch)];
+Zrot = [cos(yaw) sin(yaw) 0 ; -sin(yaw) cos(yaw) 0 ; 0 0 1];
+
+% calculate the tranformation matrix from body to navigation frame resulting from
+% a rotation in yaw, pitch, roll order
+Tbn_321 = transpose(Xrot*Yrot*Zrot)
+
+% convert to c code and save
+ccode(Tbn_321,'file','Tbn_321.c');
+fix_c_code('Tbn_321.c');
+
+% define the quaternion elements
+syms q0 q1 q2 q3 'real'
+
+% calculate the tranformation matrix from body to navigation frame as a
+% function of the quaternions
+Tbn_quat = Quat2Tbn([q0;q1;q2;q3]);
+
+% calculate the y,x terms required for calculation fo the yaw angle
+yaw_input_321 = [Tbn_quat(1,2);Tbn_quat(1,1)];
+
+% convert to c code and save
+ccode(yaw_input_321,'file','yaw_input_321.c');
+fix_c_code('yaw_input_321.c');
diff --git a/matlab/scripts/Inertial Nav EKF/yaw_input_312.c b/matlab/scripts/Inertial Nav EKF/yaw_input_312.c
new file mode 100644
index 0000000000..0a8bea9a56
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/yaw_input_312.c	
@@ -0,0 +1,2 @@
+A0[0][0] = q0*q3*2.0-q1*q2*2.0;
+A0[1][0] = q0*q0-q1*q1+q2*q2-q3*q3;
diff --git a/matlab/scripts/Inertial Nav EKF/yaw_input_321.c b/matlab/scripts/Inertial Nav EKF/yaw_input_321.c
new file mode 100644
index 0000000000..093f7ec095
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/yaw_input_321.c	
@@ -0,0 +1,2 @@
+A0[0][0] = q0*q3*-2.0+q1*q2*2.0;
+A0[1][0] = q0*q0+q1*q1-q2*q2-q3*q3;