%   Script FENSTERH.M
%
%  Einklemmschutz für Fensterheber
%
% J. Lunze
% 12.12.2002
% für 2. Auflage: 23.6.2007
% für 3. Auflage: 30.11.2011
% für 5. Auflage: 4.1.2020
%
%
clear
close all
%                   Parameter
%
RA=0.5;         % Ohm
J=13E-6;        % Nms^2/rad
kM=30E-3;       % Vs
kT=30E-3;       % Nm/A
kR=32.7E-6;     % Nms
LA=5E-4;        % H
igetr=40;
iseil=72;

TendSim=0.03;

%A=[-RA/LA  -km/LA   0;
%    km/J   -dF/J    0;
%    0       1       0];
%B=[1/LA; 0; 0];
%C=[0 1 0];
n=2;
A=[-RA/LA  -kM/LA;
    kT/J   -kR/J];
B=[1/LA; 0];
C=[0 1];
D=0;
Fensterheber=ss(A, B, C, D);


%%   Verhalten des Fensterhebers
%
figure(1)
uA=12;   % konstante Eingangsspannung 12 V
[Y, T]=step(Fensterheber, TendSim);
%
subplot(4,1,[1,2]);
plot(T, uA/2/pi*Y, 'b');
latextitle('\"Ubergangsfunktion des Fensterhebers');
latexxlabel('$$t$$ in s');
xticks([0 0.01 0.02 0.03]);
latexylabel('$$n$$ in $$\frac{1}{\rm s}$$');
rotateY;
yticks([0 20 40 60]);


%  Omega im stationären Zustand
omegas=dcgain(Fensterheber)*uA;
%  statinärer Zustand
xstat=-inv(A)*B*uA;

