MPC kinda works but fails to avoid boundaries well

SimPkg_F21(student_ver)/xenia_nonlinearopt.m

@@ -132,15 +132,15 @@ for i=1:length(T)-1
     [Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs,Y(:,i), Y_ref(:,i));
    
     %cost for states
-    Q=[100,1,100,1,50,1];
+    Q=[5,1,5,0.1,1,1];
     
     %cost for inputs
-    R=[100,1];
+    R=[0.1,0.01];
     
     H=diag([repmat(Q,[1,npred_i+1]),repmat(R,[1,npred_i])]);
     
     f=zeros(nstates*(npred_i+1)+ninputs*npred_i,1);
-    options = optimoptions('quadprog','Display','iter');
+   
     [x,fval] = quadprog(H,f,[],[],Aeq,beq,Lb,Ub,[],options);
     
     %get linearized input
@@ -207,6 +207,63 @@ function x0vec = initvec(x0, u0)
 end
 
 %% mpc functions
+% function [Aineq, bineq] =ineq_cons(initial_idx, x_initial, npred, nstates, ninputs, Q, R, currpos)
+%     n = size(x_initial,1);
+%     m = size(R,1);
+%     
+%     Y_curxy = [currpos(1)+x_initial(1); currpos(3)+x_initial(3)];
+%     sqdist_to_cl = (TestTrack.cline - Y_curxy).^2;
+%     dist_to_cl = (sqdist_to_cl(1,:) + sqdist_to_cl(2,:)).^0.5;
+%     [minDist, indMin] = min(dist_to_cl);
+% 
+%     prev_idx = max(indMin-1, 1);
+%     next_idx = min(indMin, 246);
+% 
+%     bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ... 
+%                TestTrack.br(1,prev_idx), TestTrack.br(1,next_idx)];
+%     bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ... 
+%                TestTrack.br(2,prev_idx), TestTrack.br(2,next_idx)];
+%     %phi_init = TestTrack.theta(i);
+% 
+%     lb_x = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
+%     ub_x = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
+%        
+%     Axcons = [1 0 0 0 0 0;
+%              -1 0 0 0 0 0;
+%               0 1 0 0 0 0;
+%              0 -1 0 0 0 0;
+%               0 0 1 0 0 0;
+%              0 0 -1 0 0 0;
+%               0 0 0 1 0 0;
+%              0 0 0 -1 0 0;
+%               0 0 0 0 1 0;
+%              0 0 0 0 -1 0;
+%               0 0 0 0 0 1;
+%              0 0 0 0 0 -1];
+%          
+%     bXcons = [ub_x(1); lb_x(1); ub_x(2); lb_x(2); ub_x(3); lb_x(3); ub_x(4); lb_x(4); ub_x(5); lb_x(5); ub_x(6); lb_x(6)];
+%     
+%     % next we define the inequality constraints corresponding to bound
+%     % constraints we have 2 constraints for each state and input at each time
+%     % instance
+%     Aineq = zeros(2 * n * npred + 2 * m * (npred - 1),n * npred + m * (npred - 1 ));
+%     bineq = zeros(2 * n * npred + 2 * m * (npred - 1),1);
+%     % state constraints
+%     for i = 1:npred
+%        Aineq(((i - 1) * n * 2 + 1):(i * n * 2),((i - 1) * n + 1):(i * n)) = AXcons;
+%        bineq(((i - 1) * n * 2 + 1):(i * n * 2), 1) = bXcons;
+%     end
+%        
+%     
+%         
+% end
+    
+
+
+
+
+
+
 function [Aeq,beq]=eq_cons(initial_idx,A,B,x_initial,npred,nstates,ninputs)
     %build matrix for A_i*x_i+B_i*u_i-x_{i+1}=0
     %in the form Aeq*z=beq
@@ -264,7 +321,7 @@ function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs,
         %phi_init = TestTrack.theta(i);
         
         lb_x = [min(bound_X)-Y_ref(1); -Inf; min(bound_Y)-Y_ref(3); -Inf; -pi; -Inf];
-        ub_x = [max(bound_X)-Y_ref(1); Inf; max(bound_Y)-Y_ref(3); Inf; +pi; Inf];
+        ub_x = abs([max(bound_X)-Y_ref(1); Inf; max(bound_Y)-Y_ref(3); Inf; +pi; Inf]);
         
         
         Lb=[Lb;lb_x];
@@ -277,6 +334,22 @@ function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs,
     Ub=[Ub;input_range(:,2)-U_ref(:,initial_idx+j-1)];
     end
     
+
+%     xsize=(npred+1)*nstates;
+%     usize=npred*ninputs;
+%     Lb = [];
+%     Ub = [];
+%     for j=1:ninputs
+%     Lb=[Lb;input_range(j,1)-U_ref(j,initial_idx:initial_idx+npred-1)];
+%     Ub=[Ub;input_range(j,2)-U_ref(j,initial_idx:initial_idx+npred-1)];
+%     end
+% 
+%     Lb=reshape(Lb,[usize,1]);
+%     Ub=reshape(Ub,[usize,1]);
+% 
+%     Lb=[-Inf(xsize,1);Lb];
+%     Ub=[Inf(xsize,1);Ub];
+
 end
 
 function dzdt=kinematic_bike(t,x,U,T)