jens/avpflms
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
master
avpflms/pflms.c
T
jens e93e7927cb Initial import 
git-svn-id: http://moon:8086/svn/software/trunk/libsrc/avpflms@1 b431acfa-c32f-4a4a-93f1-934dc6c82436
2014-07-19 07:44:42 +00:00

288 lines
10 KiB
C
Executable File
Raw Permalink Blame History

/*-------------------------------------------------------------------------
* 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;
}
/*------------------------------------------------------------------*/
Reference in New Issue View Git Blame Copy Permalink