1

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.

enter image description here

Jason Kleban
  • 20,024
  • 18
  • 75
  • 125
  • 1
    I wonder, is it OK to use milliseconds in the calculation, while the units of acceleration are (as you say) m/s^2? – kikon Apr 24 '23 at 12:22
  • 1
    Newtonian mechanics is the only algorithm you need. Any approach that isn't based on Newton's equations won't be correct. "Hundreds of feet of displacement" suggests to me that you have a problem with inconsistent units somewhere. – duffymo Apr 24 '23 at 12:55
  • 1
    "swell/bobbing height" make me think this is a buoy that is sending out its acceleration data in its local coordinate frame. Feels like a damped spring mass system where the external forces are buoyancy and gravity. The easiest approach would be to assume that buoyancy and gravity are equal and opposite. I'd integrate the three acceleration components you get forward in time and see where they take you. You should be choosing your time step to be less than or equal to the sampling rate from your accelerometer. You don't say what the sampling rate is. – duffymo Apr 24 '23 at 16:36
  • sampling rate can be as often as 5 samples per second. Also, a note that I'm only interested in altitude changes. – Jason Kleban Apr 24 '23 at 16:43

0 Answers0