adapt.bak

基于Matlab

#include "mex.h"
#include <math.h>
#include "adapt.h"

static complex zero={0,0};

void 
mexFunction(nlhs,plhs,nrhs,prhs)
int nlhs;
mxArray *plhs[];
int nrhs;
const mxArray *prhs[];
{
   /* For T-spaced forward filters (spacing=1) */
   /* Input args:  spacing,a,r,y,x,ahat,F0,G0,muf,mug,offset,delay,pdelay,
    *              structure,constellation,M,sig,algorithm,regfilt
    */
   double spacing;       /* spacing, i.e. BSE=1, FSE=2 */
   double *ar,*ai;       /* T-spaced desired transmitted symbols */ 
   double *rr,*ri;       /* T-spaced received data */ 
   double *yr,*yi;       /* T-spaced equalizer output */ 
   double *xr,*xi;       /* T-spaced filtered output */ 
   double *ahatr,*ahati; /* T-spaced symbol estimates */ 
   double *F0r,*F0i;     /* T-spaced forward filters */ 
   double *G0r,*G0i;     /* T-spaced feedback filter */
   double muf;           /* forward filter step-size muf */
   double mug;           /* forward filter step-size mug */
   int offset;           /* offset used to recover initial conditions */
   int delay;            /* transmission delay */ 
   int pdelay;           /* processing delay */ 
   int structure;        /* equalizer structure, i.e. LE=1, DFE=2 */
   int constellation;    /* constellation type */ 
   int M;                /* constellation size */ 
   double sig;           /* constellation power */ 
   int algorithm;        /* choice of algorithm, 
                          * i.e. IIR-LMS=1, DD-LMS=2, IIR-CMA=3 
                          */ 
   int regfilt;          /* use regressor filtering or not */

   /* For T/2-spaced forward filters (spacing=0.5) */
   /* Input args:  spacing,a,ra,rb,y,x,ahat,Fa0,Fb0,G0,muf,mug,offset,delay,
    *              pdelay,structure,constellation,M,sig,algorithm
    */
   double *rar,*rai;       /* T-spaced received even subchannel data */ 
   double *rbr,*rbi;       /* T-spaced received odd subchannel data */ 
   double *Fa0r,*Fa0i;     /* T-spaced forward even subfilter */ 
   double *Fb0r,*Fb0i;     /* T-spaced forward odd subfilter */ 
   

   /* Intermediate variables */
   int Nf,Nfa,Nfb,Ng;  /* Length of filters */ 
   int L;              /* Lenght of simulation */

   /* For T-spaced forward filters (spacing=1) */
   /* Output args: outF,outG,outy,outx,outz,outahat,outef,outeg,outphi */
   double *outFr,*outFi; 
   double *outGr,*outGi; 
   double *outyr,*outyi; 
   double *outxr,*outxi; 
   double *outzr,*outzi; 
   double *outahatr,*outahati; 
   double *outefr,*outefi; 
   double *outegr,*outegi; 
   double *outphi;


   /* For T/2-spaced forward filters (spacing=1) */
   /* Output args: outFa,outFb,outG,outy,oute,outahat */
   double *outFar,*outFai; 
   double *outFbr,*outFbi; 


   /* Find out if BSE or FSE */
   spacing=(double)mxGetScalar(prhs[0]);    

   /* Retrieve BSE variables */ 
   /* Input args:  spacing,a,r,y,x,ahat,F0,G0,muf,mug,offset,delay,pdelay,
    *              structure,constellation,M,sig,algorithm
    */
   if (spacing==1.0) {

      /* Retrieve scalars */
      L=(int)mxGetM(prhs[1]);
      Nf=(int)mxGetM(prhs[6]);
      Ng=(int)mxGetM(prhs[7]);
      muf=(double)mxGetScalar(prhs[8]);    
      mug=(double)mxGetScalar(prhs[9]);    
      offset=(int)mxGetScalar(prhs[10]);    
      delay=(int)mxGetScalar(prhs[11]);
      pdelay=(int)mxGetScalar(prhs[12]);
      structure=(int)mxGetScalar(prhs[13]);    
      constellation=(int)mxGetScalar(prhs[14]);
      M=(int)mxGetScalar(prhs[15]);
      sig=(double)mxGetScalar(prhs[16]);
      algorithm=(int)mxGetScalar(prhs[17]);    
      regfilt=(int)mxGetScalar(prhs[18]);    

      /* Get input pointers */
      ar=mxGetPr(prhs[1]);
      if (mxIsComplex(prhs[1]))
         ai=mxGetPi(prhs[1]);
      else
         ai=(double *)mxCalloc(L,sizeof(double));

      rr=mxGetPr(prhs[2]);
      if (mxIsComplex(prhs[2]))
         ri=mxGetPi(prhs[2]);
      else
         ri=(double *)mxCalloc(L,sizeof(double));

      yr=mxGetPr(prhs[3]);
      if (mxIsComplex(prhs[3]))
         yi=mxGetPi(prhs[3]);
      else
         yi=(double *)mxCalloc(L,sizeof(double));

      xr=mxGetPr(prhs[4]);
      if (mxIsComplex(prhs[4]))
         xi=mxGetPi(prhs[4]);
      else
         xi=(double *)mxCalloc(L,sizeof(double));

      ahatr=mxGetPr(prhs[5]);
      if (mxIsComplex(prhs[5]))
         ahati=mxGetPi(prhs[5]);
      else
         ahati=(double *)mxCalloc(L,sizeof(double));

      F0r=mxGetPr(prhs[6]);
      if (mxIsComplex(prhs[6]))
         F0i=mxGetPi(prhs[6]);
      else
         F0i=(double *)mxCalloc(Nf,sizeof(double));

      G0r=mxGetPr(prhs[7]);
      if (mxIsComplex(prhs[7]))
         G0i=mxGetPi(prhs[7]);
      else
         G0i=(double *)mxCalloc(Ng,sizeof(double));

      /* Allocate memory for outputs */
      plhs[0]=mxCreateDoubleMatrix(Nf,1,mxCOMPLEX);      /* F */
      plhs[1]=mxCreateDoubleMatrix(Ng,1,mxCOMPLEX);      /* G */
      plhs[2]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);       /* y */
      plhs[3]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);       /* x */
      plhs[4]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);       /* z */
      plhs[5]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);       /* ahat */
      plhs[6]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);       /* ef */
      plhs[7]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);       /* eg */
      plhs[8]=mxCreateDoubleMatrix(L,1,mxREAL);          /* phi */

      /* Get output pointers */
      outFr=mxGetPr(plhs[0]);
      outFi=mxGetPi(plhs[0]);
      outGr=mxGetPr(plhs[1]);
      outGi=mxGetPi(plhs[1]);
      outyr=mxGetPr(plhs[2]);
      outyi=mxGetPi(plhs[2]);
      outxr=mxGetPr(plhs[3]);
      outxi=mxGetPi(plhs[3]);
      outzr=mxGetPr(plhs[4]);
      outzi=mxGetPi(plhs[4]);
      outahatr=mxGetPr(plhs[5]);
      outahati=mxGetPi(plhs[5]);
      outefr=mxGetPr(plhs[6]);
      outefi=mxGetPi(plhs[6]);
      outegr=mxGetPr(plhs[7]);
      outegi=mxGetPi(plhs[7]);
      outphi=mxGetPr(plhs[8]);

      /* Adapt! */   
      adaptBSE(Nf,Ng,F0r,F0i,G0r,G0i,
               L,ar,ai,rr,ri,yr,yi,xr,xi,ahatr,ahati,
               muf,mug,offset,delay,pdelay,
               constellation,M,sig,algorithm,regfilt,structure,
               outFr,outFi,outGr,outGi,outyr,outyi,outxr,outxi,
               outzr,outzi,outahatr,outahati,
               outefr,outefi,outegr,outegi,outphi);
   }
   /* Retrieve FSE variables */ 
   /* Input args:  spacing,a,ra,rb,y,x,ahat,Fa0,Fb0,G0,muf,mug,offset,delay,
    *              pdelay,structure,constellation,M,sig,algorithm
    */
   else {
      /* Retrieve scalars */
      L=(int)mxGetM(prhs[1]);
      Nfa=(int)mxGetM(prhs[7]);
      Nfb=(int)mxGetM(prhs[8]);
      Ng=(int)mxGetM(prhs[9]);
      muf=(double)mxGetScalar(prhs[10]);    
      mug=(double)mxGetScalar(prhs[11]);    
      offset=(int)mxGetScalar(prhs[12]);    
      delay=(int)mxGetScalar(prhs[13]);
      pdelay=(int)mxGetScalar(prhs[14]);
      structure=(int)mxGetScalar(prhs[15]);    
      constellation=(int)mxGetScalar(prhs[16]);
      M=(int)mxGetScalar(prhs[17]);
      sig=(double)mxGetScalar(prhs[18]);
      algorithm=(int)mxGetScalar(prhs[19]);    
      regfilt=(int)mxGetScalar(prhs[20]);    

      /* Get input pointers */
      ar=mxGetPr(prhs[1]);
      if (mxIsComplex(prhs[1]))
         ai=mxGetPi(prhs[1]);
      else
         ai=(double *)mxCalloc(L,sizeof(double));

      rar=mxGetPr(prhs[2]);
      if (mxIsComplex(prhs[2]))
         rai=mxGetPi(prhs[2]);
      else
         rai=(double *)mxCalloc(L,sizeof(double));

      rbr=mxGetPr(prhs[3]);
      if (mxIsComplex(prhs[3]))
         rbi=mxGetPi(prhs[3]);
      else
         rbi=(double *)mxCalloc(L,sizeof(double));

      yr=mxGetPr(prhs[4]);
      if (mxIsComplex(prhs[4]))
         yi=mxGetPi(prhs[4]);
      else
         yi=(double *)mxCalloc(L,sizeof(double));

      xr=mxGetPr(prhs[5]);
      if (mxIsComplex(prhs[5]))
         xi=mxGetPi(prhs[5]);
      else
         xi=(double *)mxCalloc(L,sizeof(double));

      ahatr=mxGetPr(prhs[6]);
      if (mxIsComplex(prhs[6]))
         ahati=mxGetPi(prhs[6]);
      else
         ahati=(double *)mxCalloc(L,sizeof(double));

      Fa0r=mxGetPr(prhs[7]);
      if (mxIsComplex(prhs[7]))
         Fa0i=mxGetPi(prhs[7]);
      else
         Fa0i=(double *)mxCalloc(Nfa,sizeof(double));

      Fb0r=mxGetPr(prhs[8]);
      if (mxIsComplex(prhs[8]))
         Fb0i=mxGetPi(prhs[8]);
      else
         Fb0i=(double *)mxCalloc(Nfb,sizeof(double));

      G0r=mxGetPr(prhs[9]);
      if (mxIsComplex(prhs[9]))
         G0i=mxGetPi(prhs[9]);
      else
         G0i=(double *)mxCalloc(Ng,sizeof(double));

      /* Allocate memory for outputs */
      plhs[0]=mxCreateDoubleMatrix(Nfa,1,mxCOMPLEX);      /* Fa */
      plhs[1]=mxCreateDoubleMatrix(Nfb,1,mxCOMPLEX);      /* Fb */
      plhs[2]=mxCreateDoubleMatrix(Ng,1,mxCOMPLEX);       /* G */
      plhs[3]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);        /* y */
      plhs[4]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);        /* x */
      plhs[5]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);        /* z */
      plhs[6]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);        /* ahat */
      plhs[7]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);        /* ef */
      plhs[8]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);        /* eg */
      plhs[9]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);        /* phi */

      /* Get output pointers */
      outFar=mxGetPr(plhs[0]);
      outFai=mxGetPi(plhs[0]);
      outFbr=mxGetPr(plhs[1]);
      outFbi=mxGetPi(plhs[1]);
      outGr=mxGetPr(plhs[2]);
      outGi=mxGetPi(plhs[2]);
      outyr=mxGetPr(plhs[3]);
      outyi=mxGetPi(plhs[3]);
      outxr=mxGetPr(plhs[4]);
      outxi=mxGetPi(plhs[4]);
      outzr=mxGetPr(plhs[5]);
      outzi=mxGetPi(plhs[5]);
      outahatr=mxGetPr(plhs[6]);
      outahati=mxGetPi(plhs[6]);
      outefr=mxGetPr(plhs[7]);
      outefi=mxGetPi(plhs[7]);
      outegr=mxGetPr(plhs[8]);
      outegi=mxGetPi(plhs[8]);
      outphi=mxGetPr(plhs[9]);

      /* Adapt! */   
      adaptFSE(Nfa,Nfb,Ng,Fa0r,Fa0i,Fb0r,Fb0i,G0r,G0i,
               L,ar,ai,rar,rai,rbr,rbi,yr,yi,xr,xi,ahatr,ahati,
               muf,mug,offset,delay,pdelay,
               constellation,M,sig,algorithm,regfilt,structure,
               outFar,outFai,outFbr,outFbi,outGr,outGi,outyr,outyi,
               outxr,outxi,outzr,outzi,outahatr,outahati,
               outefr,outefi,outegr,outegi,outphi);
   }
}




