sol.m

一个基于打靶法的最优控制求解软件 求解过程中采用参数延续算法

%******************************* 
% Shoot Package v1.0
% Solution display script 
% Author: Pierre Martinon 
% INRIA FUTURS, team COMMANDS
% CMAP, ECOLE POLYTECHNIQUE
% 12/2007 
%******************************* 
 

clear all; 
%close all; 
format long; 
 
filename=input('Data file prefix: ','s') 
fid=fopen([filename '.sol']); 
 
%Parameters
text=fgets(fid)
npar = fscanf(fid,'%d',1) 
par = fscanf(fid,'%f',npar) 
 
%unknown, state, costate and control dimension 
blank=fgets(fid);
text=fgets(fid)
n = fscanf(fid,'%d',1) 
ns = fscanf(fid,'%d',1) 
nc = fscanf(fid,'%d',1) 
m = fscanf(fid,'%d',1) 

%terminal values
blank=fgets(fid);
text=fgets(fid)
fixedT = fscanf(fid,'%d',ns) 
blank=fgets(fid);
text=fgets(fid)
cf0 = fscanf(fid,'%f',ns) 
for k=1:ns+nc 
 cfind(k) = 0; 
 cf(k) = 0.0; 
end 
%for k=1:n
% if (fixedT(k) > 0) 
%  cfind(fixedT(k)) = 1; 
%  cf(fixedT(k)) = cf0(k); 
% end
%end 
 
%solution 
blank=fgets(fid);
blank=fgets(fid);
text=fgets(fid)
x0=fscanf(fid,'%f',n+1) 
 
%homotopy value 
blank=fgets(fid);
blank=fgets(fid);
text=fgets(fid) 
s=fscanf(fid,'%f',n) 
norms =fscanf(fid,'%f',1) 
 
%criterion
blank=fgets(fid);
blank=fgets(fid);
text=fgets(fid) 
dimcrit = fscanf(fid,'%d',1) 
crit = fscanf(fid,'%f',dimcrit) 
 
%optimal control and trajectory 
blank=fgets(fid);
blank=fgets(fid);
text=fgets(fid);
steps = fscanf(fid,'%d',1) 
 
for i=1:steps 
 time(1,i)=fscanf(fid,'%f',1); 
 x(:,i)=fscanf(fid,'%f' ,ns); 
 p(:,i)=fscanf(fid,'%f' ,nc); 
 u(1:m,i)=fscanf(fid,'%f',m); 
end 
 
%switching function psi 
blank=fgets(fid);
blank=fgets(fid);
text=fgets(fid)
dimpsi=fscanf(fid,'%f',1) 
if (dimpsi >0 ) 
 for i=1:steps 
  psifun(:,i) = fscanf(fid,'%f',dimpsi); 
 end 
end 

%hamiltonian
blank=fgets(fid);
blank=fgets(fid);
text=fgets(fid)
houtput=fscanf(fid,'%d',1)
if (houtput == 1 )
 for i=1:steps
  hamiltonian(i) = fscanf(fid,'%f',1);
 end
end
 
fclose(fid) 
 
 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%Generic solution: state, costate and control% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
figure 
 
%%%%%%%%%%%%%%%%%%% 
%Control components 
for k=1:m 
subplot(m+1,4,[4*(k-1)+3 4*(k-1)+4]) 
hold on 
if (k==1)
title('CONTROL','FontSize',16) 
end
xlabel(['Control u',int2str(k)],'FontSize',12) 
plot(time(:),u(k,:),'b','LineWidth',2) 
ax=axis; 
axis([time(1) time(steps) ax(3) ax(4)]) 
plot([time(1) time(steps)],[0 0],'k-') 
zoom on; 
set(gca,'FontSize',16) 
set(gca,'XTick',[])
end 
 
%%%%%%%%%%%%% 
%Control norm 
for i=1:steps 
  normu(i) = sqrt(u(:,i)' * u(:,i)); 
end 
subplot(m+1,4,[4*m+3 4*m+4]) 
hold on 
xlabel('||u||','FontSize',12) 
plot(time(:),normu(:),'b','LineWidth',2) 
ax=axis; 
axis([time(1) time(steps) 0 1]) 
zoom on; 
set(gca,'FontSize',16) 
set(gca,'XTick',[])

%%%%%% 
%State 
for k=1:ns 
subplot(ns,4,4*(k-1)+1) 
hold on 
if (k==1)
title('STATE','FontSize',16) 
end
%xlabel(['x',int2str(k)],'FontSize',12) 
plot(time(:),x(k,:),'b','LineWidth',2) 
if (cfind(k)==1) 
plot(time(steps),cf(k),'r+'); 
end 
ax=axis; 
axis([time(1) time(steps) ax(3) ax(4)]); 
zoom on; 
set(gca,'FontSize',16) 
set(gca,'XTick',[])
end 
 
%%%%%%%% 
%Costate 
for k=1:nc 
subplot(nc,4,4*(k-1)+2) 
hold on 
if (k==1)
title('COSTATE','FontSize',16) 
end
%xlabel(['p',int2str(k)],'FontSize',12) 
plot(time(:),p(k,:),'b','LineWidth',2) 
if (cfind(ns+k)==1) 
plot(time(steps),cf(ns+k),'r+'); 
end 
ax=axis; 
axis([time(1) time(steps) ax(3) ax(4)]) 
zoom on; 
set(gca,'FontSize',16) 
set(gca,'XTick',[])
end 
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 
%Optional graphs 
% 
% 1: switching function 
% 2: hamiltonian
% 3: End
 
choice = 0 
 
while (choice ~= 2) 
 
choice = menu('Optional graphs','Switching function','End');   
 
 
switch choice 
 
 
%%%%%%%%%%%%%%%%%%%% 
%Switching function% 
%%%%%%%%%%%%%%%%%%%% 
case 1 
 
if (dimpsi > 0) 
figure 
hold on 
title ('Switching function \psi','FontSize',16) 
xlabel ('Time','FontSize',16) 
ylabel ('\Psi','FontSize',16) 
psicol='brkbbb' 
for i=1:dimpsi 
  plot(time(:),psifun(i,:),psicol(i),'LineWidth',2) 
end 
ax=axis; 
axis([time(1) time(steps) ax(3) ax(4)]) 
zoom on;
set(gca,'FontSize',16) 
end 
 

%case 2
% 
%if (houtput > 0) 
%figure 
%hold on 
%title ('Hamiltonian','FontSize',16) 
%xlabel ('Time','FontSize',16) 
%ylabel ('H','FontSize',16) 
%for i=1:dimpsi 
%  plot(time(:),hamiltonian(:),'b','LineWidth',2) 
%end 
%ax=axis; 
%axis([time(1) time(steps) ax(3) ax(4)]) 
%plot([time(1) time(steps)],[0 0],'k-') 
%zoom on;
%set(gca,'FontSize',16) 
%end 
 
end %switch choice 
 
end %while