%% Particle Swarm Optimizer for an a la LQ state space controller (position controlled system)
%
% Bartlomiej UFNALSKI
% 10.01.2018 for MATLAB Central
%%
% Parameters
clear mex
clear
clc
tic
evalfit = 1;
rng('shuffle');
%
try
close(1)
catch wiad
end
iterations = 100; % if you are not short of time increase it even beyound 200
num_of_D = 4;
correction_factor = 2.05;
K=2/abs(2-2*correction_factor-sqrt((2*correction_factor)^2-8*correction_factor));
swarm_size = 30;
swarm=zeros(swarm_size,4,num_of_D);
init_position=2;
velocity_clamping=(init_position-(-init_position))/2;
historia=zeros(iterations,swarm_size,4);
jakosc=zeros(iterations);
init_speed=1;
% ---- initial swarm position -----
for index=1:swarm_size,
swarm(index, 1, :) = init_position*(rand(num_of_D,1)-0.5)*2;
swarm(index, 2, :) = init_speed*(rand(num_of_D,1)-0.5)*2;
end
swarm(:, 4, 1) = 100000; % best value so far
for iter = 1 : iterations
disp(['Iteration: ' num2str(iter) ' from ' num2str(iterations)]);
%-- evaluating position & quality ---
for n = 1 : swarm_size
k1 = swarm(n, 1, 1);
k2 = swarm(n, 1, 2);
k3 = swarm(n, 1, 3);
k4 = swarm(n, 1, 4);
eval_fitness;
if round(10000*max(sim_out.get('tout'))) == round(10000*tstop),
Yout=sim_out.get('yout');
val=Yout(numel(Yout));
else
val=100000-max(sim_out.get('tout'));
end
disp(['Particle: ' num2str(n) '/' num2str(swarm_size) ' with fitness ' num2str(val)]);
historia(iter,n,1)=k1;
historia(iter,n,2)=k2;
historia(iter,n,3)=k3;
historia(iter,n,4)=k4;
if val < swarm(n, 4, 1) % if new position is better
swarm(n, 3, 1) = swarm(n, 1, 1); % update best position
swarm(n, 3, 2) = swarm(n, 1, 2);
swarm(n, 3, 3) = swarm(n, 1, 3);
swarm(n, 3, 4) = swarm(n, 1, 4);
swarm(n, 4, 1) = val; % and best value
end
end
[temp, gbest] = min(swarm(:, 4, 1)); % global best position
k1best = swarm(gbest, 3, 1);
k2best = swarm(gbest, 3, 2);
k3best = swarm(gbest, 3, 3);
k4best = swarm(gbest, 3, 4);
jakosc(iter)=temp;
historia_gbest(iter,:)=swarm(gbest, 3, :);
clc
%% Plotting the swarm
clf
figure(1)
plot(swarm(:, 1, 1),swarm(:, 1, 2),'.'); axis([-20 20 -20 20]);
xlabel(num2str(k1best));
ylabel(num2str(k2best));
grid
figure(2)
plot(swarm(:, 1, 3),swarm(:, 1, 4),'.'); axis([-20 20 -20 20]);
xlabel(num2str(k3best));
ylabel(num2str(k4best));
grid
pause(.01);
%--- updating velocity vectors and positions
disp(['Best solution so far: ' num2str(swarm(gbest, 4, 1))]);
%
for n = 1 : swarm_size
for d=1:num_of_D,
swarm(n, 2, d) = K*(swarm(n, 2, d)...
+ correction_factor*rand*(swarm(n, 3, d) - swarm(n, 1, d))...
+ correction_factor*rand*(swarm(gbest, 3, d) - swarm(n, 1, d)));
swarm(n, 2, d)= min(max(-velocity_clamping,swarm(n, 2, d)),velocity_clamping);
swarm(n, 1, d) = swarm(n, 1, d) + swarm(n, 2, d);
end
end
end
k1 = k1best;
k2 = k2best;
k3 = k3best;
k4 = k4best;
save('kbest_DC_servo','k1','k2','k3','k4','historia_gbest','historia','jakosc','tstop');
odchylenie=zeros(iterations,num_of_D);
for i=1:iterations,
odchylenie(i,1)=std(historia(i,:,1));
odchylenie(i,2)=std(historia(i,:,2));
odchylenie(i,3)=std(historia(i,:,3));
odchylenie(i,4)=std(historia(i,:,4));
end
font_size = 10;
tick_size = 8;
line_width = 1.5;
marker_size = 10;
figure(3)
subplot(221);
plot(odchylenie(:,1),'k.','MarkerSize',2)
ylabel('$\mathrm{std}(k_1)$','FontSize',9,'Interpreter','LaTeX');
y_temp = get(gca,'Ylim'); y_lim=[0,y_temp(2)]; set(gca,'Ylim',y_lim);
subplot(222);
plot(odchylenie(:,2),'k.','MarkerSize',2)
ylabel('$\mathrm{std}(k_2)$','FontSize',9,'Interpreter','LaTeX');
y_temp = get(gca,'Ylim'); y_lim=[0,y_temp(2)]; set(gca,'Ylim',y_lim);
subplot(223);
plot(odchylenie(:,3),'k.','MarkerSize',2)
xlabel('iteracja roju','FontSize',9,'Interpreter','LaTeX');
ylabel('$\mathrm{std}(k_3)$','FontSize',9,'Interpreter','LaTeX');
y_temp = get(gca,'Ylim'); y_lim=[0,y_temp(2)]; set(gca,'Ylim',y_lim);
subplot(224);
plot(odchylenie(:,4),'k.','MarkerSize',2)
xlabel('iteracja roju','FontSize',9,'Interpreter','LaTeX');
ylabel('$\mathrm{std}(k_4)$','FontSize',9,'Interpreter','LaTeX');
y_temp = get(gca,'Ylim'); y_lim=[0,y_temp(2)]; set(gca,'Ylim',y_lim);
figure(4)
semilogy(jakosc,'k','LineWidth',2);
xlabel('iteracja roju','FontSize',9,'Interpreter','LaTeX');
ylabel('performance index $\mathcal{J}$','FontSize',font_size,'Interpreter','LaTeX'); grid;
%
evalfit=0;
eval_fitness;
disp('-------------------Finito!-------------------');
disp(['Elapsed time is ' num2str(round(toc/60)) ' minutes.']);