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)