MATLAB Examples

## Contents

% Based on the problem as suggested by Anthony A. Maciejewski of Colorado % State University http://www.engr.colostate.edu/~aam/ % use a redundent robot to use configurations that decrease sensitivity to % joint angles to increase repeatability 

## Problem Parameters

X_range=pi/180*[-100,162;-125,125]; % J_fun_num=2; Y_fun_num=1; L_1=0.29;L_2=norm([.085,0.300]);L_3=0.09+4*0.0254; %link lengths % robot functions for visualization P_1=@(theta) L_1*[cos(theta(1));sin(theta(1))]; P_2=@(theta) P_1(theta(1))+L_2*[cos(sum(theta(1:2)));sin(sum(theta(1:2)))]; P_3=@(theta) P_2(theta(1:2))+L_3*[cos(sum(theta(1:3)));sin(sum(theta(1:3)))]; Theta_o=@(P_3_pnt) -atan2(P_3_pnt(2),P_3_pnt(1)); FuncUsed={@(theta) L_1^2+L_2^2+L_3^2+2*cos(theta(1,:))*L_1*L_2+2*cos(theta(1,:)+theta(2,:))*L_1*L_3+2*cos(theta(2,:))*L_2*L_3 ;... % radial distance to tip @(theta) 10*(L_1^2*(L_2*sin(theta(1,:))+L_3*sin(theta(1,:)+theta(2,:))).^2+... L_3^2*(L_2*sin(theta(2,:))+L_1*sin(theta(1,:)+theta(2,:))).^2)}; FuncGrad={@(theta) -2*[L_1*L_3*sin(theta(1,:)+theta(2,:))+L_1*L_2*sin(theta(1,:));L_1*L_3*sin(theta(1,:)+theta(2,:))+L_2*L_3*sin(theta(2,:))];... @(theta) 10*[L_1*L_2*L_3^2*sin(theta(1,:)+2*theta(2,:))-L_1*L_2*L_3^2*sin(theta(1,:))+L_1^2*L_2^2*sin(2*theta(1,:))+2*L_1^2*L_3^2*sin(2*theta(1,:)+2*theta(2,:))+2*L_1^2*L_2*L_3*sin(2*theta(1,:)+theta(2,:));... L_1^2*L_2*L_3*sin(2*theta(1,:)+theta(2,:))-L_1^2*L_2*L_3*sin(theta(2,:))+L_2^2*L_3^2*sin(2*theta(2,:))+2*L_1^2*L_3^2*sin(2*theta(1,:)+2*theta(2,:))+2*L_1*L_2*L_3^2*sin(theta(1,:)+2*theta(2,:))]}; %note gradients are columns FunOptzgParameters=[... 0.025, 0.025, 0.08, 0.05, 0.025*[8, 2e-2], .5, 1e-1;... 0.025, 0.025, 0.08, 0.05, 0.025*[8, 2e-2], .5, 1e-1];... % First row for first function; Second row, second function... % GammaCost,MinGrad_Cost,Gamma_Out,MinGrad_Out,... % KillDist (Cost), SettledStepSize (Cost), MinGrad_Final % From test functions % 0.025, 0.025, 0.08, 0.05, 0.025*[3, 2e-5], .5, 1e-1;... FunCustngParameters=[... 250, FunOptzgParameters(1,6),... max(FunOptzgParameters(Y_fun_num,3),FunOptzgParameters(J_fun_num,1))*[10, 0.00015];... 250, FunOptzgParameters(2,6),... max(FunOptzgParameters(Y_fun_num,3),FunOptzgParameters(J_fun_num,1))*[10, 0.00015]]; % MaxTestSteps, MaxGradientSize (measure of particle is settled), % MaxClusterDist, MinClusterDist CustngTestParameters=[... 20, 6, 0.5, 0.3;... 20, 6, 0.5, 0.3]; % NumTestPerClust, NumTestPerPoint, PervertScale, addition to PervertScale J=@(X) FuncUsed{J_fun_num}(X); Y=@(X) FuncUsed{Y_fun_num}(X); J_Grad=@(X) FuncGrad{J_fun_num}(X); Y_Grad=@(X) FuncGrad{Y_fun_num}(X); XSamples=[28,35]; [X1,X2]=meshgrid(linspace(X_range(1,1),X_range(1,2),XSamples(1)),... linspace(X_range(2,1),X_range(2,2),XSamples(2))); YY=reshape(Y([X1(:),X2(:)].'),size(X1)); JJ=reshape(J([X1(:),X2(:)].'),size(X1)); figure(10); clf; subplot(2,2,1); contour(X1,X2,YY); title('Output Function'); xlabel('x_1'); ylabel('x_2'); colorbar; axis equal subplot(2,2,3); contour(X1,X2,JJ); title('Cost Function'); xlabel('x_1'); ylabel('x_2'); colorbar; axis equal subplot(1,2,2); cla; hold on; contour(X1,X2,YY); contour(X1,X2,JJ); hold off; title('Both Functions Overlayed'); xlabel('x_1'); ylabel('x_2'); axis equal;axis tight YY_GradMag=sqrt(sum(Y_Grad([X1(:),X2(:)].').^2,1)); JJ_GradMag=sqrt(sum(J_Grad([X1(:),X2(:)].').^2,1)); % figure(31) subplot(211) hist(YY_GradMag,60); title('Output Gradient Magnitude Histogram'); ylabel('# occurances'); subplot(212) hist(JJ_GradMag,60); title('Cost Gradient Magnitude Histogram'); ylabel('# occurances'); xlabel('These Gradients should be on the same order') 

