Estimating displacement from double integral of acceleration samples

Question

I have acceleration samples from an inertial measurement unit in m/s2. I also have a gravity vector orthogonal to earth. I want to calculate the approximate displacement range (swell/bobbing height).

Is my approach right? First I take the dot product of the acceleration vector against the normalized gravity vector to get samples of the vertical acceleration regardless of the IMU orientation.

Each acceleration sample and delta-T in milliseconds gets added to a cumulative "displacement envelope calculation". Maybe there's a better name for that - It's cumulative-trapezoid twice plus decay plus peak-finding. I am intending to compute the running double integral of the acceleration by holding one previous acceleration value and accumulating velocity and then displacement. I have an envelope min and max of the displacement and I intend to decay the values over 30 seconds so that over time the average displacement is assumed to be zero to counter any accumulated error from this dead-reckoning approach AND that the algorithm will catch up to calmer (sea) conditions. 30 seconds assumes that wave periods do not exceed 30 seconds.

This calculation is to be done on a microcontroller in micropython, so I want to minimize memory allocations and code storage size. Therefore I'm trying to compute this without the likes of scipy and with just a few scalars rather than retaining large numbers of samples.

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)
        
        #print('delta {:+05.1f} decay {:+0.4f}'.format(deltaMS, dampenFactor))

        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

...


    gravityVec = imu.gravity()
    gravityMag = math.sqrt(gravityVec[0] * gravityVec[0] + gravityVec[1] * gravityVec[1] + gravityVec[2] * gravityVec[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 # Meters to Feet

I don't get accurate readings from this code - I quickly get outputs of hundreds of feet displacement. It could merely be a problem of scale/unit conversion or something else. I could be making many mistakes in many places, so I'm looking for some guidance please. Thanks!

UPDATE

Jumping through multiple hoops, I was able to capture these samples of the IMU at rest toward designing a low pass filter as seems to be recommended/necessary to avoid drift. Not quite sure how to interpret the nufft output yet into a cutoff frequency.