sravan/Control-Project
1
0
Fork 0
mirror of https://github.com/ROB-535-F21-Team-3/Control-Project.git synced 2025-08-23 19:02:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

MPC kinda working/nonlinear

Browse Source
This commit is contained in:
xdemenchuk
2021-12-06 12:55:26 -05:00
parent 877912e9bf
commit fcd5d8f74a
1 changed files with 21 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
SimPkg_F21(student_ver)/xenia_nonlinearopt.m
View File

@@ -69,15 +69,26 @@ 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;
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, (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))];
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];
test1 = Ac(1)
test2 = Bc(2)
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);
@@ -90,7 +101,7 @@ ninputs = 2;
Ndec=(npred+1)*nstates+ninputs*npred;
input_range = [-0.5, 0.5; -5000, 5000];
eY0 = init';
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);
@@ -124,10 +135,10 @@ for i=1:length(T)-1
[Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs);
%cost for states
Q=[1,1,1,1,1,1];
Q=[10,1,10,1,5,1];
%cost for inputs
R=[0.1,0.01];
R=[10,1];
H=diag([repmat(Q,[1,npred_i+1]),repmat(R,[1,npred_i])]);