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/mwilliams03/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.”

References

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;
http://bilgin.esme.org/BitsBytes/KalmanFilterforDummies.aspx
http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data
http://credentiality2.blogspot.com.au/2010/08/simple-kalman-filter-example.html

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

In this order;
Guide to interfacing a Gyro and Accelerometer with a Raspberry Pi
Guide to interfacing a Gyro and Accelerometer with a Raspberry Pi – Kalman Filter
Create a Digital Compass with the Raspberry Pi – Part 1 – “The Basics”
Create a Digital Compass with the Raspberry Pi – Part 2 – “Tilt Compensation”
Create a Digital Compass with the Raspberry Pi – Part 3 – “Calibration”
Create a Digital Compass with the Raspberry Pi – Part 4- “Smartphone Replica”
Converting values from an Accelerometer to Gs – “ Proper Acceleration”

How to Create an Inclinometer using a Raspberry Pi and an IMU
Raspberry Pi Digital Spirit Levela>

9 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” —>

    https://www.youtube.com/watch?v=vj7mjKI2GHk

    Thanks you and I appreciate your help.

    CARLOS F. GOMEZ
    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;http://ozzmaker.com/berryimu/

  2. 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.

  3. 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.

Leave a Reply

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