cmProc5.h/c Added cmNlmsEc.

This commit is contained in:
kevin 2015-08-20 19:38:23 -04:00
parent 4b3d575099
commit dff56d7e7f
2 changed files with 144 additions and 9 deletions

125
cmProc5.c
View File

@ -827,18 +827,123 @@ cmRC_t cmReflectCalcWrite( cmReflectCalc_t* p, const char* dirStr )
{ {
cmRC_t rc = cmOkRC; cmRC_t rc = cmOkRC;
if( p->phVa != NULL) if( p->xVa != NULL)
{
//const char* path = NULL;
cmVectArrayWriteDirFn(p->xVa, dirStr, "reflect_calc_x.va" ); cmVectArrayWriteDirFn(p->xVa, dirStr, "reflect_calc_x.va" );
cmVectArrayWriteDirFn(p->yVa, dirStr, "reflect_calc_y.va" );
cmVectArrayWriteDirFn(p->phVa,dirStr, "reflect_calc_ph.va");
//if((rc = cmVectArrayWrite(p->phVa, path = cmFsMakeFn(path,"reflect_calc","va",dirStr,NULL) )) != cmOkRC ) if( p->yVa != NULL )
// rc = cmCtxRtCondition(&p->obj,cmSubSysFailRC,"Reflect calc file write failed."); cmVectArrayWriteDirFn(p->yVa, dirStr, "reflect_calc_y.va" );
//cmFsFreeFn(path);
} if( p->phVa != NULL )
cmVectArrayWriteDirFn(p->phVa,dirStr, "reflect_calc_ph.va");
return rc; return rc;
} }
//=======================================================================================================================
//
//
cmNlmsEc_t* cmNlmsEcAlloc( cmCtx* ctx, cmNlmsEc_t* ap, float mu, unsigned hN, unsigned delayN )
{
cmNlmsEc_t* p = cmObjAlloc(cmNlmsEc_t,ctx,ap);
// allocate the vect array
p->eVa = cmVectArrayAlloc(ctx, kFloatVaFl );
if( mu != 0 )
if( cmNlmsEcInit(p,mu,hN,delayN) != cmOkRC )
cmNlmsEcFree(&p);
return p;
}
cmRC_t cmNlmsEcFree( cmNlmsEc_t** pp )
{
cmRC_t rc = cmOkRC;
if( pp == NULL || *pp == NULL )
return rc;
cmNlmsEc_t* p = *pp;
if((rc = cmNlmsEcFinal(p)) != cmOkRC )
return rc;
cmMemFree(p->wV);
cmMemFree(p->hV);
cmVectArrayFree(&p->eVa);
cmObjFree(pp);
return rc;
}
cmRC_t cmNlmsEcInit( cmNlmsEc_t* p, float mu, unsigned hN, unsigned delayN )
{
cmRC_t rc = cmOkRC;
if((rc = cmNlmsEcFinal(p)) != cmOkRC )
return rc;
p->mu = mu;
p->hN = hN;
p->delayN = delayN;
p->wV = cmMemResizeZ(cmSample_t,p->wV,hN);
p->hV = cmMemResizeZ(cmSample_t,p->hV,hN);
p->dV = cmMemResizeZ(cmSample_t,p->dV,delayN);
p->w0i = 0;
return rc;
}
cmRC_t cmNlmsEcFinal( cmNlmsEc_t* p )
{ return cmOkRC; }
/*
for n=M:N
uv = u(n:-1:n-M+1);
e(n) = d(n)-w'*uv;
w=w+mu/(a + uv'*uv ) * uv * conj(e(n));
endfor
e = e(:).^2;
*/
cmRC_t cmNlmsEcExec( cmNlmsEc_t* p, const cmSample_t* xV, const cmSample_t* fV, cmSample_t* yV, unsigned xyN )
{
unsigned i;
for(i=0; i<xyN; ++i)
{
cmSample_t y = 0;
unsigned j;
unsigned k = 0;
float a = 0.00001;
p->hV[p->w0i] = xV[i];
for(j=p->w0i,k=0; j<p->hN; ++j,++k)
y += p->hV[j] * p->wV[k];
for(j=0; j<p->w0i; ++j,++k)
y += p->hV[j] * p->wV[k];
p->w0i = (p->w0i+1) % p->hN;
float e = fV[i] - y;
cmSample_t z = 0;
for(j=0; j<p->hN; ++j)
z += p->hV[j] * p->hV[j];
for(j=0; j<p->hN; ++j)
p->wV[j] += p->mu/(a + z) * p->hV[j] * e;
}
return cmOkRC;
}
cmRC_t cmNlmsEcWrite( cmNlmsEc_t* p, const cmChar_t* dirStr )
{
if( p->eVa != NULL )
cmVectArrayWriteDirFn(p->eVa,dirStr, "nlms_err.va");
return cmOkRC;
}

View File

@ -218,6 +218,36 @@ extern "C" {
cmRC_t cmReflectCalcExec( cmReflectCalc_t* p, const cmSample_t* xV, cmSample_t* yV, unsigned xyN ); cmRC_t cmReflectCalcExec( cmReflectCalc_t* p, const cmSample_t* xV, cmSample_t* yV, unsigned xyN );
cmRC_t cmReflectCalcWrite( cmReflectCalc_t* p, const char* dirStr ); cmRC_t cmReflectCalcWrite( cmReflectCalc_t* p, const char* dirStr );
//=======================================================================================================================
//
//
typedef struct
{
cmObj obj;
float mu; // LMS step rate
unsigned hN; // filter length
unsigned delayN; // fixed delay to apply to align xV with fV.
cmSample_t* dV; // delay line
cmSample_t* wV; // wV[hN] filter weights
cmSample_t* hV; // hV[hN] filter delay line
unsigned w0i;
cmVectArray_t* eVa;
} cmNlmsEc_t;
cmNlmsEc_t* cmNlmsEcAlloc( cmCtx* ctx, cmNlmsEc_t* p, float mu, unsigned hN, unsigned delayN );
cmRC_t cmNlmsEcFree( cmNlmsEc_t** pp );
cmRC_t cmNlmsEcInit( cmNlmsEc_t* p, float mu, unsigned hN, unsigned delayN );
cmRC_t cmNlmsEcFinal( cmNlmsEc_t* p );
// xV[] unfiltered reference signal (direct from xform output)
// fV[] filtered reference signal (from mic)
// yV[] echo-canelled signal
cmRC_t cmNlmsEcExec( cmNlmsEc_t* p, const cmSample_t* xV, const cmSample_t* fV, cmSample_t* yV, unsigned xyN );
cmRC_t cmNlmsEcWrite( cmNlmsEc_t* p, const cmChar_t* dir );
#ifdef __cplusplus #ifdef __cplusplus
} }