berg_cmegd.c

有关信道估计和信道均衡的仿真程序

/******************************************************************************
 *									      *
 * berg_cmegd.c - CME-GD algorithms for the BERGulator (called by ui_traj.m)  *
 *									      *
 * written by: 	Phil "Bert" Schniter 					      *
 *		Blind Equalization Research Group 			      *
 * 		Cornell University					      *
 *		9/20/97							      *
 *                                                                            *
 * Copyright 1997-1998 Phil Schniter                                          *
 *									      *
 ******************************************************************************/

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

/********************** 
 * real-valued CME-GD *
 **********************
 *
 * the real-valued (source, channel, noise) CME-GD routine replaces 
 * the following matlab code:
 *
 * --------------------------------------------------
 * f_cur = f_init;
 * for i = 1:N,
 *   reg = r(1, N_f+(1+is_FSE)*i:-1:(1+is_FSE)*i+1);
 *   y_cur = reg*f_cur;       % for constellation plot...
 *   h = C*f_cur;
 *   hTh = norm(h)^2;
 *   fTf = norm(f_cur)^2;
 *   srcme_cur = sqrt( kappa^2 - 2*kappa*(hTh + sigma2_n*fTf)...
 *      + 3*hTh^2 + (kappa-3)*norm(h.^2).^2 ...
 *      + 6*sigma2_n*hTh*fTf + 3*sigma2_n^2*fTf^2);
 *   dJdf = (kappa-3)*C'*(h.^3) + (3*hTh + 3*sigma2_n*fTf - kappa)*...
 *      (C'*h + sigma2_n*f_cur);
 *   f_cur = f_cur  - mu*dJdf;
 *   f(:,floor((i-1)/D_f)+1) = f_cur;
 *   y(floor((i-1)/D_e)+1) = y_cur;
 *   srcme(floor((i-1)/D_e)+1) = srcme_cur;
 * end
 * --------------------------------------------------
 */
void cmegd_rPAM(double *fr, double *yr, double *srcme, double *rr, 
	 double *fr_init, 
         double kappa, double mu,
         int N, int N_f, int D_e, int D_f, int is_FSE, int is_cmplx,
	 int len_f, int len_y, int N_h,
	 double sigma2_n, double *Cr)
{
  double *fr_cur, *hr_cur, *hr2_cur, *gradr;
  double yr_cur, srcme_cur, sqnrm_h, sqnrm_f, sum_h4, tmp;
  int    i, k, n, f_ind=0, y_ind=0, reg_ind; 

  /* allocate local memory */
  fr_cur = (double *)mxCalloc(N_f,sizeof(double));
  hr_cur = (double *)mxCalloc(N_h,sizeof(double));
  hr2_cur = (double *)mxCalloc(N_h,sizeof(double));
  gradr = (double *)mxCalloc(N_f,sizeof(double));

  /* initialize equalizer */
  for (k=0; k<N_f; k++){
    fr_cur[k] = fr_init[k];
  }

  /* adaptation routine */
  reg_ind = N_f-1;
  for (i=0; i<N; ){
    reg_ind += 1+is_FSE;		/* update regressor index */
    i++;				/* convenient to increment here */

    sqnrm_f = 0; 
    for (k=0; k<N_f; k++){ 		/* calc ||f||^2 */
      sqnrm_f += fr_cur[k]*fr_cur[k]; 
    }
    sqnrm_h = 0; 
    for (n=0; n<N_h; n++){ 		/* calc h, h^2, and ||h||^2 */
      hr_cur[n] = 0;
      for (k=0; k<N_f; k++){ hr_cur[n] += Cr[k*N_h+n]*fr_cur[k]; } 
      hr2_cur[n] = hr_cur[n]*hr_cur[n];
      sqnrm_h += hr2_cur[n];
    }

    /* calculate and store output (y) and square-root CM error (srcme) */
    if( !(i % D_e) ){
      yr[y_ind] = 0;			
      for (k=0; k<N_f; k++){ 		
        yr[y_ind] += rr[reg_ind-k]*fr_cur[k];
      }
      sum_h4 = 0; for( n=0; n<N_h; n++ ){ sum_h4 += hr2_cur[n]*hr2_cur[n]; }
      srcme[y_ind++] = sqrt(kappa*kappa - 2*kappa*(sqnrm_h + sigma2_n*sqnrm_f)
  		+ 3*sigma2_n*sigma2_n*sqnrm_f*sqnrm_f 
        	+ 3*sqnrm_h*(sqnrm_h + 2*sigma2_n*sqnrm_f)
        	+ (kappa-3)*sum_h4);	
    }

    /* calc CME gradient */
    tmp = 3*sqnrm_h + 3*sigma2_n*sqnrm_f - kappa;
    for (k=0; k<N_f; k++){
      gradr[k] = sigma2_n*fr_cur[k];
      for (n=0; n<N_h; n++){
        gradr[k] += Cr[k*N_h+n]*hr_cur[n];
      }
      gradr[k] *= tmp;
      for (n=0; n<N_h; n++){
        gradr[k] += (kappa-3)*hr2_cur[n]*hr_cur[n]*Cr[k*N_h+n];
      }
    }

    /* update equalizer coefficients */
    for (k=0; k<N_f; k++){		
      fr_cur[k] -= mu*gradr[k];
    }

    /* store equalizer coefficients */
    if( !(i % D_f) ) 
      for (k=0; k<N_f; k++){		
        fr[f_ind++] = fr_cur[k];
      }
  } 

  /* store final equalizer */
  for (k=0; k<N_f; k++){
    fr[N_f*(len_f-1)+k] = fr_cur[k];
  }

  /* store final y and srcme */
  yr[len_y-1] = 0;
  for (k=0; k<N_f; k++){
    yr[len_y-1] += rr[reg_ind-k]*(fr_cur[k]+mu*gradr[k]);
  }
  sum_h4 = 0; for( n=0; n<N_h; n++ ){ sum_h4 += hr2_cur[n]*hr2_cur[n]; }
  srcme[len_y-1] = sqrt(kappa*kappa - 2*kappa*(sqnrm_h + sigma2_n*sqnrm_f)
                + 3*sigma2_n*sigma2_n*sqnrm_f*sqnrm_f
                + 3*sqnrm_h*(sqnrm_h + 2*sigma2_n*sqnrm_f)
                + (kappa-3)*sum_h4);

}/* end of cmegd_rPAM() */




