Kalman Filter Raspberry Pi

Guide to interfacing a Gyro and Accelerometer with a Raspberry Pi – Kalman Filter

In this guide we will go over some very basics on the use of a Kalman filter for sensor fusion. There is some very complex math involved which is well over my head, however we do have some working code and very good reference sites.

A prerequisite for this guide is to have a gyro and accelerometer from an IMU already up and running on your Raspberry Pi. A guide to interfacing an IMU with a Raspberry Pi can be found here.


Git repository here
The code can be pulled down to your Raspberry Pi with;

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


The code for this guide can be found under the gyro_accelerometer_tutorial03_kalman_filter directory.


BerryIMU Raspberry Pi Gyroscope Accelerometer


Kalman Filter

The Kalman filter, also known as linear quadratic estimation (LQE), is an algorithm that uses a series of measurements observed over time, containing noise (random variations) and other inaccuracies, and produces estimates of unknown variables that tend to be more precise than those based on a single measurement alone. More formally, the Kalman filter operates recursively on streams of noisy input data to produce a statistically optimal estimate of the underlying system state. The filter is named for Rudolf (Rudy) E. Kálmán, one of the primary developers of its theory.


The Kalman filter has numerous applications in technology. A common application is for guidance, navigation and control of vehicles, particularly aircraft and spacecraft. Furthermore, the Kalman filter is a widely applied concept in time series analysis used in fields such as signal processing and econometric.

FYI: A Kalman filter was used to assist with navigation in the Apollo 11 moon landing.

If we had to explain it in one sentence what a Kalman filter is: “It’s a method of predicting the future state of a system based on the previous ones.”


If you want to understand the ins and outs of the Kalman filter, then TKJ Electronics post is a must read.  I think I must have read through it  15 times.

These three other links are also worth a read;

The Code

Once you have a working Kalman filter function, it is very easy to slot  into existing code.
Below is our Kalman filter for the Y axis, I have another one for X axis;

  float kalmanFilterY(float accAngle, float gyroRate)
    float  y, S;
    float K_0, K_1;
    KFangleY += DT * (gyroRate - y_bias);
    YP_00 +=  - DT * (YP_10 + YP_01) + Q_angle * DT;
    YP_01 +=  - DT * YP_11;
    YP_10 +=  - DT * 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 +=  K_0 * y;
    y_bias  +=  K_1 * y;
    YP_00 -= K_0 * YP_00;
    YP_01 -= K_0 * YP_01;
    YP_10 -= K_1 * YP_00;
    YP_11 -= K_1 * YP_01;
    return KFangleY;

We will also need to declare our global variables

float Q_angle  =  0.01;
float Q_gyro   =  0.0003;
float R_angle  =  0.01;
float x_bias = 0;
float y_bias = 0;
float XP_00 = 0, XP_01 = 0, XP_10 = 0, XP_11 = 0;
float YP_00 = 0, YP_01 = 0, YP_10 = 0, YP_11 = 0;
float KFangleX = 0.0;
float KFangleY = 0.0;

You then call the Kalman filter functions with the current accelerometer angles and the current gyro rotation rate

        float kalmanX = kalmanFilterX(AccXangle, rate_gyr_x);
        float kalmanY = kalmanFilterY(AccYangle, rate_gyr_y);

The code in the repository will print out both the complimentary and Kalman filter values.


If you have any recommendations or improvements, please let us know.



Other Guides and Tutorials

16 thoughts on “Guide to interfacing a Gyro and Accelerometer with a Raspberry Pi – Kalman Filter”

  1. Hi Mark…

    Please, can you explain to me why when i run the BerryIMU.py the GYRY and GRYZ values not stop to increase.

    I am PILOT, and try to create a Raspberry Pi-EFIS is like “Dynon D2” —>


    Thanks you and I appreciate your help.

    From: Bogota, Colombia

    1. That is normal. It is from the noise which all gyros have. This is why the angles from an accelerometer and gyro need to be fused.

      Gyros – A gyro measures the rate of rotation, which has to be tracked over time to calculate the current angle. This tracking causes the gyro to drift. However, gyros are good at measuring quick sharp movements.

      More info here;https://ozzmaker.com/berryimu/

  2. Hi Mark

    In your code is not calculated YAW value. please can you show us how we can to calculate a YAW value?



  3. hello mr williams , i have a basic question , its kind of you if help me, as we know, we can have angle from gyro but with drift and we have angle from accelerometer but its not usefull because static and dinamic acc are combined. in overal i want to know that can we have angle by combining gyro and acc that was very fast and removed dinamic acc from that? i tried this code and also complimentary code but in both of them is static acc.

  4. Hi Mark,

    My heading seems to vary quite a bit so I was just fusing it with the gyroZ value but my understanding of kalman filters is pretty crap, so I was wondering if you could tell me how these values were calculated (and would they change to fuse the heading)?:
    Q_angle = 0.02
    Q_gyro = 0.0015
    R_angle = 0.005

    Thank you =)

    1. To my understanding, those values are just tuning parameters you need to select . As a matter of fact, in Kalman filtering theory they have a precise meaning and are related to the statistical properties of the noise you are dealing with and may also be time-varying! However, for this type of applications it is quite unlikely to be able to get closed-form expressions for R and Q so the best you can do is to consider those as parameters. Hopefully that replies to your question.

  5. Hello Mr. Williams,

    How would we merge this filter code with a Angular Acceleration tracking code? I am using a Raspberry Pi 3 with a SenseHAT (IMU), my objective is to track angular acceleration on a steering wheel bearing to track spikes in the wheels movements but ignore regular turns or movements.


  6. Hey mark or anyone in the comments who have worked on this, Can you guys tell me how to calculate AccZangle? What formula should i use? Plus how to add AccZangle in the rotation value “If Else” condition after that in the code.


  7. Hi mark,
    If i want to introduce KalmanFilterZ do i need to extend the matrix aswell like for example
    float XP_00 = 0, XP_01 = 0, XP_10 = 0, XP_11 = 0;
    float YP_00 = 0, YP_01 = 0, YP_10 = 0, YP_11 = 0;
    do i need to extend the current XP_00 with XP_000? because if i am introducing float ZP, do i need to add extra digit or can follow the same as with two digits ZP_00 = 0 ?

  8. hi,
    How fast i can read from your gyroscop and accelerometer module?
    I have MPU6050 and every information take 5 milisecond.
    I need faster.

    1. The gyroscope and accelerometer on the BerryIMUv2 can do 952 times a second, just above 1 millisecond.
      You need to make sure your MCU can read this fast. The arduino can.

  9. Hi,
    I would be interested in your BerryGPS-IMU board. I know almost nothing of programming. I would need gps position accuracy of about 1m. I do not know if that is possible using EKF for IMU-GPS data. If that is the case I would need a guide to use EKF for the IMU-GPS data of the above board. Thank you in advance for any help.

Leave a 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.