[英]How to integrate object space acceleration to world space position (2D)
I want to double integrate 2D acceleration data in object coordinates to get 2D position in world coordinates.我想对 object 坐标中的二维加速度数据进行双重积分,以获得世界坐标中的二维 position。 The object always points in the direction of velocity (assume eg a train).
object 始终指向速度方向(假设是火车)。
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:因此,我尝试将加速度值与速度 verlet积分进行数值积分,将每一步的方向更改为 velocity verlet 算法提供的世界坐标中的先前速度:
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?谁能指出我更好的解决方案?
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:对于 plot,添加以下内容:
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')
声明:本站的技术帖子网页,遵循CC BY-SA 4.0协议,如果您需要转载,请注明本站网址或者原文地址。任何问题请咨询:yoyou2525@163.com.