From 6ca8a367821903e329cd3cb18f432d8d6aaa9def Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 17 May 2016 10:49:02 +1000
Subject: [PATCH] EKF: Add matlab derivation for calculation of rotation vector
 variance

---
 .../QuatErrTransferEquations.m                | 40 +++++++++++++++++++
 matlab/scripts/Inertial Nav EKF/rotVarVec.c   | 19 +++++++++
 2 files changed, 59 insertions(+)
 create mode 100644 matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m
 create mode 100644 matlab/scripts/Inertial Nav EKF/rotVarVec.c

diff --git a/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m b/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m
new file mode 100644
index 0000000000..25b37b6a88
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m	
@@ -0,0 +1,40 @@
+% calculate the variances for an equivalent rotation vector
+% inputs are the quaternion orientation and the 4x4 covariance matrix for the quaternions
+% output is a vector of variances for the rotation vector that is equivalent to the quaternion
+clear all;
+reset(symengine);
+syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
+
+% define quaternion rotation
+quat = [q0;q1;q2;q3];
+
+% convert to a rotation vector
+delta = 2*acos(q0);
+rotX = delta*(q1/sin(delta/2));
+rotY = delta*(q2/sin(delta/2));
+rotZ = delta*(q3/sin(delta/2));
+rotVec = [rotX;rotY;rotZ];
+
+% calculate transfer matrix from quaternion to rotation vector
+G = jacobian(rotVec, quat);
+
+% define a symbolic covariance matrix using strings to represent 
+% '_l_' to represent '( '
+% '_c_' to represent ,
+% '_r_' to represent ')' 
+% these can be substituted later to create executable code
+for rowIndex = 1:4
+    for colIndex = 1:4
+        eval(['syms P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1), '_r_ real']);
+        eval(['quatCovMat(',num2str(rowIndex),',',num2str(colIndex), ') = P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1),'_r_;']);
+    end
+end
+
+% rotate the covariance from quaternion to rotation vector
+rotCovMat = G*quatCovMat*transpose(G);
+
+% take the variances
+rotVarVec = [rotCovMat(1,1);rotCovMat(2,2);rotCovMat(3,3)];
+
+% convert to c-code
+ccode(rotVarVec,'file','rotVarVec.c');
diff --git a/matlab/scripts/Inertial Nav EKF/rotVarVec.c b/matlab/scripts/Inertial Nav EKF/rotVarVec.c
new file mode 100644
index 0000000000..7eae50465d
--- /dev/null
+++ b/matlab/scripts/Inertial Nav EKF/rotVarVec.c	
@@ -0,0 +1,19 @@
+  t2 = q0*q0;
+  t3 = acos(q0);
+  t4 = -t2+1.0;
+  t5 = t2-1.0;
+  t6 = 1.0/t5;
+  t7 = q1*t6*2.0;
+  t8 = 1.0/pow(t4,3.0/2.0);
+  t9 = q0*q1*t3*t8*2.0;
+  t10 = t7+t9;
+  t11 = 1.0/sqrt(t4);
+  t12 = q2*t6*2.0;
+  t13 = q0*q2*t3*t8*2.0;
+  t14 = t12+t13;
+  t15 = q3*t6*2.0;
+  t16 = q0*q3*t3*t8*2.0;
+  t17 = t15+t16;
+  A0[0][0] = t10*(P_l_0_c_0_r_*t10+P_l_1_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_1_r_*t10+P_l_1_c_1_r_*t3*t11*2.0)*2.0;
+  A0[1][0] = t14*(P_l_0_c_0_r_*t14+P_l_2_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_2_r_*t14+P_l_2_c_2_r_*t3*t11*2.0)*2.0;
+  A0[2][0] = t17*(P_l_0_c_0_r_*t17+P_l_3_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_3_r_*t17+P_l_3_c_3_r_*t3*t11*2.0)*2.0;