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

432 lines
14 KiB
C
Executable File
Raw Permalink Blame History

#define AVNEEDFLOAT
#include "avtypes.h"
#include "averror.h"
#include "avrtl.h"
#include "math.h"
#include "aeclib.h"
#define LAMBDA 0.6
#define pTHIS pObj
/*************************************************************************/
/* Average Magnitude Estimation
/* Rekursive Schaetzung der mittleren Amplitude
/*************************************************************************/
void AECame(
FLOAT32 *pOut, /* Ausgabevektor */
FLOAT32 *pIn, /* Eingabevektor */
INT32 len, /* Laenge der Vektoren */
FLOAT32 a_rf, /* Anstiegs-und Abfallzeitkonstante */
FLOAT32 *ic) /* Initial Condition Anfangswert */
{
INT32 i;
register FLOAT32 arf2;
arf2 = (FLOAT32)(1.0-a_rf);
pOut[0] = a_rf*(FLOAT32)fabs(pIn[0]) + arf2* *ic;
for (i=1; i < len; i++)
pOut[i] = a_rf*(FLOAT32)fabs(pIn[i]) + arf2*pOut[i-1];
*ic = pOut[len-1];
}
/*************************************************************************/
/* Average Magnitude Estimation
/* Rekursive Schaetzung der mittleren Amplitude
/*************************************************************************/
void AECame2(FLOAT32 *pOut, FLOAT32 *pIn, INT32 len, FLOAT32 a_r, FLOAT32 a_f, FLOAT32 *ic)
{
INT32 i;
register FLOAT32 ar2, af2;
ar2 = (FLOAT32)(1.0-a_r);
af2 = (FLOAT32)(1.0-a_f);
if (pIn[0] > *ic)
pOut[0] = a_r*(FLOAT32)fabs(pIn[0]) + ar2* *ic;
else
pOut[0] = a_f*(FLOAT32)fabs(pIn[0]) + af2* *ic;
for (i=1; i < len; i++)
{
if (pIn[i] > pOut[i-1])
pOut[i] = a_r*(FLOAT32)fabs(pIn[i]) + ar2 * pOut[i-1];
else
pOut[i] = a_f*(FLOAT32)fabs(pIn[i]) + af2 * pOut[i-1];
}
*ic = pOut[len-1];
}
/*************************************************************************/
/* Average Background Noise Estimation
/* Rekursive Schaetzung des Hintergrundgeraeuschs (entferntes Ende)
/*************************************************************************/
void AECXnoise_PH(
FLOAT32 *pOut,
FLOAT32 *pTalk,
FLOAT32 *pIn,
FLOAT32 *pInAme,
INT32 len,
FLOAT32 a_r,
FLOAT32 kth,
FLOAT32 *ic)
{
INT32 i;
register FLOAT32 ar2;
ar2 = (FLOAT32)(1.0-a_r);
if ((pTalk[0]=(FLOAT32)(pInAme[0] > (kth* *ic))))
pOut[0] = *ic;
else
pOut[0] = a_r*(FLOAT32)fabs(pIn[0]) + ar2* *ic;
for (i=1; i < len; i++)
{
if ((pTalk[i]=(FLOAT32)(pInAme[i] > (kth*pOut[i-1]))))
pOut[i] = pOut[i-1];
else
pOut[i] = a_r*(FLOAT32)fabs(pIn[i]) + ar2 * pOut[i-1];
}
*ic = pOut[len-1];
}
/*************************************************************************/
/* Average Background Noise Estimation
/* Rekursive Schaetzung des Hintergrundgeraeuschs (nahes Ende)
/*************************************************************************/
void AECDnoise_PH(
FLOAT32 *pNL,
FLOAT32 *pTalk,
FLOAT32 *pE,
FLOAT32 *pXS,
FLOAT32 *pXL,
FLOAT32 *pES,
FLOAT32 *pEL,
INT32 len,
FLOAT32 a_f,
FLOAT32 PL,
FLOAT32 *ic)
{
INT32 i;
register FLOAT32 af2;
af2 = (FLOAT32)(1.0-a_f);
if ((pTalk[0]=(FLOAT32)((pXS[0] > (PL*pXL[0])) && (pES[0] > (PL*pEL[0])))))
pNL[0] = *ic;
else
pNL[0] = a_f*(FLOAT32)fabs(pE[0]) + af2* *ic;
for (i=1; i < len; i++)
{
if ((pTalk[i]=(FLOAT32)((pXS[i] > (PL*pXL[i])) && (pES[i] > (PL*pEL[i])))))
pNL[i] = pNL[i-1];
else
pNL[i] = a_f*(FLOAT32)fabs(pE[i]) + af2 * pNL[i-1];
}
*ic = pNL[len-1];
}
/*************************************************************************/
/* Average Power Estimation
/* Rekursive Schaetzung der mittleren Leistung
/*************************************************************************/
void AECape(FLOAT32 *pOut, FLOAT32 *pIn, INT32 len, FLOAT32 a_rf, FLOAT32 *ic)
{
INT32 i;
register FLOAT32 arf2;
arf2 = (FLOAT32)(1.0-a_rf);
pOut[0] = a_rf*pIn[0]*pIn[0] + arf2* *ic;
for (i=1; i < len; i++)
pOut[i] = a_rf*(FLOAT32)pIn[i]*pIn[i] + arf2*pOut[i-1];
*ic = pOut[len-1];
}
/*************************************************************************/
/* Normalized Correlation
/* Rekursive Schaetzung der mittleren Leistung
/* Variante nach Peter Heitkämper
/*************************************************************************/
void AECnormXcorr_PH(FLOAT32 *pOut, FLOAT32 *pInX, FLOAT32 *pInY, UINT32 nLags, UINT32 len)
{
FLOAT32 sumMag;
FLOAT32 sum;
UINT32 i, j;
for (i=0; i < nLags; i++)
{
sum = 0;
sumMag = 0;
for (j=nLags; j < len; j++)
{
sum += pInX[j-i]*pInY[j];
sumMag += (FLOAT32)fabs(pInX[j-i]*pInY[j]);
}
pOut[i] = (FLOAT32)(fabs(sum)/(sumMag+0.001));
}
}
/*************************************************************************/
/* Normalized Correlation
/* Rekursive Schaetzung der mittleren Leistung
/* Variante aus der Statistik (nach Papula)
/*************************************************************************/
void AECnormXcorr_LP(FLOAT32 *pOut, FLOAT32 *pInX, FLOAT32 *pInY, UINT32 nLags, UINT32 len)
{
FLOAT32 sum;
FLOAT32 sumX, sumY;
UINT32 i, j;
for (i=0; i < nLags; i++)
{
sum = 0;
sumX = 0;
sumY = 0;
for (j=nLags; j < len; j++)
{
sumX += pInX[j-i]*pInX[j-i];
sumY += pInY[j]*pInY[j];
sum += (FLOAT32)(pInX[j-i]*pInY[j]);
}
pOut[i] = (FLOAT32)((sum*sum)/(sumX*sumY+0.001));
}
}
/*************************************************************************/
/* AEC
/*
/*************************************************************************/
AVERR AECInit(
AEC *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.*/
/* 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;
/*-------------------------------------------------------*/
/* 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;
}
/* Speicher fuer PX[C] (reell) */
pObj->pBufPX = (FLOAT32*)AvMemAlloc(pObj->C*sizeof(FLOAT32));
AvZeroMem(pObj->pBufPX,pObj->C*sizeof(FLOAT32));
/* 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));
/* Speicher fuer AGC-Gain allokieren */
pObj->pAgcGain = (FLOAT32*)AvMemAlloc(pObj->L*sizeof(FLOAT32));
/* 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 = (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 AECInitFilter(
AEC *pObj, /* Zeiger auf Objekt */
FLOAT32 *pWTD) /* N Filterkoeffizienten im Zeitbereich */
{
return PfftFilterInit(pObj->pFilter, pWTD, pObj->pBufWS);
}
/*-----------------------------------------------------------------*/
/* Partitioned FLMS
/*-----------------------------------------------------------------*/
AVERR AECcancel(
AEC *pObj, /* Zeiger auf Objekt */
AGC *pAGCobj, /* Zeiger auf AGC 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, gain;
/* 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(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));
if (pAGCobj != NULL)
{
(*pAGCobj->pAGCfunc)(pAGCobj, pInTDx, pInTDd, pOutTDy, pOutTDe,
pObj->pAgcGain, pObj->L);
/* Gewichtung des Fehlersignal mit AGC-Gain */
for (i=0; i < pObj->L; i++)
pTemp->pReal[C_L+i] *= pObj->pAgcGain[i];
}
/* 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((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++)
{
gain = pMu->pData[i] * pObj->C;
pWS->cmpxData.pReal[i] += ((pX->cmpxData.pReal[i] * pTemp->pReal[i]
+ pX->cmpxData.pImag[i] * pTemp->pImag[i])
* gain);
pWS->cmpxData.pImag[i] += ((pX->cmpxData.pReal[i] * pTemp->pImag[i]
- pX->cmpxData.pImag[i] * pTemp->pReal[i])
* gain);
}
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;
}
Reference in New Issue View Git Blame Copy Permalink