Incorporate xenia-nonlinear branch changes

- Copy-paste `xenia_nonlinearopt.m` from xenia-nonlinear branch

Experimentation/xenia_nonlinearopt.m

@@ -1,3 +1,4 @@
+%% initial conditions
 %Vehicle Parameterrs
 Nw=2;
 f=0.01;
@@ -30,131 +31,126 @@ curr_pos = [init(1);init(3)];
 % [g,h,dg,dh]=nonlcon(z, Xobs, nsteps);
 % [J, dJ] = costfun(z, TestTrack.cline(:,1), TestTrack.theta(1), nsteps);
 
-%%okay, idea is to create an iterative process that uses the centerline as a
-%checkpoint
-%keep trajecting until closer to the next point on centerline, then repeat
-%bounds will be based on index of current&previous cp index  
-%cost function uses next checkpoint in centerline 
 
-%generate z and make it bigger if didn't reach close enough to the goal
-%(modify nsteps)
 Nobs = randi([10 25], 1,1);
 global Xobs
 Xobs = generateRandomObstacles(Nobs);
 
-% global index_cl_cp %index of current checkpoint in centerline
-% index_cl_cp = 1;
-% global index_cl_nextcp %index of next checkpoint in centerline
-% index_cl_nextcp = 2;
-% 
-% global nsteps
-% nsteps = 100;
 
-% x0 = init;
-% x0vec = []; 
-% %initialize x0vec to be initial condition for n steps and Sravan's ref inputs
-% %note: function 'initvec' changes inputs, but i'm not sure how
-% %initialization should look with fmincon 
-% for i = 1:nsteps
-%     x0vec = [x0vec, init];
-% end
-% for i = 1:nsteps-1
-%     x0vec = [x0vec, -0.004, 3900];
-% end
 
-% dist12atcp = [];
-% iter = 0;
-% 
-% factor = 0.1; %next checkpoint if significantly closer to next checkpoint  
 U_final = [];
 Y_final = [];
 options = optimoptions('fmincon','SpecifyConstraintGradient',true,...
                         'SpecifyObjectiveGradient',true) ;
 
-load('ROB535_ControlProject_part1_Team3.mat');                    
+load('ROB535_ControlProject_part1_Team3.mat');              
+
 %[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, init);
 load('reftrack_info.mat');
+
 load('segments_info.mat');
 
-for i = 1:length(num_pts)
+%% MPC
-    [start_idx, end_idx] = get_indices(i, num_pts);
+U_ref = ROB535_ControlProject_part1_input';
+Y_ref = Y_submission';
+dt = 0.01;
+
+%discretize dynamics
+F_zf=b/(a+b)*m*g;
+F_zr=a/(a+b)*m*g;
+
+%we are just going to use cornering stiffness to make linear so this derivative
+%easier, the vehicle parameter's are close enough to problem 1 hw 2
+B=10;
+C=1.3;
+D=1;
+Ca_r= F_zr*B*C*D;
+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;
+         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))];
+     
+Bc = @(i) [0, -Ca_f/m, 0, Ca_f/m, 0, a*Ca_f/Iz; 
+     0, Nw/m, 0, 0, 0, 0]';
+ 
+A = @(i) eye(6) + dt*Ac(i);
+B = @(i) dt*Bc(i);
+
+
+%Decision variables
+npred = 10;
+nstates = 6;
+ninputs = 2;
+Ndec=(npred+1)*nstates+ninputs*npred;
+
+input_range = [-0.5, 0.5; -5000, 5000];
+eY0 = 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);
+%final trajectory
+Y=NaN(nstates,length(T));
+
+%applied inputs
+U=NaN(ninputs,length(T));
+
+%input from QP
+u_mpc=NaN(ninputs,length(T));
+
+%error in states (actual-reference)
+eY=NaN(nstates,length(T));
+
+%set random initial condition
+Y(:,1)=eY0+Y_ref(:,1);
+
+for i=1:length(T)-1
+    i
+    %shorten prediction horizon if we are at the end of trajectory
+    npred_i=min([npred,length(T)-i]);
     
