def     Show_Position_Trajectory(argv):
    gnss = argv[0]

    t, x, y, z, sigma_x, sigma_y, sigma_z = Load_GNSS_Dataset(gnss)


    #fig, (ax1, ax2, ax3, ax4) = plt.subplots(4)
    fig, axs = plt.subplots(2, 2)
    axs[0,0].plot(x, y, '-', color='red', label='GPS')
    axs[0,0].set_title('WGS84 latitude,longitude')
    axs[0,0].set(xlabel='E(degree)', ylabel='N(degree)')

    axs[0,1].plot(t, sigma_x, '-', color='red', label='x axis sigma')
    axs[0,1].set_title('GNSS x sigma[ENU]')
    axs[0,1].set(xlabel='t(s)', ylabel='m')

    axs[1,0].plot(t, sigma_y, '-', color='green', label='y axis sigma')
    axs[1,0].set_title('GNSS y sigma[ENU]')
    axs[1,0].set(xlabel='t(s)', ylabel='m')

    axs[1,1].plot(t, sigma_z, '-', color='blue', label='z axis sigma')
    axs[1,1].set_title('GNSS z sigma[ENU]')
    axs[1,1].set(xlabel='t(s)', ylabel='m')


    plt.legend()
    plt.show()
    return






def     Show_IMU_Data(argv):
    imu = argv[0]

    t, gyro_x, gyro_y, gyro_z, acce_x, acce_y, acce_z = Load_IMU_Dataset(imu)


    # gyro
    fig, axs = plt.subplots(3, 2)
    axs[0,0].plot(t, gyro_x, '-', color='red', label='Gyro-x')
    axs[0,0].set_title('Gyro')
    axs[0,0].set(xlabel='t(s)', ylabel='ang(rad/s)')
    axs[0,0].grid()

    axs[0,0].legend()


    axs[1,0].plot(t, gyro_y, '-', color='green', label='Gyro-y')
    axs[1,0].set_title('Gyro')
    axs[1,0].set(xlabel='t(s)', ylabel='ang(rad/s)')

    axs[1,0].grid()

    axs[1,0].legend()


    axs[2,0].plot(t, gyro_z, '-', color='blue', label='Gyro-z')
    axs[2,0].set_title('Gyro')
    axs[2,0].set(xlabel='t(s)', ylabel='ang(rad/s)')
  axs[2,0].grid()

    axs[2,0].legend()



    # acce
    axs[0,1].plot(t, acce_x, '-', color='red', label='Acce-x')
    axs[0,1].set_title('Acce')
    axs[0,1].set(xlabel='t(s)', ylabel='acc(m/s^2)')

    axs[0,1].grid()

    axs[0,1].legend()


    axs[1,1].plot(t, acce_y, '-', color='blue', label='Acce-y')
    axs[1,1].set_title('Acce')
    axs[1,1].set(xlabel='t(s)', ylabel='acc(m/s^2)')

    axs[1,1].grid()

    axs[1,1].legend()


    axs[2,1].plot(t, acce_z, '-', color='blue', label='Acce-z')
    axs[2,1].set_title('Acce')
    axs[2,1].set(xlabel='t(s)', ylabel='acc(m/s^2)')
    axs[2,1].grid()

    axs[2,1].legend()



    plt.legend()
    plt.show()
    return






def     Compare_GNSS_Trajectory(argv):
    gnss_gt  = argv[0]
    gnss_ekf = argv[1]

    tt, xt, yt, zt = Load_GNSS_Dataset(gnss_gt)
    t, X, Y, Z = Load_GNSS_Dataset(gnss_ekf)
    plt.plot(yt, xt, '-', color='blue', label='Ground Truth')

    #plt.scatter(Y, X, color='dodgerblue', label='ESKF')
    plt.plot(Y, X, '-', color='red', label='EKF/ESKF')

    fig, axs = plt.subplots(2, 1)
    axs[0].plot(tt, xt, '-', color='blue', label='Ground Truth')
    axs[0].plot(t, X, '-', color='red', label='EKF/ESKF')
    axs[0].set_title('Position[East]')
    axs[0].set(xlabel='t(s)', ylabel='East(m)')


    axs[1].plot(tt, yt, '-', color='blue', label='Ground Truth')
    axs[1].plot(t, Y, '-', color='red', label='EKF/ESKF')
    axs[1].set_title('Position[North]')
    axs[1].set(xlabel='t(s)', ylabel='North(m)')

    plt.legend()
    plt.show()
    return












标签: none

评论已关闭