// Functions       : FitPT2DT
// Author          : Josef Schuetzenberger - Austria
// License         : Public Domain
// Version         : 1.0
// Date            : 2020-08-11
// Release         : http://fileexchange.scilab.org/toolboxes/585000

// Supported Scilab Version: 6.1

// DESCRIPTION
// Numerically computing (fitting) a Transfer Function to a measured response(y) 
// of an unknown system. The input function(x) could be a step, impulse or whatever.
//
// NOTES
// On the odd occasion this type of Plant Transfer Function is obtained through a step response test.
//
// You may have to subtract the steady state first and
// shift the data(y) backwards to compensate a time-delay in the response 
 
// PARAMETERS:
//   Inputs:
//     x: Vector of an impulse or step or slope or...
//     y: Vector of a response to the step-function or an impulse or... (Same length than x)
//
//   Outputs:   
//     G:  Transfer Function
//     kp: Proportional gain
//     t1, t2, t3: Time constants

clear() // Clear Variables
close() // Close graphic figures, progression bars or wait bars

function [G,kp,t1,t2,t3]=FitPT2DT(x,y) // Fits the Transfer Function kp*(1+s*t1)/(1+s*t2+s^2*t3)
    a=[];x1=0;y1=0;x2=0;y2=0
    for k=1:length(x), 
        x1=x1+x(k)
        x2=x2+x1
        y1=y1+y(k)
        y2=y2+y1
        a($+1,:)=[x2,x1,y2,y1]
    end
    X= a \ y'
    X=clean(X)
    kp= -X(1)/X(3)
    t1= X(2)/X(1)
    t2= X(4)/X(3)
    t3= -1/X(3)
    G= syslin('c',kp*(1+t1*%s),1+t2*%s+t3*%s^2);
    //G= syslin('c',(X(1)+X(2)*%s+)*1,-X(3)-X(4)*%s+%s^2);
endfunction

//Demo 

//Data generation to simulate a noisy system

len=999
x=ones(1,len) //Create step 
G = syslin('c',10*(1-40*%s),1+40*%s+5000*%s^2)  //1-s / 1+s+s^2
y=csim('step',[0:1:len-1],G)  // Create a step response for unknown system
y=y+grand(x, "nor",0,0.04*18) // Add randomness
y=y-0.001*y.*y.*y             // Distort the response


//Fitting                  

[G1,k,t1,t2,t3]=FitPT2DT(x,y)   // Fit to this transfer-function
G1=clean(G1, 1e-7)
disp(G1)

//Plotting results

figure(1)
subplot(3,1,1)
title("step response PT2DT", "fontsize",3)
a=gcf()
a.background=8
plot(y,'r')
xstring( 130, -2, prettyprint(G1) )
h=get("current_entity")        // Get the newly created object
h.font_size=3;
h.font_style=10;

t = [0:1:len] // Time range
plot(csim('step',t,G1),'blue')
xlabel("Discrete Time Index n−−−−>")
ylabel("Step Response −>")

legend("Original Data", "Fitted Data",4,%t);
gca().children.children(1).thickness = 4 // Thickness of a curve.
subplot(3,1,2)
title("frequency response PT2DT", "fontsize",3)
xgrid(1)
ylabel( ' Amplitude in db')
xlabel(' Frequency −−−−>')
frq=0.0004:0.0002:1.5         // Frequency range
rep=repfreq(G1,frq)           // Complex frequency response
[db,phi]=dbphi(rep)           
plot("ln",frq,db,'blue')      // Plot magnitude of frequency response in db 
subplot(3,1,3)
title("phase response PT2DT", "fontsize",3)
xgrid(1)
ylabel( ' Phase in degrees')
plot("ln",frq,phi,'blue')      // Plot phase of frequency response in degrees 