GradTestPoints=repmat((X_range(:,2)-X_range(:,1)),1,100).*... rand(size(X_range,1),100)+repmat(X_range(:,1),1,100); GradTestPoints_diff_Y=zeros(size(GradTestPoints)); GradTestPoints_func_Y=zeros(size(GradTestPoints)); GradTestPoints_diff_J=zeros(size(GradTestPoints)); GradTestPoints_func_J=zeros(size(GradTestPoints)); TestStepMag=0.001; for lexi=1:size(GradTestPoints,2) GradTestPoints_diff_Y(:,lexi)=[... (Y(GradTestPoints(:,lexi)+TestStepMag*[1;0])-Y(GradTestPoints(:,lexi)))/TestStepMag;... (Y(GradTestPoints(:,lexi)+TestStepMag*[0;1])-Y(GradTestPoints(:,lexi)))/TestStepMag]; GradTestPoints_diff_J(:,lexi)=[... (J(GradTestPoints(:,lexi)+TestStepMag*[1;0])-J(GradTestPoints(:,lexi)))/TestStepMag;... (J(GradTestPoints(:,lexi)+TestStepMag*[0;1])-J(GradTestPoints(:,lexi)))/TestStepMag]; GradTestPoints_func_Y(:,lexi)=Y_Grad(GradTestPoints(:,lexi)); GradTestPoints_func_J(:,lexi)=J_Grad(GradTestPoints(:,lexi)); end figure(17); subplot(2,2,1);plot(GradTestPoints_diff_Y(1,:),GradTestPoints_diff_Y(1,:)-GradTestPoints_func_Y(1,:),'.'); %daspect([1,1,1]) title('Difference of finite step gradient and analyitical gradient'); subplot(2,2,3);plot(GradTestPoints_diff_Y(2,:),GradTestPoints_diff_Y(2,:)-GradTestPoints_func_Y(2,:),'.'); %daspect([1,1,1]) subplot(2,2,2);plot(GradTestPoints_diff_J(1,:),GradTestPoints_diff_J(1,:)-GradTestPoints_func_J(1,:),'.'); %daspect([1,1,1]) subplot(2,2,4);plot(GradTestPoints_diff_J(2,:),GradTestPoints_diff_J(2,:)-GradTestPoints_func_J(2,:),'.'); %daspect([1,1,1]) %looks good, error is proportional to TestStepMag 

## Optimization Parameters

NumParticles=36; %initial number of agents ParticleView=1;%[1:3];%which particles to observe as they take steps ParticleViewBaseFig=1000; for etsuko=ParticleView figure(ParticleViewBaseFig+100+etsuko);clf; end MaxSteps=300; %maximum number of steps StepDecayRate=0.12; PauseLength=0.005; BoundaryViolationAllowance=1e-8; Gamma_Cost=FunOptzgParameters(J_fun_num,1); % maximum step size MinGrad_Cost=FunOptzgParameters(J_fun_num,2); % when the cost gradient is less than this MinGrad_Cost, % the cost gradient will be reduced from the maximum step size % Note that the maximum step is also reduced by (1-cos) of the angle between % gradients MinGrad_Final=FunOptzgParameters(J_fun_num,7); MinGrad_Out=FunOptzgParameters(Y_fun_num,4); % when the output gradient is less than this MinGrad_Out, % the Output gradient will be reduced from unity resulting in a component % in the output gradient direction to be left in the step % KillDist=2*Gamma_Cost; KillDist=FunOptzgParameters(J_fun_num,5); % SettledStepSize=0.02*Gamma_Cost; SettledStepSize=FunOptzgParameters(J_fun_num,6); MinBoundGrad=FunOptzgParameters(Y_fun_num,8); %this saturates the normalization for the correction of boundary violations 

