Scilab Function
eng

steadycos

File content

function [X,U,Y,XP]=steadycos(scs_m,X,U,Y,Indx,Indu,Indy,Indxp,param)
// NAME
// steadycos - Finds an equilibrium state of a general 
// dynamical system described by a scicos diagram

// CALLING SEQUENCE
//
// [X,U,Y,XP]=steadycos(scs_m,X,U,Y,Indx,Indu,Indy [,Indxp [,param ] ])
//
// scs_m: a Scicos data structure
// X: column vector. Continuous state. Can be set to [] if zero.
// U: column vector. Input. Can be set to [] if zero.
// Y: column vector. Output. Can be set to [] if zero.
// Indx: index of entries of X that are not fixed. If all can vary, set to 1:$
// Indu: index of entries of U that are not fixed. If all can vary, set to 1:$
// Indy: index of entries of Y that are not fixed. If all can vary, set to 1:$
// Indxp: index of entries of XP (derivative of x) that need not be zero. 
//        If all can vary, set to 1:$. Default [].
// param: list with two elements (default list(1.d-6,0))
//   param(1): scalar. Perturbation level for linearization; the following variation is used
//             del([x;u])_i = param(1)+param(1)*1d-4*abs([x;u])_i
//   param(2): scalar. Time t.
//

load SCI/macros/scicos/lib

[lhs,rhs]=argn(0)
IN=[];OUT=[];

[ierr,scicos_ver,scs_m]=update_version(scs_m)
if ierr<>0 then
  message("Can''t convert old diagram (problem in version)")
  return
end

// //check version
// current_version = get_scicos_version()
// scicos_ver = find_scicos_version(scs_m)
// 
// //do version
// if scicos_ver<>current_version then
//   ierr=execstr('scs_m=do_version(scs_m,scicos_ver)','errcatch')
//   if ierr<>0 then
//     error('Can''t convert old diagram (problem in version)')
//     return
//   end
// end

if rhs==7 then
  Indxp=[ ];param=list(1.d-6,0)
elseif rhs==8 then
  param=list(1.d-6,0)
elseif rhs==9 then
else
  error('wrong number of arguments. 7, 8 or 9 expected.')
end

for i=1:length(scs_m.objs)
  if typeof(scs_m.objs(i))=='Block' then  
    if scs_m.objs(i).gui=='IN_f' then
      scs_m.objs(i).gui='INPUTPORT';
      IN=[IN scs_m.objs(i).model.ipar]
    elseif scs_m.objs(i).gui=='OUT_f' then
      scs_m.objs(i).gui='OUTPUTPORT';
      OUT=[OUT  scs_m.objs(i).model.ipar]
    end
  end
end
IN=-sort(-IN);
if or(IN<>[1:size(IN,'*')]) then 
  error('Input ports are not numbered properly.')
end
OUT=-sort(-OUT);
if or(OUT<>[1:size(OUT,'*')]) then 
  error('Output ports are not numbered properly.')
end
//load scicos lib
load('SCI/macros/scicos/lib')
//compile scs_m
[bllst,connectmat,clkconnect,cor,corinv,ok]=c_pass1(scs_m);
if ~ok then
  error('Diagram does not compile in pass 1');
end
%cpr=c_pass2(bllst,connectmat,clkconnect,cor,corinv,'silent');
if %cpr==list() then 
  ok=%f,
end
if ~ok then
  error('Diagram does not compile in pass 2');
end 
sim=%cpr.sim;state=%cpr.state;
//
inplnk=sim.inplnk;inpptr=sim.inpptr;
outlnk=sim.outlnk;outptr=sim.outptr;ipptr=sim.ipptr;

ki=[];ko=[];nyptr=1;
for kfun=1:length(sim.funs)
  if sim.funs(kfun)=='output' then
    sim.funs(kfun)='bidon'
    ko=[ko;[kfun,sim.ipar(ipptr(kfun))]];

  elseif sim.funs(kfun)=='input' then
    sim.funs(kfun)='bidon'
    ki=[ki;[kfun,sim.ipar(ipptr(kfun))]];
    
  end
end
[junk,ind]=sort(-ko(:,2));ko=ko(ind,1);
[junk,ind]=sort(-ki(:,2));ki=ki(ind,1);

pointo=[];
for k=ko' 
  pointo=[pointo;inplnk(inpptr(k))]
end
pointi=[];
for k=ki'
  pointi=[pointi;outlnk(outptr(k))]
end
nx=size(state.x,'*');
nu=0; for k=pointi', nu=nu+size(state.outtb(k),'*'), end
ny=0; for k=pointo', ny=ny+size(state.outtb(k),'*'), end

if X==[] then X=zeros(nx,1);end
if Y==[] then Y=zeros(ny,1);end
if U==[] then U=zeros(nu,1);end
if param(1)==0 then param(1)=1.d-6;end
t=param(2)

ux0=[U(Indu);X(Indx)];
sindu=size(U(Indu),'*');sindx=size(X(Indx),'*');
[err,uxopt,gopt]=optim(cost,ux0)
U(Indu)=uxopt(1:sindu);
X(Indx)=uxopt(sindu+1:sindx+sindu);
state.x=X;
Uind=1
for k=pointi'
 state.outtb(k)=matrix(U(Uind:Uind+size(state.outtb(k),'*')-1),size(state.outtb(k)));
 Uind=size(state.outtb(k),'*')+1;
end
[state,t]=scicosim(state,t,t,sim,'linear',[.1,.1,.1,.1]);
XP=state.x;
Yind=1
for k=pointo'
 Y(Yind:Yind+size(state.outtb(k),'*')-1)=state.outtb(k)(:);
 Yind=size(state.outtb(k),'*')+Yind
end
endfunction

function [f,g,ind]=cost(ux,ind)
state;
X;
U;
X(Indx)=ux(sindu+1:sindx+sindu);
U(Indu)=ux(1:sindu);
state.x=X;
Uind=1
for k=pointi'
 state.outtb(k)=matrix(U(Uind:Uind+size(state.outtb(k),'*')-1),size(state.outtb(k)));
 Uind=size(state.outtb(k),'*')+1;
end
// state.outtb(pointi)=U;
[state,t]=scicosim(state,t,t,sim,'linear',[.1,.1,.1,.1]);
zer=ones(X);zer(Indxp)=0;xp=zer.*state.x;
Yind=1
for k=pointo'
 y(Yind:Yind+size(state.outtb(k),'*')-1)=state.outtb(k)(:);
 Yind=size(state.outtb(k),'*')+Yind
end
// y=state.outtb(pointo);
zer=ones(y);zer(Indy)=0;err=zer.*(Y-y);
f=.5*(norm(xp,2)+norm(err,2));

sys=lincos(scs_m,X,U,param)
g=xp'*[sys.B(:,Indu) sys.A(:,Indx)]-..
    err'*[sys.D(:,Indu) sys.C(:,Indx)];
endfunction