Пример #1
0
int32_t splot_buff_2(void)
{
	arm_status status;                           /* Status of the example */
	arm_cfft_radix4_instance_f32 cfft_instance;  /* CFFT Structure instance */
	/* CFFT Structure instance pointer */
	arm_cfft_radix4_instance_f32 *cfft_instance_ptr =
		      (arm_cfft_radix4_instance_f32*) &cfft_instance;
    /* Initialise the fft input buffers with all zeros */
	arm_fill_f32(0.0,  buff_wej_dodatkowy_1, ile_probek);
	arm_fill_f32(0.0,  buff_wej_dodatkowy_2, ile_probek);
	/* Copy the input values to the fft input buffers */
	arm_copy_f32(ADC3ConvertedValue1,  buff_wej_dodatkowy_1, ile_probek);
	arm_copy_f32(buff_odp_imp_filtr,  buff_wej_dodatkowy_2, ile_probek);
	/* Initialize the CFFT function to compute 64 point fft */
	status = arm_cfft_radix4_init_f32(cfft_instance_ptr, 64, 0, 1);
	/* Transform input a[n] from time domain to frequency domain A[k] */
	arm_cfft_radix4_f32(cfft_instance_ptr, buff_wej_dodatkowy_1);
	/* Transform input b[n] from time domain to frequency domain B[k] */
	arm_cfft_radix4_f32(cfft_instance_ptr, buff_wej_dodatkowy_2);
	/* Complex Multiplication of the two input buffers in frequency domain */
	arm_cmplx_mult_cmplx_f32(buff_wej_dodatkowy_1, buff_wej_dodatkowy_2, buff_wyj1, ile_probek);
	/* Initialize the CIFFT function to compute 64 point ifft */
	status = arm_cfft_radix4_init_f32(cfft_instance_ptr, 64, 1, 1);
	/* Transform the multiplication output from frequency domain to time domain,
		     that gives the convolved output  */
	arm_cfft_radix4_f32(cfft_instance_ptr, buff_wyj1);

	status = ARM_MATH_SUCCESS;
}
Пример #2
0
Файл: main.c Проект: A-Paul/RIOT
/* ----------------------------------------------------------------------
* Variance calculation test
* ------------------------------------------------------------------- */
int main(void)
{
    arm_status status;
    float32_t mean;
    float32_t oneByBlockSize = 1.0 / (blockSize);
    float32_t variance;
    float32_t diff;
    status = ARM_MATH_SUCCESS;
    puts("ARM DSP lib test");
    puts("Note: This test is using 32 bit IEEE 754 floating point numbers,"
         "(24 bit mantissa, 7 bit exponent)");
    puts("Expect roughly 7 decimals precision on the result.");
    /* Calculation of mean value of input */
    /* x' = 1/blockSize * (x(0)* 1 + x(1) * 1 + ... + x(n-1) * 1) */
    /* Fill wire1 buffer with 1.0 value */
    arm_fill_f32(1.0, wire1, blockSize);
    /* Calculate the dot product of wire1 and wire2 */
    /* (x(0)* 1 + x(1) * 1 + ...+ x(n-1) * 1) */
    arm_dot_prod_f32(testInput_f32, wire1, blockSize, &mean);
    /* 1/blockSize * (x(0)* 1 + x(1) * 1 + ... + x(n-1) * 1)  */
    arm_mult_f32(&mean, &oneByBlockSize, &mean, 1);
    /* Calculation of variance value of input */
    /* (1/blockSize) * (x(0) - x') * (x(0) - x') + (x(1) - x') * (x(1) - x') + ... + (x(n-1) - x') * (x(n-1) - x') */
    /* Fill wire2 with mean value x' */
    arm_fill_f32(mean, wire2, blockSize);
    /* wire3 contains (x-x') */
    arm_sub_f32(testInput_f32, wire2, wire3, blockSize);
    /* wire2 contains (x-x') */
    arm_copy_f32(wire3, wire2, blockSize);
    /* (x(0) - x') * (x(0) - x') + (x(1) - x') * (x(1) - x') + ... + (x(n-1) - x') * (x(n-1) - x') */
    arm_dot_prod_f32(wire2, wire3, blockSize, &variance);
    /* Calculation of 1/blockSize */
    oneByBlockSize = 1.0 / (blockSize - 1);
    /* Calculation of variance */
    arm_mult_f32(&variance, &oneByBlockSize, &variance, 1);
    /* absolute value of difference between ref and test */
    diff = variance - refVarianceOut;
    /* Split into fractional and integral parts, since printing floats may not be supported on all platforms */
    float int_part;
    float frac_part = fabsf(modff(variance, &int_part));
    printf("      dsp: %3d.%09d\n", (int) int_part, (int) (frac_part * 1.0e9f + 0.5f));
    puts(  "reference:   0.903941793931839");
    frac_part = fabsf(modff(diff, &int_part));
    printf("     diff: %3d.%09d\n", (int) int_part, (int) (frac_part * 1.0e9f + 0.5f));
    /* Comparison of variance value with reference */
    if(fabsf(diff) > DELTA) {
        status = ARM_MATH_TEST_FAILURE;
    }
    if(status != ARM_MATH_SUCCESS) {
        puts("Test failed");
        while(1)
            ;
    }
    puts("Test done");
    while(1)
        ; /* main function does not return */
}
int32_t main(void)
{
  arm_status status;                           /* Status of the example */
  arm_cfft_radix4_instance_f32 cfft_instance;  /* CFFT Structure instance */

  /* CFFT Structure instance pointer */
  arm_cfft_radix4_instance_f32 *cfft_instance_ptr =
      (arm_cfft_radix4_instance_f32*) &cfft_instance;

  /* output length of convolution */
  outLen = srcALen + srcBLen - 1;

  /* Initialise the fft input buffers with all zeros */
  arm_fill_f32(0.0,  Ak, MAX_BLOCKSIZE);
  arm_fill_f32(0.0,  Bk, MAX_BLOCKSIZE);

  /* Copy the input values to the fft input buffers */
  arm_copy_f32(testInputA_f32,  Ak, MAX_BLOCKSIZE/2);
  arm_copy_f32(testInputB_f32,  Bk, MAX_BLOCKSIZE/2);

  /* Initialize the CFFT function to compute 64 point fft */
  status = arm_cfft_radix4_init_f32(cfft_instance_ptr, 64, 0, 1);

  /* Transform input a[n] from time domain to frequency domain A[k] */
  arm_cfft_radix4_f32(cfft_instance_ptr, Ak);
  /* Transform input b[n] from time domain to frequency domain B[k] */
  arm_cfft_radix4_f32(cfft_instance_ptr, Bk);

  /* Complex Multiplication of the two input buffers in frequency domain */
  arm_cmplx_mult_cmplx_f32(Ak, Bk, AxB, MAX_BLOCKSIZE/2);

  /* Initialize the CIFFT function to compute 64 point ifft */
  status = arm_cfft_radix4_init_f32(cfft_instance_ptr, 64, 1, 1);

  /* Transform the multiplication output from frequency domain to time domain,
     that gives the convolved output  */
  arm_cfft_radix4_f32(cfft_instance_ptr, AxB);

  /* SNR Calculation */
  snr = arm_snr_f32((float32_t *)testRefOutput_f32, AxB, srcALen + srcBLen - 1);

  /* Compare the SNR with threshold to test whether the
     computed output is matched with the reference output values. */
  if( snr > SNR_THRESHOLD)
  {
    status = ARM_MATH_SUCCESS;
  }

  if( status != ARM_MATH_SUCCESS)
  {
    while(1);
  }

  while(1);                             /* main function does not return */
}
Пример #4
0
void matrixInit(arm_matrix_instance_f32 *m, int rows, int cols) {
    float32_t *d;

    d = (float32_t *)aqDataCalloc(rows*cols, sizeof(float32_t));

    arm_mat_init_f32(m, rows, cols, d);
    arm_fill_f32(0, d, rows*cols);
}
Пример #5
0
void FloatArray::setAll(float value){
/// @note When built for ARM Cortex-M processor series, this method uses the optimized <a href="http://www.keil.com/pack/doc/CMSIS/General/html/index.html">CMSIS library</a>
#ifdef ARM_CORTEX
  arm_fill_f32(value, data, size);
#else
  for(int n=0; n<size; n++){
    data[n]=value;
  }
#endif /* ARM_CORTEX */
}
Пример #6
0
void ComplexFloatArray::setAll(float value){
#ifdef ARM_CORTEX
  arm_fill_f32(value, (float *)data, size *2 ); //note the *2 multiplier which accounts for real and imaginary parts
#else
  ComplexFloat val;
  val.re=value;
  val.im=value;
  setAll(val);
#endif /* ARM_CORTEX */
}
Пример #7
0
// Calculates the QR decomposition of the given matrix A Transposed (decomp's A', not A)
//      notes:  A matrix is modified
//      Adapted from Java code originaly written by Joni Salonen
//
// returns 1 for success, 0 for failure
int qrDecompositionT_f32(arm_matrix_instance_f32 *A, arm_matrix_instance_f32 *Q, arm_matrix_instance_f32 *R) {
    int minor;
    int row, col;
    int m = A->numCols;
    int n = A->numRows;
    int min;

    // clear R
    arm_fill_f32(0, R->pData, R->numRows*R->numCols);

    min = MIN(m, n);

    /*
    * The QR decomposition of a matrix A is calculated using Householder
    * reflectors by repeating the following operations to each minor
    * A(minor,minor) of A:
    */
    for (minor = 0; minor < min; minor++) {
	    float xNormSqr = 0.0f;
	    float a;

	    /*
	    * Let x be the first column of the minor, and a^2 = |x|^2.
	    * x will be in the positions A[minor][minor] through A[m][minor].
	    * The first column of the transformed minor will be (a,0,0,..)'
	    * The sign of a is chosen to be opposite to the sign of the first
	    * component of x. Let's find a:
	    */
	    for (row = minor; row < m; row++)
		    xNormSqr += A->pData[minor*m + row]*A->pData[minor*m + row];

	    a = __sqrtf(xNormSqr);
	    if (A->pData[minor*m + minor] > 0.0f)
		    a = -a;

	    if (a != 0.0f) {
		    R->pData[minor*R->numCols + minor] = a;

		    /*
		    * Calculate the normalized reflection vector v and transform
		    * the first column. We know the norm of v beforehand: v = x-ae
		    * so |v|^2 = <x-ae,x-ae> = <x,x>-2a<x,e>+a^2<e,e> =
		    * a^2+a^2-2a<x,e> = 2a*(a - <x,e>).
		    * Here <x, e> is now A[minor][minor].
		    * v = x-ae is stored in the column at A:
		    */
		    A->pData[minor*m + minor] -= a; // now |v|^2 = -2a*(A[minor][minor])

		    /*
		    * Transform the rest of the columns of the minor:
		    * They will be transformed by the matrix H = I-2vv'/|v|^2.
		    * If x is a column vector of the minor, then
		    * Hx = (I-2vv'/|v|^2)x = x-2vv'x/|v|^2 = x - 2<x,v>/|v|^2 v.
		    * Therefore the transformation is easily calculated by
		    * subtracting the column vector (2<x,v>/|v|^2)v from x.
		    *
		    * Let 2<x,v>/|v|^2 = alpha. From above we have
		    * |v|^2 = -2a*(A[minor][minor]), so
		    * alpha = -<x,v>/(a*A[minor][minor])
		    */
		    for (col = minor+1; col < n; col++) {
			    float alpha = 0.0f;

			    for (row = minor; row < m; row++)
				    alpha -= A->pData[col*m + row]*A->pData[minor*m + row];

			    alpha /= a*A->pData[minor*m + minor];

			    // Subtract the column vector alpha*v from x.
			    for (row = minor; row < m; row++)
				    A->pData[col*m + row] -= alpha*A->pData[minor*m + row];
		    }
	    }
	    // rank deficient
	    else
		return 0;
    }

    // Form the matrix R of the QR-decomposition.
    //      R is supposed to be m x n, but only calculate n x n
    // copy the upper triangle of A
    for (row = min-1; row >= 0; row--)
	    for (col = row+1; col < n; col++)
		    R->pData[row*R->numCols + col] = A->pData[col*m + row];

    // Form the matrix Q of the QR-decomposition.
    //      Q is supposed to be m x m

    // only compute Q if requested
    if (Q) {
	    arm_fill_f32(0, Q->pData, Q->numRows*Q->numCols);

	    /*
	    * Q = Q1 Q2 ... Q_m, so Q is formed by first constructing Q_m and then
	    * applying the Householder transformations Q_(m-1),Q_(m-2),...,Q1 in
	    * succession to the result
	    */
	    for (minor = m-1; minor >= min; minor--)
		    Q->pData[minor*m + minor] = 1.0f;

	    for (minor = min-1; minor >= 0; minor--) {
		    Q->pData[minor * m + minor] = 1.0f;

		    if (A->pData[minor*m + minor] != 0.0f) {
			    for (col = minor; col < m; col++) {
				    float alpha = 0.0f;

				    for (row = minor; row < m; row++)
					    alpha -= Q->pData[row*m + col]*A->pData[minor*m + row];

				    alpha /= R->pData[minor*R->numCols + minor]*A->pData[minor*m + minor];

				    for (row = minor; row < m; row++)
					    Q->pData[row*m + col] -= alpha*A->pData[minor*m + row];
			    }
		    }
	    }
    }

    return 1;
}
Пример #8
0
void srcdkfMeasurementUpdate(srcdkf_t *f, float32_t *u, float32_t *ym, int M, int N, float32_t *noise, SRCDKFMeasurementUpdate_t *measurementUpdate) {
	int S = f->S;				// number of states
	float32_t *Xa = f->Xa.pData;			// sigma points
	float32_t *xIn = f->xIn;			// callback buffer
	float32_t *xNoise = f->xNoise;		// callback buffer
	float32_t *xOut = f->xOut;			// callback buffer
	float32_t *Y = f->Y.pData;			// measurements from sigma points
	float32_t *y = f->y.pData;			// measurement estimate
	float32_t *Sn = f->Sn.pData;			// observation noise covariance
	float32_t *qrTempM = f->qrTempM.pData;
	float32_t *C1 = f->C1.pData;
	float32_t *C1T = f->C1T.pData;
	float32_t *C2 = f->C2.pData;
	float32_t *D = f->D.pData;
	float32_t *inov = f->inov.pData;		// M x 1 matrix
	float32_t *xUpdate = f->xUpdate.pData;	// S x 1 matrix
	float32_t *x = f->x.pData;			// state estimate
	float32_t *Sx = f->Sx.pData;
//	float32_t *SxT = f->SxT.pData;
//	float32_t *Pxy = f->Pxy.pData;
	float32_t *Q = f->Q.pData;
	float32_t *qrFinal = f->qrFinal.pData;
	int L;					// number of sigma points
	int i, j;

	// make measurement noise matrix if provided
	if (noise) {
		f->Sn.numRows = N;
		f->Sn.numCols = N;
		arm_fill_f32(0, f->Sn.pData, N*N);
		for (i = 0; i < N; i++)
			arm_sqrt_f32(fabsf(noise[i]), &Sn[i*N + i]);
	}

	// generate sigma points
	srcdkfCalcSigmaPoints(f, &f->Sn);
	L = f->L;

	// resize all N and M based storage as they can change each iteration
	f->y.numRows = M;
	f->Y.numRows = M;
	f->Y.numCols = L;
	f->qrTempM.numRows = M;
	f->qrTempM.numCols = (S+N)*2;
	f->Sy.numRows = M;
	f->Sy.numCols = M;
	f->SyT.numRows = M;
	f->SyT.numCols = M;
	f->SyC.numRows = M;
	f->SyC.numCols = M;
	f->Pxy.numCols = M;
	f->C1.numRows = M;
	f->C1T.numCols = M;
	f->C2.numRows = M;
	f->C2.numCols = N;
	f->D.numRows = M;
	f->D.numCols = S+N;
	f->K.numCols = M;
	f->inov.numRows = M;
	f->qrFinal.numCols = 2*S + 2*N;

	// Y = h(Xa, Xn)
	for (i = 0; i < L; i++) {
		for (j = 0; j < S; j++)
			xIn[j] = Xa[j*L + i];

		for (j = 0; j < N; j++)
			xNoise[j] = Xa[(S+j)*L + i];

		measurementUpdate(u, xIn, xNoise, xOut);

		for (j = 0; j < M; j++)
			Y[j*L + i] = xOut[j];
	}

	// sum weighted resultant sigma points to create estimated measurement
	f->w0m = (f->hh - (float32_t)(S+N)) / f->hh;
	for (i = 0; i < M; i++) {
		int rOffset = i*L;

		y[i] = Y[rOffset + 0] * f->w0m;

		for (j = 1; j < L; j++)
			y[i] += Y[rOffset + j] * f->wim;
	}

	// calculate measurement covariance components
	for (i = 0; i < M; i++) {
		int rOffset = i*(S+N)*2;

		for (j = 0; j < S+N; j++) {
			float32_t c, d;

			c = (Y[i*L + j + 1] - Y[i*L + S+N + j + 1]) * f->wic1;
			d = (Y[i*L + j + 1] + Y[i*L + S+N + j + 1] - 2.0f*Y[i*L]) * f->wic2;

			qrTempM[rOffset + j] = c;
			qrTempM[rOffset + S+N + j] = d;

			// save fragments for future operations
			if (j < S) {
				C1[i*S + j] = c;
				C1T[j*M + i] = c;
			}
			else {
				C2[i*N + (j-S)] = c;
			}
			D[i*(S+N) + j] = d;
		}
	}
//matrixDump("Y", &f->Y);
//yield(1);
//matrixDump("qrTempM", &f->qrTempM);
	qrDecompositionT_f32(&f->qrTempM, NULL, &f->SyT);	// with transposition
//matrixDump("SyT", &f->SyT);


	arm_mat_trans_f32(&f->SyT, &f->Sy);
	arm_mat_trans_f32(&f->SyT, &f->SyC);		// make copy as later Div is destructive

	// create Pxy
	arm_mat_mult_f32(&f->Sx, &f->C1T, &f->Pxy);

	// K = (Pxy / SyT) / Sy
	matrixDiv_f32(&f->K, &f->Pxy, &f->SyT, &f->Q, &f->R, &f->AQ);
	matrixDiv_f32(&f->K, &f->K, &f->Sy, &f->Q, &f->R, &f->AQ);

	// x = x + k(ym - y)
	for (i = 0; i < M; i++)
		inov[i] = ym[i] - y[i];
/*
	if(M == 3) {
		printf("Measured   :\t%8.4f\t%8.4f\t%8.4f\nCalculated:\t%8.4f\t%8.4f\t%8.4f\n",
				ym[0],ym[1],ym[2],y[0],y[1],y[2]);
	}*/

	arm_mat_mult_f32(&f->K, &f->inov, &f->xUpdate);
//matrixDump("K", &f->K);
	for (i = 0; i < S; i++)
		x[i] += xUpdate[i];

	// build final QR matrix
	//	rows = s
	//	cols = s + n + s + n
	//	use Q as temporary result storage

	f->Q.numRows = S;
	f->Q.numCols = S;
	arm_mat_mult_f32(&f->K, &f->C1, &f->Q);
	for (i = 0; i < S; i++) {
		int rOffset = i*(2*S + 2*N);

		for (j = 0; j < S; j++)
			qrFinal[rOffset + j] = Sx[i*S + j] - Q[i*S + j];
	}

	f->Q.numRows = S;
	f->Q.numCols = N;
	arm_mat_mult_f32(&f->K, &f->C2, &f->Q);
	for (i = 0; i < S; i++) {
		int rOffset = i*(2*S + 2*N);

		for (j = 0; j < N; j++)
			qrFinal[rOffset + S+j] = Q[i*N + j];
	}

	f->Q.numRows = S;
	f->Q.numCols = S+N;
	arm_mat_mult_f32(&f->K, &f->D, &f->Q);
	for (i = 0; i < S; i++) {
		int rOffset = i*(2*S + 2*N);

		for (j = 0; j < S+N; j++)
			qrFinal[rOffset + S+N+j] = Q[i*(S+N) + j];
	}

	// Sx = qr([Sx-K*C1 K*C2 K*D]')
	// this method is not susceptable to numeric instability like the Cholesky is
	qrDecompositionT_f32(&f->qrFinal, NULL, &f->SxT);	// with transposition
	arm_mat_trans_f32(&f->SxT, &f->Sx);
}
int32_t main(void)
{
	arm_status status;
	float32_t mean, oneByBlockSize;
	float32_t variance;
	float32_t diff;
	
	status = ARM_MATH_SUCCESS;
	
	/* Calculation of mean value of input */
	
	/* x' = 1/blockSize * (x(0)* 1 + x(1) * 1 + ... + x(n-1) * 1) */
	
	/* Fill wire1 buffer with 1.0 value */
	arm_fill_f32(1.0,  wire1, blockSize);
	
	/* Calculate the dot product of wire1 and wire2 */
	/* (x(0)* 1 + x(1) * 1 + ...+ x(n-1) * 1) */
	arm_dot_prod_f32(testInput_f32, wire1, blockSize, &mean);
	
	/* Calculation of 1/blockSize */
	oneByBlockSize = 1.0 / (blockSize);
	
	/* 1/blockSize * (x(0)* 1 + x(1) * 1 + ... + x(n-1) * 1)  */
	arm_mult_f32(&mean, &oneByBlockSize, &mean, 1);
	
	
	/* Calculation of variance value of input */
	
	/* (1/blockSize) * (x(0) - x') * (x(0) - x') + (x(1) - x') * (x(1) - x') + ... + (x(n-1) - x') * (x(n-1) - x') */
	
	/* Fill wire2 with mean value x' */
	arm_fill_f32(mean,  wire2, blockSize);
	
	/* wire3 contains (x-x') */		
	arm_sub_f32(testInput_f32, wire2, wire3, blockSize);
	
	/* wire2 contains (x-x') */				
	arm_copy_f32(wire3, wire2, blockSize);
	
	/* (x(0) - x') * (x(0) - x') + (x(1) - x') * (x(1) - x') + ... + (x(n-1) - x') * (x(n-1) - x') */
	arm_dot_prod_f32(wire2, wire3, blockSize, &variance); 

    /* Calculation of 1/blockSize */
	oneByBlockSize = 1.0 / (blockSize - 1);

	/* Calculation of variance */		
	arm_mult_f32(&variance, &oneByBlockSize, &variance, 1);
	
	/* absolute value of difference between ref and test */
	diff = fabsf(refVarianceOut - variance);
	
	/* Comparison of variance value with reference */
	if(diff > DELTA)
	{
		status = ARM_MATH_TEST_FAILURE;
	}
		
	if( status != ARM_MATH_SUCCESS)
	{
	  while(1);
	}
}