% Partitioned Frequency LMS Adaptive Filter -PFLMS-
% -------------------------------------------------
% Aufruf:
% Function [dd,e,wf,fpo]=pflms(x,d,mu,N,C,P,S,L,wi)
%
% Parameter:
% x    : Eingangsvektor
% d    : Referenzvektor
% mu   : zeitabhängiger Schrittweitenvektor 0 < mu <= 1.0 
% N    : Anzahl Filterkoeffizienten (Hinweis: N = P*S*L)
% C    : FFT-Länge, idealerweise eine Zweier-Potenz (0=Auto)
% P    : Anzahl der Filterpartitionen 
% S    : Anzahl der Filtersegmente
% L    : Länge eines Verarbeitungsblocks (idealerweise C=L+N/P=2N/P)
% wi   : Startwerte der Filterkoeffizienten im Zeitbereich
% 
% Rückgabewerte:
% e    : Adaptitionsfehler
% dd   : Filterausgang
% wf   : Filterkoeffizienten am Ende der Adaption im Zeitbereich
% fpo  : Anzahl benötigten der Floating-Point Operationen

% -------------------------------------------------------------------------
% Datum : 27.3.2002
% Autor : Jens Ahrensfeld
% Thema : Diplomarbeit
% Datei : pflms.m
%
% -------------------------------------------------------------------------
function [dd,e,wf,fpo]=pflms(x,d,mu,N,C,P,S,L,wi)

% -------------------------------------------------------------------------
% Überprüfen der Parameter 
% -------------------------------------------------------------------------
if (N ~= fix(N)),
    error('Fehler: N muss eine ganze Zahl sein!');
end;

if (C ~= fix(C)),
    error('Fehler: C muss eine ganze Zahl sein!');
end;

if (P ~= fix(P)),
    error('Fehler: P muss eine ganze Zahl sein!');
end;

if (S ~= fix(S)),
    error('Fehler: S muss eine ganze Zahl sein!');
end;

if (L ~= fix(L)),
    error('Fehler: L muss eine ganze Zahl sein!');
end;

% -------------------------------------------------------------------------
% Automatik-Auswahl
% -------------------------------------------------------------------------
% Falls P=0 => Automatische Wahl von P
% Anahme: L+N/P = 2N/P
if (P==0),
  P = N/L;
  if (rem(N,L) ~= 0)
    error('Fehler: Quotient aus N und L muss eine ganze Zahl sein! (N = P*S*L)');
  end;  
end;

% Falls C=0 => Automatische Wahl von C=2^r
% Anahme: C = L + N/P 
if (C==0)
  C = 2^ceil(log2(L+N/P-1));
end;

% -------------------------------------------------------------------------
% FLMS Parameter
% -------------------------------------------------------------------------
gamma  = 1.0/C;               % Sicherheitskonstante vermeidet Division durch Null
lambda = 0.4;                 % Vergessensfaktor

% -------------------------------------------------------------------------
% Initialisierungen
% -------------------------------------------------------------------------
Lx = length(x);              % Länge des Eingangsvektors
Np = S*L;                    % Teilfilter hat die Länge Np = S*L = N/P
K = Lx/L;                    % Anzahl Verarbeitungsblöcke

e=zeros(Lx,1);              % Fehlervektor
dd=zeros(Lx,1);              % Filterausgang, Schaetzung von d
wp = zeros(Np,P);             % P Teilfilter im Zeitbereich
PX = zeros(C,1);             % Schaetzung des  Leistungsdichtespektrums

WSp = zeros(C,P);            % P Teilfilter im Frequenzbereich
X = zeros(C,S*P);            % Eingangsvektor im Frequenzbereich
mu_px = zeros(C,P*S);        % Variable Schrittweite
dd_ = zeros(C,1);             % Filterausgang im Zeitbereich
DD  = zeros(C,1);            % Filterausgang im Frequenzbereich
xzp = zeros(Lx+S*L,1);       % Eingangsvektor mit S*L führenden Nullen


% -------------------------------------------------------------------------
% Tabellen zur effizienten MatLab-Implementierung
% -------------------------------------------------------------------------
pidTbl = [2:P*S];            % Index auf P*S vergangene Eingangsvektoren 
pidTbl(P*S) = 1;             % Reihenfolge: x(k-1),x(k-2),..,x(k-P*S-1),x(k)

pStbl = [0:P*S-1];           % Tabelle zur korrekten Adressierung der Eingangsvektoren...
pStbl(1) = P*S;              % ...bei der Filterung und Update der Koeffizienten.

