How to integrate object space acceleration to world space position (2D)

Question

I want to double integrate 2D acceleration data in object coordinates to get 2D position in world coordinates. The object always points in the direction of velocity (assume e.g. a train).

So I tried to numerically integrate the acceleration values with velocity verlet integration, changing the direction at each step to the previous velocity in world coordinates, provided by the velocity verlet algorithm:

import numpy as np
from math import sqrt
from matplotlib import pyplot as plt

def rotate(a, newXAxis):
    r = newXAxis
    normX = r / sqrt(np.dot(r.T,r))
    normY = [-normX[1], normX[0]]
    b = np.dot(np.array([normX, normY]).T, a)
    return(b)

"""return true if v > 1 km/h or any speed given"""
def isMoving(deltaXPosition, deltaYPosition, deltaTime, fasterThankmh=1.0):
    x = deltaXPosition
    y = deltaYPosition
    t = deltaTime
    if t*t == 0.:
        return False
    if hasattr(x, "__len__"):
        x = x[0]
    if hasattr(y, "__len__"):
        y = y[0]
    if hasattr(t, "__len__"):
        t = t[0]
    speed = float(fasterThankmh)
    return((x*x + y*y) / (t*t) > 0.077160*speed*speed)

def velocity_verlet_integration(Xacc, Yacc,
                                x0=0., y0=0.,
                                vx_0=0, vy_0=0,
                                forward=np.array([1.0, 0.0])):
    vx = np.zeros(len(Xacc))
    vy = np.zeros(len(Xacc))
    x = np.zeros(len(Xacc))
    y = np.zeros(len(Xacc))
    x[0] = x0
    y[0] = y0
    vx[0] = vx_0
    vy[0] = vy_0
    for i in range(len(Xacc)-1):
        dt = Xacc[i+1]-Xacc[i]
        a = rotate(Yacc[i,:], forward)
        x[i+1] = x[i] + vx[i]*dt + 1.0/2.0*a[0]*dt*dt
        y[i+1] = y[i] + vy[i]*dt + 1.0/2.0*a[1]*dt*dt
        if isMoving(x[i+1]-x[i], y[i+1]-y[i], dt):
            forward = np.array([x[i+1]-x[i], y[i+1]-y[i]])
        aNext = rotate(Yacc[i+1,:], forward)
        vx[i+1] = vx[i] + dt*(a[0] + aNext[0])/2
        vy[i+1] = vy[i] + dt*(a[1] + aNext[1])/2
    return x, y

Testing this with a simple circular motion with:

"""test circle"""
centripetal=-0.2
N = 0.01
xCircle = np.array(range(int(100*10**N)))/float(10**N)
yCircle = np.array([[0.0, centripetal] for i in xCircle])
xvvi, yvvi = velocity_verlet_integration(xCircle, yCircle, 0., 0., 1., 0.)
#plot it
plt.plot(xvvi, yvvi, ".-", label='position with "velocity verlet" integration')

This results in a drift outwards, because the current direction is based on the last velocity, which is obviously a bad approximation.

Can anyone point me to a better solution?

---

Some thoughts:

• Optimally, one would need the last and the next velocity in world coordinates to make a better approximation, by averaging (e.g. adding) those. But in my approach, the next velocity depends on the next acceleration in world coordinates, which in turn needs the next orientation (chasing its own tail).
• If I use my approach to get a first approximation of the next velocity and thus the next direction, I may use this to improve the current direction by the idea above. Now I can make a better approximation about the next velocity and the next direction and again use it to improve the current direction. This might be a possible solution, though it seems really ugly.

Answer 1

Based on my thoughts (at the end of my question) I added an uggly solution, so I am not going to accept it as an answer.

def my_integration(t, a_object,
                   x0=0., y0=0.,
                   vx_0=0, vy_0=0,
                   forward=np.array([1.0, 0.0])):
    v = np.zeros((len(t), 2))
    p = np.zeros((len(t), 2))
    p[0,:] = np.array([x0, y0])
    v[0,:] = np.array([vx_0, vy_0])
    v[1,:] = np.array([vx_0, vy_0])
    for i in range(len(t)-1):
        """this feels like a hack"""
        for j in range(10):
            dt = t[i+1]-t[i]
            a     = rotate(a_object[i,:],   v[i,:]+v[i+1,:])
            p[i+1,:] = p[i,:] + v[i,:]*dt + 1.0/2.0*a*dt*dt
            aNext = rotate(a_object[i+1,:], v[i,:]+v[i+1,:])
            v[i+1,:] = v[i,:] + dt*(a + aNext)/2.
            if i < len(t)-2:
                v[i+2,:] = v[i+1,:]
    return p

And for the plot, adding this:

plt.plot(np.cos(pi*2*np.array(range(21))/20)/centripetal,
        (np.sin(pi*2*np.array(range(21))/20)+1)/centripetal,
        "x", label='ground truth')
myi = my_integration(t, a, 0., 0., 1., 0.)
plt.plot(myi[:,0], myi[:,1], "--", label='position with my integration')
plt.legend(fontsize = 'x-small')