mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-19 09:12:45 +00:00
Delete some old code
This commit is contained in:
Sravan Balaji
@@ -52,167 +52,7 @@ num_preds = T_p / T_s;
|
|||||||
num_states = 6;
|
num_states = 6;
|
||||||
num_inputs = 2;
|
num_inputs = 2;
|
||||||
|
|
||||||
%% Setup
|
%% Constraint Functions
|
||||||
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
|
|
||||||
function [Lb, Ub] = bound_cons(idx, X_ref, U_ref)
|
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
|
global num_preds num_states num_inputs x_lims y_lims delta_lims Fx_lims
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user