2022 IB Diploma Extended Essays

i += 1 ax.plot(x, y, color=color, linewidth=linewidth, label=str(i)) ax.grid(True) ax.legend() def printToFile(f,t,data): for i in range(len(data)): f.write("{:.9f} {:.9f}\n".format(t[i],data[i])) def printToFile2(f, t, x, y, yaw): for i in range(len(t)): f.write("{:.9f} {:.9f} {:.9f} {:.9f}\n".format(t[i], x[i], y[i], yaw[i])) def denormalize(data):

repeat = True while (repeat):

repeat = False for i in range(len(data) - 1): if (data[i + 1] - data[i] > 2): repeat = True for j in range(len(data)): if j > i:

data[j] -= 2 * 3.14159265

repeat = True while (repeat):

repeat = False for i in range(len(data) - 1): if (data[i + 1] - data[i] < -2): repeat = True for j in range(len(data)): if j > i:

data[j] += 2 * 3.14159265 # get an instance of RosPack with the default search paths rospack = rospkg.RosPack()

# get the file path for sensor_fusion rospack.get_path('sensor_fusion') path = rospack.get_path('sensor_fusion') + '/dataTxt/laps/' data_pose = np.loadtxt(path + "pose_position.txt", delimiter=' ', dtype=np.float) data_lidar_odom = np.loadtxt(path + "pose_orientation.txt", delimiter=' ', dtype=np.float) colors = ['red', 'blue', 'green']

t_pose = data_pose[:, 0] x_pose = data_pose[:, 1] y_pose = data_pose[:, 2] yaw_pose = data_lidar_odom[:, 1]

t = t_pose x = yaw_pose

dx = np.zeros(x.shape, np.float) dx[0:-1] = np.diff(x) / np.diff(t)