%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%
%%% Institut fuer Regelungstechnik, TU Braunschweig
%%%
%%% Identifikation dynamischer Systeme
%%% Vergleich d. Identifikationsverfahren
%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all;

%%% Skalierung der Plots
xmin = 0; xmax = 1; ymin = 0; ymax = 4;

%%% Endzeit für Sprungantwort
Tstop = xmax;


%%% Strecke
V = 2;                       % Streckenverstärkung
f = 10;                      % Frequenz
w0 = 2*pi*f;                 % Kreisfrequenz
D = 0.1;                     % Dämpfung
G = tf(V,[1/w0^2 2*D/w0 1]); % ÜF der Strecke
T = 1/(10*f);                % Abtastzeit


%%% Ein- und Ausgangssignale
N = 2500;                    % lange Länge
t = [0:T:(N-1)*T]';          % Zeitvektor
uo = ones(N,1);              % Sprunganregung
yo = lsim(G,uo,t);           % Sprungantwort


%%% PRBS-Signal
up = 2*(prbs(9,N)-0.5);     % PRBS-Anregung
yp = lsim(G,up,t);           % Ausgangssignal mit PRBS-Anregung


% Ausgangsstörung
figure(1)
plot(t,yp,'r')
hold on
%d = 0*randn(size(up));       % keine Ausgangsstörung
%d = 0.2*randn(size(up));     % geringe Ausgangsstörung
%d = 0.5*randn(size(up));     % mittlere Ausgangsstörung
d = 1*randn(size(up));       % große Ausgangsstörung
%d = 4*randn(size(up));       % sehr große Ausgangsstörung
%d = 8*randn(size(up));       % extrem große Ausgangsstörung
plot(t,d,'b')
title('Störung')
legend('y_p ungestört','Störung d')
yp = yp+d;                   % gestörter Ausgang



%%% ===============
%%% Identifizierung
%%% ===============
n = 2;                       % Ordnung der zu identifizierenden Strecke

z = iddata(yp,up,T);


%%% ARX
%%% ===
na = n;                      % Ordnung A(q) - Nenner Strecke
nb = n;                      % Ordnung B(q) - Zähler Strecke
nk = 1;                      % Verschiebung
order = [na nb nk];
m = arx(z,order);
[A,B] = polydata(m)
GARX = tf(B,A,T)

figure(2)
step(G,'b',Tstop); hold on
step(GARX,'r',Tstop)
title('ARX=LS')
axis([xmin xmax ymin ymax])


%%% ARMAX
%%% =====
na = n;                      % Ordnung A(q) - Nenner Strecke
nb = n;                      % Ordnung B(q) - Zähler Strecke
nc = n;                      % Ordnung C(q) - Rauschmodell
nk = 1;                      % Verschiebung
order = [na nb nc nk];
m = armax(z,order);
[A,B] = polydata(m);
GARMAX = tf(B,A,T);

figure(3)
step(G,'b',Tstop); hold on
step(GARMAX,'r',Tstop)
title('ARMAX=GLS')
axis([xmin xmax ymin ymax])

%%%  ARARMAX
%%%  =======
na = n;                      % Ordnung A(q) - Nenner Strecke
nb = n;                      % Ordnung B(q) - Zähler Strecke
nc = n;                      % Ordnung C(q) - Rauschmodell
nd = n;                      % Ordnung D(q) - Rauschmodell
nf = 0;                      % für ARARMAX=0
nk = 1;                      % Verschiebung
order = [na nb nc nd nf nk];
m = pem(z,order);
[A,B] = polydata(m);
GARARMAX = tf(B,A,T);

figure(4)
step(G,'b',Tstop); hold on
step(GARARMAX,'r',Tstop)
title('ARARMAX=ELS')
axis([xmin xmax ymin ymax])


%%% BJ
%%% ==
nb = n;                      % Ordnung B(q) - Zähler Strecke
nc = n;                      % Ordnung C(q) - Rauschmodell
nd = n;                      % Ordnung D(q) - Rauschmodell
nf = n;                      % Ordnung A(q) - Nenner Strecke
nk = 1;                      % Verschiebung
order = [nb nc nd nf nk];
m = bj(z,order);
[A,B,C,D,F] = polydata(m);
GBJ = tf(B,F,T);

figure(5)
step(G,'b',Tstop); hold on
step(GBJ,'r',Tstop)
title('BJ')
axis([xmin xmax ymin ymax])

%%% IV
%%% ==
na = n;                      % Ordnung A(q) - Nenner Strecke
nb = n;                      % Ordnung B(q) - Zähler Strecke
nk = 1;                      % Verschiebung
order=[na nb nk];
m=iv4(z,order);
[A,B]=polydata(m);
GIV=tf(B,A,T);

figure(6)
step(G,'b',Tstop); hold on
step(GIV,'r',Tstop)
title('IV')
axis([xmin xmax ymin ymax])


%%% CorrLS
%%% ======
cuu = xcorr(up,'biased');    % Autocorrelation Eingang up
cuy = xcorr(yp,up,'biased'); % Kreuzcorrelation Eingang, Ausgang
zCorrLS = iddata(cuy,cuu,T);
na = n;                      % Ordnung A(q) - Nenner Strecke
nb = n;                      % Ordnung B(q) - Zähler Strecke
nk = 1;                      % Verschiebung
order = [na nb nk];
m = arx(zCorrLS,order);
[A,B] = polydata(m);
GCorrLS = tf(B,A,T);

figure(7)
step(G,'b',Tstop); hold on
step(GCorrLS,'r',Tstop)
title('CorrLS')
axis([xmin xmax ymin ymax])



%%% Übersicht
%%% =========
figure(8)
step(G,'k',Tstop); hold on
step(GARX,'y',Tstop)
step(GARMAX,'c',Tstop)
step(GARARMAX,'b',Tstop)
step(GBJ,'r',Tstop)
step(GIV,'g',Tstop)
step(GCorrLS,'m',Tstop)
title('Vergleich')
axis([xmin xmax ymin ymax])
legend('original','ARX','ARMAX','ARARMAX','BJ','IV','CorrLS')