BerryIMU Python Code Update - Kalman Filter and More

We have updated to the python code in our git repo.

It now includes;

  • The elusive Kalman filter.
  • Math needed when the IMU is upside down
  • Automatically calculate loop period.
  • A lot more comments.

What is a Kalman filter?  In a nutshell;
A Kalman filter is, it is an algorithm which uses a series of measurements observed over time, in this context an accelerometer and a gyroscope. These measurements will contain noise that will contribute to the error of the measurement. The Kalman filter will then try to estimate the state of the system, based on the current and previous states, that tend to be more precise that than the measurements alone.

A Kalman filter is more precise than a Complementary filter. This can be seen in the image below, which is the output of a complementary filter (CFangleX) and a Kalman filter (kalmanX) from the X axis plotted in a graph.

The red line (KalmanX) is better at filtering out noisep;

Python Kalman filter Raspberry Pi

 

The code can be found here in our Git repository here
And  can be pulled down to your Raspberry Pi with;

pi@raspberrypi ~ $ git clone http://github.com/ozzmaker/BerryIMU.git

BerryIMU Raspberry Pi Gyroscope Accelerometer

A summary of the 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

4 thoughts on “BerryIMU Python Code Update - Kalman Filter and More”

  1. Hi

    Is CFangleX directly comparable with kalmanx, as "suggested" in in the figure above? The reason why I ask is that I get very different results for rawx, CFangleX and kalmanx - really not comparable. Results are obtained using BerryGPS-IMU3 lying static on ground. raw(x,y,z) yields (0,0,1)g when calibrated using acc = acc*0.244*1e-3 (so rawx,y,z are fine). If results are comparable, then does it make sense to apply calibration (acc = acc*0.244*1e-3) on kalman and/or CF data to obtain absolute g (or m/s2) values?

      1. I am using the gyro_accelerometer_tutorial03_kalman_filter from GitHub. No changes to the code. So I guess I should be able to obtain somewhat comparable results using rawx, kalmanx, cfAngleX. So far, I have 5 BerryIMU V2 and 6 BerryGPS IMU V3 which I am trying to validate before scaling up

Leave a Reply to Stefan Cancel reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.