user55244: I am currently working with MPU-6050. Now I want to minus gravity from my accelerometer's value as we minus the offset.
Here is the code:
#include <Wire.h>
#include <MPU6050.h>
//read as rool:pitch:yaw:accX:accY:accZ
MPU6050 mpu;
unsigned long timer = 0;
float timeStep = 0.01;
float pitch = 0;
float roll = 0;
float yaw = 0;
float accX = 0;
float accY = 0;
float accZ = 0;
float rollRad = 0;
float pitchRad = 0;
float yawRad = 0;

int count = 0;

Vector minusGravity(Vector vec)
{
    /*
     float x = vec.XAxis, y = vec.YAxis, z = vec.ZAxis;
     vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) + x * cos(pitchRad) + z 
           * sin(pitchRad) + 0;
     vec.YAxis -= x * sin(yawRad) - y * cos(yawRad) + 0 + y * cos(rollRad) - 
           z * sin(rollRad);
     vec.ZAxis -= 0 - x * sin(pitchRad) + z * cos(pitchRad) + y * 
           sin(rollRad) + z * cos(rollRad);
    */

    return vec;
}

float accP(float value)
{
    int x = (int)(value * 10);
    return ((float)(x)) / 10;
}

void setup()
{
    Serial.begin(115200);
    while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
    {

    }
    //mpu.calibrateGyro();
    mpu.setThreshold(1);
 }

 void loop()
 {
    timer = millis();
    Vector norm = mpu.readNormalizeGyro();
    pitch = pitch + norm.YAxis * timeStep;
    roll = roll + norm.XAxis * timeStep;
    yaw = yaw + norm.ZAxis * timeStep;
    Vector normAccel = minusGravity(mpu.readNormalizeAccel());
    rollRad = roll * 0.0174533;
    pitchRad = pitch * 0.0174533;
    yawRad = yaw * 0.0174533;

    Serial.print(roll);
    Serial.print(":");
    Serial.print(pitch);
    Serial.print(":");
    Serial.print(yaw);
    Serial.print(":");
    accX = accP(normAccel.XAxis);
    accY = accP(normAccel.YAxis);
    accZ = accP(normAccel.ZAxis);
    Serial.print(accX);
    Serial.print(":");
    Serial.print(accY);
    Serial.print(":");
    Serial.println(accZ);
    delay((timeStep * 2500) - (millis() - timer));
}

I understand the problem is here :
vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) + x * cos(pitchRad) + z 
           * sin(pitchRad) + 0;
     vec.YAxis -= x * sin(yawRad) - y * cos(yawRad) + 0 + y * cos(rollRad) - 
           z * sin(rollRad);
     vec.ZAxis -= 0 - x * sin(pitchRad) + z * cos(pitchRad) + y * 
           sin(rollRad) + z * cos(rollRad);

New code : 
      #include <Wire.h>
  #include <MPU6050.h>
  //read as rool:pitch:yaw:accX:accY:accZ
  MPU6050 mpu;
  unsigned long timer = 0;
  float timeStep = 0.01;
  float pitch = 0;
  float roll = 0;
  float yaw = 0;
  float accX = 0;
  float accY = 0;
  float accZ = 0;
  float rollRad = 0;
  float pitchRad = 0;
  float yawRad = 0;
  float gX = 0;
  float gY = 0;
  float gZ = -9.8;

  int count = 0;

  Vector minusGravity(Vector vec)
  {


    float x = gX, y = gY, z = gZ;
    vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) ;
    vec.YAxis -= x * sin(yawRad) - y * cos(yawRad) ;

    x = vec.XAxis; z = vec.ZAxis;
    vec.XAxis -=  x * cos(pitchRad) + z * sin(pitchRad) ;
    vec.ZAxis -= - x * sin(pitchRad) + z * cos(pitchRad) ;

    y = vec.YAxis, z = vec.ZAxis;
    vec.YAxis -=  y * cos(rollRad) - z * sin(rollRad);
    vec.ZAxis -=  y * sin(rollRad) + z * cos(rollRad);


    return vec;
  }

  float accP(float value)
  {
    int x = (int)(value * 10);
    return ((float)(x)) / 10;
  }

  void setup()
  {
    Serial.begin(115200);
    while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
    {

    }
    //mpu.calibrateGyro();
    mpu.setThreshold(1);
    Vector normAccel = mpu.readNormalizeAccel();
    gX = normAccel.XAxis;
    gY = normAccel.YAxis;
    gZ = normAccel.ZAxis;
  }
  void loop()
  {
    timer = millis();
    Vector norm = mpu.readNormalizeGyro();
    pitch +=  norm.YAxis * timeStep;
    roll += norm.XAxis * timeStep;
    yaw += norm.ZAxis * timeStep;
    Vector normAccel = minusGravity(mpu.readNormalizeAccel());
    rollRad = roll * 0.0174533;
    pitchRad = pitch * 0.0174533;
    yawRad = yaw * 0.0174533;
    Serial.print(roll );
    Serial.print(":");
    Serial.print(pitch );
    Serial.print(":");
      Serial.print(yaw );
    Serial.print(":");
    accX = accP(normAccel.XAxis);
    accY = accP(normAccel.YAxis);
    accZ = accP(normAccel.ZAxis);
    Serial.print(accX);
    Serial.print(":");
    Serial.print(accY);
    Serial.print(":");
    Serial.println(accZ);
    delay((timeStep * 2500) - (millis() - timer));
  }

