Go Down

Topic: Using MPU6050 with kalman filter (Read 3 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.

Go Up