commit e93e7927cbfa166a7f81bc9eace15307091dd7ff Author: Jens Ahrensfeld Date: Sat Jul 19 07:44:42 2014 +0000 Initial import git-svn-id: http://moon:8086/svn/software/trunk/libsrc/avpflms@1 b431acfa-c32f-4a4a-93f1-934dc6c82436 diff --git a/libpflms.dsp b/libpflms.dsp new file mode 100755 index 0000000..af76ad7 --- /dev/null +++ b/libpflms.dsp @@ -0,0 +1,119 @@ +# Microsoft Developer Studio Project File - Name="libpflms" - Package Owner=<4> +# Microsoft Developer Studio Generated Build File, Format Version 6.00 +# ** NICHT BEARBEITEN ** + +# TARGTYPE "Win32 (x86) Static Library" 0x0104 + +CFG=libpflms - Win32 Debug +!MESSAGE Dies ist kein gültiges Makefile. Zum Erstellen dieses Projekts mit NMAKE +!MESSAGE verwenden Sie den Befehl "Makefile exportieren" und führen Sie den Befehl +!MESSAGE +!MESSAGE NMAKE /f "libpflms.mak". +!MESSAGE +!MESSAGE Sie können beim Ausführen von NMAKE eine Konfiguration angeben +!MESSAGE durch Definieren des Makros CFG in der Befehlszeile. Zum Beispiel: +!MESSAGE +!MESSAGE NMAKE /f "libpflms.mak" CFG="libpflms - Win32 Debug" +!MESSAGE +!MESSAGE Für die Konfiguration stehen zur Auswahl: +!MESSAGE +!MESSAGE "libpflms - Win32 Release" (basierend auf "Win32 (x86) Static Library") +!MESSAGE "libpflms - Win32 Debug" (basierend auf "Win32 (x86) Static Library") +!MESSAGE + +# Begin Project +# PROP AllowPerConfigDependencies 0 +# PROP Scc_ProjName "" +# PROP Scc_LocalPath "" +CPP=cl.exe +RSC=rc.exe + +!IF "$(CFG)" == "libpflms - Win32 Release" + +# PROP BASE Use_MFC 0 +# PROP BASE Use_Debug_Libraries 0 +# PROP BASE Output_Dir "Release" +# PROP BASE Intermediate_Dir "Release" +# PROP BASE Target_Dir "" +# PROP Use_MFC 0 +# PROP Use_Debug_Libraries 0 +# PROP Output_Dir "../../lib/Release" +# PROP Intermediate_Dir "Release" +# PROP Target_Dir "" +# ADD BASE CPP /nologo /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /YX /FD /c +# ADD CPP /nologo /G6 /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /FD /c +# SUBTRACT CPP /YX +# ADD BASE RSC /l 0x407 /d "NDEBUG" +# ADD RSC /l 0x407 /d "NDEBUG" +BSC32=bscmake.exe +# ADD BASE BSC32 /nologo +# ADD BSC32 /nologo +LIB32=link.exe -lib +# ADD BASE LIB32 /nologo +# ADD LIB32 /nologo + +!ELSEIF "$(CFG)" == "libpflms - Win32 Debug" + +# PROP BASE Use_MFC 0 +# PROP BASE Use_Debug_Libraries 1 +# PROP BASE Output_Dir "Debug" +# PROP BASE Intermediate_Dir "Debug" +# PROP BASE Target_Dir "" +# PROP Use_MFC 0 +# PROP Use_Debug_Libraries 1 +# PROP Output_Dir "../../lib/Debug" +# PROP Intermediate_Dir "Debug" +# PROP Target_Dir "" +# ADD BASE CPP /nologo /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /YX /FD /GZ /c +# ADD CPP /nologo /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /FD /GZ /c +# SUBTRACT CPP /YX +# ADD BASE RSC /l 0x407 /d "_DEBUG" +# ADD RSC /l 0x407 /d "_DEBUG" +BSC32=bscmake.exe +# ADD BASE BSC32 /nologo +# ADD BSC32 /nologo +LIB32=link.exe -lib +# ADD BASE LIB32 /nologo +# ADD LIB32 /nologo + +!ENDIF + +# Begin Target + +# Name "libpflms - Win32 Release" +# Name "libpflms - Win32 Debug" +# Begin Group "Quellcodedateien" + +# PROP Default_Filter "cpp;c;cxx;rc;def;r;odl;idl;hpj;bat" +# Begin Source File + +SOURCE=..\avfft\avfft.c +# End Source File +# Begin Source File + +SOURCE=.\pfft.c +# End Source File +# Begin Source File + +SOURCE=.\pflms.c +# End Source File +# End Group +# Begin Group "Header-Dateien" + +# PROP Default_Filter "h;hpp;hxx;hm;inl" +# Begin Source File + +SOURCE=..\..\include\pflms.h + +!IF "$(CFG)" == "libpflms - Win32 Release" + +!ELSEIF "$(CFG)" == "libpflms - Win32 Debug" + +# PROP Intermediate_Dir "../lib/Debug" + +!ENDIF + +# End Source File +# End Group +# End Target +# End Project diff --git a/libpflms.dsw b/libpflms.dsw new file mode 100755 index 0000000..d656584 --- /dev/null +++ b/libpflms.dsw @@ -0,0 +1,29 @@ +Microsoft Developer Studio Workspace File, Format Version 6.00 +# WARNUNG: DIESE ARBEITSBEREICHSDATEI DARF NICHT BEARBEITET ODER GELÖSCHT WERDEN! + +############################################################################### + +Project: "libpflms"=.\libpflms.dsp - Package Owner=<4> + +Package=<5> +{{{ +}}} + +Package=<4> +{{{ +}}} + +############################################################################### + +Global: + +Package=<5> +{{{ +}}} + +Package=<3> +{{{ +}}} + +############################################################################### + diff --git a/libpflms.ncb b/libpflms.ncb new file mode 100755 index 0000000..8b340ee Binary files /dev/null and b/libpflms.ncb differ diff --git a/libpflms.opt b/libpflms.opt new file mode 100755 index 0000000..c0ef673 Binary files /dev/null and b/libpflms.opt differ diff --git a/libpflms.plg b/libpflms.plg new file mode 100755 index 0000000..5cdc906 --- /dev/null +++ b/libpflms.plg @@ -0,0 +1,32 @@ + + +
+
Erstellungsprotokoll

+

+--------------------Konfiguration: libpflms - Win32 Release--------------------
+

+
Befehlszeilen

+Erstellen der temporären Datei  "E:\WIN98SE\TEMP\RSP20E6.TMP" mit Inhalten
+[
+/nologo /G6 /ML /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /Fo"Release/" /Fd"Release/" /FD /c 
+"H:\Develop\80X86\LIBSRC\avfft\avfft.c"
+"H:\Develop\80X86\LIBSRC\avpflms\pfft.c"
+"H:\Develop\80X86\LIBSRC\avpflms\pflms.c"
+]
+Creating command line "cl.exe @E:\WIN98SE\TEMP\RSP20E6.TMP" 
+Erstellen der Befehlzeile "link.exe -lib /nologo /out:"../../lib/Release\libpflms.lib"  .\Release\avfft.obj .\Release\pfft.obj .\Release\pflms.obj "
+
Ausgabefenster

+Kompilierung läuft...
+avfft.c
+pfft.c
+pflms.c
+Generieren von Code...
+Bibliothek wird erstellt...
+
+
+
+
Ergebnisse

