ep.c

基于Matlab

#include "mex.h"
#include <math.h>
#include "ep.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,ahat,offset,F,G,delay,pdelay,
    *              constellation,M,sig
    */
   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 *ahatr,*ahati; /* T-spaced symbol estimates */ 
   int offset;           /* offset used to recover initial conditions */
   double *Fr,*Fi;       /* T-spaced forward filters */ 
   double *Gr,*Gi;       /* T-spaced feedback filter */
   int delay;            /* transmission delay */ 
   int pdelay;           /* processing delay */ 
   int constellation;    /* constellation type */ 
   int M;                /* constellation size */ 
   double sig;           /* constellation power */ 

   /* For T/2-spaced forward filters (spacing=0.5) */
   /* Input args:  spacing,a,ra,rb,y,ahat,offset,Fa,Fb,G,delay,
    *              pdelay,constellation,M,sig
    */
   double *rar,*rai;     /* T-spaced received even subchannel data */ 
   double *rbr,*rbi;     /* T-spaced received odd subchannel data */ 
   double *Far,*Fai;     /* T-spaced forward even subfilter */ 
   double *Fbr,*Fbi;     /* T-spaced forward odd subfilter */ 
   

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

   /* For T/2 and T-spaced forward filters (spacing=1) */
   /* Output args: outy,outahat,outres,outq */
   double *outyr,*outyi; 
   double *outahatr,*outahati; 
   double *outresr,*outresi; 
   double *outqr,*outqi; 



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

   /* Retrieve BSE variables */ 
   /* Input args:  spacing,a,r,ahat,offset,F,G,delay,pdelay,
    *              constellation,M,sig
    */
   if (spacing==1.0) {

      /* Retrieve scalars */
      L=(int)mxGetM(prhs[1]);
      offset=(int)mxGetScalar(prhs[4]);    
      Nf=(int)mxGetM(prhs[5]);
      Ng=(int)mxGetM(prhs[6]);
      delay=(int)mxGetScalar(prhs[7]);
      pdelay=(int)mxGetScalar(prhs[8]);
      constellation=(int)mxGetScalar(prhs[9]);
      M=(int)mxGetScalar(prhs[10]);
      sig=(double)mxGetScalar(prhs[11]);

      /* 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));

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

      Fr=mxGetPr(prhs[5]);
      if (mxIsComplex(prhs[5]))
         Fi=mxGetPi(prhs[5]);
      else
         Fi=(double *)mxCalloc(Nf,sizeof(double));

      Gr=mxGetPr(prhs[6]);
      if (mxIsComplex(prhs[6]))
         Gi=mxGetPi(prhs[6]);
      else
         Gi=(double *)mxCalloc(Ng,sizeof(double));

      /* Allocate memory for outputs */
      plhs[0]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);       /* y */
      plhs[1]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);       /* ahat */
      plhs[2]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);       /* res */
      plhs[3]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);       /* q */

      /* Get output pointers */
      outyr=mxGetPr(plhs[0]);
      outyi=mxGetPi(plhs[0]);
      outahatr=mxGetPr(plhs[1]);
      outahati=mxGetPi(plhs[1]);
      outresr=mxGetPr(plhs[2]);
      outresi=mxGetPi(plhs[2]);
      outqr=mxGetPr(plhs[3]);
      outqi=mxGetPi(plhs[3]);

      /* Adapt! */   
      runBSE(Nf,Ng,Fr,Fi,Gr,Gi,
               L,ar,ai,rr,ri,ahatr,ahati,offset,
               delay,pdelay,
               constellation,M,sig,
               outyr,outyi,outahatr,outahati,
               outresr,outresi,outqr,outqi);
   }
   /* Retrieve FSE variables */ 
   /* Input args:  spacing,a,ra,rb,ahat,offset,Fa,Fb,G,delay,
    *              pdelay,constellation,M,sig
    */
   else {
      /* Retrieve scalars */
      L=(int)mxGetM(prhs[1]);
      offset=(int)mxGetScalar(prhs[5]);    
      Nfa=(int)mxGetM(prhs[6]);
      Nfb=(int)mxGetM(prhs[7]);
      Ng=(int)mxGetM(prhs[8]);
      delay=(int)mxGetScalar(prhs[9]);
      pdelay=(int)mxGetScalar(prhs[10]);
      constellation=(int)mxGetScalar(prhs[11]);
      M=(int)mxGetScalar(prhs[12]);
      sig=(double)mxGetScalar(prhs[13]);

      /* 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));

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

      Far=mxGetPr(prhs[6]);
      if (mxIsComplex(prhs[6]))
         Fai=mxGetPi(prhs[6]);
      else
         Fai=(double *)mxCalloc(Nfa,sizeof(double));

      Fbr=mxGetPr(prhs[7]);
      if (mxIsComplex(prhs[7]))
         Fbi=mxGetPi(prhs[7]);
      else
         Fbi=(double *)mxCalloc(Nfb,sizeof(double));

      Gr=mxGetPr(prhs[8]);
      if (mxIsComplex(prhs[8]))
         Gi=mxGetPi(prhs[8]);
      else
         Gi=(double *)mxCalloc(Ng,sizeof(double));

      /* Allocate memory for outputs */
      plhs[0]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);        /* y */
      plhs[1]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);        /* ahat */
      plhs[2]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);        /* res */
      plhs[3]=mxCreateDoubleMatrix(L,1,mxCOMPLEX);        /* q */

      /* Get output pointers */
      outyr=mxGetPr(plhs[0]);
      outyi=mxGetPi(plhs[0]);
      outahatr=mxGetPr(plhs[1]);
      outahati=mxGetPi(plhs[1]);
      outresr=mxGetPr(plhs[2]);
      outresi=mxGetPi(plhs[2]);
      outqr=mxGetPr(plhs[3]);
      outqi=mxGetPi(plhs[3]);

      /* Adapt! */   
      runFSE(Nfa,Nfb,Ng,Far,Fai,Fbr,Fbi,Gr,Gi,
             L,ar,ai,rar,rai,rbr,rbi,ahatr,ahati,offset,
             delay,pdelay,
             constellation,M,sig,
             outyr,outyi,outahatr,outahati,outresr,outresi,outqr,outqi);
   }
}




