Beispiel #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;
}
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 */
}
void getABCRotMatFromEulerAngles(float phi, float theta, float psi, arm_matrix_instance_f32 *rotationMatrix)
{
    float32_t tempRotationVector[9] = {       f_cos(psi)*f_cos(theta),                              f_cos(theta)*f_sin(psi),         -f_sin(theta),
                                              f_cos(psi)*f_sin(phi)*f_sin(theta) - f_cos(phi)*f_sin(psi), f_cos(phi)*f_cos(psi) + f_sin(phi)*f_sin(psi)*f_sin(theta), f_cos(theta)*f_sin(phi),
                                              f_sin(phi)*f_sin(psi) + f_cos(phi)*f_cos(psi)*f_sin(theta), f_cos(phi)*f_sin(psi)*f_sin(theta) - f_cos(psi)*f_sin(phi), f_cos(phi)*f_cos(theta)};

    arm_copy_f32(tempRotationVector, rotationMatrix->pData, 9);
}
Beispiel #4
0
void FloatArray::copyFrom(float* other, int length){
  ASSERT(size >= length, "Array too small");
/// @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_copy_f32(other, data, length);
#else
  memcpy((void *)getData(), (void *)other, length*sizeof(float));
#endif /* ARM_CORTEX */
}
Beispiel #5
0
/* ----------------------------------------------------------------------
* 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 */
}
Beispiel #6
0
void FloatArray::insert(FloatArray source, int sourceOffset, int destinationOffset, int samples){
  ASSERT(size >= destinationOffset+samples, "Array too small");
  ASSERT(source.size >= sourceOffset+samples, "Array too small");
/// @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_copy_f32(source.data+sourceOffset, data+destinationOffset, samples);  
#else
  memcpy((void*)(getData()+destinationOffset), (void*)(source.getData()+sourceOffset), samples*sizeof(float));
#endif /* ARM_CORTEX */
}
Beispiel #7
0
void ComplexFloatArray::copyFrom(ComplexFloat* other, int length){
  ASSERT(size >= length, "Array too small");
#ifdef ARM_CORTEX
  arm_copy_f32((float *)other, (float *)data, length*sizeof(ComplexFloat)/sizeof(float));  //note the *2 multiplier which accounts for real and imaginary parts
#else
  for(int n=0; n<length; n++){
    data[n].re=other[n].re;
    data[n].im=other[n].im;
  }
#endif /* ARM_CORTEX */
}
Beispiel #8
0
void ComplexFloatArray::copyTo(ComplexFloat* other, int length){
  ASSERT(size >= length, "Array too small");
#ifdef ARM_CORTEX
  arm_copy_f32((float *)data, (float *)other, length*sizeof(ComplexFloat)/sizeof(float));
#else
  for(int n=0; n<length; n++){
    other[n].re=data[n].re;
    other[n].im=data[n].im;
  }
#endif /* ARM_CORTEX */
}
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);
	}
}
void kalman_filter(kalman_filter_state *buffer_filtro, float medida_gyro[], float medida_accel[], float medida_mag[], float angles[], uint16_t estado_motores)
{
    GPIO_ResetBits(GPIOD, GPIO_Pin_14);
    //Insf_tancias das matrizes utilizadas para o cálculo
    arm_matrix_instance_f32 X;			//Matriz de estados. [n,1]
    arm_matrix_instance_f32 F;			//Matriz de transição de estados. [n,n]
    arm_matrix_instance_f32 Ft;			//Matriz de transição de estados transposta. [n,n]
    arm_matrix_instance_f32 I;			//Matriz identidadee. [n,n]
    arm_matrix_instance_f32 P;			//Matriz de confiabilidade do processo de atualização. [n,n]
    //arm_matrix_instance_f32 h;		//Matriz de mapeamento de observabilidade [a,n]
    arm_matrix_instance_f32 H;			//Matriz Jacobiana para atualização da confiabilidade do erro [a, n].
    arm_matrix_instance_f32 Ht;			//Matriz Jacobiana transposta para atualização da confiabilidade do erro. [n, a]
    arm_matrix_instance_f32 Q;			//Matriz de covariância multiplicada de processos; [n, n]
    arm_matrix_instance_f32 R;			//Matriz de variância [a ,a]
    arm_matrix_instance_f32 S;			//Matriz .... [a, a]
    arm_matrix_instance_f32 Sinv;		//Matriz S inversa. [a, a]
    arm_matrix_instance_f32 K;			//Matriz com os ganhos de Kalman [n,a]

    //Matrices intermediàrias para cálculo

    arm_matrix_instance_f32 temp_calc_a1_0;
    arm_matrix_instance_f32 temp_calc_a1_1;

    arm_matrix_instance_f32 temp_calc_n1_0;
    arm_matrix_instance_f32 temp_calc_n1_1;
	
    arm_matrix_instance_f32 temp_calc_aa_0;
    arm_matrix_instance_f32 temp_calc_aa_1;
	
    arm_matrix_instance_f32 temp_calc_na_0;
    arm_matrix_instance_f32 temp_calc_an_0;

    arm_matrix_instance_f32 temp_calc_nn_0;
    arm_matrix_instance_f32 temp_calc_nn_1;

	//Matriz S...
    float S_f32[a*a];
    arm_mat_init_f32(&S, a, a, S_f32);

    float Sinv_f32[a*a];
    arm_mat_init_f32(&Sinv, a, a, Sinv_f32);

	//Matriz do ganho de Kalman 
    float K_f32[n*a];
    arm_mat_init_f32(&K, n, a, K_f32);

	//Matrizes de suporte para o cálculo
		//Matrizes de 9 linhas e 1 coluna
    float temp_calc_n1_0_f32[n];
    float temp_calc_n1_1_f32[n];
	
    arm_mat_init_f32(&temp_calc_n1_0, n, 1, temp_calc_n1_0_f32);
    arm_mat_init_f32(&temp_calc_n1_1, n, 1, temp_calc_n1_1_f32);

	//Matrizes de 9 linhas e 1 coluna
    float temp_calc_a1_0_f32[a];
    float temp_calc_a1_1_f32[a];
	
    arm_mat_init_f32(&temp_calc_a1_0, a, 1, temp_calc_a1_0_f32);
    arm_mat_init_f32(&temp_calc_a1_1, a, 1, temp_calc_a1_1_f32);

	//Matrizes de 6 linhas e 6 colunas
    float temp_calc_aa_0_f32[a*a];
    float temp_calc_aa_1_f32[a*a];

    arm_mat_init_f32(&temp_calc_aa_0, a, a, temp_calc_aa_0_f32);
    arm_mat_init_f32(&temp_calc_aa_1, a, a, temp_calc_aa_1_f32);
	
	//Matrizes de 9 linhas e 6 colunas
    float temp_calc_na_0_f32[n*a];

    arm_mat_init_f32(&temp_calc_na_0, n, a, temp_calc_na_0_f32);

	//Matrizes de 6 linhas e 9 colunas
    float temp_calc_an_0_f32[a*n];

    arm_mat_init_f32(&temp_calc_an_0, a, n, temp_calc_an_0_f32);

	//Matrizes de 9 linhas e 9 colunas
    float temp_calc_nn_0_f32[n*n];
    float temp_calc_nn_1_f32[n*n];

    arm_mat_init_f32(&temp_calc_nn_0, n, n, temp_calc_nn_0_f32);
    arm_mat_init_f32(&temp_calc_nn_1, n, n, temp_calc_nn_1_f32);
	
	/*************************************Atualização dos dados para cálcuo*******************************/
	//Variáveis para cálculos
	
	float dt = buffer_filtro->dt;

	/*Velocidades angulares subtraídas dos bias. */
    float p = medida_gyro[0];
    float q = medida_gyro[1];
    float r = medida_gyro[2];

	/*Atualização dos estados dos ângulos com base nas velocidades angulares e do bias estimado anteriormente*/
    float X_f32[n];
    arm_mat_init_f32(&X, n, 1, X_f32);

    arm_copy_f32(buffer_filtro->ultimo_estado, X_f32, n);

    float phi =  X_f32[0];
    float theta = X_f32[1];
    float psi =  X_f32[2];

    float bPhi = X_f32[9];
    float bTheta = X_f32[10];
    float bPsi = X_f32[11];

    //Atualizar o estado anterior - Apenas os ângulos precisam serm inicializados, uma vez que os bias são atualizados por uma identidade.
    X_f32[0] = phi + (p)*dt + f_sin(phi)*f_tan(theta)*(q)*dt + f_cos(phi)*f_tan(theta)*(r)*dt;
    X_f32[1] = theta + f_cos(phi)*(q)*dt - f_sin(phi)*(r)*dt;
    X_f32[2] = psi + f_sin(phi)*f_sec(theta)*(q)*dt + f_cos(phi)*f_sec(theta)*(r)*dt;

    phi = X_f32[0];
    theta = X_f32[1];
    psi = X_f32[2];

    //Com os angulos calculados, cálcula os senos e cossenos utilizados para agilizar os processos de cálculo.
    float cos_Phi = f_cos(phi);
    float sin_Phi = f_sin(phi);

    float cos_Theta = f_cos(theta);
    float sin_Theta = f_sin(theta);
    float tan_Theta = f_tan(theta);

    //Elementos da matriz para atualização dos estados (F).
    float F_f32[n*n] = { dt*q*cos_Phi*tan_Theta - dt*r*sin_Phi*tan_Theta + 1,               dt*r*cos_Phi*(pow(tan_Theta,2) + 1) + dt*q*sin_Phi*(pow(tan_Theta,2) + 1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                                 - dt*r*cos_Phi - dt*q*sin_Phi,                                                                                 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                         (dt*q*cos_Phi)/cos_Theta - (dt*r*sin_Phi)/cos_Theta, (dt*r*cos_Phi*sin_Theta)/pow(cos_Theta,2) + (dt*q*sin_Phi*sin_Theta)/pow(cos_Theta,2), 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
                                                                               0,                                                                                 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1};

    arm_mat_init_f32(&F, n, n, F_f32);

	//Matriz Jacobiana transposta para atualização de P.
    float Ft_f32[n*n];
    arm_mat_init_f32(&Ft, n, n, Ft_f32);
    arm_mat_trans_f32(&F, &Ft);

	//Processo à priori para atualização da matriz de confiabilidade P.

	//Matriz de covariâncias do processo de atualização (Q).
    float qAngles = (buffer_filtro->Q_angles);
    float qBiasAcel = (buffer_filtro->Q_bias_acel);
    float qBiasMag = (buffer_filtro->Q_bias_mag);
    float qBiasAngles = (buffer_filtro->Q_bias_angle);

    /*Matriz de confiabilidade do processo de atualização. */
    /* Pk|k-1 = Pk-1|k-1 */
    float P_f32[n*n];
    arm_copy_f32(buffer_filtro->P, P_f32, n*n);
    arm_mat_init_f32(&P, n, n, P_f32);

    //temp_calc_nn_0 = F*P
    if(arm_mat_mult_f32(&F, &P, &temp_calc_nn_0) != ARM_MATH_SUCCESS)
        while(1);

    //temp_calc_nn_1 = F*P*F'
    if(arm_mat_mult_f32(&temp_calc_nn_0, &Ft, &temp_calc_nn_1) != ARM_MATH_SUCCESS)
        while(1);


    //Atualiza a matriz de covariâncias dos processos
    float Q_f32[n*n] = {	qAngles,   0,           0,          0,          0,          0,          0,          0,          0,          0,          0,              0,
                            0,          qAngles,    0,          0,          0,          0,          0,          0,          0,          0,          0,              0,
                            0,          0,          qAngles,	0,          0,          0,          0,          0,          0,          0,          0,              0,
                            0,          0,          0,          qBiasAcel,  0,          0,          0,          0,          0,          0,          0,              0,
                            0,          0,          0,          0,          qBiasAcel,  0,          0,          0,          0,          0,          0,              0,
                            0,          0,      	0,          0,          0,          qBiasAcel,  0,          0,          0,          0,          0,              0,
                            0,          0,          0,          0,          0,          0,          qBiasMag,   0,          0,          0,          0,              0,
                            0,          0,          0,          0,          0,          0,          0,          qBiasMag,   0,          0,          0,              0,
                            0,          0,          0,          0,          0,          0,          0,          0,          qBiasMag,   0,          0,              0,
                            0,          0,          0,          0,          0,          0,          0,          0,          0,          qBiasAngles, 0,             0,
                            0,          0,          0,          0,          0,          0,          0,          0,          0,          0,          qBiasAngles,    0,
                            0,          0,          0,          0,          0,          0,          0,          0,          0,          0,          0,              qBiasAngles};

    arm_mat_init_f32(&Q, n, n, Q_f32);

    //P = temp_calc_nn_1 + Q = F*P*F' + Q
    if(arm_mat_add_f32(&temp_calc_nn_1, &Q, &P) != ARM_MATH_SUCCESS)
        while(1);

	/*Estados iniciais do magnetômetro */
    float magRefVector[3];
    float acelRefVector[3];
    arm_matrix_instance_f32 magRefMatrix;
    arm_matrix_instance_f32 acelRefMatrix;

    arm_mat_init_f32(&magRefMatrix, 3, 1, magRefVector);
    arm_mat_init_f32(&acelRefMatrix, 3, 1, acelRefVector);

    float ax = buffer_filtro->AcelInicial[0];
    float ay = buffer_filtro->AcelInicial[1];
    float az = buffer_filtro->AcelInicial[2];

    float hx = buffer_filtro->MagInicial[0];
    float hy = buffer_filtro->MagInicial[1];
    float hz = buffer_filtro->MagInicial[2];

    magRefVector[0] = hx;
    magRefVector[1] = hy;
    magRefVector[2] = hz;

    acelRefVector[0] = ax;
    acelRefVector[1] = ay;
    acelRefVector[2] = az;

    //Matrizes com o resultado das operações de rotação
    float observatedStateVector[a];

    arm_matrix_instance_f32 observatedStateMatrix;
    arm_mat_init_f32(&observatedStateMatrix, a, 1, observatedStateVector);

    //Matriz contendo os valores observados utilizados pra gerar o rezíduo (yk)
    arm_matrix_instance_f32 rotatedMagMatrix;
    arm_matrix_instance_f32 rotatedAcelMatrix;

    arm_mat_init_f32(&rotatedAcelMatrix, 3, 1, observatedStateVector);
    arm_mat_init_f32(&rotatedMagMatrix, 3, 1, observatedStateVector+3);

    //Matriz de rotação com base nos angulos estimados.
    float rotationVector[9];
    arm_matrix_instance_f32 rotationMatrix;
    arm_mat_init_f32(&rotationMatrix, 3, 3, rotationVector);
    getABCRotMatFromEulerAngles(phi-bPhi, theta-bTheta, psi-bPsi, &rotationMatrix);

    /* Cálculo das referências com base no magnetômetro e no estado do acelerômetro parado [0; 0; 1]; */
    if(arm_mat_mult_f32(&rotationMatrix, &acelRefMatrix, &rotatedAcelMatrix) != ARM_MATH_SUCCESS)
        while(1);

    if(arm_mat_mult_f32(&rotationMatrix, &magRefMatrix, &rotatedMagMatrix) != ARM_MATH_SUCCESS)
        while(1);

    //Vetor com as médidas
    float zkVector[a];

    zkVector[0] = medida_accel[0];
    zkVector[1] = medida_accel[1];
    zkVector[2] = medida_accel[2];

    zkVector[3] = medida_mag[0];
    zkVector[4] = medida_mag[1];
    zkVector[5] = medida_mag[2];

    zkVector[6] = angles[0];
    zkVector[7] = angles[1];
    zkVector[8] = angles[2];

    arm_matrix_instance_f32 zkMatrix;
    arm_mat_init_f32(&zkMatrix, a, 1, zkVector);

    //Vetor de resíduo
    float ykVector[a];
    arm_matrix_instance_f32 ykMatrix;
    arm_mat_init_f32(&ykMatrix, a, 1, ykVector);

    //Adiciona os bias estimados pelo filtro
    observatedStateVector[0] += X_f32[3];
    observatedStateVector[1] += X_f32[4];
    observatedStateVector[2] += X_f32[5];
    observatedStateVector[3] += X_f32[6];
    observatedStateVector[4] += X_f32[7];
    observatedStateVector[5] += X_f32[8];

    //Remove o bias estimado pelo filtro
    float correctedPhi =    -(X_f32[0] - X_f32[9]);
    float correctedTheta =  -(X_f32[1] - X_f32[10]);
    float correctedPsi =    -(X_f32[2] - X_f32[11]);

    //Com os angulos corrigidos calculados, cálcula os senos e cossenos utilizados para agilizar os processos de cálculo.
    float cos_correctedPhi = f_cos(correctedPhi);
    float sin_correctedPhi = f_sin(correctedPhi);

    float cos_correctedTheta = f_cos(correctedTheta);
    float sin_correctedTheta = f_sin(correctedTheta);

    float cos_correctedPsi = f_cos(correctedPsi);
    float sin_correctedPsi = f_sin(correctedPsi);
    //
    observatedStateVector[6] = -correctedPhi;
    observatedStateVector[7] = -correctedTheta;
    observatedStateVector[8] = -correctedPsi;

    if(arm_mat_sub_f32(&zkMatrix, &observatedStateMatrix, &ykMatrix) != ARM_MATH_SUCCESS)
        while(1);

    float H_f32[a*n] = {                                                                                                                                                                                                                                0,                                                 ax*sin_correctedTheta*cos_correctedPsi - az*cos_correctedTheta - ay*sin_correctedPsi*sin_correctedTheta,                                                                                                         ay*cos_correctedPsi*cos_correctedTheta + ax*sin_correctedPsi*cos_correctedTheta, 1, 0, 0, 0, 0, 0,                                                                                                                                                                                                                                  0,                                                 az*cos_correctedTheta - ax*sin_correctedTheta*cos_correctedPsi + ay*sin_correctedPsi*sin_correctedTheta,                                                                                                       - ay*cos_correctedPsi*cos_correctedTheta - ax*sin_correctedPsi*cos_correctedTheta,
                         ax*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) + ay*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) + az*cos_correctedPhi*cos_correctedTheta, ay*sin_correctedPhi*sin_correctedPsi*cos_correctedTheta - ax*sin_correctedPhi*cos_correctedPsi*cos_correctedTheta - az*sin_correctedPhi*sin_correctedTheta, ay*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi) - ax*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta), 0, 1, 0, 0, 0, 0, - ax*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) - ay*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) - az*cos_correctedPhi*cos_correctedTheta, az*sin_correctedPhi*sin_correctedTheta + ax*sin_correctedPhi*cos_correctedPsi*cos_correctedTheta - ay*sin_correctedPhi*sin_correctedPsi*cos_correctedTheta, ax*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - ay*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi),
                         az*sin_correctedPhi*cos_correctedTheta - ay*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - ax*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi), az*sin_correctedTheta*cos_correctedPhi + ax*cos_correctedPhi*cos_correctedPsi*cos_correctedTheta - ay*sin_correctedPsi*cos_correctedPhi*cos_correctedTheta, ay*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) - ax*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi), 0, 0, 1, 0, 0, 0,   ax*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi) + ay*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - az*sin_correctedPhi*cos_correctedTheta, ay*sin_correctedPsi*cos_correctedPhi*cos_correctedTheta - ax*cos_correctedPhi*cos_correctedPsi*cos_correctedTheta - az*sin_correctedTheta*cos_correctedPhi, ax*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) - ay*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi),
                                                                                                                                                                                                                                                        0,                                                 hx*sin_correctedTheta*cos_correctedPsi - hz*cos_correctedTheta - hy*sin_correctedPsi*sin_correctedTheta,                                                                                                         hy*cos_correctedPsi*cos_correctedTheta + hx*sin_correctedPsi*cos_correctedTheta, 0, 0, 0, 1, 0, 0,                                                                                                                                                                                                                                  0,                                                 hz*cos_correctedTheta - hx*sin_correctedTheta*cos_correctedPsi + hy*sin_correctedPsi*sin_correctedTheta,                                                                                                       - hy*cos_correctedPsi*cos_correctedTheta - hx*sin_correctedPsi*cos_correctedTheta,
                         hx*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) + hy*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) + hz*cos_correctedPhi*cos_correctedTheta, hy*sin_correctedPhi*sin_correctedPsi*cos_correctedTheta - hx*sin_correctedPhi*cos_correctedPsi*cos_correctedTheta - hz*sin_correctedPhi*sin_correctedTheta, hy*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi) - hx*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta), 0, 0, 0, 0, 1, 0, - hx*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) - hy*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) - hz*cos_correctedPhi*cos_correctedTheta, hz*sin_correctedPhi*sin_correctedTheta + hx*sin_correctedPhi*cos_correctedPsi*cos_correctedTheta - hy*sin_correctedPhi*sin_correctedPsi*cos_correctedTheta, hx*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - hy*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi),
                         hz*sin_correctedPhi*cos_correctedTheta - hy*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - hx*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi), hz*sin_correctedTheta*cos_correctedPhi + hx*cos_correctedPhi*cos_correctedPsi*cos_correctedTheta - hy*sin_correctedPsi*cos_correctedPhi*cos_correctedTheta, hy*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) - hx*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi), 0, 0, 0, 0, 0, 1,   hx*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi) + hy*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - hz*sin_correctedPhi*cos_correctedTheta, hy*sin_correctedPsi*cos_correctedPhi*cos_correctedTheta - hx*cos_correctedPhi*cos_correctedPsi*cos_correctedTheta - hz*sin_correctedTheta*cos_correctedPhi, hx*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) - hy*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi),
                                                                                                                                                                                                                                                        1,                                                                                                                                                        0,                                                                                                                                                                                       0, 0, 0, 0, 0, 0, 0,                                                                                                                                                                                                                                 -1,                                                                                                                                                        0,                                                                                                                                                                                       0,
                                                                                                                                                                                                                                                        0,                                                                                                                                                        1,                                                                                                                                                                                       0, 0, 0, 0, 0, 0, 0,                                                                                                                                                                                                                                  0,                                                                                                                                                       -1,                                                                                                                                                                                       0,
                                                                                                                                                                                                                                                        0,                                                                                                                                                        0,                                                                                                                                                                                       1, 0, 0, 0, 0, 0, 0,                                                                                                                                                                                                                                  0,                                                                                                                                                        0,                                                                                                                                                                                      -1};

    arm_mat_init_f32(&H, a, n, H_f32);

	/* Matriz Jacobiana transposta para cálculo da confiabilidade do erro . */
    float Ht_f32[n*a];
    arm_mat_init_f32(&Ht, n, a, Ht_f32);

    arm_mat_trans_f32(&H, &Ht);

	//Matriz de variâncias
	float Racel = buffer_filtro->R_acel;
	float Rmag = buffer_filtro->R_mag;	 //Variância inicial do magnetômetro.
    float Rangles = buffer_filtro->R_angles;

    float acelModulus = getVectorModulus(medida_accel, 3);
