iphone - 使用加速度计、陀螺仪和设备运动查找距离，也使用双重积分

Question

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

我已尝试基于此链接1和链接2 的以下代码进行双重集成。

 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 清除值给出 100% 正确的结果。在这里，我们在对其应用过滤器后使用 x 加速度值。我们使用高通滤波器来减少 x 加速度的噪声。

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

任何帮助将不胜感激。