pjiTbl = [2:P];              % Index auf Teilfilter, welches bei der effizienten...
pjiTbl(P) = 1;               % ...Projektion bearbeitet wird

xzp(C-L+1:Lx+C-L) = x;       % Einfügen von Np führenden Nullen an den Eingangsvektor

pid = 1;                     % Aktueller Index zeigt auf Eingangsvektor x(0)
pji = 1;                     % Aktueller Index zeigt auf Teilfilter w_0


% -------------------------------------------------------------------------
% Anfangswerte der Filterkoeffienten partitionieren
% und in den Frequenzbereich transformieren  wi_p(k) => WS_p(k)
% -------------------------------------------------------------------------
for p=1:P,
  SLp = (p-1)*N/P;
  wp(1:Np,p) = wi(SLp+1:SLp+N/P);
  WSp(1:C,p) = 1/C*fft(wp(1:Np,p),C);
end;

% -------------------------------------------------------------------------
% Algorithmus Start
% -------------------------------------------------------------------------
flops(0);

for k=1:K,

  kL = (k-1)*L;

  % Transformation des aktuellen Eingangsvektors in den Frequenzbereich
  X(1:C,pid) = 1/C*fft(xzp(kL+1:kL+C),C);

  % Partitioned FFT-Faltung
  % Faltung des ersten Teilfilters mit aktuellem Eingangsvektor
  pS = pid;
  DD = WSp(1:C,1).*X(1:C,pid) *C;

  % Faltung der restlichen Teilfilters mit vergangenen Eingangsvektoren
  for p=2:P,
    % Ermittlung des korrekten Index 'pS' aus Tabelle
    for i = 1:S,
      pS = pStbl(pS);
    end;
    % Summation der einzelnen Filterausgänge
    DD   = DD + WSp(1:C,p).*X(1:C,pS) *C;
  end;

  
  % Transformation des Filterausgangs in den Zeitbereich
  dd_ = real(ifft(DD,C))*C;
  
  % Abspeichern der letzten L Werte des Filterausgangs
  dd(kL+1:kL+L) = dd_(C-L+1:C);

  % Berechnung des Fehlers e(k)
  e(kL+1:kL+L) = d(kL+1:kL+L) - dd(kL+1:kL+L);
  
  % Transformation des Fehlers in den Frequenzbereich
  E  = 1/C*fft([zeros(C-L,1); e(kL+1:kL+L).*mu(kL+1:kL+L)],C);
  
  % Schätzung der mittleren Eingangsleistung Px(k) aus x(k)
  PX = abs(lambda*conj(X(1:C,pid)).*X(1:C,pid) *C + (1-lambda)*PX);


  % Berechnung der variablen Schrittweite mu(k)
  % mu(k) wird niemals grösser als eins
  mu_px(1:C,pid) = (gamma/P)./(PX+gamma);

  % Filterkoeffizenten-Update
  pS = pid;
  for p=1:P,
    % Aktualisierung des p-ten Teilfilters
    WSp(1:C,p) = WSp(1:C,p) + mu_px(1:C,pS) .* conj(X(1:C,pS)) .* E *C;
    % Ermittlung des korrekten Index 'pS' aus Tabelle
    for i = 1:S,
      pS = pStbl(pS);
    end;
  end;

% Aufwändig: Projektion jedes p-ten Teilfilters wp pro k-ter Iteration
%  for p=1:P,
%    wp(1:C,p)  = real(dspifft(WSp(1:C,p),C));
%    WSp(1:C,p) = dspfft(wp(1:Np,p),C);
%  end;

% Effizient: Projektion eines pji-ten Teilfilters alternierend pro k-ter Iteration
  wp(1:C,pji)  = C*real(ifft(WSp(1:C,pji),C)); % pji = 0,1,..,P-1,0,1,..P-1,..
  WSp(1:C,pji) = 1/C*fft(wp(1:Np,pji),C);

  pji = pjiTbl(pji);         % Nächster Index pji auf Eingangsvektor aus Tabelle       
  pid = pidTbl(pid);         % Nächster Index pid auf Teilfilter aus Tabelle

end;

% Komposition des Gesamtfilters aus den P Teilfiltern
for p=1:P,
  wf((p-1)*Np+1:p*Np) = wp(1:Np,p);
end;

fpo=flops;

% -------------------------------------------------------------------------
% Ende PFLMS.M 
% -------------------------------------------------------------------------