void
runBSE(Nf,Ng,Fr,Fi,Gr,Gi,
       L,ar,ai,rr,ri,ahatr,ahati,offset,
       delay,pdelay,
       constellation,M,sig,
       outyr,outyi,outahatr,outahati,
       outresr,outresi,outqr,outqi)
int Nf,Ng;
double *Fr,*Fi;
double *Gr,*Gi;
int L;
double *ar,*ai;
double *rr,*ri;
double *ahatr,*ahati;
int offset;
int delay,pdelay;
int constellation,M;
double sig;
double *outyr,*outyi;
double *outahatr,*outahati;
double *outresr,*outresi;
double *outqr,*outqi;
{ 
   complexVec F;              /* forward filter */ 
   complexVec G;              /* feedback filter */
   complex *a;                /* complex symbols */
   complex *r;                /* complex received data */
   complex y;                 /* equalizer output */
   complex *ahat;             /* complex symbol estimates */
   int i;                     /* Tap index */
   int n;                     /* sample counter */


   /* Initialize initial guess */
   a=(complex *)mxCalloc(L,sizeof(complex));
   r=(complex *)mxCalloc(L,sizeof(complex));
   ahat=(complex *)mxCalloc(L,sizeof(complex));
   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];
      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=Fr[i];
      F.v[i].im=Fi[i];
   }

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

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

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

      ahat[n]=quantize(y,constellation,M,sig); 

      /* Store output  */
      outyr[n]=y.re;
      outyi[n]=y.im;
      outahatr[n]=ahat[n].re;
      outahati[n]=ahat[n].im;
      outresr[n]=a[n-delay].re-y.re;
      outresi[n]=a[n-delay].im-y.im;
      outqr[n]=a[n-delay].re-ahat[n].re;
      outqi[n]=a[n-delay].im-ahat[n].im;

   } 

}



