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);
}
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 */
}
/* ----------------------------------------------------------------------
* 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 */
}
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 */
}
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 */
}
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;
}
