MPC kinda working/nonlinear

SimPkg_F21(student_ver)/xenia_nonlinearopt.m

@@ -34,7 +34,7 @@ curr_pos = [init(1);init(3)];
 
 Nobs = randi([10 25], 1,1);
 global Xobs
-Xobs = generateRandomObstacles(Nobs);
+%Xobs = generateRandomObstacles(Nobs);
 
 
 
@@ -85,17 +85,14 @@ Bc = @(i) [0, 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);
 
 
 %Decision variables
-npred = 10;
+npred = 20;
 nstates = 6;
 ninputs = 2;
 Ndec=(npred+1)*nstates+ninputs*npred;
@@ -132,19 +129,19 @@ for i=1:length(T)-1
     [Aeq,beq]=eq_cons(i,A,B,eY(:,i),npred_i,nstates,ninputs);
     
     %generate boundary constraints
-    [Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs);
+    [Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs, Y(:,i), Y_ref(:,i));
     
     %cost for states
-    Q=[10,1,10,1,5,1];
+    Q=[100,1,100,1,50,1];
     
     %cost for inputs
-    R=[10,1];
+    R=[100,1];
     
     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);
+    [x,fval] = quadprog(H,f,[],[],Aeq,beq,Lb,Ub,[],options);
     
     %get linearized input
     u_mpc(:,i)=x(nstates*(npred_i+1)+1:nstates*(npred_i+1)+ninputs);
@@ -158,6 +155,23 @@ for i=1:length(T)-1
     
     %store final state
     Y(:,i+1)=ztemp(end,:)';
+    
+    traj_info = getTrajectoryInfo(Y',U');
+    
+%     if (~isempty(traj_info.left_track_position))
+%         [lb, ub] = bounds(1, npred);
+%         global nsteps 
+%         nsteps = npred;
+%         cf=@costfun
+%         nc=@nonlcon
+%         z=fmincon(cf,x,[],[],[],[],lb',ub',nc,options);
+%         Y0=reshape(z(1:6*nsteps),6,nsteps);
+%         U=reshape(z(6*nsteps+1:end),2,nsteps-1);
+%         Y_ref(:,i) = Y0(:,end);
+%         U_ref(:,i) = U(:,end);
+%         i = i - 1;
+%     end
+        
 end
 
 
@@ -226,23 +240,42 @@ function [Aeq,beq]=eq_cons(initial_idx,A,B,x_initial,npred,nstates,ninputs)
     end
 end
 
-function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs)
+function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs,currpos, Y_ref)
     %time_idx is the index along uref the initial condition is at
+    load('TestTrack.mat');
     xsize=(npred+1)*nstates;
     usize=npred*ninputs;
 
     Lb=[];
     Ub=[];
-    for j=1:ninputs
+    for i=1:(npred+1)
-    Lb=[Lb;input_range(j,1)-U_ref(j,initial_idx:initial_idx+npred-1)];
+        Y_ref_curxy = [currpos(1); currpos(3)];
-    Ub=[Ub;input_range(j,2)-U_ref(j,initial_idx:initial_idx+npred-1)];
+        sqdist_to_cl = (TestTrack.cline - Y_ref_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)-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];
+        
+        
+        Lb=[Lb;lb_x];
+        Ub=[Ub;ub_x];
+        
+    end
+    
+    for j=1:npred
+    Lb=[Lb;input_range(:,1)-U_ref(:,initial_idx+j-1)];
+    Ub=[Ub;input_range(:,2)-U_ref(:,initial_idx+j-1)];
     end
-
-    Lb=reshape(Lb,[usize,1]);
-    Ub=reshape(Ub,[usize,1]);
-
-    Lb=[-Inf(xsize,1);Lb];
-    Ub=[Inf(xsize,1);Ub];
 
 end
 
@@ -330,6 +363,8 @@ function [lb, ub] = bounds(start_idx, end_idx)
         sqdist_to_cl = (TestTrack.cline - Y_ref_curxy).^2;
         dist_to_cl = (sqdist_to_cl(1,:) + sqdist_to_cl(2,:)).^0.5;
         [minDist, indMin] = min(dist_to_cl);
+        global target_vec
+        target_vec = [TestTrack.cline(1,indMin), TestTrack.cline(2,indMin), TestTrack.theta(indMin)];
         
         prev_idx = max(indMin-1, 1);
         next_idx = min(indMin, 246);