sravan/Control-Project
1
0
Fork 0
mirror of https://github.com/ROB-535-F21-Team-3/Control-Project.git synced 2025-09-01 22:13:16 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
xenia-nonlinear
Control-Project/SimPkg_F21(student_ver)/xenia_nonlinearopt.m
xdemenchuk 4165d10a72 MPC kinda works but fails to avoid boundaries well
2021-12-09 18:00:14 -05:00

709 lines
27 KiB
Matlab
Raw Permalink Blame History

%% initial conditions
%Vehicle Parameterrs
Nw=2;
f=0.01;
Iz=2667;
a=1.35;
b=1.45;
By=0.27;
Cy=1.2;
Dy=0.7;
Ey=-1.6;
Shy=0;
Svy=0;
m=1400;
g=9.806;
%note constraints on input
%delta [-0.5, 0.5]
%Fx [-5000, 5000]
load("TestTrack.mat");
init = [287, 5, -176, 0, 2, 0];
curr_pos = [init(1);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, (By*Cy*Dy*b*g*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*sin(U_ref(1,i))*(((x(4,i) + a*x(6,i))*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*(x(4,i) + a*x(6,i)))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)), 0, x(6,i) - (By*Cy*Dy*b*g*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*sin(U_ref(1,i))*((x(2,i)*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*x(2,i))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)), 0, x(4,i) - (By*Cy*Dy*b*g*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*sin(U_ref(1,i))*((a*x(2,i)*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*a*x(2,i))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 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, -x(4,i)-((By*Cy*Dy*a*g*m*cos(Cy*atan(By*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)))*(((x(4,i) - b*x(6,i))*(Ey - 1))/((x(4,i) - b*x(6,i))^2 + x(2,i)^2) - (Ey*(x(4,i) - b*x(6,i)))/((By^2*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))^2 + 1)*((x(4,i) - b*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)^2 + 1)) + (By*Cy*Dy*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*(((x(4,i) + a*x(6,i))*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*(x(4,i) + a*x(6,i)))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/m, 0, -x(2,i)+((By*Cy*Dy*a*g*m*cos(Cy*atan(By*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)))*((x(2,i)*(Ey - 1))/((x(4,i) - b*x(6,i))^2 + x(2,i)^2) - (Ey*x(2,i))/((By^2*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))^2 + 1)*((x(4,i) - b*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)^2 + 1)) + (By*Cy*Dy*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*((x(2,i)*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*x(2,i))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/m, 0, -((By*Cy*Dy*a*g*m*cos(Cy*atan(By*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)))*((b*x(2,i)*(Ey - 1))/((x(4,i) - b*x(6,i))^2 + x(2,i)^2) - (Ey*b*x(2,i))/((By^2*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))^2 + 1)*((x(4,i) - b*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)^2 + 1)) - (By*Cy*Dy*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*((a*x(2,i)*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*a*x(2,i))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/m;
0, 0, 0, 0, 0, 1;
0, ((By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)))*(((x(4,i) - b*x(6,i))*(Ey - 1))/((x(4,i) - b*x(6,i))^2 + x(2,i)^2) - (Ey*(x(4,i) - b*x(6,i)))/((By^2*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))^2 + 1)*((x(4,i) - b*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)^2 + 1)) - (By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*(((x(4,i) + a*x(6,i))*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*(x(4,i) + a*x(6,i)))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/Iz, 0, -((By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)))*((x(2,i)*(Ey - 1))/((x(4,i) - b*x(6,i))^2 + x(2,i)^2) - (Ey*x(2,i))/((By^2*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))^2 + 1)*((x(4,i) - b*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)^2 + 1)) - (By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*((x(2,i)*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*x(2,i))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/Iz, 0, ((By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)))*((b*x(2,i)*(Ey - 1))/((x(4,i) - b*x(6,i))^2 + x(2,i)^2) - (Ey*b*x(2,i))/((By^2*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))^2 + 1)*((x(4,i) - b*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))*(Ey - 1) - (Ey*atan(By*(Shy - atan2(x(4,i) - b*x(6,i), x(2,i)))))/By)^2 + 1)) + (By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*((a*x(2,i)*(Ey - 1))/((x(4,i) + a*x(6,i))^2 + x(2,i)^2) - (Ey*a*x(2,i))/((By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1)*((x(4,i) + a*x(6,i))^2 + x(2,i)^2))))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/Iz];
% Bc = @(i) [0, -Ca_f/m, 0, Ca_f/m, 0, a*Ca_f/Iz;
% 0, Nw/m, 0, 0, 0, 0]';
Bc = @(i) [0, 0;
-(cos(U_ref(1,i))*(Svy - (Dy*b*g*m*sin(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By))))/(a + b)) + (By*Cy*Dy*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*sin(U_ref(1,i))*(1 + Ey/(By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1) - Ey))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/m, Nw/m;
0, 0;
-(sin(U_ref(1,i))*(Svy - (Dy*b*g*m*sin(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By))))/(a + b)) - (By*Cy*Dy*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*(1 + Ey/(By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1) - Ey))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/m, 0;
0, 0;
-(a*sin(U_ref(1,i))*(Svy - (Dy*b*g*m*sin(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By))))/(a + b)) - (By*Cy*Dy*a*b*g*m*cos(Cy*atan(By*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)))*cos(U_ref(1,i))*(1 + Ey/(By^2*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))^2 + 1) - Ey))/((a + b)*(By^2*((Ey - 1)*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i))) - (Ey*atan(By*(Shy + U_ref(1,i) - atan2(x(4,i) + a*x(6,i), x(2,i)))))/By)^2 + 1)))/Iz, 0];
A = @(i) eye(6) + dt*Ac(i);
B = @(i) dt*Bc(i);
%Decision variables
npred = 20;
nstates = 6;
ninputs = 2;
Ndec=(npred+1)*nstates+ninputs*npred;
input_range = [-0.5, 0.5; -5000, 5000];
eY0 = init' - 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,Y(:,i), Y_ref(:,i));
%cost for states
Q=[5,1,5,0.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,[],options);
%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,:)';
traj_info = getTrajectoryInfo(Y',U');
% if (~isempty(traj_info.left_track_position))
% [lb, ub] = bounds(1, npred);
% global nsteps
% nsteps = npred;
% cf=@costfun
% nc=@nonlcon
% z=fmincon(cf,x,[],[],[],[],lb',ub',nc,options);
% Y0=reshape(z(1:6*nsteps),6,nsteps);
% U=reshape(z(6*nsteps+1:end),2,nsteps-1);
% Y_ref(:,i) = Y0(:,end);
% U_ref(:,i) = U(:,end);
% i = i - 1;
% 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 [Aineq, bineq] =ineq_cons(initial_idx, x_initial, npred, nstates, ninputs, Q, R, currpos)
% n = size(x_initial,1);
% m = size(R,1);
%
% Y_curxy = [currpos(1)+x_initial(1); currpos(3)+x_initial(3)];
% sqdist_to_cl = (TestTrack.cline - Y_curxy).^2;
% dist_to_cl = (sqdist_to_cl(1,:) + sqdist_to_cl(2,:)).^0.5;
% [minDist, indMin] = min(dist_to_cl);
%
% prev_idx = max(indMin-1, 1);
% next_idx = min(indMin, 246);
%
% bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ...
% TestTrack.br(1,prev_idx), TestTrack.br(1,next_idx)];
% bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ...
% TestTrack.br(2,prev_idx), TestTrack.br(2,next_idx)];
% %phi_init = TestTrack.theta(i);
%
% lb_x = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
% ub_x = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
%
% Axcons = [1 0 0 0 0 0;
% -1 0 0 0 0 0;
% 0 1 0 0 0 0;
% 0 -1 0 0 0 0;
% 0 0 1 0 0 0;
% 0 0 -1 0 0 0;
% 0 0 0 1 0 0;
% 0 0 0 -1 0 0;
% 0 0 0 0 1 0;
% 0 0 0 0 -1 0;
% 0 0 0 0 0 1;
% 0 0 0 0 0 -1];
%
% bXcons = [ub_x(1); lb_x(1); ub_x(2); lb_x(2); ub_x(3); lb_x(3); ub_x(4); lb_x(4); ub_x(5); lb_x(5); ub_x(6); lb_x(6)];
%
% % next we define the inequality constraints corresponding to bound
% % constraints we have 2 constraints for each state and input at each time
% % instance
% Aineq = zeros(2 * n * npred + 2 * m * (npred - 1),n * npred + m * (npred - 1 ));
% bineq = zeros(2 * n * npred + 2 * m * (npred - 1),1);
% % state constraints
% for i = 1:npred
% Aineq(((i - 1) * n * 2 + 1):(i * n * 2),((i - 1) * n + 1):(i * n)) = AXcons;
% bineq(((i - 1) * n * 2 + 1):(i * n * 2), 1) = bXcons;
% end
%
%
%
% end
function [Aeq,beq]=eq_cons(initial_idx,A,B,x_initial,npred,nstates,ninputs)
%build matrix for A_i*x_i+B_i*u_i-x_{i+1}=0
%in the form Aeq*z=beq
%initial_idx specifies the time index of initial condition from the reference trajectory
%A and B are function handles above
%initial condition
x_initial=x_initial(:);
%size of decision variable and size of part holding states
zsize=(npred+1)*nstates+npred*ninputs;
xsize=(npred+1)*nstates;
Aeq=zeros(xsize,zsize);
Aeq(1:nstates,1:nstates)=eye(nstates); %initial condition
beq=zeros(xsize,1);
beq(1:nstates)=x_initial;
state_idxs=nstates+1:nstates:xsize;
input_idxs=xsize+1:ninputs:zsize;
for i=1:npred
%negative identity for i+1
Aeq(state_idxs(i):state_idxs(i)+nstates-1,state_idxs(i):state_idxs(i)+nstates-1)=-eye(nstates);
%A matrix for i
Aeq(state_idxs(i):state_idxs(i)+nstates-1,state_idxs(i)-nstates:state_idxs(i)-1)=A(initial_idx+i-1);
%B matrix for i
Aeq(state_idxs(i):state_idxs(i)+nstates-1,input_idxs(i):input_idxs(i)+ninputs-1)=B(initial_idx+i-1);
end
end
function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs,currpos, Y_ref)
%time_idx is the index along uref the initial condition is at
load('TestTrack.mat');
xsize=(npred+1)*nstates;
usize=npred*ninputs;
Lb=[];
Ub=[];
for i=1:(npred+1)
Y_ref_curxy = [currpos(1); currpos(3)];
sqdist_to_cl = (TestTrack.cline - Y_ref_curxy).^2;
dist_to_cl = (sqdist_to_cl(1,:) + sqdist_to_cl(2,:)).^0.5;
[minDist, indMin] = min(dist_to_cl);
prev_idx = max(indMin-1, 1);
next_idx = min(indMin, 246);
bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ...
TestTrack.br(1,prev_idx), TestTrack.br(1,next_idx)];
bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ...
TestTrack.br(2,prev_idx), TestTrack.br(2,next_idx)];
%phi_init = TestTrack.theta(i);
lb_x = [min(bound_X)-Y_ref(1); -Inf; min(bound_Y)-Y_ref(3); -Inf; -pi; -Inf];
ub_x = abs([max(bound_X)-Y_ref(1); Inf; max(bound_Y)-Y_ref(3); Inf; +pi; Inf]);
Lb=[Lb;lb_x];
Ub=[Ub;ub_x];
end
for j=1:npred
Lb=[Lb;input_range(:,1)-U_ref(:,initial_idx+j-1)];
Ub=[Ub;input_range(:,2)-U_ref(:,initial_idx+j-1)];
end
% xsize=(npred+1)*nstates;
% usize=npred*ninputs;
% Lb = [];
% Ub = [];
% for j=1:ninputs
% Lb=[Lb;input_range(j,1)-U_ref(j,initial_idx:initial_idx+npred-1)];
% Ub=[Ub;input_range(j,2)-U_ref(j,initial_idx:initial_idx+npred-1)];
% end
%
% Lb=reshape(Lb,[usize,1]);
% Ub=reshape(Ub,[usize,1]);
%
% Lb=[-Inf(xsize,1);Lb];
% Ub=[Inf(xsize,1);Ub];
end
function dzdt=kinematic_bike(t,x,U,T)
%constants
Nw=2;
f=0.01;
Iz=2667;
a=1.35;
b=1.45;
By=0.27;
Cy=1.2;
Dy=0.7;
Ey=-1.6;
Shy=0;
Svy=0;
m=1400;
g=9.806;
%generate input functions
delta_f=U(1);
F_x=U(2);
%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;
if F_total>F_max
F_x=F_max/F_total*F_x;
F_yr=F_max/F_total*F_yr;
end
%vehicle dynamics
dzdt= [x(2)*cos(x(5))-x(4)*sin(x(5));...
(-f*m*g+Nw*F_x-F_yf*sin(delta_f))/m+x(4)*x(6);...
x(2)*sin(x(5))+x(4)*cos(x(5));...
(F_yf*cos(delta_f)+F_yr)/m-x(2)*x(6);...
x(6);...
(F_yf*a*cos(delta_f)-F_yr*b)/Iz];
end
%% nonlinear opt functions
function [lb, ub] = bounds(start_idx, end_idx)
load('TestTrack.mat');
load('reftrack_info.mat');
%[m,nPts] = size(TestTrack.cline);
% numState = 6;
% numInput = 2;
% nsteps = StepsPerPoint * nPts;
lb_u = [-0.5;-5000];
ub_u = [0.5;5000];
bound_X = [TestTrack.bl(1,1), TestTrack.bl(1,2), ...
TestTrack.br(1,1), TestTrack.br(1,2)];
bound_Y = [TestTrack.bl(2,1), TestTrack.bl(2,2), ...
TestTrack.br(2,1), TestTrack.br(2,2)];
phi_init = TestTrack.theta(1);
%phi restricted to just [-pi pi]
%lb = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
%ub = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
lb = []; ub = [];
%hijacking this for loop to only consider one index for bounds
for i=start_idx:end_idx+1
Y_ref_curxy = [Y_submission(i,1); Y_submission(i,3)];
sqdist_to_cl = (TestTrack.cline - Y_ref_curxy).^2;
dist_to_cl = (sqdist_to_cl(1,:) + sqdist_to_cl(2,:)).^0.5;
[minDist, indMin] = min(dist_to_cl);
global target_vec
target_vec = [TestTrack.cline(1,indMin), TestTrack.cline(2,indMin), TestTrack.theta(indMin)];
prev_idx = max(indMin-1, 1);
next_idx = min(indMin, 246);
bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ...
TestTrack.br(1,prev_idx), TestTrack.br(1,next_idx)];
bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ...
TestTrack.br(2,prev_idx), TestTrack.br(2,next_idx)];
%phi_init = TestTrack.theta(i);
lb_x = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
ub_x = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
lb=[lb;lb_x];
ub=[ub;ub_x];
end
for i=start_idx:end_idx
ub=[ub;ub_u];
lb=[lb;lb_u];
end
end
function [J, dJ] = costfun(z)
global nsteps
global target_vec
load('TestTrack.mat');
targetPos = target_vec(1:2);
targetTheta = target_vec(3);
sumPos = [];
sumInput = [];
for i = 1:nsteps
zIndx = 6*(i-1) + 1;
sumPos(i) = (z(zIndx) - targetPos(1))^2 + (z(zIndx+2) - targetPos(2))^2 + (z(zIndx+4) - targetTheta)^2;
if (i <= nsteps-1)
uInd = 2*(i-1) + nsteps*6 - 1;
sumInput(i) = z(uInd)^2 + z(uInd+1)^2;
end
end
J = sum(sumPos) + sum(sumInput);
dJ = transpose(z.*2);
uStart = nsteps*6 - 1;
for i = 1:6:nsteps*6
zIndx = i;
dJ(i) = (z(zIndx) - targetPos(1))*2;
dJ(i+2) = (z(zIndx+2) - targetPos(2))*2;
dJ(i+4) = (z(zIndx+4) - targetTheta)*2;
end
end
function [g, h, dg, dh] = nonlcon(z)
%nsteps = (size(z,2)/8);
global nsteps
global Xobs
curr_pos = [z(1); z(3)];
Xobs_seen = senseObstacles(curr_pos, Xobs);
centroids = [];
for i = 1:size(Xobs_seen,2)
centroids = [centroids; mean(Xobs_seen{1, i})];
end
dt = 0.01;
g = []; dg = [];
%radius size
r = 3;
for i = 1:nsteps
zInd_x = 6*(i-1) + 1;
zInd_y = 6*(i-1) + 3;
curr_xy = [z(zInd_x), z(zInd_y)];
%g based on if there's obstacles around
%initialize to zero
g_curr = 0;
zeroRow = zeros(1,size(z,2));
dg(i,:) = zeroRow;
if (~isempty(centroids))
dist2Obst = [];
for j = 1:size(Xobs_seen,2)
dist = norm(curr_xy(1) - centroids(j,1)) + norm(curr_xy(2) - centroids(j,2));
dist2Obst = [dist2Obst; dist];
end
%closest obstacle used for constraint
[minDist, indMin] = min(dist2Obst);
%g
g_curr = r^2 - (curr_xy(1) - centroids(indMin, 1))^2 - (curr_xy(2) - centroids(indMin, 2))^2;
%dg
dg(i,zInd_x) = curr_xy(1)*(-2) - centroids(indMin, 1)*2;
dg(i,zInd_y) = curr_xy(2)*(-2) - centroids(indMin, 2)*2;
end
g = [g; g_curr];
end
dg = transpose(dg);
h = z(1:6);
for i = 2:nsteps
zInd_x = 6*(i-1) + 1;
zInd_x_prev = 6*(i-2) + 1;
stateCurr = z(zInd_x:zInd_x+5);
statePrev = z(zInd_x_prev:zInd_x_prev+5);
%get previous input
uInd_prev = 2*(i-1) + nsteps*6 - 1;
uPrev = [z(uInd_prev), z(uInd_prev + 1)];
derPrev= dt*bike(statePrev, uPrev);
currH = stateCurr - statePrev - derPrev;
h(6*(i-1)+1) = currH(1);
h(6*(i-1)+2) = currH(2);
h(6*(i-1)+3) = currH(3);
h(6*(i-1)+4) = currH(4);
h(6*(i-1)+5) = currH(5);
h(6*(i-1)+6) = currH(6);
end
dh = zeros(size(z,2), nsteps*6);
dh(1:6, 1:6) = eye(6);
for i = 1:nsteps-1
uInd = 2*(i-1) + nsteps*6 + 1;
uCurr = [z(uInd), z(uInd + 1)];
zInd_x = 6*(i-1) + 1;
stateCurr = z(zInd_x:zInd_x+5);
[A, B] = bikeLinearize(stateCurr, uCurr);
dh(6*i+1:6*i+6, 6*i+1:6*i+6) = eye(6);
dh(6*i-5:6*i, 6*i+1:6*i+6) = -eye(6) - dt*A;
dh(nsteps*6+2*i-1:nsteps*6+2*i, 6*i+1:6*i+6) = -dt*transpose(B);
end
end
%% linearization functions
function dzdt=bike(x,U)
%constants
Nw=2;
f=0.01;
Iz=2667;
a=1.35;
b=1.45;
By=0.27;
Cy=1.2;
Dy=0.7;
Ey=-1.6;
Shy=0;
Svy=0;
m=1400;
g=9.806;
%generate input functions
delta_f= U(1);
F_x= U(2);
%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;
if F_total>F_max
F_x=F_max/F_total*F_x;
F_yr=F_max/F_total*F_yr;
end
%vehicle dynamics
dzdt= [x(2)*cos(x(5))-x(4)*sin(x(5));...
(-f*m*g+Nw*F_x-F_yf*sin(delta_f))/m+x(4)*x(6);...
x(2)*sin(x(5))+x(4)*cos(x(5));...
(F_yf*cos(delta_f)+F_yr)/m-x(2)*x(6);...
x(6);...
(F_yf*a*cos(delta_f)-F_yr*b)/Iz];
end
function [A, B] =bikeLinearize(x,U)
%constants
Nw=2;
f=0.01;
Iz=2667;
a=1.35;
b=1.45;
By=0.27;
Cy=1.2;
Dy=0.7;
Ey=-1.6;
Shy=0;
Svy=0;
m=1400;
g=9.806;
%generate input functions
delta_f= U(1);
F_x= U(2);
%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;
if F_total>F_max
F_x=F_max/F_total*F_x;
F_yr=F_max/F_total*F_yr;
end
%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;
A = [0, cos(x(5)), 0, -sin(x(5)), x(2)*sin(x(5))-x(4)*cos(x(5)), 0;
0, (-1/m)*Ca_f*x(2)^-2, 0, -Ca_f/m + 1, 0, Ca_f*(-a/m) + 1;
0, sin(x(5)), 0, cos(x(5)), -x(4)*sin(x(5))+x(2)*cos(x(5)), 0;
0, (1/m)*(-Ca_f*x(2)^-2 - Ca_r*x(2)^-2) - 1, 0, Ca_r/m*(-1/x(2)) + Ca_f/m*(-1/x(2)), 0, Ca_r/m*(b/x(2)) + Ca_f/m*(-a/x(2)) - x(2);
0, 0, 0, 0, 0, 1
0, (1/Iz)*(-Ca_f*a*x(2)^-2 - b*Ca_r*x(2)^-2), 0, -b*Ca_r/Iz*(-1/x(2)) + a*Ca_f/Iz*(-1/x(2)), 0, -b*Ca_r/Iz*(b/x(2)) + a*Ca_f/Iz*(-a/x(2))];
B = [0, -Ca_f/(x(2)*m), 0, Ca_f/m, 0, a*Ca_f/Iz;
0, Nw/m, 0, 0, 0, 0]';
end
Reference in New Issue View Git Blame Copy Permalink