void
runFSE(Nfa,Nfb,Ng,Far,Fai,Fbr,Fbi,Gr,Gi,
       L,ar,ai,rar,rai,rbr,rbi,ahatr,ahati,offset,
       delay,pdelay,
       constellation,M,sig,
       outyr,outyi,outahatr,outahati,outresr,outresi,outqr,outqi)
int Nfa,Nfb,Ng;
double *Far,*Fai;
double *Fbr,*Fbi;
double *Gr,*Gi;
int L;
double *ar,*ai;
double *rar,*rai;
double *rbr,*rbi;
double *ahatr,*ahati;
int offset;
int delay;
int pdelay;
int constellation,M;
double sig;
double *outyr,*outyi;
double *outahatr,*outahati;
double *outresr,*outresi;
double *outqr,*outqi;
{ 
   complexVec Fa;             /* even forward filter */ 
   complexVec Fb;             /* odd forward filter */ 
   complexVec G;              /* feedback filter */
   complex *a;                /* complex symbols */
   complex *ra;               /* complex even received data */
   complex *rb;               /* complex odd received data */
   complex y;                 /* complex equalizer output */
   complex *ahat;             /* complex symbol estimates */
   int i;                     /* Tap index */
   int n;                     /* sample counter */

   /* Initialize initial guess */
   a=(complex *)mxCalloc(L,sizeof(complex));
   ra=(complex *)mxCalloc(L,sizeof(complex));
   rb=(complex *)mxCalloc(L,sizeof(complex));
   ahat=(complex *)mxCalloc(L,sizeof(complex));
   for (n=0; n<L; n++) {
      a[n].re=ar[n];
      a[n].im=ai[n];
      ra[n].re=rar[n];
      ra[n].im=rai[n];
      rb[n].re=rbr[n];
      rb[n].im=rbi[n];
      ahat[n].re=ahatr[n];
      ahat[n].im=ahati[n];
   }

   /* Initialize filters */
   Fa.len=Nfa;
   for (i=0; i<Nfa; i++) {
      Fa.v[i].re=Far[i];
      Fa.v[i].im=Fai[i];
   }

   Fb.len=Nfb;
   for (i=0; i<Nfb; i++) {
      Fb.v[i].re=Fbr[i];
      Fb.v[i].im=Fbi[i];
   }

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

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

      y=zero;
      /* Compute equalizer output */
      for (i=0; i<Nfa; i++) {
         y.re=y.re+(Fa.v[i].re*ra[n-i].re-Fa.v[i].im*ra[n-i].im);
         y.im=y.im+(Fa.v[i].re*ra[n-i].im+Fa.v[i].im*ra[n-i].re);
      }
      for (i=0; i<Nfb; i++) {
         y.re=y.re+(Fb.v[i].re*rb[n-i].re-Fb.v[i].im*rb[n-i].im);
         y.im=y.im+(Fb.v[i].re*rb[n-i].im+Fb.v[i].im*rb[n-i].re);
      }
      for (i=0; i<Ng; i++) {
         y.re=y.re-(G.v[i].re*ahat[n-pdelay-i].re-
                    G.v[i].im*ahat[n-pdelay-i].im);
         y.im=y.im-(G.v[i].re*ahat[n-pdelay-i].im+
                    G.v[i].im*ahat[n-pdelay-i].re);
      }

      ahat[n]=quantize(y,constellation,M,sig); 

      /* Store output & error */
      outyr[n]=y.re;
      outyi[n]=y.im;
      outahatr[n]=ahat[n].re;
      outahati[n]=ahat[n].im;
      outresr[n]=a[n-delay].re-y.re;
      outresi[n]=a[n-delay].im-y.im;
      outqr[n]=a[n-delay].re-ahat[n].re;
      outqi[n]=a[n-delay].im-ahat[n].im;
   } 

}




