sviterbi.c

通信系统的matlab仿真程序

     *  x0 = [x0; 0; 0; 0; starter; expense(:); solu(:); code(:); y];
     *%        |  |  |  |      |       |        |        \- output
     *%        \  \  \  \      \       \        \ decode
     *%         \  \  \  \      \start  \-weight, expense
     *%          \  \  \  \-trace_flag
     *%           \  \  \-trace_num
     *%            \  \-fig_position
     *%             \-figure handle
     *  sys = [0; length(x0); K+1; N+1; 0; 0; 1];
     *  ts = [-1, 0];
     */
  int     i, j;
  int     len_C, leng, plot_flag, NaN;
  int     N, M, K, K2, colFunc, rowFunc, n_tran_prob, m_tran_prob;
  int     num_state, n_std_sta, PowPowM;
  int     fig_position, trace_num, trace_flag, starter, expen_flag;
  int     *A, *B, *C, *D, *TRAN_A, *TRAN_B, *expense1, *expen_tmp1, *solu, *code, *Y1;
  int     *inp_pre, *cur_sta_pre, *expen_work, *pre_state, *cur_sta, *inp;
  int     *nex_sta, *out, *expenOut, *aft_state, *sol1, *tmpIwork; 
  double  *expense, *expen_tmp, *Y, *sol, *tmpRwork, *tran_prob_tmp;

  leng = (int)mxGetPr(LENG)[0];
  plot_flag = (int)mxGetPr(PLOT_FLAG)[0];
  colFunc = mxGetN(TRAN_FUNC);
  rowFunc = mxGetM(TRAN_FUNC);
  n_tran_prob = mxGetM(TRAN_PROB);
  m_tran_prob = mxGetN(TRAN_PROB);

  if ( n_tran_prob == 3 ){
    expen_flag = 1;
    NaN = 1; 
  }else{
    expen_flag = 0;
    NaN = -1;
  }

  if( mxGetPr(TRAN_FUNC)[rowFunc*colFunc-1] < 0 ){
    N = (int)mxGetPr(TRAN_FUNC)[ rowFunc*(colFunc-1) ];
    K = (int)mxGetPr(TRAN_FUNC)[ rowFunc*(colFunc-1)+1 ];
    M = (int)mxGetPr(TRAN_FUNC)[ rowFunc*(colFunc-1)+2 ];
    len_C = M*N;
    
    A = ssGetIWork(S);
    B = A + M*M;
    C = B + M*K;
    D = C + N*M;
        
    /* Get the input Matrix A, B, C, D */
    for( i=0; i < M+N; i++ ){
      for( j=0; j < M+K; j++ ){
        if( i<M   && j<M )
          A[i+j*M] = (int)mxGetPr(TRAN_FUNC)[i+j*(M+N)];
        if( i>M-1 && j<M )
          C[i+j*N-M] = (int)mxGetPr(TRAN_FUNC)[i+j*(M+N)];
        if( i<M   && j>M-1 )
          B[i+j*M-M*M] = (int)mxGetPr(TRAN_FUNC)[i+j*(M+N)];
        if( i>M-1 && j>M-1 )
          D[i+j*N-M*(N+1)] = (int)mxGetPr(TRAN_FUNC)[i+j*(M+N)];
      }
    }
  }else{
    N = (int)mxGetPr(TRAN_FUNC)[rowFunc];
    K = (int)mxGetPr(TRAN_FUNC)[rowFunc+1];
    M = (int)mxGetPr(TRAN_FUNC)[1];
    len_C = 0;

    /* Assignment */
    TRAN_A =    ssGetIWork(S);/*   !--- size of *TRAN_A */
    TRAN_B =    ssGetIWork(S) + (rowFunc-2);/* <--- size of *TRAN_B */
    A =         ssGetIWork(S) + 2*(rowFunc-2);/*    !----- size of *A */ 
    B =         ssGetIWork(S) + 2*(rowFunc-2) + (rowFunc-2)*M;/*    !----- size of *B */

    /* Get the input Matrix A, B */
    for(i=0; i < rowFunc-2; i++){
      TRAN_A[i] = (int)mxGetPr(TRAN_FUNC)[i+2];
      TRAN_B[i] = (int)mxGetPr(TRAN_FUNC)[rowFunc+i+2];
    }
    de2bi(TRAN_A, M, rowFunc-2, A);
    de2bi(TRAN_B, N, rowFunc-2, B);
  }

  num_state = M;
  n_std_sta = 1;
  for(i=0; i < M; i++)
    n_std_sta = n_std_sta * 2;
  PowPowM = n_std_sta * n_std_sta;
  K2 = 1;
  for(i=0; i < K; i++)
    K2 = K2 * 2;

  if( expen_flag != 0 ){
    tran_prob_tmp = ssGetRWork(S);
    expense     = ssGetRWork(S) + n_tran_prob*m_tran_prob;
    Y           = ssGetRWork(S) + n_tran_prob*m_tran_prob + leng*PowPowM;
    sol         = ssGetRWork(S) + n_tran_prob*m_tran_prob + leng*PowPowM + (K+1);
    expen_tmp   = sol + PowPowM;
    tmpRwork    = expen_tmp + PowPowM;

    if( mxGetPr(TRAN_FUNC)[rowFunc*colFunc-1] < 0 )
      solu = D + N*K;     /* size of *code is leng*N */
    else
      solu = B + N*(rowFunc-2);

    code = solu + leng*PowPowM;        
    inp_pre = code + leng*N;        /* allocate K*2^K for *inp_pre */
    cur_sta_pre = inp_pre + K2*K;   /*  M*2^M for *cur_sta_pre. */
    expen_work = cur_sta_pre + M*n_std_sta;  /* allocate N for *expen_work */
    pre_state = expen_work + N;     /* allocate n_std_sta for *pre_state */
    cur_sta = pre_state + n_std_sta;/* allocate M for *cur_sta */
    inp = cur_sta + M;              /* allocate K for *inp */
    nex_sta = inp + K;              /* allocate M for *nex_sta */
    out = nex_sta + M;              /* allocate N for *out */
    expenOut = out + N;             /* allocate N for *expenOut */
    aft_state = expenOut + N;       /* allocate n_std_sta for *aft_state */
    tmpIwork  = aft_state + 2*n_std_sta;

    /*  % veraible to record the weight trace back to the first state.
     *  % the column number is the state number - 1.
     *  % the first line is the previous. The second line is the current.
     *  expense = ones(leng, PowPowM) * NaN;
     *  expense(leng, [1:n_std_sta]) = zeros(1, n_std_sta);
     *      |NaN ......NaN|
     *      |NaN ......NaN|           (leng*PowPowM)
     *      |.............|   <------- Matrix *expense
     *      |NaN ......NaN|
     *      |0 . .0NaN.NaN| 
     *
     *  solu = zeros(leng, PowPowM);
     *      |0 0 ..... 0 0|
     *      |0 0 ..... 0 0|           (leng*PowPowM)
     *      |.............|   <------- Matrix *solu
     *      |0 0 ..... 0 0|
     *      |0 0 ..... 0 0|
     */

    for(i=0; i < leng*PowPowM; i++){
      expense[i] = NaN;
      solu[i] = 0;
    }
    for(i=0; i < n_std_sta; i++)
      expense[leng+i*leng-1] = 0;
    starter = 0;
    if(plot_flag > 0)
      x0[0] = -1;
    else
      x0[0] = 0;
    fig_position = 0;
    trace_num = 0;
    trace_flag = 0;
    
    x0[1] = (double)fig_position;
    x0[2] = (double)trace_num;
    x0[3] = (double)trace_flag;
    x0[4] = (double)starter;
    for(i=0; i < leng*PowPowM; i++)
      x0[i+7] = expense[i];
    for(i=0; i < leng*PowPowM; i++)
      x0[i+7+leng*PowPowM] = 0;
    for(i=0; i < leng*N; i++)
      x0[i+7+2*leng*PowPowM] = 0;
    for(i=0; i < K+1; i++)
      x0[i+7+2*leng*PowPowM+leng*N] = 0;
  }else{ /* tran_prob is not 3-row matrix */
    if( mxGetPr(TRAN_FUNC)[rowFunc*colFunc-1] < 0 )
      expense1 = D + N*K;
    else
      expense1 = B + N*(rowFunc-2);

    solu = expense1 + leng*PowPowM;    
    code = solu + leng*PowPowM;
    Y1 = code + leng*N;              /* size of *Y1 is (K+1) */
    inp_pre = Y1 + (K+1);            /* allocate K*2^K for *inp_pre */
    cur_sta_pre = inp_pre+ K2*K;   /*  M*2^M for *cur_sta_pre. */
    pre_state =  cur_sta_pre + M*n_std_sta;  /* allocate n_std_sta for *pre_state */
    cur_sta = pre_state + n_std_sta;/* allocate M for *cur_sta */
    inp = cur_sta + M;              /* allocate K for *inp */
    nex_sta = inp + K;              /* allocate M for *nex_sta */
    out = nex_sta + M;              /* allocate N for *out */
    expenOut = out + N;             /* allocate N for *expenOut */
    aft_state = expenOut + N;       /* allocate n_std_sta for *aft_state */
    sol1 = aft_state + n_std_sta;    /* allocate PowPowM for *sol1 */
    expen_tmp1 = sol1 + PowPowM;
    tmpIwork = expen_tmp1 + PowPowM;

    for(i=0; i < leng*PowPowM; i++){
      expense1[i] = NaN;
      solu[i] = 0;
    }        
    for(i=0; i < n_std_sta; i++)
      expense1[leng+i*leng-1] = 0;
    starter = 0;
    if(plot_flag > 0)
      x0[0] = -1;
    else
      x0[0] = 0;
    fig_position = 0;
    trace_num = 0;
    trace_flag = 0;
    
    x0[1] = (double)fig_position;
    x0[2] = (double)trace_num;
    x0[3] = (double)trace_flag;
    x0[4] = (double)starter;
    for(i=0; i < leng*PowPowM; i++)
      x0[i+7] = (double)expense1[i];
    for(i=0; i < leng*PowPowM; i++)
      x0[i+7+leng*PowPowM] = 0;
    for(i=0; i < leng*N; i++)
      x0[i+7+2*leng*PowPowM] = 0;
    for(i=0; i < K+1; i++)
      x0[i+7+2*leng*PowPowM+leng*N] = 0;
  }
}
/*
 * mdlInitializeSampleTimes - initialize the sample times array
 *
 * This function is used to specify the sample time(s) for your S-function.
 * If your S-function is continuous, you must specify a sample time of 0.0.
 * Sample times must be registered in ascending order.
 */
