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