Mise à jour du code Python BerryIMU – Filtre Kalman et plus

Mise à jour du code Python BerryIMU - Filtre Kalman et plus

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 ;

Filtre Python Kalman Raspberry Pi

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

Accéléromètre gyroscope BerryIMU Raspberry Pi

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