i = 0;
mat_1 = zeros(10000,11);
for sigma = 0:0.01:0.99
for alpha = 0.25:0.25:1
for beta = 0.1:0.4:1.3
[indices, observation] = LQ_LQe_Simu(sigma,alpha,beta);
m = length(observation);
store = indices(2) + 0.05;
if (indices(3) <= indices(1)) && (indices(4) <= store)
for j = 1:m
mat_1(j+i,1) = sigma;
mat_1(j+i,2) = alpha;
mat_1(j+i,3) = beta;
mat_1(j+i,4) = observation(j,1);
mat_1(j+i,5) = observation(j,2);
mat_1(j+i,6) = observation(j,3);
mat_1(j+i,7) = observation(j,4);
mat_1(j+i,8) = observation(j,5);
mat_1(j+i,9) = observation(j,6);
mat_1(j+i,10) = observation(j,7);
mat_1(j+i,11) = observation(j,8);
end
i = i+m;
end
end
end
end
mat_1 = mat_1(1:i, :);
function [indices, observation] = LQ_LQe_Simu(sigma,alpha,beta)
h=0.03;
sampleTime=h;
rho= 4.2;
v_0 = 0.0112;
theta_0 = pi/6;
x0 = [0 v_0*cos(theta_0) 0 v_0*sin(theta_0)]';
StopTime = 2*pi*20/beta;
Time=[0 StopTime];
R = 0.033;
L = 0.287;
vehicle = DifferentialDrive(R,L);
Qd=[rho^2 0 0 0; 0 0 0 0 ; 0 0 rho^2 0; 0 0 0 0];
Rd = diag([2 3]);
Ad = [1 h 0 0; 0 1 0 0; 0 0 1 h; 0 0 0 1];
Bd = [h^2/2 0; h 0; 0 h^2/2; 0 h];
Cd = [1 0 0 0; 0 0 1 0];
Dd = [0 0 0 0;0 0 0 0];
sysD= struct(...
'Ad' , Ad ,...
'Bd' , Bd,...
'Cd' , Cd,...
'x0' , x0 ,...
'alpha' , alpha ,...
'beta' , beta ...
);
[Kd,Pd,lambda]=dlqr(Ad,Bd,Qd,Rd) ;
Ed = Ad'*Pd*Ad-Pd;
Fd = Ad'*Pd*Bd*Kd;
Gd = Kd'*Bd'*Pd*Bd*Kd;
Qdbar = Qd+Kd'*Rd*Kd;
Fdbar = Ad'*Pd*Bd;
Gdbar = Bd'*Pd*Bd;
ctrlD = struct(...
'ui' , [] ,...
'Kd' , Kd ,...
'Pd' , Pd ,...
'Ed' , Ed,...
'Fd' , Fd ,...
'Fdbar' , Fdbar ,...
'Gd' , Gd ,...
'Gdbar' , Gdbar ,...
'Qdbar' , Qdbar ,...
'sigma' , sigma ...
);
simOut = sim("Commande_LQ_LQe_bloc_robot.slx", 'SrcWorkspace', 'current');
LQ.td = simOut.x.time;
LQ.xd = simOut.x.signals.values(:,1:length(sysD.Ad));
LQ.ud = simOut.u.signals.values(:,1:size(sysD.Bd,2));
LQ.yd = simOut.y.signals.values(:,1:size(sysD.Cd,1));
LQ.nb = simOut.eventnb.signals.values(:,1);
LQe.td = simOut.x.time;
LQe.xd = simOut.x.signals.values(:,1+length(sysD.Ad):2*length(sysD.Ad))
LQe.ud = simOut.u.signals.values(:,1+size(sysD.Bd,2):2*size(sysD.Bd,2));
LQe.yd = simOut.y.signals.values(:,1+size(sysD.Cd,1):2*size(sysD.Cd,1));
LQe.ed = simOut.event.signals.values(:,1);
LQe.nb = simOut.eventnb.signals.values(:,2);
instants = find([0; LQe.nb(2:end) - LQe.nb(1:end-1)]);
LQe.ti = LQe.td(instants);
LQe.xi = LQe.xd(instants);
[IAE_TT, IAU_TT, IAE, IAU, IAEP, IAUP] = deal(0);
for i = 1:length(LQ.xd)
IAE_TT = IAE_TT + norm(simOut.xref([1,3], i)' - LQ.yd(i,:))*h;
IAU_TT = IAU_TT + norm(LQ.ud(i,:))*h;
IAE = IAE + norm(simOut.xref([1,3], i)' - LQe.yd(i,:))*h;
IAU = IAU + norm(LQe.ud(i,:))*h;
IAEP = IAEP + norm(LQ.yd(i,:) - LQe.yd(i,:))*h;
IAUP = IAUP + norm(LQ.ud(i,:) - LQe.ud(i,:))*h;
end
indices = [IAE_TT, IAU_TT, IAE, IAU, IAEP, IAUP, LQe.nb(end)];
observation = [LQe.xd simOut.e.data];
end