clear
close all;
clear;
clc;
format long;
%% Schätzung kontinuierliche Strecke

% Messdaten
load('Frequenzgangsdaten_Strecke.mat')
%Totzeit
del=1.4689*10^-6;
%Daten für Optimierung
gfr1=idfrd(G1(1:end),2*pi*f(1:end),0);
%Optionen
focus='stability';
opt = oeOptions('Focus',focus,'InitialCondition','auto','Display','on','SearchMethod','lm');
%Schätzung kontinuierlich
sys=tfest(gfr1,2,0,'InputDelay',del,opt);
[y,fit1,x0]=compare(gfr1,sys);

%Herausrechnen der Totzeit
[B,A]=tfdata(sys);                                                                                                                      
 A=cell2mat(A);  
 B=cell2mat(B);
sys_s_ohnetot=tf(B,A)

%Plot Vergleich Daten System
figure('Name','Vergleich Messdaten mit geschätztem Modell','Color',[1 1 1])
bode(gfr1,sys)
set(cstprefs.tbxprefs,'FrequencyUnits','Hz')
 xlim([100 1*10^5]);
  xlabel('Frequenz {\it f }','Units','pixels','FontWeight','bold','FontSize',14);
 ylabel('Phase {\it\phi }(G(s))','FontWeight','bold','FontSize',14);
 h = findobj(gcf,'type','line');
 set(h,'linewidth',2);
 legend1=legend('Gemessener Frequenzgang',['Identifizierte Strecke im s-Bereich mit Gütekriterium G = ' num2str(fit1) '%'],'Übertragungsfunktion ohne Totzeit');
 set(legend1,'FontSize',14,'Location','southwest','linewidth',2);
set(findall(gcf,'Type','axes'),'FontSize',14,'LineWidth',2,'XColor','black','YColor','black')
grid on;
%% Diskretisierung
%Abtastrate
Ts=3*10^-6;

%Alle Diskretisierungen ungenau
sys_s_tustin=c2d(sys_s_ohnetot,Ts,'tustin');
sys_s_matched=c2d(sys_s_ohnetot,Ts,'matched');
sys_s_zoh=c2d(sys_s_ohnetot,Ts,'zoh');
sys_s_impulse=c2d(sys_s_ohnetot,Ts,'impulse');
sys_s_foh=c2d(sys_s_ohnetot,Ts,'foh');

%Plot Diskretisierung
figure('Name','Vergleich kont. Modell mit Diskretisierungen','Color',[1 1 1])
bode(sys_s_ohnetot,'b',sys_s_tustin,'r--',sys_s_matched,'g--',sys_s_zoh,'k--',sys_s_impulse,'c--',sys_s_foh,'y--')
set(cstprefs.tbxprefs,'FrequencyUnits','Hz')
 xlim([100 1*10^5]);
  xlabel('Frequenz {\it f }','Units','pixels','FontWeight','bold','FontSize',14);
 ylabel('Phase {\it\phi }(G(s))','FontWeight','bold','FontSize',14);
 h = findobj(gcf,'type','line');
 set(h,'linewidth',2);
 legend1=legend('Strecke kont.','Tustin','Matched','ZOH','Impulse','FOH');
 set(legend1,'FontSize',14,'Location','southwest','linewidth',2);
set(findall(gcf,'Type','axes'),'FontSize',14,'LineWidth',2,'XColor','black','YColor','black')
grid on;

%Invertierung
sys_inv1=1/(sys_s_tustin); %instabil! Durch diskretisierung entstehen Nullstellen bei z=1.
figure('Name','Invertierte diskrete Strecke, instabil!','Color',[1 1 1])
step(sys_inv1)
 h = findobj(gcf,'type','line');
 set(h,'linewidth',2);
 legend1=legend('Sprungantwort der diskretisierten invertierten Strecke');
 set(legend1,'FontSize',14,'Location','southwest','linewidth',2);
set(findall(gcf,'Type','axes'),'FontSize',14,'LineWidth',2,'XColor','black','YColor','black')

%% Schätzung invertierte Strecke

