python - 如何可视化绘图中的积分漂移？

Question

我正在尝试实现以下图像：

其中红色是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()

方程来自：

标签： pythonimu

Answer 1

解决方案：

psie= t*psidote


xAddot1e=np.cos(psie)*a1e - np.sin(psie)*a2e

xAddot2e=np.sin(psie)*a1e +np.cos(psie)*a2e

xA1id=np.concatenate(([5], R0 + integrate.cumtrapz(np.concatenate((np.array([0]), integrate.cumtrapz(xAddot1e, t)), axis=0), t)), axis=0)
xA2id= np.concatenate(([0, 0],integrate.cumtrapz(((R0 * omega0) + integrate.cumtrapz(xAddot2e, t)), t[:19999])), axis=0)

结果：