使用accleremoter，陀螺仪和设备运动查找距离，并使用双重积分

Question

我想计算从一个点到另一个点的距离，当我开始捕获加速度计数据时，直到我停止它，它应该收集所有样本并且应该根据速度，滚动，俯仰，偏航和位移给出距离

我尝试使用基于此link1和link2的代码进行双重集成。

 for (int i = 1; i < [arrXData count]; i++)
 {
     //========================================================================
     //Old Velocity
     double v0 = [[arrVelocity objectAtIndex:(i-1)] doubleValue];

     //Time Data
     double t0 = [[arrTimeData objectAtIndex:(i-1)] doubleValue];
     double t1 = [[arrTimeData objectAtIndex:(i)] doubleValue];

     //Acceleration Data
     double x0 = [[arrXData objectAtIndex:(i-1)] doubleValue];
     double x1 = [[arrXData objectAtIndex:(i)] doubleValue];

    //NewVelocity
    double v1 = v0 + (t1 - t0) * (x0 + x1)/2;
    //========================================================================

    //Old Distance
    double d0 = [[arrDistance objectAtIndex:(i-1)] doubleValue];

    //New Distance
    double d1 = d0 + (x1 - x0) * (v0 + v1)/2;
    //========================================================================
    //Store Distance & Velocity

    [arrVelocity addObject:[NSNumber numberWithDouble:v1]];
    [arrDistance addObject:[NSNumber numberWithDouble:d1]];
   //========================================================================
}

但是上面的代码有时会给出负值，并且不确定这是否会根据x acclearation值给出100％正确的结果。这里我们在对它应用过滤器后使用了x加速度值。我们使用高通滤波器来降低x加速度的噪声。

有谁知道我们可以依赖的任何算法？

任何帮助将不胜感激。