From a0d868db88c59654145b9937cd2a0a14b5d4ecce Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Thu, 9 Dec 2021 17:59:12 -0500 Subject: [PATCH] Delete some old code --- Experimentation/part2_MPC_controller.m | 162 +------------------------ 1 file changed, 1 insertion(+), 161 deletions(-) diff --git a/Experimentation/part2_MPC_controller.m b/Experimentation/part2_MPC_controller.m index 78d83bf..7253adf 100644 --- a/Experimentation/part2_MPC_controller.m +++ b/Experimentation/part2_MPC_controller.m @@ -52,167 +52,7 @@ num_preds = T_p / T_s; num_states = 6; num_inputs = 2; -%% Setup -curr_pos = [state_init(1);state_init(3)]; - -%state_size = tbd; -% z = [init, init, -0.004, 3900]; %for testing purposes -% nsteps = 2; -% [LB, UB] = bounds(nsteps, 1); -% [g,h,dg,dh]=nonlcon(z, Xobs, nsteps); -% [J, dJ] = costfun(z, TestTrack.cline(:,1), TestTrack.theta(1), nsteps); - - -Nobs = randi([10 25], 1,1); -global Xobs -Xobs = generateRandomObstacles(Nobs); - - - -U_final = []; -Y_final = []; -options = optimoptions('fmincon','SpecifyConstraintGradient',true,... - 'SpecifyObjectiveGradient',true) ; - -load('ROB535_ControlProject_part1_Team3.mat'); - -%[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, init); -load('reftrack_info.mat'); - -load('segments_info.mat'); - -%% MPC -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 = state_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]); - - %calculate error - eY(:,i)=Y(:,i)-Y_ref(:,i); - - %generate equality constraints - [Aeq,beq]=eq_cons(i,A,B,eY(:,i),npred_i,nstates,ninputs); - - %generate boundary constraints - [Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs); - - %cost for states - Q=[1,1,1,1,1,1]; - - %cost for inputs - R=[0.1,0.01]; - - H=diag([repmat(Q,[1,npred_i+1]),repmat(R,[1,npred_i])]); - - f=zeros(nstates*(npred_i+1)+ninputs*npred_i,1); - - [x,fval] = quadprog(H,f,[],[],Aeq,beq,Lb,Ub); - - %get linearized input - u_mpc(:,i)=x(nstates*(npred_i+1)+1:nstates*(npred_i+1)+ninputs); - - %get input - U(:,i)=u_mpc(:,i)+U_ref(:,i); - - - %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 - - -%% function start - -function [start_idx, end_idx] = get_indices(segment_num, num_pts) - if segment_num == 1 - start_idx = 1; - end_idx = num_pts(segment_num); - else - start_idx = sum(num_pts(1:segment_num-1)) + 1; - end_idx = sum(num_pts(1:segment_num)); - end -end - -%not used -function x0vec = initvec(x0, u0) - %function used because fmincon needs initial condition to be size of - %state vector - %x0 - last row of Y at checkpoint - %u0 - last row of U at checkpoint - global nsteps - x0vec = []; - for i = 1:nsteps - x0vec = [x0vec, x0]; - end - - %not sure if inputs should be instantiated or not - %will instantiate them to previous u - for i = 1:nsteps-1 - x0vec = [x0vec, u0]; - end -end - -%% mpc functions +%% Constraint Functions function [Lb, Ub] = bound_cons(idx, X_ref, U_ref) global num_preds num_states num_inputs x_lims y_lims delta_lims Fx_lims