diff --git a/matlab/scripts/Inertial Nav EKF/quat2yaw321.m b/matlab/scripts/Inertial Nav EKF/quat2yaw321.m
index ec9752154f..e5729f35e3 100644
--- a/matlab/scripts/Inertial Nav EKF/quat2yaw321.m	
+++ b/matlab/scripts/Inertial Nav EKF/quat2yaw321.m	
@@ -22,7 +22,7 @@ syms q0 q1 q2 q3 'real'
 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)];
+yaw_input_321 = [Tbn_quat(2,1);Tbn_quat(1,1)];
 
 % convert to c code and save
 ccode(yaw_input_321,'file','yaw_input_321.c');
diff --git a/matlab/scripts/Inertial Nav EKF/yaw_input_321.c b/matlab/scripts/Inertial Nav EKF/yaw_input_321.c
index 093f7ec095..cc5158702f 100644
--- a/matlab/scripts/Inertial Nav EKF/yaw_input_321.c	
+++ b/matlab/scripts/Inertial Nav EKF/yaw_input_321.c	
@@ -1,2 +1,2 @@
-A0[0][0] = q0*q3*-2.0+q1*q2*2.0;
+A0[0][0] = q0*q3*2.0+q1*q2*2.0;
 A0[1][0] = q0*q0+q1*q1-q2*q2-q3*q3;