It is little buggy.
Can anyone help me figure out what is my mistake ?

user55244: I may have come to a solution :
Vector minusGravity(Vector vec)
{


 float x = vec.XAxis, y = vec.YAxis, z = 0;
 vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) ;
 vec.YAxis -= x * sin(yawRad) - y * cos(yawRad) ;

 x = vec.XAxis; z = vec.ZAxis;
 vec.XAxis -=  x * cos(pitchRad) + z * sin(pitchRad) ;
 vec.ZAxis -= - x * sin(pitchRad) + z * cos(pitchRad) ;

 y = vec.YAxis, z = vec.ZAxis;
 vec.YAxis -=  y * cos(rollRad) - z * sin(rollRad);
 vec.ZAxis -=  y * sin(rollRad) + z * cos(rollRad);


 return vec;
}

Now this is far better, as after calculating effect of roll,pitch,yaw in every step the  x,y,z changes. I missed that part. I was calculating directly, so it was giving me wrong value.
vec.XAxis -= x * cos(yawRad) - y * sin(yawRad)
then, vec.XAxis -=  x * cos(pitchRad) + z * sin(pitchRad) 
 doesn't give the same result as the wrong one vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) + x * cos(pitchRad) + z 
           * sin(pitchRad) + 0;

55244: <p>I am currently working with MPU-6050. Now I want to minus gravity from my accelerometer's value as we minus the offset.</p>

<p>Here is the code:</p>

<pre><code>#include &lt;Wire.h&gt;
#include &lt;MPU6050.h&gt;
//read as rool:pitch:yaw:accX:accY:accZ
MPU6050 mpu;
unsigned long timer = 0;
float timeStep = 0.01;
float pitch = 0;
float roll = 0;
float yaw = 0;
float accX = 0;
float accY = 0;
float accZ = 0;
float rollRad = 0;
float pitchRad = 0;
float yawRad = 0;

int count = 0;

Vector minusGravity(Vector vec)
{
    /*
     float x = vec.XAxis, y = vec.YAxis, z = vec.ZAxis;
     vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) + x * cos(pitchRad) + z 
           * sin(pitchRad) + 0;
     vec.YAxis -= x * sin(yawRad) - y * cos(yawRad) + 0 + y * cos(rollRad) - 
           z * sin(rollRad);
     vec.ZAxis -= 0 - x * sin(pitchRad) + z * cos(pitchRad) + y * 
           sin(rollRad) + z * cos(rollRad);
    */

    return vec;
}

float accP(float value)
{
    int x = (int)(value * 10);
    return ((float)(x)) / 10;
}

void setup()
{
    Serial.begin(115200);
    while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
    {

    }
    //mpu.calibrateGyro();
    mpu.setThreshold(1);
 }

 void loop()
 {
    timer = millis();
    Vector norm = mpu.readNormalizeGyro();
    pitch = pitch + norm.YAxis * timeStep;
    roll = roll + norm.XAxis * timeStep;
    yaw = yaw + norm.ZAxis * timeStep;
    Vector normAccel = minusGravity(mpu.readNormalizeAccel());
    rollRad = roll * 0.0174533;
    pitchRad = pitch * 0.0174533;
    yawRad = yaw * 0.0174533;

    Serial.print(roll);
    Serial.print(":");
    Serial.print(pitch);
    Serial.print(":");
    Serial.print(yaw);
    Serial.print(":");
    accX = accP(normAccel.XAxis);
    accY = accP(normAccel.YAxis);
    accZ = accP(normAccel.ZAxis);
    Serial.print(accX);
    Serial.print(":");
    Serial.print(accY);
    Serial.print(":");
    Serial.println(accZ);
    delay((timeStep * 2500) - (millis() - timer));
}
</code></pre>

<p>I understand the problem is here :</p>