static void mdlInitializeSampleTimes(S)
    SimStruct *S;
{
    ssSetSampleTimeEvent(S, 0,  0.0);
    ssSetOffsetTimeEvent(S, 0,  0.0);
}

/*
 * mdlOutputs - compute the outputs
 *
 * In this function, you compute the outputs of your S-function
 * block.  The outputs are placed in the y variable.
 */
static void mdlOutputs(y, x, u, S, tid)
     double *y, *x, *u;
     SimStruct *S; 
     int tid;
{
  /*  if u(length(u)) > 0.2
   *    K = tran_func(2, size(tran_func, 2));
   *    len_x = length(x);
   *    sys = x(len_x - K: len_x);
   *  end;
   */
  int     i;
  int     colFunc, rowFunc, M, K, N, leng, len_x, n_std_sta, PowPowM;
  double  max;
  
  leng = (int)mxGetPr(LENG)[0];
  rowFunc = mxGetM(TRAN_FUNC);
  colFunc = mxGetN(TRAN_FUNC);
  
  if( mxGetPr(TRAN_FUNC)[rowFunc*colFunc-1] < 0 ){
    N = (int)mxGetPr(TRAN_FUNC)[rowFunc*(colFunc-1)];
    K = (int)mxGetPr(TRAN_FUNC)[rowFunc*(colFunc-1)+1];
    M = (int)mxGetPr(TRAN_FUNC)[rowFunc*(colFunc-1)+2];
  }else{
    M = (int)mxGetPr(TRAN_FUNC)[1];
    N = (int)mxGetPr(TRAN_FUNC)[rowFunc];
    K = (int)mxGetPr(TRAN_FUNC)[rowFunc+1];
  }
  
  n_std_sta = 1;
  for(i=0; i < M; i++)
    n_std_sta = n_std_sta * 2;
  PowPowM = n_std_sta * n_std_sta;
  
  len_x = 8+2*leng*PowPowM+leng*N+K; /* number of discrete states */
  
  if( u[N] > 0.2 ){
    for(i=0; i < K+1; i++)
      y[i] = x[len_x-1-K+i];
  }
}
/*
 * mdlUpdate - perform action at major integration time step
 *
 * This function is called once for every major integration time step.
 * Discrete states are typically updated here, but this function is useful
 * for performing any tasks that should only take place once per integration
 * step. PS: flag = 2.
 */
