Initial import
git-svn-id: http://moon:8086/svn/software/trunk/libsrc/avpflms@1 b431acfa-c32f-4a4a-93f1-934dc6c82436
This commit is contained in:
Executable
+119
@@ -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
|
||||
Executable
+29
@@ -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>
|
||||
{{{
|
||||
}}}
|
||||
|
||||
###############################################################################
|
||||
|
||||
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
+32
@@ -0,0 +1,32 @@
|
||||
<html>
|
||||
<body>
|
||||
<pre>
|
||||
<h1>Erstellungsprotokoll</h1>
|
||||
<h3>
|
||||
--------------------Konfiguration: libpflms - Win32 Release--------------------
|
||||
</h3>
|
||||
<h3>Befehlszeilen</h3>
|
||||
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 "
|
||||
<h3>Ausgabefenster</h3>
|
||||
Kompilierung läuft...
|
||||
avfft.c
|
||||
pfft.c
|
||||
pflms.c
|
||||
Generieren von Code...
|
||||
Bibliothek wird erstellt...
|
||||
|
||||
|
||||
|
||||
<h3>Ergebnisse</h3>
|
||||
libpflms.lib - 0 Fehler, 0 Warnung(en)
|
||||
</pre>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,356 @@
|
||||
/*-------------------------------------------------------------------------
|
||||
* pfft.c Partioned Fast Fourier Transform
|
||||
* $Id: $
|
||||
*-------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2001 Algo Vision Systems GmbH
|
||||
*
|
||||
*-------------------------------------------------------------------------
|
||||
*/
|
||||
#define AVNEEDFLOAT
|
||||
|
||||
#ifdef _DEBUG
|
||||
#include <stdio.h>
|
||||
#endif
|
||||
#include <math.h>
|
||||
#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;
|
||||
}
|
||||
|
||||
|
||||
/*------------------------------------------------------------------*/
|
||||
|
||||
Executable
+332
@@ -0,0 +1,332 @@
|
||||
/*-------------------------------------------------------------------------
|
||||
* pfft.c Partioned Fast Fourier Transform
|
||||
* $Id: $
|
||||
*-------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2001 Algo Vision Systems GmbH
|
||||
*
|
||||
*-------------------------------------------------------------------------
|
||||
*/
|
||||
#define AVNEEDFLOAT
|
||||
|
||||
#ifdef _DEBUG
|
||||
#include <stdio.h>
|
||||
#endif
|
||||
#include <math.h>
|
||||
#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;
|
||||
}
|
||||
}
|
||||
|
||||
/*------------------------------------------------------------------*/
|
||||
|
||||
@@ -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 */
|
||||
|
||||
Executable
+69
@@ -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 */
|
||||
|
||||
@@ -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 <stdio.h>
|
||||
#endif
|
||||
#include <math.h>
|
||||
#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;
|
||||
|
||||
}
|
||||
|
||||
/*------------------------------------------------------------------*/
|
||||
|
||||
@@ -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 */
|
||||
|
||||
@@ -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 <stdio.h>
|
||||
#endif
|
||||
#include <math.h>
|
||||
#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;
|
||||
|
||||
}
|
||||
|
||||
/*------------------------------------------------------------------*/
|
||||
|
||||
@@ -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 */
|
||||
|
||||
Executable
+228
@@ -0,0 +1,228 @@
|
||||
/*-------------------------------------------------------------------------
|
||||
* pflms.c Partioned Frequency Least-Mean-Square adaptive algorithm
|
||||
* $Id: $
|
||||
*-------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2001 Algo Vision Systems GmbH
|
||||
*
|
||||
*-------------------------------------------------------------------------
|
||||
*/
|
||||
#define AVNEEDFLOAT
|
||||
|
||||
#include <math.h>
|
||||
#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;
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------*/
|
||||
|
||||
Reference in New Issue
Block a user