## Clustering parameters

MaxTestSteps=FunCustngParameters(J_fun_num,1); MaxGradientSize=FunCustngParameters(J_fun_num,2); MaxClusterDist=FunCustngParameters(J_fun_num,3); MinClusterDist=FunCustngParameters(J_fun_num,4); Gamma_Out=FunOptzgParameters(Y_fun_num,3); % maximum step size for extending the clusters 

## Testing parameters

NumTestPerClust=CustngTestParameters(J_fun_num,1); NumTestPerPoint=CustngTestParameters(J_fun_num,2); % purtibations to ideal points PervertScale=CustngTestParameters(J_fun_num,3); % scaling of total range to apply for purtibations PervertConstant=CustngTestParameters(J_fun_num,4); % Added to scale to ensure some variation TestBaseFigNum=2000; 

## initialize particles

% Might as well do uniform random distrubution Particles_intial=repmat((X_range(:,2)-X_range(:,1)),1,NumParticles).*... rand(size(X_range,1),NumParticles)+repmat(X_range(:,1),1,NumParticles); % Particles_intial=[-0.7407;1.135];NumParticles=1; %to test a specific point 

## Run Optimization

Particles_new=Particles_intial; Particles_J=zeros(1,size(Particles_new,2)); for deedee=1:size(Particles_new,2); Particles_J(deedee)=J(Particles_new(:,deedee)); end Particles_J_last=Particles_J; StationaryPoints=zeros(nargin(Y),0); NumClusters=0; clear Clusters % Set up for function form CostStepOpt.J=J; CostStepOpt.Grad_J=J_Grad; CostStepOpt.Grad_Y=Y_Grad; CostStepOpt.MinGrad_Cost=MinGrad_Cost; CostStepOpt.Gamma_Cost=Gamma_Cost; CostStepOpt.MinGrad_Out=MinGrad_Out; CostStepOpt.Gamma_Out=Gamma_Out; CostStepOpt.MinGrad_Final=MinGrad_Final; CostStepOpt.MinGrad_Bnd=MinBoundGrad; CostStepOpt.X_range=X_range; CostStepOpt.BoundaryViolationAllowance=BoundaryViolationAllowance; CostStepOpt.MinGrad_Cost=MinGrad_Cost; CostStepOpt.Gamma_Cost=Gamma_Cost; CostStepOpt.MinGrad_Out=MinGrad_Out; CostStepOpt.Gamma_Out=Gamma_Out; CostStepOpt.MinGrad_Final=MinGrad_Final; CostStepOpt.StepDiscount=StepDecayRate; CostStepOpt.MaxDecay=40; CostStepOpt.ArmijoK=0.4; % for cluster forming ClusterFrmOpt.Y=Y; ClusterFrmOpt.J=J; ClusterFrmOpt.Grad_J=J_Grad; ClusterFrmOpt.Grad_Y=Y_Grad; ClusterFrmOpt.CostStepOpt=CostStepOpt; ClusterFrmOpt.MinGrad_Cost=MinGrad_Cost; ClusterFrmOpt.Gamma_Cost=Gamma_Cost; ClusterFrmOpt.MinGrad_Out=MinGrad_Out; ClusterFrmOpt.Gamma_Out=Gamma_Out; ClusterFrmOpt.MinGrad_Final=MinGrad_Final; ClusterFrmOpt.MinGrad_Bnd=MinBoundGrad; ClusterFrmOpt.MaxTestSteps=MaxTestSteps; ClusterFrmOpt.MaxGradientSize=MaxGradientSize; ClusterFrmOpt.MaxClusterDist=MaxClusterDist; ClusterFrmOpt.MinClusterDist=MinClusterDist; ClusterFrmOpt.X_range=X_range; ClusterFrmOpt.BoundaryViolationAllowance=BoundaryViolationAllowance; ClusterFrmOpt.Verbose=false; ClusterFrmOpt.StepsBeginDecay=floor(0.8*ClusterFrmOpt.MaxTestSteps); ClusterFrmOpt.StepDiscount=CostStepOpt.StepDiscount; ClusterFrmOpt.MaxDecay=CostStepOpt.MaxDecay; ClusterFrmOpt.ArmijoK=CostStepOpt.ArmijoK; tic; for becky=1:MaxSteps NumParticles=size(Particles_new,2); Particles_old=Particles_new; Delta_X=PSO_OptFun_CostDescent_Armijo_1(Particles_old,CostStepOpt); Particles_new=Particles_old+Delta_X; Particles_adj=false(1,size(Particles_new,2)); figure(12); clf; hold on; contour(X1,X2,YY); contour(X1,X2,JJ); plot(Particles_old(1,:),Particles_old(2,:),'r.') if ~isempty(StationaryPoints) plot(StationaryPoints(1,:),StationaryPoints(2,:),'go','markerface','g') end plot(cumsum([Particles_old(1,:);Delta_X(1,:)],1),... cumsum([Particles_old(2,:);Delta_X(2,:)],1),'r-') daspect([1,1,1]); hold off; title(['Swarm, Step: ' num2str(becky),... ' Number of particles: ', num2str(NumParticles),... ' Number of clusters: ', num2str(NumClusters)]); xlabel('x_1'); ylabel('x_2'); drawnow; % Move particles to within the joint angle limits Adj_Delta_X=any(Particles_new<repmat(X_range(:,1),1,NumParticles),1)|... any(Particles_new>repmat(X_range(:,2),1,NumParticles),1); Particles_adj(Adj_Delta_X)=true; for gabby=find(Adj_Delta_X) [Delta_X(:,gabby),Particles_new(:,gabby)]=PullStepInside3(Particles_new(:,gabby),Particles_J(gabby),... J_Grad(Particles_old(:,gabby)),Delta_X(:,gabby),J,J_Grad,Y_Grad,CostStepOpt); % note cost is based on Particles_old, but this isn't passed in Particles_J(gabby)=J(Particles_new(:,gabby)); end % test sufficient descent, testing the boundary ones can mess it up, so % exclude those points (two or more steps may not meet the condition % for one step). for gabby=find(~Adj_Delta_X) Particles_J_new=J(Particles_new(:,gabby)); %NOT an vector [Particles_new(:,gabby),Particles_J(gabby),Delta_X(:,gabby),... Particles_adj(gabby)]=ArmijoStep_1(... Particles_old(:,gabby),Particles_new(:,gabby),... Particles_J(gabby),J_Grad(Particles_old(:,gabby)),CostStepOpt); % will not cross boundary, so no need to check end hold on;plot(Particles_old(1,Particles_adj),Particles_old(2,Particles_adj),'ys');hold off;drawnow; if max(ParticleView)>NumParticles ParticleView(ParticleView>NumParticles)=[]; %#ok<SAGROW> end for etsuko=ParticleView; figure(ParticleViewBaseFig+etsuko); Grad_Cost1=J_Grad(Particles_old(:,etsuko)); Grad_Cost2=Grad_Cost1./max(sum(Grad_Cost1.^2,1).^.5,CostStepOpt.MinGrad_Cost); Grad_Out1=Y_Grad(Particles_old(:,etsuko)); Grad_Out2=Grad_Out1./max(sum(Grad_Out1.^2,1).^.5,CostStepOpt.MinGrad_Out); Delta_X1=-Grad_Cost2+(Grad_Out2.'*Grad_Cost2)*Grad_Out2; Delta_X2=CostStepOpt.Gamma_Cost*Delta_X1./max(sum(Delta_X1.^2,1).^.5,CostStepOpt.MinGrad_Final); plot(Particles_old(1,etsuko),Particles_old(2,etsuko),'bo',... cumsum([Particles_old(1,etsuko),Delta_X1(1)/CostStepOpt.MinGrad_Final],2).',cumsum([Particles_old(2,etsuko),Delta_X1(2)/CostStepOpt.MinGrad_Final],2).','r-+',... cumsum([Particles_old(1,etsuko),-Grad_Cost1(1)/CostStepOpt.MinGrad_Final],2).',cumsum([Particles_old(2,etsuko),-Grad_Cost1(2)/CostStepOpt.MinGrad_Final],2).','m-x',... cumsum([Particles_old(1,etsuko),Grad_Out1(1)/CostStepOpt.MinGrad_Final],2).',cumsum([Particles_old(2,etsuko),Grad_Out1(2)/CostStepOpt.MinGrad_Final],2).','g-^'); title(num2str(round(Particles_old(:,etsuko).'*100)/100)); legend({'Location','Actual Step','-Cost Grad','Output Grad'},'location','best') daspect([1,1,1]) figure(ParticleViewBaseFig+100+etsuko);hold on; if Particles_adj(etsuko) if (Particles_J_last(etsuko)-Particles_J(etsuko))>0 plot(becky,log10(Particles_J_last(etsuko)-Particles_J(etsuko)),'b.'); else plot(becky,log10(-(Particles_J_last(etsuko)-Particles_J(etsuko))),'bx'); end else if (Particles_J_last(etsuko)-Particles_J(etsuko))>0 plot(becky,log10(Particles_J_last(etsuko)-Particles_J(etsuko)),'r.'); else plot(becky,log10(Particles_J_last(etsuko)-Particles_J(etsuko)),'rx'); end end title('Log of the change in cost (blue increase, red decrease)') end % pause; if any(any(Particles_new<repmat(X_range(:,1)-BoundaryViolationAllowance,1,NumParticles),1)|... any(Particles_new>repmat(X_range(:,2)+BoundaryViolationAllowance,1,NumParticles),1)) disp(find(any(Particles_new<repmat(X_range(:,1),1,NumParticles),1)|... any(Particles_new>repmat(X_range(:,2),1,NumParticles),1))); Adj_Delta_X=any(Particles_new<repmat(X_range(:,1),1,NumParticles),1)|... any(Particles_new>repmat(X_range(:,2),1,NumParticles),1); for gabby=find(Adj_Delta_X) tempW1=Particles_new(:,gabby); fprintf('Initial Location: %.20g, %.20g\n',Particles_old(:,gabby)) fprintf('Initial motion: %.20g, %.20g\n',Delta_X(:,gabby)) fprintf('Next Location: %.20g, %.20g\n',tempW1) %find difference tempW2=tempW1-min(max(tempW1,X_range(:,1)),X_range(:,2)); fprintf('Boundary Violation: %.20g, %.20g\n',tempW2) tempW3=max(tempW2./Delta_X(:,gabby)); fprintf('Violation Scale: %.20g, %.20g\n',tempW2./Delta_X(:,gabby)) Delta_X_temp=(1-tempW3)*Delta_X(:,gabby); fprintf('Final motion: %.20g, %.20g\n',Delta_X_temp) fprintf('Final Location: %.20g, %.20g\n\n',Particles_new(:,gabby)) end fprintf('Did not get particle inside bound') end clear Adj_Delta_X gabby tempW1 tempW2 tempW3 % tempA and tempB are arrays of the particles in two different % directions ParticlesAll=[Particles_new, StationaryPoints]; tempA=repmat(reshape(ParticlesAll.',1,size(ParticlesAll,2),[]),size(Particles_new,2),1); tempB=repmat(reshape(Particles_new.',size(Particles_new,2),1,[]),1,size(ParticlesAll,2)); % The relative direction of any particle with respect to any other Direction_all=tempA-tempB; % Find the distance, the lower diag inf makes it so only the lower % indexed particle is labeled as violating the distance, prevents % double deletion Dist=sum(Direction_all.^2,3).^.5+... [tril(inf+zeros(size(Particles_new,2))), ... zeros(size(Particles_new,2),size(ParticlesAll,2)-size(Particles_new,2))]; % NOTE: there are computer science ways of doing this more efficiently, % but isn't needed at this scale for my purposes Dist_min=min(Dist,[],2); Particles_new(:,Dist_min<KillDist)=[]; Particles_old(:,Dist_min<KillDist)=[]; Particles_J(:,Dist_min<KillDist)=[]; Delta_X(:,Dist_min<KillDist)=[]; clear ParticlesAll tempA tempB Direction_all Dist Dist_min Delta_X_Sz=(sum(Delta_X.^2,1)).^.5; temp42=find(Delta_X_Sz<=SettledStepSize); for amy=temp42 %over the settled points % Need to add check if last cluster added made this one obsolete if NumClusters>0 %checking on first cluster can cause indexing problems Dist=sum((StationaryPoints-repmat(Particles_new(:,amy),... 1,size(StationaryPoints,2))).^2,1).^.5; Dist_min=min(Dist,[],2); % fprintf('\nMinimum Distance From Stationary Point: %g',Dist_min); if Dist_min<KillDist continue %removed this particle and move to next end end NumClusters=NumClusters+1; temp_t=toc; Clusters{NumClusters}=ClusterGenerate_4(Particles_new(:,amy),J,J_Grad,Y_Grad,ClusterFrmOpt); %#ok<SAGROW> Clusters{NumClusters}.genStartTime=temp_t; %#ok<SAGROW> StationaryPoints=... [StationaryPoints,Clusters{NumClusters}.x]; %#ok<AGROW> Clusters{NumClusters}.genElapsedTime=toc-Clusters{NumClusters}.genStartTime; %#ok<SAGROW> Clusters{NumClusters}.genStep=becky;%#ok<SAGROW> fprintf(['Generating Cluster %g, at itteration %g and ',... 'elapsed time %g min; Generation took %g min and contains %g points\n'],... NumClusters,becky,toc/60,(Clusters{NumClusters}.genElapsedTime)/60,... size(Clusters{NumClusters}.x,2)); if numel(Clusters{NumClusters}.y)>1 figure(20+NumClusters);clf; for jax=1:size(Clusters{NumClusters}.x,2) theta_temp=[Theta_o(P_3([0;Clusters{NumClusters}.x(:,jax)]));Clusters{NumClusters}.x(:,jax)]; plot3(Clusters{NumClusters}.y(jax)*[1,1],[0,[1,0]*P_1(theta_temp)],[0,[0,1]*P_1(theta_temp)],'b-o',... Clusters{NumClusters}.y(jax)*[1,1],[[1,0]*P_1(theta_temp),[1,0]*P_2(theta_temp)],[[0,1]*P_1(theta_temp),[0,1]*P_2(theta_temp)],'r-o',... Clusters{NumClusters}.y(jax)*[1,1],[[1,0]*P_2(theta_temp),[1,0]*P_3(theta_temp)],[[0,1]*P_2(theta_temp),[0,1]*P_3(theta_temp)],'g-o'); hold on; end xlabel('Output');ylabel('Radial');zlabel('Tangential'); view([26,18]);hold off;daspect([1,1,1]); end end Particles_new(:,temp42)=[]; Particles_J(:,temp42)=[]; clear Delta_X_Sz temp42 amy Delta_X if size(Particles_new,2)<1 break; end Particles_J_last=Particles_J; pause(PauseLength); end % xlabel('All Done Now, you can go back to what you were doing') fprintf(['Finished at itteration %g and ',... 'elapsed time %g min; Generated %g Clusters\n'],... becky,toc/60,NumClusters); figure(12); clf; hold on; contour(X1,X2,YY); contour(X1,X2,JJ); plot(StationaryPoints(1,:),StationaryPoints(2,:),'go','markerface','g') daspect([1,1,1]); hold off; title(['Swarm finished, Step: ' num2str(becky),... ' Number of particles: ', num2str(size(Particles_new,2)),... ' Number of clusters: ', num2str(NumClusters)]); xlabel('x_1'); ylabel('x_2'); drawnow; clear becky 
Generating Cluster 1, at itteration 6 and elapsed time 0.0665969 min; Generation took 0.00408574 min and contains 52 points Generating Cluster 2, at itteration 6 and elapsed time 0.0746527 min; Generation took 0.00305104 min and contains 19 points Generating Cluster 3, at itteration 11 and elapsed time 0.136372 min; Generation took 0.00432103 min and contains 11 points Generating Cluster 4, at itteration 12 and elapsed time 0.15531 min; Generation took 0.00475001 min and contains 57 points Generating Cluster 5, at itteration 32 and elapsed time 0.372232 min; Generation took 0.00433275 min and contains 45 points Generating Cluster 6, at itteration 96 and elapsed time 1.04032 min; Generation took 0.00812108 min and contains 15 points Finished at itteration 265 and elapsed time 2.82389 min; Generated 6 Clusters 

## Find output ranges for clusters and sort points by output

Y_Tips=zeros(2,size(Clusters,2)); X_Tips=zeros(size(Particles_intial,1),size(Clusters,2),2); J_Tips=zeros(2,size(Clusters,2)); for marsha=1:length(Clusters) [Clusters{marsha}.y, temp3]=sort(Clusters{marsha}.y); %#ok<SAGROW> Clusters{marsha}.x=Clusters{marsha}.x(:,temp3);%#ok<SAGROW> Clusters{marsha}.j=Clusters{marsha}.j(:,temp3);%#ok<SAGROW> Y_Tips(:,marsha)=[Clusters{marsha}.y(1),Clusters{marsha}.y(end)].'; X_Tips(:,marsha,1)=Clusters{marsha}.x(:,1); X_Tips(:,marsha,2)=Clusters{marsha}.x(:,end); J_Tips(:,marsha)=[Clusters{marsha}.j(:,1),Clusters{marsha}.j(:,end)].'; end clear marsha temp3 

## Remove solitary clusters, I'm not going to use them

RemoveClust=false(size(Clusters)); for marsha=1:length(Clusters) if size(Clusters{marsha}.x,2)<=1 RemoveClust(marsha)=true; end end Y_Tips(:,RemoveClust)=[]; X_Tips(:,RemoveClust,:)=[]; J_Tips(:,RemoveClust)=[]; % Clusters=Clusters{~RemoveClust};% Doesn't work, darn cell structure ClustersNew=cell(1,sum(~RemoveClust)); jones=find(~RemoveClust); for indy=1:length(ClustersNew) ClustersNew{indy}=Clusters{jones(indy)}; end Clusters=ClustersNew; clear marsha indy jones RemoveClusters ClustersNew 

## Plot Clusters

figure(3); clf; subplot(1,3,1); contour(X1,X2,YY); title('Output Function'); xlabel('\theta_1 (rad)'); ylabel('\theta_2 (rad)');% colorbar; daspect([1,1,1]); subplot(1,3,2); contour(X1,X2,JJ); title('Cost Function'); xlabel('\theta_1 (rad)'); ylabel('\theta_2 (rad)');% colorbar; daspect([1,1,1]); subplot(1,3,3); cla; hold on; contour(X1,X2,YY); contour(X1,X2,JJ); hold off; title('Both Functions Overlayed'); xlabel('\theta_1 (rad)'); ylabel('\theta_2 (rad)');% colorbar; daspect([1,1,1]); % plot in X space to verify figure(2223); ColorSet=colormap(hsv(max(length(Clusters)))); Markers='so+x^'; clf; hold on; contour(X1,X2,YY); contour(X1,X2,JJ); for marsha=1:length(Clusters) if size(Clusters{marsha}.x,2)>=1 plot(Clusters{marsha}.x(1,:),Clusters{marsha}.x(2,:),['-', Markers(mod(marsha-1,length(Markers))+1)],... 'color',ColorSet(marsha,:)); end end clear marsha ColorSet axis equal hold off; title('Optimal Inverse Functions'); xlabel('\theta_1 (rad)'); ylabel('\theta_2 (rad)');% colorbar; axis([X_range(1,:),X_range(2,:)]); % plot in Y space vs X and J figure(2323); clf; for uri=1:size(Particles_intial,1); subplot(size(Particles_intial,1)+1,1,uri);hold on;grid on; ylabel(['\theta_', num2str(uri)]); set(gca,'XTickLabel',[]); end subplot(size(Particles_intial,1)+1,1,1);hold on;grid on; title(['Optimal Inverse Functions, h_i' char(39), 's']) subplot(size(Particles_intial,1)+1,1,size(Particles_intial,1)+1);hold on;grid on; xlabel('y_d');ylabel('J'); ColorSet=colormap(hsv(max(length(Clusters)))); y_plot_min=inf;y_plot_max=-inf; j_plot_min=inf;j_plot_max=-inf; for marsha=1:length(Clusters) if size(Clusters{marsha}.x,2)>=1 for uri=1:size(Particles_intial,1) subplot(size(Particles_intial,1)+1,1,uri); plot(Clusters{marsha}.y,Clusters{marsha}.x(uri,:),['-', Markers(mod(marsha-1,length(Markers))+1)],... 'color',ColorSet(marsha,:)); end subplot(size(Particles_intial,1)+1,1,size(Particles_intial,1)+1); plot(Clusters{marsha}.y,Clusters{marsha}.j,['-', Markers(mod(marsha-1,length(Markers))+1)],... 'color',ColorSet(marsha,:)); if min(Clusters{marsha}.y)<y_plot_min y_plot_min=min(Clusters{marsha}.y); end if max(Clusters{marsha}.y)>y_plot_max y_plot_max=max(Clusters{marsha}.y); end if min(Clusters{marsha}.j)<j_plot_min j_plot_min=min(Clusters{marsha}.j); end if max(Clusters{marsha}.j)>j_plot_max j_plot_max=max(Clusters{marsha}.j); end end end for uri=1:size(Particles_intial,1) subplot(size(Particles_intial,1)+1,1,uri); axis([y_plot_min,y_plot_max,X_range(uri,1),X_range(uri,2)]) end subplot(size(Particles_intial,1)+1,1,size(Particles_intial,1)+1); axis([y_plot_min,y_plot_max,j_plot_min,j_plot_max]) clear marsha ColorSet 

## Test results

for marsha=1:length(Clusters) TestY_d=rand(1,NumTestPerClust)*... (Clusters{marsha}.y(end)-Clusters{marsha}.y(1))+Clusters{marsha}.y(1); TestX=interp1(Clusters{marsha}.y.',Clusters{marsha}.x.',TestY_d,... 'pchip','extrap').'; PervertsX=zeros(size(Clusters{marsha}.x,1),NumTestPerClust*NumTestPerPoint); for alice=1:NumTestPerClust PervertsX(:,NumTestPerPoint*(alice-1)+(1:NumTestPerPoint))=... PervertScale*repmat(max(Clusters{marsha}.x,[],2)-... min(Clusters{marsha}.x,[],2),1,NumTestPerPoint).*... (rand(size(Clusters{marsha}.x,1),NumTestPerPoint)-.5)+... PervertConstant*(rand(size(Clusters{marsha}.x,1),NumTestPerPoint)-.5)+... repmat(TestX(:,alice),1,NumTestPerPoint); end PervertsX=max(min(PervertsX,repmat(X_range(:,2),1,size(PervertsX,2))),... repmat(X_range(:,1),1,size(PervertsX,2))); TestY_a=Y(TestX); TestJ_a=J(TestX); TestJ_d=interp1(Clusters{marsha}.y.',Clusters{marsha}.j.',TestY_a,... 'pchip','extrap'); PervertsY_a=Y(PervertsX); PervertsJ_a=J(PervertsX); PervertsJ_d=interp1(Clusters{marsha}.y.',Clusters{marsha}.j.',PervertsY_a,... 'pchip','extrap'); Y_error=TestY_a-TestY_d; figure(TestBaseFigNum+marsha);clf; subplot(241) plot([min([TestY_d(:);TestY_a(:)]),max([TestY_d(:);TestY_a(:)])],... [min([TestY_d(:);TestY_a(:)]),max([TestY_d(:);TestY_a(:)])],'r-',... TestY_d,TestY_a,'b.') title('Output vs Desired') ylabel('y_a') xlabel('y_d') axis equal subplot(245) plot(TestY_d,Y_error,'b.') title('Error in output') ylabel('y_a-y_d') xlabel('y_d') subplot(242) plot(PervertsY_a,PervertsJ_a,'g.',TestY_a,TestJ_a,'b.') title('Cost of outputs') ylabel('J') xlabel('y') subplot(246) plot(PervertsY_a,PervertsJ_a-PervertsJ_d,'g.',TestY_a,TestJ_a-TestJ_d,'b.') title('Cost from expected Optimal') ylabel('J-J(y)') xlabel('y') subplot(122);hold on; contour(X1,X2,YY); contour(X1,X2,JJ); plot(PervertsX(1,:),PervertsX(2,:),'g.',TestX(1,:),TestX(2,:),'b.',... Clusters{marsha}.x(1,:),Clusters{marsha}.x(2,:),'r.') daspect([1,1,1]);axis tight hold off; xlabel('\theta_1'); ylabel('\theta_2'); end % clear marsha TestY_d TestX PervertsX alice TestY_a TestJ_a PervertsY_a % clear PervertsJ_a Y_error 

## Conclude

save('PSO_Function_ArmijoTest_20120405_3.mat')