//    float magModulus = getVectorModulus(medida_mag, 3);
//    float magInitialModulus = getVectorModulus(buffer_filtro->MagInicial, 3);

    //Racel += 100*fabsf(1 - acelModulus);
    //Rmag += 1*fabs(magInitialModulus - magModulus);


    float R_f32[a*a] = {(Racel), 0, 0, 0, 0, 0, 0, 0, 0,
                        0, (Racel), 0, 0, 0, 0, 0, 0, 0,
                        0, 0, (Racel), 0, 0, 0, 0, 0, 0,
                        0, 0, 0, (Rmag),  0, 0, 0, 0, 0,
                        0, 0, 0, 0, (Rmag),	 0, 0, 0, 0,
                        0, 0, 0, 0, 0, (Rmag), 0, 0, 0,
                        0, 0, 0, 0, 0, 0, (Rangles), 0, 0,
                        0, 0, 0, 0, 0, 0, 0, (Rangles), 0,
                        0, 0, 0, 0, 0, 0, 0, 0, (Rangles)};

    arm_mat_init_f32(&R, a, a, R_f32);

	//Cálculos do filtro de Kalman

	//S = H*P*H' + R
    if(arm_mat_mult_f32(&H, &P, &temp_calc_an_0) != ARM_MATH_SUCCESS)
        while(1);

    if(arm_mat_mult_f32(&temp_calc_an_0, &Ht, &temp_calc_aa_0) != ARM_MATH_SUCCESS)
        while(1);

    if(arm_mat_add_f32(&temp_calc_aa_0, &R, &S) != ARM_MATH_SUCCESS)
        while(1);

    //Sinv = inv(S);
    //if(arm_mat_inverse_f32(&S, &Sinv) == ARM_MATH_SINGULAR)
    //    while(1);
    arm_mat_inverse_f32(&S, &Sinv);

    //Kk = P*Ht*S^(-1)
		//P*Ht
    if(arm_mat_mult_f32(&P, &Ht, &temp_calc_na_0) != ARM_MATH_SUCCESS)
        while(1);

    if(arm_mat_mult_f32(&temp_calc_na_0, &Sinv, &K) != ARM_MATH_SUCCESS)
        while(1);
	
    //temp_calc_n11 = Kk*y
    if(arm_mat_mult_f32(&K, &ykMatrix, &temp_calc_n1_0) != ARM_MATH_SUCCESS)
        while(1);

    //X = X + temp_calc_n1_1;
    if(arm_mat_add_f32(&X, &temp_calc_n1_0, &temp_calc_n1_1) != ARM_MATH_SUCCESS)
        while(1);

    arm_copy_f32(temp_calc_n1_1_f32, X_f32, n);

	//P = (I-K*H)*P
	
	//Matriz identidade para atualização da matriz P à posteriori.
    float I_f32[n*n] = {	1,0,0,0,0,0,0,0,0,0,0,0,
                            0,1,0,0,0,0,0,0,0,0,0,0,
                            0,0,1,0,0,0,0,0,0,0,0,0,
                            0,0,0,1,0,0,0,0,0,0,0,0,
                            0,0,0,0,1,0,0,0,0,0,0,0,
                            0,0,0,0,0,1,0,0,0,0,0,0,
                            0,0,0,0,0,0,1,0,0,0,0,0,
                            0,0,0,0,0,0,0,1,0,0,0,0,
                            0,0,0,0,0,0,0,0,1,0,0,0,
                            0,0,0,0,0,0,0,0,0,1,0,0,
                            0,0,0,0,0,0,0,0,0,0,1,0,
                            0,0,0,0,0,0,0,0,0,0,0,1};

    arm_mat_init_f32(&I, n, n, I_f32);


    if(arm_mat_mult_f32(&K, &H, &temp_calc_nn_0) != ARM_MATH_SUCCESS)
        while(1);

    if(arm_mat_sub_f32(&I, &temp_calc_nn_0, &temp_calc_nn_1) != ARM_MATH_SUCCESS)
        while(1);

    if(arm_mat_mult_f32(&temp_calc_nn_1, &P, &temp_calc_nn_0) != ARM_MATH_SUCCESS)
        while(1);

    arm_copy_f32(X_f32, buffer_filtro->ultimo_estado, n);
    arm_copy_f32(temp_calc_nn_0_f32, buffer_filtro->P, n*n);
}
uint8_t * APulseController::get_response ( uint16_t& size ) {
	static union {
		uint8_t data[64];
		status_pkt_t status;
		uint32_t data32[16];
	} p;

	switch(state){
	case ST_GETPSD:
		if(cmd_idx == 32){
			// Just one more value!
			((InputDSP::powerFractional *)&p)[0] = InputDSP::get_psd()[InputDSP::transform_len / 2];
			state = ST_GETAVG;
			cmd_idx = 0;
			size = 4;
			return p.data;
		}
		arm_copy_f32((float32_t*)&InputDSP::get_psd()[cmd_idx++ * 16], (float32_t*)p.data, 16);
		size = 64;
		return p.data;
	case ST_GETAVG:
		arm_copy_f32((float32_t*)&InputDSP::get_average()[cmd_idx++ * 16], (float32_t*)p.data, 16);
		if(cmd_idx == 64){
			state = ST_RESET;
			cmd_idx = 0;
		}
		size = 64;
		return p.data;
	case ST_DUMPWAVE:
		if(do_buffer_dumps and waveform_dump.has_data()){
			waveform_dump.get_frame_copy(p.data, 64);
		} else {
			size = 0;
			state = ST_RESET;
			return p.data;
		}
		if(!waveform_dump.has_data()){
			state = ST_RESET;
		}
		size = 64;
		return p.data;
	case ST_RESET:
	case ST_RESETTING:
	default:
		// Send the status packet
		p.status.version = protocol_version;
		p.status.input_state = InputDSP::get_state();
		p.status.wavegen_state = WaveGen::get_state();
		p.status.controller_state = teststate;

		p.status.err_code = err_code;

#if CFG_TARGET_K20
		p.status.psd_frac_bits = InputDSP::powerFractional::bits_f;
#elif CFG_TARGET_K22
		p.status.psd_frac_bits = ~0;
#endif

		size = sizeof(status_pkt_t);
		return p.data;
	}
	return nullptr;
}