Nous avons mis à jour le code python dans notre référentiel git.
Il comprend désormais ;
- L’insaisissable filtre de Kalman.
- Mathématiques nécessaires lorsque l’IMU est à l’envers
- Calculer automatiquement la période de boucle.
- Beaucoup plus de commentaires.
Qu’est-ce qu’un filtre de Kalman ? En un mot;
Un filtre de Kalman est, c’est un algorithme qui utilise une série de mesures observées dans le temps, en l’occurrence un accéléromètre et un gyroscope. Ces mesures contiendront du bruit qui contribuera à l’erreur de la mesure. Le filtre de Kalman tentera alors d’estimer l’état du système, sur la base des états actuel et antérieur, qui tendent à être plus précis que les mesures seules.
Un filtre de Kalman est plus précis qu’un filtre complémentaire. Cela peut être vu dans l’image ci-dessous, qui est la sortie d’un filtre complémentaire (CFangleX) et d’un filtre de Kalman (kalmanX) à partir de l’axe X tracé dans un graphique.
La ligne rouge (KalmanX) filtre mieux le bruitp ;
Le code peut être trouvé ici dans notre référentiel Git ici
Et peut être ramené à votre Raspberry Pi avec ;
pi@raspberrypi ~ $ git clone http://github.com/ozzmaker/BerryIMU.git
Un résumé du code ;
def kalmanFilterY ( accAngle, gyroRate, DT):
y=0.0
S=0.0
global KFangleY
global Q_angle
global Q_gyro
global y_bias
global XP_00
global XP_01
global XP_10
global XP_11
global YP_00
global YP_01
global YP_10
global YP_11
KFangleY = KFangleY + DT * (gyroRate - y_bias)
YP_00 = YP_00 + ( - DT * (YP_10 + YP_01) + Q_angle * DT )
YP_01 = YP_01 + ( - DT * YP_11 )
YP_10 = YP_10 + ( - DT * YP_11 )
YP_11 = YP_11 + ( + Q_gyro * DT )
y = accAngle - KFangleY
S = YP_00 + R_angle
K_0 = YP_00 / S
K_1 = YP_10 / S
KFangleY = KFangleY + ( K_0 * y )
y_bias = y_bias + ( K_1 * y )
YP_00 = YP_00 - ( K_0 * YP_00 )
YP_01 = YP_01 - ( K_0 * YP_01 )
YP_10 = YP_10 - ( K_1 * YP_00 )
YP_11 = YP_11 - ( K_1 * YP_01 )
return KFangleY
BerryIMUpythonframboise pi