Non capisco perchè su uno sketch questo non dia problemi:
w_bx += w_err_x * _dt * zeta;
mentre in un altro questo mi ritorna errore:
wb_x += (werr_x * dt * ZETA);
expected primary-expression before '=' token
Chiaramente le variabili sono tutte definite!
Punti e virgola mancati alla riga/righe precedenti ?
ZETA una volta la scrivi minuscola ed una maiuscola ... è corretto ?
... altrimenti posta tutto il codice e proviamo a vedere dove è l'errore ... 
Guglielmo
sisi le ho definite diverse nei due sketch, lo sto rifacendo perchè avevo paura di aver sbagliato qualche segno nell'algoritmo quindi alla fine qualche variabile l'ho chiamata in modo un po' diverso. Posto il codice, per i due punti ho già guardato, ho provato anche a commentare la parte di codice appena sopra proprio per capire...
Che poi sarà solo una cavolata dovuta alla giornata passata a produrre l'algoritmo 
//Constant declaration
#define BETA = 0.05;
#define ZETA = 0.005;
//Quaternion declaration
float q_1=1.0, q_2=0.0, q_3=0.0, q_4=0.0;
//Angle declaration
float ROLL, PITCH, YAW;
//Other filter declaration
long time;
float dt;
float bx=1.0, bz=0.0;
//Estimated gyro bias error
float wb_x=0.0;
float wb_y=0.0;
float wb_z=0.0;
//IMU declaration
float ax, ay, az, mx, my, mz, wx, wy, wz;
void setup() {
Serial.begin(9600);
//IMU Settng
time = micros();
wx = 0.0;
wy = 0.0;
wz = 0.0;
mx = 0.0;
my = 0.0;
mz = 0.0;
ax = 0.0;
ay = 0.0;
az = 0.0;
}
void loop() {
//IMU request
//Filter fusion
delay(1000);
dt = (micros()-time)*0.000001;
Filter();
Serial.print(dt);Serial.print('\t');
}
void Filter (){
//Local declaration
float norm; //Auxiliary variable for vector's norm
float hx, hy, hz; //Auxiliary variable to compensate magnetic distortion
float qwt_1, qwt_2, qwt_3, qwt_4; //Quaternion gyro derivate
float qHD_1, qHD_2, qHD_3, qHD_4; //Quaternion acc-mag correction
float f_1, f_2, f_3, f_4, f_5, f_6; //Objetive funcion
float J_11_24, J_12_23, J_13_22, J_14_21, J_32, J_33; //Jacobian matrix's elements
float J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54;
float J_61, J_62, J_63, J_64;
float werr_x, werr_y, werr_z; //Estimated gyro error
//START COMPUTING
//Normalize acc-mag vectors
norm = sqrt (ax*ax + ay*ay + az*az);
ax /= norm;
ay /= norm;
az /= norm;
norm = sqrt (mx*mx + my*my + mz*mz);
mx /= norm;
my /= norm;
mz /= norm;
//Objective function
f_1 = 2 * (q_2*q_4 - q_1*q_3) - ax;
f_2 = 2 * (q_1*q_2 + q_3*q_4) - ay;
f_3 = 2 * (0.5 - q_2*q_2 - q_3*q_3) - az;
f_4 = 2 * bx * (0.5 - q_3*q_3 - q_4*q_4) + 2 * bz * (q_2*q_4 - q_1*q_3) - mx;
f_5 = 2 * bx * (q_2*q_3 + q_1*q_4) + 2 * bz * (q_1*q_2 - q_3*q_4) - my;
f_6 = 2 * bx * (q_1*q_3 + q_2*q_4) + 2 * bz * (0.5 - q_2*q_2 - q_3*q_3) - mz;
//Jacobian matrix
J_11_24 = 2 * q_3;
J_12_23 = 2 * q_4;
J_13_22 = 2 * q_1;
J_14_21 = 2 * q_2;
J_32 = 4 * q_2;
J_33 = 4 * q_3;
J_41 = 2 * bz * q_3;
J_42 = 2 * bz * q_4;
J_43 = 4 * bx * q_3 + 2 * bz * q_1;
J_44 = 4 * bx * q_4 - 2 * bz * q_2;
J_51 = 2 * (bx * q_4 - bz * q_2);
J_52 = 2 * (bx * q_3 + bz * q_1);
J_53 = 2 * (bx * q_2 + bz * q_4);
J_54 = 2 * (bx * q_1 - bz * q_3);
J_61 = 2 * bx * q_3;
J_62 = 2 * bx * q_4 - 4 * bz * q_2;
J_63 = 2 * bx * q_1 - 4 * bz * q_3;
J_64 = 2 * bx * q_2;
//Compute Gradient Descent
qHD_1 = -J_11_24 * f_1 + J_14_21 * f_2 - J_41 * f_4 -J_51 * f_5 + J_61 * f_6;
qHD_2 = J_12_23 * f_1 + J_13_22 * f_2 - J_32 * f_3 + J_42 * f_4 + J_52 * f_5 + J_62 * f_6;
qHD_3 = -J_13_22 * f_1 + J_12_23 * f_2 - J_33 * f_3 - J_43 * f_4 + J_53 * f_5 + J_63 * f_6;
qHD_4 = J_14_21 * f_1 + J_11_24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6;
//Compute unit vector to extimate error direction
norm = sqrt (qHD_1 * qHD_1 + qHD_2 * qHD_2 + qHD_3 * qHD_3 + qHD_4 * qHD_4);
qHD_1 /= norm;
qHD_2 /= norm;
qHD_3 /= norm;
qHD_4 /= norm;
//Compute angular estimated direction of gyro error
werr_x = 2 * (q_1 * qHD_1 - q_2 * qHD_1 - q_3 * qHD_4 + q_4 * qHD_3);
werr_y = 2 * (q_1 * qHD_3 - q_2 * qHD_4 - q_3 * qHD_1 + q_4 * qHD_2);
werr_z = 2 * (q_1 * qHD_4 - q_2 * qHD_3 - q_3 * qHD_2 + q_4 * qHD_1);
//Correction of w sensor's value
wb_x = wb_x + (werr_x * dt * ZETA);
wb_y += (werr_y * dt * ZETA);
wb_z += (werr_z * dt * ZETA);
//Computing gyro quaternion rate
qwt_1 = 0.5 * (- q_2 * wx - q_3 * wy - q_4 * wz);
qwt_2 = 0.5 * (q_1 * wx + q_3 * wz - q_4 * wy);
qwt_3 = 0.5 * (q_1 * wy - q_2 * wz + q_4 * wx);
qwt_4 = 0.5 * (q_1 * wz + q_2 * wy - q_3 * wx);
//Compute estimare quaternion
q_1 += (qwt_1 - BETA * qHD_1) * dt;
q_2 += (qwt_2 - BETA * qHD_2) * dt;
q_3 += (qwt_3 - BETA * qHD_3) * dt;
q_4 += (qwt_4 - BETA * qHD_4) * dt;
//Normalise quaternion
norm = sqrt(q_1 * q_1 + q_2 * q_2 + q_3 * q_3 + q_4 * q_4);
q_1 /= norm;
q_2 /= norm;
q_3 /= norm;
q_4 /= norm;
//Recompute auxiliary vector
hx = 2 * mx * (0.5 - q_3*q_3 - q_4*q_4) + 2 * my * (q_2*q_3 - q_1*q_4) + 2 * mz * (q_2*q_4 + q_1*q_3);
hy = 2 * mx * (q_2*q_3 - q_1*q_4) + 2 * my * (0.5 - q_2*q_2 - q_4*q_4) + 2 * mz * (q_3*q_4 - q_1*q_2);
hx = 2 * mx * (q_2*q_4 - q_1*q_3) + 2 * my * (q_3*q_4 + q_1*q_2) + 2 * mz * (0.5 - q_2*q_2 - q_3*q_3);
bx = sqrt (hx*hx + hy*hy);
bz = hz;
}
Grazie mille Guglielmo per la pazienza,
Andrea
Difatti è una banalità ... 
NON si scrive :
#define BETA = 0.05;
#define ZETA = 0.005;
ma si scrive
#define BETA 0.05
#define ZETA 0.005
Nelle #define mai il segno di "=" per assegnare il valore alla define e mai il ";" finale ...
Ricorda che il compilatore usa la #define per fare una sostituzione quindi ... la posto di ZETA metteva "=0.005;" e non solo il valore 0.005
Guglielmo
che stupido, ma il bello è che nel frattempo l'uguale l'ho tolto, ma il punto e virgola no
GRAZIE MILLE
il problema è qui
//Constant declaration
#define BETA = 0.05;
#define ZETA = 0.005;
non puoi definire cosi delle define ma
//Constant declaration
#define BETA 0.05
#define ZETA 0.005
ciao
scusa non avevo visto la risposta 