根据加速度计数据计算位置

请帮助我完成以下任务。我有MPU 9150，可以从中获得加速度/陀螺仪和磁力计数据。我目前感兴趣的是获取机器人的方向和位置。我可以使用四元数来获得位置。它相当稳定。静止不动时很少发生变化。
但是问题在于转换加速度计数据以计算位移。

我知道需要整合两次加速度。数据以获取位置。
使用四元数，我可以旋转加速度矢量，然后将其轴求和以获取速度，然后再次进行相同操作以获取位置。但这不是那样的。首先，将传感器移到某个位置，然后再移回去，不会像以前那样给我相同的位置。问题是，尽管传感器传来的加速度数据为零，但当我将传感器放回原处并保持静止不动时，速度不会变为零。

这里是一个示例（最初是类似this）：
重力：-0.10 -0.00 1.00
原始加速度：-785 -28 8135
缩放至+ -g后加速：-0.10 -0.00 0.99
结果使用四元数旋转加速度矢量后：0.00 -0.00 -0.00

移动传感器并将其放回后，其加速度变为：
0.00 -0.00 -0.01
0.00 -0.00- 0.01
0.00 -0.00 -0.00
0.00 -0.00 -0.01
依此类推。
如果我对其进行积分，则Z的位置会逐渐增大。

但最严重的问题是速度不会恢复为零

例如，如果我一次移动传感器并将其放回原处，速度将为：
-0.089对于vx，对于
0.15对于vy

经过几次这样的移动后，它变为：
1.22对于vx
1.08对于vy
-8.63对于vz

，接着又是这样的运动：

vx -1.43
vy 1.23
vz -9.7

如果传感器不移动但Z缓慢变化，则x和y不变。
尽管四元数根本没有改变。

执行该任务的正确方法应该是什么？

这是集成代码的一部分：

vX += wX * speed;
vY += wY * speed;
vZ += wZ * speed;

posX += vX * speed;
posY += vY * speed;
posZ += vZ * speed;

当前将速度设置为1只是为了测试其工作原理。

编辑1：这是用于检索四元数和加速数据，旋转并补偿重力的代码并获取最终的加速度数据。

        // display initial world-frame acceleration, adjusted to remove gravity
        // and rotated based on known orientation from quaternion
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetAccel(&aaReal, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);

        //Serial.print("gravity\t");
        Serial.print(gravity.x);
        Serial.print("\t");
        Serial.print(gravity.y);
        Serial.print("\t");
        Serial.print(gravity.z);
        Serial.print("\t");


        //Serial.print("accell\t");
        Serial.print(aaReal.x);
        Serial.print("\t");
        Serial.print(aaReal.y);
        Serial.print("\t");
        Serial.print(aaReal.z);
        Serial.print("\t"); 

        float val = 4.0f;
        float ax = val * (float)aaReal.x / 32768.0f;
        float ay = val * (float)aaReal.y / 32768.0f;
        float az = val * (float)aaReal.z / 32768.0f; 

        theWorldF.x = ax;            
        theWorldF.y = ay;
        theWorldF.z = az;

        //Serial.print("scaled_accel\t");
        Serial.print(ax);
        Serial.print("\t");
        Serial.print(ay);
        Serial.print("\t");
        Serial.print(az);
        Serial.print("\t"); 

        theWorldF.x -= gravity.x;
        theWorldF.y -= gravity.y;
        theWorldF.z -= gravity.z;

        theWorldF.rotate(&q);
        //gravity.rotate(&q);
        //Serial.print("gravity_compensated_accel\t");
        Serial.print(theWorldF.x);
        Serial.print("\t");
        Serial.print(theWorldF.y);
        Serial.print("\t");
        Serial.print(theWorldF.z);
        Serial.print("\t");
        Serial.print(deltaTime); 
        Serial.println();

编辑2：


dmpGetQuaternion，dmpGetAccel函数只是从中读取MPU的FIFO缓冲区。

dmpGetGravity是：

uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
    v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
    v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
    v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
    return 0;
}

编辑3：
使用MPU 9150的库：
https：// github.com/sparkfun/MPU-9150_Breakout

编辑4：另一个示例

重力矢量：-1.00 -0.02 0.02
原始加速度数据：-8459 -141 125
加速度数据刻度（+ -2g范围）：-1.03 -0.02 0.02
重力补偿和加速度数据旋转：-0.01 0.00 0.33