Go Down

Topic: Using MPU6050 with kalman filter (Read 4274 times) previous topic - next topic

kazafwing

Since I am the first time to use the MPU6050, I have some question about that.

How to link the kalman filter program with the MPU6050 program? I was found the MPU6050 program in arduino playground. Also, I found the kalman filter program in the internet.

Thanks for help.


kazafwing


The code of the Kalman filter is shown below:
Code: [Select]

// KasBot V1  -  Kalman filter module

    float Q_angle  =  0.01; //0.001    //0.005
    float Q_gyro   =  0.0003;  //0.003  //0.0003
    float R_angle  =  0.01;  //0.03     //0.008

    float x_bias = 0;
    float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
    float  y, S;
    float K_0, K_1;

  float kalmanCalculate(float newAngle, float newRate,int looptime)
  {
    float dt = float(looptime)/1000;                                    // XXXXXXX arevoir
    x_angle += dt * (newRate - x_bias);
    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
    P_01 +=  - dt * P_11;
    P_10 +=  - dt * P_11;
    P_11 +=  + Q_gyro * dt;
   
    y = newAngle - x_angle;
    S = P_00 + R_angle;
    K_0 = P_00 / S;
    K_1 = P_10 / S;
   
    x_angle +=  K_0 * y;
    x_bias  +=  K_1 * y;
    P_00 -= K_0 * P_00;
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;
   
    return x_angle;
  }

michinyon

It depends what you are trying to filter,  and if you are trying to filter a single variable or
some kind of fusion of several variables,  which may or may not have some physical model
interconnecting them.


kazafwing

How can I get the raw data from MPU6050??

As I saw that the register map mention the 1A, 1B and 1C address is used for config. Also, the 0D, 0E and 0F is used for read data from X, Y and Z.
Should I use those address to get some raw data??

michinyon

You need to get the datasheet for the device and read it carefull.  The 6050 can be used in quite
a number of ways  and it has a lot of registers to put data in and out of.

michinyon

Quote
The code of the Kalman filter is shown below:


This code is specifically for a segway-type device in one dimension  and probably won't do you much
good for anything else.

michinyon

You need a bunch of documents for this device.   The one describing the register set is called RM-MPU-6000A.pdf

This device has a lot of registers that you can read or write to, through I2C,  the register numbers go up to 117 decimal.

The accelerometer measurement is in 6 registers  3B to 40 hex  and the gyro measurement in 43 to 48 hex.

You need to configure the device,  and then get the data from the device,   and then you can do any calculations
you like ( including Kalman filter, if you want to ) after you get the data.

The device has the really nifty feature that it can be programmed to calculate the orientation of the device
itself, which means the arduino doesn't have to do it.

For configuring and accessing the data from the device, I recommend the library developed by Jeff Rowberg
which is apparently called MPU6050


dheerajp

i want to use this MPU-6050 for balancing bot using kalman filter can it be done??

thanq :)

caymanguy

this is 6050 with kalman

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

Go Up