我正在尝试实现以下图像：
其中红色是ground truth circle（车辆在一个圆圈内行驶），绿色是Integration Drift（积分时出现的数值错误）。
我从绘制圆圈开始，并且在使用 python 绘制积分漂移时遇到了困难。
注意：我正在尝试在某些地方使用 scipy 的 integration.cumtrapz 而不是乘法，但这对我没有帮助。
这些是我认为应该添加和工作的地方：
psie = 0 + integrate.cumtrapz(psidote, t); # rad
xA1id = R0 + integrate.cumtrapz(0 + integrate.cumtrapz(xAddot1e, t), t); # m
xA2id = 0 + integrate.cumtrapz(R0*omega0 + integrate.cumtrapz(xAddot2e, t), t); # m
我相信代码中唯一的问题是变量xA1id，xA2id到目前为止，一切看起来都很好。
目前的结果是：
我写的代码是：
import numpy as np
from scipy import integrate
import matplotlib.pyplot as plt
R0=5
omega0= np.pi
dt=0.001
N=1
te=np.arange(0,np.dot(2,N),dt)
psie=np.dot(omega0,te)
xA1e=np.dot(R0,np.cos(psie))
xA2e=np.dot(R0,np.sin(psie))
# circle plot
plt.plot(xA1e, xA2e, color='green', label='x-axis')
plt.show()
# Integration drift:
dt=0.01
N=100
t=np.arange(0,np.dot(2,N),dt)
# (1) Noise-free IMU "data":
psidote=np.dot(omega0,np.array([1]*len(t)))
a1e=np.dot((np.dot(- R0,omega0 ** 2)),np.array([1]*len(t)))
a2e=np.dot(0,a1e)
psie= t*psidote
xAddot1e=np.cos(psie)*a1e - np.sin(psie)*a2e
xAddot2e=np.sin(psie)*a1e +np.cos(psie)*a2e
xA1id=R0 + t* (t*xAddot1e)
xA2id= 0+t*((R0*omega0) + t* xAddot2e)
plt.plot(xA1id, xA2id, color='green', label='x-axis')
plt.show()
方程来自：