+libpflms.lib - 0 Fehler, 0 Warnung(en)
+
+ + diff --git a/pfft.c b/pfft.c new file mode 100755 index 0000000..2befdfd --- /dev/null +++ b/pfft.c @@ -0,0 +1,356 @@ +/*------------------------------------------------------------------------- + * pfft.c Partioned Fast Fourier Transform + * $Id: $ + *------------------------------------------------------------------------- + * + * Copyright (C) 2001 Algo Vision Systems GmbH + * + *------------------------------------------------------------------------- + */ +#define AVNEEDFLOAT + +#ifdef _DEBUG +#include +#endif +#include +#include "avtypes.h" +#include "averror.h" +#include "avrtl.h" +#include "avfft.h" +#include "pfft.h" + + +/*************************************************************************/ +AVERR PfftInit( +PFFT *pObj, /* Zeiger auf Objekt */ +UINT32 N, /* Filterlaenge N */ +UINT32 P, /* Anzahl Filterpartitionen */ +UINT32 S, /* Anzahl Filtersegmente pro Partition */ +UINT32 L, /* Laenge der Eingangsdaten (Blocklaenge */ +UINT32 C /* FFT-Laenge */ +) +{ + UINT32 i, s, NP, C_L; + CMPXBUF *pX; /* Temporaerer Zeiger fuer Bufferinit.*/ + + /* Auto-Param */ + if ((N*L)==0) + return AV_E_FAIL; + + if (P==0) + { + if ((N%L) != 0) + return AV_E_FAIL; + P = N/L; + } + + /* Auto-guess FFT-Groesse C */ + if (C==0) + pObj->C = (UINT32)pow(2,ceil(log(L+N/P-1)/log(2.0))); + else + pObj->C = C; + + /* Objekt initialisieren */ + pObj->L = L; + pObj->N = N; + pObj->P = P; + pObj->S = S; + NP = pObj->S * pObj->L; + C_L = pObj->C - pObj->L; + + /*-------------------------------------------------------*/ + /* Speicher allokieren */ + /*-------------------------------------------------------*/ + + /* Speicher fuer X[P*S][C] (komplex) */ + pObj->pBufX = (CMPXBUF*)AvMemAlloc(pObj->P*pObj->S*sizeof(CMPXBUF)); + + for (i=0; i < pObj->P*pObj->S; i++) + { + pObj->pBufX[i].cmpxData.pReal = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + pObj->pBufX[i].cmpxData.pImag = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + AvZeroMem(pObj->pBufX[i].cmpxData.pReal,pObj->C*sizeof(avfloat_t)); + AvZeroMem(pObj->pBufX[i].cmpxData.pImag,pObj->C*sizeof(avfloat_t)); + pObj->pBufX[i].pNext = &pObj->pBufX[i+1]; + pObj->pBufX[i].pLast = &pObj->pBufX[i-1]; + pObj->pBufX[i].pLastPS = NULL; + pObj->pBufX[i].user = i; + } + pObj->pBufX[i-1].pNext = &pObj->pBufX[0]; + pObj->pBufX[0].pLast = &pObj->pBufX[i-1]; + + /* Zeiger auf X[k-p*S] */ + for (i=0; i < pObj->P*pObj->S; i++) + { + pX = pObj->pBufX[i].pLast; + for (s=1; s < pObj->S; s++) + pX = pX->pLast; + + pObj->pBufX[i].pLastPS = pX; + } + + /* Ergebnis 'Y' der Faltung (komplex) */ + pObj->pY = (COMPLEX*)AvMemAlloc(sizeof(COMPLEX)); + pObj->pY->pReal = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + pObj->pY->pImag = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + AvZeroMem(pObj->pY->pReal,pObj->C*sizeof(avfloat_t)); + AvZeroMem(pObj->pY->pImag,pObj->C*sizeof(avfloat_t)); + + /* Overlap-Save 'xs' (reell) */ + pObj->BufXsave.pReal = (avfloat_t*)AvMemAlloc(C_L*sizeof(avfloat_t)); + pObj->BufXsave.pImag = (avfloat_t*)AvMemAlloc(C_L*sizeof(avfloat_t)); + AvZeroMem(pObj->BufXsave.pReal, C_L*sizeof(avfloat_t)); + AvZeroMem(pObj->BufXsave.pImag, C_L*sizeof(avfloat_t)); + + /* FFT initialisieren */ + pObj->pFFT = (FFT*)AvMemAlloc(sizeof(FFT)); + FFTinit(pObj->pFFT, pObj->C); + + /* Arbeitszeiger initialisieren */ + pObj->pX = pObj->pBufX; + + return AV_E_OK; +} + +/*-----------------------------------------------------------------------*/ +/* Partitioned FLMS Filterinitialisierung +/* 1. Partitionierung der Filterstartwerte in P-Partitionen +/* 2. Transformation der P Teil-Filter in den Frequenzbereich +/* Element pBufWS wird veraendert +/*-----------------------------------------------------------------------*/ +AVERR PfftFilterInit( +PFFT *pObj, /* Zeiger auf Objekt */ +COMPLEX WTD, +CMPXBUF *pWS) /* N Filterkoeffizienten im Zeitbereich */ +{ + UINT32 p, NP, C_SL; + + NP = pObj->S * pObj->L; + C_SL = pObj->C - NP; + + for (p=0; p < pObj->P; p++) + { + /* Arbeitspuffer WS auffuellen + Re{WS[p][0..C-L-1]}=wi[p*N/P..(p+1)*N/P-1] */ + AvMemCpy(pWS[p].cmpxData.pReal, + &WTD.pReal[p*NP], NP*sizeof(avfloat_t)); + + /* Arbeitspuffer WS auffuellen + Im{WS[p][0..C-L-1]}=wi[p*N/P..(p+1)*N/P-1] */ + AvMemCpy(pWS[p].cmpxData.pImag, + &WTD.pImag[p*NP], NP*sizeof(avfloat_t)); + + /* Re{WS[C-L..C]} = {0} (vorsichtshalber) */ + AvZeroMem(&pWS[p].cmpxData.pReal[NP], C_SL *sizeof(avfloat_t)); + + /* Im{WS[C-L..C]} = {0} (vorsichtshalber) */ + AvZeroMem(&pWS[p].cmpxData.pImag[NP], C_SL *sizeof(avfloat_t)); + + /* In den Frequenzbereich transformieren + WS[p][0..C-1] = 1/C*FFT{wi[p*N/P..(p+1)*N/P-1]} */ + ffts(pObj->pFFT, pWS[p].cmpxData.pReal, pWS[p].cmpxData.pImag); + } + return AV_E_OK; +} + +avfloat_t *PfftGetBufInRe(PFFT *pObj) +{ + UINT32 C_L; + C_L = pObj->C - pObj->L; /* C-L */ + return &pObj->pX->cmpxData.pReal[C_L]; +} +avfloat_t *PfftGetBufInIm(PFFT *pObj) +{ + UINT32 C_L; + C_L = pObj->C - pObj->L; /* C-L */ + return &pObj->pX->cmpxData.pImag[C_L]; +} + +avfloat_t *PfftGetBufOutRe(PFFT *pObj) +{ + UINT32 C_L; + C_L = pObj->C - pObj->L; /* C-L */ + return &pObj->pY->pReal[C_L]; +} + +avfloat_t *PfftGetBufOutIm(PFFT *pObj) +{ + UINT32 C_L; + C_L = pObj->C - pObj->L; /* C-L */ + return &pObj->pY->pImag[C_L]; +} + +/*---------------------------------------------------------------*/ +/* Speicher zuweisen fuer Koeffizienten H +/*---------------------------------------------------------------*/ +AVERR PfftFilterAlloc(PFFT *pObj, CMPXBUF **ppH) +{ + + UINT32 i; + + /* Speicher fuer WS[P][C] (komplex) */ + *ppH = (CMPXBUF*)AvMemAlloc(pObj->P*pObj->S*sizeof(CMPXBUF)); + + for (i=0; i < pObj->P*pObj->S; i++) + { + (*ppH)[i].cmpxData.pReal = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + (*ppH)[i].cmpxData.pImag = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + AvZeroMem((*ppH)[i].cmpxData.pReal,pObj->C*sizeof(avfloat_t)); + AvZeroMem((*ppH)[i].cmpxData.pImag,pObj->C*sizeof(avfloat_t)); + (*ppH)[i].pNext = &(*ppH)[i+1]; + (*ppH)[i].pLast = &(*ppH)[i-1]; + (*ppH)[i].pLastPS = NULL; + (*ppH)[i].user = i; + } + (*ppH)[i-1].pNext = &(*ppH)[0]; + (*ppH)[0].pLast = &(*ppH)[i-1]; + + return AV_E_OK; +} + +/*---------------------------------------------------------------*/ +/* Partitioned FFT-Filterung +/*---------------------------------------------------------------*/ +AVERR PfftFilter(PFFT *pObj, CMPXBUF *pH, COMPLEX x, COMPLEX y) +{ + UINT32 p, NP, C_L; + + CMPXBUF *pX; + COMPLEX *pY; + + /* Arbeitszeiger */ + pX = pObj->pX; /* Aktueller Zeiger X[k] */ + pY = pObj->pY; /* 'Y' wird nach der Filterung als + temporaerer Speicher benutzt */ + + NP = pObj->S*pObj->L; /* N/P = S*L */ + C_L = pObj->C - pObj->L; /* C-L */ + + + /* Arbeitspuffer 'X' auffuellen Re{X[S*L..C-1]} = x[0..L-1] */ + AvMemCpy(&pX->cmpxData.pReal[C_L],x.pReal, + pObj->L*sizeof(avfloat_t)); + + /* Saveblock 'xs' anfuegen Re{X[0..C-L-1]} = xs[0..C-L-1] */ + AvMemCpy(pX->cmpxData.pReal,pObj->BufXsave.pReal, + C_L*sizeof(avfloat_t)); + + /* Saveblock aktualisieren xs[0..C-L-1] = x[L..C-1] */ + AvMemCpy(pObj->BufXsave.pReal, &pX->cmpxData.pReal[pObj->L], + C_L*sizeof(avfloat_t)); + + /* Arbeitspuffer 'X' auffuellen Im{X[S*L..C-1]} = x[0..L-1] */ + AvMemCpy(&pX->cmpxData.pImag[C_L],x.pImag, + pObj->L*sizeof(avfloat_t)); + + /* Saveblock 'xs' anfuegen Im{X[0..C-L-1]} = xs[0..C-L-1] */ + AvMemCpy(pX->cmpxData.pImag,pObj->BufXsave.pImag, + C_L*sizeof(avfloat_t)); + + /* Saveblock aktualisieren xs[0..C-L-1] = x[L..C-1] */ + AvMemCpy(pObj->BufXsave.pImag, &pX->cmpxData.pImag[pObj->L], + C_L*sizeof(avfloat_t)); + + /* X = FFT{x} */ + fft(pObj->pFFT, pX->cmpxData.pReal, pX->cmpxData.pImag); + + /* 1. Partition Multiplikation im Frequenzbereich + Y = X[k][0..C-1] * H[0][0..C-1] */ + CmpxVectMul(&pX->cmpxData, &pH->cmpxData, + pY, pObj->C); + + /* 2. Partition bis P-te Partition */ + for (p=1; p < pObj->P; p++) + { + pH = pH->pNext; + + /* X[k-p*S] suchen */ + pX = pX->pLastPS; + + /* Y = X[k-p*S][0..C-1] * H[p][0..C-1] */ + CmpxVectMac(&pX->cmpxData, &pH->cmpxData, + pY, pObj->C); + } + + /* In den Zeitbereich transformieren, y = IFFT{Y} */ + ifft(pObj->pFFT, pY->pReal, pY->pImag); + + /* Abspeichern der letzten L Daten ys[0..L-1] = Re{Y[C-L..C-1] */ + AvMemCpy(y.pReal, &pY->pReal[C_L], pObj->L*sizeof(avfloat_t)); + + /* Abspeichern der letzten L Daten ys[0..L-1] = Im{Y[C-L..C-1] */ + AvMemCpy(y.pImag, &pY->pImag[C_L], pObj->L*sizeof(avfloat_t)); + + /* Fuer den naechsten Aufruf Zeiger aktualisieren */ + /* Naechstes X[k] ist: */ + pObj->pX = pObj->pX->pNext; + + return AV_E_OK; +} + +AVERR PfftFilterFast(PFFT *pObj, CMPXBUF *pH) +{ + UINT32 p, NP, C_L; + + CMPXBUF *pX; + COMPLEX *pY; + + /* Arbeitszeiger */ + pX = pObj->pX; /* Aktueller Zeiger X[k] */ + pY = pObj->pY; /* 'Y' wird nach der Filterung als + temporaerer Speicher benutzt */ + + NP = pObj->S*pObj->L; /* N/P = S*L */ + C_L = pObj->C - pObj->L; /* C-L */ + + + /* Saveblock 'xs' anfuegen Re{X[0..C-L-1]} = xs[0..C-L-1] */ + AvMemCpy(pX->cmpxData.pReal,pObj->BufXsave.pReal, + C_L*sizeof(avfloat_t)); + + /* Saveblock aktualisieren xs[0..C-L-1] = x[L..C-1] */ + AvMemCpy(pObj->BufXsave.pReal, &pX->cmpxData.pReal[pObj->L], + C_L*sizeof(avfloat_t)); + + /* Saveblock 'xs' anfuegen Im{X[0..C-L-1]} = xs[0..C-L-1] */ + AvMemCpy(pX->cmpxData.pImag,pObj->BufXsave.pImag, + C_L*sizeof(avfloat_t)); + + /* Saveblock aktualisieren xs[0..C-L-1] = x[L..C-1] */ + AvMemCpy(pObj->BufXsave.pImag, &pX->cmpxData.pImag[pObj->L], + C_L*sizeof(avfloat_t)); + + /* X = FFT{x} */ + fft(pObj->pFFT, pX->cmpxData.pReal, pX->cmpxData.pImag); + + /* 1. Partition Multiplikation im Frequenzbereich + Y = X[k][0..C-1] * H[0][0..C-1] */ + CmpxVectMul(&pX->cmpxData, &pH->cmpxData, + pY, pObj->C); + + /* 2. Partition bis P-te Partition */ + for (p=1; p < pObj->P; p++) + { + pH = pH->pNext; + + /* X[k-p*S] suchen */ + pX = pX->pLastPS; + + /* Y = X[k-p*S][0..C-1] * H[p][0..C-1] */ + CmpxVectMac(&pX->cmpxData, &pH->cmpxData, + pY, pObj->C); + } + + /* In den Zeitbereich transformieren, y = IFFT{Y} */ + ifft(pObj->pFFT, pY->pReal, pY->pImag); + + /* Fuer den naechsten Aufruf Zeiger aktualisieren */ + /* Naechstes X[k] ist: */ + pObj->pX = pObj->pX->pNext; + + return AV_E_OK; +} + + +/*------------------------------------------------------------------*/ + diff --git a/pfft.c.bak b/pfft.c.bak new file mode 100755 index 0000000..0dc2f24 --- /dev/null +++ b/pfft.c.bak @@ -0,0 +1,332 @@ +/*------------------------------------------------------------------------- + * pfft.c Partioned Fast Fourier Transform + * $Id: $ + *------------------------------------------------------------------------- + * + * Copyright (C) 2001 Algo Vision Systems GmbH + * + *------------------------------------------------------------------------- + */ +#define AVNEEDFLOAT + +#ifdef _DEBUG +#include +#endif +#include +#include "avtypes.h" +#include "averror.h" +#include "avrtl.h" +#include "avfft.h" +#include "pfft.h" + + +/*************************************************************************/ +AVERR PfftInit( +PFFT *pObj, /* Zeiger auf Objekt */ +UINT32 N, /* Filterlaenge N */ +UINT32 P, /* Anzahl Filterpartitionen */ +UINT32 S, /* Anzahl Filtersegmente pro Partition */ +UINT32 L, /* Laenge der Eingangsdaten (Blocklaenge */ +UINT32 C /* FFT-Laenge */ +) +{ + UINT32 i, s, NP, C_L; + CMPXBUF *pX; /* Temporaerer Zeiger fuer Bufferinit.*/ + + /* Auto-Param */ + if ((N*L)==0) + return AV_E_FAIL; + + if (P==0) + { + if ((N%L) != 0) + return AV_E_FAIL; + P = N/L; + } + + /* Auto-guess FFT-Groesse C */ + if (C==0) + pObj->C = (UINT32)pow(2,ceil(log(L+N/P-1)/log(2.0))); + else + pObj->C = C; + + /* Objekt initialisieren */ + pObj->L = L; + pObj->N = N; + pObj->P = P; + pObj->S = S; + NP = pObj->S * pObj->L; + C_L = pObj->C - pObj->L; + + /*-------------------------------------------------------*/ + /* Speicher allokieren */ + /*-------------------------------------------------------*/ + + /* Speicher fuer X[P*S][C] (komplex) */ + pObj->pBufX = (CMPXBUF*)AvMemAlloc(pObj->P*pObj->S*sizeof(CMPXBUF)); + + for (i=0; i < pObj->P*pObj->S; i++) + { + pObj->pBufX[i].cmpxData.pReal = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + pObj->pBufX[i].cmpxData.pImag = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + AvZeroMem(pObj->pBufX[i].cmpxData.pReal,pObj->C*sizeof(avfloat_t)); + AvZeroMem(pObj->pBufX[i].cmpxData.pImag,pObj->C*sizeof(avfloat_t)); + pObj->pBufX[i].pNext = &pObj->pBufX[i+1]; + pObj->pBufX[i].pLast = &pObj->pBufX[i-1]; + pObj->pBufX[i].pLastPS = NULL; + pObj->pBufX[i].user = i; + } + pObj->pBufX[i-1].pNext = &pObj->pBufX[0]; + pObj->pBufX[0].pLast = &pObj->pBufX[i-1]; + + /* Zeiger auf X[k-p*S] */ + for (i=0; i < pObj->P*pObj->S; i++) + { + pX = pObj->pBufX[i].pLast; + for (s=1; s < pObj->S; s++) + pX = pX->pLast; + + pObj->pBufX[i].pLastPS = pX; + } + + /* Ergebnis 'Y' der Faltung (komplex) */ + pObj->pY = (COMPLEX*)AvMemAlloc(sizeof(COMPLEX)); + pObj->pY->pReal = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + pObj->pY->pImag = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + AvZeroMem(pObj->pY->pReal,pObj->C*sizeof(avfloat_t)); + AvZeroMem(pObj->pY->pImag,pObj->C*sizeof(avfloat_t)); + + /* Overlap-Save 'xs' (reell) */ + pObj->pBufXsave = (avfloat_t*)AvMemAlloc(C_L*sizeof(avfloat_t)); + AvZeroMem(pObj->pBufXsave, C_L*sizeof(avfloat_t)); + + /* FFT initialisieren */ + pObj->pFFT = (FFT*)AvMemAlloc(sizeof(FFT)); + FFTinit(pObj->pFFT, pObj->C); + + /* Arbeitszeiger initialisieren */ + pObj->pX = pObj->pBufX; + + return AV_E_OK; +} + +/*-----------------------------------------------------------------------*/ +/* Partitioned FLMS Filterinitialisierung +/* 1. Partitionierung der Filterstartwerte in P-Partitionen +/* 2. Transformation der P Teil-Filter in den Frequenzbereich +/* Element pBufWS wird veraendert +/*-----------------------------------------------------------------------*/ +AVERR PfftFilterInit( +PFFT *pObj, /* Zeiger auf Objekt */ +avfloat_t *pWTD, +CMPXBUF *pWS) /* N Filterkoeffizienten im Zeitbereich */ +{ + UINT32 p, NP, C_SL; + + NP = pObj->S * pObj->L; + C_SL = pObj->C - NP; + + for (p=0; p < pObj->P; p++) + { + /* Arbeitspuffer WS auffuellen + Re{WS[p][0..C-L-1]}=wi[p*N/P..(p+1)*N/P-1] */ + AvMemCpy(pWS[p].cmpxData.pReal, + &pWTD[p*NP], NP*sizeof(avfloat_t)); + + /* Re{WS[C-L..C]} = {0} (vorsichtshalber) */ + AvZeroMem(&pWS[p].cmpxData.pReal[NP], C_SL *sizeof(avfloat_t)); + + /* Im{WS[0..C-1]} = {0} */ + AvZeroMem(pWS[p].cmpxData.pImag, pObj->C *sizeof(avfloat_t)); + + /* In den Frequenzbereich transformieren + WS[p][0..C-1] = 1/C*FFT{wi[p*N/P..(p+1)*N/P-1]} */ + ffts(pObj->pFFT, pWS[p].cmpxData.pReal, pWS[p].cmpxData.pImag); + } + return AV_E_OK; +} + +/*---------------------------------------------------------------*/ +/* Speicher zuweisen fuer Koeffizienten H +/*---------------------------------------------------------------*/ +AVERR PfftFilterAlloc(PFFT *pObj, CMPXBUF **ppH) +{ + + UINT32 i; + + /* Speicher fuer WS[P][C] (komplex) */ + *ppH = (CMPXBUF*)AvMemAlloc(pObj->P*pObj->S*sizeof(CMPXBUF)); + + for (i=0; i < pObj->P*pObj->S; i++) + { + (*ppH)[i].cmpxData.pReal = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + (*ppH)[i].cmpxData.pImag = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + AvZeroMem((*ppH)[i].cmpxData.pReal,pObj->C*sizeof(avfloat_t)); + AvZeroMem((*ppH)[i].cmpxData.pImag,pObj->C*sizeof(avfloat_t)); + (*ppH)[i].pNext = &(*ppH)[i+1]; + (*ppH)[i].pLast = &(*ppH)[i-1]; + (*ppH)[i].pLastPS = NULL; + (*ppH)[i].user = i; + } + (*ppH)[i-1].pNext = &(*ppH)[0]; + (*ppH)[0].pLast = &(*ppH)[i-1]; + + return AV_E_OK; +} + +/*---------------------------------------------------------------*/ +/* Partitioned FFT-Filterung +/*---------------------------------------------------------------*/ +AVERR PfftFilter(PFFT *pObj, CMPXBUF *pH, avfloat_t *px, avfloat_t *py) +{ + UINT32 p, NP, C_L; + + CMPXBUF *pX; + COMPLEX *pY; + + /* Arbeitszeiger */ + pX = pObj->pX; /* Aktueller Zeiger X[k] */ + pY = pObj->pY; /* 'Y' wird nach der Filterung als + temporaerer Speicher benutzt */ + + NP = pObj->S*pObj->L; /* N/P = S*L */ + C_L = pObj->C - pObj->L; /* C-L */ + + + /* Arbeitspuffer 'X' auffuellen Re{X[S*L..C-1]} = x[0..L-1] */ + AvMemCpy(&pX->cmpxData.pReal[C_L],px, + pObj->L*sizeof(avfloat_t)); + + /* Saveblock 'xs' anfuegen Re{X[0..C-L-1]} = xs[0..C-L-1] */ + AvMemCpy(pX->cmpxData.pReal,pObj->pBufXsave, + C_L*sizeof(avfloat_t)); + + /* Saveblock aktualisieren xs[0..C-L-1] = x[L..C-1] */ + AvMemCpy(pObj->pBufXsave, &pX->cmpxData.pReal[pObj->L], + C_L*sizeof(avfloat_t)); + + /* Imaginaerteil von 'X' auf Null setzen Im{X[0..C-1]} = {0} */ + AvZeroMem(pX->cmpxData.pImag,pObj->C*sizeof(avfloat_t)); + + /* X = 1/C*FFT{x} */ + ffts(pObj->pFFT, pX->cmpxData.pReal, pX->cmpxData.pImag); + + /* 1. Partition Multiplikation im Frequenzbereich + Y = X[k][0..C-1] * H[0][0..C-1] * C */ + CmpxVectMulS(&pX->cmpxData, &pH->cmpxData, + pY, pObj->C); + + /* 2. Partition bis P-te Partition */ + for (p=1; p < pObj->P; p++) + { + pH = pH->pNext; + + /* X[k-p*S] suchen */ + pX = pX->pLastPS; + + /* Y = X[k-p*S][0..C-1] * H[p][0..C-1] * C */ + CmpxVectMacS(&pX->cmpxData, &pH->cmpxData, + pY, pObj->C); + } + + /* In den Zeitbereich transformieren, y = IFFT{Y} */ + ifft(pObj->pFFT, pY->pReal, pY->pImag); + + /* Abspeichern der letzten L Daten ys[0..L-1] = Re{Y[C-L..C-1] */ + AvMemCpy(py, &pY->pReal[C_L], pObj->L*sizeof(avfloat_t)); + + /* Fuer den naechsten Aufruf Zeiger aktualisieren */ + /* Naechstes X[k] ist: */ + pObj->pX = pObj->pX->pNext; + + return AV_E_OK; +} + +/*-----------------------------------------------------------------*/ +/* Complex-Funktionen +/*-----------------------------------------------------------------*/ +void CmpxVectMul( +struct _sCOMPLEX *pA, +struct _sCOMPLEX *pB, +struct _sCOMPLEX *pAB, +UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAB->pReal[i] = pA->pReal[i]*pB->pReal[i] + - pA->pImag[i]*pB->pImag[i]; + pAB->pImag[i] = pA->pReal[i]*pB->pImag[i] + + pA->pImag[i]*pB->pReal[i]; + } +} + +void CmpxVectAdd( +struct _sCOMPLEX *pA, +struct _sCOMPLEX *pB, +struct _sCOMPLEX *pAB, +UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAB->pReal[i] = pA->pReal[i] + pB->pReal[i]; + pAB->pImag[i] = pA->pImag[i] + pB->pImag[i]; + } +} + +void CmpxVectMac( +struct _sCOMPLEX *pA, +struct _sCOMPLEX *pB, +struct _sCOMPLEX *pAB, +UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAB->pReal[i] += pA->pReal[i]*pB->pReal[i] + - pA->pImag[i]*pB->pImag[i]; + pAB->pImag[i] += pA->pReal[i]*pB->pImag[i] + + pA->pImag[i]*pB->pReal[i]; + } +} + +void CmpxVectMulS( +struct _sCOMPLEX *pA, +struct _sCOMPLEX *pB, +struct _sCOMPLEX *pAB, +UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAB->pReal[i] = (pA->pReal[i]*pB->pReal[i] + - pA->pImag[i]*pB->pImag[i])*len; + pAB->pImag[i] = (pA->pReal[i]*pB->pImag[i] + + pA->pImag[i]*pB->pReal[i])*len; + } +} + +void CmpxVectMacS( +struct _sCOMPLEX *pA, +struct _sCOMPLEX *pB, +struct _sCOMPLEX *pAB, +UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAB->pReal[i] += (pA->pReal[i]*pB->pReal[i] + - pA->pImag[i]*pB->pImag[i])*len; + pAB->pImag[i] += (pA->pReal[i]*pB->pImag[i] + + pA->pImag[i]*pB->pReal[i])*len; + } +} + +/*------------------------------------------------------------------*/ + diff --git a/pfft.h b/pfft.h new file mode 100755 index 0000000..cfae86c --- /dev/null +++ b/pfft.h @@ -0,0 +1,153 @@ +/*------------------------------------------------------------------------- + * pfft.h Partioned Fast Fourier Transform + * $Id: $ + *------------------------------------------------------------------------- + * + * Copyright (C) 2001 Algo Vision Systems GmbH + * + *------------------------------------------------------------------------- + */ + +#ifndef _PFFT_H +#define _PFFT_H +#include "avtypes.h" +#include "avfft.h" + +/*-----------------------------------------------------------------------*/ +/* PFLMS-Parameter */ +/*-----------------------------------------------------------------------*/ + +typedef struct _sCOMPLEX +{ + avfloat_t *pReal, *pImag; +} COMPLEX; + +typedef struct _sREALBUF +{ + avfloat_t *pData; + struct _sREALBUF *pNext, *pLast, *pLastPS; + UINT32 user; +} REALBUF; + +typedef struct _sCMPXBUF +{ + struct _sCOMPLEX cmpxData; + struct _sCMPXBUF *pNext, *pLast, *pLastPS; + UINT32 user; +} CMPXBUF; + +typedef struct _sPFFT +{ + UINT32 N, P, S, L, C; /* PFLMS-Parameter N, P, S, L, C */ + CMPXBUF *pBufX; /* komplexer Puffer X[P*S][C] */ + CMPXBUF *pX; /* aktuelle Zeiger auf Puffer[p][] */ + COMPLEX *pY; /* temporaerer Puffer fuer Y[] und E[] */ + COMPLEX BufXsave; /* Overlap-Save Puffer xs[S*L] */ + FFT *pFFT; /* FFT-Objekt */ +} PFFT; + +#ifdef __cplusplus +extern "C" { +#endif + +AVERR PfftInit(PFFT *pObj,UINT32 N, UINT32 P, UINT32 S, UINT32 L, UINT32 C); +AVERR PfftFilterAlloc(PFFT *pObj, CMPXBUF **ppH); +AVERR PfftFilterInit(PFFT *pObj, COMPLEX WTD, CMPXBUF *pH); +AVERR PfftFilter(PFFT *pObj, CMPXBUF *pH, COMPLEX x, COMPLEX y); +AVERR PfftFilterFast(PFFT *pObj, CMPXBUF *pH); +avfloat_t *PfftGetBufInRe(PFFT *pObj); +avfloat_t *PfftGetBufInIm(PFFT *pObj); +avfloat_t *PfftGetBufOutRe(PFFT *pObj); +avfloat_t *PfftGetBufOutIm(PFFT *pObj); + +/*-----------------------------------------------------------------*/ +/* Complex-Funktionen +/*-----------------------------------------------------------------*/ +_inline void CmpxVectMul( +struct _sCOMPLEX *pA, +struct _sCOMPLEX *pB, +struct _sCOMPLEX *pAB, +UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAB->pReal[i] = pA->pReal[i]*pB->pReal[i] + - pA->pImag[i]*pB->pImag[i]; + pAB->pImag[i] = pA->pReal[i]*pB->pImag[i] + + pA->pImag[i]*pB->pReal[i]; + } +} + +_inline void CmpxVectAdd( +struct _sCOMPLEX *pA, +struct _sCOMPLEX *pB, +struct _sCOMPLEX *pAB, +UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAB->pReal[i] = pA->pReal[i] + pB->pReal[i]; + pAB->pImag[i] = pA->pImag[i] + pB->pImag[i]; + } +} + +_inline void CmpxVectMac( +struct _sCOMPLEX *pA, +struct _sCOMPLEX *pB, +struct _sCOMPLEX *pAB, +UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAB->pReal[i] += pA->pReal[i]*pB->pReal[i] + - pA->pImag[i]*pB->pImag[i]; + pAB->pImag[i] += pA->pReal[i]*pB->pImag[i] + + pA->pImag[i]*pB->pReal[i]; + } +} + +_inline void CmpxVectMulS( +struct _sCOMPLEX *pA, +struct _sCOMPLEX *pB, +struct _sCOMPLEX *pAB, +UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAB->pReal[i] = (pA->pReal[i]*pB->pReal[i] + - pA->pImag[i]*pB->pImag[i])*len; + pAB->pImag[i] = (pA->pReal[i]*pB->pImag[i] + + pA->pImag[i]*pB->pReal[i])*len; + } +} + +_inline void CmpxVectMacS( +struct _sCOMPLEX *pA, +struct _sCOMPLEX *pB, +struct _sCOMPLEX *pAB, +UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAB->pReal[i] += (pA->pReal[i]*pB->pReal[i] + - pA->pImag[i]*pB->pImag[i])*len; + pAB->pImag[i] += (pA->pReal[i]*pB->pImag[i] + + pA->pImag[i]*pB->pReal[i])*len; + } +} + +#ifdef __cplusplus +} +#endif +#endif /* _PFFT_H */ + diff --git a/pfft.h.bak b/pfft.h.bak new file mode 100755 index 0000000..ef1d712 --- /dev/null +++ b/pfft.h.bak @@ -0,0 +1,69 @@ +/*------------------------------------------------------------------------- + * pfft.h Partioned Fast Fourier Transform + * $Id: $ + *------------------------------------------------------------------------- + * + * Copyright (C) 2001 Algo Vision Systems GmbH + * + *------------------------------------------------------------------------- + */ + +#ifndef _PFFT_H +#define _PFFT_H +#include "avtypes.h" +#include "avfft.h" + +/*-----------------------------------------------------------------------*/ +/* PFLMS-Parameter */ +/*-----------------------------------------------------------------------*/ + +typedef struct _sCOMPLEX +{ + avfloat_t *pReal, *pImag; +} COMPLEX; + +typedef struct _sREALBUF +{ + avfloat_t *pData; + struct _sREALBUF *pNext, *pLast, *pLastPS; + UINT32 user; +} REALBUF; + +typedef struct _sCMPXBUF +{ + struct _sCOMPLEX cmpxData; + struct _sCMPXBUF *pNext, *pLast, *pLastPS; + UINT32 user; +} CMPXBUF; + +typedef struct _sPFFT +{ + UINT32 N, P, S, L, C; /* PFLMS-Parameter N, P, S, L, C */ + CMPXBUF *pBufX; /* komplexer Puffer X[P*S][C] */ + CMPXBUF *pX; /* aktuelle Zeiger auf Puffer[p][] */ + COMPLEX *pY; /* temporaerer Puffer fuer Y[] und E[] */ + avfloat_t *pBufXsave; /* Overlap-Save Puffer xs[S*L] */ + FFT *pFFT; /* FFT-Objekt */ +} PFFT; + + +void CmpxVectMul(struct _sCOMPLEX *pA, struct _sCOMPLEX *pB, struct _sCOMPLEX *pAB, UINT32 len); +void CmpxVectAdd(struct _sCOMPLEX *pA, struct _sCOMPLEX *pB, struct _sCOMPLEX *pAB, UINT32 len); +void CmpxVectMac(struct _sCOMPLEX *pA, struct _sCOMPLEX *pB, struct _sCOMPLEX *pAB, UINT32 len); +void CmpxVectMulS(struct _sCOMPLEX *pA, struct _sCOMPLEX *pB, struct _sCOMPLEX *pAB, UINT32 len); +void CmpxVectMacS(struct _sCOMPLEX *pA, struct _sCOMPLEX *pB, struct _sCOMPLEX *pAB, UINT32 len); + +#ifdef __cplusplus +extern "C" { +#endif + +AVERR PfftInit(PFFT *pObj,UINT32 N, UINT32 P, UINT32 S, UINT32 L, UINT32 C); +AVERR PfftFilterAlloc(PFFT *pObj, CMPXBUF **ppH); +AVERR PfftFilterInit(PFFT *pObj, avfloat_t *pWTD, CMPXBUF *pH); +AVERR PfftFilter(PFFT *pObj, CMPXBUF *pH, avfloat_t *px, avfloat_t *py); + +#ifdef __cplusplus +} +#endif +#endif /* _PFFT_H */ + diff --git a/pflms.c b/pflms.c new file mode 100755 index 0000000..2436d13 --- /dev/null +++ b/pflms.c @@ -0,0 +1,287 @@ +/*------------------------------------------------------------------------- + * pflms.c Partioned Frequency Least-Mean-Square adaptive algorithm + * $Id: $ + *------------------------------------------------------------------------- + * + * Copyright (C) 2001 Algo Vision Systems GmbH + * + *------------------------------------------------------------------------- + */ +#define AVNEEDFLOAT + +#ifdef _DEBUG +#include +#endif +#include +#include "avtypes.h" +#include "averror.h" +#include "avrtl.h" +#include "avfft.h" +#include "pfft.h" +#include "pflms.h" + +/*-----------------------------------------------------------------------*/ +#define LAMBDA 0.6 +#define pTHIS pObj + +AVERR PflmsInit( +PFLMS *pObj, /* Zeiger auf Objekt */ +UINT32 N, /* Filterlaenge N */ +UINT32 P, /* Anzahl Filterpartitionen */ +UINT32 S, /* Anzahl Filtersegmente pro Partition */ +UINT32 L, /* Laenge der Eingangsdaten (Blocklaenge */ +UINT32 C /* FFT-Laenge */ +) +{ + UINT32 i, s, NP, C_L; + REALBUF *pMu; /* Temporaerer Zeiger fuer Bufferinit.*/ + +#ifdef _DEBUG + UINT32 memMu, memPX, memY; + UINT32 nBufMu, nBufPX, nBufY; +#endif + + /* PFFT initialisieren */ + pObj->pFilter = (PFFT*)AvMemAlloc(sizeof(PFFT)); + PfftInit(pObj->pFilter, N, P, S, L, C); + + /* Objekt initialisieren */ + pObj->L = pObj->pFilter->L; + pObj->N = pObj->pFilter->N; + pObj->P = pObj->pFilter->P; + pObj->S = pObj->pFilter->S; + pObj->C = pObj->pFilter->C; + NP = pObj->S * pObj->L; + C_L = pObj->C - pObj->L; + +#ifdef _DEBUG + printf("PFLMS Init:\n"); + printf("N = %d\n",pObj->N); + printf("P = %d\n",pObj->P); + printf("S = %d\n",pObj->S); + printf("L = %d\n",pObj->L); + printf("NP = %d\n",NP); + printf("C = %d\n",pObj->C); + printf("C-L = %d\n",C_L); +#endif + + /*-------------------------------------------------------*/ + /* Speicher allokieren */ + /*-------------------------------------------------------*/ + /* Speicher fuer Mu[P*S][C] (reell) */ + pObj->pBufMu = (REALBUF*)AvMemAlloc(pObj->P*pObj->S*sizeof(REALBUF)); + + for (i=0; i < pObj->P*pObj->S; i++) + { + pObj->pBufMu[i].pData = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + AvZeroMem(pObj->pBufMu[i].pData,pObj->C*sizeof(avfloat_t)); + pObj->pBufMu[i].pNext = &pObj->pBufMu[i+1]; + pObj->pBufMu[i].pLast = &pObj->pBufMu[i-1]; + pObj->pBufMu[i].pLastPS = NULL; + pObj->pBufMu[i].user = i; + } + pObj->pBufMu[i-1].pNext = &pObj->pBufMu[0]; + pObj->pBufMu[0].pLast = &pObj->pBufMu[i-1]; + + /* Zeiger auf Mu[k-p*S] */ + for (i=0; i < pObj->P*pObj->S; i++) + { + pMu = pObj->pBufMu[i].pLast; + for (s=1; s < pObj->S; s++) + pMu = pMu->pLast; + + pObj->pBufMu[i].pLastPS = pMu; + } + +#ifdef _DEBUG + printf("* Speichernutzung *\n"); + + /* Speichernutzung fuer Mu ausgeben */ + nBufMu = pObj->P*pObj->S; + memMu = pObj->P*pObj->S*pObj->C*sizeof(avfloat_t) + + pObj->P*pObj->S*sizeof(REALBUF); + + printf("Mu:\n"); + printf("Anzahl reelle Buffer : %d\n", nBufMu); + printf("Gesamt Buffergroesse : %d bytes\n", memMu); +#endif + + + /* Speicher fuer PX[C] (reell) */ + pObj->pBufPX = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + AvZeroMem(pObj->pBufPX,pObj->C*sizeof(avfloat_t)); + +#ifdef _DEBUG + /* Speichernutzung fuer PX ausgeben */ + nBufPX = 1; + memPX = pObj->C*sizeof(avfloat_t); + + printf("PX:\n"); + printf("Anzahl reelle Buffer : %d\n", nBufPX); + printf("Gesamt Buffergroesse : %d bytes\n", memPX); +#endif + + /* Ergebnis 'Y' der Faltung (komplex) */ + pObj->pTemp = (COMPLEX*)AvMemAlloc(sizeof(COMPLEX)); + pObj->pTemp->pReal = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + pObj->pTemp->pImag = (avfloat_t*)AvMemAlloc(pObj->C*sizeof(avfloat_t)); + AvZeroMem(pObj->pTemp->pReal,pObj->C*sizeof(avfloat_t)); + AvZeroMem(pObj->pTemp->pImag,pObj->C*sizeof(avfloat_t)); + + #ifdef _DEBUG + /* Speichernutzung fuer Y ausgeben */ + nBufY = 1; + memY = pObj->C*sizeof(CMPXBUF); + + printf("Y:\n"); + printf("Anzahl complexe Buffer : %d\n", nBufY); + printf("Gesamt Buffergroesse : %d bytes\n", memY); +#endif + + /* FFT initialisieren */ + pObj->pFFT = (FFT*)AvMemAlloc(sizeof(FFT)); + FFTinit(pObj->pFFT, pObj->C); + + /* Speicher fuer WS[P][C] (komplex) zuweisen */ + PfftFilterAlloc(pObj->pFilter, &pObj->pBufWS); + + /* Arbeitszeiger initialisieren */ + pObj->pX = pObj->pFilter->pBufX; + pObj->pPj = pObj->pBufWS; + pObj->pMu = pObj->pBufMu; + + /* Sicherheitskonstannte vermeidet Division durch Null */ + pObj->gamma = (avfloat_t)1.0/pObj->C; + + return AV_E_OK; +} + +/*-----------------------------------------------------------------------*/ +/* Partitioned FLMS Filterinitialisierung +/* 1. Partitionierung der Filterstartwerte in P-Partitionen +/* 2. Transformation der P Teil-Filter in den Frequenzbereich +/* Element pBufWS wird veraendert +/*-----------------------------------------------------------------------*/ +AVERR PflmsInitFilter( +PFLMS *pObj, /* Zeiger auf Objekt */ +avfloat_t *pWTD) /* N Filterkoeffizienten im Zeitbereich */ +{ + return PfftFilterInit(pObj->pFilter, pWTD, pObj->pBufWS); +} + +/*-----------------------------------------------------------------*/ +/* Partitioned FLMS +/*-----------------------------------------------------------------*/ +AVERR Pflms( +PFLMS *pObj, /* Zeiger auf Objekt */ +avfloat_t *pInTDx, /* Ein: Daten x[0..L-1] */ +avfloat_t *pInTDd, /* Ein: Referenzsignal d[0..L-1] */ +avfloat_t *pOutTDy, /* Aus: Filterausgang y[0..L-1] */ +avfloat_t *pOutTDe, /* Aus: Fehlersignal e[0..L-1] */ +avfloat_t alpha) +{ + + UINT32 i, p, NP, C_L; + + CMPXBUF *pX, *pWS, *pPj; + REALBUF *pMu; + COMPLEX *pTemp; + avfloat_t muNom; + + /* Arbeitszeiger */ + pX = pObj->pX; /* Aktueller Zeiger X[k] */ + pWS = pObj->pBufWS; /* Aktueller Zeiger WS[0] */ + pPj = pObj->pPj; /* Aktueller Zeiger Pj[] */ + pMu = pObj->pMu; /* Aktueller Zeiger Mu[k] */ + pTemp = pObj->pTemp; /* 'Y' wird nach der Filterung als + temporaerer Speicher benutzt */ + + NP = pObj->S*pObj->L; /* N/P = S*L */ + C_L = pObj->C - pObj->L; /* C-L */ + + /*---------------------------------------------------------------*/ + /* Partitioned FFT-Filterung + /*---------------------------------------------------------------*/ + /* TODO: PFFT Kommentar */ + PfftFilter(pObj->pFilter, pObj->pBufWS, pInTDx, pOutTDy); + + /*---------------------------------------------------------------*/ + /* Fehlersignal 'e = d - y' berechnen */ + /* Transformation von 'e' in den Frequenzbereich */ + /*---------------------------------------------------------------*/ + for (i=0; i < pObj->L; i++) + pOutTDe[i] = pInTDd[i] - pOutTDy[i]; + + /* Variable Y wird fuer E missbraucht, da nicht mehr gebraucht */ + /* Imaginaerteil von 'Y' auf Null setzen Im{Y[0..C-1]} = {0} */ + + AvZeroMem(pTemp->pImag,pObj->C*sizeof(avfloat_t)); + + /* Re{Y[0..C-L-1]} = {0} */ + AvZeroMem(pTemp->pReal,C_L*sizeof(avfloat_t)); + + /* Re{Y[C-L..C-1]} = e[0..L-1] */ + AvMemCpy(&pTemp->pReal[C_L], + pOutTDe,pObj->L*sizeof(avfloat_t)); + + /* E = 1/C*fft{e[0[0..C-L-1],e[0..L]} */ + ffts(pObj->pFFT, pTemp->pReal, pTemp->pImag); + + /*---------------------------------------------------------------*/ + /* Berechnung PX und Mu + /*---------------------------------------------------------------*/ + /* Zaehler Alpha*Gamma/P */ + muNom = (avfloat_t)(alpha*pObj->gamma/pObj->P); + + for (i=0; i < pObj->C; i++) + { + /* Schaetzung der mittleren Eingangsleistung PX */ + pObj->pBufPX[i] = (avfloat_t)fabs((1.0-LAMBDA) * pObj->C + * (pX->cmpxData.pReal[i]*pX->cmpxData.pReal[i] + + pX->cmpxData.pImag[i]*pX->cmpxData.pImag[i]) + + LAMBDA*pObj->pBufPX[i]); + + /* Berechnung der variablen Schrittweite Mu */ + /* mu[k] = (Alpha*Gamma) / (P*(PX+Gamma)) */ + pObj->pMu->pData[i] = muNom / (pObj->pBufPX[i] + pObj->gamma); + } + + /* Aktualisierung der Filterkoeffizienten */ + for (p=0; p < pObj->P; p++) + { + for (i=0; i < pObj->C; i++) + { + pWS->cmpxData.pReal[i] += ((pX->cmpxData.pReal[i] * pTemp->pReal[i] + + pX->cmpxData.pImag[i] * pTemp->pImag[i]) + * pMu->pData[i] * pObj->C); + pWS->cmpxData.pImag[i] += ((pX->cmpxData.pReal[i] * pTemp->pImag[i] + - pX->cmpxData.pImag[i] * pTemp->pReal[i]) + * pMu->pData[i] * pObj->C); + } + pWS = pWS->pNext; + pX = pX->pLastPS; + pMu= pMu->pLastPS; + } + + /* Effiziente Projektion der Filterkoeffizienten */ + ifft(pObj->pFFT, pPj->cmpxData.pReal, pPj->cmpxData.pImag); + AvZeroMem(pPj->cmpxData.pImag, pObj->C*sizeof(avfloat_t)); + AvZeroMem(&pPj->cmpxData.pReal[NP],(pObj->C-NP)*sizeof(avfloat_t)); + ffts(pObj->pFFT, pPj->cmpxData.pReal, pPj->cmpxData.pImag); + + /* Fuer den naechsten Aufruf Zeiger aktualisieren */ + /* Naechstes X[k] ist: */ + pObj->pX = pObj->pX->pNext; + + /* Naechstes Mu[k] ist: */ + pObj->pMu = pObj->pMu->pNext; + + /* Naechstes Teilfilter fuer Projektion ist: */ + pObj->pPj = pObj->pPj->pNext; + + return AV_E_OK; + +} + +/*------------------------------------------------------------------*/ + diff --git a/pflms.h b/pflms.h new file mode 100755 index 0000000..2d6bacd --- /dev/null +++ b/pflms.h @@ -0,0 +1,39 @@ +/*------------------------------------------------------------------------- + * pflms.h Partioned Frequency Least-Mean-Square adaptive algorithm + * $Id: $ + *------------------------------------------------------------------------- + * + * Copyright (C) 2001 Algo Vision Systems GmbH + * + *------------------------------------------------------------------------- + */ + +#ifndef _PFLMS_H +#define _PFLMS_H +#include "pfft.h" + +/*-----------------------------------------------------------------------*/ +/* PFLMS-Parameter */ +/*-----------------------------------------------------------------------*/ + +typedef struct _sPFLMS +{ + UINT32 N, P, S, L, C; /* PFLMS-Parameter N, P, S, L, C */ + avfloat_t gamma; /* Sicherheitskonstannte */ + CMPXBUF *pBufWS; /* komplexe Puffer X[P*S][C], WS[P][C] */ + CMPXBUF *pX, *pWS, *pPj; /* aktuelle Zeiger auf Puffer[p][] */ + COMPLEX *pTemp; /* temporaerer Puffer fuer Y[] und E[] */ + REALBUF *pBufMu; /* reeller Puffer Mu[P*S][C] */ + REALBUF *pMu; /* Aktueller zeiger auf Mu[k-p*S] */ + avfloat_t *pBufPX; /* reeller Puffer PX[C] */ + FFT *pFFT; /* FFT-Objekt */ + PFFT *pFilter; /* Filter-Objekt */ +} PFLMS; + + +AVERR PflmsInit(PFLMS *pObj,UINT32 N, UINT32 P, UINT32 S, UINT32 L, UINT32 C); +AVERR PflmsInitFilter(PFLMS *pObj, avfloat_t *pWTD); +AVERR Pflms(PFLMS *pObj, avfloat_t *pInTDx, avfloat_t *pInTDd, avfloat_t *pOutTDy, avfloat_t *pOutTDe, avfloat_t alpha); + +#endif /* _PFLMS_H */ + diff --git a/pflms2.c b/pflms2.c new file mode 100755 index 0000000..3f035d4 --- /dev/null +++ b/pflms2.c @@ -0,0 +1,454 @@ +/*------------------------------------------------------------------------- + * pflms.c Partioned Frequency Least-Mean-Square adaptive algorithm + * $Id: $ + *------------------------------------------------------------------------- + * + * Copyright (C) 2001 Algo Vision Systems GmbH + * + *------------------------------------------------------------------------- + */ +#define AVNEEDFLOAT + +#ifdef _DEBUG +#include +#endif +#include +#include "avtypes.h" +#include "averror.h" +#include "avrtl.h" +#include "avfft.h" +#include "pfft.h" +#include "pflms.h" + +/*-----------------------------------------------------------------------*/ +#define LAMBDA 0.6 +#define pTHIS pObj + +AVERR PflmsInit( +PFLMS *pObj, /* Zeiger auf Objekt */ +UINT32 N, /* Filterlaenge N */ +UINT32 P, /* Anzahl Filterpartitionen */ +UINT32 S, /* Anzahl Filtersegmente pro Partition */ +UINT32 L, /* Laenge der Eingangsdaten (Blocklaenge */ +UINT32 C /* FFT-Laenge */ +) +{ + UINT32 i, s, NP, C_L; + CMPXBUF *pX; /* Temporaerer Zeiger fuer Bufferinit.*/ + REALBUF *pMu; /* Temporaerer Zeiger fuer Bufferinit.*/ + +#ifdef _DEBUG + UINT32 memMu, memPX, memX, memWS, memY, memXs; + UINT32 nBufMu, nBufPX, nBufX, nBufWS, nBufY, nBufXs; +#endif + + /* Auto-Param */ + if ((N*L)==0) + return AV_E_FAIL; + + if (P==0) + { + if ((N%L) != 0) + return AV_E_FAIL; + P = N/L; + } + + /* Auto-guess FFT-Groesse C */ + if (C==0) + pObj->C = (UINT32)pow(2,ceil(log(L+N/P-1)/log(2.0))); + + /* Objekt initialisieren */ + pObj->L = L; + pObj->N = N; + pObj->P = P; + pObj->S = S; + NP = pObj->S * pObj->L; + C_L = pObj->C - pObj->L; + +#ifdef _DEBUG + printf("PFLMS Init:\n"); + printf("N = %d\n",pObj->N); + printf("P = %d\n",pObj->P); + printf("S = %d\n",pObj->S); + printf("L = %d\n",pObj->L); + printf("NP = %d\n",NP); + printf("C = %d\n",pObj->C); + printf("C-L = %d\n",C_L); +#endif + + /*-------------------------------------------------------*/ + /* Speicher allokieren */ + /*-------------------------------------------------------*/ + /* Speicher fuer Mu[P*S][C] (reell) */ + pObj->pBufMu = (REALBUF*)AvMemAlloc(pObj->P*pObj->S*sizeof(REALBUF)); + + for (i=0; i < pObj->P*pObj->S; i++) + { + pObj->pBufMu[i].pData = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32)); + AvZeroMem(pObj->pBufMu[i].pData,pObj->C*sizeof(FLOAT32)); + pObj->pBufMu[i].pNext = &pObj->pBufMu[i+1]; + pObj->pBufMu[i].pLast = &pObj->pBufMu[i-1]; + pObj->pBufMu[i].pLastPS = NULL; + pObj->pBufMu[i].user = i; + } + pObj->pBufMu[i-1].pNext = &pObj->pBufMu[0]; + pObj->pBufMu[0].pLast = &pObj->pBufMu[i-1]; + + /* Zeiger auf Mu[k-p*S] */ + for (i=0; i < pObj->P*pObj->S; i++) + { + pMu = pObj->pBufMu[i].pLast; + for (s=1; s < pObj->S; s++) + pMu = pMu->pLast; + + pObj->pBufMu[i].pLastPS = pMu; + } + +#ifdef _DEBUG + printf("* Speichernutzung *\n"); + + /* Speichernutzung fuer Mu ausgeben */ + nBufMu = pObj->P*pObj->S; + memMu = pObj->P*pObj->S*pObj->C*sizeof(FLOAT32) + + pObj->P*pObj->S*sizeof(REALBUF); + + printf("Mu:\n"); + printf("Anzahl reelle Buffer : %d\n", nBufMu); + printf("Gesamt Buffergroesse : %d bytes\n", memMu); +#endif + + + /* Speicher fuer PX[C] (reell) */ + pObj->pBufPX = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32)); + AvZeroMem(pObj->pBufPX,pObj->C*sizeof(FLOAT32)); + +#ifdef _DEBUG + /* Speichernutzung fuer PX ausgeben */ + nBufPX = 1; + memPX = pObj->C*sizeof(FLOAT32); + + printf("PX:\n"); + printf("Anzahl reelle Buffer : %d\n", nBufPX); + printf("Gesamt Buffergroesse : %d bytes\n", memPX); +#endif + + + /* Speicher fuer X[P*S][C] (komplex) */ + pObj->pBufX = (CMPXBUF*)AvMemAlloc(pObj->P*pObj->S*sizeof(CMPXBUF)); + + for (i=0; i < pObj->P*pObj->S; i++) + { + pObj->pBufX[i].cmpxData.pReal = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32)); + pObj->pBufX[i].cmpxData.pImag = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32)); + AvZeroMem(pObj->pBufX[i].cmpxData.pReal,pObj->C*sizeof(FLOAT32)); + AvZeroMem(pObj->pBufX[i].cmpxData.pImag,pObj->C*sizeof(FLOAT32)); + pObj->pBufX[i].pNext = &pObj->pBufX[i+1]; + pObj->pBufX[i].pLast = &pObj->pBufX[i-1]; + pObj->pBufX[i].pLastPS = NULL; + pObj->pBufX[i].user = i; + } + pObj->pBufX[i-1].pNext = &pObj->pBufX[0]; + pObj->pBufX[0].pLast = &pObj->pBufX[i-1]; + + /* Zeiger auf X[k-p*S] */ + for (i=0; i < pObj->P*pObj->S; i++) + { + pX = pObj->pBufX[i].pLast; + for (s=1; s < pObj->S; s++) + pX = pX->pLast; + + pObj->pBufX[i].pLastPS = pX; + } + +#ifdef _DEBUG + /* Speichernutzung fuer X ausgeben */ + nBufX = pObj->P*pObj->S; + memX = pObj->P*pObj->S*pObj->C*sizeof(CMPXBUF); + + printf("X:\n"); + printf("Anzahl complexe Buffer : %d\n", nBufX); + printf("Gesamt Buffergroesse : %d bytes\n", memX); +#endif + + /* Speicher fuer WS[P][C] (komplex) */ + pObj->pBufWS = (CMPXBUF*)AvMemAlloc(P*S*sizeof(CMPXBUF)); + + for (i=0; i < P*S; i++) + { + pObj->pBufWS[i].cmpxData.pReal = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32)); + pObj->pBufWS[i].cmpxData.pImag = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32)); + AvZeroMem(pObj->pBufWS[i].cmpxData.pReal,pObj->C*sizeof(FLOAT32)); + AvZeroMem(pObj->pBufWS[i].cmpxData.pImag,pObj->C*sizeof(FLOAT32)); + pObj->pBufWS[i].pNext = &pObj->pBufWS[i+1]; + pObj->pBufWS[i].pLast = &pObj->pBufWS[i-1]; + pObj->pBufWS[i].pLastPS = NULL; + pObj->pBufWS[i].user = i; + } + pObj->pBufWS[i-1].pNext = &pObj->pBufWS[0]; + pObj->pBufWS[0].pLast = &pObj->pBufWS[i-1]; + +#ifdef _DEBUG + /* Speichernutzung fuer WS ausgeben */ + nBufWS = pObj->P; + memWS = pObj->P*pObj->C*sizeof(CMPXBUF); + + printf("WS:\n"); + printf("Anzahl complexe Buffer : %d\n", nBufWS); + printf("Gesamt Buffergroesse : %d bytes\n", memWS); +#endif + + /* Ergebnis 'Y' der Faltung (komplex) */ + pObj->pTemp = (COMPLEX*)AvMemAlloc(sizeof(COMPLEX)); + pObj->pTemp->pReal = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32)); + pObj->pTemp->pImag = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32)); + AvZeroMem(pObj->pTemp->pReal,pObj->C*sizeof(FLOAT32)); + AvZeroMem(pObj->pTemp->pImag,pObj->C*sizeof(FLOAT32)); + + #ifdef _DEBUG + /* Speichernutzung fuer Y ausgeben */ + nBufY = 1; + memY = pObj->C*sizeof(CMPXBUF); + + printf("Y:\n"); + printf("Anzahl complexe Buffer : %d\n", nBufY); + printf("Gesamt Buffergroesse : %d bytes\n", memY); +#endif + + /* Overlap-Save 'xs' (reell) */ + pObj->pBufXsave = (FLOAT32*)AvMemAlloc(C_L*sizeof(FLOAT32)); + AvZeroMem(pObj->pBufXsave, C_L*sizeof(FLOAT32)); + +#ifdef _DEBUG + /* Speichernutzung fuer Xsave ausgeben */ + nBufXs = 1; + memXs = C_L*sizeof(FLOAT32); + + printf("Xsave:\n"); + printf("Anzahl reelle Buffer : %d\n", nBufXs); + printf("Gesamt Buffergroesse : %d bytes\n", memXs); +#endif + + /* FFT initialisieren */ + pObj->pFFT = (FFT*)AvMemAlloc(sizeof(FFT)); + FFTinit(pObj->pFFT, pObj->C); + + /* Arbeitszeiger initialisieren */ + pObj->pX = pObj->pBufX; + pObj->pPj = pObj->pBufWS; + pObj->pMu = pObj->pBufMu; + + /* Sicherheitskonstannte vermeidet Division durch Null */ + pObj->gamma = (FLOAT32)1.0/pObj->C; + + return AV_E_OK; +} + +/*-----------------------------------------------------------------------*/ +/* Partitioned FLMS Filterinitialisierung +/* 1. Partitionierung der Filterstartwerte in P-Partitionen +/* 2. Transformation der P Teil-Filter in den Frequenzbereich +/* Element pBufWS wird veraendert +/*-----------------------------------------------------------------------*/ +AVERR PflmsInitFilter( +PFLMS *pObj, /* Zeiger auf Objekt */ +FLOAT32 *pWTD) /* N Filterkoeffizienten im Zeitbereich */ +{ + UINT32 p, NP, C_SL; + + NP = pObj->S * pObj->L; + C_SL = pObj->C - NP; + + for (p=0; p < pObj->P; p++) + { + /* Arbeitspuffer WS auffuellen + Re{WS[p][0..C-L-1]}=wi[p*N/P..(p+1)*N/P-1] */ + AvMemCpy(pObj->pBufWS[p].cmpxData.pReal, + &pWTD[p*NP], NP*sizeof(FLOAT32)); + + /* Re{WS[C-L..C]} = {0} (vorsichtshalber) */ + AvZeroMem(&pObj->pBufWS[p].cmpxData.pReal[NP], + C_SL *sizeof(FLOAT32)); + + /* Im{WS[0..C-1]} = {0} */ + AvZeroMem(pObj->pBufWS[p].cmpxData.pImag, pObj->C *sizeof(FLOAT32)); + + /* In den Frequenzbereich transformieren + WS[p][0..C-1] = 1/C*FFT{wi[p*N/P..(p+1)*N/P-1]} */ + ffts(pObj->pFFT, pObj->pBufWS[p].cmpxData.pReal, + pObj->pBufWS[p].cmpxData.pImag); + } + return AV_E_OK; +} + +/*-----------------------------------------------------------------*/ +/* Partitioned FLMS +/*-----------------------------------------------------------------*/ +AVERR Pflms( +PFLMS *pObj, /* Zeiger auf Objekt */ +FLOAT32 *pInTDx, /* Ein: Daten x[0..L-1] */ +FLOAT32 *pInTDd, /* Ein: Referenzsignal d[0..L-1] */ +FLOAT32 *pOutTDy, /* Aus: Filterausgang y[0..L-1] */ +FLOAT32 *pOutTDe, /* Aus: Fehlersignal e[0..L-1] */ +FLOAT32 alpha) +{ + + UINT32 i, p, NP, C_L; + + CMPXBUF *pX, *pWS, *pPj; + REALBUF *pMu; + COMPLEX *pTemp; + FLOAT32 muNom; + + /*---------------------------------------------------------------*/ + /* Beginn: Partitioned FFT-Filterung + /*---------------------------------------------------------------*/ + /* TODO: Standalone Version der Partitioned-FFT-Filterung */ + + /* Arbeitszeiger */ + pX = pObj->pX; /* Aktueller Zeiger X[k] */ + pWS = pObj->pBufWS; /* Aktueller Zeiger WS[0] */ + pPj = pObj->pPj; /* Aktueller Zeiger Pj[] */ + pMu = pObj->pMu; /* Aktueller Zeiger Mu[k] */ + pTemp = pObj->pTemp; /* 'Y' wird nach der Filterung als + temporaerer Speicher benutzt */ + + NP = pObj->S*pObj->L; /* N/P = S*L */ + C_L = pObj->C - pObj->L; /* C-L */ + + + /* Arbeitspuffer 'X' auffuellen Re{X[S*L..C-1]} = x[0..L-1] */ + AvMemCpy(&pX->cmpxData.pReal[C_L],pInTDx, + pObj->L*sizeof(FLOAT32)); + + /* Saveblock 'xs' anfuegen Re{X[0..C-L-1]} = xs[0..C-L-1] */ + AvMemCpy(pX->cmpxData.pReal,pObj->pBufXsave, + C_L*sizeof(FLOAT32)); + + /* Saveblock aktualisieren xs[0..C-L-1] = x[L..C-1] */ + AvMemCpy(pObj->pBufXsave, &pX->cmpxData.pReal[pObj->L], + C_L*sizeof(FLOAT32)); + + /* Imaginaerteil von 'X' auf Null setzen Im{X[0..C-1]} = {0} */ + AvZeroMem(pX->cmpxData.pImag,pObj->C*sizeof(FLOAT32)); + + /* X = 1/C*FFT{x} */ + ffts(pObj->pFFT, pX->cmpxData.pReal, pX->cmpxData.pImag); + + /* 1. Partition Multiplikation im Frequenzbereich + Y = X[k][0..C-1] * H[0][0..C-1] * C */ + CmpxVectMulS(&pX->cmpxData, &pWS->cmpxData, + pTemp, pObj->C); + + /* 2. Partition bis P-te Partition */ + for (p=1; p < pObj->P; p++) + { + pWS = pWS->pNext; + + /* X[k-p*S] suchen */ + pX = pX->pLastPS; + + /* Y = X[k-p*S][0..C-1] * H[p][0..C-1] * C */ + CmpxVectMacS(&pX->cmpxData, &pWS->cmpxData, + pTemp, pObj->C); + } + + /* Zeiger wieder WS[0] setzten */ + pWS = pObj->pBufWS; + + /* Zeiger wieder auf aktuellen Block X[k] setzten */ + pX = pObj->pX; + + /* In den Zeitbereich transformieren, y = IFFT{Y} */ + ifft(pObj->pFFT, pTemp->pReal, pTemp->pImag); + + /* Abspeichern der letzten L Daten ys[0..L-1] = Re{Y[C-L..C-1] */ + AvMemCpy(pOutTDy, &pTemp->pReal[C_L], + pObj->L*sizeof(FLOAT32)); + + /*---------------------------------------------------------------*/ + /* Ende: Partitioned FFT-Filterung + /*---------------------------------------------------------------*/ + /* Die Variable 'Y' wird nun nicht mehr gebraucht. + /* In den Nachfolgenden Abschnitten wird 'Y' als temporaerer + /* Speicher benutzt. + + /*---------------------------------------------------------------*/ + /* Fehlersignal 'e = d - y' berechnen */ + /* Transformation von 'e' in den Frequenzbereich */ + /*---------------------------------------------------------------*/ + + for (i=0; i < pObj->L; i++) + pOutTDe[i] = pInTDd[i] - pOutTDy[i]; + + /* Variable Y wird fuer E missbraucht, da nicht mehr gebraucht */ + /* Imaginaerteil von 'Y' auf Null setzen Im{Y[0..C-1]} = {0} */ + + AvZeroMem(pTemp->pImag,pObj->C*sizeof(FLOAT32)); + + /* Re{Y[0..C-L-1]} = {0} */ + AvZeroMem(pTemp->pReal,C_L*sizeof(FLOAT32)); + + /* Re{Y[C-L..C-1]} = e[0..L-1] */ + AvMemCpy(&pTemp->pReal[C_L], + pOutTDe,pObj->L*sizeof(FLOAT32)); + + /* E = 1/C*fft{e[0[0..C-L-1],e[0..L]} */ + ffts(pObj->pFFT, pTemp->pReal, pTemp->pImag); + + /*---------------------------------------------------------------*/ + /* Berechnung PX und Mu + /*---------------------------------------------------------------*/ + /* Zaehler Alpha*Gamma/P */ + muNom = (FLOAT32)(alpha*pObj->gamma/pObj->P); + + for (i=0; i < pObj->C; i++) + { + /* Schaetzung der mittleren Eingangsleistung PX */ + pObj->pBufPX[i] = (FLOAT32)fabs((pX->cmpxData.pReal[i]*pX->cmpxData.pReal[i] + + pX->cmpxData.pImag[i]*pX->cmpxData.pImag[i]) + * (1.0-LAMBDA) * pObj->C + LAMBDA*pObj->pBufPX[i]); + + /* Berechnung der variablen Schrittweite Mu */ + /* mu[k] = (Alpha*Gamma) / (P*(PX+Gamma)) */ + pObj->pMu->pData[i] = muNom / (pObj->pBufPX[i] + pObj->gamma); + } + + /* Aktualisierung der Filterkoeffizienten */ + for (p=0; p < pObj->P; p++) + { + for (i=0; i < pObj->C; i++) + { + pWS->cmpxData.pReal[i] += ((pX->cmpxData.pReal[i] * pTemp->pReal[i] + + pX->cmpxData.pImag[i] * pTemp->pImag[i]) + * pMu->pData[i] * pObj->C); + pWS->cmpxData.pImag[i] += ((pX->cmpxData.pReal[i] * pTemp->pImag[i] + - pX->cmpxData.pImag[i] * pTemp->pReal[i]) + * pMu->pData[i] * pObj->C); + } + pWS = pWS->pNext; + pX = pX->pLastPS; + pMu= pMu->pLastPS; + } + + /* Effiziente Projektion der Filterkoeffizienten */ + ifft(pObj->pFFT, pPj->cmpxData.pReal, pPj->cmpxData.pImag); + AvZeroMem(pPj->cmpxData.pImag, pObj->C*sizeof(FLOAT32)); + AvZeroMem(&pPj->cmpxData.pReal[NP],(pObj->C-NP)*sizeof(FLOAT32)); + ffts(pObj->pFFT, pPj->cmpxData.pReal, pPj->cmpxData.pImag); + + /* Fuer den naechsten Aufruf Zeiger aktualisieren */ + /* Naechstes X[k] ist: */ + pObj->pX = pObj->pX->pNext; + + /* Naechstes Mu[k] ist: */ + pObj->pMu = pObj->pMu->pNext; + + /* Naechstes Teilfilter fuer Projektion ist: */ + pObj->pPj = pObj->pPj->pNext; + + return AV_E_OK; + +} + +/*------------------------------------------------------------------*/ + diff --git a/pflms2.h b/pflms2.h new file mode 100755 index 0000000..17b459f --- /dev/null +++ b/pflms2.h @@ -0,0 +1,39 @@ +/*------------------------------------------------------------------------- + * pflms.h Partioned Frequency Least-Mean-Square adaptive algorithm + * $Id: $ + *------------------------------------------------------------------------- + * + * Copyright (C) 2001 Algo Vision Systems GmbH + * + *------------------------------------------------------------------------- + */ + +#ifndef _PFLMS_H +#define _PFLMS_H +#include "pfft.h" + +/*-----------------------------------------------------------------------*/ +/* PFLMS-Parameter */ +/*-----------------------------------------------------------------------*/ + +typedef struct _sPFLMS +{ + UINT32 N, P, S, L, C; /* PFLMS-Parameter N, P, S, L, C */ + FLOAT32 gamma; /* Sicherheitskonstannte */ + CMPXBUF *pBufX, *pBufWS; /* komplexe Puffer X[P*S][C], WS[P][C] */ + CMPXBUF *pX, *pWS, *pPj; /* aktuelle Zeiger auf Puffer[p][] */ + COMPLEX *pTemp; /* temporaerer Puffer fuer Y[] und E[] */ + REALBUF *pBufMu; /* reeller Puffer Mu[P*S][C] */ + REALBUF *pMu; /* Aktueller zeiger auf Mu[k-p*S] */ + FLOAT32 *pBufPX; /* reeller Puffer PX[C] */ + FLOAT32 *pBufXsave; /* Overlap-Save Puffer xs[S*L] */ + FFT *pFFT; /* FFT-Objekt */ +} PFLMS; + + +AVERR PflmsInit(PFLMS *pObj,UINT32 N, UINT32 P, UINT32 S, UINT32 L, UINT32 C); +AVERR PflmsInitFilter(PFLMS *pObj, FLOAT32 *pWTD); +AVERR Pflms(PFLMS *pObj, FLOAT32 *pInTDx, FLOAT32 *pInTDd, FLOAT32 *pOutTDy, FLOAT32 *pOutTDe, FLOAT32 alpha); + +#endif /* _PFLMS_H */ + diff --git a/pflms_c.bak b/pflms_c.bak new file mode 100755 index 0000000..dbe1592 --- /dev/null +++ b/pflms_c.bak @@ -0,0 +1,228 @@ +/*------------------------------------------------------------------------- + * pflms.c Partioned Frequency Least-Mean-Square adaptive algorithm + * $Id: $ + *------------------------------------------------------------------------- + * + * Copyright (C) 2001 Algo Vision Systems GmbH + * + *------------------------------------------------------------------------- + */ +#define AVNEEDFLOAT + +#include +#include "../../../include/avtypes.h" +#include "../../../include/averror.h" +#include "../../../include/avrtl.h" +#include "../../../include/fft.h" +#include "pflms.h" + +/*-----------------------------------------------------------------------*/ + +AVERR PflmsInit( +PFLMS *pObj, /* Zeiger auf PFLMS-Objekt */ +UINT32 N, +UINT32 P, +UINT32 S, +UINT32 L, +UINT32 C +) +{ + UINT32 i, NP; + + /* Auto-Param */ + if ((N*L)==0) + return AV_E_FAIL; + + if (P==0) + { + if ((N%L) != 0) + return AV_E_FAIL; + P = N/L; + } + + /* Auto-guess FFT-Groesse C */ + if (C==0) + pObj->C = (UINT32)pow(2,ceil(log(L+N/P-1)/log(2.0))); + + /* Objekt initialisieren */ + pObj->L = L; + pObj->N = N; + pObj->P = P; + pObj->S = S; + NP = pObj->S * pObj->L; + + /*-------------------------------------------------------*/ + /* Speicher allokieren */ + /*-------------------------------------------------------*/ + /* Speicher fuer Mu[P*S][C] (reell) */ + pObj->pMu = (REALBUF*)AvMemAlloc(P*S*sizeof(REALBUF)); + + for (i=0; i < P*S; i++) + { + pObj->pMu[i].pData = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32)); + AvZeroMem(pObj->pMu[i].pData,pObj->C*sizeof(FLOAT32)); + pObj->pMu[i].pNext = &pObj->pMu[i+1]; + pObj->pMu[i].pLast = &pObj->pMu[i-1]; + pObj->pMu[i].user = i; + } + pObj->pMu[i-1].pNext = &pObj->pMu[0]; + pObj->pMu[0].pLast = &pObj->pMu[i-1]; + + /* Speicher fuer PX[C] (reell) */ + pObj->pPX = (REALBUF*)AvMemAlloc(sizeof(REALBUF)); + pObj->pPX->pData = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32)); + AvZeroMem(pObj->pPX->pData,pObj->C*sizeof(FLOAT32)); + pObj->pPX->pNext = pObj->pPX; + pObj->pPX->pLast = pObj->pPX; + pObj->pPX->user = 0; + + + /* Speicher fuer X[P*S][C] (complex) */ + pObj->pX = (CMPXBUF*)AvMemAlloc(P*S*sizeof(CMPXBUF)); + + for (i=0; i < P*S; i++) + { + pObj->pX[i].cmpxData.pReal = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32)); + pObj->pX[i].cmpxData.pImag = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32)); + AvZeroMem(pObj->pX[i].cmpxData.pReal,pObj->C*sizeof(FLOAT32)); + AvZeroMem(pObj->pX[i].cmpxData.pImag,pObj->C*sizeof(FLOAT32)); + pObj->pX[i].pNext = &pObj->pX[i+1]; + pObj->pX[i].pLast = &pObj->pX[i-1]; + pObj->pX[i].user = i; + } + pObj->pX[i-1].pNext = &pObj->pX[0]; + pObj->pX[0].pLast = &pObj->pX[i-1]; + + + /* Speicher fuer WS[P][C] (complex) */ + pObj->pWS = (CMPXBUF*)AvMemAlloc(P*S*sizeof(CMPXBUF)); + + for (i=0; i < P*S; i++) + { + pObj->pWS[i].pData = (COMPLEX*)AvMemAlloc(pObj->C*sizeof(COMPLEX)); + AvZeroMem(pObj->pWS[i].pData,pObj->C*sizeof(COMPLEX)); + pObj->pWS[i].pNext = &pObj->pWS[i+1]; + pObj->pWS[i].pLast = &pObj->pWS[i-1]; + pObj->pWS[i].user = i; + + } + pObj->pWS[i-1].pNext = &pObj->pWS[0]; + pObj->pWS[0].pLast = &pObj->pWS[i-1]; + pObj->pPj = pObj->pWS; /* aktueller Buffer fuer Projektion */ + + /* Ergebnis 'Y' der Faltung (complex) */ + pObj->pY = (COMPLEX*)AvMemAlloc(pObj->C*sizeof(COMPLEX)); + AvZeroMem(pObj->pY,pObj->C*sizeof(COMPLEX)); + + /* Overlap-Save 'xs' */ + pObj->pXsave = (FLOAT32*)AvMemAlloc(NP*sizeof(FLOAT32)); + AvZeroMem(pObj->pXsave,NP*sizeof(FLOAT32)); + + /* FFT initialisieren */ + pObj->pFFT = (FFT*)AvMemAlloc(sizeof(FFT)); + FFTinit(pObj->pFFT, pObj->C); + + return AV_E_OK; +} + +/*-----------------------------------------------------------------------*/ +/* Partitioned FFT +/*-----------------------------------------------------------------------*/ +AVERR Pflms(PFLMS *pObj, FLOAT32 *pDataTD) +{ + + UINT32 p, s, NP; + + CMPXBUF *pX, *pWS; + NP = pObj->S*pObj->L; + + pX = pObj->pX; /* Aktueller Zeiger *X[C] */ + pWS = pObj->pWS; /* Aktueller Zeiger *WS[C] */ + + /* Arbeitspuffer 'X' auffuellen Re{X[S*L..C-1]} = x[0..L] */ + AvMemCpy(&pX->pData[NP].real,pDataTD, pObj->L*sizeof(FLOAT32)); + + /* letzten Saveblock 'xs' anfuegen Re{X[0..S*L-1]} = xs[0..S*L-1] */ + AvMemCpy(&pX->pData->real,pObj->pXsave, NP*sizeof(FLOAT32)); + + /* Saveblock aktualisieren xs[0..S*L-1] = x[L..C-1] */ + AvMemCpy(pObj->pXsave, &pDataTD[pObj->L], NP*sizeof(FLOAT32)); + + /* Imaginaerteil von 'X' auf Null setzen Im{X[0..C-1]} = 0 */ + AvZeroMem(&pX->pData->imag,pObj->C*sizeof(FLOAT32)); + + /* X = FFT{x} */ + fft(pObj->pFFT, &pX->pData->real, &pX->pData->imag); + + /* 1. Partition Faltung im Frequenzbereich Y = X * H */ + CmpxVectMul(pX->pData, pWS->pData, pObj->pY, pObj->C); + + /* 2. Partition bis P-te Partition */ + for (p=1; p < pObj->P; p++) + { + pWS = pWS->pNext; + + for (s=0; s < pObj->S; s++) + pX = pX->pLast; + + CmpxVectMac(pX->pData, pWS->pData, pObj->pY, pObj->C); + } + + /* y = IFFT{Y} */ + ifft(pObj->pFFT, &pObj->pY->real, &pObj->pY->imag); + + /* Abspeichern der letzten L Daten */ + AvMemCpy(pDataTD, &pObj->pY[NP].real, pObj->L*sizeof(FLOAT32)); + + return AV_E_OK; + +} + +/*-----------------------------------------------------------------------*/ +/* Complex-Funktionen +/*-----------------------------------------------------------------------*/ +void CmpxVectMul(struct _sCOMPLEX *pA, struct _sCOMPLEX *pB, struct _sCOMPLEX *pAB, UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAB[i].real = pA[i].real*pB[i].real - pA[i].imag*pB[i].imag; + pAB[i].imag = pA[i].real*pB[i].imag + pA[i].imag*pB[i].real; + } +} + +void CmpxVectAdd(struct _sCOMPLEX *pA, struct _sCOMPLEX *pB, struct _sCOMPLEX *pAB, UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAB[i].real = pA[i].real + pB[i].real; + pAB[i].imag = pA[i].imag + pB[i].imag; + } +} + +void CmpxVectMac(struct _sCOMPLEX *pA, struct _sCOMPLEX *pB, struct _sCOMPLEX *pAB, UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAB[i].real += pA[i].real*pB[i].real - pA[i].imag*pB[i].imag; + pAB[i].imag += pA[i].real*pB[i].imag + pA[i].imag*pB[i].real; + } +} + +void RealVectSquConj(struct _sCOMPLEX *pA, FLOAT32 *pAA, UINT32 len) +{ + UINT32 i; + + for (i=0; i < len; i++) + { + pAA[i] = pA[i].real*pA[i].real + pA[i].imag*pA[i].imag; + } +} + +/*-----------------------------------------------------------------------*/ +