-    delta = delta_vals(i);
+    %calculate error
-    F_x = F_x_vals(i);
+    eY(:,i)=Y(:,i)-Y_ref(:,i);
+
+    %generate equality constraints
+    [Aeq,beq]=eq_cons(i,A,B,eY(:,i),npred_i,nstates,ninputs);
     
-    if (end_idx >= size(Y_submission,1))
+    %generate boundary constraints
-        start_idx = start_idx - 1;
+    [Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs);
-        end_idx = end_idx - 1;
-    end
     
-    x0 = [];
+    %cost for states
-    for j = start_idx:end_idx+1 %+1 end idx to keep z size consistent to hw
+    Q=[1,1,1,1,1,1];
-        x0 = [x0, Y_submission(j,:)];
-    end
     
-    for j = start_idx:end_idx
+    %cost for inputs
-        x0 = [x0, delta, F_x];
+    R=[0.1,0.01];
-    end
     
-    %start and end index used to maintain size of vectors
+    H=diag([repmat(Q,[1,npred_i+1]),repmat(R,[1,npred_i])]);
-    [lb, ub] = bounds(start_idx, end_idx);
     
-    %define for cost function, goal is to reach end of segment
+    f=zeros(nstates*(npred_i+1)+ninputs*npred_i,1);
-    global target_vec
-    target_vec = [Y_submission(end_idx,1), Y_submission(end_idx,3), Y_submission(end_idx,5)];
     
-    global nsteps
+    [x,fval] = quadprog(H,f,[],[],Aeq,beq,Lb,Ub);
-    nsteps = num_pts(i)+1;
     
-    cf=@costfun
+    %get linearized input
-    nc=@nonlcon
+    u_mpc(:,i)=x(nstates*(npred_i+1)+1:nstates*(npred_i+1)+ninputs);
-    z=fmincon(cf,x0,[],[],[],[],lb',ub',nc,options);
+    
-    Y0=reshape(z(1:6*nsteps),6,nsteps)';
+    %get input
-    U=reshape(z(6*nsteps+1:end),2,nsteps-1)';
+    U(:,i)=u_mpc(:,i)+U_ref(:,i);
-    info = getTrajectoryInfo(Y0,U)
+    
-    U_final = [U_final; U];
+    
+    %simulate model
+    [~,ztemp]=ode45(@(t,z)kinematic_bike(t,z,U(:,i),0),[0 dt],Y(:,i));
+    
+    %store final state
+    Y(:,i+1)=ztemp(end,:)';
 end
-% while (index_cl_cp < size(TestTrack.cline,2))
+
-%     %because of the way cf/nc need to be)
+
-%     index_cl_cp
+%% function start
-%     iter = iter + 1;
-%     [lb, ub] = bounds(nsteps, index_cl_cp);
-%     cf=@costfun
-%     nc=@nonlcon
-%     z=fmincon(cf,x0vec,[],[],[],[],lb',ub',nc,options);
-%     Y0=reshape(z(1:6*nsteps),6,nsteps)';
-%     U=reshape(z(6*nsteps+1:end),2,nsteps-1)';
-%     
-%     [Y, T] = forwardIntegrateControlInput(U, x0vec(1:6));
-%     
-%     curr_xy = [Y(end,1); Y(end,3)];
-%     dist2cp1 = norm(curr_xy - TestTrack.cline(:, index_cl_cp));
-%     dist2cp2 = norm(curr_xy - TestTrack.cline(:, index_cl_nextcp));
-%     
-%     if (dist2cp2  < (dist2cp1 - dist2cp1*factor))
-%         dist12atcp = [dist12atcp; dist2cp1, dist2cp2];
-%         %add to final solution
-%         U_final = [U_final; U];
-%         Y_final = [Y_final; Y];
-%         
-%         %reinstantiate 
-%         %nsteps = 100;
-%         x0vec = initvec(Y_final(end,:), U_final(end,:));  
-%         
-%         %update checkpoint
-%         index_cl_cp = index_cl_cp + 1
-%         index_cl_nextcp = index_cl_nextcp + 1;
-%         if (index_cl_nextcp > size(TestTrack.cline, 2))
-%             index_cl_nextcp = size(TestTrack.cline, 2);
-%         end
-%     else 
-%         %resize and try again
-%         %nsteps = nsteps + 20; 
-%         x0vec = initvec(x0vec(1:6), U(1,:));
-%         
-%     end
-%     
-% end
 
 function [start_idx, end_idx] = get_indices(segment_num, num_pts)
     if segment_num == 1
