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.
    //

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

    //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);
    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),'*')+1
    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),'*')+1
    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