I am trying to integrate instantaneous linear and angular acceleration taken at discrete time intervals (1/100 seconds, 100hz). data is collected beforehand, and integration is done offline, I only have access to these measurements at these discretized intervals, I have been trying to integrate these accelerations to reconstruct the original trajectory (position .ie x, y, z coordinates), but even at 100hz(data logging rate) there is quite a bit of deviation from the original trajectory, I have been using SEMI-IMPLICIT Euler for doing the integration.
and so I was wondering if there is any way to improve the accuracy of the integrated trajectory WITHOUT going significantly higher than 100hz (logging rate)?
note: measurements are noiseless (simulator ground truth data), logging rate has a std. deviation of 5 Hz
dataset consist of inst. angular accelerations, and inst. linear accelerations in body frame. i have access to initial orientation, velocity, position.
def integratePosition(initialPosition, initialOrientation, initialLinearVelocityBody, initialAngularVelocity, linearAccelerationsBody, angularAccelerations, frequency): initialLinearVelocityEarth = transformToEarthFrame(initialLinearVelocityBody, eulerToQuaternion(*initialOrientation)) for a, alpha, f in zip(linearAccelerationsBody, angularAccelerations, frequency): initialAngularVelocity = integrateAngularVelocity(initialAngularVelocity, alpha, f) initialOrientation = integrateOrientation(initialOrientation, initialAngularVelocity, f) initialLinearVelocityEarth += transformToEarthFrame(a, eulerToQuaternion(*initialOrientation)) / f initialPosition = integratePosition(initialPosition, initialLinearVelocityEarth, f) yield initialPosition