Non ho capito bene alcune cose riguardanti il filtro di kalman...
mettiamo che io voglia usare questo codice:
// KasBot V1 - Kalman filter module
float Q_angle = 0.01; //0.001
float Q_gyro = 0.0003; //0.003
float R_angle = 0.01; //0.03
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float y, S;
float K_0, K_1;
// newAngle = angle measured with atan2 using the accelerometer
// newRate = angle measured using the gyro
// looptime = loop time in millis()
float kalmanCalculate(float newAngle, float newRate,int looptime)
{
float dt = float(looptime)/1000;
x_angle += dt * (newRate - x_bias);
P_00 += - dt * (P_10 + P_01) + Q_angle * dt;
P_01 += - dt * P_11;
P_10 += - dt * P_11;
P_11 += + Q_gyro * dt;
y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;
x_angle += K_0 * y;
x_bias += K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;
return x_angle;
}
quando richiamo la funzione kalmanCalculate devo passare il primo valore che come dice il commento è l'arctan2 della x e della y dell'accelerometro, ma usando queste funzioni non so che unità di misura hanno i valori restituiti e soprattutto non so se posso usarli cosi come sono per la funziona kalmanCalculate.
Per quanto riguarda newRate non è specificato se devo passare la velocità angolare (non credo) o l'angolo assoluto e se cosi fosse 'unità di misura dovrebbe essere in gradi o in radianti?
Il looptime cosa sarebbe? Devo passare il valore di "intervallo" del codice scritto da me?
Grazie ancora...