%%    Beobachterentwurf
%
figure(2)
LambdaB=[-500, -500];
LT=acker(A', C', LambdaB);
L=LT';
Ab=[A zeros(n,n); L*C A-L*C];
Bb=[B;B];
Cb=[0 1 0 -1];       % Differenz omega-omega^
Db=[0];
SystemBeobachter=ss(Ab, Bb, Cb, Db);
x0b=[xstat; 0; 0];
Y=initial(SystemBeobachter, x0b, T);
%
subplot(4,1,[1,2]);
plot(T, Y(:,1), 'b', [0 TendSim], [0 0], 'k:');
latexxlabel('$$t$$ in s');
latexylabel('$$\omega-\hat{\omega}$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
axis([0 TendSim -40 500]);
yticks([0 200 400]);
xticks([0 0.01 0.02 0.03]);
hold on
latextitle('Beobachtungsfehler f\"ur den Fensterheber');



%%    Fensterheber mit Blockierung
%
figure(3)
n=2;
B=[1/LA; 0];      % Eingang u1=Betriebsspannung
E=[0; -1/J];        % Eingang u2=zusätzliches Moment durch Gegenstand in Scheibe
FensterheberFehler=ss(A, [B E], C, [0 0]);
Tend=6;
dt=0.005;
kend=Tend/dt;
T=[0:dt:Tend]';
U=uA*ones(size(T));   % konstante Betriebsspannung
k1=4/dt;              % Fehler tritt zur Zeit 4 auf
k2=5/dt;
U=[U zeros(size(T))];
for k=0:k2-k1
    U(k1+k, 2)=0.5*k*dt/1;
end
for k=0:kend-k2+1
    U(k2+k, 2)=U(k2,2);
end
UF=U(:,1);
[YF, TF, XF]=lsim(FensterheberFehler, U, T);
%
subplot(4,1,[1,2]);
plot(TF, U(:,2), 'b', [-1, 0], [0, 0], 'b', [0 Tend],[0, 0],'k:');
xleer;
xticksempty([0 2 4 6 8]);
latexylabel('$$M_{\rm K}$$ in Nm');
rotateY;
latextitle('Verhalten beim Einklemmen');
axis([-0.5 5.5 -0.1 0.7]);
yticks([0 0.2 0.4 0.6]);
%
subplot(4,1,[3,4]);
plot(TF, YF, 'b', [-1, 0], [0, 0], 'b', [0 Tend],[0, 0],'k:');
axis([-0.5 5.5 -30 440]);
yticks([0 200 400 600]);
xticks([0 2 4 6 8]);
latexxlabel('$$t$$ in s');
latexylabel('$$\omega$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;



%%    Fehlerdiagnose mit Beobachter
%
%
figure(4)
%    Beobachter
AB=A-L*C;
BU=B;   %  Eingang: Betriebsspannung
BY=L;   %  Eingang: gemessene Winkelgeschwindigkeit des Motors
CB=-C;
DB=[0 1];  % Ausgang: y - y^
Beobachter=ss(AB, [BU, BY], CB, DB);
%  Beobachter erhält YF und UF als Eingänge
[YB, TB, XB]=lsim(Beobachter, [UF, YF], TF);
%
subplot(4,1,[3,4]);
plot(TB, YB, 'b', [-1, 0], [0, 0], 'b',[0 Tend],[0, 0],'k:');
latexylabel('$$r$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
axis([-0.5 5.5 -400 70]);
yticks([-400 -200 0]);
xticks([0 1 2 3 4 5]);
latexxlabel('$$t$$ in s');
%
subplot(4,1,[1,2]);
plot(TB, XB(:,2), 'b', TB, XF(:,2),'b--', [-1, 0], [0, 0], 'b', [0 Tend],[0, 0],'k:')
latextitle('Diagnose des Fensterhebers');
axis([-0.5 5.5 -30 440]);
yticks([0 200 400]);
latextext(5, 350, '$$\hat{\omega}$$')
latextext(5, 200, '$$\omega$$');
latexylabel('$$\omega$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
xleer
%
%  Residuenauswertung
%
subplot(4,1,[3,4]);
rbar=-60;
hold on
plot([-0.5 Tend],[rbar, rbar],'b--');
hold off
latextext(2, -120, '$$\bar{r}=60$$');


%%    Beobachtungsergebnis bei stochastisch gestörten Messgrößen
%
%
figure(5)
YFs=YF+10*randn(size(YF));
%
subplot(4,1,[1,2]);
plot(TF, YFs, 'b', [-1, 0], [0, 0], 'b', [0 Tend],[0, 0],'k:')
latextitle('Diagnose des Fensterhebers bei Messrauschen');
latexylabel('$$\omega$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
xleer
axis([-0.5 5.5 -30 440]);
yticks([0 200 400]);
[YBs, TBs, XBs]=lsim(Beobachter, [UF, YFs], TF);
%
subplot(4,1,[3,4]);
plot(TBs, YBs, 'b', [-1, 0], [0, 0], 'b', [0 Tend],[0, 0],'k:');
%xleer;
latexylabel('$$r$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
axis([-0.5 5.5 -400 70]);
yticks([-400 -200 0]);
xticks([0 1 2 3 4 5 6]);
%
%subplot(6,1,[5,6]);
%plot(TBs, XBs(:,2), 'b', TB, XF(:,2), 'b', [-1, 0], [0, 0], 'k:', [0 Tend],[0, 0],'k:')
%axis([-0.5 5.5 -40 450]);
%yticks([]);
%xticks([]);
latexxlabel('$$t$$ in s');
%
%  Residuenauswertung
hold on
plot([-0.5 Tend],[rbar, rbar],'b--');
hold off


%%   Beobachtungsergebnis bei stochastischer Störung durch
%    Straßenunebenheiten
%
figure(6)
Ed=[0; -1/J];        % Eingang u3=zusätzliches Moment durch Straßenunebenheiten
FensterheberFehlerUneben=ss(A, [B E Ed], C, [0 0 0]);
U(:,3)=0.07*randn(size(U(:,2)));
[YFu, T, XFu]=lsim(FensterheberFehlerUneben, U, T);
%
subplot(4,1,[1,2]);
plot(T, YFu, 'b', [-1, 0], [0, 0], 'b', [0 Tend],[0, 0],'k:')
latextitle('Diagnose des Fensterhebers bei Strassenunebenheiten');
latexylabel('$$\omega\;\;$$');   %   in $$\frac{\rm rad}{\rm s}$$
rotateY;
xleer
axis([-0.5 5.5 -30 440]);
yticks([0 200 400]);
[YBu, T, XBu]=lsim(Beobachter, [UF, YFu], T);
%
subplot(4,1,[3,4]);
plot(T, YBu, 'b', [-1, 0], [0, 0],  'b', [0 Tend],[0, 0],'k:');
latexylabel('$$r \;\;$$');   %  in $$\frac{\rm rad}{\rm s}$$
rotateY;
axis([-0.5 5.5 -400 70]);
yticks([-400 -200 0]);
xticks([0 1 2 3 4 5 6]);
latexxlabel('$$t$$ in s');
%
%  Residuenauswertung
hold on
plot([-0.5 Tend],[rbar, rbar],'b--');
hold off


%%     Bilder in .EPS-Dateienj

%
Epsbildc15('fenst');
