Simple accelerometer calibration

Hi,
I am using MPU 6050 and complementary filter for my project. I did simple calibration by getting the average of first 100 readings. The gyro workg fine, but I have a problem with the accelerometer. When I add a calibration like I did with gyro, I get weird values. Without accel calibration, the complementary filter works fine. How to solve this problem?

piece of code from setup()

 // Set the default scale range of the gyro and accelerometer
  uint8_t FS_SEL = 0;
  uint8_t AFS_SEL = 0;
  
  uint8_t READ_FS_SEL = accelgyro.getFullScaleGyroRange();
  GYRO_FACTOR = 131.0/(FS_SEL + 1);
  uint8_t READ_AFS_SEL = accelgyro.getFullScaleAccelRange();
  ACCEL_FACTOR = 16384.0/(AFS_SEL + 1);
  // calibrate raw readings from accelerometer and gyroscope
  averageAccGyroReadings();

calibration function:

void averageAccGyroReadings()
{
  for(int x=0; x<number; x++) {
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    // sum the readings
    sumAccX += ax;
    sumAccY += ay;
    sumAccZ += az;
    sumGyroX += gx;
    sumGyroY += gy;
    sumGyroZ += gz;
  }
  
  avgAccX = sumAccX/number;
  avgAccY = sumAccY/number;
  avgAccZ = sumAccZ/number;
  avgGyroX = sumGyroX/number;
  avgGyroY = sumGyroY/number;
  avgGyroZ = sumGyroZ/number;
}

calibration in compelemtary filter function:

// remove offsets
  gyroX = (gx - avgGyroX)/GYRO_FACTOR;
  gyroY = (gy - avgGyroY)/GYRO_FACTOR;
  gyroZ = (gz - avgGyroZ)/GYRO_FACTOR; 
  accX = (ax - avgAccX)/ACCEL_FACTOR;
  accY = (ay - avgAccY)/ACCEL_FACTOR;
  accZ =( az - avgAccZ)/ACCEL_FACTOR;

I know there should be something different with Z axis of accelerometer, but I tried few other solutions and nothing helps.

Any suggestions?
Thank you

Follow this tutorial: https://learn.adafruit.com/adafruit-analog-accelerometer-breakouts/programming modifying as necessary for your accelerometer.

Thank you, but I am looking for an anwer how to do calibration in my way. I found a suggestion somewhere that I could do something like that:

accX = ax - avgAccX;
  accY = ay - avgAccY;
  accZ = az - avgAccZ + ACCEL_FACTOR;

But at the result I get 90 degrees when it should be 0 degrees (IMU is flat) and I get 0 degrees when IMU is vertical. I know there is a "trick" with z axis of accelerometer, because it shouldn't show zero but be equal to g at rest. Does somebody know how to solve this?

i think you are considering second order equation to get result.

can you attach complete code here along with datasheet

Ok here’s is my full code.
The complementary filter seems to work ok, but when I add the same calibration as I did with the gyro, I don’t get the right results… My calibration method is the simplest calibration - taking first few readings and getting average.

#include "Math.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 accelgyro;
 
int16_t ax, ay, az;
int16_t gx, gy, gz;

float RAD_TO_DEG = 180/3.14159;
float GYRO_FACTOR = 131;
 
float dt = 0; //sample rate
float lastLoopTime = 0;

float sumAccX = 0;
float sumAccY = 0;
float sumAccZ = 0;
float sumGyroX = 0;
float sumGyroY = 0;
float sumGyroZ = 0;

float avgAccX = 0;
float avgAccY = 0;
float avgAccZ = 0;
float avgGyroX = 0;
float avgGyroY = 0;
float avgGyroZ = 0;

float calcPitch = 0;

void average()
{
  int number = 100;
  for(int x=0;x<number;x++)
  {
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    sumAccX += ax;
    sumAccY += ay;
    sumAccZ += az;
    sumGyroX += gx;
    sumGyroY += gy;
    sumGyroZ += gz;
  }
  avgAccX = sumAccX/number;
  avgAccY = sumAccY/number;
  avgAccZ = sumAccZ/number;
  avgGyroX = sumGyroX/number;
  avgGyroY = sumGyroY/number;
  avgGyroZ = sumGyroZ/number;
}

