diff --git a/SimPkg_F21(student_ver)/xenia_nonlinearopt.m b/SimPkg_F21(student_ver)/xenia_nonlinearopt.m
index e81f27c..c222eb0 100644
--- a/SimPkg_F21(student_ver)/xenia_nonlinearopt.m
+++ b/SimPkg_F21(student_ver)/xenia_nonlinearopt.m
@@ -69,15 +69,26 @@ Ca_f= F_zf*B*C*D;
 
 x = @(s,i) Y_ref(s,i);
 
-Ac = @(i) [0, cos(x(5,i)), 0, -sin(x(5,i)), x(2,i)*sin(x(5,i))-x(4,i)*cos(x(5,i)), 0;
-         0, (-1/m)*Ca_f*x(2,i)^-2, 0, -Ca_f/m + 1, 0, Ca_f*(-a/m) + 1;
+Ac = @(i) [0, cos(x(5,i)), 0, -sin(x(5,i)), -x(2,i)*sin(x(5,i))-x(4,i)*cos(x(5,i)), 0;
+         0, (By*Cy*Dy*b*g*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*sin(U_ref(1,i))*(((x(4,i) + a*x(6,i))*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*(x(4,i) + a*x(6,i)))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)), 0, x(6,i) - (By*Cy*Dy*b*g*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*sin(U_ref(1,i))*((x(2,i)*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*x(2,i))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)), 0, x(4,i) - (By*Cy*Dy*b*g*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*sin(U_ref(1,i))*((a*x(2,i)*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*a*x(2,i))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1));
          0, sin(x(5,i)), 0, cos(x(5,i)), -x(4,i)*sin(x(5,i))+x(2,i)*cos(x(5,i)), 0;
-         0, (1/m)*(-Ca_f*x(2,i)^-2 - Ca_r*x(2,i)^-2) - 1, 0, Ca_r/m*(-1/x(2,i)) + Ca_f/m*(-1/x(2,i)), 0, Ca_r/m*(b/x(2,i)) + Ca_f/m*(-a/x(2,i)) - x(2,i);
-         0, 0, 0, 0, 0, 1
-         0, (1/Iz)*(-Ca_f*a*x(2,i)^-2 - b*Ca_r*x(2,i)^-2), 0, -b*Ca_r/Iz*(-1/x(2,i)) + a*Ca_f/Iz*(-1/x(2,i)), 0, -b*Ca_r/Iz*(b/x(2,i)) + a*Ca_f/Iz*(-a/x(2,i))];
+         0, -x(4,i)-((By*Cy*Dy*a*g*m*cos(Cy*atan(By*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)))*(((x(4,i) - b*x(6,i))*(Ey - 1))/((x(4,i) - b*x(6,i))^2 + x(2,i)^2) - (Ey*(x(4,i) - b*x(6,i)))/((By^2*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))^2 + 1)*((x(4,i) - b*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)^2 + 1)) + (By*Cy*Dy*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*(((x(4,i) + a*x(6,i))*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*(x(4,i) + a*x(6,i)))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/m, 0, -x(2,i)+((By*Cy*Dy*a*g*m*cos(Cy*atan(By*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)))*((x(2,i)*(Ey - 1))/((x(4,i) - b*x(6,i))^2 + x(2,i)^2) - (Ey*x(2,i))/((By^2*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))^2 + 1)*((x(4,i) - b*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)^2 + 1)) + (By*Cy*Dy*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*((x(2,i)*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*x(2,i))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/m, 0, -((By*Cy*Dy*a*g*m*cos(Cy*atan(By*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)))*((b*x(2,i)*(Ey - 1))/((x(4,i) - b*x(6,i))^2 + x(2,i)^2) - (Ey*b*x(2,i))/((By^2*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))^2 + 1)*((x(4,i) - b*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)^2 + 1)) - (By*Cy*Dy*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*((a*x(2,i)*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*a*x(2,i))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/m;
+         0, 0, 0, 0, 0, 1;
+         0, ((By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)))*(((x(4,i) - b*x(6,i))*(Ey - 1))/((x(4,i) - b*x(6,i))^2 + x(2,i)^2) - (Ey*(x(4,i) - b*x(6,i)))/((By^2*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))^2 + 1)*((x(4,i) - b*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)^2 + 1)) - (By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*(((x(4,i) + a*x(6,i))*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*(x(4,i) + a*x(6,i)))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/Iz, 0,  -((By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)))*((x(2,i)*(Ey - 1))/((x(4,i) - b*x(6,i))^2 + x(2,i)^2) - (Ey*x(2,i))/((By^2*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))^2 + 1)*((x(4,i) - b*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)^2 + 1)) - (By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*((x(2,i)*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*x(2,i))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/Iz, 0,  ((By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)))*((b*x(2,i)*(Ey - 1))/((x(4,i) - b*x(6,i))^2 + x(2,i)^2) - (Ey*b*x(2,i))/((By^2*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))^2 + 1)*((x(4,i) - b*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)^2 + 1)) + (By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*((a*x(2,i)*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*a*x(2,i))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/Iz];
      
-Bc = @(i) [0, -Ca_f/m, 0, Ca_f/m, 0, a*Ca_f/Iz; 
-     0, Nw/m, 0, 0, 0, 0]';
+% Bc = @(i) [0, -Ca_f/m, 0, Ca_f/m, 0, a*Ca_f/Iz; 
+%      0, Nw/m, 0, 0, 0, 0]';
+Bc = @(i) [0, 0;
+          -(cos(U_ref(1,i))*(Svy - (Dy*b*g*m*sin(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By))))/(a + b)) + (By*Cy*Dy*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*sin(U_ref(1,i))*(1 + Ey/(By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1) - Ey))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/m, Nw/m;
+          0, 0;
+          -(sin(U_ref(1,i))*(Svy - (Dy*b*g*m*sin(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By))))/(a + b)) - (By*Cy*Dy*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*(1 + Ey/(By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1) - Ey))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/m, 0;
+          0, 0;
+           -(a*sin(U_ref(1,i))*(Svy - (Dy*b*g*m*sin(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By))))/(a + b)) - (By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*(1 + Ey/(By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1) - Ey))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/Iz, 0];
+
+          
+test1 = Ac(1)
+test2 = Bc(2)
+ 
  
 A = @(i) eye(6) + dt*Ac(i);
 B = @(i) dt*Bc(i);
@@ -90,7 +101,7 @@ ninputs = 2;
 Ndec=(npred+1)*nstates+ninputs*npred;
 
 input_range = [-0.5, 0.5; -5000, 5000];
-eY0 = init';
+eY0 = init' - init';
 [Aeq_test1,beq_test1]=eq_cons(1,A,B,eY0,npred,nstates,ninputs);
 %simulate forward
 T = 0:0.01:(size(Y_ref,2)/100);
@@ -124,10 +135,10 @@ for i=1:length(T)-1
     [Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs);
     
     %cost for states
-    Q=[1,1,1,1,1,1];
+    Q=[10,1,10,1,5,1];
     
     %cost for inputs
-    R=[0.1,0.01];
+    R=[10,1];
     
     H=diag([repmat(Q,[1,npred_i+1]),repmat(R,[1,npred_i])]);