I am trying to estimate position change using an MPU6050 IMU. I know that drift is a problem, but I am only trying to do this over short periods of less than a few seconds.
I've adjusted for gravity using sensor fusion madgwick filter and after adjustment my acceleration readings look good, but when I double integrate them, the resulting positions are very very small.
Any insight into what I might be doing wrong?
VectorFloat getAccelerationNoGravity() {
frame++;
auto timeSinceLastSample = ReadTime();
/**
* Get the current acceleration readings compensated for gravity
* 1. Get raw readings from acc & gyro
* 2. Feed into madgwick filter, get quaternion
* 3. Get gravity vector from quaternion
* 4. subtract gravity from raw acceleration readings
*/
// 1. Get raw readings from acc & gyro
sensors_event_t acc, gyro, temp;
mpu.getEvent(&acc, &gyro, &temp);
// 2. Feed into madgwick filter, get quaternion
float x = acc.acceleration.x;
float y = acc.acceleration.y;
float z = acc.acceleration.z;
float gx = gyro.gyro.x;
float gy = gyro.gyro.y;
float gz = gyro.gyro.z;
float deltat = timeSinceLastSample / 1000000.0;
madgwickQuaternionUpdate(q, x, y, z, gx, gy, gz, deltat);
Quaternion quat = Quaternion(q[0], q[1], q[2], q[3]);
// 3. Get gravity vector from quaternion
VectorFloat gravity = getGravity(&quat); // returns percentages of gravity
// 4. subtract gravity from raw acceleration readings
VectorFloat accAdj = getLinearAcceleration(VectorFloat(x, y, z), gravity);
// 5. calculate bias & adjust
calibrateBias(accAdj); // average of x samples
accAdj.x -= bias.x;
accAdj.y -= bias.y;
accAdj.z -= bias.z;
// calc position
auto accMagnitude = accAdj.getMagnitude();
if (biasComputed /*&& accMagnitude > 0.1*/) {
// v0 (initial velocity) = v
auto v0x = currVel.x;
auto v0y = currVel.y;
auto v0z = currVel.z;
// currVel (current velocity) = v0 + a * t
currVel.x = v0x + deltat * accAdj.x;
currVel.y = v0y + deltat * accAdj.y;
currVel.z = v0z + deltat * accAdj.z;
// delta_x = v0 * t + 1/2 * a * t^2 * 100 (m -> cm)
float deltat_sq = deltat * deltat;
currPos.x += v0x * deltat + 0.5 * accAdj.x * deltat_sq * 100;
currPos.y += v0y * deltat + 0.5 * accAdj.y * deltat_sq * 100;
currPos.z += v0z * deltat + 0.5 * accAdj.z * deltat_sq * 100;
}
t_compute = micros() - t_compute;
if (biasComputed && frame % 50 == 0) {
Serial.printf("%f,%f,%f\n", accAdj.x, accAdj.y, accAdj.z);
}
return accAdj;
}
This results in readings like this
0.117377,0.135253,0.010953
0.117308,0.133007,0.010974
0.117446,0.129459,0.010982
0.117550,0.125331,0.010972
0.117732,0.120880,0.010971
0.117961,0.115567,0.011018
0.118101,0.111308,0.011070
0.118161,0.112330,0.011132
0.118256,0.114275,0.011229
0.118401,0.116992,0.011322
but I am moving the IMU more than 50 cm so it seems the result is orders of magnitude off.
Again, my adjusted accelerometer readings make sense so perhaps I made a mistake implementing the kinematics?
