/* ----------------------------------------------------------------------
* 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 */
}
int main(){
const int length = 5;
float input[length] = {-0.665365, -0.329988, 0.164465, 0.043962, 0.295885};
float output[length]; // array for storing Kalman processed values
float temp[length]; // array for storing subtraction results
float temp2[length];
float miscresult[2] = {0, 0}; // array for storing mean and std dev results
float holder[length]; // array for storing convolution results
int i, j;
float corr_temp[1] = {0};
/*START OF PART II*/
kalman_state kstate;
reset(&kstate);
//Kalmanfilter_C(input, output, &kstate, length);
Kalmanfilter_asm(output, input, length, &kstate);
printf("\n");
/*END OF PART II*/
/*START OF PART III*/
// subtract
printf("subtraction:\n");
subtract(temp, input, output, length);
arm_sub_f32(input, output, temp2, length);
for(j = 0; j < length; j++){
printf("My implementation: %f CMSIS: %f\n", temp[j], temp2[j]);
}
// misc
printf("\n");
//misc(miscresult, temp, length);
arm_mean_f32(temp, length, &miscresult[0]);
arm_std_f32(temp, length, &miscresult[1]);
printf("mean: %f stdev: %f\n", miscresult[0], miscresult[1]);
// correlation
//corr_temp[0] = correlation(input, output, length);
arm_correlate_f32(input, length, output, length, &corr_temp[0]);
printf("correlation: %f\n", corr_temp[0]);
// convolution
printf("\n");
for(i = 0; i < length; i++){
holder[i] = 0;
}
convolve(holder, input, output, length);
//arm_conv_f32(input, length, output, length, holder);
for(i = 0; i < length; i++){
printf("convolution %f \n", holder[i]);
}
/*END OF PART III*/
return 0;
}
void vector_sub(const vec_3d *a, const vec_3d *b, vec_3d *out)
{
#ifdef LIBQUAT_ARM
arm_sub_f32((float*) a, (float*) b, (float*) out);
#else
out->x = a->x - b->x;
out->y = a->y - b->y;
out->z = a->z - b->z;
#endif
}
void FloatArray::subtract(FloatArray operand2, FloatArray destination){ //allows in-place
ASSERT(operand2.size == size && destination.size==size, "Arrays must be same size");
/// @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
/* despite not explicitely documented in the CMSIS documentation,
this has been tested to behave properly even when pSrcA==pDst
void arm_sub_f32 (float32_t *pSrcA, float32_t *pSrcB, float32_t *pDst, uint32_t blockSize)
*/
arm_sub_f32(data, operand2.data, destination.data, size);
#else
for(int n=0; n<size; n++){
destination[n]=data[n]-operand2[n];
}
#endif /* ARM_CORTEX */
}
// ARM DSP Function ****************************************************************************************
int dsp_arm(float* inputArray, float* outputArray, dsp_t* analysisOut) {
// Get diffence Array
arm_sub_f32(outputArray, inputArray, analysisOut->diffArr, ARRAY_LENGTH);
// Get mean
arm_mean_f32(analysisOut->diffArr, (uint32_t) ARRAY_LENGTH, &analysisOut->meanDiff);
// Get standard deviation
arm_std_f32(analysisOut->diffArr, (uint32_t) ARRAY_LENGTH, &analysisOut->standDevDiff);
// Get correlation
arm_correlate_f32(inputArray, ARRAY_LENGTH, outputArray, ARRAY_LENGTH, analysisOut->corrArr);
// Get convolution
arm_conv_f32(inputArray, ARRAY_LENGTH, outputArray, ARRAY_LENGTH, analysisOut->convolArr);
return 0;
}
int32_t main(void)
{
uint32_t i;
arm_status status;
uint32_t index;
float32_t minValue;
/* Initialize the LMSNorm data structure */
arm_lms_norm_init_f32(&lmsNorm_instance, NUMTAPS, lmsNormCoeff_f32, lmsStateF32, MU, BLOCKSIZE);
/* Initialize the FIR data structure */
arm_fir_init_f32(&LPF_instance, NUMTAPS, (float32_t *)FIRCoeff_f32, firStateF32, BLOCKSIZE);
/* ----------------------------------------------------------------------
* Loop over the frames of data and execute each of the processing
* functions in the system.
* ------------------------------------------------------------------- */
for(i=0; i < NUMFRAMES; i++)
{
/* Read the input data - uniformly distributed random noise - into wire1 */
arm_copy_f32(testInput_f32 + (i * BLOCKSIZE), wire1, BLOCKSIZE);
/* Execute the FIR processing function. Input wire1 and output wire2 */
arm_fir_f32(&LPF_instance, wire1, wire2, BLOCKSIZE);
/* Execute the LMS Norm processing function*/
arm_lms_norm_f32(&lmsNorm_instance, /* LMSNorm instance */
wire1, /* Input signal */
wire2, /* Reference Signal */
wire3, /* Converged Signal */
err_signal, /* Error Signal, this will become small as the signal converges */
BLOCKSIZE); /* BlockSize */
/* apply overall gain */
arm_scale_f32(wire3, 5, wire3, BLOCKSIZE); /* in-place buffer */
}
status = ARM_MATH_SUCCESS;
/* -------------------------------------------------------------------------------
* Test whether the error signal has reached towards 0.
* ----------------------------------------------------------------------------- */
arm_abs_f32(err_signal, err_signal, BLOCKSIZE);
arm_min_f32(err_signal, BLOCKSIZE, &minValue, &index);
if (minValue > DELTA_ERROR)
{
status = ARM_MATH_TEST_FAILURE;
}
/* ----------------------------------------------------------------------
* Test whether the filter coefficients have converged.
* ------------------------------------------------------------------- */
arm_sub_f32((float32_t *)FIRCoeff_f32, lmsNormCoeff_f32, lmsNormCoeff_f32, NUMTAPS);
arm_abs_f32(lmsNormCoeff_f32, lmsNormCoeff_f32, NUMTAPS);
arm_min_f32(lmsNormCoeff_f32, NUMTAPS, &minValue, &index);
if (minValue > DELTA_COEFF)
{
status = ARM_MATH_TEST_FAILURE;
}
/* ----------------------------------------------------------------------
* Loop here if the signals did not pass the convergence check.
* This denotes a test failure
* ------------------------------------------------------------------- */
if( status != ARM_MATH_SUCCESS)
{
while(1);
}
}
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);
}
}
int main()
{
//initialize testing array
float testVector[] = {0.1f,0.2f,0.3f,0.4f,0.5f};
/*COMMENTED OUT LENGTH PARAM AS IT IS INCLUDED IN HEADER FILE*/
//get the size of the array
//int length = sizeof(testVector)/sizeof(float);
//initiate empty output array of size length
float outputArrayC[length];
//initialize the struct at p=r=q 0.1 and x=k=0
kalman_state currentState = {0.1f, 0.1f, 0.0f , 0.1f, 0.0f};
//call function Kalmanfilter_C
Kalmanfilter_C(measurements, outputArrayC, ¤tState, length);
//initiate empty output array of size length
float outputArrayASM[length];
//reinitialize the struct at p=r=q 0.1 and x=k=0
currentState.p = DEF_p;
currentState.r = DEF_r;
currentState.k = DEF_k;
currentState.q = DEF_q;
currentState.x = DEF_x;
//call subroutine Kalmanfilter_asm
Kalmanfilter_asm(measurements, outputArrayASM, ¤tState, length );
//Check for correctness with a error tolerance of 0.000001
float errorTolerance = 0.000001f;
float errorPercentage = 0.01;
//is_valid(outputArrayC, outputArrayASM, length, errorTolerance, "c vs asm");
//is_valid_relative(outputArrayC, outputArrayASM, length, errorTolerance, errorPercentage,"c vs asm");
int p;
//print KalmanFilter output
for ( p = 0; p < length; p++ )
{
printf("OutputASM: %f & OutputC %f\n", outputArrayASM[p], outputArrayC[p]);
}
float differenceC[length];
float differenceCMSIS[length];
//Difference
arm_sub_f32 (measurements, outputArrayC, differenceCMSIS, length);
c_sub(measurements, outputArrayC, differenceC, length);
//is_valid(differenceC, differenceCMSIS, length, errorTolerance, "Difference");
//is_valid_relative(differenceC, differenceCMSIS, length, errorTolerance, errorPercentage,"Difference");
//Print difference vector
for ( p = 0; p < length; p++ )
{
printf("DifferenceC: %f & DifferenceCMSIS %f \n", differenceC[p], differenceCMSIS[p]);
}
//Mean
float meanCMSIS;
float meanC;
arm_mean_f32 (differenceCMSIS, length , &meanCMSIS);
c_mean(differenceC,length, &meanC);
//is_valid(&meanC, &meanCMSIS, 1, errorTolerance, "mean");
//is_valid_relative(&meanC, &meanCMSIS, 1, errorTolerance, errorPercentage, "mean");
//Print mean values
printf("MeanC: %f & MeanCMSIS %f \n", meanC, meanCMSIS);
//STD
float stdC;
float stdCMSIS;
arm_std_f32 (differenceCMSIS, length, &stdCMSIS);
c_std(differenceC, length, &stdC);
//is_valid(&stdC, &stdCMSIS, 1, errorTolerance, "STD");
//is_valid_relative(&stdC, &stdCMSIS, 1, errorTolerance, errorPercentage,"STD");
//Print std values
printf("StandardDevC: %f & StandardDevCMSIS %f \n", stdC, stdCMSIS);
//correlation
float corC[2*length-1];
float corCMSIS[2*length-1];
arm_correlate_f32 (measurements, length, outputArrayC, length, corCMSIS);
c_correlate(measurements, outputArrayC, corC, length);
//is_valid(corC, corCMSIS, 2*length-1, errorTolerance, "correlation");
//is_valid_relative(corC, corCMSIS, 2*length-1, errorTolerance, errorPercentage, "correlation");
//convolution
float convC[2*length-1];
float convCMSIS[2*length-1];
arm_conv_f32 (measurements, length, outputArrayC, length, convCMSIS);
c_conv(measurements, outputArrayC, convC, length);
//is_valid(convC, convCMSIS, 2*length-1, errorTolerance, "convolution");
//is_valid_relative(convC, convCMSIS, 2*length-1, errorTolerance, errorPercentage, "convolution");
//Print correlation and convolution values
for ( p = 0; p < (2*length-1); p++ )
{
printf("ConvC: %f & ConvCMSIS: %f \n", convC[p], convCMSIS[p]);
}
for ( p = 0; p < (2*length-1); p++ )
{
printf("CorrelateC: %f & CorrelatCMSIS: %f \n", corC[p], corCMSIS[p]);
}
return 0;
}
