Below is the program:
%% Parameter Definitions
g = 9.8;
m = 3600/2.2;
L = 2.1336;
l_fl_r = 0.85;
alfaf_max = 7pi/180;
alat_max = 0.7g;
K_u = 1.1;
b = L/(1+l_fl_r);
a = L-b;
Cf = b/Lmalat_max/alfaf_max;
Cr = K_ul_fl_rCf;
U = 15;
Iz = .5ma*b;
n = 3; % number of state variables
m = 1; % number of inputs
p = 2; % number of relative parameters
%% Simulation Parameters
%%%%%%%% discretization
N = 300; % temporal nodes
T = 5.33; % final time
dt = T/N; %deltaT
Kmax = 2; % number of maximum outer loops
% Reference Path
x = linspace(-40,40,N+1);
r_ref = [x; zeros(1,N+1)];
%%%%%%% trust region
Vmax = 20; % meters/second
PsiMax = 40pi/180; % degrees
% PsiBarMax = 20pi/180; % degrees
etainit = [Vmax, PsiMax]';
etamin = [1,0.05];
etamax = [20,30*pi/180];
etas = zeros(length(etainit),Kmax); etas(:,1) = etainit;
rho_0 = 0.1; rho_1 = 0.7; rho_2 = 1.2; % trust region boundaries
betaGr = 1.1; betaSh = 0.6;
%%%%%%%%% obstacle definition
M = [41,41,2,2]; % big-M
xmin = -1; xmax = 1; ymin = -1; ymax = 1;
%%%%%%%%%%%%%%%%%initial conditions
%%%%%initial position
xinit = [0 0 0]‘; % [V(m/s), r(rad/s), Psi(deg)]
xinit_ab = [-40 0]’; % [ x_ab(m), y_ab(m)]
%%%%%initial Trajectory
xold = zeros(n,N+1); xold(:,1) = xinit;
xabo = zeros(p,N+1); xabo(:,1) = xinit_ab;
%%%%
rho = zeros(Kmax, 1);
phis = zeros(Kmax, 1);
phihats = zeros(Kmax, 1);
xhist =zeros(n,N+1,Kmax);
xabhist =zeros(p,N+1,Kmax);
%%%%%%%%% Objective function Penalty variables
q1 = [0.001, 0.001, 0.0001]‘;
q2 = [10]’;
q3 = [1000,1000,1000]';
q4 = 10^6.*ones(n,1);
q5 = 10^6.*ones(p,1);
%% STATE SPACE SYSTEMS (initial)
% [V(m/s), r(rad/s), Psi(deg)]
%%%%%%%%%%%%%continuous dynamics
A = [-(Cf+Cr)/(mU) (bCr-aCf)/(mU)-U 0;…
(bCr-aCf)/(IzU) -(b^2Cr+a^2Cf)/(IzU) 0;…
0 1 0];
%B matrix with inputs
B = [Cf/m; a*Cf/Iz; 0];
% discretized state space system
sysd = c2d(ss(A,B,zeros(1,n),0),dt);
Ad = sysd.A; Bd = sysd.B;
for i=1:N
t1 = xold(1,i); t3 = xold(3,i);% [V, Psi]
Ao = [U*cos(t3)-t1*sin(t3);…
-U*sin(t3) + t1*cos(t3)];
xabo(:,i+1) = xabo(:,i) + dt*Ao;
end
oldphi = sum(norms(xabo - r_ref,1)) + q3’*abs(xold(:,N+1));
%%
% cvx_quiet(true);
% cvx_solver_settings(‘write’,‘dump.task.gz’)
% cvx_solver_settings(‘MSK_DPAR_MIO_TOL_REL_GAP’,0.5);
% cvx_solver_settings(‘MSK_DPAR_MIO_TOL_ABS_GAP’,.5);
for i=1:2
tic
cvx_begin
cvx_precision low
variables x(n,N+1) u(m,N)
variable delta(4,N+1) integer
xab = cvx(zeros(2, N+1));
minimize( sum(norms(xab-r_ref,1)) + q3'*abs(x(:,N+1)) )
subject to:
% ref state vector [V(m/s), r(rad/s), Psi(deg)]
% relaxed integer constraint
for j=1:N+1
0 <= delta(:,j) <= 1;
end
%input constraint
abs(u(:)) <= 30*pi/180;
%(body fixed coordinate)
for j=1:N
x(:,j+1) == Ad*x(:,j) + Bd*u(j);
end
%initial conditions
xab(:,1) = xinit_ab;
x(:,1) == xinit;
% relative obstacle motion
for j=1:N
t1 = xold(1,j); t3 = xold(3,j);% [V, Psi]
Ao = [cos(t3) -sin(t3) -U*sin(t3)-t1*cos(t3);...
-sin(t3) cos(t3) -U*cos(t3)-t1*sin(t3)];
xab(:,j+1) = xab(:,j) + dt*Ao*[U;x(1,j);x(3,j)];
end
% constraint inequalities
for j=1:N+1
xab(1,j) <= xmin + M(1)*delta(1,j);
-xab(1,j) <= -xmax + M(2)*delta(2,j);
xab(2,j) <= ymin + M(3)*delta(3,j);
-xab(2,j) <= -ymax + M(4)*delta(4,j);
ones(4,1)'*delta(:,j) <= 3;
end
%Trust Region Constraints
% abs( xold(1,:) - x(1,:) ) <= etas(1,i);
% abs( xold(3,:) - x(3,:) ) <= etas(2,i);
cvx_end
toc
%calculate actual global position
xabn = zeros(p,N); xabn(:,1) = xinit_ab;
for j=1:N
var1 = x(1,j); var2 = x(2,j); var3 = x(3,j);
Ao = [U*cos(var3) - var1*sin(var3);...
-U*sin(var3) + var1*cos(var3)];
xabn(:,j+1) = xabn(:,j) + dt*Ao;
end
Js(i) = sum(norms(xabn-r_ref,1)); % actual cost
phis(i) = Js(i) + q3'*abs(x(:,N+1)); % with updated xab values
phihats(i) = sum(norms(xab-r_ref,1)) + q3'*abs(x(:,N+1)) ; % approximated value
deltahat = abs(oldphi - phihats(i));
delta = abs(oldphi - phis(i));
rho(i) = deltahat/delta;
oldphi = phis(i);
% trust region update
% not applied
%
etas(1,i+1) = etas(1,i); % unchanged
etas(2,i+1) = etas(2,i); % unchanged
xhist(:,:,i)=x;
xabhist(:,:,i)=xab;
uhist(i,:) = u;
xold = x;
disp(cvx_status);
disp(' iter phihat phi deltahat delta rho cvx')
disp([i phihats(i) phis(i) deltahat delta rho(i) cvx_optval])
end
cvx_quiet(false);
%% PLOT
%obstacle
xobs = [-1 1 1 -1 -1]; yobs=[-1 -1 1 1 -1];
[val,I] = min(phis);
[val2,I2] =min(phihats);
if(val>val2)
xabest =xabhist(:,:,I2);
xbest = xhist(:,:,I2);
ubest = uhist(I2,:);
else
xabest =xabhist(:,:,I);
xbest = xhist(:,:,I);
ubest = uhist(I,:);
end
plot(xabest(1,:),xabest(2,:),xobs,yobs,r_ref(1,:),r_ref(2,:),‘–’,…
xab(1,:),xab(2,:));
xlim([-20 20]); ylim([-5 5]);
legend(‘LOBJ’,‘Obstacle’,‘ref’,‘last iteration’)