Lauszus Kalman Filter, code change for acceleration data, MPU6050

Hello, I try to find usefully sketches to run the mysterious KALMAN FILTER for MPU6050 (GY521) on Arduino Uno.

The sketch from Lauszus, is running successful on my board. The very detail documentation and instructions can be find here: KalmanFilter/examples/MPU6050 at master · TKJElectronics/KalmanFilter · GitHub

As well I know the very long discussion & cross questions thread about Kalman filtering from Lauszus:

My question:
The sketch from Lauszus for MPU6050 (MPU6050.ino) is designed to use the Kalman filter for:
"roll -> kalAngleX" and "pitch -> kalAngleY".

The sketch handles roll and pitch cases 180°, -180°. What means some very specific cases for angle measurement built in, what makes this code not flexible for other sensor data eg. the acceleration of MPU6050.

Until now I don't find a way to change the code to use the Kalman filter for the acceleration data, accX, accY & accZ.

May there is someone who can help to change the already written code from Lauszus or help to write a flash able sketch to use the acceleration sensor on MPU6050 with Kalman?


The "Kalman filter" code you are using is a poor place to start learning, because it has never worked very well and the code is simply wrong. In fact, it is surprising that it works at all.

You can read more about the problems and find a solution (in the Scilab programming language) here:

The Kalman filter is intended to merge data from a variety of sensors, not just to filter accelerometer data. Use a simple average for that. If you want to learn more by working your way through a good textbook, I recommend "Optimal State Estimation" by Dan Simon.

Hi jremington, thank you for the attached links.
I took a while to study the links to figure out the Kalman functions. While searching I found this Kalman description for a single signal processing. I seems to be that what I'm looking for...

// Implementing Kalman in C
// Implementing this filter in C code (e.g. for an Arduino) is quite simple.
// First of all we define a state for a kalman filter:

typedef struct {
double q; //process noise covariance
double r; //measurement noise covariance
double x; //value
double p; //estimation error covariance
double k; //kalman gain
} kalman_state;

// Next we need a function to initialize the kalman filter:

kalman_init(double q, double r, double p, double intial_value)
kalman_state result;
result.q = q;
result.r = r;
result.p = p;
result.x = intial_value;

return result;

// And a function to update the kalman state, by calculating a prediction and verifying that against the real measurement:

kalman_update(kalman_state* state, double measurement)
//prediction update
//omit x = x
state->p = state->p + state->q;

//measurement update
state->k = state->p / (state->p + state->r);
state->x = state->x + state->k * (measurement - state->x);
state->p = (1 - state->k) * state->p;

And here is a code example Bruno Azevedo Chagas from Brazil type down: GitHub - bachagas/Kalman: A simplified one dimensional Kalman filter implementation for Arduino.
I did not proove this code until now!

#include <Kalman.h>

//Creates the variables:

double measurement, filteredMeasurement;

Kalman myFilter(0.125,32,1023,0); //suggested initial values for high noise filtering

void loop() {


//reads measurement and filter it
measurement = (double) analogRead(A1); //read new value from sensor in analog pin A1, for example
filteredMeasurement = myFilter.getFilteredValue(measurement);


// Other (maybe) useful stuff

//Reset filter parameters:

void setParameters(double process_noise, double sensor_noise, double estimated_error)

//Get filter parameters:
double getProcessNoise()
double getSensorNoise()
double getEstimatedError()

// And that is it! :wink: