clear();

pa=1.2929002
pb=5.6333807
pc=3069.5433

u_begrenzt_aktuell = 0;  //global, wird in modell und simulation benötigt.

exec einachsermodell.sce;
exec ruku.sce;

//Rauschen:
MITTELWERT = 0.0;
STREUUNG   = 0.2;

exec simulation.sce
exec fehlerfunktion.sce
exec optimierer.sce

P_ANTEIL=25000.0;
I_ANTEIL=1000.0;
D_ANTEIL=500.0;

x0=[P_ANTEIL;I_ANTEIL;D_ANTEIL];

xopt = meinoptimierer(berechneFehler,x0,x0.*0.01, x0.*100.0);

P_ANTEIL_OPT=xopt(1);
I_ANTEIL_OPT=xopt(2);
D_ANTEIL_OPT=xopt(3);

//Testsimulationen durchführen:

y0=[0.2;0;0];
t=0:dt:1.5;

y=simulation(xopt,y0,t);

plot(t,y(1,:));

Code 0-1: hauptprogramm.sce

Agii=[   0, 1,    0;
         0, 0,    1;
         0,-pc/pa,-pb/pa];

Bgii=[0;0;1/pa];

function f = modell(t,yy)
    f = Agii*yy+Bgii*u_begrenzt_aktuell; 
endfunction

Code 0-2: einachsermodell.sce

function yneu=ruku(y,t,dt)
       k1=modell(t,y);
       k2=modell(t+0.5*dt,y+0.5.*dt.*k1);
       k3=modell(t+0.5*dt,y+0.5.*dt.*k2);
       k4=modell(t+dt,y+dt.*k3);
       yneu=y+dt.*(k1+2.*k2+2.*k3+k4)./6;
endfunction

Code 0-3: ruku.sce

function ygir_sim=simulation(x,y0,t)
    P_ANTEIL=x(1);
    I_ANTEIL=x(2);
    D_ANTEIL=x(3);

    zs=size(t);
    z=zs(2);

    t0 = 0;

    yalt = y0;
    ygir_sim = yalt

    u_begrenzt_aktuell = 0;
    u_merk = [0];

    index_umerk = 1;

    //Für die numerische Bestimmung der Zustandsgrößen
    phi       = y0(1);
    phi_alt   = y0(1);
    phi_integ = 0.0;    
    omega     = 0.0;

    for i=1:1:z-1
        takt = t(i);

        phi_alt = phi;
        phi = yalt(1) + grand(1, 1, "nor", MITTELWERT, STREUUNG);
;
        phi_integ = phi_integ + phi*dt;    
        omega = (phi - phi_alt)/dt;

        //Zeitschritt durchführen:
        yneu=ruku(yalt,takt,dt);
        ygir_sim=[ygir_sim,yneu];
        yalt=yneu;

        //Erst danach das Stellsignal aktualisieren:
        u_begrenzt_aktuell = -P_ANTEIL*phi - I_ANTEIL*phi_integ - D_ANTEIL*omega;
        if u_begrenzt_aktuell>480 then
            u_begrenzt_aktuell=480;
        end

        if u_begrenzt_aktuell<-480 then
            u_begrenzt_aktuell=-480;
        end
    end
endfunction

Code 0-4: simulation.sce

dt=0.01;
t=0:dt:1.5;
zs=size(t);
z=zs(2);

function err=berechneFehler(x)
    err=0.0;

    t=0:dt:3.0;


    y0=[0.2;0;0];
    ygir_sim=simulation(x,y0,t);
    err = err + ygir_sim(1,1:z)*ygir_sim(2,1:z)';

    y0=[0.0;0.2;0];
    ygir_sim=simulation(x,y0,t);
    err = err + ygir_sim(1,1:z)*ygir_sim(2,1:z)';

    y0=[-0.4;0;0];
    ygir_sim=simulation(x,y0,t);
    err = err + ygir_sim(1,1:z)*ygir_sim(2,1:z)';

    y0=[0.3;0.2;0];
    ygir_sim=simulation(x,y0,t);
    err = err + ygir_sim(1,1:z)*ygir_sim(2,1:z)';

endfunction

Code 0-5: fehlerfunktion.sce

function bestparam = meinoptimierer(fehlerfunktion,startparam,minparam,maxparam)
    x0 = startparam;
    deltax = (maxparam-minparam).*0.0001;
    xakt=x0;

    sx = size(x0);
    zx = sx(1);

    fehler_akt = fehlerfunktion(x0);

    for i=1:100
        k = grand(1, 1, "uin", 1, zx);
        xneu = xakt;
        if xneu(k)+deltax(k)<maxparam(k) & xneu(k)-deltax(k)>minparam(k) then
            xneu(k)=xneu(k)+deltax(k);
            fehler_neu_plus = fehlerfunktion(xneu);
            xneu = xakt;
            xneu(k)=xneu(k)-deltax(k);
            fehler_neu_minus = fehlerfunktion(xneu);
 
            if fehler_neu_plus<fehler_akt then
                fehler_akt=fehler_neu_plus;
                xakt(k)=xakt(k)+deltax(k);
                deltax(k) = deltax(k)*1.01;
            elseif fehler_neu_minus<fehler_akt then
                fehler_akt=fehler_neu_minus;
                xakt(k)=xakt(k)-deltax(k);
                deltax(k) = deltax(k)*1.01;
            else
                deltax(k) = deltax(k)*0.99;
            end
        end
    end    

    bestparam = xakt;
endfunction

Code 0-6: optimierer.sce

P_ANTEIL = 25009.874
I_ANTEIL = 980.10298 
D_ANTEIL = 495.63126

Code 0-7: Gefundene Parameter.