Control-Project/SimPkg_F21(student_ver)/xenia_attempts_save.m

%% trying to figure out jacobian 
%constants
syms Nw f Iz a b By Cy Dy Ey Shy Svy m g real

syms delta_f F_x real
x = sym('x', [1 6], 'real');
%generate input functions
% delta_f=interp1(T,U(:,1),t,'previous','extrap');
% F_x=interp1(T,U(:,2),t,'previous','extrap');

%slip angle functions in degrees
a_f=delta_f-atan2(x(4)+a*x(6),x(2));
a_r=-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;


% eq1 = x(2)*cos(x(5))-x(4)*sin(x(5));
% a11 = diff(eq1, x(1))
% a12 = diff(eq1, x(2))
% a13 = diff(eq1, x(3))
% a14 = diff(eq1, x(4))
% a15 = diff(eq1, x(5))
% a16 = diff(eq1, x(6))
% b11 = diff(eq1, delta_f)
% b12 = diff(eq1, F_x)

% eq2 = 1/m*(-f*m*g + Nw*F_x - F_yf*sin(delta_f))+x(4)*x(6);
% a21 = diff(eq2, x(1));
% a22 = diff(eq2, x(2));
% a23 = diff(eq2, x(3));
% a24 = diff(eq2, x(4));
% a25 = diff(eq2, x(5));
% a26 = diff(eq2, x(6));
% b21 = diff(eq2, delta_f);
% b22 = diff(eq2, F_x);

% eq3 = x(2)*sin(x(5)) + x(4)*cos(x(5));
% a31 = diff(eq3, x(1))
% a32 = diff(eq3, x(2))
% a33 = diff(eq3, x(3))
% a34 = diff(eq3, x(4))
% a35 = diff(eq3, x(5))
% a36 = diff(eq3, x(6))
% b31 = diff(eq3, delta_f)
% b32 = diff(eq3, F_x)

% eq4 = 1/m*(F_yf*cos(delta_f) + F_yr) - x(2)*x(4)
% a41 = diff(eq4, x(1))
% a42 = diff(eq4, x(2))
% a43 = diff(eq4, x(3))
% a44 = diff(eq4, x(4))
% a45 = diff(eq4, x(5))
% a46 = diff(eq4, x(6))
% b41 = diff(eq4, delta_f)
% b42 = diff(eq4, F_x)

eq5 = x(6);
% a51 = diff(eq5, x(1))
% a52 = diff(eq5, x(2))
% a53 = diff(eq5, x(3))
% a54 = diff(eq5, x(4))
% a55 = diff(eq5, x(5))
% a56 = diff(eq5, x(6))
% b51 = diff(eq5, delta_f)
% b52 = diff(eq5, F_x)

eq6 = 1/Iz*(a*F_yf*cos(delta_f)-b*F_yr);
a61 = diff(eq6, x(1))
a62 = diff(eq6, x(2))
a63 = diff(eq6, x(3))
a64 = diff(eq6, x(4))
a65 = diff(eq6, x(5))
a66 = diff(eq6, x(6))
b61 = diff(eq6, delta_f)
b62 = diff(eq6, F_x)
% (delta_f - arctan((v+a*r)/u))
% ((1-E_y)*((delta_f - arctan((v+a*r)/u)) + S_hy) + E_y/B_y * arctan(B_y*((delta_f - arctan((v+a*r)/u))+S_hy)))
% F_zf*D_y*sin(C_y*arctan(B_y*((1-E_y)*((delta_f - arctan((v+a*r)/u)) + S_hy) + E_y/B_y 
% * arctan(B_y*((delta_f - arctan((v+a*r)/u))+S_hy))))) + S_vy


%% nonlinear using reference trajectory segments 
% for i = 1:length(num_pts)
%     [start_idx, end_idx] = get_indices(i, num_pts);
%     
%     delta = delta_vals(i);
%     F_x = F_x_vals(i);
%     
%     if (end_idx >= size(Y_submission,1))
%         start_idx = start_idx - 1;
%         end_idx = end_idx - 1;
%     end
%     
%     dif = num_pts(i)/5; 
%     if (num_pts(i) > 500)
%         dif = num_pts(i)/10;
%     end
%     
%     global nsteps
%     nsteps = dif;
%     
%     temp_end_idx = start_idx+dif-1; 
%     while (temp_end_idx <= end_idx)
%         x0 = [];
%         for j = start_idx:temp_end_idx+1 %+1 end idx to keep z size consistent to hw
%             x0 = [x0, Y_submission(j,:)];
%         end
% 
%         for j = start_idx:temp_end_idx
%             x0 = [x0, delta, F_x];
%         end
% 
%         %start and end index used to maintain size of vectors
%         [lb, ub] = bounds(start_idx, temp_end_idx);
% 
%         %define for cost function, goal is to reach end of segment
%         global target_vec
%         target_vec = [Y_submission(end_idx,1), Y_submission(end_idx,3), Y_submission(end_idx,5)];
% 
% 
%         cf=@costfun
%         nc=@nonlcon
%         z=fmincon(cf,x0,[],[],[],[],lb',ub',nc,options);
%         Y0=reshape(z(1:6*nsteps),6,nsteps)';
%         U=reshape(z(6*nsteps+1:end),2,nsteps-1)';
%         info = getTrajectoryInfo(Y0,U, Xobs)
%         U_final = [U_final; U];
%         
%         start_idx = start_idx + dif;
%         temp_end_idx = start_idx+dif; 
%     end
% end

%% original attemp at nonlinear with centerline
%%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)
% 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  
% while (index_cl_cp < size(TestTrack.cline,2))
%     %because of the way cf/nc need to be)
%     index_cl_cp
%     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