/*********************** 
 * cmplx-PAM CME-GD *
 ***********************
 *
 * the cmplx-channel (rotationally invariant noise) real-source CME-GD 
 * routine replaces the following matlab code:
 *
 * --------------------------------------------------
 * f_cur = f_init;
 * for i = 1:N,
 *     reg = r(1, N_f+(1+is_FSE)*i:-1:(1+is_FSE)*i+1);
 *     y_cur = reg*f_cur;       % for constellation plot...
 *     h = C*f_cur;
 *     hTh = norm(h)^2;
 *     fTf = norm(f_cur)^2;
 *     srcme_cur = sqrt( (kappa-3)*sum(abs(h).^4) + 2*hTh^2 + abs(h.'*h)^2 ...
 *	 + 2*sigma2_n^2*fTf^2 + 4*sigma2_n*hTh*fTf...
 *       - 2*kappa*(hTh + sigma2_n*fTf) + kappa^2 );
 *     dJdf = (kappa-3)*C'*(h.*(abs(h).^2)) + (h.'*h)*C'*conj(h)...
 *       + (2*hTh + 2*sigma2_n*fTf - kappa)*(C'*h + sigma2_n*f_cur);
 *     f_cur = f_cur - mu*dJdf;
 *     f(:,floor((i-1)/D_f)+1) = f_cur;
 *     y(floor((i-1)/D_e)+1) = y_cur;
 *     srcme(floor((i-1)/D_e)+1) = srcme_cur;
 * end
 * --------------------------------------------------
 */