@@ -166,6 +162,7 @@ function [start_idx, end_idx] = get_indices(segment_num, num_pts)
     end
 end
 
+%not used
 function x0vec = initvec(x0, u0)
     %function used because fmincon needs initial condition to be size of
     %state vector
@@ -184,6 +181,115 @@ function x0vec = initvec(x0, u0)
     end
 end
 
+%% mpc functions
+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
+    %initial_idx specifies the time index of initial condition from the reference trajectory 
+    %A and B are function handles above
+
+    %initial condition
+    x_initial=x_initial(:);
+
+    %size of decision variable and size of part holding states
+    zsize=(npred+1)*nstates+npred*ninputs;
+    xsize=(npred+1)*nstates;
+
+    Aeq=zeros(xsize,zsize);
+    Aeq(1:nstates,1:nstates)=eye(nstates); %initial condition 
+    beq=zeros(xsize,1);
+    beq(1:nstates)=x_initial;
+
+    state_idxs=nstates+1:nstates:xsize;
+    input_idxs=xsize+1:ninputs:zsize;
+
+    for i=1:npred
+        %negative identity for i+1
+        Aeq(state_idxs(i):state_idxs(i)+nstates-1,state_idxs(i):state_idxs(i)+nstates-1)=-eye(nstates);
+        
+        %A matrix for i
+        Aeq(state_idxs(i):state_idxs(i)+nstates-1,state_idxs(i)-nstates:state_idxs(i)-1)=A(initial_idx+i-1);
+
+        %B matrix for i
+        Aeq(state_idxs(i):state_idxs(i)+nstates-1,input_idxs(i):input_idxs(i)+ninputs-1)=B(initial_idx+i-1);
+    end
+end
+
+function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs)
+    %time_idx is the index along uref the initial condition is at
+    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)
+%constants
+Nw=2;
+f=0.01;
+Iz=2667;
+a=1.35;
+b=1.45;
+By=0.27;
+Cy=1.2;
+Dy=0.7;
+Ey=-1.6;
+Shy=0;
+Svy=0;
+m=1400;
+g=9.806;
+
+
+%generate input functions
+delta_f=U(1);
+F_x=U(2);
+
+%slip angle functions in degrees
+a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2)));
+a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2)));
+
+%Nonlinear Tire Dynamics
+phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy));
+phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy));
+
+F_zf=b/(a+b)*m*g;
+F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy;
+
+F_zr=a/(a+b)*m*g;
+F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy;
+
+F_total=sqrt((Nw*F_x)^2+(F_yr^2));
+F_max=0.7*m*g;
+
+if F_total>F_max
+    
+    F_x=F_max/F_total*F_x;
+  
+    F_yr=F_max/F_total*F_yr;
+end
+
+%vehicle dynamics
+dzdt= [x(2)*cos(x(5))-x(4)*sin(x(5));...
+          (-f*m*g+Nw*F_x-F_yf*sin(delta_f))/m+x(4)*x(6);...
+          x(2)*sin(x(5))+x(4)*cos(x(5));...
+          (F_yf*cos(delta_f)+F_yr)/m-x(2)*x(6);...
+          x(6);...
+          (F_yf*a*cos(delta_f)-F_yr*b)/Iz];
+end
+
+%% nonlinear opt functions
 function [lb, ub] = bounds(start_idx, end_idx)
     load('TestTrack.mat');
     load('reftrack_info.mat');
@@ -361,6 +467,8 @@ function [g, h, dg, dh] = nonlcon(z)
         
 end
 
+%% linearization functions
+
 function dzdt=bike(x,U)
 %constants
 Nw=2;