%   Script VENTILHUBF.M
%
%  Überwachung eines Stellgerätes
%  Fehler f1: Druckabfall über Membran
%  Fehler f2: Abfall des Versorgungsdruckes
%
% J. Lunze
% 10.6.2002
% für 2. Auflage: 16.6.2007
% für 3. Auflage: 7.1.2012
% für 4. Auflage: 4.11.2015
% für 5. Auflage: 11.01.2020
%
%
clear
close all
%                   Parameter
dt=0.01;
Tend=150;             %  Simulationshorizont
Tendfig=100;          %  Darstellungszeitraum
T=[0:dt:Tend-dt];       %  Zeitachse
kend=Tend/dt;
%
kR=1500;           % Ns/m     Reibung der Packung
kF=60000;           % N/m       Federkonstante
m=2.5;              % kg        Masse der Antriebsstange und des Kegels
AM=pi/4*0.2*0.2;    % m^2       Fläche der Membran
TD=1;               % s         Zeitkonstante des Druckaufbaus über der Membran
%
%  Stellglied mit Stellungsregler
%  Zustand x=[s sdot]'
%
%  Modell des Stellantriebs  p -> s
AS=[0      1;
   -kF/m -kR/m];
BS=[0;  AM/m];
CS=[1 0];
DS=0;
Stellantrieb=ss(AS, BS, CS, DS);
ksp=dcgain(Stellantrieb);
kN=1/ksp;                       % Normierung des Hubs s auf den Bereich -50%...50%
%
%  Modell mit korrigierter Statik, Beschreibung des Druckaufbaus
%  und P-Regler  p=-kPV(s-u)
%
%  Zustand = [pM s sdot]'

%%  Übergangsfunktion des geregelten Stellantriebs
%
figure(1)
subplot(4,1,[1,2]);
kPV=3; 
AV=[-1/TD     kPV/TD        0;
    0             0          1;
    -kN*AM/m   -kF/m        -kR/m];
BV=[-kPV/TD;  0;  0];              % Eingang u
CV=[0 1 0];                     % Ausgang s
kQs=0.166;
CVQ=kQs*CV;                      % Ausgang Qdot
DV=0;
EV1=[0; 0; kN*AM/m];          % Fehlereingang +EV1*f1
EV2=[1/TD; 0; 0];              % Fehlereingang   + EV2*f2
Stellglied=ss(AV, BV, CV, DV);
[Y, Ty]=step(0.1*Stellglied);
ksV=100*dcgain(0.1*Stellglied);
plot(Ty, 100*Y, 'b', [-10 10], [ksV ksV], 'k:');
%axis([min(T), max(T), 0, 10]);
%yticks([]);
%xticks([]);
latexxlabel('$$t$$ in s');
latexylabel('$$s$$ in \%');
rotateY;
axis([0 1.2 0 10]);
yticks([0 5 10]);
xticks([0 0.2 0.4 0.6 0.8 1 1.2 1.4]);
latextitle('Geregelter Stellantrieb bei $$u=10\%$$');


%% Füllstandsregelkreis
%
figure(2)
%  PI-Füllstandsregler
kP=4.5;
kI=0.8;
%
%   Modell des fehlerfreien Füllstandsregelkreises
%        Zustandsvektor=(h, pM, s, sdot, xr)'
A0=[0     1/3*CVQ   0;
    -kP*BV   AV   -kI*BV;
    1     0  0  0    0];
B0=[-1/3; 0;  0; 0; 0];             % Eingang: Störgröße Qddot
C0=[1  0  0  0  0;                 % y1=h (Füllstand in dm)
    0  0  1  0  0;                % y2=s (Hub des Stellgliedes in %)
    -kP 0 0 0 -kI];                % y3=u (Stellsignal des PI-Reglers in %)
