def add_imu_bias_tester_velocities(pdf, imu_bias_tester_velocities):
plt.figure()
plot_helpers.plot_vector3ds(imu_bias_tester_velocities.velocities, imu_bias_tester_velocities.times, 'Vel.')
plt.xlabel('Time (s)')
plt.ylabel('Velocities')
plt.title('IMU Bias Tester Velocities')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
def add_imu_bias_tester_velocities(pdf, imu_bias_tester_velocities):
plt.figure()
plot_helpers.plot_vector3ds(imu_bias_tester_velocities.velocities,
imu_bias_tester_velocities.times, "Vel.")
plt.xlabel("Time (s)")
plt.ylabel("Velocities")
plt.title("IMU Bias Tester Velocities")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
def plot_imu_measurements(pdf, imu_measurements, prefix=''):
# Acceleration
plt.figure()
plot_helpers.plot_vector3ds(imu_measurements.accelerations,
imu_measurements.times, 'Acc.')
plt.xlabel('Time (s)')
plt.ylabel('Acceleration (m/s^2)')
plt.title(prefix + 'Acceleration')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Angular Velocity
plt.figure()
plot_helpers.plot_vector3ds(imu_measurements.angular_velocities,
imu_measurements.times, 'Ang. Vel.')
plt.xlabel('Time (s)')
plt.ylabel('Angular Velocities')
plt.title(prefix + 'Angular Velocities')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
def plot_imu_measurements(pdf, imu_measurements, prefix=""):
# Acceleration
plt.figure()
plot_helpers.plot_vector3ds(imu_measurements.accelerations,
imu_measurements.times, "Acc.")
plt.xlabel("Time (s)")
plt.ylabel("Acceleration (m/s^2)")
plt.title(prefix + "Acceleration")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Angular Velocity
plt.figure()
plot_helpers.plot_vector3ds(imu_measurements.angular_velocities,
imu_measurements.times, "Ang. Vel.")
plt.xlabel("Time (s)")
plt.ylabel("Angular Velocities")
plt.title(prefix + "Angular Velocities")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
def add_graph_plots(pdf, sparse_mapping_poses, ar_tag_poses,
graph_localization_states,
imu_augmented_graph_localization_poses):
colors = ['r', 'b', 'g']
position_plotter = vector3d_plotter.Vector3dPlotter(
'Time (s)', 'Position (m)', 'Graph vs. Sparse Mapping Position', True)
position_plotter.add_pose_position(sparse_mapping_poses,
linestyle='None',
marker='o',
markeredgewidth=0.1,
markersize=1.5)
if ar_tag_poses.times:
position_plotter.add_pose_position(ar_tag_poses,
linestyle='None',
marker='x',
markeredgewidth=0.1,
markersize=1.5)
position_plotter.add_pose_position(graph_localization_states)
position_plotter.plot(pdf)
# orientations
orientation_plotter = vector3d_plotter.Vector3dPlotter(
'Time (s)', 'Orientation (deg)',
'Graph vs. Sparse Mapping Orientation', True)
orientation_plotter.add_pose_orientation(sparse_mapping_poses,
linestyle='None',
marker='o',
markeredgewidth=0.1,
markersize=1.5)
if ar_tag_poses.times:
orientation_plotter.add_pose_orientation(ar_tag_poses,
linestyle='None',
marker='x',
markeredgewidth=0.1,
markersize=1.5)
orientation_plotter.add_pose_orientation(graph_localization_states)
orientation_plotter.plot(pdf)
# Imu Augmented Loc vs. Loc
position_plotter = vector3d_plotter.Vector3dPlotter(
'Time (s)', 'Position (m)', 'Graph vs. IMU Augmented Graph Position',
True)
position_plotter.add_pose_position(graph_localization_states,
linestyle='None',
marker='o',
markeredgewidth=0.1,
markersize=1.5)
position_plotter.add_pose_position(imu_augmented_graph_localization_poses,
linewidth=0.5)
position_plotter.plot(pdf)
# orientations
orientation_plotter = vector3d_plotter.Vector3dPlotter(
'Time (s)', 'Orientation (deg)',
'Graph vs. IMU Augmented Graph Orientation', True)
orientation_plotter.add_pose_orientation(graph_localization_states,
marker='o',
markeredgewidth=0.1,
markersize=1.5)
orientation_plotter.add_pose_orientation(
imu_augmented_graph_localization_poses, linewidth=0.5)
orientation_plotter.plot(pdf)
# Velocity
plt.figure()
plot_helpers.plot_vector3ds(graph_localization_states.velocities,
graph_localization_states.times, 'Vel.')
plt.xlabel('Time (s)')
plt.ylabel('Velocities')
plt.title('Graph Velocities')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Integrated Velocities
position_plotter = vector3d_plotter.Vector3dPlotter(
'Time (s)', 'Position (m)',
'Integrated Graph Velocities vs. Sparse Mapping Position', True)
position_plotter.add_pose_position(sparse_mapping_poses,
linestyle='None',
marker='o',
markeredgewidth=0.1,
markersize=1.5)
if ar_tag_poses.times:
position_plotter.add_pose_position(ar_tag_poses,
linestyle='None',
marker='x',
markeredgewidth=0.1,
markersize=1.5)
integrated_graph_localization_states = utilities.integrate_velocities(
graph_localization_states)
position_plotter.add_pose_position(integrated_graph_localization_states)
position_plotter.plot(pdf)
def add_other_vector3d_plots(pdf, imu_augmented_graph_localization_states,
sparse_mapping_poses, ar_tag_poses):
colors = ['r', 'b', 'g']
# Acceleration
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.accelerations,
imu_augmented_graph_localization_states.times, 'Acc.')
plt.xlabel('Time (s)')
plt.ylabel('Acceleration (m/s^2)')
plt.title('Acceleration')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Biases
# Plot Accelerometer Biases on different pages since they can start with quite different
# values, plotting on the same page will lead to a large y axis scale and hide subtle changes.
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.xs,
'r')
plt.xlabel('Time (s)')
plt.ylabel('Accelerometer Biases (X)')
plt.title('Accelerometer Biases (X)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.ys,
'r')
plt.xlabel('Time (s)')
plt.ylabel('Accelerometer Biases (Y)')
plt.title('Accelerometer Biases (Y)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.zs,
'r')
plt.xlabel('Time (s)')
plt.ylabel('Accelerometer Biases (Z)')
plt.title('Accelerometer Biases (Z)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.xs, 'r')
plt.xlabel('Time (s)')
plt.ylabel('Gyro Biases (X)')
plt.title('Gyro Biases (X)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.ys, 'r')
plt.xlabel('Time (s)')
plt.ylabel('Gyro Biases (Y)')
plt.title('Gyro Biases (Y)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.zs, 'r')
plt.xlabel('Time (s)')
plt.ylabel('Gyro Biases (Z)')
plt.title('Gyro Biases (Z)')
pdf.savefig()
plt.close()
# Angular Velocity
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.angular_velocities,
imu_augmented_graph_localization_states.times, 'Ang. Vel.')
plt.xlabel('Time (s)')
plt.ylabel('Angular Velocities')
plt.title('Angular Velocities')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Velocity
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.velocities,
imu_augmented_graph_localization_states.times, 'Vel.')
plt.xlabel('Time (s)')
plt.ylabel('Velocities')
plt.title('IMU Augmented Graph Velocities')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Integrated Velocities
plt.figure()
plot_helpers.plot_positions(sparse_mapping_poses,
colors,
linestyle='None',
marker='o',
markeredgewidth=0.1,
markersize=1.5)
if ar_tag_poses.times:
plot_helpers.plot_positions(ar_tag_poses,
colors,
linestyle='None',
marker='x',
markeredgewidth=0.1,
markersize=1.5)
integrated_imu_augmented_graph_localization_states = utilities.integrate_velocities(
imu_augmented_graph_localization_states)
plot_helpers.plot_positions(
integrated_imu_augmented_graph_localization_states,
colors,
linewidth=0.5)
plt.xlabel('Time (s)')
plt.ylabel('Position (m)')
plt.title(
'Integrated IMU Augmented Graph Velocities vs. Sparse Mapping Position'
)
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Position covariance
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
l2_map(
imu_augmented_graph_localization_states.position_covariances),
'r',
linewidth=0.5,
label='Position Covariance')
plt.title('Position Covariance')
plt.xlabel('Time (s)')
plt.ylabel('Position Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Orientation covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(
imu_augmented_graph_localization_states.orientation_covariances),
'r',
linewidth=0.5,
label='Orientation Covariance')
plt.title('Orientation Covariance (Quaternion)')
plt.xlabel('Time (s)')
plt.ylabel('Orientation Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Velocity covariance
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
l2_map(
imu_augmented_graph_localization_states.velocity_covariances),
'r',
linewidth=0.5,
label='Velocity Covariance')
plt.title('Velocity Covariance')
plt.xlabel('Time (s)')
plt.ylabel('Velocity Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Accel Bias covariance
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.
accelerometer_bias_covariances),
'r',
linewidth=0.5,
label='Accelerometer Bias Covariance')
plt.title('Accelerometer Bias Covariance')
plt.xlabel('Time (s)')
plt.ylabel('Accelerometer Bias Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Gyro Bias covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.gyro_bias_covariances),
'r',
linewidth=0.5,
label='Gyro Bias Covariance')
plt.title('Gyro Bias Covariance')
plt.xlabel('Time (s)')
plt.ylabel('Gyro Bias Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
def add_graph_plots(
pdf,
sparse_mapping_poses,
ar_tag_poses,
graph_localization_states,
imu_augmented_graph_localization_poses,
):
colors = ["r", "b", "g"]
position_plotter = vector3d_plotter.Vector3dPlotter(
"Time (s)", "Position (m)", "Graph vs. Sparse Mapping Position", True)
position_plotter.add_pose_position(
sparse_mapping_poses,
linestyle="None",
marker="o",
markeredgewidth=0.1,
markersize=1.5,
)
if ar_tag_poses.times:
position_plotter.add_pose_position(
ar_tag_poses,
linestyle="None",
marker="x",
markeredgewidth=0.1,
markersize=1.5,
)
position_plotter.add_pose_position(graph_localization_states)
position_plotter.plot(pdf)
# orientations
orientation_plotter = vector3d_plotter.Vector3dPlotter(
"Time (s)", "Orientation (deg)",
"Graph vs. Sparse Mapping Orientation", True)
orientation_plotter.add_pose_orientation(
sparse_mapping_poses,
linestyle="None",
marker="o",
markeredgewidth=0.1,
markersize=1.5,
)
if ar_tag_poses.times:
orientation_plotter.add_pose_orientation(
ar_tag_poses,
linestyle="None",
marker="x",
markeredgewidth=0.1,
markersize=1.5,
)
orientation_plotter.add_pose_orientation(graph_localization_states)
orientation_plotter.plot(pdf)
# Imu Augmented Loc vs. Loc
position_plotter = vector3d_plotter.Vector3dPlotter(
"Time (s)", "Position (m)", "Graph vs. IMU Augmented Graph Position",
True)
position_plotter.add_pose_position(
graph_localization_states,
linestyle="None",
marker="o",
markeredgewidth=0.1,
markersize=1.5,
)
position_plotter.add_pose_position(imu_augmented_graph_localization_poses,
linewidth=0.5)
position_plotter.plot(pdf)
# orientations
orientation_plotter = vector3d_plotter.Vector3dPlotter(
"Time (s)",
"Orientation (deg)",
"Graph vs. IMU Augmented Graph Orientation",
True,
)
orientation_plotter.add_pose_orientation(graph_localization_states,
marker="o",
markeredgewidth=0.1,
markersize=1.5)
orientation_plotter.add_pose_orientation(
imu_augmented_graph_localization_poses, linewidth=0.5)
orientation_plotter.plot(pdf)
# Velocity
plt.figure()
plot_helpers.plot_vector3ds(graph_localization_states.velocities,
graph_localization_states.times, "Vel.")
plt.xlabel("Time (s)")
plt.ylabel("Velocities")
plt.title("Graph Velocities")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Integrated Velocities
integrated_graph_localization_states = utilities.integrate_velocities(
graph_localization_states)
plot_positions(pdf, integrated_graph_localization_states,
sparse_mapping_poses, ar_tag_poses)
def add_other_vector3d_plots(pdf, imu_augmented_graph_localization_states,
sparse_mapping_poses, ar_tag_poses):
colors = ["r", "b", "g"]
# Acceleration
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.accelerations,
imu_augmented_graph_localization_states.times,
"Acc.",
)
plt.xlabel("Time (s)")
plt.ylabel("Acceleration (m/s^2)")
plt.title("Acceleration")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Biases
# Plot Accelerometer Biases on different pages since they can start with quite different
# values, plotting on the same page will lead to a large y axis scale and hide subtle changes.
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.xs,
"r",
)
plt.xlabel("Time (s)")
plt.ylabel("Accelerometer Biases (X)")
plt.title("Accelerometer Biases (X)")
pdf.savefig()
plt.close()
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.ys,
"r",
)
plt.xlabel("Time (s)")
plt.ylabel("Accelerometer Biases (Y)")
plt.title("Accelerometer Biases (Y)")
pdf.savefig()
plt.close()
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.zs,
"r",
)
plt.xlabel("Time (s)")
plt.ylabel("Accelerometer Biases (Z)")
plt.title("Accelerometer Biases (Z)")
pdf.savefig()
plt.close()
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.xs,
"r",
)
plt.xlabel("Time (s)")
plt.ylabel("Gyro Biases (X)")
plt.title("Gyro Biases (X)")
pdf.savefig()
plt.close()
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.ys,
"r",
)
plt.xlabel("Time (s)")
plt.ylabel("Gyro Biases (Y)")
plt.title("Gyro Biases (Y)")
pdf.savefig()
plt.close()
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.zs,
"r",
)
plt.xlabel("Time (s)")
plt.ylabel("Gyro Biases (Z)")
plt.title("Gyro Biases (Z)")
pdf.savefig()
plt.close()
# Angular Velocity
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.angular_velocities,
imu_augmented_graph_localization_states.times,
"Ang. Vel.",
)
plt.xlabel("Time (s)")
plt.ylabel("Angular Velocities")
plt.title("Angular Velocities")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Velocity
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.velocities,
imu_augmented_graph_localization_states.times,
"Vel.",
)
plt.xlabel("Time (s)")
plt.ylabel("Velocities")
plt.title("IMU Augmented Graph Velocities")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Integrated Velocities
plt.figure()
plot_helpers.plot_positions(
sparse_mapping_poses,
colors,
linestyle="None",
marker="o",
markeredgewidth=0.1,
markersize=1.5,
)
if ar_tag_poses.times:
plot_helpers.plot_positions(
ar_tag_poses,
colors,
linestyle="None",
marker="x",
markeredgewidth=0.1,
markersize=1.5,
)
integrated_imu_augmented_graph_localization_states = utilities.integrate_velocities(
imu_augmented_graph_localization_states)
plot_helpers.plot_positions(
integrated_imu_augmented_graph_localization_states,
colors,
linewidth=0.5)
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
plt.title(
"Integrated IMU Augmented Graph Velocities vs. Sparse Mapping Position"
)
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Position covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.position_covariances),
"r",
linewidth=0.5,
label="Position Covariance",
)
plt.title("Position Covariance")
plt.xlabel("Time (s)")
plt.ylabel("Position Covariance")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Orientation covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(
imu_augmented_graph_localization_states.orientation_covariances),
"r",
linewidth=0.5,
label="Orientation Covariance",
)
plt.title("Orientation Covariance (Quaternion)")
plt.xlabel("Time (s)")
plt.ylabel("Orientation Covariance")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Velocity covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.velocity_covariances),
"r",
linewidth=0.5,
label="Velocity Covariance",
)
plt.title("Velocity Covariance")
plt.xlabel("Time (s)")
plt.ylabel("Velocity Covariance")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Accel Bias covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.
accelerometer_bias_covariances),
"r",
linewidth=0.5,
label="Accelerometer Bias Covariance",
)
plt.title("Accelerometer Bias Covariance")
plt.xlabel("Time (s)")
plt.ylabel("Accelerometer Bias Covariance")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Gyro Bias covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.gyro_bias_covariances),
"r",
linewidth=0.5,
label="Gyro Bias Covariance",
)
plt.title("Gyro Bias Covariance")
plt.xlabel("Time (s)")
plt.ylabel("Gyro Bias Covariance")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
def add_other_vector3d_plots(pdf, imu_augmented_graph_localization_states):
# Acceleration
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.accelerations,
imu_augmented_graph_localization_states.times, 'Acc.')
plt.xlabel('Time (s)')
plt.ylabel('Acceleration (m/s^2)')
plt.title('Acceleration')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Biases
# Plot Accelerometer Biases on different pages since they can start with quite different
# values, plotting on the same page will lead to a large y axis scale and hide subtle changes.
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.xs,
'r')
plt.xlabel('Time (s)')
plt.ylabel('Accelerometer Biases (X)')
plt.title('Accelerometer Biases (X)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.ys,
'r')
plt.xlabel('Time (s)')
plt.ylabel('Accelerometer Biases (Y)')
plt.title('Accelerometer Biases (Y)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.zs,
'r')
plt.xlabel('Time (s)')
plt.ylabel('Accelerometer Biases (Z)')
plt.title('Accelerometer Biases (Z)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.xs, 'r')
plt.xlabel('Time (s)')
plt.ylabel('Gyro Biases (X)')
plt.title('Gyro Biases (X)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.ys, 'r')
plt.xlabel('Time (s)')
plt.ylabel('Gyro Biases (Y)')
plt.title('Gyro Biases (Y)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.zs, 'r')
plt.xlabel('Time (s)')
plt.ylabel('Gyro Biases (Z)')
plt.title('Gyro Biases (Z)')
pdf.savefig()
plt.close()
# Angular Velocity
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.angular_velocities,
imu_augmented_graph_localization_states.times, 'Ang. Vel.')
plt.xlabel('Time (s)')
plt.ylabel('Angular Velocities')
plt.title('Angular Velocities')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Velocity
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.velocities,
imu_augmented_graph_localization_states.times, 'Vel.')
plt.xlabel('Time (s)')
plt.ylabel('Velocities')
plt.title('Velocities')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Position covariance
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
l2_map(
imu_augmented_graph_localization_states.position_covariances),
'r',
linewidth=0.5,
label='Position Covariance')
plt.title('Position Covariance')
plt.xlabel('Time (s)')
plt.ylabel('Position Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Orientation covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(
imu_augmented_graph_localization_states.orientation_covariances),
'r',
linewidth=0.5,
label='Orientation Covariance')
plt.title('Orientation Covariance (Quaternion)')
plt.xlabel('Time (s)')
plt.ylabel('Orientation Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Velocity covariance
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
l2_map(
imu_augmented_graph_localization_states.velocity_covariances),
'r',
linewidth=0.5,
label='Velocity Covariance')
plt.title('Velocity Covariance')
plt.xlabel('Time (s)')
plt.ylabel('Velocity Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Accel Bias covariance
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.
accelerometer_bias_covariances),
'r',
linewidth=0.5,
label='Accelerometer Bias Covariance')
plt.title('Accelerometer Bias Covariance')
plt.xlabel('Time (s)')
plt.ylabel('Accelerometer Bias Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Gyro Bias covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.gyro_bias_covariances),
'r',
linewidth=0.5,
label='Gyro Bias Covariance')
plt.title('Gyro Bias Covariance')
plt.xlabel('Time (s)')
plt.ylabel('Gyro Bias Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
