Wednesday, August 2, 2017

New Plan

The new plan is to organize the inputs as follows:

particle_cloud_mean_pose_x_4 - particle_cloud_mean_pose_x_0
particle_cloud_mean_pose_x_3 - particle_cloud_mean_pose_x_0
particle_cloud_mean_pose_x_2 - particle_cloud_mean_pose_x_0
particle_cloud_mean_pose_x_1 - particle_cloud_mean_pose_x_0
particle_cloud_mean_pose_x_0 - particle_cloud_mean_pose_x_0

particle_cloud_mean_pose_y_4
particle_cloud_mean_pose_y_3
particle_cloud_mean_pose_y_2
particle_cloud_mean_pose_y_1
particle_cloud_mean_pose_y_0

particle_cloud_mean_pose_qx_4
particle_cloud_mean_pose_qx_3
particle_cloud_mean_pose_qx_2
particle_cloud_mean_pose_qx_1
particle_cloud_mean_pose_qx_0

particle_cloud_mean_pose_qy_4
particle_cloud_mean_pose_qy_3
particle_cloud_mean_pose_qy_2
particle_cloud_mean_pose_qy_1
particle_cloud_mean_pose_qy_0

particle_cloud_mean_pose_qz_4
particle_cloud_mean_pose_qz_3
particle_cloud_mean_pose_qz_2
particle_cloud_mean_pose_qz_1
particle_cloud_mean_pose_qz_0

particle_cloud_mean_pose_qw_4
particle_cloud_mean_pose_qw_3
particle_cloud_mean_pose_qw_2
particle_cloud_mean_pose_qw_1
particle_cloud_mean_pose_qw_0

particle_cloud_stddev_x_4
particle_cloud_stddev_x_3
particle_cloud_stddev_x_2
particle_cloud_stddev_x_1
particle_cloud_stddev_x_0

particle_cloud_stddev_y_4
particle_cloud_stddev_y_3
particle_cloud_stddev_y_2
particle_cloud_stddev_y_1
particle_cloud_stddev_y_0

particle_cloud_stddev_qx_4
particle_cloud_stddev_qx_3
particle_cloud_stddev_qx_2
particle_cloud_stddev_qx_1
particle_cloud_stddev_qx_0

particle_cloud_stddev_qy_4
particle_cloud_stddev_qy_3
particle_cloud_stddev_qy_2
particle_cloud_stddev_qy_1
particle_cloud_stddev_qy_0

particle_cloud_stddev_qz_4
particle_cloud_stddev_qz_3
particle_cloud_stddev_qz_2
particle_cloud_stddev_qz_1
particle_cloud_stddev_qz_0

particle_cloud_stddev_qw_4
particle_cloud_stddev_qw_3
particle_cloud_stddev_qw_2
particle_cloud_stddev_qw_1
particle_cloud_stddev_qw_0

The new plan is to organize the OUTPUTS as a metric of error volume:
(AMCL_X - Gazebo_X) * (AMCL_Y - Gazebo_Y) * (AMCL_QX - Gazebo_QX) * (AMCL_QY - Gazebo_QY) * (AMCL_QZ - Gazebo_QZ) * (AMCL_QW - Gazebo_QW)

Note: the QX/QY/QZ/QW stuff is the angle in quaternion form.  A common angle is [0 0 0 1], and if AMCL and Gazebo poses ever align to that, the (0 - 0) in the resulting equation will cause the whole error metric to zero out.







No comments:

Post a Comment