diff --git a/Matlab/SimTimeStep.m b/Matlab/SimTimeStep.m
index cd8dc66..7da392e 100755
--- a/Matlab/SimTimeStep.m
+++ b/Matlab/SimTimeStep.m
@@ -44,12 +44,12 @@
     l_r = ModelParams.lr;
 
     
-    phi   =x(3);
-    v_x     =x(4);
-    v_y     =x(5);
-    omega   =x(6);
-    D     =u(1);
-    delta =u(2);
+    phi   = x(ModelParams.stateindex_phi);
+    v_x   = x(ModelParams.stateindex_vx);
+    v_y   = x(ModelParams.stateindex_vy);
+    omega = x(ModelParams.stateindex_omega);
+    D     = u(ModelParams.inputindex_D);
+    delta = u(ModelParams.inputindex_delta);
     
     
     alpha_f = -atan2(l_f*omega + v_y,abs(v_x))+delta;
@@ -69,4 +69,4 @@
        u(3)];
 
     
-return
\ No newline at end of file
+return
diff --git a/Matlab/getMPCmatrices.m b/Matlab/getMPCmatrices.m
index 26b9898..2b56f18 100755
--- a/Matlab/getMPCmatrices.m
+++ b/Matlab/getMPCmatrices.m
@@ -114,7 +114,7 @@
     end
         
     theta_virt=mod(Xk(end),pathinfo.ppx.breaks(end));
-    [grad_eC, grad_eL] = getErrorGradient(pathinfo, theta_virt, ModelParams,Xk(1), Xk(2));
+    [grad_eC, grad_eL] = getErrorGradient(pathinfo, theta_virt, ModelParams, Xk(ModelParams.stateindex_x), Xk(ModelParams.stateindex_y));
     errorgrad = [grad_eC; grad_eL]; 
     Qtilde = errorgrad'*Q*errorgrad; 
 end