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);
}
f_besj0() /* j0(a) = sin(a)/a */
{
struct value a;
a = top_of_stack;
f_sin();
push(&a);
f_div();
}
f_besj1() /* j1(a) = sin(a)/(a**2) - cos(a)/a */
{
struct value a;
a = top_of_stack;
f_sin();
push(&a);
push(&a);
f_mult();
f_div();
push(&a);
f_cos();
push(&a);
f_div();
f_minus();
}
f_besy1() /* y1(a) = -cos(a)/(a**2) - sin(a)/a */
{
struct value a;
a = top_of_stack;
f_cos();
push(&a);
push(&a);
f_mult();
f_div();
push(&a);
f_sin();
push(&a);
f_div();
f_plus();
f_uminus();
}
float delayline::envelope()
{
float fdist = ((float) distance) / time[tap];
if (fdist > 0.5f) {
if (fdist <= 1.0f)
fdist = 1.0f - fdist;
else
fdist = 0.0f;
}
if (fdist <= 0.125f) {
fdist =
1.0f - f_sin(M_PI * fdist * 4.0f + 1.5707963267949f);
} else
fdist = 1.0f;
return fdist;
};
int main(int argc, char**argv){
fun_t *f;
f = f_mult(f_window(f_cu(0,SEC),f_cu(3,SEC)),
f_sin(f_c(1),f_cu(50,HZ),f_c(0)) );
f = f_add(f,
f_mult(f_window(f_cu(3,SEC),f_cu(6,SEC)),
f_tri(f_c(1),f_cu(50,HZ),f_c(0)) ));
f = f_add(f,
f_mult(f_window(f_cu(6,SEC),f_cu(9,SEC)),
f_square(f_c(1),f_cu(50,HZ),f_c(0),f_c(0.5)) ));
set_bpm(f_ramp(f_cu(2,SEC),f_cu(8,SEC),f_c(60),f_c(70)));
set_period(f_c(4));
/*
fun_print(f);
*/
f = s_down_sample(f_c(10),f);
fun_record_16b(f,0,SAMPLING_RATE*10,2,buffer);
audio_write_stereo_16b(buffer,SAMPLING_RATE*2*2*10,"test.wav");
return 0;
}
bool TestExtMath::test_sin() {
VC(f_sin(f_deg2rad(90)), 1);
return Count(true);
}
void
Waveshaper::waveshapesmps (int n, float * smps, int type,
int drive, int eff)
{
int nn=n;
temps = (float *) malloc (sizeof (float) * param->PERIOD * period_coeff);
if(Wave_res_amount >= 0) {
nn=n*period_coeff;
U_Resample->mono_out(smps,temps,n,u_up,nn);
}
else memcpy(temps,smps,sizeof(float)*n);
//temps = smps;
int i;
float ws = (float)drive / 127.0f + .00001f;
ws = 1.0f - expf (-ws * 4.0f);
float tmpv;
float factor;
switch (type + 1 ) {
case 1:
ws = powf (10.0f, ws * ws * 3.0f) - 1.0f + 0.001f; //Arctangent
for (i = 0; i < nn; i++)
temps[i] = atanf (temps[i] * ws) / atanf (ws);
break;
case 2:
ws = ws * ws * 32.0f + 0.0001f; //Asymmetric
if (ws < 1.0)
tmpv = f_sin (ws) + 0.1f;
else
tmpv = 1.1f;
for (i = 0; i < nn; i++) {
temps[i] = f_sin (temps[i] * (0.1f + ws - ws * temps[i])) / tmpv;
};
break;
case 3:
ws = ws * ws * ws * 20.0f + 0.0001f; //Pow
for (i = 0; i < nn; i++) {
temps[i] *= ws;
if (fabsf (temps[i]) < 1.0) {
temps[i] = (temps[i] - powf (temps[i], 3.0f)) * 3.0f;
if (ws < 1.0)
temps[i] /= ws;
} else
temps[i] = 0.0;
};
break;
case 4:
ws = ws * ws * ws * 32.0f + 0.0001f; //Sine
if (ws < 1.57f)
tmpv = f_sin (ws);
else
tmpv = 1.0f;
for (i = 0; i < nn; i++)
temps[i] = f_sin (temps[i] * ws) / tmpv;
break;
case 5:
ws = ws * ws + 0.000001f; //Quantisize
for (i = 0; i < nn; i++)
temps[i] = floorf (temps[i] / ws + 0.15f) * ws;
break;
case 6:
ws = ws * ws * ws * 32.0f + 0.0001f; //Zigzag
if (ws < 1.0)
tmpv = f_sin (ws);
else
tmpv = 1.0f;
for (i = 0; i < nn; i++)
temps[i] = asinf (f_sin (temps[i] * ws)) / tmpv;
break;
case 7:
ws = powf (2.0f, -ws * ws * 8.0f); //Limiter
for (i = 0; i < nn; i++) {
float tmp = temps[i];
if (fabsf (tmp) > ws) {
if (tmp >= 0.0)
temps[i] = 1.0f;
else
temps[i] = -1.0f;
} else
temps[i] /= ws;
};
break;
case 8:
ws = powf (2.0f, -ws * ws * 8.0f); //Upper Limiter
for (i = 0; i < nn; i++) {
float tmp = temps[i];
if (tmp > ws)
temps[i] = ws;
temps[i] *= 2.0f;
};
break;
case 9:
ws = powf (2.0f, -ws * ws * 8.0f); //Lower Limiter
for (i = 0; i < nn; i++) {
float tmp = temps[i];
if (tmp < -ws)
temps[i] = -ws;
temps[i] *= 2.0f;
};
break;
case 10:
ws = (powf (2.0f, ws * 6.0f) - 1.0f) / powf (2.0f, 6.0f); //Inverse Limiter
for (i = 0; i < nn; i++) {
float tmp = temps[i];
if (fabsf (tmp) > ws) {
if (tmp >= 0.0)
temps[i] = tmp - ws;
else
temps[i] = tmp + ws;
} else
temps[i] = 0;
};
break;
case 11:
ws = powf (5.0f, ws * ws * 1.0f) - 1.0f; //Clip
for (i = 0; i < nn; i++)
temps[i] =
temps[i] * (ws + 0.5f) * 0.9999f - floorf (0.5f +
temps[i] * (ws +
0.5f) * 0.9999f);
break;
case 12:
ws = ws * ws * ws * 30.0f + 0.001f; //Asym2
if (ws < 0.3)
tmpv = ws;
else
tmpv = 1.0f;
for (i = 0; i < nn; i++) {
float tmp = temps[i] * ws;
if ((tmp > -2.0) && (tmp < 1.0))
temps[i] = tmp * (1.0f - tmp) * (tmp + 2.0f) / tmpv;
else
temps[i] = 0.0f;
};
break;
case 13:
ws = ws * ws * ws * 32.0f + 0.0001f; //Pow2
if (ws < 1.0)
tmpv = ws * (1.0f + ws) / 2.0f;
else
tmpv = 1.0f;
for (i = 0; i < nn; i++) {
float tmp = temps[i] * ws;
if ((tmp > -1.0f) && (tmp < 1.618034f))
temps[i] = tmp * (1.0f - tmp) / tmpv;
else if (tmp > 0.0)
temps[i] = -1.0f;
else
temps[i] = -2.0f;
};
break;
case 14:
ws = powf (ws, 5.0f) * 80.0f + 0.0001f; //sigmoid
if (ws > 10.0)
tmpv = 0.5f;
else
tmpv = 0.5f - 1.0f / (expf (ws) + 1.0f);
for (i = 0; i < nn; i++) {
float tmp = temps[i] * ws;
if (tmp < -10.0)
tmp = -10.0f;
else if (tmp > 10.0)
tmp = 10.0f;
tmp = 0.5f - 1.0f / (expf (tmp) + 1.0f);
temps[i] = tmp / tmpv;
};
break;
case 15: //Sqrt "Crunch" -- Asymmetric square root distortion.
ws = ws*ws*CRUNCH_GAIN + 1.0f;
for (i = 0; i < nn; i++) {
float tmp = temps[i] * ws;
if (tmp < Tlo) {
temps[i] = Tlc - sqrtf(-tmp*DIV_TLC_CONST);
} else if (tmp > Thi) {
temps[i] = Thc + sqrtf(tmp*DIV_THC_CONST);
} else {
temps[i] = tmp;
};
if (temps[i] < -1.0)
temps[i] = -1.0f;
else if (temps[i] > 1.0)
temps[i] = 1.0f;
};
break;
case 16: //Sqrt "Crunch2" -- Asymmetric square root distortion.
ws = ws*ws*CRUNCH_GAIN + 1.0f;
for (i = 0; i < nn; i++) {
float tmp = temps[i] * ws;
if (tmp < Tlo) {
temps[i] = Tlc;
} else if (tmp > Thi) {
temps[i] = Thc + sqrtf(tmp*DIV_THC_CONST);
} else {
temps[i] = tmp;
};
if (temps[i] < -1.0)
temps[i] = -1.0f;
else if (temps[i] > 1.0)
temps[i] = 1.0f;
};
break;
case 17: //Octave Up
ws = ws*ws*30.0f + 1.0f;
for (i = 0; i < nn; i++) {
float tmp = fabs(temps[i])* ws;
if (tmp > 1.0f) {
tmp = 1.0f;
}
temps[i] = tmp; //a little bit of DC correction
};
break;
case 18:
ws = ws*D_PI+.00001;
if (ws < 1.57f)
tmpv = f_sin (ws);
else
tmpv = 1.0f;
for (i = 0; i < nn; i++)
temps[i]=f_sin(ws * temps[i] + f_sin( ws * 2.0f*temps[i]))/ tmpv;
break;
case 19:
ws = ws * D_PI + 0.0001f;
if (ws < 1.57f)
tmpv = f_sin (ws);
else
tmpv = 1.0f;
for (i = 0; i < nn; i++)
temps[i]=f_sin(ws * temps[i] + f_sin(ws * temps[i])/tmpv);
break;
case 20: //Compression
cratio = 1.0f - 0.25f * ws;
ws = 1.5f*ws*CRUNCH_GAIN + 4.0f;
for (i = 0; i < nn; i++) { //apply compression
tmpv = fabs(ws * temps[i]);
dyno += 0.01f * (1.0f - dynodecay) * tmpv;
dyno *= dynodecay;
tmpv += dyno;
if(tmpv > dthresh) { //if envelope of signal exceeds thresh, then compress
compg = dthresh + dthresh*(tmpv - dthresh)/tmpv;
dthresh = 0.25f + cratio*(compg - dthresh); //dthresh changes dynamically
if (temps[i] > 0.0f) {
temps[i] = compg;
} else {
temps[i] = -1.0f * compg;
}
} else {
temps[i] *= ws;
}
if(tmpv < dthresh) dthresh = tmpv;
if(dthresh < 0.25f) dthresh = 0.25f;
};
break;
case 21: //Overdrive
ws = powf (10.0f, ws * ws * 3.0f) - 1.0f + 0.001f;
for (i = 0; i < nn; i++) {
if(temps[i]>0.0f) temps[i] = sqrtf(temps[i]*ws);
else temps[i] = -sqrtf(-temps[i]*ws);
}
break;
case 22: //Soft
ws = powf(4.0f, ws*ws+1.0f);
for (i = 0; i < nn; i++) {
if(temps[i]>0.0f) temps[i] = ws*powf(temps[i],1.4142136f);
else temps[i] = ws* -powf(-temps[i],1.4142136f);
}
break;
case 23: //Super Soft
ws = powf (20.0f, ws * ws) + 0.5f;
factor = 1.0f / ws;
for (i = 0; i < nn; i++) {
if(temps[i] > 1.0) temps[i] = 1.0f;
if(temps[i] < -1.0) temps[i] = -1.0f;
if(temps[i]<factor) temps[i]=temps[i];
else if(temps[i]>factor) temps[i]=factor+(temps[i]-factor)/powf(1.0f+((temps[i]-factor)/(1.0f-temps[i])),2.0f);
else if(temps[i]>1.0f) temps[i]=(factor+1.0f)*.5f;
temps[i]*=ws;
}
break;
case 24: // Hard Compression (used by stompboxes)
cratio = 0.05;
if (eff) {
ws = 1.5f*ws*CRUNCH_GAIN + 1.0f;
} else {
ws = 1.0f;
} //allows functions applying gain before waveshaper
for (i = 0; i < nn; i++) { //apply compression
tmpv = fabs(ws * temps[i]);
if(tmpv > dthresh) { //if envelope of signal exceeds thresh, then compress
compg = dthresh + dthresh*(tmpv - dthresh)/tmpv;
dthresh = 0.5f + cratio*(compg - dthresh); //dthresh changes dynamically
if (temps[i] > 0.0f) {
temps[i] = compg;
} else {
temps[i] = -1.0f * compg;
}
} else {
temps[i] *= ws;
}
if(tmpv < dthresh) dthresh = tmpv;
if(dthresh < 0.5f) dthresh = 0.5f;
};
break;
case 25: // Op Amp limiting (used by stompboxes), needs to get a large signal to do something
cratio = 0.05;
for (i = 0; i < nn; i++) { //apply compression
tmpv = fabs(temps[i]);
if(tmpv > dthresh) { //if envelope of signal exceeds thresh, then compress
compg = dthresh + dthresh*(tmpv - dthresh)/tmpv;
dthresh = 3.5f + cratio*(compg - dthresh); //dthresh changes dynamically
if (temps[i] > 0.0f) {
temps[i] = compg;
} else {
temps[i] = -1.0f * compg;
}
} else {
temps[i] *= 1.0f;
}
if(tmpv < dthresh) dthresh = tmpv;
if(dthresh < 3.5f) dthresh = 3.5f;
};
break;
case 26: //JFET
ws = powf (35.0f, ws * ws) + 4.0f;
factor = sqrt(1.0f / ws);
for (i = 0; i < nn; i++) {
temps[i] = temps[i] + factor;
if(temps[i] < 0.0) temps[i] = 0.0f;
temps[i] = 1.0f - 2.0f/(ws*temps[i]*temps[i] + 1.0f);
}
break;
case 27: //dyno JFET
ws = powf (85.0f, ws * ws) + 10.0f;
for (i = 0; i < nn; i++) {
tmpv = fabs(temps[i]);
if(tmpv > 0.15f) { // -16dB crossover distortion... dyno only picks up the peaks above 16dB. Good for nasty fuzz
dyno += (1.0f - dynodecay) * tmpv;
}
dyno *= dynodecay; //always decays
temps[i] = temps[i] + sqrtf((1.0f + 0.05f*dyno) / ws);
if(temps[i] < 0.0) temps[i] = 0.0f;
temps[i] = 1.0f - 2.0f/(ws*temps[i]*temps[i] + 1.0f);
}
break;
case 28: //Valve 1
ws = powf (4.0f, ws * ws) - 0.98f;
for (i = 0; i < nn; i++) {
Vg = Vgbias + ws*temps[i] - 0.1f*Vdyno;
if(Vg<=0.05f) Vg = 0.05f/((-20.0f*Vg) + 2.0f);
Ip = P*powf(Vg,Vfactor);
tmpv = Vsupp - (Vmin - (Vmin/(R*Ip + 1.0f))); //Here is the plate voltage
tmpv = (tmpv - 106.243f)/100.0f;
Vdyno += (1.0f - dynodecay) * tmpv;
Vdyno *= dynodecay;
temps[i] = tmpv;
}
break;
case 29: //Valve 2
ws = powf (110.0f, ws);
for (i = 0; i < nn; i++) {
Vg2 = mu*(V2bias + V2dyno + ws*temps[i]);
if(Vg2 <= vfact) Vg2 = vfact/((-Vg2/vfact) + 2.0f); //Toward cut-off, behavior is a little different than 2/3 power law
Vlv2out = Vsupp - R*Is*powf(Vg2,1.5f); //2/3 power law relationship
if(Vlv2out <= ffact) Vlv2out = ffact/((-Vlv2out/ffact) + 2.0f); //Then as Vplate decreases, gain decreases until saturation
temps[i] = (Vlv2out - 95.0f)*0.01f;
V2dyno += (1.0f - dynodecay)*temps[i];
V2dyno *= dynodecay; //always decays
}
break;
case 30: //Diode clipper
ws = 5.0f + powf (110.0f, ws);
for (i = 0; i < nn; i++) {
tmpv = ws*temps[i];
if (tmpv>0.0f) tmpv = 1.0f - 1.0f/powf(4.0f, tmpv);
else tmpv = -(1.0f - 1.0f/powf(4.0f, -tmpv));
temps[i] = tmpv;
}
break;
};
if(Wave_res_amount >= 0) {
D_Resample->mono_out(temps,smps,nn,u_down,n);
} else
memcpy(smps,temps,sizeof(float)*n);
free(temps);
};
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);
}
/*
* Compute the shape of the LFO
*/
float EffectLFO::getlfoshape (float x)
{
float tmpv;
float out=0.0;
int iterations = 1; //make fractal go faster
switch (lfotype) {
case 1: //EffectLFO_TRIANGLE
if ((x > 0.0) && (x < 0.25))
out = 4.0f * x;
else if ((x > 0.25) && (x < 0.75))
out = 2.0f - 4.0f * x;
else
out = 4.0f * x - 4.0f;
break;
case 2: //EffectLFO_RAMP Ramp+
out = 2.0f * x - 1.0f;
break;
case 3: //EffectLFO_RAMP Ramp-
out = - 2.0f * x + 1.0f;
break;
case 4: //ZigZag
x = x * 2.0f - 1.0f;
tmpv = 0.33f * f_sin(x);
out = f_sin(f_sin(x*D_PI)*x/tmpv);
break;
case 5: //Modulated Square ?? ;-)
tmpv = x * D_PI;
out=f_sin(tmpv+f_sin(2.0f*tmpv));
break;
case 6: // Modulated Saw
tmpv = x * D_PI;
out=f_sin(tmpv+f_sin(tmpv));
break;
case 8: //Lorenz Fractal, faster, using X,Y outputs
iterations = 4;
case 7: // Lorenz Fractal
for(int j=0; j<iterations; j++) {
x1 = x0 + h * a * (y0 - x0);
y1 = y0 + h * (x0 * (b - z0) - y0);
z1 = z0 + h * (x0 * y0 - c * z0);
x0 = x1;
y0 = y1;
z0 = z1;
}
if(lfotype==7) {
if((radius = (sqrtf(x0*x0 + y0*y0 + z0*z0) * scale) - 0.25f) > 1.0f) radius = 1.0f;
if(radius < 0.0) radius = 0.0;
out = 2.0f * radius - 1.0f;
}
break;
case 9: //Sample/Hold Random
if(fmod(x,0.5f)<=(2.0f*incx)) { //this function is called by left, then right...so must toggle each time called
rreg = lreg;
lreg = RND1;
}
if(xlreg<lreg) xlreg += maxrate;
else xlreg -= maxrate;
if(xrreg<rreg) xrreg += maxrate;
else xrreg -= maxrate;
oldlreg = xlreg*tca + oldlreg*tcb;
oldrreg = xrreg*tca + oldrreg*tcb;
if(holdflag) {
out = 2.0f*oldlreg -1.0f;
holdflag = (1 + holdflag)%2;
} else {
out = 2.0f*oldrreg - 1.0f;
}
break;
//more to be added here; also ::updateparams() need to be updated (to allow more lfotypes)
default:
out = f_cos (x * D_PI); //EffectLFO_SINE
};
return (out);
};
