基于加速度采样二重积分估算位移：算法问题求助

拥有输出单位为m/s²的线性加速度采样数据及地球重力向量的IMU，需估算垂向位移范围（浪涌/颠簸高度）。核心思路如下：

1. 将加速度向量与归一化重力向量点积，得到与IMU朝向无关的垂向加速度
2. 通过梯形积分+衰减抑制+峰值检测计算位移包络：仅用少量标量存储中间状态（适配MicroPython内存限制），通过30秒衰减使位移均值回归0，抑制航位推算累积误差（假设浪周期≤30秒）

当前代码输出结果异常，短时间内出现数百英尺的位移值，同时需解决：基于静止状态采样数据设计低通滤波时，如何解读NUFFT输出确定截止频率的问题。

import math

def getDisplacemntEnvelope(decayMS = 30 * 1000):
    prevAccel = 0.0
    accVelocity = 0.0
    accDisplacement = 0.0
    envMin = 0.0
    envMax = 0.0

    def step(currAccel, deltaMS):
        nonlocal prevAccel, accVelocity, accDisplacement, envMin, envMax
        
        deltaT = deltaMS / 1000.0
        
        dampenFactor = 0.9 # 注释：正确公式应为1.0 - (deltaMS / decayMS)
        
        # 梯形积分计算速度
        accVelocity += (prevAccel + currAccel) * deltaT * dampenFactor / 2.0
        # 位移积分（当前逻辑存在问题）
        accDisplacement += accVelocity * deltaT * (dampenFactor * dampenFactor) / 2.0
        prevAccel = currAccel
        
        print('currAccel  {:+08.4f} m/s2  accVelocity {:+05.1f} m/s accDisplacement {:+05.1f} m'
              .format(currAccel, accVelocity, accDisplacement))

        envMin = min(envMin * dampenFactor, accDisplacement)
        envMax = max(envMax * dampenFactor, accDisplacement)

        return envMax - envMin
    
    return step

# 主循环调用示例（伪代码）
# displacemntEnvelope = getDisplacemntEnvelope()
# while True:
#     deltaMS = 获取采样间隔（毫秒）
#     gravityVec = imu.gravity()
#     gravityMag = math.sqrt(gravityVec[0]**2 + gravityVec[1]**2 + gravityVec[2]**2)
#     gravityNormalized = (gravityVec[0]/gravityMag, gravityVec[1]/gravityMag, gravityVec[2]/gravityMag)
#     accelVec = imu.lin_acc()
#     # 计算垂向加速度
#     dotProduct = -(gravityNormalized[0]*accelVec[0] + gravityNormalized[1]*accelVec[1] + gravityNormalized[2]*accelVec[2])
#     # 计算位移范围并转英尺
#     displacementFt = displacemntEnvelope(dotProduct, deltaMS) * 3.28084

1. 衰减因子硬编码错误 #

当前代码将dampenFactor固定为0.9，完全偏离注释中的正确公式1.0 - (deltaMS / decayMS)，这是位移快速漂移的核心原因：

• 正确的衰减因子应随采样间隔动态计算，确保30秒内位移均值回归0
• 修正代码：

  dampenFactor = 1.0 - (deltaMS / decayMS)
  # 防止因子过小导致数值不稳定
  dampenFactor = max(dampenFactor, 0.99)
  

2. 位移积分逻辑错误 #

当前位移计算直接使用更新后的速度乘以时间，未采用梯形积分（速度的平均），导致积分误差快速累积：

• 需新增prevVelocity变量存储上一时刻速度，用梯形积分计算位移
• 修正后的积分逻辑：

  def getDisplacemntEnvelope(decayMS = 30 * 1000):
      prevAccel = 0.0
      prevVelocity = 0.0  # 新增：存储上一时刻速度
      accDisplacement = 0.0
      envMin = 0.0
      envMax = 0.0
  
      def step(currAccel, deltaMS):
          nonlocal prevAccel, prevVelocity, accDisplacement, envMin, envMax
          
          deltaT = deltaMS / 1000.0
          dampenFactor = max(1.0 - (deltaMS / decayMS), 0.99)
          
          # 梯形积分计算当前速度
          currVelocity = prevVelocity + (prevAccel + currAccel) * deltaT * dampenFactor / 2.0
          # 梯形积分计算位移
          accDisplacement += (prevVelocity + currVelocity) * deltaT * dampenFactor / 2.0
          
          # 更新状态变量
          prevAccel = currAccel
          prevVelocity = currVelocity
          
          # 更新包络
          envMin = min(envMin * dampenFactor, accDisplacement)
          envMax = max(envMax * dampenFactor, accDisplacement)
          
          return envMax - envMin
      return step
  

3. 垂向加速度符号验证 #

代码中对dotProduct添加了负号，需验证IMU坐标系是否匹配：

• 静止状态下，imu.lin_acc()的垂向分量应为0，此时dotProduct应接近0
• 若静止时dotProduct不为0，说明符号错误或IMU的lin_acc未正确去除重力分量，需调整符号或重新校准IMU

4. NUFFT输出解读与滤波截止频率确定 #

针对静止采样数据的NUFFT输出：

• 横坐标为频率（Hz），纵坐标为对应频率的信号幅值
• 静止状态下，理想的垂向加速度应仅含噪声，若存在极低频（接近0Hz）的峰值，即为漂移分量
• 截止频率需满足：略高于浪的最低频率（浪周期30秒对应频率≈0.033Hz），同时低于漂移分量的频率，建议设为0.05Hz，确保浪信号通过的同时过滤直流漂移

内容的提问来源于stack exchange，提问作者Jason Kleban