/* 
 * addComplex - add complex numbers a and b 
 */ 
complex
addComplex(a,b)
complex a;
complex b;
{
   complex c;

   c.re=a.re+b.re;
   c.im=a.im+b.im;

   return c;
}


/* 
 * subComplex - subtract complex numbers a and b 
 */ 
complex
subComplex(a,b)
complex a;
complex b;
{
   complex c;

   c.re=a.re-b.re;
   c.im=a.im-b.im;

   return c;
}

 
/* 
 * multComplex - multiply complex numbers a and b 
 */ 
complex
multComplex(a,b)
complex a;
complex b;
{
   complex c;

   c.re=a.re*b.re-a.im*b.im;
   c.im=a.re*b.im+a.im*b.re;

   return c;
}


/* 
 * modulus - modulus |z| of z 
 */
double
modulus(z)
complex z;
{
   return sqrt(z.re*z.re + z.im*z.im);
}


/* 
 * conj - conjugate of a=(x,y) is (x,-y) 
 */
complex
conj(a)
complex a;
{
   complex c;

   c.re=a.re;
   c.im=-a.im;

   return c;
}





/*
 * quantize - make a symbol estimate.
 */
complex
quantize(z,constellation,M,sig)
complex z;
int constellation;
int M;
double sig;
{
   complex a;
   complex y;

   y.re=z.re*sig;
   y.im=z.im*sig;
   if (constellation==1)
      a=unpam(y,M);
   else if (constellation==2)
      a=unqam(y,M);
   else if (constellation==3)
      a=unpam(y,M);
   a.re=a.re/sig;
   a.im=a.im/sig;


   return a;
}

     



/*
 * constellationStats - 4th and 2nd moments of constellation
 */
void
constellationStats(constellation,M,m4,m2)
int constellation;
int M;
double *m4;
double *m2;
{
   int i,j;
   complex a;
   int rootM;

   if (constellation==1) {
      /* Compute 4th moment */
      *m4=0;
      for (i=1; i<=M/2; i++) {
         *m4=*m4+2.0/(double)M*pow((double)(2*i-1),4.0);
      }

      /* Compute 2nd moment */
      *m2=((double)M*M-1.0)/3.0;
   }
   else if (constellation==2) {
      /* Compute 4th moment */
      *m4=0;
      rootM=sqrt((double) M);
      for (i=1; i<=rootM/2; i++)  {
         for (j=1; j<=rootM/2; j++)  {
            a.re=2*i-1;
            a.im=2*j-1;
            *m4=*m4+4.0/(double)M*pow(modulus(a),4.0);
         }
      }

      /* Compute 2nd moment */
      *m2=(2.0*((double)M-1.0))/3.0;
   }
}






/*
 * unpam - decode an M-PAM symbol to nearest neighbor.
 */
complex
unpam(z,M)
complex z;
int M;
{
   complex a=zero;

   if (modulus(z)>M-1.0) {
      a.re=sign(z.re)*((double)M-1.0);
   }
   else {
      a.re=sign(z.re)*(round((modulus(z)+1.0)/2.0)*2.0-1.0);
   }

   return a;
}


/*
 * unqam - make a M-QAM estimate.
 */
complex
unqam(z,M)
complex z;
int M;
{
   complex zre=zero;
   complex zim=zero;
   complex a,are,aim;
   int rootM;

   rootM=sqrt(M);

   zre.re=z.re;
   are=unpam(zre,rootM);
   zim.re=z.im;
   aim=unpam(zim,rootM);

   a.re=are.re;
   a.im=aim.re;

   return a;
}


/*
 * sign - signum function
 */
double
sign(x)
double x;
{
   double y;
 
   if (x>0.0)
      y=1.0;
   else
      y=-1.0;

   return y;
}


/*
 * round - round to nearest integer
 */
double
round(x)
double x;
{
   double y;

   if (x>((double)floor(x)+(double)ceil(x))/2.0)
      y=(double)ceil(x);
   else
      y=(double)floor(x);

   return y;
}