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-09 15:38:11 -05:00
parent fcd5d8f74a
commit f27a3b82e3
1 changed files with 56 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
SimPkg_F21(student_ver)/xenia_nonlinearopt.m
View File

@@ -34,7 +34,7 @@ curr_pos = [init(1);init(3)];
Nobs = randi([10 25], 1,1);
global Xobs
Xobs = generateRandomObstacles(Nobs);
%Xobs = generateRandomObstacles(Nobs);
@@ -85,17 +85,14 @@ Bc = @(i) [0, 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)
A = @(i) eye(6) + dt*Ac(i);
B = @(i) dt*Bc(i);
%Decision variables
npred = 10;
npred = 20;
nstates = 6;
ninputs = 2;
Ndec=(npred+1)*nstates+ninputs*npred;
@@ -132,19 +129,19 @@ for i=1:length(T)-1
[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);
[Lb,Ub]=bound_cons(i,U_ref,npred_i,input_range,nstates,ninputs, Y(:,i), Y_ref(:,i));
%cost for states
Q=[10,1,10,1,5,1];
Q=[100,1,100,1,50,1];
%cost for inputs
R=[10,1];
R=[100,1];
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 = optimoptions('quadprog','Display','iter');
[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);
@@ -158,6 +155,23 @@ for i=1:length(T)-1
%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
@@ -226,23 +240,42 @@ function [Aeq,beq]=eq_cons(initial_idx,A,B,x_initial,npred,nstates,ninputs)
end
end
function [Lb,Ub]=bound_cons(initial_idx,U_ref,npred,input_range,nstates,ninputs)
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 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)];
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 = [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
Lb=reshape(Lb,[usize,1]);
Ub=reshape(Ub,[usize,1]);
Lb=[-Inf(xsize,1);Lb];
Ub=[Inf(xsize,1);Ub];
end
@@ -330,6 +363,8 @@ function [lb, ub] = bounds(start_idx, end_idx)
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);