uwb_tracker_node.py 文件源码

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