solorb.m

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

%*******************************
% Simplicial package
% Solution display script
% Orbital transfer
% Author: Pierre Martinon
% ENSEEIHT-IRIT, UMR CNRS 5505
% 09/2006
%*******************************



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(1:ns,i)=fscanf(fid,'%f' ,ns); 
 p(1:nc,i)=fscanf(fid,'%f' ,nc); 
 u(1:m,i)=fscanf(fid,'%f',m); 
end 

%datasize = steps*(1+ns+nc+m)
%data(1:datasize) = fscanf(fid,'%f',datasize);
%offset = 1+ns+nc+m
%for i=1:steps
%  time(1,i)=data((i-1)*offset+1);
%  x(1:ns,i)=data((i-1)*offset+2:(i-1)*offset+1+ns)';
%  p(1:nc,i)=data((i-1)*offset+ns+2:(i-1)*offset+1+ns+nc)';
%  u(1:m,i)=data((i-1)*offset+ns+nc+2:(i-1)*offset+1+ns+nc+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 
 % datasize = steps*dimpsi
 % data(1:datasize) = fscanf(fid,'%f',datasize);
 % for i=1:steps
 %   psi(1:dimpsi,i) = data((i-1)*dimpsi+1:i*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)



%rescale final longitude
%cf(ns-2) = time(steps);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%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')
end
%title (['Control u',int2str(k)])
plot(time(:),u(k,:),'LineWidth',2)
ax=axis;
axis([time(1) time(steps) -1 1])
plot([time(1) time(steps)],[0 0],'k-')
zoom on;
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
title ('|u|')
plot(time(:),normu(:),'LineWidth',2)
ax=axis;
axis([time(1) time(steps) 0 1])
zoom on;

%%%%%%
%State
for k=1:ns
subplot(ns,4,4*(k-1)+1)
hold on
if (k==1)
title ('STATE')
end
%title (['State x',int2str(k)])
plot(time(:),x(k,:),'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;
end

%%%%%%%%
%Costate
for k=1:nc
subplot(nc,4,4*(k-1)+2)
hold on
if (k==1)
title ('COSTATE')
end
%title (['Costate p',int2str(k)])
plot(time(:),p(k,:),'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;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Optional graphs
%
% 1: switching function
% 2: hamiltonian
% 3: Trajectory (Gauss)
% 4: End

choice = 0

while (choice ~= 3)

choice = menu('Optional graphs','Switching function','Trajectory','End');  


switch choice


%%%%%%%%%%%%%%%%%%%%
%Switching function%
%%%%%%%%%%%%%%%%%%%%
case 1

if (dimpsi > 0)
figure
hold on
title ('SWITCHING FUNCTION','FontSize',16)
xlabel ('TIME','FontSize',16)
ylabel ('\psi','FontSize',16)
psicol='bkr'
for i=1:dimpsi
  plot(time(:),psifun(i,:),psicol(i),'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


%%%%%%%%%%%%%%%%%%%%
%Hamiltonian%
%%%%%%%%%%%%%%%%%%%%
%case 2
%
%if (houtput > 0)
%figure
%hold on
%title ('HAMILTONIAN','FontSize',16)
%xlabel ('TIME','FontSize',16)
%ylabel ('H','FontSize',16)
%  plot(time(:),hamiltonian(:),'g','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) 
%end






%%%%%%%%%%%%%%%%%%%%
%Trajectory (Gauss)%
%%%%%%%%%%%%%%%%%%%%
case 2

if m==2 then
%initial and terminal orbits
P0 = x(1,1);
ex0 = x(2,1);
ey0 = x(3,1);
Pf = cf0(1);
exf = cf0(2);
eyf = cf0(3);
for i=1:101
theta = (i-1) * 2 * pi / 100;
co = cos(theta);
si = sin(theta);
w = 1 + ex0 * co + ey0 * si;
aux = P0 / w; 
x1(i) = aux * co;
y1(i) = aux * si;
w = 1 + exf * co + eyf * si;
aux = Pf / w; 
x2(i) = aux * co;
y2(i) = aux * si;
end

figure;
hold on
plot(x1(:),y1(:),'k')
plot(x2(:),y2(:),'k')

%Transfer
for i=1:steps
co = cos(x(4,i));
si = sin(x(4,i));
w = 1 + x(2,i) * co + x(3,i) * si;
aux = x(1,i) / w; 
r1(i) = aux * co;
r2(i) = aux * si;
end
plot(r1(:),r2(:))
axis equal;
ax = axis;
plot([ax(1) ax(2)],[0 0],'k-')
plot([0 0],[ax(3) ax(4)],'k-')

zoom on;



else

P = x(1,:);
ex = x(2,:);
ey = x(3,:);
hx = x(4,:);
hy = x(5,:);
L = time;

W = 1+ex.*cos(L)+ey.*sin(L);
Z = hx.*sin(L)-hy.*cos(L);
C = 1+hx.^2+hy.^2;
r = zeros(3,steps);
r(1,:) = P.*( (1+hx.^2-hy.^2).*cos(L) + 2*hx.*hy.*sin(L) ) ./ (C.*W);
r(2,:) = P.*( (1-hx.^2+hy.^2).*sin(L) + 2*hx.*hy.*cos(L) ) ./ (C.*W);
r(3,:) = 2*P.*Z ./ (C.*W);

P0(1:steps) = P(1);
ex0(1:steps) = ex(1);
ey0(1:steps) = ey(1);
hx0(1:steps) = hx(1);
hy0(1:steps) = hy(1);

W0 = 1+ex0.*cos(L)+ey0.*sin(L);
Z0 = hx0.*sin(L)-hy0.*cos(L);
C0 = 1+hx0.^2+hy0.^2;
r0 = zeros(3,steps);
r0(1,:) = P0.*( (1+hx0.^2-hy0.^2).*cos(L) + 2*hx0.*hy0.*sin(L) ) ./ (C0.*W0);
r0(2,:) = P0.*( (1-hx0.^2+hy0.^2).*sin(L) + 2*hx0.*hy0.*cos(L) ) ./ (C0.*W0);
r0(3,:) = 2*P0.*Z0 ./ (C0.*W0);



%P1(1:steps) = P(steps);
%ex1(1:steps) = ex(steps);
%ey1(1:steps) = ey(steps);
%hx1(1:steps) = hx(steps);
%hy1(1:steps) = hy(steps);

P1(1:steps) = 42.165;
ex1(1:steps) = 0;
ey1(1:steps) = 0;
hx1(1:steps) = 0;
hy1(1:steps) = 0;


W1 = 1+ex1.*cos(L)+ey1.*sin(L);
Z1 = hx1.*sin(L)-hy1.*cos(L);
C1 = 1+hx1.^2+hy1.^2;
r1 = zeros(3,steps);
r1(1,:) = P1.*( (1+hx1.^2-hy1.^2).*cos(L) + 2*hx1.*hy1.*sin(L) ) ./ (C1.*W1);
r1(2,:) = P1.*( (1-hx1.^2+hy1.^2).*sin(L) + 2*hx1.*hy1.*cos(L) ) ./ (C1.*W1);
r1(3,:) = 2*P1.*Z1 ./ (C1.*W1);

figure
hold on
plot3(r(1,:),r(2,:),r(3,:),'LineWidth',2);
plot3(r0(1,:),r0(2,:),r0(3,:),'k','LineWidth',2);
plot3(r1(1,:),r1(2,:),r1(3,:),'g','LineWidth',2);
axis equal

end


end %switch choice

end %while 








%%%%%%%%%%%%%%%%%%%%%%%%
%Trajectory (Cartesian)%
%%%%%%%%%%%%%%%%%%%%%%%%
%case 3


%initial and terminal orbits
%r0 = x(1,1);
%rf = cf0(1);
%for i=1:101
%theta = (i-1) * 2 * pi / 100;
%co = cos(theta);
%si = sin(theta);
%x1(i) = r0 * co;
%y1(i) = r0 * si;
%x2(i) = rf * co;
%y2(i) = rf * si;
%end 

%figure;
%hold on
%plot(x1(:),y1(:),'k')
%plot(x2(:),y2(:),'k')

%Transfer
%for i=1:steps
%r1(i) = x(1,i) * cos(x(2,i));
%r2(i) = x(1,i) * sin(x(2,i));
%end
%plot(r1(:),r2(:))
%axis equal;
%ax = axis;
%plot([ax(1) ax(2)],[0 0],'k-')
%plot([0 0],[ax(3) ax(4)],'k-')
%zoom on;