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

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

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

%%% Abtastzeit
T = 0.25;

%Gd = c2d(G,T);

%%% Streckenordnung
nG = max(size(eig(G)));

%%% Ordnung des PRBS
np = 5;

%%% Taktzeit des PRBS
k = 4;

%%% Laenge einer Periode des PRBS
N = k*(2^np-1);

%%% Periodendauer PRBS
Tp = N*T;

%%% Perdioden des PRBS
p = 10;

%%% Amplitude PRBS
V = 1;

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

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

%%% Simulation
y = lsim(G,u,t);


%%% Stoerung erzeugen
st = 0
if st == 1
  [z k] = size(y);
  d = rand(z,k);
  for i = 1:z,
     y(i) = y(i)+d(i);
  end
end


%%% Anfangswerte fuer P und Theta
[i,N] = size(t);

Theta = zeros(2*nG,1);

alpha = 10000;
P = eye(2*nG,2*nG);
P = alpha * P;


%%% RLS-Verfahren implementieren
  % (1) Datenvektor
for i = nG+1:N,
  for k = 1:nG,
    Psi(k   ,1) = -y(i-k);
    Psi(k+nG,1) =  u(i-k);
  end

  % (3) Schaetzwerte mit altem Parametervektor berechnen
  y_ = Psi' * Theta;

  % (4) Skalierungsfaktor berechnen
  gamma = P * Psi;
  gamma = (1 / (1 + Psi' * P * Psi)) * gamma;

  % (2) Neuen Parametervektor berechnen
  Theta = Theta + gamma * (y(i) - y_);

  % (5) Neues P fuer naechsten Schritt
  P = P - gamma * Psi' * P;

end 


%%% identifizierte Nennerkoeffizienten
a2 = Theta(2);
a1 = Theta(1);

%%% identifizierte Zaehlerkoeffizienten
b2 = Theta(4);
b1 = Theta(3);

%%% identifizierte Strecke
Gi = tf([b1 b2],[1 a1 a2],T);

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