%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%
%%% Institut fuer Regelungstechnik, TU Braunschweig
%%%
%%% Identifikation dynamischer Systeme
%%% Least Square Identifikation
%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% Strecke G (System das identifiziert werden soll)
G=tf([1 -5],[1 0.5 1]);

%%% gewählte Abtastzeit
T=0.25;                  % siehe letzte Rechnerübung

%%% Sprungantwort
figure(1)
clf
step(G)

%%% Ordnug der zu identifizierenden Strecke
n = 2;


%%% Ordnung PRBS
np = 5;

%%% Taktzeit
k = 4;

%%% Länge
N = k*(2^np-1);

%%% Periodendauer
Tp = N*T;

%%% Perioden PRBS
p = 10;

%%% Amplitude PRBS
V = 1;

%%% Generierung PRBS
u = prbs(np,k,p,V,0);

%%% zugehöriger Zeitvektor
t = 0:T:p*Tp-T;

%%% Ausgang mit PRBS-Anregung
yp = lsim(G,u,t);


%%% mit Störung
st = 0
if st == 1
   % Störung
   var = 0.1;
   d = var*randn(length(t),1);
   yp = yp+d;
   figure(2)
   clf
   plot(t,d)
end


%%% Auffüllen der Matrizen

%%% Messwertmatrix Psi
for i = n+1:N
    for j=1:n
        tmp(i,j)   = -yp(i-j);
        tmp(i,j+n) =   u(i-j);
    end
end
Psi = tmp(n+1:N,:)

%%% Vektor ym
for i = n+1:N
    tmp(i) = yp(i);
end
ym = tmp(n+1:N)';


%%% Parametervektor
Theta = inv(Psi'*Psi)*Psi'*ym


%%% Zähler und Nenner des ident. Systems
a = Theta(1:n)';
b = Theta(n+1:2*n)';
num = b;
den = [1 a];


%%% ident. System als LTI-System
Gi = tf(num,den,T)

%%% Sprungantwort des ident. Systems
figure(1)
hold on
step(Gi)