void setup() 
{
  Wire.begin();
   
  // initialize serial communication
  Serial.begin(115200);
   
  // initialize device
  accelgyro.initialize();
  // get average
  average();

}
 
void loop() 
{
  dt = millis() - lastLoopTime;
  // read raw accel/gyro measurements from device
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // REMOVING OFFSETS --------------------------------------------------
  float gyroX = (gx - avgGyroX)/GYRO_FACTOR;
  float gyroY = (gy - avgGyroY)/GYRO_FACTOR;
  float gyroZ = (gz - avgGyroZ)/GYRO_FACTOR;
  float accX = ax;
  float accY = ay;
  float accZ = az;

  // CALCULATING ACCELEROMETER ANGLE -----------------------------------
  float accPitch = atan(-accX/sqrt(pow(accY,2) + pow(accZ,2))); //pitch

  // CALCULATING GYROSCOPE ANGLE ---------------------------------------
  float gyroPitch = gyroY * (dt / 1000);

  // COMPLEMENTARY FILTER IMPLEMENT ------------------------------------
  calcPitch = 0.98 * (calcPitch + gyroPitch) + (0.02 * accPitch) * RAD_TO_DEG;
  lastLoopTime = millis();
}

datasheet: http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf

#include "Math.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

float RAD_TO_DEG = 180/3.14159;
float GYRO_FACTOR = 131;

float dt = 0; //sample rate
unsigned int lastLoopTime = 0;

float sumAccX = 0.0;
float sumAccY = 0.0;
float sumAccZ = 0.0;
float sumGyroX = 0.0;
float sumGyroY = 0.0;
float sumGyroZ = 0.0;

float avgAccX =  0.0;
float avgAccY =  0.0;
float avgAccZ =  0.0;
float avgGyroX =  0.0;
float avgGyroY =  0.0;
float avgGyroZ =  0.0;

float calcPitch =  0.0;

void average()
{
  int number = 20;
  for(int x=0;x<number;x++)
  {
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    sumAccX += ax;
    sumAccY += ay;
    sumAccZ += az;
    sumGyroX += gx;
    sumGyroY += gy;
    sumGyroZ += gz;
  }
  avgAccX = sumAccX/number;
  avgAccY = sumAccY/number;
  avgAccZ = sumAccZ/number;
  avgGyroX = sumGyroX/number;
  avgGyroY = sumGyroY/number;
  avgGyroZ = sumGyroZ/number;
}

void setup() 
{
  Wire.begin();

  // initialize serial communication
  Serial.begin(9600);

  // initialize device
  accelgyro.initialize();
  // get average
//  average();

}

void loop() 
{
  dt = millis() - lastLoopTime;
  // read raw accel/gyro measurements from device
  
  
  
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  average();
  // REMOVING OFFSETS --------------------------------------------------
  float gyroX = (gx - avgGyroX)/GYRO_FACTOR;
  float gyroY = (gy - avgGyroY)/GYRO_FACTOR;
  float gyroZ = (gz - avgGyroZ)/GYRO_FACTOR;
  float accX = ax;
  float accY = ay;
  float accZ = az;

  // CALCULATING ACCELEROMETER ANGLE -----------------------------------
  float accPitch = atan(-accX/sqrt(pow(accY,2) + pow(accZ,2))); //pitch

  // CALCULATING GYROSCOPE ANGLE ---------------------------------------
  float gyroPitch = gyroY * (dt / 1000);

  // COMPLEMENTARY FILTER IMPLEMENT ------------------------------------
  calcPitch = 0.98 * (calcPitch + gyroPitch) + (0.02 * accPitch) * RAD_TO_DEG;
  lastLoopTime = millis();
}

Please serial output for the code. I dont have library function so test it.

The results are in attachments. They show results when IMU is vertical -90 deg, when it’s flat and when it’s vertical +90 deg.

Capture2.PNG

Capture3.PNG

It wont look like mine code. if you modifying your code lets us know.

Keep one point as reference Where all value must read as Zero. from where you change value

#include "Math.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