<pre><code>vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) + x * cos(pitchRad) + z 
           * sin(pitchRad) + 0;
     vec.YAxis -= x * sin(yawRad) - y * cos(yawRad) + 0 + y * cos(rollRad) - 
           z * sin(rollRad);
     vec.ZAxis -= 0 - x * sin(pitchRad) + z * cos(pitchRad) + y * 
           sin(rollRad) + z * cos(rollRad);
</code></pre>

<p>New code : </p>

<pre><code>      #include &lt;Wire.h&gt;
  #include &lt;MPU6050.h&gt;
  //read as rool:pitch:yaw:accX:accY:accZ
  MPU6050 mpu;
  unsigned long timer = 0;
  float timeStep = 0.01;
  float pitch = 0;
  float roll = 0;
  float yaw = 0;
  float accX = 0;
  float accY = 0;
  float accZ = 0;
  float rollRad = 0;
  float pitchRad = 0;
  float yawRad = 0;
  float gX = 0;
  float gY = 0;
  float gZ = -9.8;

  int count = 0;

  Vector minusGravity(Vector vec)
  {


    float x = gX, y = gY, z = gZ;
    vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) ;
    vec.YAxis -= x * sin(yawRad) - y * cos(yawRad) ;

    x = vec.XAxis; z = vec.ZAxis;
    vec.XAxis -=  x * cos(pitchRad) + z * sin(pitchRad) ;
    vec.ZAxis -= - x * sin(pitchRad) + z * cos(pitchRad) ;

    y = vec.YAxis, z = vec.ZAxis;
    vec.YAxis -=  y * cos(rollRad) - z * sin(rollRad);
    vec.ZAxis -=  y * sin(rollRad) + z * cos(rollRad);


    return vec;
  }

  float accP(float value)
  {
    int x = (int)(value * 10);
    return ((float)(x)) / 10;
  }

  void setup()
  {
    Serial.begin(115200);
    while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
    {

    }
    //mpu.calibrateGyro();
    mpu.setThreshold(1);
    Vector normAccel = mpu.readNormalizeAccel();
    gX = normAccel.XAxis;
    gY = normAccel.YAxis;
    gZ = normAccel.ZAxis;
  }
  void loop()
  {
    timer = millis();
    Vector norm = mpu.readNormalizeGyro();
    pitch +=  norm.YAxis * timeStep;
    roll += norm.XAxis * timeStep;
    yaw += norm.ZAxis * timeStep;
    Vector normAccel = minusGravity(mpu.readNormalizeAccel());
    rollRad = roll * 0.0174533;
    pitchRad = pitch * 0.0174533;
    yawRad = yaw * 0.0174533;
    Serial.print(roll );
    Serial.print(":");
    Serial.print(pitch );
    Serial.print(":");
      Serial.print(yaw );
    Serial.print(":");
    accX = accP(normAccel.XAxis);
    accY = accP(normAccel.YAxis);
    accZ = accP(normAccel.ZAxis);
    Serial.print(accX);
    Serial.print(":");
    Serial.print(accY);
    Serial.print(":");
    Serial.println(accZ);
    delay((timeStep * 2500) - (millis() - timer));
  }
</code></pre>

<p>It is little buggy.</p>

<p>Can anyone help me figure out what is my mistake ?</p>
 55244: <p>I may have come to a solution :</p>

<pre><code>Vector minusGravity(Vector vec)
{


 float x = vec.XAxis, y = vec.YAxis, z = 0;
 vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) ;
 vec.YAxis -= x * sin(yawRad) - y * cos(yawRad) ;

 x = vec.XAxis; z = vec.ZAxis;
 vec.XAxis -=  x * cos(pitchRad) + z * sin(pitchRad) ;
 vec.ZAxis -= - x * sin(pitchRad) + z * cos(pitchRad) ;

 y = vec.YAxis, z = vec.ZAxis;
 vec.YAxis -=  y * cos(rollRad) - z * sin(rollRad);
 vec.ZAxis -=  y * sin(rollRad) + z * cos(rollRad);


 return vec;
}
</code></pre>

<p>Now this is far better, as after calculating effect of <code>roll,pitch,yaw</code> in every step the  <code>x,y,z</code> changes. I missed that part. I was calculating directly, so it was giving me wrong value.</p>

<p><code>vec.XAxis -= x * cos(yawRad) - y * sin(yawRad)</code></p>

<p>then, <code>vec.XAxis -=  x * cos(pitchRad) + z * sin(pitchRad)</code> 
 doesn't give the same result as the wrong one <code>vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) + x * cos(pitchRad) + z 
           * sin(pitchRad) + 0;</code></p>
