past attempts/diff calculations

SimPkg_F21(student_ver)/xenia_attempts_save.m

@@ -0,0 +1,171 @@
+%% trying to figure out jacobian 
+%constants
+syms Nw f Iz a b By Cy Dy Ey Shy Svy m g 
+
+syms delta_f F_x 
+x = sym('x', [1 6]);
+%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=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;
+
+
+
+negFyf = -F_yf;
+negFyfsindeltaf = negFyf*sin(delta_f);
+
+Fyfcosdeltaf = F_yf*cos(delta_f);
+
+aFyfcosdeltaf = a*F_yf*cos(delta_f);
+
+negbFyr = -b*F_yr;
+
+eq1difu = diff(negFyfsindeltaf, x(2))
+eq1difv = diff(negFyfsindeltaf, x(4))
+eq1difr = diff(negFyfsindeltaf, x(6))
+eq1difdeltaf = diff(negFyfsindeltaf, delta_f)
+eq1difFx = diff(negFyfsindeltaf, F_x)
+
+
+%% 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