def animate(step):
global xTrue, relativeState
u = data.calcInput_FlyIn1m(step) #u=xEsti
# u = data.calcInput_PotentialField(step, xTrue)
# u = data.calcInput_Formation01(step, relativeState)
#uNois 为输入的速度和yaw
xTrue, zNois, uNois = data.update(
xTrue, u
) ## Calculate the updated groundTruth(xTrue), noised observation(zNoise), and noised input(uNoise)
if step % ekfStride == 0:
relativeState = relativeEKF.EKF(uNois, zNois, relativeState,
ekfStride) #EKF
#利用relativeState计算绝对坐标以方便显示
xEsti = transform.calcAbsPosUseRelaPosWRTRob0(
xTrue[:, 0], relativeState, xTrue,
numRob) #relativeState 就是xij, yij, yawij
pointsTrue.set_data(xTrue[0, :], xTrue[1, :]) # plot groundTruth points
pointsEsti.set_data(xEsti[0, :], xEsti[1, :]) # plot estimated points
pointsTrueHead.set_data(xTrue[0, :] + 0.07 * np.cos(xTrue[2, :]),
xTrue[1, :] +
0.07 * np.sin(xTrue[2, :])) # heading
pointsEstiHead.set_data(xEsti[0, :] + 0.07 * np.cos(xEsti[2, :]),
xEsti[1, :] +
0.07 * np.sin(xEsti[2, :])) # heading
circle.center = (xTrue[0, 0], xTrue[1, 0])
circle.radius = zNois[
0, 1] # plot a circle to show the distance between robot 0 and robot 1
time_text.set_text("t={:.2f}s".format(step * dt))
return pointsTrue, pointsEsti, circle, pointsTrueHead, pointsEstiHead, time_text
def animate(step):
global relativeState
xTrue, zNois, uNois = dataFromReal.calcInputDataset(uList[step], zList[step], GtList[step])
relativeState = relaEKFonRealData.EKF(uNois, zNois, relativeState)
xEsti = transform.calcAbsPosUseRelaPosWRTRob0(xTrue[:,0], relativeState, xTrue, numRob)
pointsTrue.set_data(xTrue[0, :], xTrue[1, :])
pointsEsti.set_data(xEsti[0, :], xEsti[1, :])
pointsTrueHead.set_data(xTrue[0, :]+0.07*np.cos(xTrue[2, :]), xTrue[1, :]+0.07*np.sin(xTrue[2, :]))
pointsEstiHead.set_data(xEsti[0, :]+0.07*np.cos(xEsti[2, :]), xEsti[1, :]+0.07*np.sin(xEsti[2, :]))
circle.center = (xTrue[0, 0], xTrue[1, 0])
circle.radius = zNois[0, 1]
time_text.set_text("t={:.2f}s".format(step * dt))
return pointsTrue, pointsEsti, circle, pointsTrueHead, pointsEstiHead, time_text
def animate(step):
global xTrue, relativeState, xEsti
u = data.calcInput_FlyIn1m(step) #
# xTrue[0:2,0] = 0
# u[:,:] = 0
xTrue, zNois, uNois = data.update2(xTrue, u)
if step == 0:
relativeState = relativeEKF.EKF(uNois, zNois, relativeState, ekfStride)
xEsti = transform.calcAbsPosUseRelaPosWRTRob0(xTrue[:,
0], relativeState,
xTrue, numRob)
# xEsti[0:2,:] = -8
# xEsti = xTrue
if step % ekfStride == 0:
# xEsti = estiEKF.nEKF(uNois, zNois, xEsti, ekfStride, 0)
xEsti[0:2, 0] = xTrue[0:2, 0]
xEsti = estiEKF.nEKF(uNois, zNois, xEsti, ekfStride, 1)
# xEsti[0:2, 1] = xTrue[0:2, 1]
xEsti = estiEKF.nEKF(uNois, zNois, xEsti, ekfStride, 2)
# xEsti[0:2, 2] = xTrue[0:2, 2]
xEsti = estiEKF.nEKF(uNois, zNois, xEsti, ekfStride, 3)
xEsti = estiEKF.nEKF(uNois, zNois, xEsti, ekfStride, 4)
xEsti = estiEKF.nEKF(uNois, zNois, xEsti, ekfStride, 5)
xEsti = estiEKF.nEKF(uNois, zNois, xEsti, ekfStride, 6)
xEsti = estiEKF.nEKF(uNois, zNois, xEsti, ekfStride, 7)
xEsti = estiEKF.nEKF(uNois, zNois, xEsti, ekfStride, 8)
xEsti = estiEKF.nEKF(uNois, zNois, xEsti, ekfStride, 9)
xEsti = estiEKF.nEKF(uNois, zNois, xEsti, ekfStride, 10)
pointsTrue.set_data(xTrue[0, :], xTrue[1, :]) # plot groundTruth points
pointsEsti.set_data(xEsti[0, :], xEsti[1, :]) # plot estimated points
pointsTrueHead.set_data(xTrue[0, :] + 0.07 * np.cos(xTrue[2, :]),
xTrue[1, :] +
0.07 * np.sin(xTrue[2, :])) # heading
pointsEstiHead.set_data(xEsti[0, :] + 0.07 * np.cos(xEsti[2, :]),
xEsti[1, :] +
0.07 * np.sin(xEsti[2, :])) # heading
circle.center = (xTrue[0, 0], xTrue[1, 0])
circle.radius = zNois[
0, 1] # plot a circle to show the distance between robot 0 and robot 1
time_text.set_text("t={:.2f}s".format(step * dt))
return pointsTrue, pointsEsti, circle, pointsTrueHead, pointsEstiHead, time_text
ax.legend()
circle = plt.Circle((0, 0), 0.2, color='black', fill=False)
ax.add_patch(circle)
time_text = ax.text(0.01, 0.97, '', transform=ax.transAxes)
time_text.set_text('')
ani = animation.FuncAnimation(fig, animate, frames=None, interval=10, blit=True)
#ani.save('particle_box.mp4', fps=30, extra_args=['-vcodec', 'libx264'])
plt.show()
else:
pos01ekfVSgt = [[] for i in range(6)] # x, xGT, y, yGT, z, zGT
step = 0
while simTime >= dt*step:
step += 1
xTrue, zNois, uNois = dataFromReal.calcInputDataset(uList[step], zList[step], GtList[step])
relativeState = relaEKFonRealData.EKF(uNois, zNois, relativeState)
xEsti = transform.calcAbsPosUseRelaPosWRTRob0(xTrue[:,0], relativeState, xTrue, numRob)
cmpWhichRob = 1 # show position of cmpWhichRob calculated by rob0 and relativeState
pos01ekfVSgt[0].append(xEsti[0,cmpWhichRob])
pos01ekfVSgt[1].append(xTrue[0,cmpWhichRob])
pos01ekfVSgt[2].append(xEsti[1,cmpWhichRob])
pos01ekfVSgt[3].append(xTrue[1,cmpWhichRob])
pos01ekfVSgt[4].append(xEsti[2,cmpWhichRob])
pos01ekfVSgt[5].append(xTrue[2,cmpWhichRob])
pos01array = np.array(pos01ekfVSgt)
timePlot = np.arange(0, len(pos01ekfVSgt[0]))/100
f, (ax1, ax2, ax3) = plt.subplots(3, sharex=True)
plt.margins(x=0)
ax1.plot(timePlot, pos01array[0,:])
ax1.plot(timePlot, pos01array[1,:])
ax1.set_ylabel("x (m)")
ax1.grid(True)