/* 
 * adaptBSE - update baud spaced equalizer using gradient
 * search algorithm (either LMS, CMA, or DD).
 */
void
adaptBSE(Nf,Ng,F0r,F0i,G0r,G0i,
         L,ar,ai,rr,ri,yr,yi,xr,xi,ahatr,ahati,
         muf,mug,offset,delay,pdelay,
         constellation,M,sig,algorithm,regfilt,structure,
         outFr,outFi,outGr,outGi,outyr,outyi,outxr,outxi,
         outzr,outzi,outahatr,outahati,
         outefr,outefi,outegr,outegi,outphi)
int Nf,Ng;
double *F0r,*F0i;
double *G0r,*G0i;
int L;
double *ar,*ai;
double *rr,*ri;
double *yr,*yi;
double *xr,*xi;
double *ahatr,*ahati;
double muf,mug;
int offset;
int delay;
int pdelay;
int constellation,M;
double sig;
int algorithm;
int regfilt;
int structure;
double *outFr,*outFi;
double *outGr,*outGi;
double *outyr,*outyi;
double *outzr,*outzi;
double *outahatr,*outahati;
double *outxr,*outxi;
double *outefr,*outefi;
double *outegr,*outegi;
double *outphi;
{ 
   complexVec F;              /* forward filter */ 
   complexVec G;              /* feedback filter */
   complex *a;                /* complex symbols */
   complex *r;                /* complex received data */
   complex *y;                /* complex equalizer output */
   complex *x;                /* filtered output */
   complex *z;                /* complex equalizer output w/ phase recovery */
   complex *ahat;             /* complex symbol estimates */
   complex q;                 /* decisions for phase recovery */
   complex ef;                /* forward filter update error */
   complex eg;                /* feedback filter update error */
   complex corr;              /* phase corrrection */ 
   complex ep0,ep1;           /* current and past phase error */ 
   double epmod;              /* modulus of phase error */ 
   double phi,c;              /* phase estimate and correction */ 
   static double a1=1.6;      /* loop filter coefficients */
   static double a0=-0.9;     /* loop filter coefficient */
   double g;                  /* CMA constant */
   double m4,m2;              /* Constellation 4th and 2nd moments */
   int i;                     /* Tap index */
   int n;                     /* sample counter */

   /* Get constellation variance and define CMA constant */
   constellationStats(constellation,M,sig,&m4,&m2);
   g=m4;

   /* Initialize initial guess */
   a=(complex *)mxCalloc(L,sizeof(complex));
   r=(complex *)mxCalloc(L,sizeof(complex));
   y=(complex *)mxCalloc(L,sizeof(complex));
   x=(complex *)mxCalloc(L,sizeof(complex));
   z=(complex *)mxCalloc(L,sizeof(complex));
   ahat=(complex *)mxCalloc(L,sizeof(complex));
   ep0.re=1.0; ep0.im=0.0;
   ep1.re=1.0; ep1.im=0.0;
   c=0;
   phi=0;
   for (n=0; n<L; n++) {
      a[n].re=ar[n];
      a[n].im=ai[n];
      r[n].re=rr[n];
      r[n].im=ri[n];
      y[n].re=yr[n];
      y[n].im=yi[n];
      x[n].re=xr[n];
      x[n].im=xi[n];
      ahat[n].re=ahatr[n];
      ahat[n].im=ahati[n];
   }

   /* Initialize regressors & filters */
   F.len=Nf;
   for (i=0; i<Nf; i++) {
      F.v[i].re=F0r[i];
      F.v[i].im=F0i[i];
   }

   G.len=Ng;
   for (i=0; i<Ng; i++) {
      G.v[i].re=G0r[i];
      G.v[i].im=G0i[i];
   }

   /* Run adaptation until output variance is below threshold */ 
   for (n=offset; n<L; n++) { 

      y[n]=zero;
      /* Compute equalizer output */
      for (i=0; i<Nf; i++) {
         y[n].re=y[n].re+(F.v[i].re*r[n-i].re-F.v[i].im*r[n-i].im);
         y[n].im=y[n].im+(F.v[i].re*r[n-i].im+F.v[i].im*r[n-i].re);
      }
      if (structure==1)
         for (i=0; i<Ng; i++) {
            y[n].re=y[n].re-(G.v[i].re*y[n-pdelay-i].re-
                             G.v[i].im*y[n-pdelay-i].im);
            y[n].im=y[n].im-(G.v[i].re*y[n-pdelay-i].im+
                             G.v[i].im*y[n-pdelay-i].re);
         }
      else
         for (i=0; i<Ng; i++) {
            y[n].re=y[n].re-(G.v[i].re*ahat[n-pdelay-i].re-
                             G.v[i].im*ahat[n-pdelay-i].im);
            y[n].im=y[n].im-(G.v[i].re*ahat[n-pdelay-i].im+
                             G.v[i].im*ahat[n-pdelay-i].re);
         }

      /* Use regressor filtering if desired */
      x[n]=y[n-pdelay];
      if ((regfilt==1) & (structure==1)) 
         for (i=0; i<Ng; i++) {
            x[n].re=x[n].re-(G.v[i].re*x[n-pdelay-i].re-
                             G.v[i].im*x[n-pdelay-i].im);
            x[n].im=x[n].im-(G.v[i].re*x[n-pdelay-i].im+
                             G.v[i].im*x[n-pdelay-i].re);
         } 
      
       


      /* LMS */
      if (algorithm==1)  {
         ahat[n]=a[n-delay]; 
         ef.re=y[n].re-ahat[n].re; ef.im=y[n].im-ahat[n].im;
         eg=ef;

         /* Phase recovered output */
         z[n].re=y[n].re;
         z[n].im=y[n].im;
      }
      /* DD */
      else if (algorithm==2) {
         ahat[n]=quantize(y[n],constellation,M,sig,n); 
         ef.re=y[n].re-ahat[n].re; ef.im=y[n].im-ahat[n].im;
         eg=ef;

         /* Phase recovered output */
         z[n].re=y[n].re;
         z[n].im=y[n].im;
      }
      /* CMA */
      else if (algorithm==3) {
         ef.re=y[n].re*(y[n].re*y[n].re+y[n].im*y[n].im-g);
         ef.im=y[n].im*(y[n].re*y[n].re+y[n].im*y[n].im-g);
         eg=ef;

         /* Phase recovered output */
         corr.re=cos(phi); corr.im=-sin(phi);
         z[n].re=y[n].re*corr.re-y[n].im*corr.im;
         z[n].im=y[n].im*corr.re+y[n].re*corr.im;
         ahat[n]=quantize(z[n],constellation,M,sig,n); 

         q=quantize(z[n],constellation,M,sig,n); 

         ep1.re=z[n].re*q.re+z[n].im*q.im;
         ep1.im=-z[n].re*q.im+z[n].im*q.re;
         epmod=(double)sqrt(ep1.re*ep1.re+ep1.im*ep1.im); 
         ep1.re=ep1.re/epmod;
         ep1.im=ep1.im/epmod;

         c=c+a1*ep1.im+a0*ep0.im;
         ep0=ep1;
         phi=phi+c;
      }

      /* Update equalizers */
      for (i=0; i<Nf; i++) {
         F.v[i].re=F.v[i].re-muf*(ef.re*r[n-i].re+ef.im*r[n-i].im);
         F.v[i].im=F.v[i].im-muf*(-ef.re*r[n-i].im+ef.im*r[n-i].re);
      }
      if (structure==1) 
         for (i=0; i<Ng; i++) {
            G.v[i].re=G.v[i].re+mug*(eg.re*x[n-i].re+
                                     eg.im*x[n-i].im);
            G.v[i].im=G.v[i].im+mug*(-eg.re*x[n-i].im+
                                      eg.im*x[n-i].re);
         }
      else
         for (i=0; i<Ng; i++) {
           G.v[i].re=G.v[i].re+mug*(eg.re*ahat[n-pdelay-i].re+
                                    eg.im*ahat[n-pdelay-i].im);
           G.v[i].im=G.v[i].im+mug*(-eg.re*ahat[n-pdelay-i].im+
                                     eg.im*ahat[n-pdelay-i].re);
         }

      /* Store output & error */
      outyr[n]=y[n].re;
      outyi[n]=y[n].im;
      outxr[n]=x[n].re;
      outxi[n]=x[n].im;
      outzr[n]=z[n].re;
      outzi[n]=z[n].im;
      outahatr[n]=ahat[n].re;
      outahati[n]=ahat[n].im;
      outefr[n]=ef.re;
      outefi[n]=ef.im; 
      outegr[n]=ef.re;
      outegi[n]=ef.im; 
      outphi[n]=phi;

      /* Check for instability */
      if (ef.re*ef.re+ef.im*ef.im>100000 | eg.re*eg.re+eg.im*eg.im>100000)
         mexErrMsgTxt("Adaptation blowing up! Stopping adaptation.");

   } 


   /* Store final filters */
   for (i=0; i<Nf; i++) {