%Schätzung der invertierten Strecke ist zu ungenau sowohl diskret als auch
%kontinuierlich:
G_inv=1./G1;
%Frequenzdatan kont. und dis.
gfr_kont=idfrd(G_inv(1:end),2*pi*f(1:end),0);
gfr_dis=idfrd(G_inv(1:end),2*pi*f(1:end),Ts);

%Modellschätzung mit verschiedenen Funktionen
for np=1:4 %Ordnung Pole
    for nz=1:4 %Ordnung Nullstellen
        sys_inv_tfest=tfest(gfr_kont,np,nz,opt)
        compare(gfr_kont,sys_inv_tfest)
        
        sys_inv_oe=oe(gfr_dis,[np nz 0],opt)
        compare(gfr_dis,sys_inv_oe)
        
        sys_inv_poly=polyest(gfr1,[np nz+1 0 0 0 0],opt)
        compare(gfr_dis,sys_inv_poly)
    end
end
%% Schätzung diskrete Strecke
%Funktion muss bei Invertierung Stabil sein!! Invertertierte Strecke soll
%bis 100kHz genau (delta Mag <0.6 dB delta Phi< 5 deg ) sein Koeffizienten
%sollten am besten kleinter 6 sein und Funktion darf nicht schwingen
%Optionen
focus='prediction';
opt = oeOptions('Focus',focus,'InitialCondition','auto','Display','on');

%Daten für Optimierung (Schätzung aus kont. Strecke, da ohne totzeit)
[magS, phiS]=bode(sys_s_ohnetot,2*pi*f)
G1=squeeze(magS).*exp(i*squeeze(phiS)/180*pi);

%Suche nach Funktionen ohne Nullstellen, zu ungenau
gfr2=idfrd(G1(1:end),2*pi*f(1:end),Ts);
for np=1:4
    for nz=0
        sys_dis_oe=oe(gfr2,[np nz 0],opt)
        compare(gfr2,sys_dis_oe)

        sys_dis_arx=arx(gfr2,[np nz 0],opt)
        compare(gfr2,sys_dis_arx)
        
        sys_dis_poly=polyest(gfr2,[np nz+1 0 0 0 0],opt)
        compare(gfr2,sys_dis_poly)    
        
    end
end
%% Schätzung diskrete Strecke2

%Suche nach Funktionen mit Nullstellen in linker halbgebene, Einschränkung
%des Suchraums nur bedingt möglich, zu ungenau
for b1=0.09;%0.01:0.01:0.99
    for np=1:4
        for nz=1:4
            %Startfunktion: Pole sollen frei wählbar sein, Grenzen für
            %Nullstellen (können nicht direkt vorgegeben werden?!) müssen in der linken Halbebene liegen, Stabilität der Pole unwesentlich 
            %Durch die Vorgabe vom ersten Koeffizienten b1 werden alle reelen Nullstellen negativ,
            %funktioniert bei konj, komplexen Nullstellen nicht
            sys0=idtf([b1 zeros(1,nz)],[1 zeros(1,np)])% Startfunktion
            sys0.Structure.num.Minimum=-abs(b1)+0.001;
            sys0.Structure.num.Maximum=abs(b1)-0.001;
            sys0.Structure.num.Free(1)=0;
            %Modellschätzung
            sys_dis_oe=oe(gfr2,sys0,opt)
            [y,fit1,x0]=compare(gfr2,sys_dis_oe);
            y=step(1/sys);
            [b a]=tfdata(1/sys_dis_oe);
            b=cell2mat(b);a=cell2mat(a);
            if fit1>= 88 %Modell steigt Sollwert für Gütekriterium               
                     compare(gfr2,sys_dis_oe)
                     if max(abs(y))<12 && max(abs(b))<12; %invertiertes Modell ist Stabil (geringes Überschwingen) und hat keine großen Koeffizienten
                        eval(['sys_' num2str(b1*100) '_' num2str(np) '_' num2str(nz) '=sys']); %Modell speichern, welches Anforderungen erfüllt
                     end
            end
        end
    end
end