La science autour du filtre de kalman peux paraitre complete, l'implementation elle est generalement plutot simple, je suis pas un dieu en math non plus ^^
L'integralite du calcul est dans le fichier .h la:
https://github.com/TKJElectronics/KalmanFilter/blob/master/Kalman.hfloat getAngle(float newAngle, float newRate, float dt) {
// KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
// Modified by Kristian Lauszus
// See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
// Discrete Kalman filter time update equations - Time Update ("Predict")
// Update xhat - Project the state ahead
/* Step 1 */
rate = newRate - bias;
angle += dt * rate;
// Update estimation error covariance - Project the error covariance ahead
/* Step 2 */
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;
// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
// Calculate Kalman gain - Compute the Kalman gain
/* Step 4 */
float S = P[0][0] + R_measure; // Estimate error
/* Step 5 */
float K[2]; // Kalman gain - This is a 2x1 vector
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
// Calculate angle and bias - Update estimate with measurement zk (newAngle)
/* Step 3 */
float y = newAngle - angle; // Angle difference
/* Step 6 */
angle += K[0] * y;
bias += K[1] * y;
// Calculate estimation error covariance - Update the error covariance
/* Step 7 */
float P00_temp = P[0][0];
float P01_temp = P[0][1];
P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;
return angle;
};
Bien sur le code la tel que ne va pas te convenir car il est pour des gyros, mais c'est pour te montrer que la theorie peux paraitre complexe, la pratique l'est beaucoup moins (heureusement d'ailleurs, car ce type de filtre est utilise dans des robots qui on des contraintes de CPU plutot forte, si c'etait un truc lourdingue, ca marcherais pas aussi bien
)
C'est en gros un gros filtre moyeneur predictif, bien regle il elimite assez efficacement le bruit.
Apres tu n'es pas oblige de l'utiliser, tu demande des methodes, je te donne celle que je connais qui est couramment utilise en robotique.
Ton gros soucis va etre l'imprecision des acceleros, car ils sont generalement tres bruité, meme et ce meme dans une position stable (ie sur unr table, sans un train ou un tornade qui passe dans les parages) alros fixe au poignet, tu va avoir plein de mouvement parasites, il faut correctement filtrer ton signal d'entree avant de l'appliquer a un quelconque algo comme celui propose par Zero (d'ailleurs meme bien filtre une hystérésis est indispensable)
squalyl (./8) :
ps pour rappel, une accélération à zéro signifie un changement de direction de la vitesse, donc le bout d'une élongation de brasse par exemple. oui, c'est évident.
Non une acceleration de zero signifie aucun changement sur cet axe en terme de vitesse c'est quand l'acceleration est differente de zero que c'est en train de bouger differement.
Un accelero n'est pas un compteur de vitesse, si tu met un accelero sur un objet que tu peux faire bouger linéairement a une vitesse fixe, au moment ou il va commencer a avancer, tu va noter une accélération sur l'axe correspondant a la direction de l'engin, et une fois celui-ci a vitesse stable tu aura 0 sur le meme axe car il n'y a ni acceleration dans le sens de l'axe (valeur positive) ni deceleration dans le sens de l'axe (valeur negative)
)