void cmegd_cPAM(double *fr, double *fi, double *yr, double *yi, double *srcme, 
	 double *rr, double *ri, double *fr_init, double *fi_init,
         double kappa, double mu,
         int N, int N_f, int D_e, int D_f, int is_FSE, int is_cmplx,
	 int len_f, int len_y, int N_h,
	 double sigma2_n, double *Cr, double *Ci)
{
  double *fr_cur, *fi_cur, *hr_cur, *hi_cur, *h2_cur, *f2_cur, *gradr, *gradi;
  double yr_cur, yi_cur, srcme_cur;
  double sqnrm_h, sqnrm_f, sum_h4, hthr, hthi, tmp;
  int    i, k, n, f_ind=0, y_ind=0, reg_ind; 

  /* allocate local memory */
  fr_cur = (double *)mxCalloc(N_f,sizeof(double));
  fi_cur = (double *)mxCalloc(N_f,sizeof(double));
  f2_cur = (double *)mxCalloc(N_f,sizeof(double));
  hr_cur = (double *)mxCalloc(N_h,sizeof(double));
  hi_cur = (double *)mxCalloc(N_h,sizeof(double));
  h2_cur = (double *)mxCalloc(N_h,sizeof(double));
  gradr = (double *)mxCalloc(N_f,sizeof(double));
  gradi = (double *)mxCalloc(N_f,sizeof(double));

  /* initialize equalizer */
  for (k=0; k<N_f; k++){
    fr_cur[k] = fr_init[k];
    fi_cur[k] = fi_init[k];
  }

  /* adaptation routine */
  reg_ind = N_f-1;
  for (i=0; i<N; ){
    reg_ind += 1+is_FSE;		/* update regressor index */
    i++;				/* convenient to increment here */

    sqnrm_f = 0; 
    for (k=0; k<N_f; k++){ 		/* calc |f|^2 and ||f||^2 */
      f2_cur[k] = fr_cur[k]*fr_cur[k] + fi_cur[k]*fi_cur[k]; 
      sqnrm_f += f2_cur[k];
    }
    sqnrm_h = 0; 
    hthr = 0;
    hthi = 0;
    for (n=0; n<N_h; n++){ 		/* calc h, hth, |h|^2, & ||h||^2 */
      hr_cur[n] = 0;
      hi_cur[n] = 0;
      for (k=0; k<N_f; k++){ 
        hr_cur[n] += Cr[k*N_h+n]*fr_cur[k] - Ci[k*N_h+n]*fi_cur[k]; 
        hi_cur[n] += Cr[k*N_h+n]*fi_cur[k] + Ci[k*N_h+n]*fr_cur[k]; 
      } 
      h2_cur[n] = hr_cur[n]*hr_cur[n] + hi_cur[n]*hi_cur[n];
      sqnrm_h += h2_cur[n];
      hthr += hr_cur[n]*hr_cur[n] - hi_cur[n]*hi_cur[n];
      hthi += 2*hr_cur[n]*hi_cur[n];
    }

    /* calculate and store output (y) and square-root CM error (srcme) */
    if( !(i % D_e) ){
      yr[y_ind] = 0;			
      yi[y_ind] = 0;			
      for (k=0; k<N_f; k++){ 		
        yr[y_ind] += rr[reg_ind-k]*fr_cur[k] - ri[reg_ind-k]*fi_cur[k];
        yi[y_ind] += rr[reg_ind-k]*fi_cur[k] + ri[reg_ind-k]*fr_cur[k];
      }
      sum_h4 = 0; for( n=0; n<N_h; n++ ){ sum_h4 += h2_cur[n]*h2_cur[n]; }
      srcme[y_ind++] = sqrt(kappa*kappa - 2*kappa*(sqnrm_h + sigma2_n*sqnrm_f)
  		+ 2*sigma2_n*sigma2_n*sqnrm_f*sqnrm_f 
        	+ 2*sqnrm_h*(sqnrm_h + 2*sigma2_n*sqnrm_f)
        	+ (kappa-3)*sum_h4 + (hthr*hthr + hthi*hthi));
    }

    /* calc CME gradient */
    tmp = 2*sqnrm_h + 2*sigma2_n*sqnrm_f - kappa;
    for (k=0; k<N_f; k++){
      gradr[k] = sigma2_n*fr_cur[k];
      gradi[k] = sigma2_n*fi_cur[k];
      for (n=0; n<N_h; n++){
        gradr[k] += Cr[k*N_h+n]*hr_cur[n] + Ci[k*N_h+n]*hi_cur[n];
        gradi[k] += Cr[k*N_h+n]*hi_cur[n] - Ci[k*N_h+n]*hr_cur[n];
      }
      gradr[k] *= tmp;
      gradi[k] *= tmp;
      for (n=0; n<N_h; n++){
        gradr[k] += (kappa-3)*h2_cur[n]*
		(hr_cur[n]*Cr[k*N_h+n] + hi_cur[n]*Ci[k*N_h+n])
		+ hthr*(hr_cur[n]*Cr[k*N_h+n] - hi_cur[n]*Ci[k*N_h+n]) 
		+ hthi*(hr_cur[n]*Ci[k*N_h+n] + hi_cur[n]*Cr[k*N_h+n]);
        gradi[k] += (kappa-3)*h2_cur[n]*
		(hi_cur[n]*Cr[k*N_h+n] - hr_cur[n]*Ci[k*N_h+n])
		+ hthi*(hr_cur[n]*Cr[k*N_h+n] - hi_cur[n]*Ci[k*N_h+n]) 
		- hthr*(hr_cur[n]*Ci[k*N_h+n] + hi_cur[n]*Cr[k*N_h+n]);
      }
    }

    /* update equalizer coefficients */
    for (k=0; k<N_f; k++){		
      fr_cur[k] -= mu*gradr[k];
      fi_cur[k] -= mu*gradi[k];
    }

    /* store equalizer coefficients */
    if( !(i % D_f) ) 
      for (k=0; k<N_f; k++){		
        fr[f_ind] = fr_cur[k];
        fi[f_ind++] = fi_cur[k];
      }
  } 

  /* store final equalizer */
  for (k=0; k<N_f; k++){
    fr[N_f*(len_f-1)+k] = fr_cur[k];
    fi[N_f*(len_f-1)+k] = fi_cur[k];
  }

  /* store final y and srcme */
  yr[len_y-1] = 0;
  yi[len_y-1] = 0;
  for (k=0; k<N_f; k++){
    yr[len_y-1] += rr[reg_ind-k]*(fr_cur[k]+mu*gradr[k])
			- ri[reg_ind-k]*(fi_cur[k]+mu*gradi[k]);
    yi[len_y-1] += rr[reg_ind-k]*(fi_cur[k]+mu*gradi[k])
			+ ri[reg_ind-k]*(fr_cur[k]+mu*gradr[k]);
  }
  sum_h4 = 0; for( n=0; n<N_h; n++ ){ sum_h4 += h2_cur[n]*h2_cur[n]; }
  srcme[len_y-1] = sqrt(kappa*kappa - 2*kappa*(sqnrm_h + sigma2_n*sqnrm_f)
  		+ 2*sigma2_n*sigma2_n*sqnrm_f*sqnrm_f 
        	+ 2*sqnrm_h*(sqnrm_h + 2*sigma2_n*sqnrm_f)
        	+ (kappa-3)*sum_h4 + (hthr*hthr + hthi*hthi));