2

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?

enter image description here


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.
Dunkelkoon
  • 398
  • 2
  • 10
  • Do you have the equations you are trying to implement somewhere? It is easier to check whether the code is correct when comparing with the equations. – Edgar H Apr 10 '18 at 07:45
  • 1
    @EdgarH I added a link to wikipedia, where I got the equations for velocity verlet. The rest is self made, since I could not find a known solution yet. This seems to be like such a standard problem, though. There must be an answer out there ... somewhere. – Dunkelkoon Apr 10 '18 at 10:08

1 Answers1

2

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')

enter image description here

Dunkelkoon
  • 398
  • 2
  • 10
  • I really think you should accept this as the answer. The error in the first solution is from ommission of higher order terms in the kinematic equation. This is exactly what you add in your solution through the term dt*(a + aNext)/2. I don't see how it could be done much cleaner. – Mats Lind Apr 13 '18 at 09:16
  • @MatsLind I really hoped to find a standard solution, though. The `dt*(a+aNext)/2` part is in my initial solution too. The main difference really is the direction used in the `rotate` part. – Dunkelkoon Apr 13 '18 at 12:26