// Functions       : FitPID
// 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
// A step includes a wide range of frequencies and is therefore a good choice.
// You may have to subtract the steady state first.
 
// 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: is the proportional gain
//     tn: is the integral time
//     kv: is the derivative gain
//     tv: is the derivative time

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

function [G,kp,tn,kv,tv]=FitPID(x,y)//Transfer-function of a PID-T1 Controller kp*(1+s+s^2)/(s+s^2)
    A=[];x1=0;y1=0;x2=0             
    for k=1:length(x), 
        x1=x1+x(k)
        x2=x2+x1
        y1=y1+y(k)
        A($+1,:)=[x2,x1,x(k),y1]
    end
    X= A \ y'
    tv=-1/X(4)
    tn= (X(2)-X(1)*tv)/X(1)
    kp= X(1)*tv*tn
    kv= (X(3)-X(1)*tv*tn)/(X(1)*tn)/tv
    G= syslin('c',(X(1)+X(2)*%s+X(3)*%s^2)*1,-X(4)*%s+%s^2) 
endfunction

//Demo 

//Data generation to simulate PID-T1 Controller

len=999
x=ones(1,len);                  //Create step 
Kp=1;Tn=400;Kv=3;Tv=10
G = syslin('c',1,Tn*%s)         //Integral-Element
y=csim('step',[1:1:len],G)      //Create a step response 
G = syslin('c',Kv*Tv*%s,1+Tv*%s)//DT-Element
y=y+csim('step',[1:1:len],G)    //Create a step response 
y=(y+x)*Kp                      //Proportional-Element
y=y+grand(x, "nor",0,0.04*1)    //Add a little randomness

//Fitting

[G1,kp,tn,kv,tv]=FitPID(x,y)//Fit this (1+s+s^2)/(s+s^2) transfer-function
G1=clean(G1, 1e-7)
disp('kp=' + string(kp),'tn=' + string(tn),'kv=' + string(kv),'tv=' + string(tv),G1)

//Plotting results

figure(1)
subplot(3,1,1)
title("step response  PID-T1 Controller", "fontsize",3)
a=gcf()
a.background=8
xstring( 30, 2, prettyprint(G1) )
h=get("current_entity") // Get the newly created object
h.font_size=3;
h.font_style=10;

t = [1:1:len] // Time range
plot(y,'r')   // Plot step response for unknown system
gca().children.children(1).thickness = 4 // Thickness of a curve
plot(csim('step',t,G1),'blue')
legend("Original Data", "Fitted Data",4,%t);
xlabel("Discrete Time Index n−−−−>")
ylabel("Step Response −->")

subplot(3,1,2)
title("frequency response  PID-T1 Controller", "fontsize",3)
xgrid(1)
ylabel(' Amplitude in db')
xlabel(' Frequency −−−−>')
frq=0.0004:0.0002:0.015*100   // 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  PID-T1 Controller", "fontsize",3)
xgrid(1)
ylabel(' Phase in degrees')
plot("ln",frq,phi,'blue')      // Plot phase of frequency response in degrees 