float RAD_TO_DEG = 180/3.14159;
float GYRO_FACTOR = 131;

float dt = 0; //sample rate
unsigned int lastLoopTime = 0;

float sumAccX = 0.0;
float sumAccY = 0.0;
float sumAccZ = 0.0;
float sumGyroX = 0.0;
float sumGyroY = 0.0;
float sumGyroZ = 0.0;

float avgAccX =  0.0;
float avgAccY =  0.0;
float avgAccZ =  0.0;
float avgGyroX =  0.0;
float avgGyroY =  0.0;
float avgGyroZ =  0.0;

float calcPitch =  0.0;

void average()
{
  int number = 20;
  for(int x=0;x<number;x++)
  {
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    sumAccX += ax;
    sumAccY += ay;
    sumAccZ += az;
    sumGyroX += gx;
    sumGyroY += gy;
    sumGyroZ += gz;
  }
  avgAccX = sumAccX/number;
  avgAccY = sumAccY/number;
  avgAccZ = sumAccZ/number;
  avgGyroX = sumGyroX/number;
  avgGyroY = sumGyroY/number;
  avgGyroZ = sumGyroZ/number;
}

void setup() 
{
  Wire.begin();

  // initialize serial communication
  Serial.begin(9600);

  // initialize device
  accelgyro.initialize();
  // get average
//  average();

}

void loop() 
{
 //dt = millis() - lastLoopTime;
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  average();
  // REMOVING OFFSETS --------------------------------------------------
  float gyroX = (gx - avgGyroX)/GYRO_FACTOR;
  float gyroY = (gy - avgGyroY)/GYRO_FACTOR;
  float gyroZ = (gz - avgGyroZ)/GYRO_FACTOR;
  float accX = ax;
  float accY = ay;
  float accZ = az;

  // CALCULATING ACCELEROMETER ANGLE -----------------------------------
  float accPitch = atan(-accX/sqrt(pow(accY,2) + pow(accZ,2))); //pitch
Serial.print("accPitch:");Serial.println(accPitch);

  // CALCULATING GYROSCOPE ANGLE ---------------------------------------
  float gyroPitch = gyroY * (dt / 1000);
Serial.print("gyroPitch:");Serial.println(gyroPitch);

  // COMPLEMENTARY FILTER IMPLEMENT ------------------------------------
  calcPitch = 0.98 * (calcPitch + gyroPitch) + (0.02 * accPitch) * RAD_TO_DEG;
  Serial.print("calcPitch:");Serial.println(calcPitch);
  delay(500);
 // lastLoopTime = millis();
}

So you say my code is wrong? I think I don't understand what you mean... Is there anybody who know how to program the complementary filter and could check if mine one is ok? And does anybody know how to calibrate the accelerometer?

Many of us know how to calibrate an accelerometer, and I pointed you to one method. Your method won't work.

In this article http://cache.freescale.com/files/sensors/doc/app_note/AN3447.pdf I can read:

AmeasZ = (ZCA-(Z1g_current-S))/S;

But when I do something like this, it's not working. Does anyone know why? I am sorry for maybe a "stupid" question, but I cannot get the acc calibration right. I know few peoples did it this way. Maybe if I were a boy I could understand it better, but I ask for help, because I'm just a blonde girl :blush: :stuck_out_tongue_closed_eyes: and I don't understand why in others codes this trick works, but not in mine.

I don't understand why in others codes this trick works, but not in mine

It is not a trick. The method has a sound mathematical basis. However, the method proposed by Freescale to estimate the axial offsets and scale factors is error-prone.

I suspect that your implementation does not work, because you have not followed all the recommended steps to estimate the scale and offset correctly.

I recommend following the Adafruit method, which would work better than the Freescale method, because it uses the measured acceleration due to gravity to correct all three axes for offset and scale factor errors.

If you want to do the best possible job, follow the method on this site.

One more question. Is that possible to use DMP calibration like this:
accelgyro.setXAccelOffset(ax_offset);
accelgyro.setYAccelOffset(ay_offset);
accelgyro.setZAccelOffset(az_offset);

accelgyro.setYGyroOffset(gy_offset);

and later use complementary filter to calculate pitch?