def _read_configuration(self):
"""Initialize configuration from ROS parameters.
"""
self.uwb_multi_range_topic = rospy.get_param('~multi_range_raw_topic', '/uwb/multi_range_with_offsets')
self.uwb_tracker_topic = rospy.get_param('~tracker_topic', '/uwb/tracker')
self.tracker_frame = rospy.get_param('~tracker_frame', 'uwb')
self.target_frame = rospy.get_param('~target_frame', 'target')
# Get parameters for covariance matrices
self.initial_position_covariance = rospy.get_param('~initial_position_covariance', 10)
self.process_covariance_position = rospy.get_param('~process_covariance_position', 0)
self.process_covariance_velocity = rospy.get_param('~process_covariance_velocity', 1)
self.measurement_covariance = rospy.get_param('~measurement_covariance', 0.1 ** 2)
# Get parameters for filter update and initial gauss-newton estimation
self.ignore_z_position = rospy.get_param('~ignore_z_position', True)
# The default value of 7.779 represents the 0.9 quantile of a Chi-Square distribution
# with 4 degrees of freedom (4 UWB measurements).
self.outlier_threshold_quantile = rospy.get_param('~outlier_threshold_quantile', 0.1)
self.ikf_iterations = rospy.get_param('~ikf_iterations', 2)
self.initial_guess_position = np.empty((3, 1), dtype=np.float)
self.initial_guess_position[0] = rospy.get_param('~initial_guess_position_x', 0)
self.initial_guess_position[1] = rospy.get_param('~initial_guess_position_y', 0)
self.initial_guess_position[2] = rospy.get_param('~initial_guess_position_z', 0)
self.initial_guess_iterations = rospy.get_param('~initial_guess_iterations', 200)
self.initial_guess_tolerance = rospy.get_param('~initial_guess_tolerance', 1e-5)
self.initial_guess_residuals_threshold = rospy.get_param('~initial_guess_residuals_threshold', 0.1)
self.ikf_max_outlier_count = rospy.get_param('~ikf_max_outlier_count', 200)
评论列表
文章目录