diff --git a/SimPkg_F21(student_ver)/xenia_attempts_save.m b/SimPkg_F21(student_ver)/xenia_attempts_save.m new file mode 100644 index 0000000..06b7028 --- /dev/null +++ b/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