ftf2.cpp

快速横向滤波器FTF用于系统参数估计

#include "mex.h" 

# define N 2
# include <iostream.h>
#include <iomanip.h>
#include <stdlib.h>
#include <time.h>
#include "ftf2.h"


void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]) { 
//void FTF (void)
//{
//	mexPrintf("Hello, world!\n");
	//int N;

//	N=nrhs;

	double a[N][1]={0}, acopy[N][1]={0};
  double b[N][1]={0}, bcopy[N][1]={0};
  double w[N][1]={0};
  double g[N][1]={0}, gcopy[N][1]={0};
  double rN=1.0, rNplus1=0.0;
  double front_f=1,back_f=1,front_f_copy=0.0;
  double front_e_n_1, front_e_n;
  double X[1][N]={0}, Xcopy[1][N]={0};
  double K[N][1]={0};
  double k=0.0;
  double back_e_n_1, back_e_n;
  double error;
  double x_n_N;
  double x,x2=0.0;
  double d;
  double *output;
  int h=0,adjust_c_r;
  double *data1,*data2;
 
  /* Create the array */
plhs[0] = mxCreateDoubleMatrix(20,2,mxREAL);
//output = mxGetPr(plhs[0]);

 /* Create an array for the output's data */
    output = (double *) mxMalloc(20*2 * sizeof(double));

	 /* Get the data passed in */
    data1 = mxGetPr(prhs[0]);
	data2 = mxGetPr(prhs[1]);
	//data3 = mxGetPr(prhs[2]);
 
 
 
  
 for(int n=1;n<21;n++)
{ 
   //cout<<"please input:"; 
    //cin>>x;
    //cout<<"\n";
	 x=rand()%9/2;
	
     d=data1[0]*x+data2[0]*x2;

   front_e_n_1=x-matrix_product(X,a,N);
   //cout<<front_e_n_1<<"\n";
   front_e_n=rN*front_e_n_1;
   //cout<<front_e_n<<"\n";
   front_f_copy=front_f;
   front_f=front_f+front_e_n*front_e_n_1;

   matrix_copy(gcopy,g,N);
   matrix_coefficient(front_e_n_1,gcopy,N);
   matrix_sum(a,gcopy,N);
   rNplus1=front_f_copy/front_f*rN;
   matrix_copy(acopy,a,N);
   matrix_coefficient(-front_e_n/front_f,acopy,N);
   matrix_sum(acopy,g,N);
   matrix_equivalent(K,acopy,N,front_e_n/front_f);
   k=acopy[N-1][0];
   x_n_N=X[0][N-1];
   matrix_new(x,X,N);
   matrix_copy2(Xcopy,X,N);
   back_e_n_1=x_n_N-matrix_product(Xcopy,b,N);
   rN=rNplus1/(1-k*back_e_n_1);
   back_e_n=rN*back_e_n_1;
   back_f=back_f+back_e_n*back_e_n_1;
   matrix_copy(bcopy,b,N);
   matrix_coefficient(k,bcopy,N);
   matrix_sum(K,bcopy,N);
   matrix_coefficient(rN/rNplus1,K,N);
   matrix_copy(g,K,N);
   matrix_copy(gcopy,g,N);
   matrix_coefficient(back_e_n_1,gcopy,N);
   matrix_sum(b,gcopy,N);
   error=d-matrix_product(Xcopy,w,N);
   matrix_copy(gcopy,g,N);
   matrix_coefficient(error,gcopy,N);
   matrix_sum(w,gcopy,N);
   adjust_c_r=h;
   for(int i=0;i<N;i++)
   {
	   //adjust_c_r=h;

    //for(int j=0;j<1;j++)
   output[adjust_c_r] = w[i][0]; 
   adjust_c_r+=20;
   }
   h=h+1;

   //cout<<w[i][j]<<"  " ;
//	cout<<"\n";
//	x3=x2;
   x2=x;
}
mxSetPr(plhs[0], output);
 /* Populate the output */
//for (int j = 0; j < 6; j++) 
 //for(int i=0;i<N;i++)
//memcpy(output,w, 6*sizeof(double));
//output[j][i] = w[0][0]; 
}