D0=[0; 0; 0];
E01=[0; EV1; 0];                      % Fehlereingang für f1
E02=[0; EV2; 0];                   % Fehlereingang für f2
CU=[-kP 0 0 0 -kI];                 % u (Stellgröße)
DU=0;
Regelkreis=ss(A0, B0, C0, D0);
%
%   Störübergangsfunktion bei Qddot=0,05 [dm^3/s] = 3 dm^3/min
[YRk, TRk]=step(0.05*Regelkreis);
%
subplot(4,1,[1,2]);
plot(TRk, 10*YRk(:,1), 'b', [-10 70], [0 0], 'k:');
xleer;
xticksempty([0 20 40 60 80 100]);
latexylabel('$$h$$ in cm');
rotateY;
latextitle('St\"orverhalten des F\"ullstandsregelkreises bei $$\dot{Q}_{\rm d}=0,05 {\rm dm}^3/{\rm s}$$');
axis([0 70 -0.8 0.2]);
yticks([-0.5 0]);
%
subplot(4,1,[3,4]);
plot(TRk, 100*YRk(:,2), 'b', [-10 70], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$s$$ in \%');
rotateY;
axis([0 70 0 50]);
yticks([0 20 40]);
xticks([0 20 40 60 80 100]);


%%   Füllstandsregelkreis
%
figure(3)
% Verlauf der Störgröße
k1=0;
for k=1:kend/3
    k1=k;
    Qddot(k,1)=0.05;
end
k2=0;
for k=1:kend/3
    k2=k1+k;
    Qddot(k1+k,1)=0;
end
for k=1:kend/3
    Qddot(k2+k,1)=0.033;
end
Y0=lsim(Regelkreis, Qddot, T);
%
subplot(6,1,[1,2]);
plot(T, Qddot, 'b', [-10 Tendfig], [0 0], 'k:')
xleer
latexylabel('$$q_{\rm d}$$ in $$\frac{\rm dm^3}{\rm s}$$')
rotateY;
latextitle('Verhalten des F\"ullstandsregelkreises');
axis([0 Tendfig -0.01 0.06]);
yticks([0 0.05]);
%
subplot(6,1,[3,4]);
plot(T, 10*Y0(:,1), 'b', [-10 Tendfig], [0 0], 'k:');
xleer;
latexylabel('$$h$$ in cm');
rotateY;
axis([0 Tendfig -0.6 0.6]);
yticks([-0.5 0 0.5]);
%
subplot(6,1,[5,6]);
plot(T, 100*Y0(:,2), 'b', [-10 Tendfig], [0 0], 'k:');
hold on
% Vergleich mit der Stellgröße, die der PI-Regler berechnet
RegelkreisU=ss(A0, B0, CU, DU);
U0=lsim(RegelkreisU, Qddot, T);
plot(T, 100*U0, 'b--');               % Stellgröße in %
latexxlabel('$$t$$ in s');
latexylabel('$$s, u$$ in \%');
rotateY;
axis([0 Tendfig -30 60]);
yticks([-20 0 20 40 60]);
xticks([0 20 40 60 80 100]);
hold off



%%  Vergleich des Regelkreisverhaltens mit und ohne Fehler im Stellgerät
%   Fehler f1: Druckabfall über der Membran, Fehlereintritt zum Zeitpunkt t=0
%
figure(4)
%   Modell des Füllstandsregelkreises mit Fehler f1
%        Zustandsvektor=(h, pM, s, sdot, xr)'
AF=A0;
BF1=[B0 E01];         % Erweiterung des Eingangs um den Fehlereingang für f1
CF1=C0;               % y1=h; y2=s; y3=u
DF1=zeros(3,2);
RegelkreisF1=ss(AF, BF1, CF1, DF1);
F1=0.1*ones(size(Qddot));   % Fehlersignal
[YF1, T, XF1]=lsim(RegelkreisF1, [Qddot F1], T);
%
subplot(6,1,[1,2]);
plot(T, Qddot, 'b', [-10 Tendfig], [0 0], 'k:')
xleer
latexylabel('$$q_{\rm d}$$ in $$\frac{\rm dm^3}{\rm s}$$')
rotateY;
set(gca, 'YTick', [0, 0.05]);
latextitle('F\"ullstandsregelkreis mit (---) und ohne (- - -) Membranfehler');
axis([0 Tendfig -0.01 0.06]);
yticks([0 0.05]);
%
subplot(6,1,[3,4]);
plot(T, 10*Y0(:,1),'b--', T, 10*YF1(:,1), 'b', [-10 Tendfig], [0 0], 'k:');
xleer;
latexylabel('$$h$$ in cm');
rotateY;
set(gca, 'YTick', [-0.5 0.5]);
axis([0 Tendfig -0.6 0.6]);
yticks([-0.5 0 0.5]);
%
subplot(6,1,[5,6]);
plot(T, 100*Y0(:,2),'b--', T, 100*YF1(:,2), 'b', [-10 Tendfig], [0 0], 'k:');
%xleer;
latexylabel('$$s$$ in \%');
rotateY;
set(gca, 'YTick', [0 50]);
axis([0 Tendfig -30 60]);
yticks([0 50]);
% Darstellung der Stellgröße gestrichen, da Bilder mit vier Teilbildern
% nicht korrekt in eps-Bilder übersetzt werden: 30.12.2019, MATLAB2019b
% subplot(8,1,[7,8]);
% plot(T, 100*Y0(:,3),'b--', T, 100*YF1(:,3), 'b', [-10, Tendfig],[0,0],'k:');
% latexylabel('$$u$$ in \%');
% rotateY;
% set(gca, 'YTick', [0 50]);
 latexxlabel('$$t$$ in s');
% axis([0 Tendfig -30 60]);
% yticks([0 50]);
 xticks([0 20 40 60 80 100]);
% % Stellsignal, das im Regelkreis mit Ventil mit Fehler f1 auftritt:
% UF1=YF1(:,3);



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%    neue Parameter
%
Tendfig=50;


%%     Fehler f2: Schwankungen des Versorgungsdruckes
%
figure(5)
BF2=[B0 E02];         % Erweiterung des Eingangs um den Fehlereingang für f2
CF2=C0;              % y1=h; y2=s; y3=u
DF2=zeros(3,2);
RegelkreisF2=ss(AF, BF2, CF2, DF2);
TF2=7;          % Zeitkonstante der Druckschwankungen
Fehlermodell2=ss(-1/TF2, 1, 1, 0);
xF20=0.3;       % Fehleramplitude
Druckwelle=initial(Fehlermodell2, xF20, T);
F2=zeros(size(Qddot));   % Fehlersignal
T1=10;
k1=T1/dt;
T2=30;
k2=T2/dt;
for k=0:k2
    F2(k1+k)=Druckwelle(k+1);
end
for k=0:kend-k2
    F2(k2+k)=-Druckwelle(k+1);
end
[YF2, T, XF2]=lsim(RegelkreisF2, [Qddot F2], T);
%
subplot(6,1,[1,2]);
%plot(T, Qddot, 'b', [-10 Tendfig], [0 0], 'k:')
%ylabel('{\itQ}_d in dm^3/s');
%axis([0 Tendfig -0.01 0.06]);
plot(T, F2, 'b', [-10 Tendfig], [0 0], 'k:')
latexylabel('$$f_2$$');
rotateY;
set(gca, 'YTick', [-0.2 0.2]);
axis([0 Tendfig -0.4 0.4]);
yticks([-0.3 0 0.3]);
xleer
latextitle('F\"ullstandsregelkreis mit (---) und ohne (- - -) Fehler im Versorgungsdruck');
%
subplot(6,1,[3,4]);
plot(T, 10*Y0(:,1),'b--', T, 10*YF2(:,1), 'b', [-10 Tendfig], [0 0], 'k:');
xleer;
latexylabel('$$h$$ in cm');
rotateY;
set(gca, 'YTick', [-0.5 0.5]);
axis([0 Tendfig -0.6 0.3]);
yticks([-0.5 0]);
%
subplot(6,1,[5,6]);
plot(T, 100*Y0(:,2),'b--', T, 100*YF2(:,2), 'b', [-10 Tendfig], [0 0], 'k:');
latexylabel('$$s$$ in \%');
rotateY;
set(gca, 'YTick', [0 50]);
axis([0 Tendfig -10 50]);
yticks([0 50]);
% Darstellung der Stellgröße gestrichen, da Bilder mit vier Teilbildern
% nicht korrekt in eps-Bilder übersetzt werden: 30.12.2019, MATLAB2019b
% subplot(4,1,4);
% plot(T, 100*Y0(:,3),'b--', T, 100*YF2(:,3), 'b', [-10, Tendfig],[0,0],'k:');
% latexylabel('$$u$$ in \%');
% rotateY;
% set(gca, 'YTick', [0 50]);
 latexxlabel('$$t$$ in s');
% axis([0 Tendfig -10 70]);
% yticks([0 50]);
 xticks([0 10 20 30 40 50 60]);
% Stellsignal, das im Regelkreis mit Ventil mit Fehler f2 auftritt:
UF2=YF2(:,3);


%%   Beobachterentwurf
%  Beobachter für Ventil
%  Ventilzustand = (pM, s, sdot, f1, f2)
A=[   AV         EV1       EV2;
   zeros(1,3)     0         0;
   zeros(1,3)     0     -1/TF2];
B=[BV; 0; 0];
C=[CV 0 0];
D=0;
LambdaB=20*[-1, -1, -1, -1, -1];
LT=acker(A', C', LambdaB);
L=LT';
%       Beschreibung der Kopplung von Ventil und Beobachter
%       Zustand = (pM, s, sdot, f, pM^, s^, sdot^, f^)'
AB=A-L*C;
BB=[B L];         % 1. Eingang: u;  2. Eingang: y
CB=[0 0 0 1 0;            % geschätzes Fehlersignal f1
    0 1 0 0 0;            % geschätzter Hub s^
    0 0 0 0 1];             % geschätztes Fehlersignal f2
DB=zeros(3,2);
Beobachter=ss(AB, BB, CB, DB);

%% Fehlerdiagnose, wenn Fehler f1 zum Zeitpunkt t=0 eintritt
%
figure(6)
xB0=zeros(1, 5);
%                   Fehler tritt zum Zeitpunkt t=0 ein
%    Stellsignal wie vorher aus dem fehlerbehafteten Füllstandsregelkreis
[YBF1, T, XBF1]=lsim(Beobachter, [UF1 YF1(:,2)], T, xB0);
%
subplot(6,1,[1,2]);
plot(T, YBF1(:,1), 'b', T, F1,'b--', [0, Tendfig],[0,0],'k:');
xleer;
latexylabel('$$f_1$$');
rotateY;
latextitle('Fehlerdetektion f\"ur Fehler $$f_1$$');
axis([0 Tendfig -0.4, 0.4]);
yticks([-0.4 0 0.4]);
%
subplot(6,1,[3,4]);
plot(T, YBF1(:,3), 'b',  [0 Tendfig], [0 0], 'k:');
latexylabel('$$f_2$$');
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 10 20 30 40 50]);
axis([0 Tendfig -0.6 0.6]);
%
%subplot(6,1,[5,6]);
%plot(T, 100*YBF1(:,2), 'b', T, 100*YF1(:,2),'b--', [0 Tendfig], [0,0],'k:');
%ylabel('{\its}, {\its}_B in %');
rotateY;
%axis([0 Tendfig -30 60]);
%yticks([]);
%xticks([]);



