def initialize_estimate(self, estimate_id, initial_state):
"""Initialize a state estimate with identity covariance.
The initial estimate is saved in the `self.estimates` dictionary.
The timestamp in the `self.estimate_times` is updated.
Args:
estimate_id (int): ID of the tracked target.
initial_state (int): Initial state of the estimate.
Returns:
X (numpy.ndarray): Solution of equation.
"""
x = initial_state
P = self.initial_position_covariance * np.eye(6)
P[3:6, 3:6] = 0
estimate = UWBTracker.StateEstimate(x, P)
self.estimates[estimate_id] = estimate
self.estimate_times[estimate_id] = rospy.get_time()
self.ikf_prev_outlier_flags[estimate_id] = False
self.ikf_outlier_counts[estimate_id] = 0
评论列表
文章目录