%% 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