static void mdlUpdate(x, u, S, tid)
     double *x, *u; 
     SimStruct *S;
     int tid;
{
  int     NaN, initial_flag;
  int     i, j, l, j2, jj, j_k, indx_j, j_pre, num_N, num_K;
  int     leng, plot_flag, len_C, len_x,lenIndx0, lenIndx1, len_aft_state, dec, tmp;
  int     N, M, K, K2, colFunc, rowFunc, n_tran_prob, m_tran_prob;
  int     num_state, n_std_sta, PowPowM, tran_indx, nex_sta_de;
  int     fig_position, trace_num, trace_flag, starter, expen_flag, trace_eli;
  int     loc_tmp, plot_flag_test, trace_pre, len_pre_state, numnotnan, loc_exp1, loca_exp1;  
  int     *A, *B, *C, *D, *expense1, *solu, *code, *Y1, *TRAN_A, *TRAN_B;
  int     *inp_pre, *cur_sta_pre, *expen_work, *expen_tmp1, *pre_state, *cur_sta, *inp;
  int     *nex_sta, *out, *expenOut, *aft_state, *sol1, *tmpIwork; 
  double  max, min, loca_exp, loc_exp;
  double  *expense, *expen_tmp, *Y, *sol, *tmpRwork, *tran_prob_tmp;
    
  leng = (int)mxGetPr(LENG)[0];
  plot_flag = (int)mxGetPr(PLOT_FLAG)[0];
  rowFunc = mxGetM(TRAN_FUNC);
  colFunc = mxGetN(TRAN_FUNC);
  n_tran_prob = mxGetM(TRAN_PROB);
  m_tran_prob = mxGetN(TRAN_PROB);
  
  if ( n_tran_prob == 3 ){
    expen_flag = 1;
    NaN = 1; 
  }else{
    expen_flag = 0;
    NaN = -1;
  }

  if( mxGetPr(TRAN_FUNC)[rowFunc*colFunc-1] < 0 ){
    N = (int)mxGetPr(TRAN_FUNC)[ rowFunc*(colFunc-1) ];
    K = (int)mxGetPr(TRAN_FUNC)[ rowFunc*(colFunc-1)+1 ];
    M = (int)mxGetPr(TRAN_FUNC)[ rowFunc*(colFunc-1)+2 ];
    len_C = M*N;
    
    A = ssGetIWork(S);
    B = A + M*M;
    C = B + M*K;
    D = C + N*M;
  }else{
    N = (int)mxGetPr(TRAN_FUNC)[rowFunc];
    K = (int)mxGetPr(TRAN_FUNC)[rowFunc+1];
    M = (int)mxGetPr(TRAN_FUNC)[1];
    len_C = 0;

    /* Assignment */
    TRAN_A =    ssGetIWork(S);/*   !--- size of *TRAN_A */
    TRAN_B =    ssGetIWork(S) + (rowFunc-2);/* <--- size of *TRAN_B */
    A =         ssGetIWork(S) + 2*(rowFunc-2);/*    !----- size of *A */ 
    B =         ssGetIWork(S) + 2*(rowFunc-2) + (rowFunc-2)*M;/*    !----- size of *B */
  }
  
  num_state = M;
  n_std_sta = 1;
  for(i=0; i < M; i++)
    n_std_sta = n_std_sta * 2;
  PowPowM = n_std_sta * n_std_sta;
  K2 = 1;
  for(i=0; i < K; i++)
    K2 = K2 * 2;

    /*  % the major processing routine.
     *  if u(length(u)) < .2
     *    % in the case of no signal, no processing.
     *    return;
     *  end;
     *  % otherwise, there is a signal, have to process.
     *   u = u(:)';
     *
     *  % initial parameters.
     *  [A, B, C, D, N, K, M] = gen2abcd(tran_func);
     *  num_state = M;
     *  n_std_sta = 2^M;
     *  PowPowM = n_std_sta^2;
     *  K2 = 2^K;
     *  inp_pre = de2bi([0:K2-1]', K); 
     *  cur_sta_pre = de2bi([0:n_std_sta-1], M);
     */

  if ( u[N] >= 0.2 ){
    if( expen_flag != 0 ){  /* Tran_prob is a THREE rows Matrix */
      tran_prob_tmp    = ssGetRWork(S);
      expense     = tran_prob_tmp + n_tran_prob*m_tran_prob;
      Y           = ssGetRWork(S) + n_tran_prob*m_tran_prob + leng*PowPowM;
      sol         = ssGetRWork(S) + n_tran_prob*m_tran_prob + leng*PowPowM + (K+1);
      expen_tmp   = sol +  PowPowM;
      tmpRwork    = expen_tmp + PowPowM;
   
      if( mxGetPr(TRAN_FUNC)[rowFunc*colFunc-1] < 0 )
        solu = D + N*K;
      else
        solu = B + N*(rowFunc-2);
   
      code        = solu + leng*PowPowM;        
      inp_pre     = code + leng*N;        /* allocate K*2^K for *inp_pre */
      cur_sta_pre = inp_pre + K2*K;   /*  M*2^M for *cur_sta_pre. */
      expen_work  = cur_sta_pre + M*n_std_sta;  /* allocate N for *expen_work */
      pre_state   = expen_work + N;     /* allocate n_std_sta for *pre_state */
      cur_sta     = pre_state + n_std_sta;/* allocate M for *cur_sta */
      inp         = cur_sta + M;              /* allocate K for *inp */
      nex_sta     = inp + K;              /* allocate M for *nex_sta */
      out         = nex_sta + M;              /* allocate N for *out */
      expenOut    = out + N;             /* allocate N for *expenOut */
      aft_state   = expenOut + N;       /* allocate n_std_sta for *aft_state */
      tmpIwork    = aft_state + 2*n_std_sta;

      if ( x[6] != 12345 ){
        for(i=0; i < leng*PowPowM; i++){
          expense[i] = NaN;
          solu[i] = 0;
        }
        for(i=0; i < n_std_sta; i++)
          expense[leng+i*leng-1] = 0;
        starter = 0;
        x[0] = 0;
        if(plot_flag > 0)
          x[5] = 1;
        else
          x[5] = 0;
        x[6] = 12345;

        for(i=0; i < leng*N; i++)
          code[i] = 0;
        for(i=0; i < K+1; i++)
          Y[i] = 0;
        fig_position = 0;
        trace_num = 0;
        trace_flag = 0;

        x[1] = (double)fig_position;
        x[2] = (double)trace_num;
        x[3] = (double)trace_flag;
        x[4] = (double)starter;
      } 
			
      inp_pre[0] = -1;
      cur_sta_pre[0] = -1;
      de2bi(inp_pre, K, K2, inp_pre);
      de2bi(cur_sta_pre, M, n_std_sta, cur_sta_pre);

      starter = (int)x[4];
      plot_flag_test = (int)x[5];
      initial_flag = (int)x[6];
      loc_tmp = 8;

#ifdef MATLAB_MEX_FILE
      if(plot_flag_test > 0){