%% Fehlerdiagnose für Fehler f2
%
figure(7)
xB0=zeros(1, 5);
[YBF2, T, XBF2]=lsim(Beobachter, [UF2 YF2(:,2)], T, xB0);
%
subplot(6,1,[1,2]);
plot(T, YBF2(:,1), 'b', [0, Tendfig],[0,0],'k:');
xleer;
latexylabel('$$f_1$$');
rotateY;
set(gca, 'YTick', [-0.2 0.2]);
latextitle('Fehlerdetektion f\"ur Fehler $$f_2$$');
axis([0 Tendfig -0.4 0.4]);
yticks([-0.3 0 0.3]);
%
subplot(6,1,[3,4]);
plot(T, YBF2(:,3), 'b', T, F2,'b--', [0 Tendfig], [0 0], 'k:');
latexylabel('$$f_2$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tendfig -0.6 0.6]);
yticks([-0.5 0 0.5]);
xticks([0 10 20 30 40 50 60]);
%
%subplot(6,1,[5,6]);
%plot(T, 100*YBF2(:,2), 'b', T, 100*YF2(:,2),'b--', [0 Tendfig], [0,0],'k:');
%ylabel('{\its, s}_B in %');
%axis([0 Tendfig -30 60]);
%yticks([]);
%xticks([]);



%% Fehlerdiagnose, wenn Fehler vor Beginn der Diagnose eintreten ist
%  (Diagnosebeginn zur Zeit t=20)
%
figure(8)
%       Beschreibung der Kopplung von Ventil und Beobachter
%       Zustand = (pM, s, sdot, f, pM^, s^, sdot^, f^)'
AB=[A zeros(5,5); L*C A-L*C];
BB=[B;B];
CB=[0 0 0 0 0 0 0 0 1 0;            % geschätzes Fehlersignal f1
    0 1 0 0 0 0 0 0 0 0;            % tatsächlicher Hub s
    0 0 0 0 0 0 1 0 0 0];            % geschätzter Hub s^
DB=[0; 0; 0];
SystemBeobachter=ss(AB, BB, CB, DB);
%
t0=Tendfig/3;                  % Fehlereintritt zur Zeit t0
k0=ceil(Tendfig/dt/3);               % zugehöriger Zeitindex
xT0=XF1(k0,2:4);             % Zustand des Ventils zur Zeit t0 aus Simulation
                            % des fehlerbehafteten Füllstandsregelkreises auslesen
                            % (pM(t0), s(t0), sdot(t0))
xT0(1,4)=0.1;               % Fehler f1 "einschalten"
xT0(1,5)=0;
xBT0=[xT0 zeros(1,5)];            % Beobachterzustand zur Zeit t0 hinzufügen
%    Stellsignal wie vorher, aber ab t=t0
TT0=T(k0:kend);
UT0=UF1(k0:kend);
[YB2, TT0, XB2]=lsim(SystemBeobachter, UT0, TT0, xBT0);
%
subplot(6,1,[1,2]);
plot(TT0, YB2(:,1), 'b', [t0 t0],[-100 100],'k:', [0 Tendfig], [0 0], 'k:');
xleer;
latexylabel('$$f_1$$');
rotateY;
latextitle(['Fehler seit $$t=0$$; Diagnose seit $$t_0=$$', num2str(t0)]);
axis([0 Tendfig -0.4 0.4]);
yticks([-0.3 0 0.3]);
%
subplot(6,1,[3,4]);
plot(TT0, 100*YB2(:,2), 'b', T, 100*YF1(:,2),'b--', [t0 t0],[-100 100],'k:', [0 Tendfig],[0 0],'k:');
xleer;
latexylabel('$$s$$ in \%');
rotateY;
axis([0 Tendfig -30 60]);
yticks([0 50]);
%
subplot(6,1,[5,6]);
plot(TT0, 100*UT0, 'b', T, 100*UF1,'b--', [t0 t0],[-100 100],'k:', [0 Tendfig], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$u$$ in \%');
rotateY;
axis([0 Tendfig -30 60]);
yticks([0 50]);
xticks([0 10 20 30 40 50 60]);


%%  Fehlerdiagnose, wenn Fehler vor Beginn der Diagnose eintreten ist
%  (Diagnosebeginn zur Zeit t=30)
%
figure(9)
t0=Tendfig/2;                  % Fehlereintritt zur Zeit t0
k0=ceil(Tendfig/dt/2);
xT0=XF1(k0,2:4);             % Zustand des Ventils zur Zeit t0 aus Simulation
                            % des fehlerbehafteten Füllstandsregelkreises auslesen
                            % (pM(t0), s(t0), sdot(t0))
xT0(1,4)=0.1;               % Fehler "einschalten"
xT0(1,5)=0;
xBT0=[xT0 zeros(1,5)];            % Beobachterzustand zur Zeit t0 hinzufügen
%    Stellsignal wie vorher, aber ab t=t0
TT0=T(k0:kend);
UT0=UF1(k0:kend);
[YB2, TT0, XB2]=lsim(SystemBeobachter, UT0, TT0, xBT0);
%
subplot(6,1,[1,2]);
plot(TT0, YB2(:,1), 'b', [t0 t0],[-100 100],'k:', [0 Tendfig], [0 0], 'k:');
xleer;
latexylabel('$$f_1$$');
rotateY;
latextitle(['Fehler seit $$t=0$$; Diagnose seit $$t_0=$$', num2str(t0)]);
axis([0 Tendfig -0.4 0.4]);
yticks([-0.3 0 0.3]);
%
subplot(6,1,[3,4]);
plot(TT0, 100*YB2(:,2), 'b', T, 100*YF1(:,2),'b--', [t0 t0],[-100 100],'k:', [0 Tendfig],[0 0],'k:');
xleer;
latexylabel('$$s$$ in \%');
rotateY;
axis([0 Tendfig -30 60]);
yticks([0 50]);
%
subplot(6,1,[5,6]);
plot(TT0, 100*UT0, 'b', T, 100*UF1,'b--', [t0 t0],[-100 100],'k:', [0 Tendfig], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$u$$ in \%');
rotateY;
axis([0 Tendfig -30 60]);
yticks([0 50]);
xticks([0 10 20 30 40 50 60]);


%%  Fehlerdiagnose, wenn Fehler vor Beginn der Diagnose eintreten ist
%  (Diagnosebeginn zur Zeit t=40)
%
figure(10)
t0=Tendfig/3*2;                  % Fehlereintritt zur Zeit t0
k0=ceil(Tendfig/dt/3*2);
xT0=XF1(k0,2:4);             % Zustand des Ventils zur Zeit t0 aus Simulation
                            % des fehlerbehafteten Füllstandsregelkreises auslesen
                            % (pM(t0), s(t0), sdot(t0))
xT0(1,4)=0.1;               % Fehler "einschalten"
xT0(1,5)=0;
xBT0=[xT0 zeros(1,5)];            % Beobachterzustand zur Zeit t0 hinzufügen
%    Stellsignal wie vorher, aber ab t=t0
TT0=T(k0:kend);
UT0=UF1(k0:kend);
[YB2, TT0, XB2]=lsim(SystemBeobachter, UT0, TT0, xBT0);
%
subplot(6,1,[1,2]);
plot(TT0, YB2(:,1), 'b', [t0 t0],[-100 100],'k:', [0 Tendfig], [0 0], 'k:');
xleer;
latexylabel('$$f_1$$');
rotateY;
set(gca, 'YTick', [-0.2 0.2]);
latextitle(['Fehler seit $$t=0$$; Diagnose seit $$t_0=$$', num2str(t0)]);
axis([0 Tendfig -0.4 0.4]);
yticks([-0.3 0 0.3]);
%
subplot(6,1,[3,4]);
plot(TT0, 100*YB2(:,2), 'b', T, 100*YF1(:,2),'b--', [t0 t0],[-100 100],'k:', [0 Tendfig],[0 0],'k:');
xleer;
latexylabel('$$s$$ in \%');
rotateY;
set(gca, 'YTick', [0 50]);
axis([0 Tendfig -30 60]);
yticks([0 50]);
%
subplot(6,1,[5,6]);
plot(TT0, 100*UT0, 'b', T, 100*UF1,'b--', [t0 t0],[-100 100],'k:', [0 Tendfig], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$u$$ in \%');
rotateY;
set(gca, 'YTick', [0 50]);
axis([0 Tendfig -30 60]);
yticks([0 50]);
xticks([0 10 20 30 40 50 60]);

%%     Bilder in .EPS-Dateien
%
Epsbildc15('ventilh');
