Dead Reckoning: Obtaining Position Estimation from Accelerometer Acceleration Integration

Question

Good day,

I have been reading papers about position integration from accelerometer readings.

I have consulted this paper from freescale on how that is achievable and this article regarding leaky integrators to help in preventing accumulation of errors from integration.

I was testing this algorithm by moving the imu by approximately 0.1 meter. The algorithm does get it right at the instant it arrives at approx 0.1 meter however when left still at that position, the integrated position goes to zero.

It turns out the velocity readings become negative at a certain period after reaching 0.1 meters.

Does anyone have any suggestions in dealing with this error?

Plots (Red is the position, Blue is the velocity.)

The imu(accelerometer) was moved alternating positions 0 meters and 0.1 meters with a stop of approximately 3-5 seconds in between before moving to the next position

1. Actual Data

2. Desired Data output (Green - Desired position integration)

Code:

// Get acceleration per axis
float AccX = accelmagAngleArray.AccX;
float AccY = accelmagAngleArray.AccY;
float AccZ = accelmagAngleArray.AccZ;

AccX -= dc_offsetX;
AccY -= dc_offsetY;
AccZ -= dc_offsetZ;

//Calculate Current Velocity (m/s)
float leakRateAcc = 0.99000;
velCurrX = velCurrX*leakRateAcc + ( prevAccX + (AccX-prevAccX)/2 ) * deltaTime2;
velCurrY = velCurrY*leakRateAcc + ( prevAccY + (AccY-prevAccY)/2 ) * deltaTime2;
velCurrZ = velCurrZ*0.99000 + ( prevAccZ + (AccZ-prevAccZ)/2 ) * deltaTime2;
prevAccX = AccX;
prevAccY = AccY;
prevAccZ = AccZ;

//Discrimination window for Acceleration
if ((0.12 > AccX) && (AccX > -0.12)){
  AccX = 0;
}

if ((0.12 > AccY) && (AccY > -0.12)){
  AccY = 0;
}

//Count number of times acceleration is equal to zero to drive velocity to zero when acceleration is "zero"
//X-axis---------------
if (AccX == 0){ //Increment no of times AccX is = to 0
    counterAccX++;    
}
else{ //Reset counter
    counterAccX = 0;
}

if (counterAccX>25){ //Drive Velocity to Zero
velCurrX = 0;
    prevVelX = 0;
    counterAccX = 0;
}

//Y-axis--------------
if (AccY == 0){ //Increment no of times AccY is = to 0
    counterAccY++;    
}
else{ //Reset counter
    counterAccY = 0;
}

if (counterAccY>25){ //Drive Velocity to Zero
    velCurrY = 0;
    prevVelY = 0;
    counterAccY = 0;
}

//Print Acceleration and Velocity
cout << " AccX = " << AccX ;// << endl;
cout << " AccY = " << AccY ;// << endl;
cout << " AccZ = " << AccZ << endl;

cout << " velCurrX = " << velCurrX ;// << endl;
cout << " velCurrY = " << velCurrY ;// << endl;
cout << " velCurrZ = " << velCurrZ << endl;

//Calculate Current Position in Meters
float leakRateVel = 0.99000;
posCurrX = posCurrX*leakRateVel + ( prevVelX + (velCurrX-prevVelX)/2 ) * deltaTime2;
posCurrY = posCurrY*leakRateVel + ( prevVelY + (velCurrY-prevVelY)/2 ) * deltaTime2;
posCurrZ = posCurrZ*0.99000 + ( prevVelZ + (velCurrZ-prevVelZ)/2 ) * deltaTime2;
prevVelX = velCurrX;
prevVelY = velCurrY;
prevVelZ = velCurrZ;

//Print X and Y position in meters
cout << " posCurrX = " << posCurrX ;// << endl;
cout << " posCurrY = " << posCurrY ;// << endl;
cout << " posCurrZ = " << posCurrZ << endl;

Answer 1

I am now able to get position from the accelerometer. Plot (Green - Acc, Blue -Vel, Red - Pos)

I was able to get the propper values through disabling the leaky integrator (setting the factor from 0.99 to 1).